#include 
#include 

// Ultrasonic sensor pins
const int trigPin = 11;
const int echoPin = 10;

// Servo motor pin
const int servoPin = 9;

// Buzzer pin
const int buzzerPin = 12;

// RFID module
RFID1 rfid;

// Length of RFID UID (4 bytes)
#define ID_LEN 4

// Authorized card UID
uchar userId[ID_LEN] = {0x33, 0xF8, 0xB8, 0x1A};

// Buffer for scanned UID
uchar userIdRead[ID_LEN];

// Distance settings (cm)
const int ENTRY_DISTANCE = 12;   // Detect vehicle approaching
const int CLEAR_DISTANCE = 25;   // Detect vehicle leaving
const int CLEAR_COUNT    = 7;    // About 1 second (7 × 150 ms)

// Minimum time the gate stays open (ms)
const unsigned long MIN_OPEN_TIME = 2000;

// System states
enum GateMode {
  WAIT_ENTRY,   // Waiting for vehicle to enter (ultrasonic)
  WAIT_EXIT     // Waiting for vehicle to exit (RFID)
};

GateMode mode = WAIT_ENTRY;

// Servo object
Servo myServo;

// Play a simple beep sound
void beepOnce(int freq, int dur) {
  tone(buzzerPin, freq, dur);
  delay(dur * 1.2);
  noTone(buzzerPin);
}

// Beep sound for unauthorized card
void beepError() {
  for (int i = 0; i < 2; i++) {
    tone(buzzerPin, 800);
    delay(120);
    noTone(buzzerPin);
    delay(80);
  }
}

// Open the gate smoothly
void openGate() {
  for (int pos = 0; pos <= 90; pos++) {
    myServo.write(pos);
    delay(10);
  }
}

// Close the gate smoothly
void closeGate() {
  for (int pos = 90; pos >= 0; pos--) {
    myServo.write(pos);
    delay(10);
  }
}

// Measure distance using ultrasonic sensor (cm)
float getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 25000);
  if (duration == 0) return 999;

  return duration * 0.034 / 2;
}

// Read RFID card UID
bool getId() {
  uchar status, str[MAX_LEN];
  status = rfid.anticoll(str);

  if (status == MI_OK) {
    for (int i = 0; i < ID_LEN; i++) {
      userIdRead[i] = str[i];
    }
    rfid.halt();
    return true;
  }
  return false;
}

// Compare scanned UID with authorized UID
bool idVerify() {
  for (int i = 0; i < ID_LEN; i++) {
    if (userIdRead[i] != userId[i]) return false;
  }
  return true;
}

// Clear UID buffer
void clearBuffer() {
  for (int i = 0; i < ID_LEN; i++) {
    userIdRead[i] = 0;
  }
}

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

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(buzzerPin, OUTPUT);

  myServo.attach(servoPin);
  myServo.write(0);   // Gate starts closed

  // RFID initialization (pin order must match wiring)
  rfid.begin(7, 5, 4, 3, 6, 2);
  rfid.init();

  Serial.println("System Ready - WAIT_ENTRY");
}

void loop() {

  // Entry mode: ultrasonic detection
  if (mode == WAIT_ENTRY) {

    float distance = getDistance();
    Serial.print("Distance: ");
    Serial.println(distance);

    if (distance < ENTRY_DISTANCE) {

      beepOnce(1000, 200);
      openGate();

      // Wait until the vehicle completely leaves
      int clearCounter = 0;
      while (clearCounter < CLEAR_COUNT) {
        float d = getDistance();

        if (d > CLEAR_DISTANCE)
          clearCounter++;
        else
          clearCounter = 0;

        delay(150);
      }

      closeGate();
      mode = WAIT_EXIT;
      delay(500);
    }
  }

  // Exit mode: RFID verification
  else if (mode == WAIT_EXIT) {

    uchar status, str[MAX_LEN];
    status = rfid.request(PICC_REQIDL, str);

    if (status == MI_OK) {

      clearBuffer();

      if (getId() && idVerify()) {

        beepOnce(1500, 150);
        openGate();

        // Keep gate open for a minimum time
        unsigned long openTime = millis();
        while (millis() - openTime < MIN_OPEN_TIME) {
          delay(50);
        }

        // Wait until the vehicle leaves
        int clearCounter = 0;
        while (clearCounter < CLEAR_COUNT) {
          float d = getDistance();

          if (d > CLEAR_DISTANCE)
            clearCounter++;
          else
            clearCounter = 0;

          delay(150);
        }

        closeGate();
        mode = WAIT_ENTRY;
        delay(500);
      }
      else {
        beepError();
      }
    }
  }

  delay(100);
}