#include <Wire.h>               // Library untuk komunikasi I2C
#include <Adafruit_GFX.h>       // Library dasar untuk grafik OLED
#include <Adafruit_SSD1306.h>   // Library untuk OLED Display

// Mendefinisikan ukuran layar OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1           // Tidak menggunakan pin reset khusus

// Membuat objek display untuk OLED
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Mendefinisikan pin sensor HC-SR04 pada ESP32
#define trigPin 5   // GPIO5 sebagai pin trigger
#define echoPin 18  // GPIO18 sebagai pin echo

// Mendefinisikan pin LED pada ESP32 (gunakan pin yang aman)
#define redLedPin 13      // GPIO13 untuk LED Merah (indikator jarak dekat)
#define yellowLedPin 14   // GPIO14 untuk LED Kuning (indikator jarak sedang)
#define greenLedPin 27    // GPIO27 untuk LED Hijau (indikator jarak jauh)

// Menentukan kecepatan suara dalam cm/us
#define SOUND_SPEED 0.0343

long duration;             // Variabel untuk menyimpan durasi waktu gelombang suara
float distanceCm;          // Variabel untuk menyimpan jarak dalam cm
float prevDistance = 0;    // Variabel untuk menyimpan jarak sebelumnya

// Fungsi untuk menggambar animasi dua mata pada OLED
void drawEyes(int position) {
  display.clearDisplay();

  // Menggambar bola mata kiri
  display.drawCircle(32, 32, 20, SSD1306_WHITE);  // lingkaran luar mata kiri
  display.fillCircle(32 + position, 32, 8, SSD1306_WHITE); // bola hitam mata kiri

  // Menggambar bola mata kanan
  display.drawCircle(96, 32, 20, SSD1306_WHITE);  // lingkaran luar mata kanan
  display.fillCircle(96 + position, 32, 8, SSD1306_WHITE); // bola hitam mata kanan

  display.display();
}

void setup() {
  Serial.begin(115200);           // Memulai komunikasi serial pada baud rate 115200
  pinMode(trigPin, OUTPUT);       // Menetapkan pin trigPin sebagai OUTPUT
  pinMode(echoPin, INPUT);        // Menetapkan pin echoPin sebagai INPUT

  // Menetapkan pin LED sebagai OUTPUT
  pinMode(redLedPin, OUTPUT);
  pinMode(yellowLedPin, OUTPUT);
  pinMode(greenLedPin, OUTPUT);

  // Menghidupkan LED hijau untuk memastikan LED terhubung dengan benar
  digitalWrite(greenLedPin, HIGH);
  delay(1000);                    // LED hijau menyala selama 1 detik
  digitalWrite(greenLedPin, LOW); // Matikan LED hijau setelah 1 detik

  // Inisialisasi OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED initialization failed"));
    while (true); // Berhenti jika OLED gagal diinisialisasi
  }
  display.display();              // Menampilkan layar awal
  delay(2000);                    // Menunggu selama 2 detik
  display.clearDisplay();         // Membersihkan layar OLED
}

void loop() {
  // Mengirim sinyal LOW ke trigPin untuk memastikan kondisi awal
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2); // Delay 2 mikrodetik

  // Mengirim sinyal HIGH ke trigPin selama 10 mikrodetik untuk memicu sensor
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW); // Mengembalikan trigPin ke LOW

  // Membaca durasi pantulan ultrasonik dari echoPin
  duration = pulseIn(echoPin, HIGH);

  // Menghitung jarak berdasarkan durasi (kecepatan suara = 0.0343 cm/us)
  distanceCm = (duration * SOUND_SPEED) / 2;

  // Mencetak hasil jarak pada Serial Monitor
  Serial.print("Distance (cm): ");
  Serial.println(distanceCm);

  // Jika ada perubahan jarak lebih dari 1 cm, tampilkan animasi mata bergerak
  if (abs(distanceCm - prevDistance) > 1) {
    // Animasi mata melihat ke tengah
    drawEyes(0);
    delay(200);
    // Animasi mata melihat ke kanan
    drawEyes(5);
    delay(200);
    // Animasi mata melihat ke kiri
    drawEyes(-5);
    delay(200);
    // Animasi mata kembali ke tengah
    drawEyes(0);
  }

  prevDistance = distanceCm; // Update jarak sebelumnya

  // Menampilkan jarak pada OLED di bawah animasi mata
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 55); // Menampilkan di bawah mata
  display.print("Distance: ");
  display.print(distanceCm);
  display.print(" cm");
  display.display();

  // Menyalakan LED berdasarkan jarak yang diukur
  if (distanceCm < 100) {  // Jarak dekat
    digitalWrite(redLedPin, HIGH);    // Menyalakan LED Merah
    digitalWrite(yellowLedPin, LOW);  // Mematikan LED Kuning
    digitalWrite(greenLedPin, LOW);   // Mematikan LED Hijau
  }
  else if (distanceCm >= 100 && distanceCm < 250) {  // Jarak sedang
    digitalWrite(redLedPin, LOW);     // Mematikan LED Merah
    digitalWrite(yellowLedPin, HIGH); // Menyalakan LED Kuning
    digitalWrite(greenLedPin, LOW);   // Mematikan LED Hijau
  }
  else {  // Jarak jauh
    digitalWrite(redLedPin, LOW);     // Mematikan LED Merah
    digitalWrite(yellowLedPin, LOW);  // Mematikan LED Kuning
    digitalWrite(greenLedPin, HIGH);  // Menyalakan LED Hijau
  }

  delay(500); // Delay sebelum pembacaan berikutnya
}