#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>
#include <WiFi.h>
#include <MQTT.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

#define t1 4  
#define t2 16
#define t3 17

#define IR_SENSOR_PIN 5
#define SERVO_PIN 18

#define GREEN_LED1 12
#define GREEN_LED2 14
#define GREEN_LED3 27

#define RED_LED1 26
#define RED_LED2 25
#define RED_LED3 33

Servo servoMotor;
int distanceThreshold = 150;
int lastStatus = -1;

const char* ssid = "Wokwi-GUEST";          // Ganti dengan SSID WiFi kamu
const char* password = "";                 // Ganti dengan password WiFi kamu

const char* mqttServer = "mountainrat662.cloud.shiftr.io";
const char* mqttUser = "mountainrat662";
const char* mqttPassword = "b0X48fxDejD3HhdU";
const int mqttPort = 1883;

WiFiClient net;
MQTTClient client;

void setupWiFi() {
  Serial.print("Connecting to WiFi...");
  WiFi.begin(ssid, password);
  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }
  Serial.println("\nWiFi connected.");
}

void setupMQTT() {
  client.begin(mqttServer, net);
  while (!client.connect("ESP32_Client", mqttUser, mqttPassword)) {
    delay(1000);
    Serial.print(".");
  }
  Serial.println("Connected to MQTT Broker");
}

void publishMQTT(const char* topic, const char* payload) {
  if (!client.connected()) {
    setupMQTT();
  }
  client.publish(topic, payload);
}

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();
  
  pinMode(IR_SENSOR_PIN, INPUT_PULLUP);
  
  pinMode(GREEN_LED1, OUTPUT);
  pinMode(GREEN_LED2, OUTPUT);
  pinMode(GREEN_LED3, OUTPUT);
  
  pinMode(RED_LED1, OUTPUT);
  pinMode(RED_LED2, OUTPUT);
  pinMode(RED_LED3, OUTPUT);

  servoMotor.attach(SERVO_PIN);

  setupWiFi();
  setupMQTT();
}

long readDistance(int triggerPin, int echoPin) {
  pinMode(triggerPin, OUTPUT);
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  pinMode(echoPin, INPUT);
  return pulseIn(echoPin, HIGH);
}

void handleLEDs(float distance, int greenLedPin, int redLedPin) {
  if (distance >= distanceThreshold){
    digitalWrite(greenLedPin, HIGH);
    digitalWrite(redLedPin, LOW);
  } else {
    digitalWrite(redLedPin, HIGH);
    digitalWrite(greenLedPin, LOW);
  }
}

void updateLCD(int currentStatus, float d1, float d2, float d3) {
  if (currentStatus != lastStatus) {
    lcd.clear();
    const char* statusMessage;

    switch (currentStatus) {
      case 3:
        lcd.setCursor(0, 0);
        lcd.print("3 Slots Free");
        Serial.println("3 Slots Free");
        lcd.setCursor(0, 1);
        lcd.print("Slot 1 2 3 Free");
        Serial.println("Slot 1 2 3 Free");
        statusMessage = "3 slots available";
        break;

      case 2:
        lcd.setCursor(0, 0);
        lcd.print("2 Slots Free");
        Serial.println("2 Slots Free");
        lcd.setCursor(0, 1);
        
        if (d1 > 100 && d2 > 100) {
          lcd.print("Slot 1 & 2 Free");
          Serial.println("Slot 1 & 2 Free");
        } else if (d1 > 100 && d3 > 100) {
          lcd.print("Slot 1 & 3 Free");
          Serial.println("Slot 1 & 3 Free");
        } else {
          lcd.print("Slot 2 & 3 Free");
          Serial.println("Slot 2 & 3 Free");
        }
        statusMessage = "2 slots available";
        break;

      case 1:
        lcd.setCursor(0, 0);
        lcd.print("1 Slot Free");
        Serial.println("1 Slot Free");
        lcd.setCursor(0, 1);
        
        if (d1 > 100) {
          lcd.print("Slot 1 is Free");
          Serial.println("Slot 1 is Free");
        } else if (d2 > 100) {
          lcd.print("Slot 2 is Free");
          Serial.println("Slot 2 is Free");
        } else {
          lcd.print("Slot 3 is Free");
          Serial.println("Slot 3 is Free");
        }
        statusMessage = "1 slot available";
        break;

      case 0:
        lcd.setCursor(0, 0);
        lcd.print("No Slot Free");
        Serial.println("No Slot Free");
        lcd.setCursor(0, 1);
        lcd.print("Parking Full");
        Serial.println("Parking Full");
        statusMessage = "Parking full";
        break;
    }

    publishMQTT("parking/slots", statusMessage);
    lastStatus = currentStatus;
  }
}

void loop() {
  float d1 = 0.01723 * readDistance(t1, t1);
  float d2 = 0.01723 * readDistance(t2, t2);
  float d3 = 0.01723 * readDistance(t3, t3);

  if (digitalRead(IR_SENSOR_PIN) == LOW) {
    if (d1 > 100 || d2 > 100 || d3 > 100) {
      servoMotor.write(90);
      delay(500);
      servoMotor.write(0);
      delay(500);
    }
  }

  handleLEDs(d1, GREEN_LED1, RED_LED1);
  handleLEDs(d2, GREEN_LED2, RED_LED2);
  handleLEDs(d3, GREEN_LED3, RED_LED3);

  int currentStatus = 0;
  if (d1 > 100) currentStatus++;
  if (d2 > 100) currentStatus++;
  if (d3 > 100) currentStatus++;

  updateLCD(currentStatus, d1, d2, d3);

  client.loop(); // Keep MQTT connection alive
  delay(100);
}