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

// Inisialisasi objek LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Inisialisasi pin sensor ultrasonik
const int trigPin = 9;
const int echoPin = 10;

// Inisialisasi pin buzzer dan LED
const int buzzerPin = 7;
const int ledPin = 8;
const int ledPin2 = 4;

// Inisialisasi objek Servo
Servo myservo;

// Status servo
bool servoActivated = false;

void setup() {
  // Inisialisasi pin-mode
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(ledPin, OUTPUT);
  pinMode(ledPin2, OUTPUT);

  // Inisialisasi LCD
  lcd.begin(16, 2);
  lcd.print("Jarak: ");

  // Inisialisasi Servo
  myservo.attach(6); // Servo dihubungkan ke pin 6
}

void loop() {
  // Mengirim pulse ultrasonik
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Menerima hasil echo
  long duration = pulseIn(echoPin, HIGH);

  // Menghitung jarak dalam centimeter
  int distance = duration * 0.034 / 2;

  // Menampilkan hasil di LCD
  lcd.setCursor(7, 0);
  lcd.print("    "); // Membersihkan karakter sebelumnya
  lcd.setCursor(7, 0);
  lcd.print(distance);
  lcd.print(" cm");

  // Kondisi ketika jarak kurang dari 10cm
  if (distance < 10) {
    digitalWrite(buzzerPin, HIGH); // Mengaktifkan buzzer
    digitalWrite(ledPin, HIGH);    // Mengaktifkan LED 1
    digitalWrite(ledPin2, LOW);     // Mematikan LED 2
    myservo.write(90);              // Mematikan servo pada 90 derajat
    servoActivated = false;         // Menyimpan status servo
  } else if (distance < 300) {
    digitalWrite(buzzerPin, LOW);  // Mematikan buzzer
    digitalWrite(ledPin, LOW);     // Mematikan LED 1
    digitalWrite(ledPin2, HIGH);   // Mengaktifkan LED 2

    // Kondisi ketika jarak lebih dari 10cm
    if (!servoActivated) {
      myservo.write(180); // Memutar servo ke sudut 180 derajat
      delay(500);          // Delay 500ms (5 detik)
      myservo.write(0);   // Memutar servo ke sudut 0 derajat
      delay(500);         // Delay 500ms (5 detik)
      myservo.write(90);  // Memutar servo ke sudut 90 derajat
      servoActivated = true; // Menyimpan status servo
    }
  } else {
    digitalWrite(buzzerPin, HIGH); // Mengaktifkan buzzer
    digitalWrite(ledPin, HIGH);    // Mengaktifkan LED 1
    digitalWrite(ledPin2, LOW);     // Mematikan LED 2
    myservo.write(90);              // Mematikan servo pada 90 derajat
    servoActivated = false;         // Menyimpan status servo
  }
  
  // Delay untuk memperlambat pembacaan
  delay(500);
}