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

// Inisialisasi LCD I2C (alamat 0x27 dan ukuran LCD 16x2)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Pin LED
#define LED_HIJAU 4
#define LED_KUNING 5
#define LED_MERAH 6

// Pin Ultrasonik
#define TRIGGER_PIN 7
#define ECHO_PIN 8

// Fungsi untuk membaca jarak menggunakan sensor ultrasonik
long bacaJarak(int triggerPin, int echoPin) {
  pinMode(triggerPin, OUTPUT);    // Set pin trigger sebagai output
  digitalWrite(triggerPin, LOW); // Standby dalam kondisi LOW
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH); // Mulai generate gelombang ultrasonik
  delayMicroseconds(10);         // Tahan selama 10 microseconds
  digitalWrite(triggerPin, LOW); // Stop generate gelombang
  pinMode(echoPin, INPUT);       // Set pin echo sebagai input
  return pulseIn(echoPin, HIGH); // Baca durasi sinyal pantulan
}

void setup() {
  Serial.begin(9600);

  // Inisialisasi LCD
  lcd.init();
  lcd.backlight(); // Nyalakan backlight LCD
  lcd.setCursor(0, 0);
  lcd.print("Mengukur Jarak...");

  // Inisialisasi LED
  pinMode(LED_HIJAU, OUTPUT);
  pinMode(LED_KUNING, OUTPUT);
  pinMode(LED_MERAH, OUTPUT);

  // Tampilkan pesan awal
  lcd.setCursor(0, 1);
  lcd.print("Tunggu...");
}

void loop() {
  int cm = 0; // Reset nilai jarak
  cm = (0.03431 * bacaJarak(TRIGGER_PIN, ECHO_PIN)) / 2; // Konversi ke cm

  // Tampilkan di Serial Monitor
  Serial.print("Jarak: ");
  Serial.print(cm);
  Serial.println(" cm");

  // Tampilkan di LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Jarak: ");
  lcd.print(cm);
  lcd.print(" cm");

  // Logika LED berdasarkan jarak
  if (cm > 15) {
    digitalWrite(LED_HIJAU, HIGH);
    digitalWrite(LED_KUNING, LOW);
    digitalWrite(LED_MERAH, LOW);
  } else if (cm > 5 && cm <= 15) {
    digitalWrite(LED_HIJAU, LOW);
    digitalWrite(LED_KUNING, HIGH);
    digitalWrite(LED_MERAH, LOW);
  } else {
    digitalWrite(LED_HIJAU, LOW);
    digitalWrite(LED_KUNING, LOW);
    digitalWrite(LED_MERAH, HIGH);
  }

  delay(500); // Delay untuk pembacaan ulang
}