#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Servo.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);

const byte ROWS = 4;
const byte COLS = 4;

char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};

byte rowPins[ROWS] = {9, 8, 7, 6};
byte colPins[COLS] = {5, 4, 3, 2};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

Servo servo;
int safeDistance = 100;
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);
}

void loop() {
  // Membaca jarak dengan menggunakan sensor ultrasonic
long distance = readDistance();
// Memutar servo ke posisi tengah
servo.write(servoPos);

// Menampilkan jarak di LCD
lcd.init();
lcd.backlight();
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);

// 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) {
  // Menyalakan lampu LED Merah
  digitalWrite(ledRed, HIGH);
  // Mematikan lampu LED Hijau
  digitalWrite(ledGreen, LOW);
}
else if (distanceLeft < distanceRight) {
  // Menyalakan lampu LED Hijau
  digitalWrite(ledGreen, HIGH);
  // Mematikan lampu LED Merah
  digitalWrite(ledRed,LOW);
}

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

// Meminta input jarak aman dari keypad
char key = keypad.getKey();
if (key != NO_KEY) {
if (key == 'A') {
// Jika tombol A ditekan, maka jarak aman akan diset ke 10 cm
safeDistance = 10;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else if (key == 'B') {
// Jika tombol B ditekan, maka jarak aman akan diset ke 20 cm
safeDistance = 20;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else if (key == 'C') {
// Jika tombol C ditekan, maka jarak aman akan diset ke 30 cm
safeDistance = 30;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else if (key == 'D') {
// Jika tombol D ditekan, maka jarak aman akan diset ke 40 cm
safeDistance = 40;
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
else {
// Jika tombol angka ditekan, maka jarak aman akan diset ke nilai yang sesuai
safeDistance = key - '0';
lcd.setCursor(0, 0);
lcd.print("Safe distance: ");
lcd.print(safeDistance);
lcd.print(" cm ");
}
}

delay(100);
}