#define button1 34
#define button2 35
#define button3 32
#define button4 33
#define button5 25
#define button6 26
#define button7 27
#define button8 14

#define led1 22
#define led2 17
#define led3 16

volatile bool button1_released = false;
volatile bool button2_released = false;
volatile bool button3_released = false;
volatile bool timerBlink_interrupted = false;
TaskHandle_t HandleTask1; // Handle de la tarea 1

volatile int cont = 0;
volatile bool isBlinking = false;
char cod1;
char cod2;
char cod3;

char intento1;
char intento2;
char intento3;

#define ESTADO_0 0
#define ESTADO_1 1
#define ESTADO_2 2
#define ESTADO_3 3

int state = ESTADO_0;

void IRAM_ATTR button1_ISR() {
  button1_released = true;
}

void IRAM_ATTR button2_ISR() {
  button2_released = true;
  Serial.println("k2");
}

void IRAM_ATTR button3_ISR() {
  button3_released = true;
  Serial.println("k3");

}

hw_timer_t *timerBlink = NULL;

void IRAM_ATTR timerBlink_int() {
  timerBlink_interrupted = true;
}

void clearButtons() {
  button1_released = false;
  button2_released = false;
  button3_released = false;
}

volatile bool correct_button = false;
/*
  void FSM(void *parameter) {
  while (true) {
    switch (state) {
      case ESTADO_0:
        if (button1_released) {
          button1_released = false;
          intento1 = 'a';
          if (intento1 == cod1) {
            Serial.println("Primer botón correcto");
            timerStart(timerBlink);
            correct_button = true;
          }
        }
        if (button2_released) {
          intento1 = 'b';
          if (intento1 == cod1) {
            Serial.println("Botón correcto");
          }
        }

        if (timerBlink_interrupted) {
          timerBlink_interrupted = false;
          cont++;
          digitalWrite(led1, !digitalRead(led1));
          if (cont == 5 && correct_button) {
            correct_button = false;
            cont = 0;
            state = ESTADO_1;
          }
        }
        break;

      case ESTADO_1:
        break;
    }    vTaskDelay(10);


  }
  }
*/


void FSM(void *parameter) {
  while (true) {
    switch (state) {
      case ESTADO_0:

        if (button1_released) {
          button1_released = false;
          intento1 = 'a';
          if (intento1 == cod1) {
            state = ESTADO_1;
            Serial.println("Primer botón correcto");
            timerStart(timerBlink);
            isBlinking = true;
            correct_button = true;
          }
        }
        if (button2_released) {
          button2_released = false;


          intento1 = 'b';
          if (intento1 == cod1) {
            //Serial.println("0 botones correctos");
          }
        }
        if (button3_released) {
          button3_released = false;

          intento1 = 'c';
          if (intento1 == cod1) {
            //Serial.println("0 botones correctos");
          }
        }
        break;

      case ESTADO_1:
      if(isBlinking) {
        if (timerBlink_interrupted) {
          timerBlink_interrupted = false;
          cont++;
          digitalWrite(led1, !digitalRead(led1));
          if (cont == 5) {
            timerStop(timerBlink);
            cont = 0;
            isBlinking = false;
          }
        }
      }else{
        if (button1_released ) {
          button1_released = false;
          intento2 = 'a';
          if (intento2 == cod2) {
          }
          else {
            state = ESTADO_0;
            digitalWrite(led1, LOW);
            digitalWrite(led2, LOW);
            digitalWrite(led3, LOW);
            Serial.println("0 botones correctos");

          }
        }
        if (button2_released) {
          button2_released = false;
          intento2 = 'b';
          if (intento2 == cod2) {
            state = ESTADO_2;
            Serial.println("Segundo botón correcto");
            timerStart(timerBlink);
            isBlinking = true;
            correct_button = true;
          }
          else {
          }
        }
        if (button3_released) {
          button3_released = false;
          intento2 = 'c';
          if (intento2 == cod2) {
          }
          else {
            state = ESTADO_0;
            digitalWrite(led1, LOW);
            digitalWrite(led2, LOW);
            digitalWrite(led3, LOW);
            Serial.println("0 botones correctos");

          }
        }
      }
        break;

      case ESTADO_2:
      if(isBlinking){
        if (timerBlink_interrupted) {
          timerBlink_interrupted = false;
          cont++;
          digitalWrite(led2, !digitalRead(led2));
          if (cont == 5) {
            timerStop(timerBlink);
            cont = 0;
            isBlinking = false;
          }
        }
      }else{
        if (button1_released) {
          button1_released = false;
          intento3 = 'a';
          if (intento3 == cod3) {
          }
          else {
            state = ESTADO_0;
            digitalWrite(led1, LOW);
            digitalWrite(led2, LOW);
            digitalWrite(led3, LOW);
            Serial.println("0 botones correctos");

          }
        }
        if (button2_released) {
          button2_released = false;
          intento3 = 'b';
          if (intento3 == cod3) {
          }
          else {
            state = ESTADO_0;
            digitalWrite(led1, LOW);
            digitalWrite(led2, LOW);
            digitalWrite(led3, LOW);
            Serial.println("0 botones correctos");
          }
        }
        if (button3_released) {
          button3_released = false;
          intento3 = 'c';
          Serial.println("droga");

          if (intento3 == cod3) {
            state = ESTADO_3;
            Serial.println("Tercer botón correcto");
            timerStart(timerBlink);
            correct_button = true;
          }
          else {
          }
        }
      }
        break;

      case ESTADO_3:

        if (timerBlink_interrupted) {
          timerBlink_interrupted = false;
          cont++;
          digitalWrite(led3, !digitalRead(led3));
          if (cont == 5) {
            timerStop(timerBlink);
            cont = 0;
            Serial.println("Codigo correcto: Apertura de la puerta");
          }
        }
        break;
    }    vTaskDelay(10);


  }
}




void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);

  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);
  pinMode(button3, INPUT_PULLUP);


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

  attachInterrupt(digitalPinToInterrupt(button1), button1_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(button2), button2_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(button3), button3_ISR, FALLING);


  cod1 = 'a';
  cod2 = 'b';
  cod3 = 'c';

  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
}