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

// Inisialisasi LCD (alamat I2C umum: 0x27 atau 0x3F)
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Servo
Servo servoRadar;    // Servo pemindai (D6)
Servo servoAksi;     // Servo aksi (D5)

// Pin sensor, buzzer, LED, dan tombol (disesuaikan untuk Mega)
const int trigPin = 9;
const int echoPin = 10;
const int buzzerPin = 7;
const int ledMerah = 8;
const int ledHijau = 4;
const int buttonPin = 3; // tombol mode siaga (gunakan INPUT_PULLUP)

long duration;
int distance;
int threshold = 10; // batas jarak deteksi (cm)
unsigned long lastAnimationTime = 0;
bool animasiAktif = true;
bool modeSiaga = false; // false = aktif, true = siaga

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

  servoRadar.attach(6); // servo radar -> D6 (PWM)
  servoAksi.attach(5);  // servo aksi  -> D5 (PWM)

  lcd.init();
  lcd.backlight();

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);
  pinMode(ledMerah, OUTPUT);
  pinMode(ledHijau, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP); // tombol aktif LOW

  // --- Tampilkan logo awal dengan efek beep ---
  tampilkanLogo(true); // true = aktifkan beep

  // Nyalakan LED hijau tanda siap
  digitalWrite(ledHijau, HIGH);
}

void loop() {
  // Periksa tombol siaga (toggle)
  if (digitalRead(buttonPin) == LOW) {
    delay(500); // debounce ringan
    modeSiaga = !modeSiaga; // toggle
    if (modeSiaga) {
      masukModeSiaga();
    } else {
      keluarModeSiaga();
    }
  }

  if (modeSiaga) return; // hentikan fungsi radar saat siaga

  // Gerakan servo radar ke kanan
  for (int angle = 0; angle <= 180 && !modeSiaga; angle += 4) {
    servoRadar.write(angle);
    delay(30);
    distance = getDistance();
    tampilkanData(angle, distance);
    if (distance <= threshold && distance > 0) {
      kondisiBahaya(angle);
    }
  }

  // Gerakan servo radar ke kiri
  for (int angle = 180; angle >= 0 && !modeSiaga; angle -= 4) {
    servoRadar.write(angle);
    delay(30);
    distance = getDistance();
    tampilkanData(angle, distance);
    if (distance <= threshold && distance > 0) {
      kondisiBahaya(angle);
    }
  }

  // Jalankan animasi tulisan jika aman dan tidak ada objek
  if (!modeSiaga && millis() - lastAnimationTime > 7000 && animasiAktif) {
    tampilkanLogo(false); // false = tanpa beep
    lastAnimationTime = millis();
  }
}

// Fungsi baca jarak HC-SR04
int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  duration = pulseIn(echoPin, HIGH);
  int distanceCm = duration * 0.034 / 2;
  return distanceCm;
}

// Fungsi tampilkan data radar
void tampilkanData(int angle, int distance) {
  Serial.print("Sudut:");
  Serial.print(angle);
  Serial.print("° | Jarak:");
  Serial.print(distance);
  Serial.println("cm");

  lcd.setCursor(0, 0);
  lcd.print("Sudut:");
  lcd.print(angle);
  lcd.print(" ");
  lcd.setCursor(0, 1);
  lcd.print("Jarak:");
  lcd.print(distance);
  lcd.print("cm ");

  if (distance > threshold || distance <= 0) {
    lcd.setCursor(12, 1);
    lcd.print("AMAN ");
    animasiAktif = true;
    digitalWrite(buzzerPin, LOW);
    digitalWrite(ledMerah, LOW);
    digitalWrite(ledHijau, HIGH);
  } else {
    animasiAktif = false;
  }
}

// Fungsi ketika objek dekat
void kondisiBahaya(int angle) {
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print(" !!! BAHAYA !!!");
  lcd.setCursor(0, 1);
  lcd.print("TERDETEKSI BOLOO");
  lcd.print(angle);

  digitalWrite(ledHijau, LOW);

  // Buzzer & LED merah berkedip
  for (int i = 0; i < 10; i++) {
    digitalWrite(buzzerPin, HIGH);
    digitalWrite(ledMerah, HIGH);
    delay(100);
    digitalWrite(buzzerPin, LOW);
    digitalWrite(ledMerah, LOW);
    delay(100);
  }

  // Servo aksi bergerak 90° selama 1 detik sebanyak 3x
  for (int i = 0; i < 3; i++) {
    servoAksi.write(90);
    delay(600);
    servoAksi.write(0);
    delay(600);
  }

  // Servo radar berhenti sampai objek menjauh
  while (getDistance() <= threshold && !modeSiaga) {
    delay(100);
  }

  digitalWrite(buzzerPin, LOW);
  digitalWrite(ledMerah, LOW);
  digitalWrite(ledHijau, HIGH);
  lcd.clear();
  animasiAktif = true;
}

// Fungsi animasi logo (beep opsional)
void tampilkanLogo(bool denganBeep) {
  for (int i = 0; i < 1; i++) {
    lcd.clear();
    tampilkanTeksBergerak("RADAR HAMABURUNG", 0, denganBeep);
    tampilkanTeksBergerak("SMADA TRENGGALEK", 1, denganBeep);
    delay(1000);
  }
}

// Fungsi teks bergerak + beep opsional
void tampilkanTeksBergerak(String teks, int baris, bool beep) {
  int panjang = teks.length();
  int startPos = (16 - panjang) / 2;
  for (int i = 0; i < panjang; i++) {
    lcd.setCursor(startPos + i, baris);
    lcd.print(teks[i]);
    if (beep) {
      digitalWrite(buzzerPin, HIGH);
      delay(40);
      digitalWrite(buzzerPin, LOW);
    }
    delay(150); //mengatur jeda waktu munculnya iklan logo tulisan 1 per 1 RADAR HAMABURUNG
  }
}

// Fungsi masuk ke mode siaga
void masukModeSiaga() {
  servoRadar.detach();
  servoAksi.detach();
  digitalWrite(buzzerPin, LOW);
  digitalWrite(ledMerah, LOW);
  digitalWrite(ledHijau, LOW);

  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("== MODE SIAGA ==");
  lcd.setCursor(0, 1);
  lcd.print("Tekan Tombol ON");

  Serial.println("Radar dalam mode SIAGA");
}

// Fungsi keluar dari mode siaga
void keluarModeSiaga() {
  servoRadar.attach(6);
  servoAksi.attach(5);
  digitalWrite(ledHijau, HIGH);
  lcd.clear();
  lcd.print("Radar Aktif Lagi");
  delay(200);
  lcd.clear();
  Serial.println("Radar kembali AKTIF");
}