#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


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");






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

  // 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();

}

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());
  }
  
  delay(100);
}