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

// --- Shared SPI Pins ---
#define SPI_SCK   18
#define SPI_MOSI  23
#define SPI_MISO  19

// --- RFID Readers per Lane ---
// Lane 1
#define RFID1_SS   4
#define RFID1_RST  22
MFRC522 mfrc522_1(RFID1_SS, RFID1_RST);

// Lane 2
#define RFID2_SS   16
#define RFID2_RST  17
MFRC522 mfrc522_2(RFID2_SS, RFID2_RST);

// Lane 3
#define RFID3_SS   2
#define RFID3_RST  21
MFRC522 mfrc522_3(RFID3_SS, RFID3_RST);

// Array of pointers for easy iteration
MFRC522* rfidReaders[3] = { &mfrc522_1, &mfrc522_2, &mfrc522_3 };
const int laneForReader[3] = {1, 2, 3}; // lane number for each reader

// --- Traffic Light Pins (unchanged) ---
#define LANE1_RED    32
#define LANE1_YELLOW 33
#define LANE1_GREEN  25
#define LANE2_RED    26
#define LANE2_YELLOW 27
#define LANE2_GREEN  14
#define LANE3_RED    12
#define LANE3_YELLOW 15
#define LANE3_GREEN  13

// --- Push Button ---
#define BUTTON_PIN   34

const int totalLanes = 3;
int activeLane = 1; // currently green lane

// Registered Tag UID (change to your tag's UID)
//byte registeredUID[4] = {55:66:77:88, 01:02:03:04, AA:BB:CC:DD, 11:22:33:44};
// List of authorized UIDs (each is 4 bytes)
byte authorizedUIDs[][4] = {
  {0x55, 0x66, 0x77, 0x88},   // Tag 1
  {0x01, 0x02, 0x03, 0x04},   // Tag 2
  {0xAA, 0xBB, 0xCC, 0xDD},   // Tag 3
  {0x11, 0x22, 0x33, 0x44}    // Tag 4
};
int numAuthorizedTags = 4;     // How many tags are registered


// Debounce for button
unsigned long lastDebounceTime = 0;
const unsigned long debounceDelay = 50;

// --- Function Prototypes ---
void setLights(int lane, bool red, bool yellow, bool green);
void switchToLane(int newActiveLane);
void performRedToGreenTransition(int targetLane);
void performGreenToRedTransition(int currentLane);
bool isTagRegistered(MFRC522 &reader);
void printUIDToSerial(MFRC522 &reader);

// --- Setup ---
void setup() {
  Serial.begin(115200);

  // Init LED pins
  pinMode(LANE1_RED, OUTPUT);
  pinMode(LANE1_YELLOW, OUTPUT);
  pinMode(LANE1_GREEN, OUTPUT);
  pinMode(LANE2_RED, OUTPUT);
  pinMode(LANE2_YELLOW, OUTPUT);
  pinMode(LANE2_GREEN, OUTPUT);
  pinMode(LANE3_RED, OUTPUT);
  pinMode(LANE3_YELLOW, OUTPUT);
  pinMode(LANE3_GREEN, OUTPUT);

  // Button
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  // SPI bus (shared)
  SPI.begin(SPI_SCK, SPI_MISO, SPI_MOSI, -1); // -1 for default SS not used here

  // Initialize all three RFID readers
  for (int i = 0; i < 3; i++) {
    rfidReaders[i]->PCD_Init();
    delay(10);
  }
  Serial.println("3 RFID Readers initialized.");

  // Start with all red
  setLights(1, true, false, false);
  setLights(2, true, false, false);
  setLights(3, true, false, false);
  activeLane = 1;
  switchToLane(1); // give green to lane 1

  randomSeed(analogRead(0));
}

// --- Main Loop ---
void loop() {
  // 1. Button: random lane change
  int buttonState = digitalRead(BUTTON_PIN);
  if (buttonState == LOW) {
    if ((millis() - lastDebounceTime) > debounceDelay) {
      int newLane = random(1, totalLanes + 1);
      if (newLane != activeLane) {
        Serial.print("Manual Override: Switching to Lane ");
        Serial.println(newLane);
        switchToLane(newLane);
      } else {
        Serial.println("Same lane chosen, no change.");
      }
      lastDebounceTime = millis();
    }
  }

  // 2. Check each RFID reader for a registered tag
  for (int i = 0; i < 3; i++) {
    MFRC522* reader = rfidReaders[i];
    if (reader->PICC_IsNewCardPresent() && reader->PICC_ReadCardSerial()) {
      Serial.print("RFID detected on Lane ");
      Serial.print(laneForReader[i]);
      Serial.print(" UID: ");
      printUIDToSerial(*reader);

      if (isTagRegistered(*reader)) {
        int detectedLane = laneForReader[i];
        Serial.print("Registered tag! Granting priority to Lane ");
        Serial.println(detectedLane);
        // Switch to the lane where tag was detected
        if (detectedLane != activeLane) {
          switchToLane(detectedLane);
        } else {
          // If already green, optionally restart green timer or just keep it
          Serial.println("Lane already has green. Priority acknowledged.");
        }
      } else {
        Serial.println("Unregistered tag ignored.");
      }
      reader->PICC_HaltA();
      reader->PCD_StopCrypto1();
      delay(100); // avoid re-reading same tag
    }
  }

  delay(50);
}

// --- Traffic Light Functions (same as before, but keep for completeness)---
void setLights(int lane, bool red, bool yellow, bool green) {
  int r, y, g;
  switch(lane) {
    case 1: r = LANE1_RED; y = LANE1_YELLOW; g = LANE1_GREEN; break;
    case 2: r = LANE2_RED; y = LANE2_YELLOW; g = LANE2_GREEN; break;
    case 3: r = LANE3_RED; y = LANE3_YELLOW; g = LANE3_GREEN; break;
    default: return;
  }
  digitalWrite(r, red ? HIGH : LOW);
  digitalWrite(y, yellow ? HIGH : LOW);
  digitalWrite(g, green ? HIGH : LOW);
}

void performRedToGreenTransition(int targetLane) {
  Serial.print("Lane "); Serial.print(targetLane); Serial.println(": Red -> Green");
  setLights(targetLane, true, false, false);
  delay(1000); // red for 1 sec (safety)
  setLights(targetLane, false, false, true);
  Serial.print("Lane "); Serial.print(targetLane); Serial.println(" is now GREEN.");
}

void performGreenToRedTransition(int currentLane) {
  Serial.print("Lane "); Serial.print(currentLane); Serial.println(": Green -> Yellow -> Red");
  setLights(currentLane, false, true, false);
  delay(3000); // yellow for 3 seconds
  setLights(currentLane, true, false, false);
  Serial.print("Lane "); Serial.print(currentLane); Serial.println(" is now RED.");
}

void switchToLane(int newActiveLane) {
  if (newActiveLane < 1 || newActiveLane > totalLanes) return;
  // First, turn off current green (if any)
  if (activeLane >= 1 && activeLane <= totalLanes) {
    performGreenToRedTransition(activeLane);
  }
  delay(100);
  // Then turn on new lane green
  performRedToGreenTransition(newActiveLane);
  activeLane = newActiveLane;
  Serial.print("Now active lane: ");
  Serial.println(activeLane);
}
/*
// --- RFID Helper ---
bool isTagRegistered(MFRC522 &reader) {
  for (byte i = 0; i < 4; i++) {
    if (reader.uid.uidByte[i] != registeredUID[i]) return false;
  }
  return true;
}
*/
bool isTagRegistered(MFRC522 &reader) {
  for (int tagIndex = 0; tagIndex < numAuthorizedTags; tagIndex++) {
    bool match = true;
    for (byte i = 0; i < 4; i++) {
      if (reader.uid.uidByte[i] != authorizedUIDs[tagIndex][i]) {
        match = false;
        break;
      }
    }
    if (match) return true;
  }
  return false;
}

void printUIDToSerial(MFRC522 &reader) {
  for (byte i = 0; i < reader.uid.size; i++) {
    if (reader.uid.uidByte[i] < 0x10) Serial.print("0");
    Serial.print(reader.uid.uidByte[i], HEX);
    if (i < reader.uid.size - 1) Serial.print(":");
  }
  Serial.println();
}