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

// Pin configuration
const int TRIG_PIN = 18;        // Pin TRIG sensor ultrasonik
const int ECHO_PIN = 19;        // Pin ECHO sensor ultrasonik
const int RELAY_PIN = 5;        // Pin untuk relay
const int BUZZER_PIN = 4;       // Pin untuk buzzer
const int LED_PIN = 2;          // Pin untuk LED indikator

// OLED display configuration
const int SCREEN_WIDTH = 128;   // Lebar OLED dalam pixel
const int SCREEN_HEIGHT = 64;   // Tinggi OLED dalam pixel
const int OLED_RESET = -1;      // Reset pin untuk OLED (tidak dipakai pada ESP32)

Ultrasonic ultrasonic(TRIG_PIN, ECHO_PIN);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Variables for managing time with millis()
unsigned long lastDistanceCheck = 0;
unsigned long lastRelayCheck = 0;
unsigned long lastDisplayUpdate = 0;
unsigned long lastDetected = 0;

// Timings (in milliseconds)
const unsigned long distanceInterval = 100;  // Interval untuk cek jarak
const unsigned long relayInterval = 100;     // Interval cek relay
const unsigned long displayInterval = 500;   // Interval update display
const unsigned long closeDelay = 5000;       // Delay sebelum pintu menutup (5 detik)

// State variables
float distance = 0;
bool relayActive = false;
bool objectDetected = false;

void setup() {
  pinMode(RELAY_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);  // Relay off (pintu tertutup)
  digitalWrite(BUZZER_PIN, LOW); // Buzzer off

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("Inisialisasi...");
  display.display();

  Serial.begin(115200);
}

// Function to check distance
void checkDistance() {
  distance = ultrasonic.read();  // Baca jarak dari sensor ultrasonik
  if (distance <= 10) {
    objectDetected = true;
    lastDetected = millis();     // Reset timer jika objek terdeteksi
    if (distance <= 5) {
      tone(BUZZER_PIN, 1000);    // Bunyikan buzzer pada frekuensi 1000Hz jika jarak kurang dari 5 cm
    } else {
      noTone(BUZZER_PIN);        // Nonaktifkan tone buzzer jika jarak lebih dari 5 cm
    }
  } else {
    objectDetected = false;
    noTone(BUZZER_PIN);          // Pastikan buzzer off jika jarak lebih dari 10 cm
  }
}

// Function to control relay
void controlRelay() {
  if (objectDetected) {
    if (!relayActive) {
      digitalWrite(RELAY_PIN, HIGH);  // Buka pintu
      relayActive = true;
    }
  } else if (millis() - lastDetected >= closeDelay && relayActive) {
    digitalWrite(RELAY_PIN, LOW);     // Tutup pintu
    relayActive = false;
  }
}

// Function to update OLED display
void updateDisplay() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Jarak: ");
  display.print(distance);
  display.println(" cm");

  if (relayActive) {
    display.println("Pintu Terbuka");
    digitalWrite(LED_PIN, HIGH); // Nyalakan LED saat pintu terbuka
  } else {
    display.println("Pintu Tertutup");
    digitalWrite(LED_PIN, LOW);  // Matikan LED saat pintu tertutup
  }

  display.display();
}

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

  // Check distance periodically
  if (currentMillis - lastDistanceCheck >= distanceInterval) {
    lastDistanceCheck = currentMillis;
    checkDistance();
  }

  // Control relay periodically
  if (currentMillis - lastRelayCheck >= relayInterval) {
    lastRelayCheck = currentMillis;
    controlRelay();
  }

  // Update OLED display periodically
  if (currentMillis - lastDisplayUpdate >= displayInterval) {
    lastDisplayUpdate = currentMillis;
    updateDisplay();
  }
}