#include <ESP32Servo.h>

// --- Pinout Definitions ---
// Ultrasonic Sensor
const int trigPin = 33;   // GPIO33 สำหรับ Trig
const int echoPin = 25;   // GPIO25 สำหรับ Echo

// LEDs
const int redLedPin = 26;     // GPIO26 สำหรับ LED แดง
const int yellowLedPin = 27;  // GPIO27 สำหรับ LED เหลือง

// Servos
const int servoLeftPin = 13;   // Servo ซ้าย
const int servoRightPin = 12;  // Servo ขวา

// Button
const int btnPin = 14;      // ปุ่มกด (GPIO34 = input only ใช้ได้)

int degree = 90; // ตำแหน่งเริ่มต้น (เตรียมพร้อม)

// --- Servo Objects ---
Servo servoLeft;
Servo servoRight;

// --- Main Program ---
void setup() {
  Serial.begin(115200);

  // --- Initialize Pins ---
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(redLedPin, OUTPUT);
  pinMode(yellowLedPin, OUTPUT);
  pinMode(btnPin, INPUT_PULLUP);

  // --- Attach Servos ---
  servoLeft.attach(servoLeftPin);
  servoRight.attach(servoRightPin);

  // --- Initial Ready State ---
  Serial.println("System Ready. Initializing to Ready State.");
  servoLeft.write(degree);
  servoRight.write(180 - degree);
  digitalWrite(redLedPin, LOW);
  digitalWrite(yellowLedPin, LOW);
}

void loop() {
  // --- 1. Ultrasonic Distance Measurement ---
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  float cm = duration / 58.2;

  Serial.print("Distance: ");
  Serial.print(cm);
  Serial.println(" cm");

  // --- 2. Control Logic ---
  if (cm <= 100 && cm > 30) {
    // --- เปิดหนีบ \._./ และเปิดไฟเหลือง ---
    Serial.println("Object detected! Opening claw.");
    servoLeft.write(degree - 45);         // กางออกเพิ่ม
    servoRight.write(180 - (degree - 45));
    digitalWrite(yellowLedPin, HIGH);
    digitalWrite(redLedPin, LOW);
    delay(1000); // ✅ ให้ไฟเหลืองติดค้าง 1 วินาที
    digitalWrite(yellowLedPin, LOW);

  } else if (cm <= 30) {
    // --- หุบหนีบ /._.\ และเปิดไฟแดง ---
    Serial.println("Gripping object!");
    servoLeft.write(degree + 45);         // หุบเข้า
    servoRight.write(180 - (degree + 45));
    digitalWrite(redLedPin, HIGH);        // ✅ ไฟแดงติดค้าง
    digitalWrite(yellowLedPin, LOW);
    delay(500); // หนีบในเวลา 500ms

    // ตรวจสอบการกดปุ่ม
    if (digitalRead(btnPin) == LOW) {
      Serial.println("Button pressed! Releasing for 3 seconds.");
      // --- คลายออก (._.) ---
      servoLeft.write(45);   // คลายออก
      servoRight.write(135);
      digitalWrite(redLedPin, LOW);   // ✅ ปิดไฟแดงตอนปล่อย
      digitalWrite(yellowLedPin, LOW);
      delay(3000);

      // --- กลับสู่สถานะเตรียมพร้อม ---
      servoLeft.write(degree);
      servoRight.write(180 - degree);
      digitalWrite(redLedPin, LOW);
      digitalWrite(yellowLedPin, LOW);
    }

  } else {
    // --- Ready State |._.|
    Serial.println("Ready State.");
    servoLeft.write(degree);
    servoRight.write(180 - degree);
    digitalWrite(redLedPin, LOW);
    digitalWrite(yellowLedPin, LOW);
  }

  delay(100);
}