#include <Servo.h>

Servo servo1;
Servo servo2;

int trigPin1 = 2;
int echoPin1 = 3;
int trigPin2 = 4;
int echoPin2 = 5;
int irPin1 = 6;
int irPin2 = 7;
int buzzer = 8;
int ledRed = 11; // Pin untuk LED merah
int ledGreen = 12; // Pin untuk LED hijau

bool pintuTerbuka = false;

// Definisi batas aman jarak pintu terbuka
const int batasJarakAman = 30;  // Ubah sesuai kebutuhan

void setup() {
  servo1.attach(9);
  servo2.attach(10);
  pinMode(trigPin1, OUTPUT);
  pinMode(echoPin1, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(irPin1, INPUT);
  pinMode(irPin2, INPUT);
  pinMode(buzzer, OUTPUT);
  pinMode(ledRed, OUTPUT); // Atur pin LED merah sebagai output
  pinMode(ledGreen, OUTPUT); // Atur pin LED hijau sebagai output
  servo1.write(0);
  servo2.write(0);
  Serial.begin(9600); // Mulai komunikasi serial
}

void loop() {
  long duration1, distance1, duration2, distance2;
  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;

  // Periksa apakah ada objek mendekati pintu
  if (distance1 < batasJarakAman || distance2 < batasJarakAman || digitalRead(irPin1) == LOW || digitalRead(irPin2) == LOW) {
    // Tutup pintu dan bunyikan buzzer hanya jika pintu terbuka
    if (!pintuTerbuka) {
      servo1.write(90);
      servo2.write(90);
      pintuTerbuka = true;
      // Matikan inframerah (opsional)
      digitalWrite(irPin1, HIGH);
      digitalWrite(irPin2, HIGH);
      // Bunyikan buzzer
      digitalWrite(buzzer, HIGH);
      // Nyalakan LED merah dan matikan LED hijau
      digitalWrite(ledRed, HIGH);
      digitalWrite(ledGreen, LOW);
      delay(1000); // Tunda 1 detik (bisa disesuaikan)
      digitalWrite(buzzer, LOW);
    }
  } else {
    // Buka pintu hanya jika pintu sudah terbuka
    if (pintuTerbuka) {
      servo1.write(0);
      servo2.write(0);
      pintuTerbuka = false;
      // Aktifkan inframerah (opsional)
      digitalWrite(irPin1, LOW);
      digitalWrite(irPin2, LOW);
      // Nyalakan LED hijau dan matikan LED merah
      digitalWrite(ledGreen, HIGH);
      digitalWrite(ledRed, LOW);
    }
  }

  // Tampilkan jarak pada Serial Monitor
  Serial.print("Jarak 1: ");
  Serial.print(distance1);
  Serial.print(" cm, Jarak 2: ");
  Serial.print(distance2);
  Serial.println(" cm");

  delay(500);
}