#include <ESP32Servo.h>
#include <WiFi.h>
#include <PubSubClient.h>
#include <WiFiClientSecure.h>
#include <ArduinoJson.h>
#include <LiquidCrystal_I2C.h>
#include <ESPDateTime.h>
#include<WiFiUdp.h>
#include<NTPClient.h>

// WiFi
const char *ssid = "Wokwi-GUEST";
const char *password = "";

// MQTT
const char *mqtt_server = "test.mosquitto.org";
const char *button_topic = "/button_status";
const char *mode_topic = "/mode_control";
const char *servo_control_topic = "/servo_control";
const char *distance_topic = "/Distance";
const char *number_of_cars = "/cars_count";
const char *peak_hours_topic = "/car_detection/peak_hours";
const char *system_state_topic = "/system_state";
const char *car_data_topic = "/Car_Data"; // Modified MQTT topic

// Ultrasonic Sensor
#define TRIGGER_PIN 2  // Pin connected to the trigger pin of the ultrasonic sensor
#define ECHO_PIN 5     // Pin connected to the echo pin of the ultrasonic sensor

// Servo
Servo s;

// LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

// PubSubClient
WiFiClient espClient;
PubSubClient client(espClient);

// Car Detection
const int MAX_HISTORY_SIZE = 100;
unsigned long historyTimestamps[MAX_HISTORY_SIZE];
int historyCarCount[MAX_HISTORY_SIZE];
int historyIndex = 0;
int carCount = 0;
bool carDetected = false;
bool prevCarDetected = false;
bool automaticMode = true;
bool systemActive = true;

// Other Variables
unsigned long lastMsg = 0;
int value = 0;

// Function forward declarations
int analyzePeakHour(unsigned long *timestamps, int *carCounts, int historySize);
int hourOfDay(unsigned long timestamp);

// NTP
WiFiUDP ntpUDP;
NTPClient timeClient(ntpUDP, "pool.ntp.org");

void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  randomSeed(micros());

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

int lastServoPosition = 90;
bool messagePrinted = false;

void callback(char *topic, byte *payload, unsigned int length) {
  String string;
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  for (int i = 0; i < length; i++) {
    string += ((char)payload[i]);
  }

  if (strcmp(topic, "/Distance_of_detection") == 0) {
    // Convert the payload to an integer
    int distanceValue = atoi((char *)payload);

    // Publish the distance value to the "update_gauge" topic
    client.publish("update_gauge", String(distanceValue).c_str());
  }
  if (strcmp(topic, "/mode_control") == 0) {
    lcd.print("               ");
    if (string == "automatic") {
      lcd.setCursor(0, 0);
      lcd.print("Automatic Mode");
      Serial.println(string);
      automaticMode = true;
    }
    if (string == "manual") {
      Serial.println(string);
      automaticMode = false;
    }
  }
  if (strcmp(topic, "/ThinkIOT/Servo-nodered") == 0) {
    Serial.print(" ");
    if (!automaticMode) {
      int status = string.toInt();
      Serial.println(status);
      lastServoPosition = status; // Update the last servo position
      s.write(lastServoPosition);
    }
  }
  if (strcmp(topic, "/reset_car_count") == 0) {
    carCount = 0;
    Serial.println("Car count reset");
  }

  if (strcmp(topic, "/system_state") == 0) {
    if (string == "1") {
      systemActive = true;
      Serial.println("System activated");
    } else if (string == "0") {
      systemActive = false;
      Serial.println("System deactivated");
    }
  }
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    String clientId = "ESP32Client-";
    clientId += String(random(0xffff), HEX);
    if (client.connect(clientId.c_str())) {
      Serial.println("Connected");
      client.publish("/CarDetection/Publish", "Welcome");
      client.subscribe("/CarDetection/Subscribe");
      client.subscribe("/ThinkIOT/Servo-nodered");
      client.subscribe("/mode_control");
      client.subscribe("/cars_count");
      client.subscribe("/reset_car_count");
      client.subscribe("/car_detection/peak_hours");
      client.subscribe("/system_state");
      client.subscribe("/Distance_of_detection");

      // Subscribe to the modified topic for car data
      client.subscribe(car_data_topic);
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  s.attach(25); // Assuming servo is connected to pin 25

  setup_wifi();
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
  client.subscribe(system_state_topic);
  lcd.init(); // Initialize the LCD with 16 columns and 2 rows
  lcd.backlight();    
  timeClient.begin();
  while (!timeClient.update()) {
    timeClient.forceUpdate();
    delay(100);
  }
  
}

void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

  long duration, distance;
  timeClient.update();

  // Triggering the ultrasonic sensor only in automatic mode
  if (automaticMode) {
    digitalWrite(TRIGGER_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIGGER_PIN, HIGH);
    delayMicroseconds(100);
    digitalWrite(TRIGGER_PIN, LOW);

    // Measuring the duration of the echo pulse
    duration = pulseIn(ECHO_PIN, HIGH);

    // Calculating the distance
    distance = duration * 0.034 / 2;

    // Checking if a car is detected within a certain range
    if (distance < 40 && distance > 0) {
      // Only increment car count if car is newly detected
      if (!carDetected) {
        carDetected = true;
        carCount++; // Increment car count when a car is detected
        Serial.print("Total Cars: ");
        Serial.println(carCount);
        String carCountStr = String(carCount);
        client.publish("/cars_count", carCountStr.c_str());
        // Store detection event in history
        historyTimestamps[historyIndex] = millis();
        historyCarCount[historyIndex] = carCount;
        historyIndex = (historyIndex + 1) % MAX_HISTORY_SIZE;
      }

      // Only print the message once per detection
      if (!messagePrinted) {
        messagePrinted = true;
        lcd.setCursor(0, 1);
        lcd.print("A car detected  ");
        s.write(0);
        String distanceStr = String(distance);
        char formattedTimestamp[32];
        time_t rawTime = timeClient.getEpochTime();
        struct tm *timeInfo;
        timeInfo = localtime(&rawTime);
        strftime(formattedTimestamp, sizeof(formattedTimestamp), "%Y-%m-%d %H:%M:%S", timeInfo);
        client.publish("/Distance_of_detection", distanceStr.c_str()); // Convert String to const char
        StaticJsonDocument<256> doc;
        doc["id"] = "s001";
        doc["distance"] = distance;
        doc["car_detection_status"] = "detected";
        doc["timestamp"] = formattedTimestamp;
        // Serialize the JSON document to a char array
        char payload[256];
        serializeJson(doc, payload);

        // Publish the payload to the car data topic
        client.publish(car_data_topic, payload);
      }
    } else {
      carDetected = false;
      messagePrinted = false; // Reset the message printed flag
      lcd.setCursor(0, 1);
      lcd.print("No cars detected");
      s.write(90);
    }    
  } else {
    // Update LCD to show Manual Mode
    lcd.setCursor(0, 0);
    lcd.print("Manual Mode     ");
  }

  delay(100); // Adjust the delay as needed
}

int analyzePeakHour(unsigned long *timestamps, int *carCounts, int historySize) {
  int maxCarCount = 0;
  int peakHour = -1;

  // Loop through the historical data
  for (int i = 0; i < historySize; i++) {
    // Calculate the hour of the day for the current timestamp
    int hour = hourOfDay(timestamps[i]);

    // Sum up the car counts for each hour of the day
    int totalCarCount = 0;
    for (int j = 0; j < historySize; j++) {
      if (hourOfDay(timestamps[j]) == hour) {
        totalCarCount += carCounts[j];
      }
    }

    // Update the peak hour if the total car count is higher
    if (totalCarCount > maxCarCount) {
      maxCarCount = totalCarCount;
      peakHour = hour;
    }
  }

  return peakHour;
}

int hourOfDay(unsigned long timestamp) {
  // Convert milliseconds since epoch to hours
  unsigned long hours = timestamp / (1000 * 60 * 60);
  // Return the hour component
  return hours % 24;
}