/*
 * ============================================================================
 *  FAB ENCLOSER - Firmware XIAO ESP32-S3
 * ============================================================================
 *  Enclosure inteligente para impresion 3D.
 *  Control y monitoreo por BLE desde la app PyQt6 (backendFAB.py).
 *
 *  Hardware:
 *    - Seeed Studio XIAO ESP32-S3
 *    - Sensor SHT31 (temperatura + humedad) por I2C
 *    - OLED SH1106G 128x64 1.3" por I2C
 *    - Heater PTC 100W 12V via MOSFET   -> D0
 *    - Ventilador 12V via MOSFET        -> D9
 *    - Luces LED via MOSFET             -> D8
 *    - I2C: SDA=D4, SCL=D5 (por defecto en XIAO ESP32-S3)
 *
 *  Librerias (Library Manager):
 *    - Adafruit SHT31 Library
 *    - Adafruit SH110X
 *    - Adafruit GFX Library
 *    (BLE va incluido en el core ESP32 de Arduino)
 *
 * ----------------------------------------------------------------------------
 *  PROTOCOLO BLE (Nordic UART Service) - debe coincidir con backendFAB.py
 * ----------------------------------------------------------------------------
 *  SERVICE_UUID 6e400001-b5a3-f393-e0a9-e50e24dcca9e
 *  RX  (Write)  6e400002-...  la APP escribe comandos aqui
 *  TX  (Notify) 6e400003-...  el ESP32 notifica estado (JSON) aqui
 *
 *  Comandos que llegan por RX (texto, terminados en '\n'):
 *    H:1 / H:0      Heater ON/OFF
 *    F:1 / F:0      Ventilador ON/OFF
 *    L:1 / L:0      Luces ON/OFF
 *    MODE:AUTO      Modo automatico
 *    MODE:MANUAL    Modo manual
 *    SP:45          Setpoint = 45 C
 *    MAT:ABS        Material (informativo)
 *    PAUSE          Pausar impresion (no implementado el hardware aqui)
 *    EMERGENCY      Paro: heater OFF, fan ON
 *
 *  Estado enviado por TX (JSON, una linea con '\n'):
 *    {"t":32.4,"h":38,"heater":1,"fan":0,"lights":1,
 *     "mode":"AUTO","sp":45,"status":"NORMAL"}
 * ============================================================================
 */

#include <Wire.h>
#include <Adafruit_SHT31.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>

#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

// ----------------------------------------------------------------------------
// CONFIGURACION
// ----------------------------------------------------------------------------
#define DEVICE_NAME   "FAB_ENCLOSER"

#define SERVICE_UUID  "6e400001-b5a3-f393-e0a9-e50e24dcca9e"
#define RX_CHAR_UUID  "6e400002-b5a3-f393-e0a9-e50e24dcca9e"  // APP -> ESP32
#define TX_CHAR_UUID  "6e400003-b5a3-f393-e0a9-e50e24dcca9e"  // ESP32 -> APP

// Pines de actuadores (gate de cada MOSFET)
#define PIN_HEATER    D0
#define PIN_FAN       D9
#define PIN_LIGHTS    D8

// OLED
#define SCREEN_WIDTH  128
#define SCREEN_HEIGHT 64
#define OLED_ADDR     0x3C   // si no funciona, prueba 0x3D

// --- Limites de SEGURIDAD (independientes del BLE) -------------------------
#define TEMP_MAX_HARD     65.0   // corte duro: por encima -> heater OFF siempre
#define TEMP_HYSTERESIS    2.0   // banda de histeresis del control automatico
#define SETPOINT_MIN      20
#define SETPOINT_MAX      60
#define SENSOR_TIMEOUT_MS 5000   // sin lectura valida -> heater OFF (watchdog)
#define SENSOR_MAX_FAILS  3      // lecturas NaN seguidas toleradas antes de cortar

// Periodo de envio de estado por BLE
#define STATE_PERIOD_MS   1000

// ----------------------------------------------------------------------------
// OBJETOS
// ----------------------------------------------------------------------------
Adafruit_SHT31 sht31 = Adafruit_SHT31();
Adafruit_SH1106G display = Adafruit_SH1106G(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

BLEServer*         pServer  = nullptr;
BLECharacteristic* pTxChar  = nullptr;
bool deviceConnected = false;

// ----------------------------------------------------------------------------
// ESTADO DEL SISTEMA
// ----------------------------------------------------------------------------
struct SystemState {
  float temperature = 0.0;
  float humidity    = 0.0;
  bool  heater      = false;
  bool  fan         = false;
  bool  lights      = false;
  bool  autoMode    = true;
  int   setpoint    = 30;
  String status     = "NORMAL";   // NORMAL | WARN | ALERT
  String material   = "PLA";
  bool   sensorOK   = false;
  unsigned long lastSensorMs = 0;
  int    sensorFails = 0;        // lecturas NaN consecutivas
} st;

unsigned long lastStateSend = 0;
unsigned long lastSensorRead = 0;

// ----------------------------------------------------------------------------
// ACTUADORES
// ----------------------------------------------------------------------------
void applyOutputs() {
  digitalWrite(PIN_HEATER, st.heater ? HIGH : LOW);
  digitalWrite(PIN_FAN,    st.fan    ? HIGH : LOW);
  digitalWrite(PIN_LIGHTS, st.lights ? HIGH : LOW);
}

// Apaga el heater de forma segura y deja constancia en el estado.
void heaterOff(const char* reason) {
  st.heater = false;
  digitalWrite(PIN_HEATER, LOW);
}

// ----------------------------------------------------------------------------
// LOGICA DE SEGURIDAD + CONTROL AUTOMATICO
// ----------------------------------------------------------------------------
void safetyAndControl() {
  unsigned long now = millis();

  // 1) Watchdog del sensor: si hace mucho que no hay lectura valida -> corte
  if (!st.sensorOK || (now - st.lastSensorMs > SENSOR_TIMEOUT_MS)) {
    heaterOff("sensor");
    st.status = "ALERT";
    return;
  }

  // 2) Corte duro por sobretemperatura (siempre, en cualquier modo)
  if (st.temperature >= TEMP_MAX_HARD) {
    heaterOff("overtemp");
    st.fan = true;                 // ayuda a disipar
    st.status = "ALERT";
    return;
  }

  // 3) Aviso si estamos cerca del limite duro
  if (st.temperature >= (TEMP_MAX_HARD - 5.0)) {
    st.status = "WARN";
  } else {
    st.status = "NORMAL";
  }

  // 4) Control automatico con histeresis (solo en modo AUTO)
  if (st.autoMode) {
    float sp = (float)st.setpoint;
    if (st.temperature < (sp - TEMP_HYSTERESIS)) {
      st.heater = true;            // por debajo del setpoint: calienta
    } else if (st.temperature > (sp + TEMP_HYSTERESIS)) {
      st.heater = false;           // por encima: apaga
    }
    // dentro de la banda: mantiene el estado actual (evita parpadeo)
  }
}

// ----------------------------------------------------------------------------
// SENSOR
// ----------------------------------------------------------------------------
// Watchdog robusto:
//  - Una lectura buena resetea el contador y actualiza temperatura/humedad.
//  - Una lectura NaN NO corta de inmediato: incrementa el contador y se sigue
//    usando la ULTIMA lectura valida (un glitch de ruido no apaga el heater).
//  - Solo si se acumulan SENSOR_MAX_FAILS fallos seguidos se considera que el
//    sensor cayo (sensorOK=false); el corte final lo hace safetyAndControl()
//    via ese flag o via SENSOR_TIMEOUT_MS.
void readSensor() {
  float t = sht31.readTemperature();
  float h = sht31.readHumidity();

  if (!isnan(t) && !isnan(h)) {
    // Lectura buena: actualiza y resetea el contador de fallos
    st.temperature  = t;
    st.humidity     = h;
    st.sensorOK     = true;
    st.sensorFails  = 0;
    st.lastSensorMs = millis();
  } else {
    // Lectura mala: tolera unos pocos glitches seguidos
    st.sensorFails++;

    // --- DIAGNOSTICO: avisa cada fallo de lectura del SHT31 ---
    Serial.print("[SENSOR] Lectura invalida (NaN) #");
    Serial.print(st.sensorFails);
    Serial.print("  (usando ultima t=");
    Serial.print(st.temperature, 1);
    Serial.println("C)");

    if (st.sensorFails >= SENSOR_MAX_FAILS) {
      // Demasiados fallos seguidos: el sensor se considera caido
      st.sensorOK = false;
    }
    // Mientras no se supere el umbral, sensorOK sigue true y el control
    // opera con la ultima lectura valida (no se actualiza lastSensorMs,
    // asi que el timeout de 5 s tambien sigue corriendo como respaldo).
  }
}

// ----------------------------------------------------------------------------
// OLED
// ----------------------------------------------------------------------------
void updateDisplay() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SH110X_WHITE);

  display.setCursor(0, 0);
  display.print("FAB ENCLOSER ");
  display.print(deviceConnected ? "BLE" : "---");

  display.setCursor(0, 14);
  display.print("Temp: ");
  display.print(st.temperature, 1);
  display.print(" C");

  display.setCursor(0, 26);
  display.print("Hum:  ");
  display.print(st.humidity, 0);
  display.print(" %");

  display.setCursor(0, 38);
  display.print(st.autoMode ? "AUTO" : "MAN");
  display.print("  SP:");
  display.print(st.setpoint);
  display.print("C");

  display.setCursor(0, 50);
  display.print("H");
  display.print(st.heater ? "1" : "0");
  display.print(" F");
  display.print(st.fan ? "1" : "0");
  display.print(" L");
  display.print(st.lights ? "1" : "0");
  display.print(" ");
  display.print(st.status);

  display.display();
}

// ----------------------------------------------------------------------------
// BLE: envio de estado (JSON)
// ----------------------------------------------------------------------------
void sendState() {
  if (!deviceConnected) return;

  char buf[200];
  snprintf(buf, sizeof(buf),
    "{\"t\":%.1f,\"h\":%d,\"heater\":%d,\"fan\":%d,\"lights\":%d,"
    "\"mode\":\"%s\",\"sp\":%d,\"status\":\"%s\"}\n",
    st.temperature,
    (int)st.humidity,
    st.heater ? 1 : 0,
    st.fan ? 1 : 0,
    st.lights ? 1 : 0,
    st.autoMode ? "AUTO" : "MANUAL",
    st.setpoint,
    st.status.c_str());

  pTxChar->setValue((uint8_t*)buf, strlen(buf));
  pTxChar->notify();
}

// ----------------------------------------------------------------------------
// BLE: parseo de comandos recibidos
// ----------------------------------------------------------------------------
void handleCommand(String cmd) {
  cmd.trim();
  if (cmd.length() == 0) return;

  if (cmd == "EMERGENCY") {
    heaterOff("emergency");
    st.fan = true;
    st.autoMode = false;           // sale de auto para no recalentar
    st.status = "ALERT";
  }
  else if (cmd == "PAUSE") {
    // Aqui podrias activar un pin hacia la impresora si lo cableas.
    // Por ahora solo se registra.
  }
  else if (cmd == "MODE:AUTO") {
    st.autoMode = true;
  }
  else if (cmd == "MODE:MANUAL") {
    st.autoMode = false;
  }
  else if (cmd.startsWith("H:")) {
    // En AUTO el heater lo gobierna el control; ignora orden manual directa.
    if (!st.autoMode) st.heater = (cmd.charAt(2) == '1');
  }
  else if (cmd.startsWith("F:")) {
    st.fan = (cmd.charAt(2) == '1');
  }
  else if (cmd.startsWith("L:")) {
    st.lights = (cmd.charAt(2) == '1');
  }
  else if (cmd.startsWith("SP:")) {
    int v = cmd.substring(3).toInt();
    if (v < SETPOINT_MIN) v = SETPOINT_MIN;
    if (v > SETPOINT_MAX) v = SETPOINT_MAX;
    st.setpoint = v;
  }
  else if (cmd.startsWith("MAT:")) {
    st.material = cmd.substring(4);
  }
}

// ----------------------------------------------------------------------------
// BLE: callbacks
// ----------------------------------------------------------------------------
class ServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* s) override {
    deviceConnected = true;
  }
  void onDisconnect(BLEServer* s) override {
    deviceConnected = false;
    // SEGURIDAD: si se pierde la app, no dejamos el heater en modo manual.
    if (!st.autoMode) {
      heaterOff("ble_lost");
    }
    BLEDevice::startAdvertising();   // vuelve a anunciarse
  }
};

class RxCallbacks : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic* c) override {
    String rx = c->getValue().c_str();
    // Puede llegar mas de un comando junto, separados por '\n'
    int start = 0;
    int nl;
    while ((nl = rx.indexOf('\n', start)) != -1) {
      handleCommand(rx.substring(start, nl));
      start = nl + 1;
    }
    if (start < rx.length()) {
      handleCommand(rx.substring(start));
    }
  }
};

// ----------------------------------------------------------------------------
// SETUP
// ----------------------------------------------------------------------------
void setup() {
  Serial.begin(115200);

  // Pines de salida en estado SEGURO antes que nada
  pinMode(PIN_HEATER, OUTPUT);
  pinMode(PIN_FAN,    OUTPUT);
  pinMode(PIN_LIGHTS, OUTPUT);
  digitalWrite(PIN_HEATER, LOW);
  digitalWrite(PIN_FAN,    LOW);
  digitalWrite(PIN_LIGHTS, LOW);

  // I2C
  Wire.begin();

  // SHT31
  if (!sht31.begin(0x44)) {       // 0x44 por defecto; 0x45 si ADDR a VDD
    Serial.println("SHT31 no encontrado");
  }

  // OLED
  if (!display.begin(OLED_ADDR, true)) {
    Serial.println("OLED no encontrada");
  } else {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(0, 0);
    display.println("FAB ENCLOSER");
    display.println("Iniciando...");
    display.display();
  }

  // BLE
  BLEDevice::init(DEVICE_NAME);
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new ServerCallbacks());

  BLEService* pService = pServer->createService(SERVICE_UUID);

  // TX (notify)
  pTxChar = pService->createCharacteristic(
      TX_CHAR_UUID, BLECharacteristic::PROPERTY_NOTIFY);
  pTxChar->addDescriptor(new BLE2902());

  // RX (write)
  BLECharacteristic* pRxChar = pService->createCharacteristic(
      RX_CHAR_UUID,
      BLECharacteristic::PROPERTY_WRITE | BLECharacteristic::PROPERTY_WRITE_NR);
  pRxChar->setCallbacks(new RxCallbacks());

  pService->start();

  BLEAdvertising* pAdv = BLEDevice::getAdvertising();
  pAdv->addServiceUUID(SERVICE_UUID);
  pAdv->setScanResponse(true);
  BLEDevice::startAdvertising();

  Serial.println("FAB ENCLOSER listo. Anunciando BLE...");
}

// ----------------------------------------------------------------------------
// LOOP
// ----------------------------------------------------------------------------
void loop() {
  unsigned long now = millis();

  // Lee sensor a ritmo razonable (cada ~1 s)
  if (now - lastSensorRead >= 1000) {
    lastSensorRead = now;
    readSensor();
  }

  // Guarda el estado del heater ANTES de la logica de control
  bool heaterBefore = st.heater;

  // Seguridad + control SIEMPRE (no depende del BLE)
  safetyAndControl();

  // --- DIAGNOSTICO: reporta si el heater cambio y por que ---
  if (st.heater != heaterBefore) {
    Serial.print("[HEATER] ");
    Serial.print(heaterBefore ? "ON->OFF" : "OFF->ON");
    Serial.print("  t=");
    Serial.print(st.temperature, 2);
    Serial.print("C  sp=");
    Serial.print(st.setpoint);
    Serial.print("C  status=");
    Serial.print(st.status);
    Serial.print("  sensorOK=");
    Serial.println(st.sensorOK ? "1" : "0");
  }

  // Aplica salidas fisicas
  applyOutputs();

  // Envia estado a la app periodicamente
  if (now - lastStateSend >= STATE_PERIOD_MS) {
    lastStateSend = now;
    sendState();
    updateDisplay();
  }

  delay(10);
}