#include "TinyGPSPlus.h"
#include "DHT.h"
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include "Arduino.h"
#include "math.h"
#include "WiFi.h"
#include "WebServer.h"
#include "esp_pm.h"
#include "esp_sleep.h"

/*
Uncomment and set up if you want to use custom pins for the SPI communication
#define REASSIGN_PINS
int sck = -1;
int miso = -1;
int mosi = -1;
int cs = -1;
*/

// Inicia I2C en pines default ESP32 (21 SDA, 22 SCL)
#define SCREEN_WIDTH 128  // Ancho píxeles
#define SCREEN_HEIGHT 64  // Alto píxeles
#define OLED_RESET -1     // Reset pin (no usado)

// Pines para UART2 (Serial2)
#define RX_PIN 16  // RX del ESP32 conectado a TX del GPS
#define TX_PIN 17  // TX del ESP32 conectado a RX del GPS
#define GPS_BAUD 115200

#define DHTPIN 4     // Digital pin connected to the DHT sensor
#define DHTTYPE DHT22 

#define BUTTON_PIN 27  // pin para pulsador

//definicion de tiempos de pulsacion
#define PULASCION_LARGA_MS 2000
#define DURACION_WATCHDOG_MS 10000

#define MEASUREMENT_INTERVAL_S 1    //separación entre mediciones (s) 

#define DEG2RAD (M_PI/180.0)


// Objeto TinyGPS++
TinyGPSPlus gps;
HardwareSerial gpsSerial(2); // Usar UART2
DHT dht(DHTPIN, DHTTYPE);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//valores de los sensores

struct SensorData {
  double latitude = 0.0;
  double longitude = 0.0;
  float altura = 0.0;
  String tiempo = "";
  float velocidad = 0.0;
  float temperature = 0.0;
  float humidity = 0.0;
  float pressure = 0.0;
};

SensorData latestData;
SensorData datosAntiguos;

SemaphoreHandle_t dataMutex; // Mutex para proteger el acceso a latestData
SemaphoreHandle_t buttonSemaphore; // Semáforo para la tarea del botón

WebServer server(80);

bool wifiActivado = false;
unsigned long wifiLastActivity = 0;
const unsigned long WIFI_TIMEOUT_MS = 300000; // 5 minutos
const char* apSSID = "ESP32_GPS_Logger";
const char* apPassword = "12345678";
bool grabando = false; //inicia apagado
bool finalizado = true; //indica que no hay ninguna grabacion ni iniciada ni pausada
TaskHandle_t medicionesHandle = NULL; //para suspend/resume
int pantallaEstado_grab = -1; //maquina de estados cuando se graba ruta
int pantallaEstado_menu = -1; //maquina de estados cuando no se esta grabando ruta
float distancia_total = 0.0;
volatile unsigned long ignore_isr_until = 0; //para debounce

char filename[13] = "/panchas.gpx";

void OLED_print(const String& line1, const String& line2) {
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(line1);
  display.println(line2);
  display.display();
}

void DHT_test() {
  dht.begin();
  float h = dht.readHumidity();
  float t = dht.readTemperature();
  if (isnan(h) || isnan(t)) {
    Serial.println("Failed to read from DHT sensor!");
    OLED_print("DHT22", "Error");
    delay(5000);
    DHT_test(); // Reintentar
  }
  else OLED_print("DHT22", "Correcto");
}

void OLED_test() {  //pantallazo a blanco y luego iniciando
  // Inicia I2C en pines default ESP32 (21 SDA, 22 SCL)
  Wire.begin();
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {  // Dirección común: 0x3C
    Serial.println(F("Error: OLED no encontrado!"));
    for (;;);  // Para siempre
  }
  display.clearDisplay();
  display.fillScreen(SSD1306_WHITE); // Pantalla blanca
  delay(500);
  display.display();
  display.clearDisplay();
  display.setTextSize(2);         // Tamaño texto
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);        // Posición
  display.println("Iniciando...");
  display.display();              // Muestra
  delay(1000);
}


void SD_test(){
if (!SD.begin()) {
    OLED_print("SD Card", "Error\nInserte");
    while (!SD.begin());
    OLED_print("SD Card", "Insertada");
  } else {
    OLED_print("SD Card", "Correcto");
  }
  uint8_t cardType = SD.cardType();
  if (cardType == CARD_NONE) {
    OLED_print("SD Card", "No detectada");
    while (cardType == CARD_NONE) {
      delay(1000);
      cardType = SD.cardType();
    }
    OLED_print("SD Card", "Detectada");
  }
  uint8_t cardSize = SD.cardSize() / (1024 * 1024);
}

void GPS_test_wait() {
  // Iniciar Serial2 para GPS
  bool fixObtained = false;
  gpsSerial.begin(GPS_BAUD, SERIAL_8N1, RX_PIN, TX_PIN);
  while (!fixObtained) {
    while (gpsSerial.available() > 0) {
      if (gps.encode(gpsSerial.read())) {  // Procesa si hay sentencia NMEA completa
        if (gps.location.isValid() && gps.date.isValid() && gps.time.isValid()) {
          fixObtained = true;
          break;
        }
      }
    }
    delay(300);
    OLED_print("GPS", "Esperando");
    delay(300);
    OLED_print("GPS", "Esperando .");
    delay(300);
    OLED_print("GPS", "Esperando ..");
    delay(300);
    OLED_print("GPS", "Esperando ...");
  }
  OLED_print("GPS", "Encontrado");
}

float calcular_delta_dist(float lat1, float  long1, float lat2, float long2){
  float R = 6371.0; // Radio de la Tierra en km
  float delta_lat = (lat2 - lat1) * DEG2RAD;
  float delta_long = (long2 - long1) * DEG2RAD;
  lat1 = lat1 * DEG2RAD;
  lat2 = lat2 * DEG2RAD;
  float a = sin(delta_lat/2)*sin(delta_lat/2)+cos(lat1)*cos(lat2)*sin(delta_long/2)*sin(delta_long/2);
  float c = 2 * atan2(sqrt(a),sqrt(1-a));
  return R * c;  //En km
}

void task_mediciones(void *pvParameters) {
  TickType_t xLastWakeTime = xTaskGetTickCount();
  while(1) {
    unsigned long startMillis = millis();
    // se leen los valores antes de utilizar el semaphore    
    while (gpsSerial.available() > 0) {
      char c = gpsSerial.read();
      Serial.print(c);
    }

    float new_latitude = gps.location.lat();
    float new_longitude = gps.location.lng();
    float new_altitude = gps.altitude.meters();
    String new_fecha = String(gps.date.year())+"-"+String(gps.date.month())+"-"+
                   String(gps.date.day())+"T"+String(gps.time.hour())+":"+
                   String(gps.time.minute())+":"+String(gps.time.second())+"."+
                   String(gps.time.centisecond(),3);
    float new_speed = gps.speed.kmph();
    float new_temp = dht.readTemperature();
    float new_hum = dht.readHumidity();
    float new_press = 0.0; // Placeholder, no hay sensor de presión 
    
    distancia_total += calcular_delta_dist(datosAntiguos.latitude, datosAntiguos.longitude, new_latitude, new_longitude);
    

    if (xSemaphoreTake(dataMutex, portMAX_DELAY) == pdTRUE) {
      latestData.latitude = new_latitude;
      latestData.longitude = new_longitude;
      latestData.altura = new_altitude;
      latestData.tiempo = new_fecha;
      latestData.velocidad = new_speed;
      latestData.temperature = new_temp;
      latestData.humidity = new_hum;
      latestData.pressure = new_press;
      datosAntiguos = latestData;
      xSemaphoreGive(dataMutex);
    }

    File file = SD.open(filename, FILE_APPEND);
    if (file) {
      //Crear la string para escribir en el archivo
      file.print(F("\t\t\t<trkpt lat=\""));
      file.print(datosAntiguos.latitude, 6);
      file.print(F("\" lon=\""));
      file.print(datosAntiguos.longitude, 6);
      file.println(F("\">"));

      file.print(F("\t\t\t\t<ele>"));
      file.print(datosAntiguos.altura);
      file.println(F("</ele>"));

      file.print(F("\t\t\t\t<time>"));
      file.print(datosAntiguos.tiempo);
      file.println(F("</time>"));

      file.print(F("\t\t\t\t<speed>"));
      file.print(datosAntiguos.velocidad);
      file.println(F("</speed>"));

      file.println(F("\t\t\t\t<extensions>"));
      file.println(F("\t\t\t\t\t<gpxtpx:TrackPointExtension>"));
      file.print(F("\t\t\t\t\t\t<gpxtpx:atemp>"));
      file.print(datosAntiguos.temperature);
      file.println(F("</gpxtpx:atemp>"));
      file.println(F("\t\t\t\t\t</gpxtpx:TrackPointExtension>"));

      file.print(F("\t\t\t\t\t<custom:humidity>"));
      file.print(datosAntiguos.humidity);
      file.println(F("</custom:humidity>"));

      file.print(F("\t\t\t\t\t<custom:pressure>"));
      file.print(datosAntiguos.pressure);
      file.println(F("</custom:pressure>"));

      file.println(F("\t\t\t\t</extensions>"));
      file.println(F("\t\t\t</trkpt>"));
      
      file.close();
    }
    unsigned long elapsedMillis = millis() - startMillis;
    Serial.println(elapsedMillis);

    vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(MEASUREMENT_INTERVAL_S*1000)); // Espera x*1000 milisegundos
    
  }
}

void crear_archivo(){
  distancia_total = 0.0;
  int num = 1;
  sprintf(filename, "/data%03d.gpx", num);
  while (SD.exists(filename)) {
    num++;
    sprintf(filename, "/data%03d.gpx", num);
  }

  File file = SD.open(filename, FILE_WRITE);
  if (file) {
    file.println("<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
                "<gpx creator=\"ESP32 GPS LOGGER\" version=\"1.1\"\n"
                "\txmlns=\"http://www.topografix.com/GPX/1/1\"\n"
                "\txmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"\n"
                "\txmlns:gpxtpx=\"http://www.garmin.com/xmlschemas/TrackPointExtension/v2\"\n"
                "\txmlns:gpxdata=\"http://www.cluetrust.com/XML/GPXDATA/1/0\"\n"
                "\txsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\">\n"
                "\t<metadata>\n"
                "\t\t<name>Ruta grabada con ESP32 GPS Logger</name>\n"
                "\t\t<time>");
    file.print(String(gps.date.year()) + "-" + String(gps.date.month()) + "-" +
               String(gps.date.day()) + "T" + String(gps.time.hour()) + ":" +
               String(gps.time.minute()) + ":" + String(gps.time.second()) +
               "." + String(gps.time.centisecond()));
    file.println("</time>\n\t</metadata>\n"
                "\t<trk>\n"
                "\t\t<name>Rutita</name>\n"
                "\t\t<type>hiking</type>\n"
                "\t\t<trkseg>");
    file.close();
  } else {
    OLED_print("Error","creando archivo");
    delay(2000);
  }
}

void cerrar_archivo() {
  File file = SD.open(filename, FILE_APPEND);
  if (file){
    file.print("\t\t</trkseg>\n\t</trk>\n</gpx>");
    file.close();
  }
  //int num = 1;
  //sprintf(filename, "/data%03d.gpx", num);
  //while (SD.exists(filename)) {
  //  num++;
  //  sprintf(filename, "/data%03d.gpx", num);
  //}
}

void activarWiFi(){
  OLED_print("WiFi","Activando...");

  WiFi.mode(WIFI_AP);
  WiFi.softAP(apSSID, apPassword);

  IPAddress IP = WiFi.softAPIP();
  OLED_print("WiFi Activo", IP.toString());
  delay(2000);

  server.on("/", HTTP_GET, []() {
    wifiLastActivity = millis();
    String html = "<!DOCTYPE html><html><head><meta charset='UTF-8'><title>ESP32 GPS Logger</title>";
    html += "<style>body{font-family:Arial;text-align:center;margin:50px;}";
    html += "h1{color:#333;} a.button{background:#4CAF50;color:white;padding:20px 40px;";
    html += "text-decoration:none;font-size:24px;border-radius:12px;display:inline-block;margin:20px;}</style></head>";
    html += "<body><h1>GPS Logger</h1><p>Archivo listo para descargar:</p>";
    String nombreArchivo = String(filename).substring(1);  // Ej. "data001.gpx"
    html += "<a href='/download' class='button' download='" + nombreArchivo + "'>Descargar " + nombreArchivo + "</a>";
    html += "<hr><p>IP: " + WiFi.softAPIP().toString() + "</p>";
    html += "<p>Se apagar&aacute; en 5 min sin uso.</p></body></html>";
    server.send(200, "text/html", html);
  });

  server.on("/download",HTTP_GET, []() {
    wifiLastActivity = millis();

    if (!SD.exists(filename)) {
      server.send(404, "text/plain", "Archivo no encontrado");
      return;
    }

    File file = SD.open(filename, FILE_READ);
    if (!file) {  
      server.send(404, "text/plain", "Error al abrir el archivo");
      return;
    }

    String nombreArchivo = String(filename).substring(1);  // Ej. "data001.gpx"
    server.sendHeader("Content-Disposition", "attachment; filename=\"" + nombreArchivo + "\"");

    server.streamFile(file, "application/gpx+xml");
    file.close();
  });

  server.begin();
  wifiActivado = true;
  wifiLastActivity = millis();

}

void desactivarWiFi(){
  server.stop();
  WiFi.softAPdisconnect(true);
  WiFi.mode(WIFI_OFF);
  wifiActivado = false;
  OLED_print("WiFi","Apagado");
}

void IRAM_ATTR isr_button() {
  unsigned long now = millis();
  if (now < ignore_isr_until) {
    return; // Ignorar interrupción si está dentro del período de debounce
  }
  static unsigned long lastInterrupt = 0;
  
  if ((now - lastInterrupt) > 300 ){ // debounce de 300 ms 
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xSemaphoreGiveFromISR(buttonSemaphore, &xHigherPriorityTaskWoken); 
    lastInterrupt = now;
    if (xHigherPriorityTaskWoken) {
      portYIELD_FROM_ISR();
    }
  }
  
}

void drawProgressBar(int x, int y, int w, int h, unsigned long progress, unsigned long total) {
  display.drawRect(x, y, w, h, SSD1306_WHITE); // Dibuja el borde
  int filledWidth = (progress * w) / total;
  display.fillRect(x + 1, y + 1, filledWidth - 2, h - 2, SSD1306_WHITE); // Dibuja la barra llena
  display.display();
}

void task_ui(void *pvParameters){
  unsigned long pressTime = 0;
  unsigned long lastActivity = millis();
  bool pantallaOn = true; //comprobar el estado inicial, no se cual sera
  bool processingButton = false;

  while(1){
    if (xSemaphoreTake(buttonSemaphore, pdMS_TO_TICKS(200)) == pdTRUE){ //button pressed
      if (processingButton) continue; //evita reentradas
      processingButton = true;

      pressTime = millis();
      lastActivity = millis(); //reset watchdog

      if (!pantallaOn){
        display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
        pantallaOn = true;
      }

      bool timed_out = false;
      unsigned long checkTime = millis();
      while (digitalRead(BUTTON_PIN) == LOW){
        vTaskDelay(pdMS_TO_TICKS(10));
        checkTime = millis();
        if ((checkTime - pressTime) > DURACION_WATCHDOG_MS){ //10s timeout para evitar bloqueos
          timed_out = true;
          break;
        }
        drawProgressBar(0, SCREEN_HEIGHT - 10, SCREEN_WIDTH, 8, checkTime - pressTime, PULASCION_LARGA_MS);
      }

      ignore_isr_until = millis() + 500; //ignorar nuevas interrupciones durante 500 ms

      unsigned long duration = checkTime - pressTime;

      if (timed_out){
        OLED_print("Apagando","pantalla");
        display.ssd1306_command(SSD1306_DISPLAYOFF); //se apaga la
        pantallaOn = false;
      } else {
        if (grabando){
          if (duration >= PULASCION_LARGA_MS){
            grabando = false;
            vTaskSuspend(medicionesHandle);
            OLED_print("Ruta","pausada");
          } else {
            pantallaEstado_grab = (pantallaEstado_grab + 1) % 5; //cicla entre 0-4
            SensorData currentData;
            if(xSemaphoreTake(dataMutex, portMAX_DELAY) == pdTRUE){
              currentData = latestData;
              xSemaphoreGive(dataMutex);
            }
            switch (pantallaEstado_grab){
              case 0:
                OLED_print("Posicion",String(currentData.longitude) + "," + String(currentData.latitude));
                break;
              case 1:
                OLED_print("Distancia",String(distancia_total)+"km");
                break;
              case 2:
                OLED_print("Altitud",String(currentData.altura, 1)+"m");
                break;
              case 3:
                OLED_print("Temp/Hum",String(currentData.temperature,1)+"C/"+String(currentData.humidity,1)+"%");
                break;  
              case 4:
                OLED_print("Velocidad",String(currentData.velocidad, 1)+"km/h");
                break;
            }
          }
        } else {
          if (duration >= PULASCION_LARGA_MS){
            switch (pantallaEstado_menu){
              case 0:
                //activar la ruta y crear el archivo
                crear_archivo();
                vTaskResume(medicionesHandle);
                OLED_print("Ruta","iniciada");
                finalizado = false;
                grabando = true;
                break;
              case 1:
                //cerrar el archivo y cambiar el valor de 'filename'
                cerrar_archivo();
                finalizado = true;
                break;
              case 2:
                //implementacion wifi
                if(!wifiActivado){
                  activarWiFi();
                } else {
                  desactivarWiFi();
                }
                break;
            }
          } else {
            pantallaEstado_menu = (pantallaEstado_menu + 1) % 3;
            int previous_state = -1;
            while (pantallaEstado_menu != previous_state) {
              previous_state = pantallaEstado_menu;
              switch (pantallaEstado_menu) {
                case 0:
                  if (!finalizado) OLED_print("Reanudar","ruta");
                  else OLED_print("Iniciar","ruta");
                  break;
                case 1:
                  if (SD.exists(filename) && !finalizado) {
                    OLED_print("Finalizar","ruta");
                    break;
                  } else {
                    pantallaEstado_menu = (pantallaEstado_menu + 1) % 3;
                  }
                  break;
                case 2:
                  if (finalizado) { 
                    OLED_print("Conexion","WiFi");
                    break;
                  } else {
                    pantallaEstado_menu = (pantallaEstado_menu + 1) % 3;
                  }
                  break;
              }
            }
          }
        }        
      }
      lastActivity = millis();  //reset watchdog
      processingButton = false;
      vTaskDelay(pdMS_TO_TICKS(100)); //pequeño delay para no busy waiting
    }
    //check watchdog fuera del boton
    if (pantallaOn && ((millis() - lastActivity) > DURACION_WATCHDOG_MS)){
          display.ssd1306_command(SSD1306_DISPLAYOFF); //se apaga la pantalla
          pantallaOn = false;
    }
    //check wifi timeout
    if (wifiActivado){
      server.handleClient();
      
      if (WiFi.softAPgetStationNum() > 0){
        wifiLastActivity = millis(); //reset si hay clientes conectados
      }
      
      if ((millis() - wifiLastActivity) > WIFI_TIMEOUT_MS){
        desactivarWiFi();
      }
    }
  }
}


void setup() {
  Serial.begin(115200);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(BUTTON_PIN), isr_button, FALLING);
  
  buttonSemaphore = xSemaphoreCreateBinary();
  dataMutex = xSemaphoreCreateMutex();

  // OLED check
  OLED_test();
  delay(1000);
  // DHT check
  DHT_test();
  delay(1000);
  // SD Card check
  SD_test();
  delay(1000);
  // GPS check
  // Enable RMC and VTG at 1 Hz
  gpsSerial.println("$PMTK314,0,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29");  // Enables GGA, RMC, VTG
  gpsSerial.println("$PMTK220,1000*1F");  // Set update rate to 1 Hz (1000ms)
  delay(1000);  // Give time to apply
  GPS_test_wait();
  delay(2000);

  // Crear tarea para mediciones
  xTaskCreatePinnedToCore(
    task_mediciones,     // Función de la tarea
    "Mediciones",        // Nombre de la tarea
    8192,                // Tamaño del stack
    NULL,     // Parámetro de la tarea
    10,                   // Prioridad de la tarea
    &medicionesHandle,                // Handle de la tarea
    0                    // Núcleo donde se ejecuta
  );

  xTaskCreatePinnedToCore(
    task_ui,             // Función de la tarea
    "UI",                // Nombre de la tarea
    8192,                // Tamaño del stack
    NULL,                // Parámetro de la tarea
    5,                   // Prioridad de la tarea
    NULL,                // Handle de la tarea
    1                    // Núcleo donde se ejecuta
  );

  esp_pm_config_esp32_t pm_config = {
    .max_freq_mhz = 240,
    .min_freq_mhz = 80,
    .light_sleep_enable = true
  };

  esp_err_t err = esp_pm_configure(&pm_config);

  if (err != ESP_OK) {
    Serial.println("Error configuring power management");
  }

  WiFi.setSleep(true); // Desactiva el modo de ahorro de energía del WiFi

  vTaskSuspend(medicionesHandle); //inicia suspendida

}



void loop() {

  vTaskDelete(NULL); // Deshabilita el loop
}