//KELOMPOK INFORMATIKA 12D TIMI,RAKHA,ALVIN, dan GHANNIA
#include <Servo.h>

const int pinTrig = 5;
const int pinEcho = 6;

const int pinServoGerbang1 = 9;
const int pinServoGerbang2 = 11;

const int pinBuzzer = 4;
const int pinLEDMerah = 12;
const int pinLEDHijau = 13;

long durasi;
int jarak;

int statusGerbangTertutup = 1;
int statusGerbangTerbuka = 0;
int statusGerbang = statusGerbangTertutup;

int posisiServo = 0;
int langkahServo = 1;
int delayServo = 50;

const int batasJarak = 25;
unsigned long waktuSebelumnya = 0;
unsigned long waktuDebounceTerakhir = 0;
const unsigned long delayDebounce = 200;

int statusNada = 0;
Servo servoGerbang1, servoGerbang2;

void setup() {
  Serial.begin(9600);
  pinMode(pinTrig, OUTPUT);
  pinMode(pinEcho, INPUT);
  pinMode(pinLEDMerah, OUTPUT);
  pinMode(pinLEDHijau, OUTPUT);
  pinMode(pinBuzzer, OUTPUT);
  servoGerbang1.attach(pinServoGerbang1);
  servoGerbang2.attach(pinServoGerbang2);
  tutupGerbangPerlahan();
  delay(5000);
}

void loop() {
  digitalWrite(pinTrig, LOW);
  delayMicroseconds(2);
  digitalWrite(pinTrig, HIGH);
  delayMicroseconds(10);
  digitalWrite(pinTrig, LOW);

  durasi = pulseIn(pinEcho, HIGH);
  jarak = durasi * 0.034 / 2;

  if (jarak <= batasJarak) {
    digitalWrite(pinLEDMerah, LOW);
    digitalWrite(pinLEDHijau, HIGH);
  }else {
    digitalWrite(pinLEDMerah, HIGH);
    digitalWrite(pinLEDHijau, LOW);
  }

unsigned long waktuSekarang = millis();
if(jarak <= batasJarak && statusGerbang == statusGerbangTerbuka && (waktuSekarang - waktuDebounceTerakhir) > delayDebounce){
  tutupGerbangPerlahan();
  waktuDebounceTerakhir = waktuSekarang; 
}else if (jarak > batasJarak && statusGerbang == statusGerbangTertutup && (waktuSekarang - waktuDebounceTerakhir) > delayDebounce){
  bukaGerbangPerlahan();
  waktuDebounceTerakhir = waktuSekarang;
}
delay(1000);
}

void bukaGerbangPerlahan(){
  digitalWrite(pinLEDMerah, LOW);
  digitalWrite(pinLEDHijau, HIGH);
  for (posisiServo = 0; posisiServo <= 90; posisiServo += langkahServo){
    servoGerbang1.write(posisiServo);
    servoGerbang2.write(posisiServo);
    if(millis()- waktuSebelumnya > 500){
      waktuSebelumnya = millis();
      if(statusNada == 0){
        tone(pinBuzzer, 300);
        statusNada = 1;
      }else{
        tone(pinBuzzer, 150);
        statusNada = 0;
      }
    }
    delay(delayServo);
  }
  statusGerbang = statusGerbangTerbuka;
  noTone(pinBuzzer);
}
  
void tutupGerbangPerlahan (){
  digitalWrite(pinLEDHijau, LOW);
  digitalWrite(pinLEDMerah, HIGH);
  for (posisiServo = 90; posisiServo >= 0;
  posisiServo -= langkahServo){
    servoGerbang1.write(posisiServo);
    servoGerbang2.write(posisiServo);
    if (millis() - waktuSebelumnya > 500){
      waktuSebelumnya = millis();
      if (statusNada == 0){
        tone(pinBuzzer, 300);
        statusNada = 1;
      }else{
        tone(pinBuzzer, 150);
        statusNada = 0;
      }
    }
    delay(delayServo);
  }
   statusGerbang = statusGerbangTertutup;
  noTone(pinBuzzer);
  delay(1000);
}