/*
  Author: Jose
  Date: 05/01/2026
  Description: práctica 6.2, enviando los datos por la pantalla TFT
*/

// ====================================
// Definición de librerías y variables
// ====================================

// Librerías
// Pantalla TFT
#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"

// IMU
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>

Adafruit_MPU6050 mpu;

#define TFT_DC 2
#define TFT_CS 5
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

#define BOTON_A_PIN 25 // Pulsador verde conectado al GPIO25
#define BOTON_B_PIN 26 // Pulsador amarillo conectado al GPIO26
#define BOTON_C_PIN 27 // Pulsador rojo conectado al GPIO27
#define LED_PIN 4      // Led rojo conectado al GPIO04

int boton_A_contador = 0;
int boton_C_contador = 0;

bool usar_datos_imu = false;
bool alarma_vibracion = false;

volatile bool boton_A_presionado = false;
volatile bool boton_B_presionado = false;
volatile bool boton_C_presionado = false;

bool imprimir_req_A = false;
bool imprimir_req_B = false;
bool imprimir_req_C = false;

// Handles de tareas
TaskHandle_t HandleTask1;             // Manejador de la tarea 1
TaskHandle_t HandleTask2;             // Manejador de la tarea 2
TaskHandle_t HandleTask3;             // Manejador de la tarea 3
TaskHandle_t HandleTask4;             // Manejador de la tarea 4

// Variables de la IMU
float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;

// ===============================
//         Interrupciones
// ===============================

void IRAM_ATTR IsrPulsacionBotonA(){
  boton_A_presionado = true;
}

void IRAM_ATTR IsrPulsacionBotonB(){
  boton_B_presionado = true;
}

void IRAM_ATTR IsrPulsacionBotonC(){
  boton_C_presionado = true;
}

// ===============================
//            Tareas
// ===============================

// Monitoriza la pulsación de los botones
void MonitorButtons(void *pvParameters)
{
  TickType_t xLastWakeTime;                      // Almacena el tiempo en ticks de la última vez que se ejecutó
  const TickType_t xPeriod = pdMS_TO_TICKS(10); // Periodo en ticks en lugar de ms (10ms). Por defecto, FreeRTOS establece 1tick = 1ms. 100Hz

  // Inicializa xLastWakeTime con la cuenta de ticks actual
  xLastWakeTime = xTaskGetTickCount();

  while (true)
  {
    if (boton_A_presionado)
    {
      boton_A_contador += 1;
      imprimir_req_A = true;
      boton_A_presionado = false;
      
    }
    if (boton_B_presionado)
    {
      usar_datos_imu = !usar_datos_imu;
      imprimir_req_B = true;
      boton_B_presionado = false;
      
    } 
    if (boton_C_presionado) 
    {
      boton_C_contador += 1;
      imprimir_req_C = true;
      boton_C_presionado = false;
    } 
    vTaskDelayUntil(&xLastWakeTime, xPeriod); // Espera hasta el siguiente ciclo. xLastWakeTime es pasado por referencia, se actualiza en cada ejecución
  }
}

// Lee la IMU y escribe en la pantalla (Serial)
void printIMUData(void *pvParameters)
{
  TickType_t xLastWakeTime;                      // Almacena el tiempo en ticks de la última vez que se ejecutó
  const TickType_t xPeriod = pdMS_TO_TICKS(40); // Periodo en ticks en lugar de ms (40ms). Por defecto, FreeRTOS establece 1tick = 1ms. 25Hz

  // Inicializa xLastWakeTime con la cuenta de ticks actual
  xLastWakeTime = xTaskGetTickCount();
  
  while (true)
  {
    // Leemos SIEMPRE los datos para actualizar las variables globales (para la alarma)
    // Usamos getEvent para leer Accel, Gyro y Temp a la vez (Más eficiente)
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);

    // Actualizamos las variables globales para que la tarea vibrationAlarm tenga datos frescos
    accX = a.acceleration.x;
    accY = a.acceleration.y;
    accZ = a.acceleration.z;

    gyroX = g.gyro.x;
    gyroY = g.gyro.y;
    gyroZ = g.gyro.z;

    // Solo imprimimos si el usuario lo ha activado con el Botón B
    if (usar_datos_imu) 
    {
      // Truco: Usamos (COLOR_TEXTO, COLOR_FONDO) para que al escribir el numero nuevo
      // se borre el anterior automáticamente y no parpadee la pantalla.
      
      // --- ZONA 1: CABECERA (ARRIBA) ---
      tft.setTextSize(2);

      // Tiempo
      tft.setCursor(10, 10); 
      tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);
      tft.print("t: "); 
      tft.print(millis());
      tft.print(" ms   "); // Espacios extra para limpiar residuos

      // Temperatura (Justo debajo del tiempo)
      tft.setCursor(10, 30);
      tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);
      tft.print("Tmp: "); 
      tft.print(temp.temperature);
      tft.print(" C");

      // --- ZONA 3: DATOS (ABAJO, dejando hueco para mensajes en medio) ---
      
      // Aceleración (Empezamos en Y=90)
      tft.setCursor(10, 90); 
      tft.setTextColor(ILI9341_RED, ILI9341_BLACK);
      tft.setTextSize(2);
      tft.println("ACEL (m/s^2):");

      tft.setCursor(20, 115);
      tft.print("X: "); tft.println(a.acceleration.x);
      
      tft.setCursor(20, 135);
      tft.print("Y: "); tft.println(a.acceleration.y);

      tft.setCursor(20, 155);
      tft.print("Z: "); tft.println(a.acceleration.z);

      // Giroscopio (Empezamos en Y=190)
      tft.setCursor(10, 190);
      tft.setTextColor(ILI9341_GREEN, ILI9341_BLACK);
      tft.println("GIRO (rad/s):");

      tft.setCursor(20, 215);
      tft.print("X: "); tft.println(g.gyro.x);

      tft.setCursor(20, 235);
      tft.print("Y: "); tft.println(g.gyro.y);

      tft.setCursor(20, 255);
      tft.print("Z: "); tft.println(g.gyro.z);

      // --- FIN BLOQUE PANTALLA TFT ---
    }
    vTaskDelayUntil(&xLastWakeTime, xPeriod); // Espera hasta el siguiente ciclo. xLastWakeTime es pasado por referencia, se actualiza en cada ejecución
  }
}

// Genera un patrón de vibración a modo de alarma al superar una aceleración de 2G
void vibrationAlarm(void *pvParameters)
{
  TickType_t xLastWakeTime;                      // Almacena el tiempo en ticks de la última vez que se ejecutó
  const TickType_t xPeriod = pdMS_TO_TICKS(200); // Periodo en ticks en lugar de ms (200ms). Por defecto, FreeRTOS establece 1tick = 1ms. 5Hz

  // Inicializa xLastWakeTime con la cuenta de ticks actual
  xLastWakeTime = xTaskGetTickCount();
  
  while (true)
  {
    // Calculamos la magnitud del vector aceleración
    // Restamos la gravedad (aprox 9.8) si queremos detectar movimiento puro, 
    // pero para detectar "golpes" o fuerza total > 2G (aprox 19.6 m/s^2), usamos la magnitud absoluta.
    // Ojo: El sensor devuelve m/s^2. 1G = 9.8m/s^2.

    
    if (sqrt(accX*accX + accY*accY + accZ*accZ) > 19.6)  // Si se supera una aceleración de 2G
    {
      alarma_vibracion = true;      
    }

    if (alarma_vibracion)
    {
      // Limpiamos la zona de mensajes y escribimos ALARMA
      tft.fillRect(0, 50, 240, 35, ILI9341_RED); // Fondo rojo para que se vea bien
      tft.setCursor(10, 55); 
      tft.setTextColor(ILI9341_WHITE); 
      tft.setTextSize(2);
      tft.println("! ALARMA !");

      for (uint8_t i = 0; i < 5; i++)
      {
        digitalWrite(LED_PIN, HIGH);  // Enciende el led
        vTaskDelay(200);
        digitalWrite(LED_PIN, LOW);  // Apaga el led
        vTaskDelay(200);
      }
      // Al terminar la alarma, borramos el mensaje rojo
      tft.fillRect(0, 50, 240, 35, ILI9341_BLACK);
      alarma_vibracion = false;            
    }
    vTaskDelayUntil(&xLastWakeTime, xPeriod); // Espera hasta el siguiente ciclo. xLastWakeTime es pasado por referencia, se actualiza en cada ejecución
  }
}

// Muestra los datos por pantalla
void writeSerialData(void *pvParameters)
{

  TickType_t xLastWakeTime;                      // Almacena el tiempo en ticks de la última vez que se ejecutó
  const TickType_t xPeriod = pdMS_TO_TICKS(100); // Periodo en ticks en lugar de ms (100ms). Por defecto, FreeRTOS establece 1tick = 1ms.

  // Inicializa xLastWakeTime con la cuenta de ticks actual
  xLastWakeTime = xTaskGetTickCount();

  while (true)
  {
    if (imprimir_req_A || imprimir_req_C || imprimir_req_B) {
      tft.fillRect(0, 50, 240, 35, ILI9341_BLACK); 
      // Configuramos cursor para todos
      tft.setCursor(10, 55); 
      tft.setTextColor(ILI9341_ORANGE); // Color diferente para mensajes
      tft.setTextSize(2);
    }

    if (imprimir_req_A)
    {
      tft.print("Btn A: ");
      tft.print(boton_A_contador);
      tft.println(" veces.");
      imprimir_req_A = false;
    }
    else if (imprimir_req_C)
    {
      tft.print("Btn C: ");
      tft.print(boton_C_contador);
      tft.println(" veces.");
      imprimir_req_C = false;
    }
    else if (imprimir_req_B)
    {
      if (usar_datos_imu)
      {
        tft.setTextColor(ILI9341_GREEN);
        tft.print("IMU: ACTIVADO");
      }
      else
      {
        // Si desactivamos la IMU, limpiamos la parte de abajo de la pantalla
        tft.fillRect(0, 90, 240, 230, ILI9341_BLACK);
        tft.setTextColor(ILI9341_RED);
        tft.print("IMU: APAGADO");
      }
      imprimir_req_B = false;
    }
    vTaskDelayUntil(&xLastWakeTime, xPeriod); // Espera hasta el siguiente ciclo. xLastWakeTime es pasado por referencia, se actualiza en cada ejecución
  }
}

// ===============================
//        Función setup()
// ===============================

void setup()
{
  Serial.begin(115200);
  tft.begin();

  // IMU
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  // Pines
  pinMode(LED_PIN, OUTPUT);
  pinMode(BOTON_A_PIN, INPUT_PULLUP);
  pinMode(BOTON_B_PIN, INPUT_PULLUP);
  pinMode(BOTON_C_PIN, INPUT_PULLUP);

  // Interrupciones
  attachInterrupt(digitalPinToInterrupt(BOTON_A_PIN), IsrPulsacionBotonA, RISING); // Asocia la interrupción con el pin del pulsador verde
  attachInterrupt(digitalPinToInterrupt(BOTON_B_PIN), IsrPulsacionBotonB, RISING); // Asocia la interrupción con el pin del pulsador amarillo
  attachInterrupt(digitalPinToInterrupt(BOTON_C_PIN), IsrPulsacionBotonC, RISING); // Asocia la interrupción con el pin del pulsador rojo


  // Tarea 1: Monotoriza pulsación de botones
  xTaskCreatePinnedToCore(
  MonitorButtons,                                           // Nombre de la función en la que se implementa la tarea
  "get button status",                                      // Descripción de la tarea
  4096/2,                                                     // 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. Sólo cuando haya varias tareas en un mismo core)
  &HandleTask1,                                        // Handle (manejador) de la tarea monitoriza botones.
  0);                                                       // Core donde va a correr la tarea 1 (Puede ser core 0 o core 1)

  // Tarea 2: Escribe los datos de la IMU en la pantalla del M5
  xTaskCreatePinnedToCore(
  printIMUData,                // Nombre de la función en la que se implementa la tarea
  "Escribe los datos de la IMU",             // Descripción de la tarea
  4096/2,                     // 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. Sólo cuando haya varias tareas en un mismo core)
  &HandleTask2,         // Handle (manejador) de la tarea Write.
  0);                       // Core donde va a correr la tarea 1 (Puede ser core 0 o core 1)

  // Tarea 3: 
  xTaskCreatePinnedToCore(
  vibrationAlarm,                // Nombre de la función en la que se implementa la tarea
  "vibration alarm",             // Descripción de la tarea
  4096/2,                    // Memoria (en Bytes) asignados a esta tarea
  NULL,                    // Parámetro de entrada de la tarea (no hay ningún parámetro)
  3,                       // Prioridad de la tarea (cuanto más alto, más prioridad. Sólo cuando haya varias tareas en un mismo core)
  &HandleTask3,         // Handle (manejador) de la tarea vibrator.
  0);                      // Core donde va a correr la tarea 1 (Puede ser core 0 o core 1)

  // Tarea WiFi
  xTaskCreatePinnedToCore(
  writeSerialData,                // Nombre de la función en la que se implementa la tarea
  "Send serial notifications",             // Descripción de la tarea
  4096/2,                    // Memoria (en Bytes) asignados a esta tarea
  NULL,                    // Parámetro de entrada de la tarea (no hay ningún parámetro)
  0,                       // Prioridad de la tarea (cuanto más alto, más prioridad. Sólo cuando haya varias tareas en un mismo core)
  &HandleTask4,            // Handle (manejador) de la tarea Puerto.
  0);                      // Core donde va a correr la tarea 1 (Puede ser core 0 o core 1)
}


// ===============================
//        Función loop()
// ===============================

void loop()
{

}