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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define OLED_SDA 26
#define OLED_SCL 27
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int trigPin = 12;
const int echoPin = 14;
const int buzzer  = 13;

// =========================================================================
// FUNGSI TAMPIL OLED
// =========================================================================
void tampilOLED(int jarak, String status) {
  display.clearDisplay();

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(20, 0);
  display.println("SENSOR FORKLIFT");
  display.drawLine(0, 10, 127, 10, SSD1306_WHITE);

  display.setCursor(0, 14);
  display.print("Jarak: ");
  if (jarak == -1) {
    display.println("-- cm");
  } else {
    display.print(jarak);
    display.println(" cm");
  }

  display.setTextSize(2);
  if (status == "BAHAYA") {
    display.setCursor(10, 30);
    display.println("! BAHAYA !");
  } else if (status == "WASPADA") {
    display.setCursor(8, 30);
    display.println("WASPADA");
  } else if (status == "HATI-HATI") {
    display.setTextSize(1);
    display.setCursor(20, 30);
    display.println("HATI - HATI");
  } else {
    display.setCursor(25, 30);
    display.println("AMAN");
  }

  display.setTextSize(1);
  display.setCursor(0, 54);
  if (jarak == -1) {
    display.println("[ no signal ]");
  } else if (jarak <= 100) {
    display.println("[###########]");
  } else if (jarak <= 200) {
    display.println("[#######    ]");
  } else if (jarak <= 300) {
    display.println("[###        ]");
  } else {
    display.println("[           ]");
  }

  display.display();
}

// =========================================================================
// FUNGSI BACA JARAK
// =========================================================================
int bacaJarak() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(4);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long dur = pulseIn(echoPin, HIGH, 30000);
  if (dur == 0) return -1;
  return (int)(dur * 0.034 / 2);
}

// =========================================================================
// SETUP
// =========================================================================
void setup() {
  Serial.begin(115200);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzer, OUTPUT);
  digitalWrite(buzzer, LOW);

  Wire.begin(OLED_SDA, OLED_SCL);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("OLED gagal! Cek kabel.");
    while (true);
  }

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(20, 20);
  display.println("Sistem Siap!");
  display.display();
  delay(1000);
}

// =========================================================================
// LOOP UTAMA
// =========================================================================
void loop() {
  int jarak = bacaJarak();

  // jarak -1 = tidak terdeteksi = AMAN (bukan BAHAYA)
  String status;
  if (jarak == -1)          status = "AMAN";
  else if (jarak <= 100)    status = "BAHAYA";
  else if (jarak <= 200)    status = "WASPADA";
  else if (jarak <= 300)    status = "HATI-HATI";
  else                      status = "AMAN";

  Serial.print("Jarak: ");
  Serial.print(jarak == -1 ? 0 : jarak);
  Serial.print(" cm | STATUS: ");
  Serial.println(status);

  tampilOLED(jarak, status);

  // Buzzer — AMAN tidak bunyi
  if (jarak > 0 && jarak <= 100) {
    // BAHAYA: bunyi super cepat
    digitalWrite(buzzer, HIGH);
    delay(80);
    digitalWrite(buzzer, LOW);
    delay(80);

  } else if (jarak > 100 && jarak <= 200) {
    // WASPADA: bunyi sedang
    digitalWrite(buzzer, HIGH);
    delay(150);
    digitalWrite(buzzer, LOW);
    delay(150);
    digitalWrite(buzzer, HIGH);
    delay(150);
    digitalWrite(buzzer, LOW);
    delay(400);

  } else if (jarak > 200 && jarak <= 300) {
    // HATI-HATI: 5x per detik
    for (int i = 0; i < 5; i++) {
      digitalWrite(buzzer, HIGH);
      delay(100);
      digitalWrite(buzzer, LOW);
      delay(100);
    }

  } else {
    // AMAN: buzzer mati total
    digitalWrite(buzzer, LOW);
  }

  Serial.println("------------------------");
  delay(60);
}