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

const int pinTombolTurun = 2;
const int pinTombolNaik = 3;
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
const int pinMotionSensor = 10;

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};
int durasiNada = 500; // initial duration
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(pinTombolTurun, INPUT_PULLUP);
  pinMode(pinTombolNaik, INPUT_PULLUP);
  pinMode(pinLed1, OUTPUT);
  pinMode(pinLed2, OUTPUT);
  pinMode(pinBuzzer, OUTPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(pinMotionSensor, INPUT);

  palang.attach(pinServo);
  palang.write(90);

  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
}

void loop() {
  // put your main code here, to run repeatedly:
  static bool statusTombolTurunSebelumnya = HIGH;
  static bool statusTombolNaikSebelumnya = HIGH;
  static bool motionDetected = false;

  bool statusTombolTurun = digitalRead(pinTombolTurun);
  bool statusTombolNaik = digitalRead(pinTombolNaik);
  bool motionState = digitalRead(pinMotionSensor);

  if (statusTombolTurun == LOW && statusTombolTurunSebelumnya == HIGH) {
    simulasiBerjalan = true;
    delay(50);
  }

  statusTombolTurunSebelumnya = statusTombolTurun;

  if (statusTombolNaik == LOW && statusTombolNaikSebelumnya == HIGH) {
    simulasiBerjalan = false;
    delay(50);
  }

  if (motionState == HIGH &&!motionDetected) {
    motionDetected = true;
    simulasiBerjalan = true;
  } else if (motionState == LOW && motionDetected) {
    motionDetected = false;
    simulasiBerjalan = false;
  }

  statusTombolNaikSebelumnya = statusTombolNaik;

  if (simulasiBerjalan) {
    unsigned long milisSaatIni = millis();
    int distance = readDistance();
    if (distance < 150) {
      durasiNada = 200; // faster duration when distance is less than 150cm
    } else {
      durasiNada = 500; // normal duration when distance is more than 150cm
    }

    if (milisSaatIni - milisSebelumnya >= durasiNada) {
      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, 1); // set the cursor to the second row and first column
    lcd.print("                "); // clear the previous message
    lcd.setCursor(0, 1); // set the cursor to the second row and first column
    lcd.print("Distance: ");
    lcd.print(distance);
    lcd.print(" cm");
  } else {
    noTone(pinBuzzer);
    digitalWrite(pinLed1, LOW);
    digitalWrite(pinLed2, LOW);
    palang.write(90);
  }
}

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;
}