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

// --- 1. Konfigurasi Wi-Fi Simulasi Wokwi ---
const char* WIFI_SSID = "Wokwi-GUEST";
const char* WIFI_PASSWORD = "";

// --- 2. Konfigurasi Broker MQTT HiveMQ ---
const char* MQTT_BROKER = "broker.hivemq.com";
const int MQTT_PORT = 1883;
const char* MQTT_TOPIC = "sic8/jerkingdom/parking/data";

// --- 3. Konfigurasi Pin GPIO ESP32 ---
const int TRIG_PIN = 5;
const int ECHO_PIN = 18;
const int LED_GREEN = 25;
const int LED_YELLOW = 33;
const int LED_RED = 32;
const int BUZZER_PIN = 26;
const int SERVO_PIN = 27;

// --- 4. Batas Jarak Parkir (Threshold) & Timer ---
const float EMPTY_THRESHOLD = 100.0;     // > 100 cm: Slot Kosong
const float VIOLATION_THRESHOLD = 20.0;  // < 20 cm : Parkir Lewat Batas
unsigned long lastPublishTime = 0;
const unsigned long PUBLISH_INTERVAL = 1000; // Interval 1 detik (1000 ms)

// --- 5. Pembuatan Objek Perangkat & Komunikasi ---
WiFiClient espClient;
PubSubClient mqtt(espClient);
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo gateServo;

// --- Fungsi Menghubungkan ESP32 ke Wi-Fi ---
void connectWiFi() {
  Serial.println("Menghubungkan ke WiFi...");
  WiFi.mode(WIFI_STA);
  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);

  int attempts = 0;
  while (WiFi.status() != WL_CONNECTED && attempts < 30) {
    delay(500);
    Serial.print(".");
    attempts++;
  }

  if (WiFi.status() == WL_CONNECTED) {
    Serial.println("\nWiFi Terhubung!");
    Serial.print("IP Address: ");
    Serial.println(WiFi.localIP());
  } else {
    Serial.println("\nKoneksi WiFi Gagal.");
  }
}

// --- Fungsi Menghubungkan ESP32 ke Broker MQTT HiveMQ ---
bool connectMQTT() {
  if (mqtt.connected()) {
    return true;
  }

  Serial.println("Menghubungkan ke MQTT Broker HiveMQ...");
  String clientId = "ESP32-JERKINGDOM-";
  clientId += String((uint32_t)ESP.getEfuseMac(), HEX);

  if (mqtt.connect(clientId.c_str())) {
    Serial.println("MQTT Terhubung!");
    Serial.print("Topic: ");
    Serial.println(MQTT_TOPIC);
    return true;
  }

  Serial.print("Koneksi MQTT Gagal. Kode State: ");
  Serial.println(mqtt.state());
  return false;
}

// --- Fungsi Membaca Jarak dari Sensor HC-SR04 ---
float readDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  unsigned long duration = pulseIn(ECHO_PIN, HIGH, 30000);
  if (duration == 0) {
    return -1.0;
  }

  float distance = duration * 0.0343 / 2.0;
  return distance;
}

// --- Fungsi Mengatur LED, Buzzer, Servo, dan LCD Berdasarkan Jarak ---
void updateParkingSystem(float distance) {
  lcd.setCursor(0, 0);
  if (distance < 0) {
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_RED, LOW);
    digitalWrite(BUZZER_PIN, LOW);
    lcd.print("Jarak: Error    ");
    lcd.setCursor(0, 1);
    lcd.print("SENSOR ERROR    ");
    Serial.println("STATUS: SENSOR ERROR");
    return;
  }

  lcd.print("Jarak: " + String(distance, 1) + " cm   ");
  lcd.setCursor(0, 1);

  if (distance > EMPTY_THRESHOLD) {
    // Kondisi 1: Slot Parkir Kosong (> 100 cm)
    digitalWrite(LED_GREEN, HIGH);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_RED, LOW);
    digitalWrite(BUZZER_PIN, LOW);
    gateServo.write(90); // Palang gerbang terbuka (90 derajat)
    lcd.print("SLOT: KOSONG    ");
    Serial.println("STATUS: AVAILABLE (Slot Kosong)");
  } else if (distance >= VIOLATION_THRESHOLD && distance <= EMPTY_THRESHOLD) {
    // Kondisi 2: Parkir Terisi Rapi (20 - 100 cm)
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_YELLOW, HIGH);
    digitalWrite(LED_RED, LOW);
    digitalWrite(BUZZER_PIN, LOW);
    gateServo.write(0);  // Palang gerbang tertutup (0 derajat)
    lcd.print("PARKIR: RAPI    ");
    Serial.println("STATUS: OCCUPIED (Parkir Rapi)");
  } else {
    // Kondisi 3: Parkir Lewat Batas (< 20 cm)
    digitalWrite(LED_GREEN, LOW);
    digitalWrite(LED_YELLOW, LOW);
    digitalWrite(LED_RED, HIGH);
    digitalWrite(BUZZER_PIN, HIGH); // Alarm teguran aktif
    gateServo.write(0);  // Palang gerbang tertutup (0 derajat)
    lcd.print("RAPIKAN PARKIR! ");
    Serial.println("STATUS: VIOLATION (Lewat Batas!)");
  }
}

// --- Fungsi Mengirim Data JSON ke MQTT HiveMQ ---
void publishMQTT(float distance) {
  if (distance < 0) {
    Serial.println("Jarak tidak valid. Pengiriman MQTT dilewati.");
    return;
  }

  if (!mqtt.connected()) {
    if (!connectMQTT()) {
      return;
    }
  }

  const char* status = "OCCUPIED";
  int gateAngle = 0;
  const char* buzzerState = "OFF";

  if (distance > EMPTY_THRESHOLD) {
    status = "AVAILABLE";
    gateAngle = 90;
    buzzerState = "OFF";
  } else if (distance < VIOLATION_THRESHOLD) {
    status = "VIOLATION";
    gateAngle = 0;
    buzzerState = "ON";
  }

  char payload[140];
  snprintf(
      payload,
      sizeof(payload),
      "{\"distance\":%.1f,\"status\":\"%s\",\"gate\":%d,\"buzzer\":\"%s\"}",
      distance,
      status,
      gateAngle,
      buzzerState
  );

  bool published = mqtt.publish(MQTT_TOPIC, payload);
  Serial.print("MQTT Payload: ");
  Serial.println(payload);

  if (published) {
    Serial.println("Publish MQTT Berhasil.\n");
  } else {
    Serial.println("Publish MQTT Gagal.\n");
  }
}

void setup() {
  Serial.begin(115200);
  delay(1000);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_GREEN, OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_RED, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  digitalWrite(TRIG_PIN, LOW);
  digitalWrite(LED_GREEN, LOW);
  digitalWrite(LED_YELLOW, LOW);
  digitalWrite(LED_RED, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  gateServo.attach(SERVO_PIN, 500, 2400);
  gateServo.write(90);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("JER KINGDOM IoT");
  lcd.setCursor(0, 1);
  lcd.print("Menyiapkan WiFi");

  connectWiFi();

  mqtt.setServer(MQTT_BROKER, MQTT_PORT);
  mqtt.setKeepAlive(60);
  mqtt.setSocketTimeout(5);

  if (WiFi.status() == WL_CONNECTED) {
    connectMQTT();
  }
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    Serial.println("WiFi terputus. Menyambung ulang...");
    connectWiFi();
    delay(500);
    return;
  }

  if (!mqtt.connected()) {
    connectMQTT();
  }

  mqtt.loop();

  unsigned long now = millis();
  if (now - lastPublishTime >= PUBLISH_INTERVAL) {
    lastPublishTime = now;
    float distance = readDistance();
    updateParkingSystem(distance);
    publishMQTT(distance);
  }
}