#include <ESP32Servo.h>
#include "Definition.h"
#include "Sensors.h"
#include "Actuators.h"
#include "Timers.h"
#include "FSM_Transition.h"
#include "Logs.h"

//----------------------------------------------
void do_init()
{
  Serial.begin(9600);
  
  pinMode(PIN_LED, OUTPUT);
  pinMode(PIN_SERVO , OUTPUT);
  pinMode(PIN_TRIGGER,OUTPUT);
  
  pinMode(PIN_SENSOR_PULSADOR,INPUT_PULLDOWN);
  pinMode(PIN_SENSOR_LUZ,INPUT);
  pinMode(PIN_ECHO,INPUT);

  sensores[SENSOR_LUZ].pin    = PIN_SENSOR_LUZ;
  sensores[SENSOR_LUZ].estado = ESTADO_SENSOR_OK;

  sensores[SENSOR_DISTANCIA].pin    = PIN_ECHO;
  sensores[SENSOR_DISTANCIA].estado = ESTADO_SENSOR_OK;  

  sensores[SENSOR_PULSADOR].pin    = PIN_SENSOR_PULSADOR;
  sensores[SENSOR_PULSADOR].estado = ESTADO_SENSOR_OK;
  
   //Inicializamos la posicion del servo
  servo.attach(PIN_SERVO, 500, 2500);
  
  // Inicializo el evento inicial
  current_state = ST_IDLE;
  
  //Creo la tarea del loop
  xTaskCreate(vLoopTask,"vLoopTask",2048,NULL,1,&loopNewEventHandler); 
  //creo la tarea de getEVent
  xTaskCreate(vGetNewEventTask,"vGetNewEventTask",2048,NULL,1,&loopTaskHandler); 

  // Creo el temporizador
  timerServoTaskHandle = xTimerCreate("Timer", pdMS_TO_TICKS(TIEMPO_TIMER), pdTRUE, NULL, callbackTemporizador);

}
//----------------------------------------------


//----------------------------------------------
void vGetNewEventTask(void *pvParameters)
{
   enum events new_event;
   short indice = 0;

    while(1)
    {
      indice = (ult_indice_tipo_sensor % MAX_TIPO_EVENTOS);

      ult_indice_tipo_sensor++;

      new_event=verificar_sensor[indice]();
    
      if (xQueueSend(queueEvents, &new_event, 0) != pdPASS)
      {
        Serial.println("Get_event: Queue is full.");
      } 
      vTaskDelay(pdMS_TO_TICKS(200));
    }
  
}
//----------------------------------------------

//----------------------------------------------
//Handler del temporizador
void callbackTemporizador(TimerHandle_t xTimer)
{
  // Solo notifica a la tarea loop
  xTaskNotifyGive(loopTaskHandler);
  Serial.print("detiene timer");
  xTimerStop(timerServoTaskHandle,0);
}

// Tarea principal
void vLoopTask(void *pvParameters)
{
  while(1)
  {
    maquina_estados();
  }
}

//----------------------------------------------
void maquina_estados( )
{
  enum events new_event;
  //Serial.println("esperando cola sensor");

  //La tarea se va a quedar bloqueada hasta que haya un evento en la cola
  xQueueReceive(queueEvents, &new_event, portMAX_DELAY); 

  if( (new_event >= 0) && (new_event < MAX_EVENTS) && (current_state >= 0) && (current_state < MAX_STATES) )
  {
    showStateEvent(current_state,new_event);
    
    state_table[current_state][new_event]();
  }
  else
  {
    showStateEvent(ST_ERROR,EV_UNKNOW);
  }

 // Consumo el evento...
  new_event   = EV_CONT;
}
//----------------------------------------------


// Funciones de arduino !. 
//----------------------------------------------
void setup()
{
  do_init();
}
//----------------------------------------------

//----------------------------------------------
void loop()
{
}
//----------------------------------------------