#define SW_D 32
#define SW_U 33
#define O 35
#define SL 34

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

TaskHandle_t HandleTask1; // Handle de la tarea 1

volatile bool timer30_interrupted = false;
hw_timer_t *timer_30 = NULL;

volatile int counter_30 = 0;


void IRAM_ATTR timer_30_int() {
  timer30_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 0: //SW_D = 1, SW_U = 0. Para pasar a estado 1 --> buttonO pulsado

          motorStop();

          if (buttonO_released && SW_D == 1 && SW_U == 0) {
            buttonO_released = false;
            Serial.println("Pulsado el botón de apertura");
            motorUp();
            state = 1;
            timerStart(timer_30);
            Serial.println("Arrancado el timer");

          }
          break;
        case 1: //SW_D = 0, SW_U = 1. Volveremos al estado 0 --> pasados 30s
          if (timer30_interrupted) {
             timer30_interrupted = false;
            if(counter_30 == 5 && SL == 0 && SW_U == 1){ //Han pasado 30s, no hay coche y está arriba
              Serial.println("Timer llego a 30, no hay coche y la barrera está arriba. Voy hacia abajo");
              counter_30 = 0;
              state = 0;
              motorDown();
              timer30_interrupted = false;
              timerStop(timer_30);
            }else if (counter_30 == 5 && SL == 1 && SW_U == 1){// Han pasado 30s, hay coche y está arriba
              motorStop();
              state = 1;
              Serial.println("Timer llego a 30, hay coche y la barrera está arriba. Me quedo arriba");
            }else if (counter_30 > 5 && SL == 0 && SW_U == 1){ // Han pasado más de 30s, no hay coche y estoy arriba
              motorDown();
              timerStop(timer_30);
              counter_30 = 0;
              state = 0;
              Serial.println("Timer llego a más 30, no hay coche y la barrera está arriba. Voy hacia abajo");
            }else if (counter_30 < 5 && SW_U == 1) { //No han pasado 30s y estoy arriba
              counter_30++;
              motorStop();
              state = 1;
              Serial.println("Timer no llegó a 30, si/no hay coche y la barrera está arriba. Sigo contando, me quedo arriba");
            }
          }

          if(buttonO_released) {}

          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(SW_D, INPUT_PULLUP);
  pinMode(SW_U, INPUT_PULLDOWN);
  pinMode(O, INPUT_PULLDOWN);

  timer_30 = timerBegin(80000);
  timerAttachInterrupt(timer_30, &timer_30_int);
  timerAlarm(timer_30, 80000, true, 0); //timer que nos avisa a N = t*f, es decir cada 1s

  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
}