#include "ESP32Servo.h"


// Definisi pin
#define TRIG 25      // Pin trig sensor ultrasonik (jika diperlukan)
#define ECHO 33      // Pin echo sensor ultrasonik (jika diperlukan)
#define PIR 15       // Pin sensor PIR
#define BUTTON 14    // Pin tombol

Servo servo1;   // Servo 1 (biasa, 0°-180°)
Servo servo2;   // Servo 2 (biasa, 0°-180°)

// Mendefinisikan state untuk state machine
enum State { IDLE, MOVE_SERVO1, STOP_SERVO1, MOVE_SERVO2 };
State currentState = IDLE;

bool lastButtonState = false;

void setup() {
  Serial.begin(115200);
  
  // Konfigurasi pin
  pinMode(BUTTON, INPUT);
  pinMode(PIR, INPUT);
  // Jika menggunakan sensor ultrasonik, konfigurasi TRIG dan ECHO
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  
  // Hubungkan servo ke pin yang sesuai
  servo1.attach(13);
  servo2.attach(17);
  
  // Set posisi awal servo
  servo1.write(0);
  servo2.write(0);
  
  Serial.println("Sistem siap.");
}

void loop() {
  // Baca status tombol
  bool buttonState = digitalRead(BUTTON);
  
  // State machine dengan switch-case
  switch (currentState) {
    case IDLE:
      // Menunggu tombol ditekan
      if (buttonState == HIGH && lastButtonState == LOW) {
        Serial.println("Tombol ditekan. Mulai gerakan Servo 1.");
        currentState = MOVE_SERVO1;
      }
      break;
      
    case MOVE_SERVO1:
      // Gerakkan servo1 bolak-balik hingga sensor PIR mendeteksi gerakan
      // Pertama, naikkan dari 0° ke 180°
      for (int pos = 0; pos <= 180; pos++) {
        servo1.write(pos);
        delay(15);
        if (digitalRead(PIR) == HIGH) {
          currentState = STOP_SERVO1;
          break;
        }
      }
      // Jika belum terdeteksi, gerakkan dari 180° ke 0°
      if (currentState == MOVE_SERVO1) {
        for (int pos = 180; pos >= 0; pos--) {
          servo1.write(pos);
          delay(15);
          if (digitalRead(PIR) == HIGH) {
            currentState = STOP_SERVO1;
            break;
          }
        }
      }
      break;
      
    case STOP_SERVO1:
      // Hentikan servo1 (atur ke posisi 0°)
      servo1.write(0);
      Serial.println("Gerakan terdeteksi! Servo 1 dihentikan.");
      currentState = MOVE_SERVO2;
      break;
      
    case MOVE_SERVO2:
      Serial.println("Servo 2 bergerak dari 0° ke 180°...");
      // Gerakkan servo2 ke 180°
      for (int pos = 0; pos <= 180; pos++) {
        servo2.write(pos);
        delay(15);
      }
      delay(500); // Tahan di posisi 180°
      
      Serial.println("Servo 2 kembali dari 180° ke 0°...");
      // Gerakkan servo2 kembali ke 0°
      for (int pos = 180; pos >= 0; pos--) {
        servo2.write(pos);
        delay(15);
      }
      Serial.println("Gerakan Servo 2 selesai. Kembali ke mode IDLE.");
      currentState = IDLE;
      break;
  }
  
  lastButtonState = buttonState;
  delay(100); // Delay utama untuk stabilitas loop
}