#ifndef ZIGBEE_MODE_ED
#error "Zigbee end device mode is not selected in Tools->Zigbee mode"
#endif

#include "Zigbee.h"

/* Zigbee occupancy sensor configuration */
#define OCCUPANCY_SENSOR_ENDPOINT_NUMBER 10
uint8_t button = BOOT_PIN;
uint8_t sensor_pin = 4;

ZigbeeOccupancySensor zbOccupancySensor = ZigbeeOccupancySensor(OCCUPANCY_SENSOR_ENDPOINT_NUMBER);


#include "DFRobot_C4001.h"

HardwareSerial RadarSerial(1);
DFRobot_C4001_UART radar(&RadarSerial, 9600, /*rx*/17, /*tx*/16);  // ESP32-C6 pins

eMode_t currentMode = eExitMode;  // 0=presence, 1=speed
unsigned long lastSpeedReport = 0;
bool lastOccupancy = false;
unsigned long occupancyStartTime = 0;
const unsigned long OCCUPANCY_TIMEOUT = 4;  // 4 -> 2 seconds - adjust as needed
const unsigned long TRIGGER_DELAY = 10; // 10 -> 100 ms







#define CARBON_DIOXIDE_SENSOR_ENDPOINT_NUMBER 11
ZigbeeCarbonDioxideSensor zbCarbonDioxideSensor = ZigbeeCarbonDioxideSensor(CARBON_DIOXIDE_SENSOR_ENDPOINT_NUMBER);

#include <DFRobot_SCD4X.h>
// DFRobot_SCD4X SCD4X(&Wire, /*i2cAddr = */SCD4X_I2C_ADDR);

// Define your custom SCL/SDA pins  
#define I2C_SCL_PIN 20  // Change to your SCL pin
#define I2C_SDA_PIN 6  // Change to your SDA pin
// Create custom I2C instance
TwoWire sharedI2C = TwoWire(0);  // I2C0 (or TwoWire(1) for I2C1)
// SCD41 with custom I2C
DFRobot_SCD4X SCD4X(&sharedI2C, SCD4X_I2C_ADDR);




#define TEMP_SENSOR_ENDPOINT_NUMBER 12
ZigbeeTempSensor zbTempSensor_SCD4X = ZigbeeTempSensor(TEMP_SENSOR_ENDPOINT_NUMBER);








// #include <bme68xLibrary.h>
// Bme68x bme;
// TwoWire BME68X_I2C = TwoWire(0);  // I2C0 (or TwoWire(1) for I2C1)
// #include <DFRobot_BME680.h>  // BME680 library
// DFRobot_BME680_I2C bme680(0x76);  // 0x76
#include <Adafruit_Sensor.h>
#include "Adafruit_BME680.h"
Adafruit_BME680 bme680;

unsigned long bme_report_time = millis();

#define TEMP_SENSOR_ENDPOINT_NUMBER 13
ZigbeeTempSensor zbTempSensor_BME68X = ZigbeeTempSensor(TEMP_SENSOR_ENDPOINT_NUMBER);
#define ANALOG_DEVICE_ENDPOINT_NUMBER 14
ZigbeeAnalog zbAnalogDevice_BME68X = ZigbeeAnalog(ANALOG_DEVICE_ENDPOINT_NUMBER);








void setPresenceMode() {
  radar.setSensorMode(eExitMode);
  currentMode = eExitMode;
  Serial.println("Switched to PRESENCE mode");
  // Presence mode settings from sample
  // radar.setDetectionRange(30, 1000, 1000);  // min,max,trig (cm)
  // radar.setTrigSensitivity(1);
  // radar.setKeepSensitivity(2);
  // radar.setDelay(100, 4);  // trig,keep






  sSensorStatus_t data;
  data = radar.getStatus();
  //  0 stop  1 start
  Serial.print("work status  = ");
  Serial.println(data.workStatus);

  //  0 is exist   1 speed
  Serial.print("work mode  = ");
  Serial.println(data.workMode);

  //  0 no init    1 init success
  Serial.print("init status = ");
  Serial.println(data.initStatus);
  Serial.println();

  /*
   * min Detection range Minimum distance, unit cm, range 0.3~25m (30~2500), not exceeding max, otherwise the function is abnormal.
   * max Detection range Maximum distance, unit cm, range 2.4~25m (240~2500)
   * trig Detection range Maximum distance, unit cm, default trig = max
   */
  if(radar.setDetectionRange(/*min*/30, /*max*/1000, /*trig*/1000)){
    Serial.println("set detection range successfully!");
  }
  // set trigger sensitivity 0 - 9
  if(radar.setTrigSensitivity(1)){
    Serial.println("set trig sensitivity successfully!");
  }

  // set keep sensitivity 0 - 9
  if(radar.setKeepSensitivity(2)){
    Serial.println("set keep sensitivity successfully!");
  }
  /*
   * trig Trigger delay, unit 0.01s, range 0~2s (0~200)
   * keep Maintain the detection timeout, unit 0.5s, range 2~1500 seconds (4~3000)
   */
  if(radar.setDelay(/*trig*/TRIGGER_DELAY, /*keep*/OCCUPANCY_TIMEOUT)){
    Serial.println("set delay successfully!");
  }


  // get confige params
  Serial.print("trig sensitivity = ");
  Serial.println(radar.getTrigSensitivity());
  Serial.print("keep sensitivity = ");
  Serial.println(radar.getKeepSensitivity());

  Serial.print("min range = ");
  Serial.println(radar.getMinRange());
  Serial.print("max range = ");
  Serial.println(radar.getMaxRange());
  Serial.print("trig range = ");
  Serial.println(radar.getTrigRange());

  Serial.print("keep time = ");
  Serial.println(radar.getKeepTimerout());

  Serial.print("trig delay = ");
  Serial.println(radar.getTrigDelay());



}







void setSpeedMode() {
  radar.setSensorMode(eSpeedMode);
  currentMode = eSpeedMode;
  Serial.println("Switched to SPEED mode");
  // Speed mode settings from sample
  // radar.setDetectThres(11, 1200, 10);  // min,max,thres (cm)
  // radar.setFrettingDetection(eON);




  sSensorStatus_t data;
  data = radar.getStatus();
  //  0 stop  1 start
  Serial.print("work status  = ");
  Serial.println(data.workStatus);

  //  0 is exist   1 speed
  Serial.print("work mode  = ");
  Serial.println(data.workMode);

  //  0 no init    1 init success
  Serial.print("init status = ");
  Serial.println(data.initStatus);
  Serial.println();

  /*
   * min Detection range Minimum distance, unit cm, range 0.3~20m (30~2500), not exceeding max, otherwise the function is abnormal.
   * max Detection range Maximum distance, unit cm, range 2.4~20m (240~2500)
   * thres Target detection threshold, dimensionless unit 0.1, range 0~6553.5 (0~65535)
   */
  if (radar.setDetectThres(/*min*/ 11, /*max*/ 1200, /*thres*/ 10)) {
    Serial.println("set detect threshold successfully");
  }

  // set Fretting Detection
  radar.setFrettingDetection(eON);

  // get confige params
  Serial.print("min range = ");
  Serial.println(radar.getTMinRange());
  Serial.print("max range = ");
  Serial.println(radar.getTMaxRange());
  Serial.print("threshold range = ");
  Serial.println(radar.getThresRange());
  Serial.print("fretting detection = ");
  Serial.println(radar.getFrettingDetection());
  
}



void handleSerialCommands() {
  while (Serial.available()) {
    char cmd = Serial.read();
    if (cmd == 'P' || cmd == 'p') {
      setPresenceMode();
    } else if (cmd == 'S' || cmd == 's') {
      setSpeedMode();
    }
  }
  
}

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

  RadarSerial.begin(9600, SERIAL_8N1, 17, 16);  // C4001 UART (RX=16, TX=17)

  while (!radar.begin()) {
    Serial.println("NO Device found!");
    delay(1000);
  }
  Serial.println("C4001 connected!");

  // Start in presence mode
  setPresenceMode();
  Serial.println("Send 'P' for presence, 'S' for speed mode");




  // Initialize custom I2C with YOUR pins
  // SCD41_I2C.begin(SCD41_SDA_PIN, SCD41_SCL_PIN, 100000);  // 100kHz standard
  // Initialize SHARED I2C bus ONCE
  sharedI2C.begin(I2C_SDA_PIN, I2C_SCL_PIN, 100000);

  while( !SCD4X.begin() ){
    Serial.println("Communication with device failed, please check connection");
    delay(3000);
  }
  Serial.println("SCD4x: Begin ok!");
  SCD4X.enablePeriodMeasure(SCD4X_STOP_PERIODIC_MEASURE);

  SCD4X.setTempComp(4.0);
  float temp = 0;
  temp = SCD4X.getTempComp();
  Serial.print("The current temperature compensation value : ");
  Serial.print(temp);
  Serial.println(" C");

  SCD4X.setSensorAltitude(540);
  uint16_t altitude = 0;
  altitude = SCD4X.getSensorAltitude();
  Serial.print("Set the current environment altitude : ");
  Serial.print(altitude);
  Serial.println(" m");

  SCD4X.enablePeriodMeasure(SCD4X_START_PERIODIC_MEASURE);

  // Set minimum and maximum carbon dioxide measurement value in ppm
  zbCarbonDioxideSensor.setMinMaxValue(0, 1500);
  // Add endpoints to Zigbee Core
  Zigbee.addEndpoint(&zbCarbonDioxideSensor);

  // Set minimum and maximum temperature measurement value (10-50°C is default range for chip temperature measurement)
  zbTempSensor_SCD4X.setMinMaxValue(10, 50);
  // Optional: Set tolerance for temperature measurement in °C (lowest possible value is 0.01°C)
  zbTempSensor_SCD4X.setTolerance(1);
  // Optional: Time cluster configuration (default params, as this device will revieve time from coordinator)
  // zbTempSensor.addTimeCluster();
  // Add humidity cluster to the temperature sensor device with min, max and tolerance values
  zbTempSensor_SCD4X.addHumiditySensor(0, 100, 1);
  // Add endpoint to Zigbee Core
  Zigbee.addEndpoint(&zbTempSensor_SCD4X);





  // // Init BME680
  // if (bme680.begin(sharedI2C) != 0) {
  //   Serial.println("BME680 init failed!");
  // } else {
  //   Serial.println("BME680 ready!");
  // }
  // Wire.begin(SCD41_SDA_PIN, SCD41_SCL_PIN);
  // Wire.setClock(100000); // Set I2C clock speed to 100kHz
  // // Initialize the BME680
  // bme.begin(BME68X_I2C_ADDR_HIGH, Wire); // BME68X_I2C_ADDR_HIGH=0x76, BME68X_I2C_ADDR_LOW=0x77
  // bme.begin(BME68X_I2C_ADDR_HIGH, &BME68X_I2C); // BME68X_I2C_ADDR_HIGH=0x76, BME68X_I2C_ADDR_LOW=0x77
  // bme680.begin()
  // // Set up the BME680
  // // bme.setGasStatus(BME680_ENABLE_GAS_MEAS);
  // // bme.setOversampling(T2, OS_2X);
  // // bme.setOversampling(T1, OS_16X);
  // // bme.setOversampling(H1, OS_2X);
  // // bme.setFilter(FILTER_SIZE_3);
  // // bme.setGasSettings(320, 25, 0x4, 130, 410, 4, 0x01, 0x10, 0);
  // if(bme.checkStatus())
  // {
  //   if (bme.checkStatus() == BME68X_ERROR)
  //   {
  //     Serial.println("Sensor error:" + bme.statusString());
  //     return;
  //   }
  //   else if (bme.checkStatus() == BME68X_WARNING)
  //   {
  //     Serial.println("Sensor Warning:" + bme.statusString());
  //   }
  // }
  // /* Set the default configuration for temperature, pressure and humidity */
  // bme.setTPH();
  // /* Set the heater configuration to 300 deg C for 100ms for Forced mode */
  // bme.setHeaterProf(300, 100);

  // bme68xData data;
  // bme.setOpMode(BME68X_FORCED_MODE);


  Serial.println("Connecting to BME680...");
  
  // Note the syntax: .begin(Address, TwoWirePointer)
  if (!bme680.begin(0x77, &sharedI2C)) { 
    Serial.println("BME680 not found at 0x77, trying 0x76...");
    if (!bme680.begin(0x76, &sharedI2C)) {
      Serial.println("BME680 Init Failed! Check wiring.");
      while (1);
    }
  }
  Serial.println("BME680 initialized via TwoWire.");
  // BME680 Settings
  bme680.setTemperatureOversampling(BME680_OS_8X);
  bme680.setHumidityOversampling(BME680_OS_2X);
  bme680.setPressureOversampling(BME680_OS_4X);
  bme680.setIIRFilterSize(BME680_FILTER_SIZE_3);
  bme680.setGasHeater(320, 150);

  zbTempSensor_BME68X.setMinMaxValue(-20, 80);
  zbTempSensor_BME68X.setTolerance(1);
  zbTempSensor_BME68X.addHumiditySensor(0, 100, 1);
  Zigbee.addEndpoint(&zbTempSensor_BME68X);


  // Set analog resolution to 10 bits
  // analogReadResolution(10);
  // Add analog clusters to Zigbee Analog according your needs
  // zbAnalogDevice_BME68X.addAnalogInput();
  // // Add endpoints to Zigbee Core
  // Zigbee.addEndpoint(&zbAnalogDevice_BME68X);





  // Optional: set Zigbee device name and model
  zbOccupancySensor.setManufacturerAndModel("Espressif", "ZigbeeOccupancyPIRSensor_Node17");
  
  zbOccupancySensor.setPowerSource(ZB_POWER_SOURCE_BATTERY, 100, 35);

  // Add endpoint to Zigbee Core
  Zigbee.addEndpoint(&zbOccupancySensor);





  Serial.println("Starting Zigbee...");
  // When all EPs are registered, start Zigbee in End Device mode
  if (!Zigbee.begin()) {
    Serial.println("Zigbee failed to start!");
    Serial.println("Rebooting...");
    ESP.restart();
  } else {
    Serial.println("Zigbee started successfully!");
  }
  Serial.println("Connecting to network");
  while (!Zigbee.connected()) {
    Serial.print(".");
    delay(100);
  }
  Serial.println();


  // Set reporting interval for carbon dioxide measurement to be done every 30 seconds, must be called after Zigbee.begin()
  // min_interval and max_interval in seconds, delta (carbon dioxide change in ppm)
  // if min = 1 and max = 0, reporting is sent only when carbon dioxide changes by delta
  // if min = 0 and max = 10, reporting is sent every 10 seconds or when carbon dioxide changes by delta
  // if min = 0, max = 10 and delta = 0, reporting is sent every 10 seconds regardless of delta change
  zbCarbonDioxideSensor.setReporting(0, 30, 0);
  zbTempSensor_SCD4X.setReporting(1, 0, 1);
  // zbTempSensor_BME68X.setReporting(1, 0, 1);
  // zbAnalogDevice.setAnalogInputReporting(0, 30, 10);  // report every 30 seconds if value changes by 10

}

void loop() {
  handleSerialCommands();

  // // Remove if (radar.available()) - just read directly
  // sSensorStatus_t status = radar.getStatus();  // Always safe to call
  
  // Serial.print("Status: work=");
  // Serial.print(status.workStatus);
  // Serial.print(", mode=");
  // Serial.print(status.workMode);
  // Serial.print(", init=");
  // Serial.println(status.initStatus);


  if (currentMode == eExitMode) {
    // PRESENCE MODE
    // static bool occupancy = false;
    // bool occupancyNow = radar.motionDetection();
    // Serial.println(occupancyNow);
    // // if (radar.motionDetection() == 1 && !occupancy) {
    // if (occupancyNow != lastOccupancy) {
    //   Serial.println("MOTION DETECTED");
    //   zbOccupancySensor.setOccupancy(occupancyNow);
    //   zbOccupancySensor.report();
    //   // occupancy = true;
    //   lastOccupancy = occupancyNow;
    // // } else if (radar.motionDetection() == 0 && occupancy) {
    // // } else if (radar.motionDetection() == 0) {
    // //   Serial.println("MOTION ENDED");
    // //   zbOccupancySensor.setOccupancy(false);
    // //   zbOccupancySensor.report();
    // //   occupancy = false;
    // }
    // PRESENCE MODE - proper state machine
    bool motionNow = radar.motionDetection();
    Serial.println(motionNow);
    Serial.println(lastOccupancy);
    Serial.println(millis() - occupancyStartTime);

    if (motionNow && !lastOccupancy) {
      // Motion STARTED → Set occupancy ON
      Serial.println("MOTION DETECTED → OCCUPANCY ON");
      lastOccupancy = true;
      occupancyStartTime = millis();
      zbOccupancySensor.setOccupancy(true);
      zbOccupancySensor.report();
      
    } else if (motionNow && lastOccupancy) {
      // Motion ENDED → Start timeout timer
      Serial.println("Motion still present → timeout reset");
      occupancyStartTime = millis();

    } else if (!motionNow && lastOccupancy && (millis() - occupancyStartTime > OCCUPANCY_TIMEOUT*1000/2)) {
      // TIMEOUT → Clear occupancy
      Serial.println("OCCUPANCY TIMEOUT → OFF");
      lastOccupancy = false;
      zbOccupancySensor.setOccupancy(false);
      zbOccupancySensor.report();
      
    } else if (!motionNow && lastOccupancy) {
      // Motion ENDED → Start timeout timer
      Serial.println("Motion ended → timeout started");
      // occupancyStartTime = millis();
    }



  } else if (currentMode == eSpeedMode) {
    // SPEED MODE
    Serial.print("Targets: ");
    Serial.print(radar.getTargetNumber());
    Serial.print(" | Speed: ");
    Serial.print(radar.getTargetSpeed(), 2);
    Serial.print(" m/s | Range: ");
    Serial.print(radar.getTargetRange(), 2);
    Serial.print(" m | Energy: ");
    Serial.println(radar.getTargetEnergy());
  }



  if(SCD4X.getDataReadyStatus()) {
    DFRobot_SCD4X::sSensorMeasurement_t data;
    SCD4X.readMeasurement(&data);

    Serial.print("Carbon dioxide concentration : ");
    Serial.print(data.CO2ppm);
    Serial.println(" ppm");

    Serial.print("Environment temperature : ");
    Serial.print(data.temp);
    Serial.println(" C");

    Serial.print("Relative humidity : ");
    Serial.print(data.humidity);
    Serial.println(" RH");

    Serial.println();

    zbCarbonDioxideSensor.setCarbonDioxide(data.CO2ppm);
    zbCarbonDioxideSensor.report();

    // Update temperature and humidity values in Temperature sensor EP
    zbTempSensor_SCD4X.setTemperature(data.temp);
    zbTempSensor_SCD4X.setHumidity(data.humidity);
    // Report temperature and humidity values
    zbTempSensor_SCD4X.report();  // reports temperature and humidity values (if humidity sensor is not added, only temperature is reported)
    // Serial.printf("Reported temperature: %.2f°C, Humidity: %.2f%%\r\n", temperature, humidity);
  
    Serial.print("SCD41 reported.");

  
  }


  // float temperature(NAN), humidity(NAN), pressure(NAN), gasResistance(NAN);
  // bme68xData data_BME68X;
  // delayMicroseconds(bme.getMeasDur());
  // Serial.print("Debug 1.");
  // if (bme.fetchData()) {
  //     bme.getData(data_BME68X);
  //     temperature = data_BME68X.temperature; // Temperature in °C
  //     humidity = data_BME68X.humidity;       // Humidity in %
  //     pressure = data_BME68X.pressure;       // Pressure in hPa
  //     gasResistance = data_BME68X.gas_resistance; // Gas resistance in ohms
  //     // errorCount = 0; // Reset error count on successful read

  //     // zbTempSensor_BME68X.setTemperature(temperature);
  //     // zbTempSensor_BME68X.setHumidity(humidity);
  //     // // Report temperature and humidity values
  //     // zbTempSensor_BME68X.report();  // reports temperature and humidity values (if humidity sensor is not added, only temperature is reported)

  //     // zbAnalogDevice_BME68X.setAnalogInput(gasResistance);
  //     // zbAnalogDevice_BME68X.reportAnalogInput();

  //     Serial.print("BME68X reported.");

  // }

  // Read BME680 (temperature, humidity, pressure, gas)
  // if (bme680.performReading()) {
  //   Serial.printf("BME680 - T:%.1f°C H:%.1f%% P:%.0fhPa G:%.0f kOhm\n",
  //     bme680.getTemperature(), bme680.getHumidity(),
  //     bme680.getPressure() / 100.0, bme680.getGas_resistance() / 1000.0);
  //   Serial.print("BME68X reported.");
  // }
  bool bmeReady = bme680.performReading();
  if (bmeReady && (millis() - bme_report_time > 5000)) {
    Serial.print("BME680 -> Gas: "); Serial.print(bme680.gas_resistance / 1000.0); 
    Serial.print(" KOhms | Temp: "); Serial.print(bme680.temperature); Serial.println(" C");
    bme_report_time = millis();
  } else {
    Serial.println("BME680 -> Waiting...");
  }



  
  delay(100);
}