#include <MQTT.h>
#include <WiFi.h>
#include <NusabotSimpleTimer.h>
#include <NewPing.h>
#include <ESP32Servo.h>

Servo myservo;
WiFiClient net;
MQTTClient client;
NusabotSimpleTimer timer;

// Gate
#define TRIGGER_PIN 25
#define ECHO_PIN    26

// Parking place 1
#define TRIGGER_PIN_1 23
#define ECHO_PIN_1    22

// Parking place 2
#define TRIGGER_PIN_2 19
#define ECHO_PIN_2    18

// Parking place 3
#define TRIGGER_PIN_3 17
#define ECHO_PIN_3    16

#define MAX_DISTANCE 400

// Gate
NewPing cm(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

// State for each parking place
NewPing cm1(TRIGGER_PIN_1, ECHO_PIN_1, MAX_DISTANCE);
NewPing cm2(TRIGGER_PIN_2, ECHO_PIN_2, MAX_DISTANCE);
NewPing cm3(TRIGGER_PIN_3, ECHO_PIN_3, MAX_DISTANCE);

// Servo
const int servoPin = 22;

const char ssid[] = "Wokwi-GUEST";
const char pass[] = "";

int lastDistanceGate = 0;    // Variable for gate distance
int lastDistance1 = 0;       // Variable for parking place 1
int lastDistance2 = 0;       // Variable for parking place 2
int lastDistance3 = 0;       // Variable for parking place 3

char messageBuffer[60];
bool parkingFull[3] = {false, false, false}; // Track status of each parking place

void setup() {
  Serial.begin(9600);
  WiFi.begin(ssid, pass);

  // Menunggu hingga WiFi terhubung sebelum memulai
  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
  }
  myservo.attach(servoPin);

  client.begin("system-smart.cloud.shiftr.io", net);
  connect();
}

void loop() {
  if (WiFi.status() != WL_CONNECTED) {
    connect();
  }

  if (!client.connected()) {
    connect();
  }

  // Timer untuk memeriksa gate
  timer.setInterval(1000, []() {
    int distanceGate = cm.ping_cm();
    // Gate
    if (distanceGate != lastDistanceGate) {
      lastDistanceGate = distanceGate;
      if (distanceGate < 75) {
        myservo.write(0);
        publish("Mobil terdeteksi! Gate dibuka!");
      } else {
        myservo.write(90);
        publish("Mobil tidak terdeteksi! Gate ditutup!");
      }
    }
  });

  // Timer untuk memeriksa tempat parkir
  timer.setInterval(1000, []() {
    int distance1 = cm1.ping_cm();
    int distance2 = cm2.ping_cm();
    int distance3 = cm3.ping_cm();

    // Parkir 1
    if (distance1 != lastDistance1) {
      lastDistance1 = distance1;
      publish(distance1 < 75 ? "Parkir 1 terisi." : "Parkir 1 kosong.");
      parkingFull[0] = distance1 < 75;
    }

    // Parkir 2
    if (distance2 != lastDistance2) {
      lastDistance2 = distance2;
      publish(distance2 < 75 ? "Parkir 2 terisi." : "Parkir 2 kosong.");
      parkingFull[1] = distance2 < 75;
    }

    // Parkir 3
    if (distance3 != lastDistance3) {
      lastDistance3 = distance3;
      publish(distance3 < 75 ? "Parkir 3 terisi." : "Parkir 3 kosong.");
      parkingFull[2] = distance3 < 75;
    }

    // Cek status parkir dan kirim pesan jika semua parkir terisi
    if (parkingFull[0] && parkingFull[1] && parkingFull[2]) {
      publish("Semua tempat parkir terisi.");
    }
  });

  client.loop();
  timer.run();
  delay(10);
}


void connect() {
  Serial.print("Menghubungkan ke WiFi ");
  while (WiFi.status() != WL_CONNECTED) {
    Serial.print(".");
    delay(100);
  }
  Serial.println(" Terhubung ke WiFi");

  client.setWill("system/parking/fadhil", "Belum terhubung", true, 1);
  while (!client.connect("unix_id", "system-smart", "fadhil")) {
    Serial.print(".");
    delay(100);
  }
  Serial.println("Terhubung ke broker MQTT");
}

void publish(const char* text) {
  client.publish("system/parking/fadhil", text, true, 1);
}