#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h> // Sertakan library Servo

// Alamat I2C dari LCD
int i2c_address = 0x27; // Anda mungkin perlu mengganti ini sesuai dengan alamat LCD Anda

// Inisialisasi objek LCD
LiquidCrystal_I2C lcd(i2c_address, 16, 2); // 16 kolom, 2 baris

// Deklarasi PIN
int trig = 6;
int echo = 7;
long durasi, jarak;
int merah = 4;
int kuning = 3;
int hijau = 2;
int buzzer = 5;

// Deklarasi PIN servo
int servoPin = 9;

// Inisialisasi objek Servo
Servo servoMotor;

void setup() {
  // Mengatur pin sebagai OUTPUT atau INPUT
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(merah, OUTPUT);
  pinMode(kuning, OUTPUT);
  pinMode(hijau, OUTPUT);
  pinMode(buzzer, OUTPUT);
  Serial.begin(9600);
  
  // Menginisialisasi LCD
  lcd.init();
  lcd.backlight(); // Menyalakan backlight LCD
  lcd.setCursor(0, 0); // Posisi awal cursor
  lcd.print("Jarak :"); // Menampilkan judul

  // Menghubungkan servo ke pin
  servoMotor.attach(servoPin);
}

void loop() {
  // Mengukur jarak menggunakan sensor ultrasonik
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  durasi = pulseIn(echo, HIGH);
  jarak = (durasi / 2 / 29.1);
  
  // Menampilkan jarak di LCD
  lcd.setCursor(0, 1); // Posisi cursor LCD baris kedua
  lcd.print("                "); // Membersihkan tampilan jarak sebelumnya
  lcd.setCursor(0, 1); // Posisi cursor LCD baris kedua
  lcd.print("Jarak: " + String(jarak) + " cm"); // Menampilkan jarak

  // Memutuskan tindakan berdasarkan jarak
  if (jarak >= 150) {
    lcd.setCursor(0, 2); // Posisi cursor LCD baris ketiga
    lcd.print("===> AMAN"); // Menampilkan status hijau jika jarak >= 150
    digitalWrite(hijau, HIGH);
    digitalWrite(merah, LOW);
    digitalWrite(kuning, LOW);
    digitalWrite(buzzer, LOW);
    servoMotor.write(0); // Posisi servo untuk jarak aman
  } else if (jarak > 100 && jarak < 150) {
    lcd.setCursor(0, 2); // Posisi cursor LCD baris ketiga
    lcd.print("===> HATI-HATI"); // Menampilkan status kuning jika 100 < jarak < 150
    digitalWrite(hijau, LOW);
    digitalWrite(merah, LOW);
    digitalWrite(kuning, HIGH);
    digitalWrite(buzzer, LOW);
    servoMotor.write(90); // Posisi servo untuk jarak hati-hati
  } else {
    lcd.setCursor(0, 2); // Posisi cursor LCD baris ketiga
    lcd.print("===> BAHAYA"); // Menampilkan status merah jika jarak < 100
    digitalWrite(hijau, LOW);
    digitalWrite(merah, HIGH);
    digitalWrite(kuning, LOW);
    digitalWrite(buzzer, HIGH);
    servoMotor.write(180); // Posisi servo untuk jarak berbahaya
  }
}