#include <Servo.h>

// กำหนดพิน
const int trigPin = 9;
const int echoPin = 10;
const int servoPin = 11; 

Servo myServo;
int lastState = 0; 
int confirmCount = 0; 

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myServo.attach(servoPin);
  myServo.write(0); 
  
  Serial.begin(115200);
  Serial.println("System Ready...");
}

void loop() {
  // สั่งยิง Ultrasonic
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);
  
  // ตรวจจับระยะทาง (Timeout 15ms ~ 2.5 เมตร)
  long duration = pulseIn(echoPin, HIGH, 15000); 
  
  // เงื่อนไขระยะ 15 cm (Duration ประมาณ 870us)
  if (duration > 0 && duration <= 870) { 
    confirmCount++; 
  } else {
    confirmCount = 0; 
  }

  // ส่วนตัดสินใจการทำงาน
  if (confirmCount >= 3) { // ยืนยัน 3 รอบเพื่อกันสัญญาณกวน
    if (lastState == 0) {
      myServo.write(90);        
      lastState = 1; 
      Serial.println("Action: Open");
      delay(250); // รอให้ Servo หมุนเสร็จ
    }
  } 
  else if (confirmCount == 0) {
    if (lastState == 1) {
      myServo.write(0);         
      lastState = 0; 
      Serial.println("Action: Close");
      delay(250); // รอให้ Servo กลับตำแหน่งเดิม
    }
  }

  delay(10); // พักลูปเล็กน้อยเพื่อลดภาระ CPU
}