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

#define TRIG_PIN 25
#define ECHO_PIN 33
#define RELAY_PIN 17
#define BUZZER_PIN 16
#define MAX_DISTANCE 200
#define OLED_RESET -1
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Inisialisasi sensor ultrasonik
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);

// Inisialisasi OLED display
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Variabel untuk millis()
unsigned long previousMillis = 0;
unsigned long relayMillis = 0;
unsigned long interval = 100;  // Interval pengukuran jarak
unsigned long relayInterval = 5000;  // Interval waktu relay saat tidak ada objek

// Status
bool relayState = false;
bool buzzerState = false;
float distance = 0;

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


  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  // Matikan relay dan buzzer di awal
  digitalWrite(RELAY_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);

  // Inisialisasi OLED
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED GAGAL TERSAMBUNG"));
    for (;;);
  }
  display.clearDisplay();
  display.display();
}

void loop() {
  unsigned long currentMillis = millis();

  // Mengukur jarak setiap 100 ms
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    distance = sonar.ping_cm();
    Serial.print("Jarak: ");
    Serial.print(distance);
    Serial.println(" cm");
    
    // Tampilkan jarak di OLED
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print("Jarak: ");
    display.print(distance);
    display.print(" cm");

    // Kondisi relay (pintu)
    if (distance > 0 && distance < 10) {
      digitalWrite(RELAY_PIN, HIGH);  // Aktifkan relay (buka pintu)
      relayState = true;
      relayMillis = currentMillis;    // Reset timer relay
      display.setCursor(0, 20);
      display.print("Pintu Terbuka");
    } else if (currentMillis - relayMillis >= relayInterval) {
      digitalWrite(RELAY_PIN, LOW);   // Matikan relay (tutup pintu)
      relayState = false;
      display.setCursor(0, 20);
      display.print("Pintu Tertutup");
    }

    // Kondisi buzzer
    if (distance > 0 && distance < 5) {
      tone(BUZZER_PIN, 1000);  // Aktifkan buzzer dengan frekuensi 1000 Hz
      buzzerState = true;
    } else {
      noTone(BUZZER_PIN);   // Matikan suara buzzer
      buzzerState = false;
    }

    // Update OLED display
    display.display();
  }
}