#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#include <Servo.h>
#include <EEPROM.h>

const int trigPin = 15;
const int echoPin = 16;
const int servoPin = 10;
const int ledGreen = 14;
const int ledYellow = 12;
const int ledRed = 13;

LiquidCrystal_I2C lcd(0x27, 16, 2);


Servo servo;
int safeDistance;
int servoPos = 90;

// Fungsi tersendiri untuk membaca jarak dengan sensor ultrasonic
long readDistance() {
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
return distance;
}

void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(ledGreen, OUTPUT);
pinMode(ledYellow, OUTPUT);
pinMode(ledRed, OUTPUT);
servo.attach(servoPin);
lcd.init();
lcd.backlight();
lcd.setCursor(0, 0);
lcd.print("Masukan jarak aman");
lcd.setCursor(0, 1);
lcd.print("10-400 cm");
lcd.print(" Tekan '*' untuk enter");
int safeDistance;

// Membaca nilai jarak aman dari memori EEPROM
EEPROM.get(0, safeDistance);

// Jika nilai jarak aman tidak valid, maka menggunakan nilai default
if (safeDistance < 10 || safeDistance > 400) {
safeDistance = 100;
// Menunggu input dari keypad
char key = NO_KEY;
while (key != '*') {
key = keypad.getKey();
if (key != NO_KEY) {
// Menampilkan input di LCD
lcd.setCursor(0, 1);
lcd.print(key);

  // Mengubah nilai jarak aman jika input merupakan angka
  if (key >= '0' && key <= '9') {
    safeDistance = (safeDistance * 10) + (key - '0');
  } else if (key == '#') { // Menghapus nilai jarak aman jika input merupakan '#'
    safeDistance = 0;
  }
}

}

// Menyimpan nilai jarak aman ke memori EEPROM
EEPROM.put(0, safeDistance);
}


}


void loop() {


// Membaca jarak dengan sensor ultrasonic
long distance = readDistance();

// Menampilkan nilai jarak di LCD
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Distance: ");
lcd.print(distance);
lcd.print(" cm ");

// Memeriksa apakah jarak lebih besar dari jarak aman
if (distance < safeDistance) {
// Menyalakan lampu LED Kuning
digitalWrite(ledYellow, HIGH);
// Mematikan lampu LED Hijau
digitalWrite(ledGreen, LOW);

// Memutar servo ke kanan
servo.write(servoPos - 45);
delay(500);

// Membaca jarak ke kanan
long distanceRight = readDistance();

// Memutar servo ke kiri
servo.write(servoPos + 45);
delay(500);

// Membaca jarak ke kiri
long distanceLeft = readDistance();

// Memutar servo ke posisi tengah
servo.write(servoPos);

// Memeriksa apakah jarak ke kanan atau ke kiri lebih besar
if (distanceRight > distanceLeft) {
// Jika jarak ke kanan lebih besar dari jarak aman, maka menyalakan lampu LED Hijau
if (distanceRight > safeDistance) {
digitalWrite(ledGreen, HIGH);
lcd.setCursor(0, 1);
lcd.print("Kanan");
delay(2000);
} else {
// Jika jarak ke kanan lebih kecil atau sama dengan jarak aman, maka menyalakan lampu LED Hijau
digitalWrite(ledRed, HIGH);
lcd.setCursor(0, 1);
lcd.print("Kiri");
delay(2000);
}
} else {
// Jika jarak ke kiri lebih besar dari jarak aman, maka menyalakan lampu LED Merah
if (distanceLeft > safeDistance) {
digitalWrite(ledRed, HIGH);
lcd.setCursor(0, 1);
lcd.print("Kiri");
delay(2000);
} else {
// Jika jarak ke kiri lebih kecil atau sama dengan jarak aman, maka menyalakan lampu LED Hijau
digitalWrite(ledGreen, HIGH);
lcd.setCursor(0, 1);
lcd.print("Kanan");
delay(2000);
}
}

// Mematikan lampu LED Merah dan Hijau
digitalWrite(ledRed, LOW);
digitalWrite(ledGreen, LOW);

// Mematikan lampu LED Kuning
digitalWrite(ledYellow,LOW);




// Memutar servo ke posisi tengah
servo.write(servoPos);

// Menunggu selama 1 detik
delay(1000);

// Mematikan semua lampu LED
digitalWrite(ledGreen, LOW);
digitalWrite(ledYellow, LOW);
digitalWrite(ledRed, LOW);

// Mematikan servo
servo.detach();

// Menunggu selama 1 detik
delay(1000);

// Kembali ke loop utama
}

}