#include <SPI.h>
#include <MFRC522.h>
#include <LiquidCrystal_I2C.h>
#include <ESP32Servo.h>

// Define Pins
#define SS_PIN 5
#define RST_PIN 4
#define GREEN_LED 2
#define RED_LED 15
#define FING_BTN 13
#define SERVO_PIN 26
#define TRIG_PIN 32
#define ECHO_PIN 33

// Initialize Components
MFRC522 rfid(SS_PIN, RST_PIN);
LiquidCrystal_I2C lcd(0x27, 16, 2); 
Servo gateServo;

// Hardcoded Data
String validCardUID = "01 02 03 04"; // Update this after the first scan
bool userIsInside = false; // Local anti-passback state

void setup() {
  Serial.begin(115200);
  SPI.begin();
  rfid.PCD_Init();
  
  lcd.init();
  lcd.backlight();
  
  pinMode(GREEN_LED, OUTPUT);
  pinMode(RED_LED, OUTPUT);
  pinMode(FING_BTN, INPUT_PULLUP);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Servo Setup
  gateServo.setPeriodHertz(50);
  gateServo.attach(SERVO_PIN, 500, 2400);
  gateServo.write(0); // Gate closed position

  resetSystem();
}

void loop() {
  if (!rfid.PICC_IsNewCardPresent() || !rfid.PICC_ReadCardSerial()) {
    return;
  }

  // Read UID
  String scannedUID = "";
  for (byte i = 0; i < rfid.uid.size; i++) {
    scannedUID += String(rfid.uid.uidByte[i] < 0x10 ? " 0" : " ");
    scannedUID += String(rfid.uid.uidByte[i], HEX);
  }
  scannedUID.trim();
  scannedUID.toUpperCase();

  lcd.clear();
  
  if (scannedUID == validCardUID) {
    lcd.print("Press Finger...");
    bool fingerVerified = false;
    
    // Give the user 3 seconds to press the button (30 loops of 100ms)
    for(int i = 0; i < 30; i++) { 
      if(digitalRead(FING_BTN) == LOW) {
        fingerVerified = true;
        break; // Stop waiting as soon as they press it
      }
      delay(100);
    }
    
    if (fingerVerified) { 
      executeGateSequence();
    } else {
      denyAccess("Fingerpt Failed");
    }
  } else {
    denyAccess("Invalid Card");
  }

  resetSystem();
}

void executeGateSequence() {
  lcd.clear();
  digitalWrite(GREEN_LED, HIGH);
  
  if (userIsInside) {
    lcd.print("Exiting Campus..");
  } else {
    lcd.print("Entering Campus.");
  }

  // Open Gate
  gateServo.write(90); 
  delay(500); // Give servo time to move

  lcd.setCursor(0, 1);
  lcd.print("Walk through...");

  // Wait for physical crossing (distance drops below 50cm)
  while (getDistance() > 50) {
    delay(100);
  }
  
  // Person detected in threshold. Wait for them to clear it.
  lcd.setCursor(0, 1);
  lcd.print("Clearing gate...");
  while (getDistance() <= 50) {
    delay(100);
  }

  // Person has crossed completely. Close gate and update state.
  gateServo.write(0);
  userIsInside = !userIsInside; // Toggle anti-passback state
  
  lcd.clear();
  lcd.print("Gate Secured");
  delay(2000);
}

void denyAccess(String reason) {
  lcd.clear();
  lcd.print(reason);
  digitalWrite(RED_LED, HIGH);
  delay(3000);
}

float getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  
  long duration = pulseIn(ECHO_PIN, HIGH);
  float distanceCm = duration * 0.034 / 2;
  return distanceCm;
}

void resetSystem() {
  digitalWrite(GREEN_LED, LOW);
  digitalWrite(RED_LED, LOW);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Smart ID System");
  lcd.setCursor(0, 1);
  lcd.print("Tap Card...");
  
  rfid.PICC_HaltA();
  rfid.PCD_StopCrypto1();
}