#include <WiFi.h>
#include "PubSubClient.h"
#include <LiquidCrystal.h>
#include <ESP32Servo.h>
#include <string.h>
#include "ThingSpeak.h"
#include <ArduinoJson.h> // Thêm dòng này
#include <cstring>

//-UltraSonic
const int trigPin_1 = 4;
const int trigPin_2 = 16;
const int trigPin_3 = 23;
const int trigPin_4 = 2;

const int echoPin_1 = 34;
const int echoPin_2 = 35;
const int echoPin_3 = 22;
const int echoPin_4 = 21;

//--Temp
bool parking_lot[2] = {false, false};
int status[3];
unsigned long previousMillis_1 = 0; 
unsigned long previousMillis_2 = 0; 
unsigned long previousMillis_3 = 0; 
unsigned long previousMillis_4 = 0; 
const int interval = 3000;  

const char* host = "maker.ifttt.com";
const uint16_t port1 = 80;
//--Servo
Servo sv1, sv2;

//--Led
LiquidCrystal lcd(27, 26, 25, 14, 13, 12);
const int redLED_1 = 33;
const int greenLED_1 = 32;
const int redLED_2 = 5;
const int greenLED_2 = 17;

//Wifi and MQTT
const char* ssid = "Wokwi-GUEST";
const char* password = "";
const char* mqttServer = "test.mosquitto.org";
const int port = 1883;
WiFiClient espClient;
PubSubClient client(espClient);
bool try_connect = 0;

//global running
int globalSwitchID = -1;
String globalStatus = "";
// int lastStatus[3];

//MQTT topic
const char* mqtt_receive_topic1 = "servo";
const char* mqtt_receive_topic2 = "form";

//ThingSpeak
unsigned long myChannelNumber = 2392345;
const char* myWriteAPIKey = "G5JDZCMV0WJXJOFW";
const char* myReadAPIKey = "V5MHQIXN2W5TYT9D";

// Car Information
String carAct[] = {"IN", "OUT"};  //Two car actions are go in and go out
String carParkingTime[] = {"", ""};  //The time car go in
const int parkingFeePerSec = 10; // Parking fee per second

// Form - Parking Infomation
bool publishTrigger = false;
char buffer[30] = {'\0'};

// Chart - Statistical Information
const int timeInDay = 180000;  // Time in 1 day (3 minutes)
unsigned long previousTime = 0;  // Previous time
int carCounterPerDay = 0;

void wifiConnect() {
  if (try_connect == 0) {
    Serial.println(" Attempting WiFi_connection");
    WiFi.begin(ssid, password);
    try_connect = 1;
  }
  if (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
  }
  else {
    Serial.println(" Connected!");
    try_connect = 0;
  }
}

unsigned long parseAndConvertToTimestamp(String createdAtString) {
  int year, month, day, hour, minute, second;

  // Parse the ISO 8601 timestamp
  sscanf(createdAtString.c_str(), "%d-%d-%dT%d:%d:%dZ", &year, &month, &day, &hour, &minute, &second);

  // Convert to Unix timestamp
  struct tm timeinfo;
  timeinfo.tm_year = year - 1900;
  timeinfo.tm_mon = month - 1;
  timeinfo.tm_mday = day;
  timeinfo.tm_hour = hour;
  timeinfo.tm_min = minute;
  timeinfo.tm_sec = second;

  return mktime(&timeinfo);
}

void getFormRequest(int position, const char* carID, bool park, bool leave) {
  // Store data to Cloud
  ThingSpeak.setField(1, position);
  ThingSpeak.setField(2, carID);
  ThingSpeak.setField(3, (park) ? carAct[0] : carAct[1]);
  int returncode = ThingSpeak.writeFields(myChannelNumber, myWriteAPIKey);

  // ThingSpeak - Check return code
  if (returncode == 200) {
    Serial.println("ThingSpeak: Channel update successfully.");
  } else {
    Serial.println("ThingSpeak: Problem updating channel.");
  }

  // Read the time associated with the latest update
  String time_str = ThingSpeak.readCreatedAt(myChannelNumber, myReadAPIKey);
  delay(1000);

  // Calculate the parking fee
  //   Case01: Car go in
  if (park) {
    // Store the time car go in
    Serial.println("Car go in!!!");
    carParkingTime[position - 1] = time_str;

    // Reset fee and carID
    publishTrigger = true;
    sprintf(buffer, "{\"fee\":\"\",\"carID\":\"\"}");
  }
  //   Case02: Car go out
  if (leave) {
    Serial.println("Car go out!!!");
    unsigned long timeIn = parseAndConvertToTimestamp(carParkingTime[position - 1]);
    unsigned long timeOut = parseAndConvertToTimestamp(time_str);
    unsigned long duration_sec = timeOut - timeIn;  // Duration as seconds
    
    // Calculate total fee
    int totalFee = duration_sec * parkingFeePerSec;
    Serial.print("-- Parking Fee: ");
    Serial.println(totalFee);

    // Reset time
    carParkingTime[position - 1] = "";

    // Prepare to send to website
    publishTrigger = true;
    sprintf(buffer, "{\"fee\":%d,\"carID\":\"%s\"}", totalFee, carID);
  }

  // Count number of cars in 1 day
  if (leave)
    carCounterPerDay++;
}

void callback(char* topic, byte* message, unsigned int length) {
  Serial.print("Message from topic: ");
  Serial.println(topic);

  // Read Message
  String stMessage;
  for (int i = 0; i < length; i++) {
    stMessage += (char) message[i];
  }
  
  // Extract values from JSON
  DynamicJsonDocument doc(1024);
  deserializeJson(doc, stMessage);

  // Message from topic 1
  if (strcmp(topic, mqtt_receive_topic1) == 0) {
    delay(1000);
    globalSwitchID = doc["switch_id"];
    globalStatus = doc["action"].as<String>();
  }

  // Message from topic 2
  if (strcmp(topic, mqtt_receive_topic2) == 0) {
    delay(1000);
    getFormRequest(doc["position"], doc["carID"], doc["park"], doc["leave"]);
  }
}

void mqttReconnect() {
  while (!client.connected()) {
    Serial.print("Attempting MQTT_connection...");
    if (client.connect("12345678")) {
      Serial.println(" connected");
      client.subscribe(mqtt_receive_topic1);
      client.subscribe(mqtt_receive_topic2);
    } else {
      Serial.println(" try again in 5 seconds");
      delay(5000);
    }
  }
}

void sendRequest(String vitri) {
  Serial.print("connecting to ");
  Serial.print(host); Serial.print(":");
  Serial.println(port1);
  WiFiClient client;
  while (!client.connect(host, port1)) {
    Serial.println("connection fail");
    delay(1000);
  }
  String data = "?value1=" + vitri;

  String url = "/trigger/" + String("thong_bao_vat_can") + "/with/key/" + String("dCRD_aUd9nW0TkNyDOq8vM2ogjIsFxx7m5UAfiqUa85") + data;
  client.print(String("GET ") + url + " HTTP/1.1\r\n" + "Host: " + host + "\r\n" + "Connection: close\r\n\r\n");
  delay(500);
}

void updateInformation(int status[3]){
  // 1 day -> filled
  // 2 trong -> empty
  // 3 loi -> obstacle
  String jsonData = "{";//\"1\":\"filled\",\"2\":\"obstacle\",\"3\":\"empty\"}";

  if (status[0] == 1)
    jsonData = jsonData + "\"1\":\"filled\",";
  else if (status[0] == 2)
    jsonData = jsonData + "\"1\":\"empty\",";
  else
    jsonData = jsonData + "\"1\":\"obstacle\",";

  if (status[1] == 1)
    jsonData = jsonData + "\"2\":\"filled\",";
  else if (status[1] == 2)
    jsonData = jsonData + "\"2\":\"empty\",";
  else
    jsonData = jsonData + "\"2\":\"obstacle\",";

  jsonData = jsonData + "\"3\":\"empty\"}";

  client.publish("test", jsonData.c_str());
  Serial.println(jsonData);
}

const char* name_status[] = {"filled", "empty", "obstacle"};

void printStatus(int status[3]) {
  lcd.clear();

  // Print the first row
  lcd.setCursor(0, 0);
  delay(100);  
  lcd.print("s1: ");
  delay(100);
  lcd.print(name_status[status[0] - 1]);
  delay(100);

  // Print the second row
  lcd.setCursor(0, 1); 
  delay(100); 
  lcd.print("s2: ");
  delay(100);
  lcd.print(name_status[status[1] - 1]);
  delay(100);
}

void setup() {
  Serial.begin(9600);

  lcd.begin(16, 2);
  delay(100);

  parking_lot[0] = 0;
  parking_lot[1] = 0;

  status[0] = 2;
  status[1] = 2;

  // lastStatus[0] = 0;
  // lastStatus[1] = 0;
  
  updateInformation(status);

  sv1.attach(18);
  sv2.attach(19);

  sv1.write(0);
  sv2.write(180);

  pinMode(trigPin_1, OUTPUT);
  pinMode(trigPin_2, OUTPUT);
  pinMode(trigPin_3, OUTPUT);
  pinMode(trigPin_4, OUTPUT);
  pinMode(echoPin_1, INPUT);
  pinMode(echoPin_2, INPUT);
  pinMode(echoPin_3, INPUT);
  pinMode(echoPin_4, INPUT);

  pinMode(redLED_1, OUTPUT);
  pinMode(greenLED_1, OUTPUT);
  pinMode(redLED_2, OUTPUT);
  pinMode(greenLED_2, OUTPUT);

  Serial.println("Connecting to WiFi");
  wifiConnect();
  // sendRequest();
  client.setServer(mqttServer, port);
  client.setCallback(callback);
  ThingSpeak.begin(espClient);
}

int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;  // Tính khoảng cách, với tốc độ âm thanh là 34 cm/ms
  return distance;
}

void loop() {
  // Check if 3 minutes have passed
  unsigned long currentTime = millis();
  if (currentTime - previousTime >= timeInDay) {
    // Save the current time
    previousTime = currentTime;

    // Sent the number of cars per day to the web
    char counterData[10];
    sprintf(counterData, "%d", carCounterPerDay);
    if (!client.publish("car_per_day", counterData)) {
      Serial.println("Failed to publish message to \"car_per_day\"");
      Serial.println(counterData);
    } else {
      Serial.println("Success to publish message to \"car_per_day\"");
      Serial.println(counterData);
    }

    // Reset counter
    carCounterPerDay = 0;
  }
  
  status[0] = 2;
  status[1] = 2;
  delay(100);
 
  if (!client.connected()) { 
    mqttReconnect();
  }
  client.loop();

  if (globalSwitchID != -1) {
    Serial.print("Bat/Tat servo o: ");
    Serial.println(globalSwitchID);
    parking_lot[globalSwitchID - 1] = (globalStatus == "on");
    //parking_lot[globalSwitchID - 1] = !parking_lot[globalSwitchID - 1];
    globalSwitchID = -1;
  }

  // Send total parking fee to website
  // [NOTE: The message needs to be sent twice]
  if (publishTrigger) {    
    if (!client.publish("parking_fee", buffer)) {
      Serial.println("Failed to publish message to \"parking_fee\"");
      Serial.println(buffer);
    } else {
      Serial.println("Success to publish message to \"parking_fee\"");
      Serial.println(buffer);

      publishTrigger = false;
      memset(buffer, '\0', sizeof(buffer));
    }
  }

  // O so 1
  if (parking_lot[0]){
    sv1.write(90);
    Serial.print("Mo cua o so: ");
    Serial.println(1);
  } else {
    sv1.write(0);
    Serial.print("Dong cua o so: ");
    Serial.println(1);
  }

  // O so 2
  if (parking_lot[1]){
    sv2.write(90);
    Serial.print("Mo cua o so: ");
    Serial.println(2);
  } else {
    sv2.write(180);
    Serial.print("Dong cua o so: ");
    Serial.println(2);
  }

  //--------------Ultra Sonic-------------------//
    // Đọc khoảng cách từng cảm biến
  int distance_1 = getDistance(trigPin_1, echoPin_1);
  delay(100);
  int distance_2 = getDistance(trigPin_2, echoPin_2);
  delay(100);
  int distance_3 = getDistance(trigPin_3, echoPin_3);
  delay(100);
  int distance_4 = getDistance(trigPin_4, echoPin_4);
  delay(100);

  // In giá trị khoảng cách ra Serial Monitor
  Serial.print("O so 1, phia truoc: ");
  Serial.println(distance_2);
  Serial.print("O so 1, phia sau: ");
  Serial.println(distance_1);
  Serial.print("O so 2, phia truoc: ");
  Serial.println(distance_3);
  Serial.print("O so 2, phia sau: ");
  Serial.println(distance_4);

  unsigned long currentMillis = millis();
  if (parking_lot[0]){
    //Truong hop servo 1 dang mo
    //TH: Co xe vao
    if (distance_1 < 100 && distance_2 < 100) {
        if (currentMillis - previousMillis_1 >= interval && currentMillis - previousMillis_2 >= interval) {
          //sendRequest(1, 2);
          previousMillis_1 = currentMillis;
          previousMillis_2 = currentMillis;
          
          Serial.print("Có xe ở ô số: ");
          Serial.println(1);
          digitalWrite(redLED_1, LOW);
          digitalWrite(greenLED_1, LOW);

          status[0] = 1;
        }
    } else
    //TH: Khong co xe vao
    if (distance_1 >= 100 && distance_2 >= 100) {
      if (currentMillis - previousMillis_1 >= interval && currentMillis - previousMillis_2 >= interval) {
        //sendRequest(1, 2);
        previousMillis_1 = currentMillis;
        previousMillis_2 = currentMillis;
        
        Serial.print("Đang trống ô số: ");
        Serial.println(1);
        digitalWrite(redLED_1, LOW);
        digitalWrite(greenLED_1, HIGH);

        status[0] = 2;
      }
    } else
    //TH: Co vat can
    if (distance_1 < 100) {
      if (currentMillis - previousMillis_1 >= interval >= interval) {
        //sendRequest(1, 2);
        previousMillis_1 = currentMillis;
        
        Serial.print("Phát hiện vật cản ở ô số:");
        Serial.println(1);
        digitalWrite(redLED_1, HIGH);
        digitalWrite(greenLED_1, LOW);
        status[0] = 3;
        sendRequest("1");

      }
    }
  } else {
    //Truong hop servo 1 dang dong
    //TH: Co vat can
    if (distance_1 < 100 || distance_2 < 100) {
      if (currentMillis - previousMillis_1 >= interval || currentMillis - previousMillis_2 >= interval) {
        //sendRequest(1, 2);
        previousMillis_1 = currentMillis;
        previousMillis_2 = currentMillis;
        
        Serial.print("Phát hiện vật cản ở ô số: ");
        Serial.println(1);
        digitalWrite(redLED_1, HIGH);
        digitalWrite(greenLED_1, LOW);
        status[0] = 3;
        sendRequest("1");

      }
    }
    //TH: Khong co vat can
    if (distance_1 >= 100 && distance_2 >= 100) {
      if (currentMillis - previousMillis_1 >= interval || currentMillis - previousMillis_2 >= interval) {
        //sendRequest(1, 2);
        previousMillis_1 = currentMillis;
        previousMillis_2 = currentMillis;
        
        Serial.print("Đang trống ô số: ");
        Serial.println(1);
        digitalWrite(redLED_1, LOW);
        digitalWrite(greenLED_1, LOW);

        status[0] = 2;
      }
    }
  }
  delay(100);
  if (parking_lot[1]){
    //Truong hop servo 2 dang mo
    //TH: Co xe vao
    if (distance_3 < 100 && distance_4 < 100) {
        if (currentMillis - previousMillis_3 >= interval && currentMillis - previousMillis_4 >= interval) {
        //sendRequest(1, 2);
        previousMillis_3 = currentMillis;
        previousMillis_4 = currentMillis;
        
        Serial.print("Có xe ở ô số: ");
        Serial.println(2);
        digitalWrite(redLED_2, LOW);
        digitalWrite(greenLED_2, LOW);

        status[1] = 1;
      }
    } else
    //TH: Khong co xe vao
    if (distance_3 >= 100 && distance_4 >= 100) {
      if (currentMillis - previousMillis_3 >= interval && currentMillis - previousMillis_4 >= interval) {
        //sendRequest(1, 2);
        previousMillis_3 = currentMillis;
        previousMillis_4 = currentMillis;
        
        Serial.print("Đang trống ô số: ");
        Serial.println(2);
        digitalWrite(redLED_2, LOW);
        digitalWrite(greenLED_2, HIGH);

        status[1] = 2;
      }
    } else
    //TH: Co vat can
    if (distance_4 < 100) {
      if (currentMillis - previousMillis_4 >= interval) {
        //sendRequest(1, 2);
        previousMillis_4 = currentMillis;
        
        Serial.print("Phát hiện vật cản ở ô số:");
        Serial.println(2);
        digitalWrite(redLED_2, HIGH);
        digitalWrite(greenLED_2, LOW);

        status[1] = 3;
        sendRequest("2");

      }
    }
  } else {
    //Truong hop servo 2 dang dong
    //TH: Co vat van
    if (distance_3 < 100 || distance_4 < 100) {
      if (currentMillis - previousMillis_3 >= interval || currentMillis - previousMillis_4 >= interval) {
        //sendRequest(1, 2);
        previousMillis_3 = currentMillis;
        previousMillis_4 = currentMillis;
        
        Serial.print("Phát hiện vật cản ở ô số: ");
        Serial.println(2);
        digitalWrite(redLED_2, HIGH);
        digitalWrite(greenLED_2, LOW);

        status[1] = 3;
        sendRequest("2");

      }
    }
    //TH: Khong co vat van
    if (distance_3 >= 100 && distance_4 >= 100) {
      if (currentMillis - previousMillis_3 >= interval || currentMillis - previousMillis_4 >= interval) {
        //sendRequest(1, 2);
        previousMillis_3 = currentMillis;
        previousMillis_4 = currentMillis;
        
        Serial.print("Đang trống ô số: ");
        Serial.println(2);
        digitalWrite(redLED_2, LOW);
        digitalWrite(greenLED_2, LOW);

        status[1] = 2;
      }
    }
  }
  delay(100);
  updateInformation(status);
  delay(100);
  printStatus(status);
  delay(2000);
}


//- nếu 1 trong 2 mà bật, trong khi đó servo đang đóng, thì gửi thông báo về lcd: có vật cản//
//- 2 cảm biến bật cùng lúc, servo đang mở thì nó sẽ báo là có xe
//- 2 cảm biến bật cùng lúc, servo đang đóng thì nó sẽ báo là có vật cản, đèn đỏ báo
//- nếu 1 trong 2 mà bật, trong khi đó servo đang đóng, thì gửi thông báo về lcd: có vật cản
//- Nếu servo mở, báo đèn xanh