#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h> 

#define OLED_RESET    -1
Adafruit_SSD1306 display(OLED_RESET);

#define WLAN_SSID       "Wokwi-GUEST"
#define WLAN_PASS       ""
const char* mqttServer = "cobaaja.cloud.shiftr.io";
const char* mqttUser = "cobaaja";
const char* mqttPassword = "9TZCgDsHhcGlRVJh";
const int mqttPort = 1883; 
const char* clientID = "ESP32Client"; 

WiFiClient espClient;
PubSubClient client(espClient);

const int trigPin1 = 12; 
const int echoPin1 = 13; 
const int trigPin2 = 14; 
const int echoPin2 = 15; 
const int distanceThreshold = 150; 
int counter = 0;

const int redLed1 = 5;
const int greenLed1 = 4;
const int redLed2 = 16;
const int greenLed2 = 17;

const int buttonPin = 2;
const int buttonPin2 = 23;

Servo doorServo;
Servo doorServo2;

unsigned long previousMillis1 = 0;
unsigned long previousMillis2 = 0;
const long interval = 4500;
const long interval2 = 8000;

bool indoor = false;
int incar = 0;
int outcar = 0;

void setup_wifi() {
  delay(10);
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(WLAN_SSID);

  WiFi.begin(WLAN_SSID, WLAN_PASS);
  
  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
  }
  
  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void reconnect() {
while (!client.connected()) {
    Serial.println("Menghubungkan ke MQTT...");
    if (client.connect(clientID, mqttUser, mqttPassword)) {
      Serial.println("Terhubung ke MQTT");
    } else {
      Serial.print("Gagal menghubungkan ke MQTT, mencoba lagi dalam 5 detik...");
      delay(5000);
    }
  }
}

void setup() {
  Serial.begin(9600);
  
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);

  pinMode(redLed1, OUTPUT);
  pinMode(greenLed1, OUTPUT);
  pinMode(redLed2, OUTPUT);
  pinMode(greenLed2, OUTPUT);

  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);

  doorServo.attach(18);
  doorServo2.attach(19);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(15, 0);
  display.println("E-PARKIR");
  display.display();

  setup_wifi();
  client.setServer(mqttServer, mqttPort);
}

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

  long duration1, duration2, distance1, distance2;
  unsigned long currentMillis = millis();

  digitalWrite(trigPin1, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin1, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin1, LOW);
  duration1 = pulseIn(echoPin1, HIGH);
  distance1 = (duration1 / 2) / 29.1;

  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);
  duration2 = pulseIn(echoPin2, HIGH);
  distance2 = (duration2 / 2) / 29.1;

  if (distance1 < distanceThreshold && distance2 < distanceThreshold) {
    counter = 0;
    digitalWrite(redLed1, HIGH);
    digitalWrite(greenLed1, LOW);
    digitalWrite(redLed2, HIGH);
    digitalWrite(greenLed2, LOW);
  } else if (distance1 < distanceThreshold || distance2 < distanceThreshold) {
    counter = 1;
    if (distance1 < distanceThreshold) {
      digitalWrite(redLed1, HIGH);
      digitalWrite(greenLed1, LOW);
      digitalWrite(redLed2, LOW);
      digitalWrite(greenLed2, HIGH);
    } else {
      digitalWrite(redLed1, LOW);
      digitalWrite(greenLed1, HIGH);
      digitalWrite(redLed2, HIGH);
      digitalWrite(greenLed2, LOW);
    }
  } else {
    counter = 2;
    digitalWrite(redLed1, LOW);
    digitalWrite(greenLed1, HIGH);
    digitalWrite(redLed2, LOW);
    digitalWrite(greenLed2, HIGH);
  }

  if (digitalRead(buttonPin) == LOW) {
    doorServo.write(0);
    previousMillis1 = currentMillis;
    incar++;
    client.publish("parking/masuk", String(incar).c_str()); // Kirim data ke shiftr.io
  } else {
    if (currentMillis - previousMillis1 >= interval) {
      doorServo.write(90);
    }
  }

  if (digitalRead(buttonPin2) == LOW) {
    doorServo2.write(0);
    previousMillis2 = currentMillis;
    outcar++;
    client.publish("parking/keluar", String(outcar).c_str()); // Kirim data ke shiftr.io
  } else {
    if (currentMillis - previousMillis2 >= interval2) {
      doorServo2.write(90);
    }
  }

  client.publish("parking/sisa", String(counter).c_str());

  display.clearDisplay();
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print("JUMLAH MASUK:");
  display.setCursor(90, 0);
  display.println(incar);
  display.setCursor(0, 15);
  display.print("JUMLAH KELUAR:");
  display.setCursor(90, 15);
  display.println(outcar);
  display.setCursor(0, 30);
  display.print("SISA PARKIR:");
  display.setCursor(90, 30);
  display.println(counter);
  display.display();

  delay(100);
}