#include <Servo.h>

// Definisi pin untuk sensor ultrasonik
#define trigPin1 2
#define echoPin1 3
#define trigPin2 4
#define echoPin2 5
#define trigPin3 6
#define echoPin3 7
#define trigPin4 8
#define echoPin4 9
#define trigPin5 10
#define echoPin5 11

// Definisi pin untuk servo
Servo servo1;
Servo servo2;
#define servoPin1 12
#define servoPin2 13

// Variabel jarak
long duration1, distance1;
long duration2, distance2;
long duration3, distance3;
long duration4, distance4;
long duration5, distance5;

// Fungsi untuk menghitung jarak
long calculateDistance(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 * 0.034 / 2;  // Menghitung jarak (cm)
  
  return distance;
}

void setup() {
  // Inisialisasi serial monitor
  Serial.begin(9600);

  // Inisialisasi pin servo
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  // Inisialisasi pin sensor ultrasonik
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);
  pinMode(echoPin3, INPUT);
  pinMode(trigPin4, OUTPUT);
  pinMode(echoPin4, INPUT);
  pinMode(trigPin5, OUTPUT);
  pinMode(echoPin5, INPUT);
  
  // Servo dimulai dalam kondisi terbuka
  servo1.write(90);  // Buka gate parkir utama
  servo2.write(90);  // Buka gate keluar
}

void loop() {
  // Menghitung jarak sensor
  distance1 = calculateDistance(trigPin1, echoPin1);
  distance2 = calculateDistance(trigPin2, echoPin2);
  distance3 = calculateDistance(trigPin3, echoPin3);
  distance4 = calculateDistance(trigPin4, echoPin4);
  distance5 = calculateDistance(trigPin5, echoPin5);

  // Mengecek apakah semua slot parkir sudah penuh (jarak <= 15 cm)
  if (distance1 <= 15 && distance2 <= 15 && distance3 <= 15 && distance4 <= 15) {
    // Parkir penuh
    Serial.println("Parkiran Penuh");
    servo1.write(0);  // Menutup gate parkir utama
  } else {
    // Parkir belum penuh
    Serial.println("Slot Parkir Tersedia");
    servo1.write(90);  // Membuka gate parkir utama
  }

  // Mengecek apakah ada mobil di sensor 5 (keluar parkir) dengan jarak <= 20 cm
  if (distance5 <= 20) {
    Serial.println("Mobil Terdeteksi di Gerbang Keluar");
    servo2.write(0);  // Membuka gate keluar
    delay(5000);  // Delay untuk memberikan waktu mobil keluar
    servo2.write(90);  // Menutup kembali gate keluar
  }

  // Delay untuk stabilisasi sensor
  delay(1000);
}