#include <ESP32Servo.h>

// กำหนดขาใช้งาน
const int sv1 = 12, sv2 = 14;
const int btn = 27;
const int led = 26;
const int potenDir = 33;
const int potenSpeed = 25;

// ประกาศออบเจกต์เซอร์โว
Servo servo1, servo2;

// ตัวแปรสถานะระบบ
bool systemOn = false;
int currentSpeed = 0;
int lastReportedSpeed = -1;

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

  // ผูกเซอร์โวกับขา
  servo1.attach(sv1);
  servo2.attach(sv2);

  // ตั้งค่าขา
  pinMode(btn, INPUT_PULLUP);
  pinMode(led, OUTPUT);
  pinMode(potenDir, INPUT);
  pinMode(potenSpeed, INPUT);

  // เริ่มต้นเซอร์โวที่มุม 0
  servo1.write(0);
  servo2.write(0);
}

void loop() {
  static bool lastButtonState = HIGH;
  bool currentButtonState = digitalRead(btn);

  // ตรวจจับการกดปุ่ม (Edge Detection)
  if (lastButtonState == HIGH && currentButtonState == LOW) {
    systemOn = !systemOn;  // toggle ระบบ
    delay(300);            // กันเด้งของปุ่ม
  }

  lastButtonState = currentButtonState;

  if (systemOn) {
    digitalWrite(led, HIGH);  // เปิด LED เพื่อแสดงว่า "ระบบทำงานอยู่"

    // อ่านค่าทิศทางจากโพเทนชิ
    int dirVal = analogRead(potenDir);
    int angle = map(dirVal, 0, 4095, 0, 180);  // แปลงค่าเป็นองศาเซอร์โว
    servo1.write(angle);
    servo2.write(angle);

    // อ่านค่าความเร็วจากโพเทนชิ
    int speedVal = analogRead(potenSpeed);
    int targetSpeed = map(speedVal, 0, 4095, 0, 200);  // แปลงค่าเป็นความเร็ว

    // อัปเดตความเร็วเมื่อเปลี่ยน
    if (currentSpeed != targetSpeed) {
      currentSpeed = targetSpeed;

      // แสดงผลความเร็วผ่าน Serial Monitor
      Serial.print("Speed: ");
      Serial.print(currentSpeed);
      Serial.println(" km/hr.");
    }

  } else {
    // ปิดระบบ
    digitalWrite(led, LOW);
    servo1.write(0);
    servo2.write(0);
    currentSpeed = 0;
  }
}