#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

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

// Definisi pin LED
const int redLedPin = 3;
const int yellowLedPin = 4;
const int greenLedPin = 5;

// Definisi kecepatan suara
#define SOUND_SPEED 0.034

// Variabel untuk waktu dan jarak
long duration;
float distanceCm;

// OLED configuration
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET     -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

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

  // Setup pin sensor
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

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

  // Cek OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // berhenti di sini jika OLED gagal
  }

  // Menampilkan startup message
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);
  display.println("Distance Monitor");
  display.display();
  delay(1000);

  // Tes LED hijau
  digitalWrite(greenLedPin, HIGH);
  delay(1000);
  digitalWrite(greenLedPin, LOW);
}

void loop() {
  // Trigger sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Baca durasi dan hitung jarak
  duration = pulseIn(echoPin, HIGH);
  distanceCm = duration * SOUND_SPEED / 2;

  // Cetak ke serial
  Serial.print("distance (cm):  ");
  Serial.println(distanceCm);

  // Tampilkan di OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Distance: ");
  display.print(distanceCm);
  display.println(" cm");

  // Logika LED dan tampilan status di OLED
  if (distanceCm < 50 ) {
    digitalWrite(redLedPin, HIGH);
    digitalWrite(yellowLedPin, LOW);
    digitalWrite(greenLedPin, LOW);
    display.println("Status: Dekat");
  }
  else if (distanceCm >= 50 && distanceCm < 100) {
    digitalWrite(redLedPin, LOW);
    digitalWrite(yellowLedPin, HIGH);
    digitalWrite(greenLedPin, LOW);
    display.println("Status: Sedang");
  }
  else {
    digitalWrite(redLedPin, LOW);
    digitalWrite(yellowLedPin, LOW);
    digitalWrite(greenLedPin, HIGH);
    display.println("Status: Jauh");
  }

  display.display(); // Tampilkan data di layar OLED
  delay(1000); // Tunggu 1 detik
}