#include <Servo.h>

const byte TRIG_PIN = 10, ECHO_PIN = 11;
const byte SERVO_PIN = 9, LED_PIN = 7;
const int CLOSED_ANGLE = 0, OPEN_ANGLE = 90;
const int DETECT_CM = 20;
const unsigned long CLEAR_DELAY_MS = 3000;
Servo gate;
bool isOpen = false;
unsigned long clearSince = 0;

long distanceCm() {
  digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  unsigned long us = pulseIn(ECHO_PIN, HIGH, 25000UL);
  return us == 0 ? -1 : us / 58;
}

void setGate(bool open) {
  gate.write(open ? OPEN_ANGLE : CLOSED_ANGLE);
  digitalWrite(LED_PIN, open ? HIGH : LOW);
  isOpen = open;
}

void setup() {
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  gate.attach(SERVO_PIN);
  setGate(false);
}

void loop() {
  long cm = distanceCm();
  if (cm < 0) { delay(100); return; } // No echo: hold state

  if (cm >= 2 && cm <= DETECT_CM) {
    clearSince = 0;
    if (!isOpen) setGate(true);
  } else if (isOpen) {
    if (clearSince == 0) clearSince = millis();
    if (millis() – clearSince >= CLEAR_DELAY_MS) {
      setGate(false);
      clearSince = 0;
    }
  }
  delay(80);
}