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

#include <ModbusRTU.h>   // from modbus-esp8266 library

// -------------------- I2C (MATCH YOUR WORKING CODE) --------------------
static const int I2C_SDA = 22;
static const int I2C_SCL = 21;
static const uint32_t I2C_FREQ = 100000;

// Try both common addresses (your working sketch likely uses one of these)
static const uint8_t BME_ADDR_1 = 0x76;
static const uint8_t BME_ADDR_2 = 0x77;

// -------------------- MODBUS UART (MATCH YOUR WORKING UART PINS) --------
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 ---------------------------------
ModbusRTU mb;

enum {
  HR_COUNTER         = 0,
  HR_TEMP_X100       = 1,
  HR_RH_X100         = 2,
  HR_PRESS_HPA_X10   = 3,
  HR_GAS_OHMS_LO     = 4,
  HR_GAS_OHMS_HI     = 5,
  HR_STATUS          = 6,
  HR_ERROR_COUNT     = 7,
  HR_REG_COUNT       = 8
};

// status bits
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);

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

// -------------------- Values / timing ----------------------------------
uint16_t counter = 0;
uint16_t error_count = 0;
bool sample_toggle = false;

int16_t  temp_x100 = 2500;   // fallback 25.00 C
uint16_t rh_x100   = 5000;   // fallback 50.00 %
uint16_t press_x10 = 10132;  // fallback 1013.2 hPa
uint32_t gas_ohms  = 123456; // fallback

static const uint32_t SAMPLE_PERIOD_MS = 1000;
uint32_t last_sample_ms = 0;

static inline void write_u32_to_regs(uint16_t loReg, uint16_t hiReg, uint32_t v) {
  mb.Hreg(loReg, (uint16_t)(v & 0xFFFF));
  mb.Hreg(hiReg, (uint16_t)((v >> 16) & 0xFFFF));
}

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;
    return;
  }

  // Oversampling (sane defaults)
  bme.setTemperatureOversampling(BME680_OS_8X);
  bme.setHumidityOversampling(BME680_OS_2X);
  bme.setPressureOversampling(BME680_OS_4X);

  // Heater needed for gas resistance
  bme.setGasHeater(320, 150);

  bme_ok = true;
}

void setup_modbus() {
  // Use Serial1 on ESP32-C6 (Serial0 is USB serial)
  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);
  }

  // initialize
  mb.Hreg(HR_COUNTER, counter);
  mb.Hreg(HR_TEMP_X100, (uint16_t)temp_x100);
  mb.Hreg(HR_RH_X100, rh_x100);
  mb.Hreg(HR_PRESS_HPA_X10, press_x10);
  write_u32_to_regs(HR_GAS_OHMS_LO, HR_GAS_OHMS_HI, gas_ohms);
  mb.Hreg(HR_ERROR_COUNT, error_count);

  uint16_t st = 0;
  if (bme_ok) st |= ST_OK;
  else st |= ST_FALLBACK;
  mb.Hreg(HR_STATUS, st);
}

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

  Serial.println("=== ESP32-C6 BME680 -> Modbus RTU (Holding Registers) ===");
  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);

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

  setup_modbus();
  Serial.println("Modbus ready. Read HR0..HR7 with FC03.");
}

void loop() {
  mb.task();

  const uint32_t now = millis();
  if (now - last_sample_ms >= SAMPLE_PERIOD_MS) {
    last_sample_ms = now;
    counter++;

    bool used_fallback = false;

    if (bme_ok) {
      if (bme.performReading()) {
        float t = bme.temperature;            // C
        float h = bme.humidity;               // %
        float p_hpa = bme.pressure / 100.0f;  // Pa -> hPa
        uint32_t g = (uint32_t)bme.gas_resistance; // ohms

        temp_x100 = (int16_t)(t * 100.0f);
        rh_x100   = (uint16_t)(h * 100.0f);
        press_x10 = (uint16_t)(p_hpa * 10.0f);
        gas_ohms  = g;
      } else {
        error_count++;
        used_fallback = true;
      }
    } else {
      used_fallback = true;
    }

    // update modbus registers
    mb.Hreg(HR_COUNTER, counter);
    mb.Hreg(HR_TEMP_X100, (uint16_t)temp_x100);
    mb.Hreg(HR_RH_X100, rh_x100);
    mb.Hreg(HR_PRESS_HPA_X10, press_x10);
    write_u32_to_regs(HR_GAS_OHMS_LO, HR_GAS_OHMS_HI, gas_ohms);
    mb.Hreg(HR_ERROR_COUNT, error_count);

    sample_toggle = !sample_toggle;

    uint16_t st = 0;
    if (bme_ok && !used_fallback) st |= ST_OK;
    if (used_fallback) st |= ST_FALLBACK;
    if (sample_toggle) st |= ST_NEW_SAMPLE;
    mb.Hreg(HR_STATUS, st);

    // debug
    Serial.printf("Tick %u | T=%.2fC RH=%.2f%% P=%.1fhPa Gas=%luΩ Status=0x%X Err=%u\n",
                  counter,
                  temp_x100 / 100.0f,
                  rh_x100 / 100.0f,
                  press_x10 / 10.0f,
                  (unsigned long)gas_ohms,
                  st,
                  error_count);
  }
}
