#include <Arduino.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

// Pines de la pantalla TFT
#define TFT_DC 0
#define TFT_CS 15
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

// Pines de botones y potenciómetro
#define PIN_BUTTON_DIS 17
#define PIN_BUTTON_DIS_COP 5
#define PIN_POTENTIOMETER 35

// Pines de LEDs
#define PIN_DIS_LED1 4
#define PIN_DIS_LED2 16
#define PIN_DIS_COP_LED1 25
#define PIN_DIS_COP_LED2 33
#define PIN_DIS_COP_LED3 32
#define PIN_DIS_COP_LED4 26

// Variables compartidas
volatile bool modeDIS = false;
volatile bool modeDIS_COP = false;
volatile float targetSpeed = 0; // Velocidad objetivo
volatile float currentSpeed = 0; // Velocidad actual

// Prototipos de funciones
void taskDIS(void *parameter);
void taskDIS_COP(void *parameter);
void taskUpdateDisplay(void *parameter);
void checkButtons(void *parameter);
int getDelayFromPotentiometer();
void drawDial();
void drawNeedle(float currentSpeed, float targetSpeed);

void setup() {
  // Configuración de pines
  pinMode(PIN_BUTTON_DIS, INPUT_PULLDOWN);
  pinMode(PIN_BUTTON_DIS_COP, INPUT_PULLDOWN);
  pinMode(PIN_DIS_LED1, OUTPUT);
  pinMode(PIN_DIS_LED2, OUTPUT);
  pinMode(PIN_DIS_COP_LED1, OUTPUT);
  pinMode(PIN_DIS_COP_LED2, OUTPUT);
  pinMode(PIN_DIS_COP_LED3, OUTPUT);
  pinMode(PIN_DIS_COP_LED4, OUTPUT);

  // Configuración de la pantalla TFT
  tft.begin();
  tft.setRotation(1);
  drawDial(); // Mostrar el velocímetro al iniciar

  // Crear tareas FreeRTOS
  xTaskCreate(taskDIS, "Task DIS", 1024, NULL, 1, NULL);
  xTaskCreate(taskDIS_COP, "Task DIS COP", 1024, NULL, 1, NULL);
  xTaskCreate(taskUpdateDisplay, "Task Update Display", 2048, NULL, 1, NULL);
  xTaskCreate(checkButtons, "Check Buttons", 1024, NULL, 2, NULL);
}

void loop() {
  // El loop principal queda vacío porque usamos FreeRTOS
}

// Tarea para manejar el modo DIS
void taskDIS(void *parameter) {
  while (true) {
    if (modeDIS) {
      int delayTime = getDelayFromPotentiometer();
      targetSpeed = map(delayTime, 100, 2000, 240, 0);

      digitalWrite(PIN_DIS_LED1, HIGH);
      digitalWrite(PIN_DIS_LED2, LOW);
      vTaskDelay(pdMS_TO_TICKS(delayTime));

      digitalWrite(PIN_DIS_LED1, LOW);
      digitalWrite(PIN_DIS_LED2, HIGH);
      vTaskDelay(pdMS_TO_TICKS(delayTime));
    } else {
      digitalWrite(PIN_DIS_LED1, LOW);
      digitalWrite(PIN_DIS_LED2, LOW);
      vTaskDelay(pdMS_TO_TICKS(100));
    }
  }
}

// Tarea para manejar el modo DIS COP
void taskDIS_COP(void *parameter) {
  int sequence[] = {PIN_DIS_COP_LED1, PIN_DIS_COP_LED3, PIN_DIS_COP_LED4, PIN_DIS_COP_LED2};
  while (true) {
    if (modeDIS_COP) {
      int delayTime = getDelayFromPotentiometer();
      targetSpeed = map(delayTime, 100, 2000, 240, 0);

      for (int i = 0; i < 4; i++) {
        digitalWrite(sequence[i], HIGH);
        vTaskDelay(pdMS_TO_TICKS(delayTime));
        digitalWrite(sequence[i], LOW);
      }
    } else {
      for (int i = 0; i < 4; i++) {
        digitalWrite(sequence[i], LOW);
      }
      vTaskDelay(pdMS_TO_TICKS(100));
    }
  }
}

// Tarea para actualizar la pantalla TFT
void taskUpdateDisplay(void *parameter) {
  while (true) {
    // Si ninguno de los modos está activo, establecer la velocidad objetivo en 0
    if (!modeDIS && !modeDIS_COP) {
      targetSpeed = 0;
    }

    // Interpolar suavemente entre la velocidad actual y la velocidad objetivo
    currentSpeed += (targetSpeed - currentSpeed) * 0.1;
    drawNeedle(currentSpeed, targetSpeed);

    vTaskDelay(pdMS_TO_TICKS(20)); // Actualización cada 20ms
  }
}

// Tarea para verificar los botones
void checkButtons(void *parameter) {
  static bool lastStateDIS = false;
  static bool lastStateDIS_COP = false;

  while (true) {
    bool currentStateDIS = digitalRead(PIN_BUTTON_DIS);
    bool currentStateDIS_COP = digitalRead(PIN_BUTTON_DIS_COP);

    if (currentStateDIS && !lastStateDIS && !modeDIS_COP) {
      modeDIS = !modeDIS;
    }

    if (currentStateDIS_COP && !lastStateDIS_COP && !modeDIS) {
      modeDIS_COP = !modeDIS_COP;
    }

    lastStateDIS = currentStateDIS;
    lastStateDIS_COP = currentStateDIS_COP;

    vTaskDelay(pdMS_TO_TICKS(50));
  }
}

// Función para obtener el tiempo de retraso del potenciómetro
int getDelayFromPotentiometer() {
  int analogValue = analogRead(PIN_POTENTIOMETER);
  return map(analogValue, 0, 4095, 100, 2000);
}

// Dibujar el dial en la pantalla TFT con rango máximo de 8 (x1000 RPM)
void drawDial() {
  tft.fillScreen(ILI9341_BLACK);

  // Círculos principales
  tft.drawCircle(160, 120, 100, ILI9341_WHITE);
  tft.drawCircle(160, 120, 95, ILI9341_WHITE);

  // Dibujar marcas principales y secundarias
  for (int i = 0; i <= 32; i++) { // Dividir hasta 8 en incrementos de 0.25
    float angle = map(i, 0, 32, -225, 45) * 3.14159 / 180; // Ajustar ángulo
    int x1 = 160 + cos(angle) * 90;
    int y1 = 120 + sin(angle) * 90;
    int x2 = 160 + cos(angle) * (i % 4 == 0 ? 80 : 85); // Marcas largas cada 1 unidad
    int y2 = 120 + sin(angle) * (i % 4 == 0 ? 80 : 85);
    tft.drawLine(x1, y1, x2, y2, ILI9341_WHITE);

    // Solo números en las marcas principales (cada 1 unidad)
    if (i % 4 == 0) {
      int textX = 160 + cos(angle) * 65 - 5;
      int textY = 120 + sin(angle) * 65 - 5;
      tft.setCursor(textX, textY);
      tft.setTextColor(ILI9341_WHITE);
      tft.setTextSize(1);
      tft.print(i / 4); // Mostrar valores de 0 a 8
    }
  }

  // Dibujar la zona roja (máximo)
  for (int i = 28; i <= 32; i++) { // Resaltar zona roja de 7 a 8
    float angle = map(i, 0, 32, -225, 45) * 3.14159 / 180;
    int x1 = 160 + cos(angle) * 90;
    int y1 = 120 + sin(angle) * 90;
    int x2 = 160 + cos(angle) * 80;
    int y2 = 120 + sin(angle) * 80;
    tft.drawLine(x1, y1, x2, y2, ILI9341_RED);
  }

  // Título de RPM
  tft.setCursor(140, 160);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print("RPM");
}

// Dibujar la aguja del velocímetro
void drawNeedle(float currentSpeed, float targetSpeed) {
  static float prevAngle = -255;

  // Calcular el ángulo actual
  float angle = map(currentSpeed, 0, 8000, -225, 45); // Rango ajustado hasta 8000 RPM
  float radian = angle * 3.14159 / 180;

  // Borrar la aguja anterior
  float prevRadian = prevAngle * 3.14159 / 180;
  int xPrev = 160 + cos(prevRadian) * 70;
  int yPrev = 120 + sin(prevRadian) * 70;
  tft.drawLine(160, 120, xPrev, yPrev, ILI9341_BLACK);

  // Dibujar la nueva aguja
  int x = 160 + cos(radian) * 70;
  int y = 120 + sin(radian) * 70;
  tft.drawLine(160, 120, x, y, ILI9341_ORANGE);

  prevAngle = angle;
}