#include <ESP32Servo.h>  // Use ESP32Servo instead of Servo
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define TRIG_PIN_IN 5
#define ECHO_PIN_IN 18
#define TRIG_PIN_OUT 19
#define ECHO_PIN_OUT 21
#define SERVO_PIN_IN 13
#define SERVO_PIN_OUT 12

#define MAX_PARKIR 15
int kendaraan = 0;

LiquidCrystal_I2C lcd(0x27, 16, 2);

Servo servoIn;
Servo servoOut;

long getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  long distance = (duration / 2) * 0.0344;
  return distance;
}

void setup() {
  pinMode(TRIG_PIN_IN, OUTPUT);
  pinMode(ECHO_PIN_IN, INPUT);
  pinMode(TRIG_PIN_OUT, OUTPUT);
  pinMode(ECHO_PIN_OUT, INPUT);

  servoIn.attach(SERVO_PIN_IN);
  servoOut.attach(SERVO_PIN_OUT);
  servoIn.write(0);
  servoOut.write(0);

  lcd.begin(16, 2);
  lcd.setBacklight(1);
}

void loop() {
  long distanceIn = getDistance(TRIG_PIN_IN, ECHO_PIN_IN);
  long distanceOut = getDistance(TRIG_PIN_OUT, ECHO_PIN_OUT);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.println("KENDARAAN = ");
  lcd.println(kendaraan);
  lcd.setCursor(0, 1);
  lcd.println("SISA PARKIR = ");
  lcd.println(MAX_PARKIR - kendaraan);

  if (distanceIn < 10 && kendaraan < MAX_PARKIR) {
    servoIn.write(90);
    delay(3000);
    servoIn.write(0);
    kendaraan++;
  }

  if (distanceOut < 10 && kendaraan > 0) {
    servoOut.write(90);
    delay(3000);
    servoOut.write(0);
    kendaraan--;
  }

  if (kendaraan >= MAX_PARKIR) {
    servoIn.write(0);
  }

  if (kendaraan <= 0) {
    servoOut.write(0);
  }
  delay(500);
}