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

// Inisialisasi LCD I2C dengan alamat yang benar untuk ukuran 16x2
LiquidCrystal_I2C lcd1(0x28, 16, 2); // Inisialisasi LCD pertama dengan alamat 0x27
LiquidCrystal_I2C lcd2(0x27, 16, 2); // Inisialisasi LCD kedua dengan alamat 0x3F

// Inisialisasi servo
Servo servo1; // Inisialisasi objek servo pertama
Servo servo2; // Inisialisasi objek servo kedua

const int servo1Pin = 10; // Pin digital untuk servo 1
const int servo2Pin = 11; // Pin digital untuk servo 2

// Definisikan pin untuk sensor proximity, lampu, dan buzzer
const int sensor1ProximityPin = 4; // Pin untuk sensor proximity 1
const int sensor2ProximityPin = 7; // Pin untuk sensor proximity 2
const int sensor1UltrasonicEchoPin = 3; // Pin untuk echo sensor ultrasonik 1
const int sensor1UltrasonicTrigPin = 2; // Pin untuk trig sensor ultrasonik 1
const int sensor2UltrasonicEchoPin = 6; // Pin untuk echo sensor ultrasonik 2
const int sensor2UltrasonicTrigPin = 5; // Pin untuk trig sensor ultrasonik 2
const int greenLampPin = 9; // Pin untuk lampu hijau
const int redLampPin = 13; // Pin untuk lampu merah
const int buzzerPin = 8; // Pin untuk buzzer aktif

bool alertMode = false; // Status mode peringatan
bool trainDetectedWest = false; // Status deteksi kereta di barat
bool trainDetectedEast = false; // Status deteksi kereta di timur

void setup() {
  Serial.begin(9600); // Memulai komunikasi serial dengan baud rate 9600

  pinMode(sensor1ProximityPin, INPUT); // Mengatur pin sensor proximity 1 sebagai input
  pinMode(sensor2ProximityPin, INPUT); // Mengatur pin sensor proximity 2 sebagai input
  pinMode(sensor1UltrasonicEchoPin, INPUT); // Mengatur pin echo sensor ultrasonik 1 sebagai input
  pinMode(sensor1UltrasonicTrigPin, OUTPUT); // Mengatur pin trig sensor ultrasonik 1 sebagai output
  pinMode(sensor2UltrasonicEchoPin, INPUT); // Mengatur pin echo sensor ultrasonik 2 sebagai input
  pinMode(sensor2UltrasonicTrigPin, OUTPUT); // Mengatur pin trig sensor ultrasonik 2 sebagai output
  pinMode(greenLampPin, OUTPUT); // Mengatur pin lampu hijau sebagai output
  pinMode(redLampPin, OUTPUT); // Mengatur pin lampu merah sebagai output
  pinMode(buzzerPin, OUTPUT); // Mengatur pin buzzer sebagai output

  servo1.attach(servo1Pin); // Menghubungkan servo 1 ke pin yang ditentukan
  servo2.attach(servo2Pin); // Menghubungkan servo 2 ke pin yang ditentukan
  servo1.write(0); // Mengatur posisi awal servo 1 ke 0 derajat
  servo2.write(0); // Mengatur posisi awal servo 2 ke 0 derajat

  lcd1.init(); // Inisialisasi LCD pertama
  lcd2.init(); // Inisialisasi LCD kedua

  // Menampilkan pesan awal pada kedua LCD
  lcd1.backlight(); // Menghidupkan lampu latar LCD pertama
  lcd1.setCursor(0, 0); // Mengatur kursor LCD pertama ke baris pertama
  lcd1.print(" Sistem  Palang "); // Menampilkan pesan di baris pertama LCD pertama
  lcd1.setCursor(0, 1); // Mengatur kursor LCD pertama ke baris kedua
  lcd1.print(" Pintu  Dimulai "); // Menampilkan pesan di baris kedua LCD pertama

  lcd2.backlight(); // Menghidupkan lampu latar LCD kedua
  lcd2.setCursor(0, 0); // Mengatur kursor LCD kedua ke baris pertama
  lcd2.print(" Sistem  Palang "); // Menampilkan pesan di baris pertama LCD kedua
  lcd2.setCursor(0, 1); // Mengatur kursor LCD kedua ke baris kedua
  lcd2.print(" Pintu  Dimulai "); // Menampilkan pesan di baris kedua LCD kedua

  delay(5000); // Tunggu 5 detik sebelum memulai loop utama

  lcd1.clear(); // Membersihkan layar LCD pertama
  lcd2.clear(); // Membersihkan layar LCD kedua
}

void loop() {
  int sensor1ProximityValue = digitalRead(sensor1ProximityPin); // Membaca nilai dari sensor proximity 1
  int sensor2ProximityValue = digitalRead(sensor2ProximityPin); // Membaca nilai dari sensor proximity 2
  int sensor1Distance = getUltrasonicDistance(sensor1UltrasonicTrigPin, sensor1UltrasonicEchoPin); // Mengukur jarak dari sensor ultrasonik 1
  int sensor2Distance = getUltrasonicDistance(sensor2UltrasonicTrigPin, sensor2UltrasonicEchoPin); // Mengukur jarak dari sensor ultrasonik 2

  // Menampilkan nilai sensor ke Serial Monitor
  Serial.print("Sensor 1 Proximity: ");
  Serial.println(sensor1ProximityValue);
  Serial.print("Sensor 2 Proximity: ");
  Serial.println(sensor2ProximityValue);
  Serial.print("Sensor 1 Ultrasonic Distance: ");
  Serial.println(sensor1Distance);
  Serial.print("Sensor 2 Ultrasonic Distance: ");
  Serial.println(sensor2Distance);

  if (!alertMode) { // Jika mode peringatan tidak aktif
    if (sensor1ProximityValue == HIGH && sensor1Distance <= 6) { // Jika kereta terdeteksi di barat
      setAlertMode(true); // Mengatur mode peringatan dengan deteksi dari barat
      displayWarning(true); // Menampilkan peringatan di LCD untuk kereta dari barat
    } else if (sensor2ProximityValue == HIGH && sensor2Distance <= 6) { // Jika kereta terdeteksi di timur
      setAlertMode(false); // Mengatur mode peringatan dengan deteksi dari timur
      displayWarning(false); // Menampilkan peringatan di LCD untuk kereta dari timur
    } else {
      setStandbyMode(); // Mengatur mode siaga jika tidak ada kereta terdeteksi
    }
  } else { // Jika mode peringatan aktif
    if ((trainDetectedWest && sensor2ProximityValue == HIGH && sensor2Distance <= 6) ||
        (trainDetectedEast && sensor1ProximityValue == HIGH && sensor1Distance <= 6)) {
      delay(3000); // Jeda 3 detik
      setStandbyMode(); // Mengatur mode siaga
      alertMode = false; // Mengatur status mode peringatan menjadi tidak aktif
    }
  }

  delay(500); // Menunggu 5 milidetik sebelum membaca nilai lagi
}

void setAlertMode(bool westDetected) {
  digitalWrite(redLampPin, HIGH); // Menyalakan lampu merah
  digitalWrite(greenLampPin, LOW); // Mematikan lampu hijau
  tone(buzzerPin, 1000); // Mengaktifkan buzzer dengan frekuensi 1000 Hz
  alertMode = true; // Mengatur status mode peringatan menjadi aktif

  trainDetectedWest = westDetected; // Mengatur status deteksi kereta di barat
  trainDetectedEast = !westDetected; // Mengatur status deteksi kereta di timur

  // Tambahkan jeda 3 detik sebelum memutar servo
  delay(3000); // Jeda 3 detik

  for (int pos = 0; pos <= 40; pos++) { // Memutar servo ke 40 derajat secara perlahan
    servo1.write(pos); // Mengatur posisi servo 1
    servo2.write(pos); // Mengatur posisi servo 2
    delay(15); // Jeda 15 milidetik antara setiap langkah
  }
}

void setStandbyMode() {
  digitalWrite(redLampPin, LOW); // Mematikan lampu merah
  digitalWrite(greenLampPin, HIGH); // Menyalakan lampu hijau
  noTone(buzzerPin); // Mematikan buzzer
  alertMode = false; // Mengatur status mode peringatan menjadi tidak aktif

  // Menampilkan pesan siaga pada kedua LCD
  lcd1.clear(); // Membersihkan layar LCD pertama
  lcd1.setCursor(0, 0); // Mengatur kursor LCD pertama ke baris pertama
  lcd1.print("Perlintasan aman"); // Menampilkan pesan di baris pertama LCD pertama
  lcd1.setCursor(0, 1); // Mengatur kursor LCD pertama ke baris kedua
  lcd1.print(" untuk  dilalui "); // Menampilkan pesan di baris kedua LCD pertama

  lcd2.clear(); // Membersihkan layar LCD kedua
  lcd2.setCursor(0, 0); // Mengatur kursor LCD kedua ke baris pertama
  lcd2.print("Perlintasan aman"); // Menampilkan pesan di baris pertama LCD kedua
  lcd2.setCursor(0, 1); // Mengatur kursor LCD kedua ke baris kedua
  lcd2.print(" untuk  dilalui "); // Menampilkan pesan di baris kedua LCD kedua

  if (servo1.read() != 0 || servo2.read() != 0) { // Jika posisi servo tidak di 0 derajat
    for (int pos = 40; pos >= 0; pos--) { // Mengembalikan servo ke posisi awal 0 derajat secara perlahan
      servo1.write(pos); // Mengatur posisi servo 1
      servo2.write(pos); // Mengatur posisi servo 2
      delay(15); // Jeda 15 milidetik antara setiap langkah
    }
  }
}

int getUltrasonicDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW); // Mengatur pin trig menjadi LOW
  delayMicroseconds(2); // Jeda 2 mikrodetik
  digitalWrite(trigPin, HIGH); // Mengatur pin trig menjadi HIGH
  delayMicroseconds(10); // Jeda 10 mikrodetik
  digitalWrite(trigPin, LOW); // Mengatur pin trig menjadi LOW

  long duration = pulseIn(echoPin, HIGH); // Mengukur durasi pulsa HIGH pada pin echo
  int distance = duration * 0.034 / 2; // Menghitung jarak berdasarkan durasi pulsa

  return distance; // Mengembalikan jarak yang diukur
}

void displayWarning(bool fromWest) {
  lcd1.clear(); // Membersihkan layar LCD pertama
  lcd2.clear(); // Membersihkan layar LCD kedua

  if (fromWest) { // Jika kereta datang dari barat
    lcd1.setCursor(0, 0); // Mengatur kursor LCD pertama ke baris pertama
    lcd1.print("KA AKAN MELINTAS"); // Menampilkan pesan peringatan di baris pertama LCD pertama
    lcd1.setCursor(0, 1); // Mengatur kursor LCD pertama ke baris kedua
    lcd1.print("==>===>  ===>==>"); // Menampilkan pesan peringatan di baris kedua LCD pertama
    lcd2.setCursor(0, 0); // Mengatur kursor LCD kedua ke baris pertama
    lcd2.print("KA AKAN MELINTAS"); // Menampilkan pesan peringatan di baris pertama LCD kedua
    lcd2.setCursor(0, 1); // Mengatur kursor LCD kedua ke baris kedua
    lcd2.print("<==<===  <===<=="); // Menampilkan pesan peringatan di baris kedua LCD kedua

  } else { // Jika kereta datang dari timur
    lcd1.setCursor(0, 0); // Mengatur kursor LCD pertama ke baris pertama
    lcd1.print("KA AKAN MELINTAS"); // Menampilkan pesan peringatan di baris pertama LCD pertama
    lcd1.setCursor(0, 1); // Mengatur kursor LCD pertama ke baris kedua
    lcd1.print("<==<===  <===<=="); // Menampilkan pesan peringatan di baris kedua LCD pertama
    lcd2.setCursor(0, 0); // Mengatur kursor LCD kedua ke baris pertama
    lcd2.print("KA AKAN MELINTAS"); // Menampilkan pesan peringatan di baris pertama LCD kedua
    lcd2.setCursor(0, 1); // Mengatur kursor LCD kedua ke baris kedua
    lcd2.print("==>===>  ===>==>"); // Menampilkan pesan peringatan di baris kedua LCD kedua

  }
}