#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <ServoESP32.h>  // Ganti dengan library ServoESP32
#include <Adafruit_GFX.h>

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

// Pin definitions
#define LED1 2
#define LED2 4
#define LED3 5
#define BUZZER 16
#define PIR_SENSOR 17
#define SERVO_PIN 15

ServoESP32 myServo;  // Ganti dengan ServoESP32

void setup() {
  // Initialize Serial
  Serial.begin(115200);
  Serial.println(F("Initializing..."));

  // Initialize LEDs
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);

  // Initialize Buzzer
  pinMode(BUZZER, OUTPUT);

  // Initialize PIR Sensor
  pinMode(PIR_SENSOR, INPUT);

  // Initialize Servo
  myServo.attach(SERVO_PIN);  // Ganti attach() untuk ServoESP32
  myServo.write(0); // Set servo to initial position (0 degrees)

  // Initialize OLED
  if (!display.begin(SSD1306_I2C_ADDRESS, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    while (true); // Stop program if OLED initialization fails
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(F("ESP32 Initialized"));
  display.display();
  delay(2000);
  display.clearDisplay();
}

void loop() {
  // Test LEDs (Sequential blinking)
  digitalWrite(LED1, HIGH);
  delay(500);
  digitalWrite(LED1, LOW);
  digitalWrite(LED2, HIGH);
  delay(500);
  digitalWrite(LED2, LOW);
  digitalWrite(LED3, HIGH);
  delay(500);
  digitalWrite(LED3, LOW);

  // Test Buzzer (Short beep)
  digitalWrite(BUZZER, HIGH);
  delay(200);
  digitalWrite(BUZZER, LOW);

  // Read PIR Sensor
  static int lastPIRState = LOW; // Track previous state of PIR
  int currentPIRState = digitalRead(PIR_SENSOR);

  if (currentPIRState == HIGH && lastPIRState == LOW) {
    // Motion detected
    Serial.println("Motion Detected!");
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println(F("Motion Detected!"));
    display.display();

    // Move servo to 90 degrees
    myServo.write(90);
    digitalWrite(LED1, HIGH); // Turn on LED1 as an indicator
  } else if (currentPIRState == LOW && lastPIRState == HIGH) {
    // No motion
    Serial.println("No Motion");
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println(F("No Motion"));
    display.display();

    // Move servo back to 0 degrees
    myServo.write(0);
    digitalWrite(LED1, LOW); // Turn off LED1
  }

  // Update last PIR state
  lastPIRState = currentPIRState;

  delay(100); // Small delay to stabilize readings
}