#include <Servo.h>
#include <Wire.h> // for I2C communication with LCD
#include <LiquidCrystal_I2C.h> // for LCD 16x2
#include "pitches.h"

const int pinLed1 = 4;
const int pinLed2 = 5;
const int pinBuzzer = 6;
const int pinServo = 7;
const int trigPin = 8; // HC-SR04 trig pin
const int echoPin = 9; // HC-SR04 echo pin

Servo palang;
LiquidCrystal_I2C lcd(0x27, 16, 2); // set the LCD address to 0x27

const int melodi[] = {NOTE_C4, NOTE_E4, NOTE_C4, NOTE_E4};
const int durasiNada = 500;

bool simulasiBerjalan = false;
unsigned long milisSebelumnya = 0;
const long interval = durasiNada;
int indeksNada = 0;

void setup() {
  // put your setup code here, to run once:
  pinMode(pinLed1, OUTPUT);
  pinMode(pinLed2, OUTPUT);
  pinMode(pinBuzzer, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  palang.attach(pinServo);

  lcd.init(); // initialize the LCD
  lcd.backlight(); // turn on the LCD backlight
  lcd.setCursor(0, 0); // set the cursor to the first row and first column
  lcd.print("Distance: "); // print the initial message
  
}

int readDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  int duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2;
  return distance;
}

void loop() {
  int distance = readDistance();
    lcd.setCursor(9, 0); // set the cursor to the second row and first column
    lcd.print("     "); // clear the previous message
    lcd.setCursor(9, 0); // set the cursor to the second row and first column
    lcd.print(distance);
    lcd.print(" cm");

  if (distance < 150) {
    unsigned long milisSaatIni = millis();
    if (milisSaatIni - milisSebelumnya >= interval) {
      milisSebelumnya = milisSaatIni;
    tone(pinBuzzer, melodi[indeksNada]);
      if (indeksNada % 2 == 0) {
        digitalWrite(pinLed1, HIGH);
        digitalWrite(pinLed2, LOW);
      } else {
        digitalWrite(pinLed1, LOW);
        digitalWrite(pinLed2, HIGH);
      }
      indeksNada = (indeksNada + 1) % 4;
    }
    palang.write(0);
    lcd.setCursor(0, 2);
    lcd.print("Gerbang Tertutup ");
  } else {
    noTone(pinBuzzer);
    digitalWrite(pinLed1, LOW);
    digitalWrite(pinLed2, LOW);
    lcd.setCursor(0, 2);
    lcd.print("Gerbang Terbuka ");
    palang.write(90);
  }
}