#include <WiFi.h>
#include <esp_now.h>
#include <PubSubClient.h>

bool forzarEnvio = false;

// --- CONFIGURACIÓN WI-FI (Específica para Wokwi) ---
const char* ssid = "Wokwi-GUEST";
const char* password = "";

// --- CONFIGURACIÓN THINGSPEAK ---
const char* channelID = "3343185"; // Ej: "1234567"
String writeAPIKey = "AL2KU1UKPVFXLUP6";

// --- CREDENCIALES MQTT THINGSPEAK ---
const char* mqtt_server = "mqtt3.thingspeak.com";
const char* mqtt_client_id = "HB8GFQMvMBwMOSslDBIwIAI";
const char* mqtt_username = "HB8GFQMvMBwMOSslDBIwIAI";
const char* mqtt_password = "umWsc5THgCXJ2f9NJmeGqU+h";

struct DatosPelota {
  float temp;
  float accX, accY, accZ;
  float gyroX, gyroY, gyroZ;
};

DatosPelota misDatos;

WiFiClient espClient;
PubSubClient mqttClient(espClient);

int CLK=22; // pin 3 is set for clock
int DTA=21; // pin 4 is set for data
int TIME=1; // time reference; increase it if you want to debug the code

int16_t accX, accY, accZ;
int16_t gyroX, gyroY, gyroZ;
int16_t tempRaw;

byte ADR_MPU_W=0xD0; //MPU6050 0x68 (01101000) + Escribir (0) = 0xD0 (11010000)
byte ADR_MPU_R=0xD1; //MPU6050 0x68 (01101000) + Leer (1) = 0xD1 (11010001)

// --- EL ÍNDICE DE FUNCIONES ---
void start(int d,int c,int t);
void stop(int d,int c,int t);
void writeOne(int d,int c,int t);
void writeZero(int d,int c,int t);
void writeByte(int d,int c,int t,byte b);
byte readByte(int d, int c, int t, bool sendAck);
void readMPU6050_All();
void reconectarMQTT();
void enviarPorMQTT(DatosPelota &datos);

class Temporizador {
  private:
    unsigned long tiempoAnterior; // Su memoria interna privada
    unsigned long intervalo;      // Su límite de tiempo

  public:
    // Se ejecuta cuando creamos el temporizador
    Temporizador(unsigned long _intervalo) {
      intervalo = _intervalo;
      tiempoAnterior = 0;
    }

    // La función principal
    bool listo() {
      unsigned long tiempoActual = millis();
      if (tiempoActual - tiempoAnterior >= intervalo) {
        tiempoAnterior = tiempoActual; // Se reinicia a sí mismo
        return true;
      }
      return false;
    }
};

// Variables para el cronómetro no bloqueante
Temporizador timerTSfree(15000); // ThingSpeak gratuito exige esperar 15 segundos entre envíos
Temporizador Midelay1(500); // Intervalo1 500ms
Temporizador Midelay2(5000); // Intervalo2 5000ms

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

  // sets pins 3 (clock) and 4 (data) as outputs
  pinMode(CLK, OUTPUT);
  pinMode(DTA, OUTPUT);
  
  // sets initial conditions of clock and data as 1 (high level)
  digitalWrite(CLK, HIGH);
  digitalWrite(DTA, HIGH);

  // Inicializamos el sensor MPU6050
  // Despertar al sensor escribiendo 0x00 en el registro 0x6B
  start(DTA, CLK, TIME);
  writeByte(DTA, CLK, TIME, ADR_MPU_W); // Dirección MPU6050 + Escribir
  writeByte(DTA, CLK, TIME, 0x6B); // Registro de energía
  writeByte(DTA, CLK, TIME, 0x00); // Valor 0 (Despertar)
  stop(DTA, CLK, TIME);

  //Conectar a Wi-Fi
  WiFi.begin(ssid, password);
  Serial.print("Conectando a WiFi");
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nWiFi Conectado!");

  // Configurar servidor MQTT
  mqttClient.setServer(mqtt_server, 1883);
}

void loop() {
  // put your main code here, to run repeatedly:
  //delay(10); // this speeds up the simulation

  // Asegurar que MQTT siga conectado
  if (!mqttClient.connected()) {
    reconectarMQTT();
  }
  mqttClient.loop();

  readMPU6050_All();

  // Guardamos las conversiones directo en el struct
  misDatos.temp = (tempRaw / 340.0) + 36.53;
  misDatos.accX = accX / 16384.0;
  misDatos.accY = accY / 16384.0;
  misDatos.accZ = accZ / 16384.0;
  misDatos.gyroX = gyroX / 131.0;
  misDatos.gyroY = gyroY / 131.0;
  misDatos.gyroZ = gyroZ / 131.0;

  if (timerTSfree.listo() || forzarEnvio) {
    Serial.println("--- Enviando Datos ---");
    enviarPorMQTT(misDatos); 
    forzarEnvio = false; // Apagamos la bandera hasta la próxima desconexión
  }

  delay(10);

}

void start(int d,int c,int t){
  // c is the clock pin
  // d is the data pin
  // t is the time reference
  
  digitalWrite(d, LOW);
  delay(t/6);
  digitalWrite(c, LOW);
  delay(t/6);
}

// method for stoping transmition - I2C protocol

void stop(int d,int c,int t){
  // c is the clock pin
  // d is the data pin
  // t is the time reference
  
  digitalWrite(c, HIGH);
  delay(t/6);
  digitalWrite(d, HIGH);
}

// method for transmiting a bit 1 - I2C protocol

void writeOne(int d,int c,int t){
  // c is the clock pin
  // d is the data pin
  // t is the time reference
  
  digitalWrite(d, HIGH);
  delay(t/6);
  digitalWrite(c, HIGH);
  delay(t/3);
  digitalWrite(c, LOW);
  delay(t/6);
  digitalWrite(d, LOW);
}

// method for transmiting a bit 0 - I2C protocol

void writeZero(int d,int c,int t){
  // c is the clock pin
  // d is the data pin
  // t is the time reference

  digitalWrite(d, LOW);
  digitalWrite(c, LOW);
  delay(t/6);
  digitalWrite(c, HIGH);
  delay(t/3);
  digitalWrite(c, LOW);
  delay(t/6);
}

// method for transmiting an entire byte

void writeByte(int d,int c,int t,byte b){
  // c is the clock pin
  // d is the data pin
  // t is the time reference
  // b is the byte to be sent

  // loop for transmiting the 8 bits
  for(int i=8;i>=1;i--){
    if((( b & (1 << (i-1) ) ) ? 1 : 0 )==0){
      writeZero(d,c,t);
    }
    else{
      writeOne(d,c,t);
    }
  }

  // procedure for receiving the acknowledge bit from the LCD - I2C protocol
  // NOTE: nothing is done with this bit, but you can verify it if you want
  pinMode(d, INPUT);
  delay(t/6);
  digitalWrite(c, HIGH);
  delay(t/6);
  digitalWrite(c, LOW); 
  delay(t/6);
  pinMode(d, OUTPUT);
  delay(t/6);
 
}

// method for receiving an entire byte - I2C protocol
byte readByte(int d, int c, int t, bool sendAck) {
  byte receivedByte = 0;
  
  pinMode(d, INPUT); // Cambiamos el pin de datos a modo lectura
  
  // loop para leer los 8 bits
  for(int i=8; i>=1; i--) {
    digitalWrite(c, HIGH); // Sube el reloj
    delay(t/6);
   
    // Si el pin está en HIGH, ponemos un 1 en esa posición del byte
    if(digitalRead(d) == HIGH) {
      receivedByte = receivedByte | (1 << (i-1));
    }
    
    digitalWrite(c, LOW); // Baja el reloj
    delay(t/3);
  }
  
  pinMode(d, OUTPUT); // Volvemos a poner el pin como salida
  
  // Enviar ACK (reconocimiento) o NACK (fin de transmisión)
  if(sendAck) {
    writeZero(d, c, t); // Pull low para ACK 
  } else {
    writeOne(d, c, t);  // High para NACK
  }
  
  return receivedByte;
}

void readMPU6050_All() {
  // Apuntar al primer registro (Acelerómetro X Alto)
  start(DTA, CLK, TIME);
  writeByte(DTA, CLK, TIME, ADR_MPU_W); 
  writeByte(DTA, CLK, TIME, 0x3B); // <--- Empezamos en el 0x3B
  stop(DTA, CLK, TIME);

  // Pedir leer los 14 bytes de corrido
  start(DTA, CLK, TIME);
  writeByte(DTA, CLK, TIME, ADR_MPU_R); 

  byte alto, bajo; // Variables temporales para ir armando

  // --- ACELERÓMETRO ---
  alto = readByte(DTA, CLK, TIME, true); 
  bajo = readByte(DTA, CLK, TIME, true);
  accX = (alto << 8) | bajo;

  alto = readByte(DTA, CLK, TIME, true); 
  bajo = readByte(DTA, CLK, TIME, true);
  accY = (alto << 8) | bajo;

  alto = readByte(DTA, CLK, TIME, true); 
  bajo = readByte(DTA, CLK, TIME, true);
  accZ = (alto << 8) | bajo;

  // --- TEMPERATURA ---
  alto = readByte(DTA, CLK, TIME, true); 
  bajo = readByte(DTA, CLK, TIME, true);
  tempRaw = (alto << 8) | bajo;

  // --- GIROSCOPIO ---
  alto = readByte(DTA, CLK, TIME, true); 
  bajo = readByte(DTA, CLK, TIME, true);
  gyroX = (alto << 8) | bajo;

  alto = readByte(DTA, CLK, TIME, true); 
  bajo = readByte(DTA, CLK, TIME, true);
  gyroY = (alto << 8) | bajo;

  alto = readByte(DTA, CLK, TIME, true); 
  bajo = readByte(DTA, CLK, TIME, false); // <--- (NACK)
  gyroZ = (alto << 8) | bajo;

  stop(DTA, CLK, TIME); // Terminamos la comunicacion
}

// ==========================================
// FUNCIÓN 2: Envío por MQTT (BLINDADA)
// ==========================================
void enviarPorMQTT(DatosPelota &datos) {
  if (mqttClient.connected()) {
    String topic = String("channels/") + channelID + "/publish";
    
    // 1. Creamos un espacio de memoria seguro y cerrado de 150 caracteres
    char payload[150]; 
    
    // 2. Inyectamos las 7 variables de golpe, sin fragmentar la memoria
    snprintf(payload, sizeof(payload), "field1=%.2f&field2=%.2f&field3=%.2f&field4=%.2f&field5=%.2f&field6=%.2f&field7=%.2f", 
             datos.temp, datos.accX, datos.accY, datos.accZ, datos.gyroX, datos.gyroY, datos.gyroZ);
    
    // 3. Enviamos
    if (mqttClient.publish(topic.c_str(), payload)) {
      Serial.println("MQTT Enviado correctamente.");
    } else {
      Serial.println("Error enviando por MQTT.");
    }
  }
}

// ==========================================
// Función de soporte para reconectar MQTT
// ==========================================
void reconectarMQTT() {
  if (Midelay2.listo()) {
    while (!mqttClient.connected()) {
      Serial.print("Intentando conexión MQTT...");
      if (mqttClient.connect(mqtt_client_id, mqtt_username, mqtt_password)) {
        Serial.println("Conectado a MQTT ThingSpeak");
        forzarEnvio = true;
        break;
      } else {
        Serial.print("Fallo, rc=");
        Serial.print(mqttClient.state());
        Serial.println(" Intentando de nuevo en 5 segundos...");
        break;
      }
    }
  }
}