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

#define TRIG_PIN 18
#define ECHO_PIN 19
#define RELAY_PIN 17
#define BUZZ_PIN  12

Adafruit_SSD1306 display(128, 64, &Wire);

enum State {OPEN, WAITING, CLOSED};
State statusPintu = CLOSED;

unsigned long lastTimeJarak = 0;
unsigned long timeDelay = 0;
unsigned long tungguDelay = 5000;
unsigned long jarakInterval = 100;

void setup() {
  Serial.begin(115200);
  pinMode(BUZZ_PIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.display();
}

void loop() {
  long dur;
  if (millis() - lastTimeJarak >= jarakInterval) {
    lastTimeJarak = millis();

    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    dur = pulseIn(ECHO_PIN, HIGH);
    long jarak = dur * 0.034 / 2;

    display.clearDisplay();
    display.setCursor(0, 0);
    display.print("Jarak: ");
    display.print(jarak);
    display.println(" cm");
    display.print("Status: ");

    if (statusPintu == OPEN) {
      display.println("Terbuka");
    } else if (statusPintu == WAITING) {
      display.println("Menunggu...");
    } else {
      display.println("Tertutup");
    }
    display.display();

    switch (statusPintu) {
      case CLOSED:
        if (jarak <= 10) {
          statusPintu = OPEN;
          tone(BUZZ_PIN, 200);
          digitalWrite(RELAY_PIN, HIGH);
        }
        break;

      case OPEN:
        if (jarak > 10) {
          statusPintu = WAITING;
          timeDelay = millis();
        }
        break;

      case WAITING:
        if (millis() - timeDelay >= tungguDelay) {
          statusPintu = CLOSED;
          tone(BUZZ_PIN, 0);
          digitalWrite(RELAY_PIN, LOW);
        }
        break;
    }
  }
  delay(10);
}