#include <SPI.h>
#include <SD.h>
#include <Wire.h>
#include <freertos/FreeRTOS.h>
#include <freertos/task.h>
#include <freertos/queue.h>
#include "I2Cdev.h"
#include "MPU6050.h"

#define PIN_CS 5                    // pin CS de la SD
#define RUTA "/log_siget.txt"
#define NUM_LECTURAS 3

const int POSICION = 10;            // posicion donde se inserta el ID
const char* ID_SENSOR = "ID_SENS_007";
const char LETRA_ORIGINAL = 'e';    // letra que se busca
const char LETRA_NUEVA = '3';       // letra por la que se cambia
const char* ALERTA = "Alerta: Verifique el estado del servidor de optimizacion de rutas. Contacto: [email protected]";

struct Lectura {
  float x;
  float y;
  float z;
};

MPU6050 mpu;
QueueHandle_t colaLecturas;
Lectura lecturas[NUM_LECTURAS];

// imprime el archivo en el monitor serial para ver como va quedando
void mostrarArchivo(const char* titulo) {
  Serial.println();
  Serial.print("--- ");
  Serial.print(titulo);
  Serial.println(" ---");

  File f = SD.open(RUTA, FILE_READ);
  if (!f) {
    Serial.println("No se pudo abrir el archivo para leerlo");
    return;
  }
  while (f.available()) {
    Serial.write(f.read());
  }
  f.close();
  Serial.println("------------------------------");
}

// PASO 1: crear el archivo y escribir encabezado + lecturas (secuencial)
bool paso1() {
  File f = SD.open(RUTA, FILE_WRITE);   // en ESP32 FILE_WRITE = "w" (crea o borra y empieza de cero)
  if (!f) {
    Serial.println("Error: no se pudo crear el archivo");
    return false;
  }

  f.print("Registro de eventos SIGET - Fecha: ");
  f.print(__DATE__);                    // fecha de compilacion, no hay RTC
  f.println(", version: 1.0.0. Este log captura datos cruciales para la optimizacion del trafico");

  for (int i = 0; i < NUM_LECTURAS; i++) {
    char linea[80];
    snprintf(linea, sizeof(linea), "Lectura %d -> X: %.2f g | Y: %.2f g | Z: %.2f g",
             i + 1, lecturas[i].x, lecturas[i].y, lecturas[i].z);
    f.println(linea);
  }

  f.close();
  return true;
}

// PASO 2: insertar el ID en la posicion 10 (aleatorio)
// Igual que en C++, no se puede insertar sin pisar, entonces:
// leo lo que hay desde la posicion 10, escribo el ID y luego lo que habia leido.
bool paso2() {
  File f = SD.open(RUTA, "r+");         // lectura y escritura sin borrar nada
  if (!f) {
    Serial.println("Error: no se pudo abrir el archivo en r+");
    return false;
  }
  if (f.size() < POSICION) {
    Serial.println("Error: el archivo es mas corto que la posicion 10");
    f.close();
    return false;
  }

  f.seek(POSICION);
  String resto = "";
  while (f.available()) {
    resto += (char)f.read();
  }

  f.seek(POSICION);
  f.print(ID_SENSOR);
  f.print(resto);

  f.close();
  return true;
}

// PASO 3: cambiar cada 'e' por '3' directo en el archivo (in-situ)
// Es 1 caracter por 1 caracter, el tamano no cambia.
int paso3() {
  File f = SD.open(RUTA, "r+");
  if (!f) {
    Serial.println("Error: no se pudo abrir el archivo para normalizar");
    return -1;
  }

  uint32_t total = f.size();
  int cambios = 0;

  for (uint32_t i = 0; i < total; i++) {
    f.seek(i);
    int c = f.read();
    if (c == LETRA_ORIGINAL) {
      f.seek(i);
      f.write((uint8_t)LETRA_NUEVA);
      cambios++;
    }
  }

  f.close();
  return cambios;
}

// PASO 4: agregar la alerta al final (secuencial)
bool paso4() {
  File f = SD.open(RUTA, "r+");
  if (!f) {
    Serial.println("Error: no se pudo abrir el archivo para la alerta");
    return false;
  }

  f.seek(f.size());                     // me paro al final
  f.println(ALERTA);

  f.close();
  return true;
}

// Tarea 1: lee el MPU6050 cada 500 ms y manda la lectura por la cola
void tareaSensor(void* parametro) {
  for (;;) {
    int16_t ax, ay, az;
    mpu.getAcceleration(&ax, &ay, &az);

    Lectura l;
    l.x = ax / 16384.0;                 // escala de +-2g: 16384 por cada g
    l.y = ay / 16384.0;
    l.z = az / 16384.0;

    xQueueSend(colaLecturas, &l, 0);    // si la cola esta llena simplemente se descarta
    vTaskDelay(pdMS_TO_TICKS(500));
  }
}

// Tarea 2: espera las lecturas y hace los 4 pasos
void tareaArchivo(void* parametro) {
  for (int i = 0; i < NUM_LECTURAS; i++) {
    xQueueReceive(colaLecturas, &lecturas[i], portMAX_DELAY);
  }
  Serial.println("Lecturas recibidas, empiezo con el archivo");

  if (!paso1()) vTaskDelete(NULL);
  Serial.println("Paso 1 listo: archivo creado con encabezado y lecturas");
  mostrarArchivo("Despues del paso 1");

  if (!paso2()) vTaskDelete(NULL);
  Serial.println("Paso 2 listo: ID insertado en la posicion 10");
  mostrarArchivo("Despues del paso 2");

  int cambios = paso3();
  if (cambios < 0) vTaskDelete(NULL);
  Serial.print("Paso 3 listo: letras cambiadas = ");
  Serial.println(cambios);
  mostrarArchivo("Despues del paso 3");

  if (!paso4()) vTaskDelete(NULL);
  Serial.println("Paso 4 listo: alerta agregada al final");
  mostrarArchivo("Archivo final");

  Serial.println("Proceso terminado");
  vTaskDelete(NULL);                    // la tarea ya hizo su trabajo
}

void setup() {
  Serial.begin(115200);
  delay(500);
  Serial.println("=== SIGET - Gestion de archivos (Wokwi + FreeRTOS) ===");

  Wire.begin(21, 22);                   // SDA = 21, SCL = 22
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("Aviso: el MPU6050 no responde, revisa las conexiones");
  }

  if (!SD.begin(PIN_CS)) {
    Serial.println("Error: no se pudo iniciar la tarjeta SD, revisa el cableado");
    while (true) {
      delay(1000);
    }
  }
  Serial.println("SD iniciada correctamente");

  colaLecturas = xQueueCreate(10, sizeof(Lectura));
  if (colaLecturas == NULL) {
    Serial.println("Error: no se pudo crear la cola");
    while (true) {
      delay(1000);
    }
  }

  xTaskCreate(tareaSensor, "Sensor", 4096, NULL, 1, NULL);
  xTaskCreate(tareaArchivo, "Archivo", 8192, NULL, 1, NULL);
}

void loop() {
  vTaskDelay(pdMS_TO_TICKS(1000));      // todo el trabajo lo hacen las tareas
}