/*
  Author: Juan M. Gandarias
  Date: 10/01/2024
  Description: EscapeRoom_botonera
  muchos botones multicore con fms
*/

// GPIO
#define B1 34
#define B2 33
#define B3 25
#define L1 21
#define L2 19
#define L3 18

// Variable para seleccionar LED
uint8_t LED_PIN = L1;

// Constante de número de blinks. Tener en cuenta que un blink es apagado y encendido. 6 blinks significa que el LED parpadea 3 veces
const uint8_t numberOfBlinks = 5;

// Variable para indicar si el botón pulsado es correcto
bool correct_button = false;

// Variables pulsación botones
volatile bool B1_released = false;
volatile bool B2_released = false;
volatile bool B3_released = false;
volatile bool reset_code = false;

volatile uint8_t fsm_state = 0;                // Variable volátil para almacenar el estado de la FSM

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

// Callback de la interrupción por pulsación de B1
void IRAM_ATTR B1Released()
{
  Serial.println("B1 pulsado");
  B1_released = true;
}

// Callback de la interrupción por pulsación de B2
void IRAM_ATTR B2Released()
{
  Serial.println("B2 pulsado");
  B2_released = true;
}

// Callback de la interrupción por pulsación de B3
void IRAM_ATTR B3Released()
{
  Serial.println("B3 pulsado");
  B3_released = true;
}

void setup()
{
  // Puerto serie
  Serial.begin(112500);

  // Configurar botones como entrada digital
  pinMode(B1, INPUT_PULLUP);
  pinMode(B2, INPUT_PULLUP);
  pinMode(B3, INPUT_PULLUP);

  // Configurar LEDs como salida digital
  pinMode(L1, OUTPUT);
  pinMode(L2, OUTPUT);
  pinMode(L3, OUTPUT);

  // Configurar las interrupciones por pulsación de botones
  attachInterrupt(digitalPinToInterrupt(B1), B1Released, FALLING);
  attachInterrupt(digitalPinToInterrupt(B2), B2Released, FALLING);
  attachInterrupt(digitalPinToInterrupt(B3), B3Released, FALLING);

  // 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
    1);                       // 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(100); // this speeds up the simulation
}


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

      B1_released = false;
      B2_released = false;
      B3_released = false;

      digitalWrite(L1, LOW);
      digitalWrite(L2, LOW);
      digitalWrite(L3, LOW);
      reset_code = false;
    }
    vTaskDelay(10);
  }
}

void FSM(void *parameter)
{
  while (1)
  {
    if (!reset_code) {
      // Compruebo en que estado estoy
      switch (fsm_state)
      {
        case 0:
          LED_PIN = L1; // En el estado 0, tengo que encender L1
          if (B1_released && !B2_released && !B3_released)
          {
            correct_button = true;
            Serial.println("Primer botón correcto");
          }
          else if (B2_released || B3_released)
          {
            correct_button = false;
            Serial.println("Primer botón incorrecto");
            reset_code = true;
          }
          break;

        case 1:
          LED_PIN = L2; // En el estado 1, tengo que encender L2
          if (!B1_released && B2_released && !B3_released)
          {
            correct_button = true;
            Serial.println("Segundo botón correcto");
          }
          else if (B1_released || B3_released)
          {
            correct_button = false;
            Serial.println("Segundo botón incorrecto");
            reset_code = true;
          }
          break;

        case 2:
          LED_PIN = L3; // En el estado 2, tengo que encender L3
          if (!B1_released && !B2_released && B3_released)
          {
            correct_button = true;
            Serial.println("Tercer botón correcto");
          }
          else if (B1_released || B2_released)
          {
            correct_button = false;
            Serial.println("Tercer botón incorrecto");
            reset_code = true;
          }
          break;
      }

      if (correct_button && fsm_state<3 && (B1_released || B2_released || B3_released))
      { // Si se habia pulsado el botón correcto, cambio de estado, hago que el LED parpade, y lo dejo encendido. Si no, reinicio
        Serial.println("Boton correcto");
        fsm_state += 1;
        B1_released = false;
        B2_released = false;
        B3_released = false;
        Serial.println(fsm_state);
        for (uint8_t i = 0; i < numberOfBlinks; i++) {
          digitalWrite(LED_PIN, !digitalRead(LED_PIN));
          vTaskDelay(500);
        }
        digitalWrite(LED_PIN, HIGH);
      }
      else if ((B1_released || B2_released || B3_released) && fsm_state==3)
      {
        Serial.println("Boton incorrecto");
        reset_code = true;
      }

    }
    vTaskDelay(10);
  }
}