#define SW_D 32
#define SW_U 33
#define O 35
#define SL 14
#define WL 17

#define CERRADA 0
#define SUBIENDO 1
#define ABIERTA 2
#define BAJANDO 3


volatile int state = CERRADA;
volatile bool buttonO_released = false;
volatile int currentTime = 0;


TaskHandle_t HandleTask1; // Handle de la tarea 1

volatile bool timerBlink_interrupted = false;
hw_timer_t *timerBlink = NULL;

void IRAM_ATTR timerBlink_int() {
  timerBlink_interrupted = true;
}

void motorUp () {
  digitalWrite(23, HIGH);
  digitalWrite(22, LOW);
  digitalWrite(21, LOW);
}

void motorDown () {
  digitalWrite(23, LOW);
  digitalWrite(22, HIGH);
  digitalWrite(21, LOW);
}

void motorStop () {
  digitalWrite(23, LOW);
  digitalWrite(22, LOW);
  digitalWrite(21, HIGH);
}

void IRAM_ATTR buttonO_ISR() {
  buttonO_released = true;
}

void FSM(void *parameter)
{
  while (true) {
    switch (state) {
      case CERRADA:
        motorStop();
        if (buttonO_released) {
          buttonO_released = false;
          Serial.println("Pulsado el botón de apertura manual");
          motorUp();
          state = SUBIENDO;
          timerStart(timerBlink);
          Serial.println("Subiendo...");
        }
        break;
      case SUBIENDO:
        if (digitalRead(SW_U)) {
          Serial.println("Se ha llegado al fin de carrera superior");
          state = ABIERTA;
          motorStop();
          timerStop(timerBlink);
          currentTime = millis();
        }

        if (timerBlink_interrupted) {
          timerBlink_interrupted = false;
          digitalWrite(WL, !digitalRead(WL));
        }

        if (buttonO_released) {
           buttonO_released = false;

        }

        break;

      case ABIERTA:
        if ((millis() - currentTime) >= 3000 && digitalRead(SL) == 0) {
          Serial.println("Han pasado 30s, y no hay vehiculo");
          motorDown();
          state = BAJANDO;
          timerStart(timerBlink);
          Serial.println("Bajando...");
        }

        if (buttonO_released) {
           buttonO_released = false;

        }
        break;

      case BAJANDO:
        if (digitalRead(SL) == 1 || buttonO_released) {
          Serial.println("Se ha detectado vehiculo en la bajada o se ha pulsado el botón de apertura");
          motorUp();
          buttonO_released = false;
          state = SUBIENDO;
          timerStart(timerBlink);
          Serial.println("Subiendo...");
        }

        if (digitalRead(SW_D) == 1) {
          Serial.println("Se ha cerrado la barrera");
          motorStop();
          state = CERRADA;
          timerStop(timerBlink);
        }

        if (timerBlink_interrupted) {
          timerBlink_interrupted = false;
          digitalWrite(WL, !digitalRead(WL));
        }
        
        if (buttonO_released) {
           buttonO_released = false;

        }

        break;
    }
    vTaskDelay(10);
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(21, OUTPUT);
  pinMode(22, OUTPUT);
  pinMode(23, OUTPUT);
  pinMode(WL, OUTPUT);

  pinMode(SW_D, INPUT_PULLDOWN);
  pinMode(SL, INPUT_PULLDOWN);
  pinMode(SW_U, INPUT_PULLDOWN);
  pinMode(O, INPUT_PULLDOWN);

  timerBlink = timerBegin(1000000); //Es la frecuencia a la que configuramos el timer
  timerAttachInterrupt(timerBlink, &timerBlink_int);
  timerAlarm(timerBlink, 250000, true, 0); // timer que nos avisa a N = t*f, es decir cada 0.25s. N = 250000 = 0.25 * 1000000

  attachInterrupt(digitalPinToInterrupt(O), buttonO_ISR, FALLING);

  xTaskCreatePinnedToCore(
    FSM,                 // Function en la que se implementa la tarea
    "FSM",                   // Nombre de la tarea
    2048, // Memoria (en Bytes) asignados a esta tarea
    NULL,                     // Parámetro de entrada de la tarea (no hay ningún parámetro)
    1,                        // Prioridad de la tarea (cuanto más alto, más prioridad)
    &HandleTask1,             // Handle de la tarea 1
    0);

}

void loop() {
  // put your main code here, to run repeatedly:
  delay(10); // this speeds up the simulation
}