#include <WiFi.h>
#include "time.h"
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <ESP32Servo.h>  // Changed to ESP32-specific servo library

// Task Handles
TaskHandle_t SecurityTaskHandle;
TaskHandle_t LoggingTaskHandle;
TaskHandle_t DisplayTaskHandle;

// LCD Configuration
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Servo Configuration
Servo doorServo;
const int SERVO_PIN = 13;  // GPIO13 for servo

// Keypad Configuration
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
byte rowPins[ROWS] = {32, 33, 25, 26};
byte colPins[COLS] = {27, 14, 12, 15};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

// Pin Definitions
const int PIN_PIR = 23;          // PIR sensor
const int PIN_BUZZER = 19;       // Buzzer
const int PIN_LED_GREEN = 2;     // Green LED
const int PIN_LED_RED = 4;       // Red LED
const int PIN_LED_PEOPLE = 16;   // LED for people counter display
const int PIN_RFID_BTN = 5;      // RFID simulation button
const int PIN_COUNT_BTN = 17;    // Button to show people count

// System States
bool waitingForPIN = false;
bool alarmActive = false;
volatile int peopleCount = 0;
String currentMessage = "";

// Access Control
const String VALID_RFID_CARDS[] = {
    "1234", "5678", "9012"
};
const int NUM_CARDS = 3;
String entered_pin = "";
int failed_attempts = 0;
const int MAX_ATTEMPTS = 3;

void setup() {
    Serial.begin(115200);
    
    // Initialize LCD
    lcd.init();
    lcd.backlight();
    
    // Initialize ESP32 Servo
    ESP32PWM::allocateTimer(0);  // Allocate timer 0
    doorServo.setPeriodHertz(50);  // Standard 50Hz servo
    doorServo.attach(SERVO_PIN, 500, 2400);  // Attach with range of 500-2400 microseconds
    doorServo.write(0);  // Initial position (locked)
    
    // Configure pins
    pinMode(PIN_PIR, INPUT);
    pinMode(PIN_BUZZER, OUTPUT);
    pinMode(PIN_LED_GREEN, OUTPUT);
    pinMode(PIN_LED_RED, OUTPUT);
    pinMode(PIN_LED_PEOPLE, OUTPUT);
    pinMode(PIN_RFID_BTN, INPUT_PULLUP);
    pinMode(PIN_COUNT_BTN, INPUT_PULLUP);
    
    // Initialize LEDs
    digitalWrite(PIN_LED_GREEN, LOW);
    digitalWrite(PIN_LED_RED, LOW);
    digitalWrite(PIN_LED_PEOPLE, LOW);
    
    welcomeMessage();
    
    // Create tasks
    xTaskCreatePinnedToCore(SecurityTask, "SecurityTask", 10000, NULL, 1, &SecurityTaskHandle, 0);
    xTaskCreatePinnedToCore(DisplayTask, "DisplayTask", 10000, NULL, 1, &DisplayTaskHandle, 1);
}

void welcomeMessage() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Access Control");
    lcd.setCursor(0, 1);
    lcd.print("Ready...");
}

void SecurityTask(void * parameter) {
    for(;;) {
        checkRFIDButton();
        if(waitingForPIN) {
            checkKeypadInput();
        }
        checkPeopleCountButton();
        checkPresence();
        processAlarms();
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

void DisplayTask(void * parameter) {
    for(;;) {
        updateDisplay();
        vTaskDelay(pdMS_TO_TICKS(500));
    }
}

void checkRFIDButton() {
    static bool lastButtonState = HIGH;
    bool currentButtonState = digitalRead(PIN_RFID_BTN);
    
    if(lastButtonState == HIGH && currentButtonState == LOW) {
        waitingForPIN = true;
        entered_pin = "";
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Card PIN:");
    }
    
    lastButtonState = currentButtonState;
}

void checkKeypadInput() {
    char key = keypad.getKey();
    
    if(key) {
        if(key == '*') {
            entered_pin = "";
        }
        else if(key == '#') {
            processAccess();
            waitingForPIN = false;
        }
        else if(entered_pin.length() < 4) {
            entered_pin += key;
        }
    }
}

void processAccess() {
    bool authorized = false;
    
    for(int i = 0; i < NUM_CARDS; i++) {
        if(entered_pin == VALID_RFID_CARDS[i]) {
            authorized = true;
            break;
        }
    }
    
    if(authorized) {
        accessGranted();
    } else {
        accessDenied();
    }
    
    entered_pin = "";
}

void accessGranted() {
    currentMessage = "Access Granted";
    digitalWrite(PIN_LED_GREEN, HIGH);
    digitalWrite(PIN_LED_RED, LOW);
    
    // Unlock door
    doorServo.write(90);
    
    // Success beep
    digitalWrite(PIN_BUZZER, HIGH);
    delay(100);
    digitalWrite(PIN_BUZZER, LOW);
    
    delay(2000);
    
    // Relock door
    doorServo.write(0);
    digitalWrite(PIN_LED_GREEN, LOW);
}

void accessDenied() {
    currentMessage = "Access Denied";
    digitalWrite(PIN_LED_RED, HIGH);
    failed_attempts++;
    
    // Error beeps
    for(int i = 0; i < 2; i++) {
        digitalWrite(PIN_BUZZER, HIGH);
        delay(200);
        digitalWrite(PIN_BUZZER, LOW);
        delay(200);
    }
    
    if(failed_attempts >= MAX_ATTEMPTS) {
        triggerAlarm();
    }
    
    delay(2000);
    digitalWrite(PIN_LED_RED, LOW);
}

void checkPresence() {
    if(digitalRead(PIN_PIR) == HIGH) {
        peopleCount++;
        delay(1000);  // Simple debounce
    }
}

void checkPeopleCountButton() {
    if(digitalRead(PIN_COUNT_BTN) == LOW) {
        displayPeopleCount();
    }
}

void displayPeopleCount() {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("People Count:");
    lcd.setCursor(0, 1);
    lcd.print(peopleCount);
    
    // Blink people LED according to count
    for(int i = 0; i < peopleCount; i++) {
        digitalWrite(PIN_LED_PEOPLE, HIGH);
        delay(200);
        digitalWrite(PIN_LED_PEOPLE, LOW);
        delay(200);
    }
    
    delay(2000);
}

void triggerAlarm() {
    alarmActive = true;
    currentMessage = "ALARM: Too many attempts";
    
    // Alarm sequence
    for(int i = 0; i < 5; i++) {
        digitalWrite(PIN_LED_RED, HIGH);
        digitalWrite(PIN_BUZZER, HIGH);
        delay(500);
        digitalWrite(PIN_LED_RED, LOW);
        digitalWrite(PIN_BUZZER, LOW);
        delay(500);
    }
    
    failed_attempts = 0;
    alarmActive = false;
}

void processAlarms() {
    if(alarmActive) {
        digitalWrite(PIN_LED_RED, (millis() / 500) % 2);
        digitalWrite(PIN_BUZZER, (millis() / 500) % 2);
    }
}

void updateDisplay() {
    if(!waitingForPIN && currentMessage == "") {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Scan Card");
        lcd.setCursor(0, 1);
        lcd.print("to Access");
    }
    else if(waitingForPIN) {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter PIN:");
        lcd.setCursor(0, 1);
        lcd.print(entered_pin);
    }
    else if(currentMessage != "") {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(currentMessage);
        currentMessage = "";
    }
}

void loop() {
    // Empty since we're using FreeRTOS tasks
    delay(1000);
}