#include <LiquidCrystal_I2C.h>

#define I2C_addr    0x27
#define columnsLCD 20
#define rowsLCD   4

LiquidCrystal_I2C lcd(I2C_addr, columnsLCD, rowsLCD);

#define ledPI 23
#define ledPA 17
#define ledM 16
#define ledA 4
#define ledAlerta 5

#define button1 32
#define button2 33
#define buttonAlert 14

#define PARADA_INACTIVO 0
#define PARADA_ACTIVO 1
#define MANUAL 2
#define AUTOMATICO 3

volatile int state = PARADA_INACTIVO;
volatile bool button1_released = false;
volatile bool button2_released = false;
volatile bool buttonAlert_released = false;
volatile bool alertActivated = false;
volatile bool rearme = 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 buttonAlert_ISR() {
  buttonAlert_released = true;
}

void FSM_ALERTA(void *parameter) {
  TickType_t xLastWakeTime;
  const TickType_t xPeriod = pdMS_TO_TICKS(200);  //1000/500=2Hz
  xLastWakeTime = xTaskGetTickCount();
  while (1) {
    if (buttonAlert_released) {
      buttonAlert_released = false;
      
      if (alertActivated)
        rearme = true;
      else
        alertActivated = true;

    }
    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }


}

void FSM_TREN(void *parameter) {
  TickType_t xLastWakeTime;
  const TickType_t xPeriod = pdMS_TO_TICKS(500);  //1000/500=2Hz
  xLastWakeTime = xTaskGetTickCount();
  while (1) {
    switch (state) {
      case PARADA_INACTIVO:

        digitalWrite(ledPI, HIGH);
        digitalWrite(ledPA, LOW);
        digitalWrite(ledM, LOW);
        digitalWrite(ledA, LOW);
        lcd.setCursor(0, 0);
        lcd.print("Estado: P.inactivo");
        lcd.setCursor(0, 2);
        lcd.print("Boton1: P.activo");

        if (button1_released) {
          button1_released = false;
          state = PARADA_ACTIVO;
          lcd.clear();
        }
        if (button2_released) {
          // button2_released = false;
        }
        if (buttonAlert_released) {

        }
        break;


      case PARADA_ACTIVO:

        digitalWrite(ledPI, LOW);
        digitalWrite(ledPA, HIGH);
        digitalWrite(ledM, LOW);
        digitalWrite(ledA, LOW);
        lcd.setCursor(0, 0);
        lcd.print("Estado: P.activo");
        lcd.setCursor(0, 2);
        lcd.print("Boton1: P.inactivo");
        lcd.setCursor(0, 3);
        lcd.print("Boton2: Manual");

        if (button1_released) {
          button1_released = false;
          state = PARADA_INACTIVO;
          lcd.clear();
        }
        if (button2_released && !alertActivated) {
          button2_released = false;
          state = MANUAL;
          lcd.clear();

        }

        if (buttonAlert_released) {

        }
        break;

      case MANUAL:

        if (alertActivated) {

          digitalWrite(ledPI, LOW);
          digitalWrite(ledPA, LOW);
          digitalWrite(ledM, LOW);
          digitalWrite(ledA, LOW);
          lcd.setCursor(8, 0);
          lcd.print("¡ALERTA!");
          lcd.setCursor(0, 2);
          lcd.print("Estado: Manual");
          lcd.setCursor(0, 3);
          lcd.print("BotonALERTA: Rearma");
          digitalWrite(5, HIGH);

          if (rearme) {
            rearme = false;
            alertActivated = false;
            digitalWrite(5, LOW);
            lcd.clear();
          }

        } else {
          digitalWrite(ledPI, LOW);
          digitalWrite(ledPA, LOW);
          digitalWrite(ledM, HIGH);
          digitalWrite(ledA, LOW);
          lcd.setCursor(0, 0);
          lcd.print("Estado: Manual");
          lcd.setCursor(0, 2);
          lcd.print("Boton1: P.activo");
          lcd.setCursor(0, 3);
          lcd.print("Boton2: Automatico");

          if (button1_released) {
            button1_released = false;
            state = PARADA_ACTIVO;
            lcd.clear();
          }
          if (button2_released) {
            button2_released = false;
            state = AUTOMATICO;
            lcd.clear();
          }

          if (buttonAlert_released) {
            alertActivated = false;

          }
        }

        break;

      case AUTOMATICO:
        digitalWrite(ledPI, LOW);
        digitalWrite(ledPA, LOW);
        digitalWrite(ledM, LOW);
        digitalWrite(ledA, HIGH);
        lcd.setCursor(0, 0);
        lcd.print("Estado: Automatico");
        lcd.setCursor(0, 2);
        lcd.print("Boton1: Manual");

        if (button1_released) {
          button1_released = false;
          state = MANUAL;
          lcd.clear();
        }
        if (button2_released) {
          //button2_released = false;
        }
        if (alertActivated) {
          state = MANUAL;
          lcd.clear();
        }
        break;
    }

    vTaskDelayUntil(&xLastWakeTime, xPeriod);
  }
}

void setup() {
  // put your setup code here, to run once:

  lcd.init();
  lcd.backlight();

  Serial.begin(115200);
  lcd.begin(20, 4);
  lcd.clear();

  pinMode(ledPI, OUTPUT);
  pinMode(ledPA, OUTPUT);
  pinMode(ledM, OUTPUT);
  pinMode(ledA, OUTPUT);
  pinMode(ledAlerta, OUTPUT);


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

  attachInterrupt(digitalPinToInterrupt(button1), button1_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(button2), button2_ISR, FALLING);
  attachInterrupt(digitalPinToInterrupt(buttonAlert), buttonAlert_ISR, FALLING);




  xTaskCreatePinnedToCore(
    FSM_TREN,                 // Function en la que se implementa la tarea
    "FSM_TREN",                   // 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);

  xTaskCreatePinnedToCore(
    FSM_ALERTA,                 // Function en la que se implementa la tarea
    "FSM_ALERTA",                   // 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)
    2,                        // Prioridad de la tarea (cuanto más alto, más prioridad)
    &HandleTask2,             // Handle de la tarea 1
    0);

}

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