#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

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*/10, /*keep*/4)){
    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");
}

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
    if (radar.motionDetection()) {
      Serial.println("MOTION DETECTED");
    }
    
  } 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);
}