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


LiquidCrystal lcd(27, 26, 25, 14, 13, 12);

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

Servo sv1, sv2;

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

int turn_on_servo = 0;
unsigned long previousMillis = 0;

bool try_connect = 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;
  }
}

void callback(char* topic, byte* message, unsigned int length) {
  String stMessage;
  for (int i = 0; i < length; i++) {
    stMessage += (char) message[i];
  }
  DynamicJsonDocument doc(1024);
  deserializeJson(doc, stMessage);

  // Extract values from JSON
  const char* action = doc["action"];
  int switch_id = doc["switch_id"];
  Serial.print(stMessage);
}

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

// void sendRequest() {
//   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 url = "/trigger/" + String("hehehe") + "/with/key/" + String("bOSbq6047_GEhz1BsFqVXf");
//   client.print(String("GET ") + url + " HTTP/1.1\r\n" + "Host: " + host + "\r\n" + "Connection: close\r\n\r\n");
//   delay(500);
// }


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

  lcd.begin(16, 2);

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

  pinMode(commonTrigPin, 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);
}

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

   unsigned long currentMillis = millis();  // Lấy thời gian hiện tại

  // Kiểm tra nếu đã đủ thời gian để thay đổi góc
  if (currentMillis - previousMillis >= 3000){
    previousMillis = currentMillis;
    turn_on_servo = (turn_on_servo + 1) % 2;
  }
  // Đưa servo 1 về góc 90 độ
  if (turn_on_servo == 0){
    sv1.write(90);
    sv2.write(0);
  } else {
    sv1.write(0);
    sv2.write(90);
  }
  delay(1000);  // Đợi 1 giây để servo di chuyển

}