/*
  ESP32-C6 Umbrella Env Sensor (FLL)
  - BME680 (LIVE) OR BLE-controlled SIM mode
  - Modbus RTU (Holding Registers) clean map HR0..HR7
  - BLE GATT control (NimBLE) with compact binary packets

  Modbus HR map (Holding Registers):
    HR0 counter
    HR1 status (bitfield)
    HR2 mode   (0=LIVE, 1=SIM)
    HR3 error_count
    HR4 temp_x100   (int16 stored in 16-bit register)
    HR5 rh_x100     (uint16)
    HR6 press_x10   (uint16)
    HR7 iaq         (uint16, 0..500, higher=worse, AQI-style)

  BLE control packets (little-endian):
    CMD 0x01 (set mode):  [0]=0x01, [1]=mode(0/1)
    CMD 0x02 (set sim):   [0]=0x02,
                          [1..2]=temp_x100 (int16 LE),
                          [3..4]=rh_x100   (uint16 LE),
                          [5..6]=press_x10 (uint16 LE),
                          [7..8]=iaq (uint16 LE) OPTIONAL
                        If iaq not provided, device computes a simple heuristic IAQ for SIM.

  Libraries:
    - Adafruit BME680 library
    - modbus-esp8266 (ModbusRTU.h)
    - NimBLE-Arduino

  Pins (match your working setup):
    I2C SDA=22, SCL=21
    RS-485 UART: RX=4, TX=5 @ 9600
*/

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BME680.h>

#include <ModbusRTU.h>      // modbus-esp8266 library
#include <NimBLEDevice.h>   // NimBLE-Arduino

// -------------------- I2C --------------------
static const int I2C_SDA = 22;
static const int I2C_SCL = 21;
static const uint32_t I2C_FREQ = 100000;
static const uint8_t BME_ADDR_1 = 0x76;
static const uint8_t BME_ADDR_2 = 0x77;

// -------------------- MODBUS UART -----------
static const int MB_RX = 4;
static const int MB_TX = 5;
static const uint32_t MB_BAUD = 9600;
static const uint8_t SLAVE_ID = 10;

// -------------------- Modbus registers -------
enum {
  HR_COUNTER     = 0,
  HR_STATUS      = 1,
  HR_MODE        = 2,
  HR_ERROR_COUNT = 3,
  HR_TEMP_X100   = 4,
  HR_RH_X100     = 5,
  HR_PRESS_X10   = 6,
  HR_IAQ         = 7,
  HR_REG_COUNT   = 8
};

// status bits (HR1)
static const uint16_t ST_OK            = (1 << 0);
static const uint16_t ST_FALLBACK      = (1 << 1);
static const uint16_t ST_NEW_SAMPLE    = (1 << 2);
static const uint16_t ST_SIM_ACTIVE    = (1 << 3);
static const uint16_t ST_BLE_CONNECTED = (1 << 4);

// mode (HR2)
static const uint16_t MODE_LIVE = 0;
static const uint16_t MODE_SIM  = 1;

// -------------------- Data model -------------
struct EnvData {
  uint16_t counter;
  uint16_t status;
  uint16_t mode;
  uint16_t error_count;
  int16_t  temp_x100;
  uint16_t rh_x100;
  uint16_t press_x10;
  uint16_t iaq;
};

EnvData env;   // published
EnvData sim;   // simulated target values

// -------------------- BME680 -----------------
Adafruit_BME680 bme;
bool bme_ok = false;

// -------------------- Modbus -----------------
ModbusRTU mb;

// -------------------- Timing -----------------
static const uint32_t SAMPLE_PERIOD_MS = 1000;
uint32_t last_sample_ms = 0;
bool heartbeat = false;

// -------------------- IAQ-ish baseline --------
// NOTE: This is NOT official AQI (PM2.5 etc.). It's an AQI-style 0..500 indicator
// derived from BME680 gas resistance relative to a slowly moving baseline.
float gas_baseline = 0.0f;
bool baseline_ready = false;

static const float BASELINE_ALPHA = 0.005f;   // slower drift baseline (0.5% new per sample)
static const float HUM_TARGET = 40.0f;        // %RH target
static const float HUM_PENALTY_GAIN = 0.5f;   // penalty per %RH away from target (scaled later)

// -------------------- BLE UUIDs --------------
// Use your own if you prefer, but keep them stable once deployed.
static const char* BLE_DEV_NAME   = "FLL-Umbrella-Env";
static const char* UUID_SVC_CTRL  = "12345678-1234-5678-1234-56789abcdef0";
static const char* UUID_CHR_CTRL  = "12345678-1234-5678-1234-56789abcdef1";

volatile bool ble_connected = false;

// ---------- Helpers ----------
static inline uint16_t clamp_u16(int32_t v, uint16_t lo, uint16_t hi) {
  if (v < (int32_t)lo) return lo;
  if (v > (int32_t)hi) return hi;
  return (uint16_t)v;
}

uint16_t computeIAQFromGas(uint32_t gas_ohms, float humidity) {
  if (gas_ohms < 100) gas_ohms = 100; // avoid divide weirdness

  if (!baseline_ready) {
    gas_baseline = (float)gas_ohms;
    baseline_ready = true;
  } else {
    gas_baseline = gas_baseline * (1.0f - BASELINE_ALPHA) + (float)gas_ohms * BASELINE_ALPHA;
  }

  // worse air tends to LOWER gas resistance => baseline/gas increases
  float ratio = (gas_baseline > 1.0f) ? (gas_baseline / (float)gas_ohms) : 1.0f;

  // map ratio to 0..500-ish, tuned to be stable and demo-friendly
  float score = (ratio - 1.0f) * 120.0f;

  // humidity compensation (far from ~40% adds penalty)
  score += fabsf(humidity - HUM_TARGET) * HUM_PENALTY_GAIN;

  if (score < 0) score = 0;
  if (score > 500) score = 500;
  return (uint16_t)(score + 0.5f);
}

// If SIM IAQ not provided, compute a simple heuristic so it still "moves"
uint16_t computeIAQHeuristicSIM(int16_t temp_x100, uint16_t rh_x100, uint16_t press_x10) {
  // Start "good"
  int32_t score = 25;

  float t = temp_x100 / 100.0f;
  float rh = rh_x100 / 100.0f;
  float p = press_x10 / 10.0f;

  // Heuristic penalties (demo knobs)
  // Too hot or too cold => worse
  if (t > 30) score += (int32_t)((t - 30) * 12);
  if (t < 15) score += (int32_t)((15 - t) * 8);

  // Humidity far from 40-55 => worse
  score += (int32_t)(fabsf(rh - 45.0f) * 2.0f);

  // Pressure unusually low (storm vibe) => worse (just for demo)
  if (p < 990) score += (int32_t)((990 - p) * 1.5f);

  return clamp_u16(score, 0, 500);
}

void publishModbus() {
  mb.Hreg(HR_COUNTER,     env.counter);
  mb.Hreg(HR_STATUS,      env.status);
  mb.Hreg(HR_MODE,        env.mode);
  mb.Hreg(HR_ERROR_COUNT, env.error_count);
  mb.Hreg(HR_TEMP_X100,   (uint16_t)env.temp_x100); // int16 stored as uint16
  mb.Hreg(HR_RH_X100,     env.rh_x100);
  mb.Hreg(HR_PRESS_X10,   env.press_x10);
  mb.Hreg(HR_IAQ,         env.iaq);
}

void setup_bme() {
  Wire.begin(I2C_SDA, I2C_SCL, I2C_FREQ);

  if (bme.begin(BME_ADDR_1))      bme_ok = true;
  else if (bme.begin(BME_ADDR_2)) bme_ok = true;
  else                            bme_ok = false;

  if (!bme_ok) return;

  bme.setTemperatureOversampling(BME680_OS_8X);
  bme.setHumidityOversampling(BME680_OS_2X);
  bme.setPressureOversampling(BME680_OS_4X);
  bme.setGasHeater(320, 150); // needed for gas resistance
}

void setup_modbus() {
  // ESP32-C6: Serial0 is USB serial; Serial1 for RS-485 UART
  // If your board core uses different serial mapping, adjust here.
  Serial1.begin(MB_BAUD, SERIAL_8N1, MB_RX, MB_TX);

  mb.begin(&Serial1);
  mb.slave(SLAVE_ID);

  for (uint16_t r = 0; r < HR_REG_COUNT; r++) {
    mb.addHreg(r, 0);
  }
  publishModbus();
}

// ---------- BLE Server ----------
class CtrlCallbacks : public NimBLECharacteristicCallbacks {
  void onWrite(NimBLECharacteristic* pChr) override {
    std::string v = pChr->getValue();
    if (v.size() < 2) return;

    const uint8_t* b = (const uint8_t*)v.data();
    uint8_t cmd = b[0];

    if (cmd == 0x01) { // set mode
      uint16_t m = b[1] ? MODE_SIM : MODE_LIVE;
      env.mode = m;
      // keep sim values as-is; just switching source
    } else if (cmd == 0x02) { // set sim values
      if (v.size() < 7) return;

      auto u16le = [&](int idx) -> uint16_t {
        return (uint16_t)b[idx] | ((uint16_t)b[idx + 1] << 8);
      };
      auto i16le = [&](int idx) -> int16_t {
        return (int16_t)u16le(idx);
      };

      sim.temp_x100 = i16le(1);
      sim.rh_x100   = u16le(3);
      sim.press_x10 = u16le(5);

      if (v.size() >= 9) {
        sim.iaq = u16le(7);
      } else {
        sim.iaq = computeIAQHeuristicSIM(sim.temp_x100, sim.rh_x100, sim.press_x10);
      }
    }
  }
};

class ServerCallbacks : public NimBLEServerCallbacks {
  void onConnect(NimBLEServer* pServer) override { ble_connected = true; }
  void onDisconnect(NimBLEServer* pServer) override {
    ble_connected = false;
    // keep advertising
    NimBLEDevice::startAdvertising();
  }
};

void setup_ble() {
  NimBLEDevice::init(BLE_DEV_NAME);
  NimBLEDevice::setPower(ESP_PWR_LVL_P9);

  NimBLEServer* server = NimBLEDevice::createServer();
  server->setCallbacks(new ServerCallbacks());

  NimBLEService* svc = server->createService(UUID_SVC_CTRL);

  NimBLECharacteristic* chr = svc->createCharacteristic(
    UUID_CHR_CTRL,
    NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_NR
  );
  chr->setCallbacks(new CtrlCallbacks());

  svc->start();

  NimBLEAdvertising* adv = NimBLEDevice::getAdvertising();
  adv->addServiceUUID(UUID_SVC_CTRL);
  adv->setScanResponse(true);
  adv->start();
}

// ---------- Main ----------
void setup() {
  Serial.begin(115200);
  delay(200);

  Serial.println("\n=== ESP32-C6 BME680 + BLE SIM -> Modbus RTU (HR0..HR7) ===");
  Serial.printf("I2C SDA/SCL: %d/%d\n", I2C_SDA, I2C_SCL);
  Serial.printf("Modbus RX/TX: %d/%d @ %lu\n", MB_RX, MB_TX, (unsigned long)MB_BAUD);
  Serial.printf("Slave ID: %u\n", SLAVE_ID);

  // init defaults
  env.counter = 0;
  env.status = 0;
  env.mode = MODE_LIVE;
  env.error_count = 0;
  env.temp_x100 = 2500;
  env.rh_x100 = 5000;
  env.press_x10 = 10132;
  env.iaq = 25;

  // default sim values
  sim = env;

  setup_bme();
  Serial.printf("BME680 OK: %s\n", bme_ok ? "YES" : "NO");

  setup_modbus();
  setup_ble();

  Serial.println("Modbus ready (FC03 HR0..HR7). BLE control ready.");
}

void loop() {
  mb.task();

  uint32_t now = millis();
  if (now - last_sample_ms < SAMPLE_PERIOD_MS) return;
  last_sample_ms = now;

  // heartbeat
  heartbeat = !heartbeat;

  env.counter++;

  // rebuild status fresh each tick (don’t accumulate stale bits)
  uint16_t st = 0;
  if (heartbeat) st |= ST_NEW_SAMPLE;
  if (ble_connected) st |= ST_BLE_CONNECTED;
  if (env.mode == MODE_SIM) st |= ST_SIM_ACTIVE;

  bool used_fallback = false;
  bool update_ok = false;

  if (env.mode == MODE_SIM) {
    // publish simulated values
    env.temp_x100 = sim.temp_x100;
    env.rh_x100   = sim.rh_x100;
    env.press_x10 = sim.press_x10;
    env.iaq       = sim.iaq;  // may be written by BLE or auto heuristic
    update_ok = true;
  } else {
    // LIVE mode
    if (bme_ok && bme.performReading()) {
      float t = bme.temperature;           // C
      float h = bme.humidity;              // %
      float p_hpa = bme.pressure / 100.0f; // Pa -> hPa
      uint32_t gas = (uint32_t)bme.gas_resistance;

      env.temp_x100 = (int16_t)(t * 100.0f);
      env.rh_x100   = (uint16_t)(h * 100.0f);
      env.press_x10 = (uint16_t)(p_hpa * 10.0f);
      env.iaq       = computeIAQFromGas(gas, h);

      update_ok = true;
    } else {
      env.error_count++;
      used_fallback = true;
      update_ok = false;

      // keep last good values (or defaults if never had good)
      // env.* remain unchanged
    }
  }

  if (update_ok) st |= ST_OK;
  if (used_fallback) st |= ST_FALLBACK;

  env.status = st;

  publishModbus();

  // debug print (kept short)
  Serial.printf("HR0=%u HR2=%u T=%.2f RH=%.2f P=%.1f IAQ=%u ST=0x%04X Err=%u BLE=%c\n",
    env.counter,
    env.mode,
    env.temp_x100 / 100.0f,
    env.rh_x100 / 100.0f,
    env.press_x10 / 10.0f,
    env.iaq,
    env.status,
    env.error_count,
    ble_connected ? 'Y' : 'N'
  );
}
