#include <Arduino.h>
#include <Wire.h>
#include "driver/dac.h"
#include <math.h>
#include <Adafruit_PCF8574.h>
#include "esp32-hal-timer.h"
#include "SingleEMAFilterLib.h"
#include <LiquidCrystal_I2C.h>
#include <BigNumbers_I2C.h>
#include <TinyGPSPlus.h>
#include <HardwareSerial.h>
#include <Adafruit_NeoPixel.h>

// InicializaciÃ³n de PCF8574 y filtro
Adafruit_PCF8574 AuxPCF;
Adafruit_PCF8574 MotorPCF;
SingleEMAFilter<int> AcceleratorFilter(0.1);
SingleEMAFilter<float> BatVoltageFilter(0.2);

#define NUMPIXELS 16

// Token de acceso (placeholder)
#define ACCESS_TOKEN 23412312312312312222211

HardwareSerial SerialGPS(1);
TinyGPSPlus gps;

byte customChar[8][8] = {
  { 0b00000, 0b00000, 0b00000, 0b00000, 0b00000, 0b00000, 0b00000, 0b00000 },  // 0/8
  { 0b10000, 0b10000, 0b10000, 0b10000, 0b10000, 0b10000, 0b10000, 0b10000 },  // 1/8
  { 0b11000, 0b11000, 0b11000, 0b11000, 0b11000, 0b11000, 0b11000, 0b11000 },  // 2/8
  { 0b11100, 0b11100, 0b11100, 0b11100, 0b11100, 0b11100, 0b11100, 0b11100 },  // 3/8
  { 0b11110, 0b11110, 0b11110, 0b11110, 0b11110, 0b11110, 0b11110, 0b11110 },  // 4/8
  { 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111 },  // 5/8
  { 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111 },  // 6/8
  { 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111, 0b11111 }   // 7/8
};

// Definiciones de pines
#define TACHOMETER1_INPUT 23        // IO23 - Tachometer 1 Input
#define I2C_SCL 22                  // IO22 - i2C SCL
#define UART_TX0 1                  // TXD0 -
#define UART_RX0 3                  // RXD0 -
#define I2C_SDA 21                  // IO21 - i2C SDA
#define ENCODER_CLOCK 19            // IO19 - Encoder Clock
#define BLINKER_LEFT 18             // IO18 - Encoder Data
#define BLINKER_RIGHT 5             // IO5  - Encoder Switch
#define UART_TX2 17                 // IO17 - Serial Bus B (TX)
#define UART_RX2 16                 // IO16 - Serial Bus B (RX)
#define VOLTAGE_MONITOR_3V3 4       // IO4  - 3V3 Voltage Monitor
#define ENABLE_PIN 0                // IO0  - Enable Pin
#define MOTOR_A_PWM 15              // IO15 - PWM Signal Motor A
#define MOTOR_B_PWM 14              // IO13 - PWM Signal Motor B
#define BATTERY_VOLTAGE_MONITOR 34  // IO34 - Battery Voltage Monitor
#define VOLTAGE_MONITOR_5V 35       // IO35 - 5V Voltage Monitor
#define REGULATOR_CONTROL_3V3 32    // IO32 - 3V3 Voltage Regulator Control
#define TACHOMETER2_INPUT 33        // IO33 - Tachometer 2 Input
#define DAC_OUTPUT 25               // IO25 - DAC
#define DAC_CHANNEL DAC_CHANNEL_1
#define REGULATOR_CONTROL_5V 26  // IO26 - 5V Voltage Regulator Control
#define NEOPIXEL_SIGNAL 27       // IO27 - Neopixel Signal Output
#define FAN_PWM 13               // IO14 - Fan PWM Signal
#define THROTTLE_SIGNAL 12       // IO12 - Throttle Signal

// Definiciones de pines del PCF8574
#define ENABLE_BRIDGE1 4  // P4 - Enable Bridge 1
#define ENABLE_BRIDGE2 1  // P1 - Enable Bridge 2
#define BRAKE_SIGNAL 1

#define KEY_1 2
#define KEY_2 3
#define KEY_3 4
#define KEY_4 5

// CÃ³digo de error
#define PCF_ERROR 101

// InicializaciÃ³n de la pantalla LCD I2C en la direcciÃ³n 0x27
LiquidCrystal_I2C lcd(0x27, 20, 4);
BigNumbers_I2C bigNum(&lcd);
Adafruit_NeoPixel pixels(NUMPIXELS, NEOPIXEL_SIGNAL, NEO_GRB + NEO_KHZ800);

// Variables de configuraciÃ³n
const long NormalReadings = 50;
float WheelDiameter = 0.25;
float WheelCircumference = 3.1415 * WheelDiameter;

const int PWM_FREQ = 440;      // Frecuencia del PWM
const int PWM_RESOLUTION = 8;  // ResoluciÃ³n del PWM (8 bits)
const uint32_t GPSBaud = 9600;
int TimezoneOffset = -6;
const unsigned long debounceDelay = 20;

unsigned long previousMillisNeo = 0;  // Almacena el tiempo en el que ocurriÃ³ el Ãºltimo parpadeo
const long intervalNeo = 400;         // Intervalo de parpadeo en milisegundos
bool pixelsOn = false;


// Variables funcionales
int Speed = 0;
int TachoRPM = 0;
int TachoSpeed = 0;
int ThrottleRead = 0;
int Throttle = 0;
int ThrottlePercent = 0;
int FilterThrottle = 0;
float BatteryVoltage = 0;
float totalDistance = 0;
int MenuSelect = 0;
int MaxThrottle = 255;
bool EcoMode = false;

bool LatchState1 = false;
bool LatchState2 = false;

volatile int Pulse = 0;
volatile unsigned long lastDebounceTime = 0;
volatile long totalRevolutions = 0;

bool BrakePressed = false;
bool TachometerApulse = false;

// Variables de alerta
int DiagnosticError = 0;

// Timers
hw_timer_t *timer1 = NULL;
hw_timer_t *timer2 = NULL;
volatile bool flag1 = false;
volatile bool flag2 = false;

//GPS Variables
int ClockHour = 0;
int ClockMinute = 0;
int ClockSecond = 0;

int GPSYear = 0;
int GPSMonth = 0;
int GPSDay = 0;

float GPSlatitude = 0;
float GPSlongitude = 0;

int GPSspeed = 0;

bool BlinkerLeft = false;
bool BlinkerRight = false;
bool WarningLights = false;

bool lastButtonState = LOW;
bool latchState = LOW;
unsigned long lastDebounceTime2 = 0;
const unsigned long debounceDelay2 = 50;

// FunciÃ³n de interrupciÃ³n para el primer timer
void IRAM_ATTR onTimer1() {
  flag1 = true;
}

// FunciÃ³n de interrupciÃ³n para el segundo timer
void IRAM_ATTR onTimer2() {
  flag2 = true;
}

void IRAM_ATTR handleTachometerInterrupt() {
  unsigned long currentTime = millis();
  if ((currentTime - lastDebounceTime) > debounceDelay) {
    Pulse++;
    lastDebounceTime = currentTime;
  }
}

// ConfiguraciÃ³n inicial
void setup() {
  Serial.begin(115200);
  SerialGPS.begin(GPSBaud, SERIAL_8N1, UART_RX2, UART_TX2);

  Wire.begin();
  pixels.begin();
  pixels.show();

  // Inicializar la pantalla LCD
  lcd.init();
  lcd.backlight();
  bigNum.begin();

  // Escaneo del bus I2C
  scanI2C();
  if (!AuxPCF.begin(0x26, &Wire)) {
    Serial.println("Error inicializando PCF8574 Aux");
    DiagnosticError = PCF_ERROR;
  }
  if (!MotorPCF.begin(0x21, &Wire)) {
    Serial.println("Error inicializando PCF8574 Motor");
    DiagnosticError = PCF_ERROR;
  }

  AuxPCF.pinMode(BRAKE_SIGNAL, INPUT_PULLUP);
  AuxPCF.pinMode(KEY_1, INPUT_PULLUP);
  AuxPCF.pinMode(KEY_2, INPUT_PULLUP);
  AuxPCF.pinMode(KEY_3, INPUT_PULLUP);
  AuxPCF.pinMode(KEY_4, INPUT_PULLUP);
  MotorPCF.pinMode(ENABLE_BRIDGE1, OUTPUT);
  MotorPCF.pinMode(ENABLE_BRIDGE2, OUTPUT);
  MotorPCF.digitalWrite(ENABLE_BRIDGE1, LOW);
  MotorPCF.digitalWrite(ENABLE_BRIDGE2, LOW);

  pinMode(TACHOMETER1_INPUT, INPUT_PULLDOWN);
  pinMode(TACHOMETER2_INPUT, INPUT_PULLDOWN);
  pinMode(BLINKER_LEFT, INPUT_PULLDOWN);
  pinMode(BLINKER_RIGHT, INPUT_PULLDOWN);
  attachInterrupt(digitalPinToInterrupt(TACHOMETER2_INPUT), handleTachometerInterrupt, RISING);

  ledcSetup(0, PWM_FREQ, PWM_RESOLUTION);
  ledcAttachPin(MOTOR_A_PWM, 0);

  ledcSetup(1, PWM_FREQ, PWM_RESOLUTION);
  ledcAttachPin(MOTOR_B_PWM, 1);

  delay(2000);

  if (DiagnosticError > 0) {
    Serial.println("System Error");
    while (1)
      ;
  }

  Serial.println("All Systems OK");
  generateTone(DAC_CHANNEL, 440, 1000);
  delay(1000);  // Pausa de 1 segundo

  // ConfiguraciÃ³n del primer timer
  timer1 = timerBegin(0, 80, true);
  timerAttachInterrupt(timer1, &onTimer1, true);
  timerAlarmWrite(timer1, 50000, true);  // 50 ms
  timerAlarmEnable(timer1);

  // ConfiguraciÃ³n del segundo timer
  timer2 = timerBegin(1, 80, true);
  timerAttachInterrupt(timer2, &onTimer2, true);
  timerAlarmWrite(timer2, 200000, true);  // 200ms
  timerAlarmEnable(timer2);
}

// Bucle principal
void loop() {
  BlinkerLeft = digitalRead(BLINKER_LEFT);
  BlinkerRight = digitalRead(BLINKER_RIGHT);

  if (flag1) {
    flag1 = false;
    ReadThrottle();
    BatteryVoltage = getBatVoltage();

    // Sistema de frenos
    BrakePressed = AuxPCF.digitalRead(BRAKE_SIGNAL);
    ledcWrite(0, Throttle);  // Motor A
    ledcWrite(1, Throttle);  // Motor B
    ReadGPS();
  }

  if (flag2) {
    flag2 = false;
    MeasureSpeed();
    UpdateScreen();
  }

  if (BlinkerRight == true && BlinkerLeft == false && BrakePressed == true && WarningLights == false) {
    blinkNeoPixels(255, 50, 0, 8, 16);
  } else if (BlinkerRight == false && BlinkerLeft == true && BrakePressed == true && WarningLights == false) {
    blinkNeoPixels(255, 50, 0, 0, 7);
  } else if (BrakePressed == false && BlinkerLeft == false && BlinkerRight == false && WarningLights == false) {
    lightUpRed();
  } else if (BrakePressed == true && BlinkerLeft == false && BlinkerRight == false && WarningLights == true) {
    blinkNeoPixels(255, 0, 0, 0, 16);
  } else if (BrakePressed == true && BlinkerRight == false && BlinkerLeft == false && WarningLights == false) {
    turnOffPixels();
  }

  if (AuxPCF.digitalRead(KEY_1) == false) {
    generateTone(DAC_CHANNEL, 5000, 50);
    lcd.clear();
    MenuSelect++;
    delay(200);
    if (MenuSelect > 1) {
      MenuSelect = 0;
    }
  }

  else if (AuxPCF.digitalRead(KEY_2) == false) {
    generateTone(DAC_CHANNEL, 5000, 50);
    LatchState1 = !LatchState1;
    WarningLights = LatchState1;
    delay(200);
  }

  else if (AuxPCF.digitalRead(KEY_3) == false) {
    generateTone(DAC_CHANNEL, 5000, 50);
    LatchState2 = !LatchState2;
    EcoMode = LatchState2;
    delay(200);
  }

  else if (AuxPCF.digitalRead(KEY_4) == false) {
    generateTone(DAC_CHANNEL, 5000, 50);
    LatchState2 = !LatchState2;
    EcoMode = LatchState2;
    delay(200);
  }
}

// Escaneo del bus I2C
void scanI2C() {
  byte error, address;
  int nDevices;

  Serial.println("Escaneando el bus I2C...");

  nDevices = 0;
  for (address = 1; address < 127; address++) {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0) {
      Serial.print("Dispositivo I2C encontrado en la direcciÃ³n 0x");
      if (address < 16)
        Serial.print("0");
      Serial.print(address, HEX);
      Serial.println(" !");

      nDevices++;
    } else if (error == 4) {
      Serial.print("Error desconocido en la direcciÃ³n 0x");
      if (address < 16)
        Serial.print("0");
      Serial.println(address, HEX);
    }
  }
  if (nDevices == 0)
    Serial.println("No se encontraron dispositivos I2C\n");
  else
    Serial.println("Escaneo I2C completado\n");
}

// Lectura del acelerador
void ReadThrottle() {
  if (EcoMode) {
    MaxThrottle = 150;
  } else {
    MaxThrottle = 255;
  }

  ThrottleRead = analogRead(THROTTLE_SIGNAL);
  AcceleratorFilter.AddValue(ThrottleRead);
  FilterThrottle = AcceleratorFilter.GetLowPass();
  int PreThrottle = map(FilterThrottle, 2960, 2680, 0, MaxThrottle);
  Throttle = constrain(PreThrottle, 0, MaxThrottle);
  float DivThrottle = constrain(PreThrottle, 0, MaxThrottle);
  ThrottlePercent = round((DivThrottle / 255) * 100);

  if (BrakePressed == false || ThrottlePercent == 0) {
    MotorPCF.digitalWrite(ENABLE_BRIDGE1, 0);
    MotorPCF.digitalWrite(ENABLE_BRIDGE2, 0);
  } else {
    MotorPCF.digitalWrite(ENABLE_BRIDGE1, 1);
    MotorPCF.digitalWrite(ENABLE_BRIDGE2, 1);
  }
}

// Generar tono con el DAC
void generateTone(dac_channel_t channel, int freq, int duration) {
  dac_output_enable(channel);  // Habilitar la salida del DAC

  int period = 1000000 / freq;  // Periodo en microsegundos
  int halfPeriod = period / 2;
  int cycles = (duration * 1000) / period;

  for (int i = 0; i < cycles; i++) {
    dac_output_voltage(channel, 255);  // Salida mÃ¡xima
    delayMicroseconds(halfPeriod);
    dac_output_voltage(channel, 0);  // Salida mÃ­nima
    delayMicroseconds(halfPeriod);
  }

  dac_output_disable(channel);  // Deshabilitar la salida del DAC
}

float getBatVoltage() {
  float Reading = analogRead(BATTERY_VOLTAGE_MONITOR);
  BatVoltageFilter.AddValue((Reading / 4095) * 33.33);
  float Voltage = BatVoltageFilter.GetLowPass();
  return (Voltage);
}

void UpdateScreen() {
  switch (MenuSelect) {
    case 0:
      //Battery Info
      lcd.setCursor(0, 0);
      lcd.print(BatteryVoltage);
      lcd.print("V ");
      lcd.print("    ");

      //Throttle Info
      lcd.setCursor(15, 3);
      lcd.print(ThrottlePercent);
      lcd.print("% ");
      drawProgressBar(3, 0, 14, ThrottlePercent, 100, 0);

      //Speedometer
      bigNum.displayLargeInt(TachoSpeed, 7, 0, 4, false);
      lcd.setCursor(15, 2);
      lcd.print("km/h");

      //Podometer
      lcd.setCursor(0, 1);
      lcd.print(totalDistance / 1000);
      lcd.print("km  ");

      //Tachometer
      lcd.setCursor(0, 2);
      lcd.print(TachoRPM);
      lcd.print("RPM  ");

      //Modes
      if (EcoMode) {
        lcd.setCursor(9, 2);
        lcd.print("ECO");
      } else {
        lcd.setCursor(9, 2);
        lcd.print("   ");
      }


      break;
    case 1:
      //Speedometer
      bigNum.displayLargeInt(TachoSpeed, 7, 0, 4, false);
      lcd.setCursor(15, 2);
      lcd.print("km/h");

      //GPS Info
      lcd.setCursor(0, 2);
      lcd.print("Lat: ");
      lcd.print(GPSlatitude);
      lcd.print(" ");
      lcd.setCursor(0, 3);
      lcd.print("Lng: ");
      lcd.print(GPSlongitude);
      lcd.print(" ");


      //Clock
      lcd.setCursor(0, 1);
      if (ClockHour < 10) {
        lcd.print("0");
        lcd.print(ClockHour);
      } else if (ClockHour >= 10) {
        lcd.print(ClockHour);
      }
      lcd.print(":");
      if (ClockMinute < 10) {
        lcd.print("0");
        lcd.print(ClockMinute);
      } else if (ClockMinute >= 10) {
        lcd.print(ClockMinute);
      }
      break;
  }
}

void ReadGPS() {
  while (SerialGPS.available() > 0) {
    char c = SerialGPS.read();
    gps.encode(c);  // Decodificar los datos recibidos del GPS

    if (gps.location.isUpdated()) {
      GPSlatitude = gps.location.lat();
      GPSlongitude = gps.location.lng();
    }

    if (gps.date.isUpdated()) {
      GPSYear = gps.date.year();
      GPSMonth = gps.date.month();
      GPSDay = gps.date.day();
    }

    if (gps.time.isUpdated()) {
      ClockHour = gps.time.hour() + TimezoneOffset;
      if (ClockHour < 0) {
        ClockHour += 24;  // Ajuste para horas negativas
        // AquÃ­ puedes ajustar tambiÃ©n la fecha si lo deseas
      } else if (ClockHour >= 24) {
        ClockHour -= 24;  // Ajuste para horas mayores a 23
        // AquÃ­ puedes ajustar tambiÃ©n la fecha si lo deseas
      }
      ClockMinute = gps.time.minute();
      ClockSecond = gps.time.second();
    }
    if (gps.speed.isUpdated()) {
      GPSspeed = gps.speed.kmph();
    }
  }
}

void MeasureSpeed() {
  static unsigned long previousTime = 0;
  unsigned long currentTime = millis();

  // Medir el tiempo transcurrido y calcular la velocidad cada segundo
  if ((currentTime - previousTime) >= 1000) {
    noInterrupts();  // Desactivar interrupciones temporalmente para leer Pulse de forma segura
    int pulseCount = Pulse;
    Pulse = 0;     // Reiniciar el contador de pulsos
    interrupts();  // Reactivar interrupciones

    // CÃ¡lculo de RPM y velocidad teniendo en cuenta 5 pulsos por revoluciÃ³n
    TachoRPM = (pulseCount * 60.0) / 5;
    TachoSpeed = ((pulseCount / 5.0) * WheelCircumference) * 3.6;

    // Calcular el nÃºmero de revoluciones en el Ãºltimo segundo y acumularlas
    long revolutions = pulseCount / 5;
    totalRevolutions += revolutions;  // Acumular las revoluciones totales

    // Calcular la distancia total recorrida
    float totalDistance = totalRevolutions * WheelCircumference;  // Distancia total recorrida en metros
    previousTime = currentTime;
  }
}

void drawProgressBar(int row, int col, int maxLength, int value, int minValue, int maxValue) {
  // Calcular el porcentaje de llenado
  int percent = map(value, minValue, maxValue, 0, 100);

  // Calcular el nÃºmero de bloques completos y el nivel del bloque parcial
  int numBlocks = (percent * maxLength) / 100;
  int partialBlock = ((percent * maxLength) % 100) / 12.5;

  // Mover el cursor a la posiciÃ³n de inicio
  lcd.setCursor(col, row);

  // Dibujar bloques completos
  for (int i = 0; i < numBlocks; i++) {
    lcd.write(byte(5));
  }

  // Dibujar el bloque parcial si hay espacio
  if (numBlocks < maxLength) {
    lcd.write(byte(partialBlock));
    numBlocks++;
  }

  // Rellenar con bloques vacÃ­os si es necesario
  for (int i = numBlocks; i < maxLength; i++) {
    lcd.write(byte(0));
  }
}

void lightUpRed() {
  for (int i = 0; i < 16; i++) {
    pixels.setPixelColor(i, pixels.Color(255, 0, 0));  // Rojo
  }
  pixels.show();  // Actualizar la tira de NeoPixel para mostrar los cambios
}

void turnOffPixels() {
  for (int i = 0; i < NUMPIXELS; i++) {
    pixels.setPixelColor(i, pixels.Color(0, 0, 0));  // Apagado (Negro)
  }
  pixels.show();  // Actualizar la tira de NeoPixel para mostrar los cambios
}

void blinkNeoPixels(uint8_t red, uint8_t green, uint8_t blue, int startPixel, int endPixel) {
  unsigned long currentMillis = millis();
  // Verificar si ha pasado el tiempo de intervalo
  if (currentMillis - previousMillisNeo >= intervalNeo) {
    // Guardar el tiempo actual
    previousMillisNeo = currentMillis;

    // Cambiar el estado de los pÃ­xeles
    if (pixelsOn) {
      for (int i = startPixel; i <= endPixel; i++) {
        pixels.setPixelColor(i, pixels.Color(0, 0, 0));  // Apagar pÃ­xel
      }
      generateTone(DAC_CHANNEL, 2000, 5);
      lcd.setCursor(7, 2);
      lcd.print("  ");
      lcd.setCursor(12, 2);
      lcd.print("  ");
      pixelsOn = false;
    } else {
      for (int i = startPixel; i <= endPixel; i++) {
        pixels.setPixelColor(i, pixels.Color(red, green, blue));  // Encender pÃ­xel con color especificado
      }
      pixelsOn = true;
      generateTone(DAC_CHANNEL, 2500, 5);
      if (BlinkerLeft) {
        lcd.setCursor(7, 2);
        lcd.print("<-");
      } else if (BlinkerRight) {
        lcd.setCursor(12, 2);
        lcd.print("->");
      }else if (WarningLights) {
        lcd.setCursor(12, 2);
        lcd.print("->");
        lcd.setCursor(7, 2);
        lcd.print("<-");
      }
    }
    pixels.show();  // Actualizar la tira de NeoPixel para mostrar los cambios
  }
}