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

// ---------------- WIFI ----------------
const char* WIFI_SSID = "UTN FRRO - 5to Piso"; 
const char* WIFI_PASS = "UTN_FRRO_P5"; 

// ---------------- THINGSBOARD ----------------
const char* TB_HOST = "mqtt.eu.thingsboard.cloud"; 
const int TB_PORT = 1883; 
const char* TB_TOKEN = "ma0e9hrvgbi3id7wuwbi";

// ---------------- SERIAL (D2=RX, D3=TX) ----------------
SoftwareSerial arduino(D2, D3);   

WiFiClient wifiClient;   
PubSubClient mqtt(wifiClient);   

// ---------------- CALLBACK (Recepción desde el Dashboard) ----------------
void callback(char* topic, byte* payload, unsigned int length) {
  String mensaje = "";
  for (unsigned int i = 0; i < length; i++) {
    mensaje += (char)payload[i];
  }

  Serial.print("CMD recibido desde la nube: ");
  Serial.println(mensaje);

  // Parsing manual del JSON de ThingsBoard
  if (mensaje.indexOf("setSetpoint") >= 0) {
    int pos = mensaje.indexOf("\"params\":");
    if (pos >= 0) {
      String val = mensaje.substring(pos + 9);
      val.replace("}", "");
      val.trim();

      arduino.println("R:" + val); // Despacha al Arduino Uno
      Serial.println("Enviado a Arduino -> R:" + val);
    }
  }
  else if (mensaje.indexOf("setMode") >= 0) {
    if (mensaje.indexOf("true") >= 0) {
      arduino.println("MODE:AUTO");
      Serial.println("Enviado a Arduino -> MODE:AUTO");
    } else {
      arduino.println("MODE:MANUAL");
      Serial.println("Enviado a Arduino -> MODE:MANUAL");
    }
  }
}

void conectarWiFi() {
  Serial.println("\nConectando WiFi...");
  WiFi.begin(WIFI_SSID, WIFI_PASS);

  while (WiFi.status() != WL_CONNECTED) {
    delay(250);
    Serial.print(".");
  }
  Serial.println("\nWiFi conectado");
}

void conectarMQTT() {
  while (!mqtt.connected()) {
    Serial.print("Conectando MQTT a ThingsBoard... ");
    if (mqtt.connect("ESP8266-Gateway", TB_TOKEN, "")) {
      Serial.println("OK");
      
      // SUSCRIPCIÓN NATIVA PARA DETECTAR ACCIONES DEL DASHBOARD
      mqtt.subscribe("v1/devices/me/rpc/request/+"); 
    } else {
      Serial.print("FALLO rc=");
      Serial.print(mqtt.state());
      Serial.println(" intentando en 3s...");
      delay(3000);
    }
  }
}

void setup() {
  Serial.begin(9600);
  arduino.begin(9600);

  conectarWiFi();
  mqtt.setServer(TB_HOST, TB_PORT);
  mqtt.setCallback(callback);
  conectarMQTT();
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) { conectarWiFi(); return; }
  if (!mqtt.connected()) { conectarMQTT(); }
  
  mqtt.loop();

  // ----------- PROCESAMIENTO DE DATOS SERIALES -----------
  if (arduino.available()) {
    String linea = arduino.readStringUntil('\n');
    linea.trim();

    if (linea.length() == 0) return;

    // Filtros de Seguridad básicos
    if (linea.length() < 20 || linea.length() > 80) {
      Serial.println("Línea descartada por longitud anómala.");
      return;
    }
    if (linea.indexOf("AUTO") == -1 && linea.indexOf("MANUAL") == -1) {
      Serial.println("Línea descartada (falta indicador de modo).");
      return;
    }

    // Conteo Dinámico y seguro de Comas
    int indices[7];
    int contador = 0;

    for (int i = 0; i < linea.length(); i++) {
      if (linea.charAt(i) == ',') {
        if (contador < 7) {
          indices[contador] = i;
        }
        contador++;
      }
    }

    if (contador != 7) {
      Serial.print("Línea descartada. Comas incorrectas: ");
      Serial.println(contador);
      return;
    }

    // Extracción limpia de campos
    String y = linea.substring(0, indices[0]);
    String r = linea.substring(indices[0]+1, indices[1]);
    String e = linea.substring(indices[1]+1, indices[2]);
    String u = linea.substring(indices[2]+1, indices[3]);
    String kp = linea.substring(indices[3]+1, indices[4]);
    String ki = linea.substring(indices[4]+1, indices[5]);
    String kd = linea.substring(indices[5]+1, indices[6]);
    String mode = linea.substring(indices[6]+1);

    // Validación extra de consistencia de strings
    if (y.length() > 8 || r.length() > 8 || mode.length() > 6) return;

    // Construcción del Payload JSON estructurado
    String payload = "{";
    payload += "\"y\":" + y + ",";
    payload += "\"r\":" + r + ",";
    payload += "\"e\":" + e + ",";
    payload += "\"u\":" + u + ",";
    payload += "\"kp\":" + kp + ",";
    payload += "\"ki\":" + ki + ",";
    payload += "\"kd\":" + kd + ",";
    payload += "\"mode\":\"" + mode + "\"";
    payload += "}";

    // Evitar caracteres corruptos por ruido de masa
    if (payload.indexOf("") >= 0) return;

    // Publicación nativa
    Serial.println("ENVIANDO A THINGSBOARD:");
    Serial.println(payload);

    bool ok = mqtt.publish("v1/devices/me/telemetry", payload.c_str());
    Serial.print("MQTT Publish -> ");
    Serial.println(ok ? "OK" : "ERROR");
  }
}