// Si se pulsa el botón correcto, el led asociado al botón parpadeará 3 veces con T = 500ms y luego permanecerá encendido. Si el botón es incorrecto los 3 leds se apagarán y se empieza de nuevo
// La pulsación de 3 tiene que ser larga, durante más de 1 segundo para pasar al estado correcto
// Usar FreeRTOS

#define led1 21
#define led2 19
#define led3 18

#define button1 34
#define button2 25
#define button3 13

volatile bool button1_released = false;
volatile bool button2_released = false;
volatile bool button3_interrupted = false;
volatile bool button3_pressed = false;

volatile bool long_pressed = false;

volatile bool timer1_interrupted = false;
volatile bool timer0_interrupted = false;

volatile int counter = 0;
hw_timer_t *timer1 = NULL;
hw_timer_t *timer0 = NULL;

volatile int state = 0;

volatile bool reset_code = false;

TaskHandle_t HandleTask1; // Handle de la tarea 1
TaskHandle_t HandleTask2; // Handle de la tarea 2

void IRAM_ATTR button1_ISR() {
  button1_released = true;
}

void IRAM_ATTR button2_ISR() {
  button2_released = true;
}

void IRAM_ATTR button3_ISR() {
  button3_interrupted = true;
  button3_pressed = !button3_pressed;
}

void IRAM_ATTR timer_blink() {
  timer1_interrupted = true;
}

void IRAM_ATTR timer_counter() {
  timer0_interrupted = true;
}

void reset(void *parameter)
{
  while (1)
  {
    if (reset_code)
    {
      //Serial.println("Reseteando codigo");
      state = 0;
      // Serial.println(state);

      button1_released = false;
      button2_released = false;
      button3_interrupted = false;

      timer1_interrupted = false;
      timerAlarmDisable(timer1);
      timer0_interrupted = false;
      timerAlarmDisable(timer0);

      long_pressed = false;
      counter = 0;

      digitalWrite(led1, LOW);
      digitalWrite(led2, LOW);
      digitalWrite(led3, LOW);

      reset_code = false;
    }
    vTaskDelay(10);
  }
}

void FSM(void *parameter)
{
  while (true) {
    if (!reset_code) {
      switch (state) {
        case 0:
          //En el estado 0 está todo apagado, cambio al estado 1 si se pulsa el button1
          //Botón correcto --> B1
          if (button1_released) {
            button1_released = false;
            Serial.println("Pulsado correctamente");
            timerRestart(timer1);
            timerAlarmEnable(timer1);
          }
          //Botones incorrectos --> B2, B3
          if (button2_released || button3_interrupted) {
            Serial.println("Pulsado incorrectamente");
            reset_code = true;
          }
          //Timer encargado de los parpadeos
          if (timer1_interrupted) {
            counter++;
            if (counter < 6) {
              digitalWrite(led1, !digitalRead(led1));
            } else {
              state = 1;
              counter = 0;
              timerAlarmDisable(timer1);
            }
            timer1_interrupted = false;
          }
          break;
        case 1:
          //En el estado 1 led1 on, cambio al estado 2 si se pulsa el button2
          //Botón correcto --> B2
          if (button2_released) {
            button2_released = false;
            Serial.println("Pulsado correctamente");
            timerRestart(timer1);
            timerAlarmEnable(timer1);

          }
          //Botones incorrectos --> B1, B3
          if (button1_released || button3_interrupted) {
            Serial.println("Pulsado incorrectamente");
            reset_code = true;
          }

          if (timer1_interrupted) {
            counter++;
            if (counter < 6) {
              digitalWrite(led2, !digitalRead(led2));
            } else {
              state = 2;
              counter = 0;
              timerAlarmDisable(timer1);
            }
            timer1_interrupted = false;
          }
          break;

        case 2:
          //En el estado 2 led1 on, led2, cambio al estado 3 si se pulsa el button3 durante más de 1s
          //Botón correcto --> B3
          if (button3_interrupted) {
            button3_interrupted = false;
            if (button3_pressed) {
              //Botón presionado, arranca el timer de contar segundos
              timerRestart(timer0);
              timerAlarmEnable(timer0);
            }
            //Botón soltado, para el timer de contar segundos
            else {
              timerAlarmDisable(timer0);
              if (long_pressed) {
                Serial.println("Pulsado correctamente, pulsación larga de 1s"); //Parpadea 3 veces y cambia de estado
                timerRestart(timer1);
                timerAlarmEnable(timer1);
              } else {
                Serial.println("Pulsado incorrectamente");
                 //Se va a el estado inicial sin hacer nada
                reset_code = true;
                state = 0;
              }
            }
          }
          //Botones incorrectos --> B1, B2
          if (button1_released || button2_released) {
            Serial.println("Pulsado incorrectamente");
            reset_code = true;
          }

          if (timer0_interrupted) {
            Serial.println("Es larga");
            long_pressed = true;
            timer0_interrupted = false;
          }

          if (timer1_interrupted) {
            counter++;
            if (counter < 6) {
              digitalWrite(led3, !digitalRead(led3));
            } else {
              state = 3;
              //counter = 0;
              timerRestart(timer1);
              timerAlarmEnable(timer1);
            }
            timer1_interrupted = false;
          }
          break;

        case 3:
          //Secuencia correcta
          if (button3_interrupted || button1_released || button2_released) {
            reset_code = true;
            Serial.println("Secuencia correcta, vuelta al inicio");
            timerRestart(timer1);
            timerAlarmEnable(timer1);
          }
          //Parpadeo intermitente de todos, hasta que no se pulse cualquier botón a modo de reset
          if (timer1_interrupted) {
            digitalWrite(led1, !digitalRead(led1));
            digitalWrite(led2, !digitalRead(led2));
            digitalWrite(led3, !digitalRead(led3));
            timer1_interrupted = false;
          }
          break;
      }
      vTaskDelay(10);
    }
  }
}
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);
  pinMode(button3, INPUT_PULLUP);

  // Configurar LEDs como salida digital
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);

  // Configurar las interrupciones por pulsación de botones
  attachInterrupt(digitalPinToInterrupt(button1), button1_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(button2), button2_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(button3), button3_ISR, CHANGE);

  timer1 = timerBegin(1, 80, true);
  timerAttachInterrupt(timer1, &timer_blink, true);
  timerAlarmWrite(timer1, 500000, true); // Aviso cada 0.5s

  timer0 = timerBegin(0, 80, true);
  timerAttachInterrupt(timer0, &timer_counter, true);
  timerAlarmWrite(timer0, 1000000, true); // Aviso cada 1s

  // Creamos la Tarea 1: CPU0
  xTaskCreatePinnedToCore(
    reset,                 // Function en la que se implementa la tarea
    "Reset",                   // 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);                       // Core donde va a correr la tarea 1

  // Creamos la Tarea 2: CPU1
  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)
    5,                        // Prioridad de la tarea (cuanto más alto, más prioridad)
    &HandleTask2,             // Handle de la tarea 2
    1);                       // Core donde va a correr la tarea 2
}

void loop() {
  delay(10); // this speeds up the simulation
}