#include "Zigbee.h"

#define RX2_PIN 4
#define TX2_PIN 5

// Normal mode command
#define COMMAND "FDFCFBFA0800120000006400000004030201"
#define PRESENCE_DISTANCE 100
#define COUNT_STATUS_DIFFERENCE 5

// Zigbee
#define OCCUPANCY_SENSOR_ENDPOINT_NUMBER 1
#define ZCL_OCCUPANCY_SENSOR_CLUSTER_SERVER 
ZigbeeOccupancySensor zbOccupancySensor = ZigbeeOccupancySensor(OCCUPANCY_SENSOR_ENDPOINT_NUMBER);

bool isPresent = false;
int countDifferent = 0;
 
void setup() {
  initSerial();
  sendCommandAsHex(COMMAND);
  Serial.println("Waiting for sensor readings...");
  initZigbee();
}
 
void loop() {
  readAndProcessSensorLines();
  delay(500);
}

void initSerial() {
  // Start the primary serial communication (USB Monitor)
  Serial.begin(115200);
  unsigned long startAttemptTime = millis();
  while (!Serial && millis() - startAttemptTime < 2000) {
    delay(100);
  }
  Serial.println("Serial Monitor Initialized.");

  // Start Serial2 for the HMMD Sensor
  Serial2.begin(115200, SERIAL_8N1, RX2_PIN, TX2_PIN);
  Serial.println("Serial2 Initialized on RX:" + String(RX2_PIN) + ", TX:" + String(TX2_PIN));
}

void initZigbee() {
  zbOccupancySensor.setManufacturerAndModel("EspressIf", "Presence detector");
  zbOccupancySensor.allowMultipleBinding(true);
  Serial.println("Adding ZigbeeSwitch endpoint to Zigbee Core");
  Zigbee.addEndpoint(&zbOccupancySensor);

  if (!Zigbee.begin()) {
    ESP.restart();
  }

  while (!Zigbee.connected()) {
    delay(100);
  }
}

void sendCommandAsHex(String hexString) {
  int hexStringLength = hexString.length();
  if (hexStringLength % 2 != 0) {
    Serial.println("Error: Hex string must have an even number of characters.");
    return;
  }
  int byteCount = hexStringLength / 2;
  byte hexBytes[byteCount];
  for (int i = 0; i < hexStringLength; i += 2) {
    String byteString = hexString.substring(i, i + 2);
    byte hexByte = (byte)strtoul(byteString.c_str(), NULL, 16);
    hexBytes[i / 2] = hexByte;
  }
  // Print confirmation of what's being sent
  Serial.print("Sending ");
  Serial.print(byteCount);
  Serial.print(" bytes: ");
  for(int i=0; i<byteCount; i++) {
    if (hexBytes[i] < 16) Serial.print("0");;
  }
  Serial.println();
  // Send the data
  Serial2.write(hexBytes, byteCount);
  Serial.println("Initial command sent to Serial2.");
}
 
void readAndProcessSensorLines() {
  // Check if data is available on Serial2
  while (Serial2.available() > 0) {
    // Read a line of text until a newline character (\n) is received
    // The timeout helps prevent blocking forever if a line ending is missed
    String line = Serial2.readStringUntil('\n');
 
    // Clean up the line: remove potential carriage return (\r) and leading/trailing whitespace
    line.trim();
 
    // Check if the line contains the "Range" information
    if (line.startsWith("Range ")) {
      // Extract the substring after "Range "
      String distanceStr = line.substring(6);
      int distance = distanceStr.toInt();
      bool currentStatus = distance <= PRESENCE_DISTANCE;
      
      if (currentStatus != isPresent) {
        countDifferent++;
      } else {
        countDifferent = 0;
      }
 
      // change the status, if the last n updates were different than the current status
      if (countDifferent >= COUNT_STATUS_DIFFERENCE) {
        isPresent = !isPresent;
        bool res = sendZigbeeOnOffCommand(isPresent);
        Serial.print("Message sent: ");
        Serial.print(isPresent);
        Serial.print(" - successful: ");
        Serial.println(res);
      }
    }
  }
}

bool sendZigbeeOnOffCommand(bool isPresent) {
  bool result1 = zbOccupancySensor.setOccupancy(isPresent);
  bool result2 = zbOccupancySensor.report();
  return result2;
}