#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <SPI.h>
#include <MFRC522.h>
#include <Keypad.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define RST_PIN 5
#define SS_PIN 53
MFRC522 rfid(SS_PIN, RST_PIN);

#define TRIG_PIN 7
#define ECHO_PIN 6
#define RED_LED 8
#define BLUE_LED 9
#define IGNITION_BUTTON 2

bool ignitionOn = false;
bool pinVerified = false;
const String correctPIN = "1111";
String enteredPIN = "";

// Keypad setup
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] = {30, 31, 32, 33};
byte colPins[COLS] = {34, 35, 36, 37};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

byte allowedUID[] = { 0xBD, 0x31, 0x15, 0x2B };

bool compareUID(byte *uid, byte *allowedUID, byte size) {
  for (byte i = 0; i < size; i++) {
    if (uid[i] != allowedUID[i]) return false;
  }
  return true;
}

void setup() {
  Serial.begin(9600);
  SPI.begin();
  rfid.PCD_Init();

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(RED_LED, OUTPUT);
  pinMode(BLUE_LED, OUTPUT);
  pinMode(IGNITION_BUTTON, INPUT_PULLUP);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED failed"));
    while (true);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("SAVA-X Initializing...");
  display.display();
  delay(1500);
  display.clearDisplay();
  display.setCursor(0, 0);
  display.println("Enter PIN:");
  display.display();
}

void loop() {
  // Keypad PIN Entry
  if (!pinVerified) {
    char key = keypad.getKey();
    if (key) {
      if (key == '#') {
        if (enteredPIN == correctPIN) {
          pinVerified = true;
          display.clearDisplay();
          display.setCursor(0, 0);
          display.println("PIN Accepted");
          display.display();
          delay(1000);
        } else {
          display.clearDisplay();
          display.setCursor(0, 0);
          display.println("Incorrect PIN");
          display.display();
          delay(1000);
          enteredPIN = "";
          display.clearDisplay();
          display.setCursor(0, 0);
          display.println("Enter PIN:");
          display.display();
        }
      } else if (key == '*') {
        enteredPIN = "";
        display.clearDisplay();
        display.setCursor(0, 0);
        display.println("PIN Cleared");
        display.display();
        delay(1000);
        display.clearDisplay();
        display.setCursor(0, 0);
        display.println("Enter PIN:");
        display.display();
      } else {
enteredPIN += key;
display.clearDisplay();
display.setCursor(0, 0);
display.println("Enter PIN:");
display.setCursor(0, 10);
for (int i = 0; i < enteredPIN.length(); i++) {
  display.print("*");
}
display.display();
      }
    }
    return;
  }

  // Check ignition toggle
  static bool lastButtonState = HIGH;
  bool currentButtonState = digitalRead(IGNITION_BUTTON);
  if (lastButtonState == HIGH && currentButtonState == LOW) {
    ignitionOn = !ignitionOn;
    Serial.println(ignitionOn ? "Ignition ON" : "Ignition OFF");
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println(ignitionOn ? "Vehicle ON" : "Vehicle OFF");
    display.display();
    delay(300);
  }
  lastButtonState = currentButtonState;

  // LED status
  digitalWrite(BLUE_LED, ignitionOn);
  digitalWrite(RED_LED, !ignitionOn);

  // If ignition is ON, allow RFID and ACC
  if (ignitionOn) {
    if (rfid.PICC_IsNewCardPresent() && rfid.PICC_ReadCardSerial()) {
      byte *uid = rfid.uid.uidByte;
      byte uidSize = rfid.uid.size;

      display.clearDisplay();
      display.setCursor(0, 0);
      display.println("RFID Scanned");

      if (uidSize == sizeof(allowedUID) && compareUID(uid, allowedUID, uidSize)) {
        display.println("Access Granted");
      } else {
        display.println("Access Denied");
        for (int i = 0; i < 3; i++) {
          digitalWrite(BLUE_LED, HIGH);
          digitalWrite(RED_LED, LOW);
          delay(200);
          digitalWrite(BLUE_LED, LOW);
          digitalWrite(RED_LED, HIGH);
          delay(200);
        }
      }
      display.display();
      delay(2000);
      rfid.PICC_HaltA();
      rfid.PCD_StopCrypto1();
    }

    // Adaptive Cruise Control
    long duration, distance;
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH);
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    duration = pulseIn(ECHO_PIN, HIGH);
    distance = duration * 0.034 / 2;

    display.setCursor(0, 40);
    display.print("Distance: ");
    display.print(distance);
    display.println(" cm");
    display.display();
  }
  delay(100);
}