#include <ESP32Servo.h>

#define RLED 25
#define GLED 33
#define RLED2 21
#define GLED2 19
#define BUZZER 2

Servo myservo1;
Servo myservo2;
int trigPin = 26;
int echoPin = 27;
int trigPin2 = 5;
int echoPin2 = 18;
long duration, duration2;
int distance, distance2;
unsigned long previousMillis = 0;
const long interval = 2000; // Interval for the buzzer (2 seconds)

void setup() {
  Serial.begin(9600);
  myservo1.attach(14);
  myservo2.attach(22);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);
  pinMode(echoPin2, INPUT);
  pinMode(RLED, OUTPUT);
  pinMode(GLED, OUTPUT);
  pinMode(RLED2, OUTPUT);
  pinMode(GLED2, OUTPUT);
  pinMode(BUZZER, OUTPUT);
}

void loop() {
  // Check the first ultrasonic sensor
  distance = checkDistance(trigPin, echoPin);
  Serial.print("Jarak gerbang masuk: ");
  Serial.print(distance);
  Serial.println(" cm");

  if (distance >= 0 && distance <= 200) {
    myservo1.write(90);
    BUDI(GLED, RLED);
    soundBuzzer();
  } else {
    myservo1.write(0);
    BUDI(RLED, GLED);
  }

  // Check the second ultrasonic sensor
  distance2 = checkDistance(trigPin2, echoPin2);
  Serial.print("Jarak gerbang keluar: ");
  Serial.print(distance2);
  Serial.println(" cm");

  if (distance2 >= 0 && distance2 <= 200) {
    myservo2.write(90);
    YANTO(GLED2, RLED2);
    soundBuzzer();
  } else {
    myservo2.write(0);
    YANTO(RLED2, GLED2);
  }
}

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

  return pulseIn(echoPin, HIGH) * 0.034 / 2;
}

void BUDI(int whatled1, int whatled2) {
  digitalWrite(whatled1, HIGH);
  digitalWrite(whatled2, LOW);
}

void YANTO(int whatled1, int whatled2) {
  digitalWrite(whatled1, HIGH);
  digitalWrite(whatled2, LOW);
}

void soundBuzzer() {
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    digitalWrite(BUZZER, HIGH);
    delay(2000);
    digitalWrite(BUZZER, LOW);
    previousMillis = currentMillis;
  }
}