// Two RFID Readers Writing when a Card is Detected and Removed to an LCD - Robot Compatible //

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

// LCD Pins: RS, E, D4, D5, D6, D7
LiquidCrystal lcd(12, 14, 27, 26, 25, 33);

// RFID reader pins
#define RST_Main 4
#define RST_Rear 15
#define SS_Main 5
#define SS_Rear 13

MFRC522 rfidMain(SS_Main, RST_Main);
MFRC522 rfidRear(SS_Rear, RST_Rear);

// State tracking
bool MainPresent = false;
bool RearPresent = false;
byte lastMainUID[4] = { 0 };
byte lastRearUID[4] = { 0 };
unsigned long lastSeenMain = 0;
unsigned long lastSeenRear = 0;
const unsigned long timeout = 500;  // ms to consider card removed

void setup() {
  Serial.begin(115200);
  SPI.begin(18, 19, 23);  // SCK, MISO, MOSI

  pinMode(SS_Main, OUTPUT);
  pinMode(SS_Rear, OUTPUT);
  digitalWrite(SS_Main, HIGH);
  digitalWrite(SS_Rear, HIGH);

  rfidMain.PCD_Init();
  rfidRear.PCD_Init();

  lcd.begin(16, 2);
  lcd.clear();
  lcd.setCursor(1, 0);
  lcd.print("Please Connect");
  lcd.setCursor(5, 1);
  lcd.print("Insert");

  Serial.println("Readers initialized.");
}

void loop() {
  checkReader(rfidMain, "Main Insert", MainPresent, lastSeenMain, lastMainUID);
  checkReader(rfidRear, "Rear Insert", RearPresent, lastSeenRear, lastRearUID);
  delay(200);
}

void checkReader(MFRC522 &rfid, const char *readerLabel, bool &present, unsigned long &lastSeen, byte *lastUID) {
  rfid.PCD_Init();  // Optional but helpful with multiple readers

  if (rfid.PICC_IsNewCardPresent() || rfid.PICC_ReadCardSerial()) {
    if (rfid.PICC_ReadCardSerial()) {
      if (!present || !sameUID(rfid.uid.uidByte, lastUID)) {
        Serial.print(readerLabel);
        Serial.print(" connected: ");
        printUID(rfid.uid.uidByte);

        lcd.clear();
        lcd.setCursor(2, 0);
        lcd.print(readerLabel);
        lcd.setCursor(3, 1);
        lcd.print("Connected");

        memcpy(lastUID, rfid.uid.uidByte, 4);
        present = true;
      }
    }
    lastSeen = millis();
    rfid.PICC_HaltA();
    rfid.PCD_StopCrypto1();
  }

  if (present && (millis() - lastSeen > timeout)) {
    Serial.print(readerLabel);
    Serial.print(" removed: ");
    printUID(lastUID);

    lcd.clear();
    lcd.setCursor(2, 0);
    lcd.print(readerLabel);
    lcd.setCursor(4, 1);
    lcd.print("Removed");

    present = false;
    memset(lastUID, 0, 4);
  }
}

bool sameUID(byte *a, byte *b) {
  for (byte i = 0; i < 4; i++) {
    if (a[i] != b[i]) return false;
  }
  return true;
}

void printUID(byte *uid) {
  for (byte i = 0; i < 4; i++) {
    Serial.print(uid[i] < 0x10 ? " 0" : " ");
    Serial.print(uid[i], HEX);
  }
  Serial.println();
}

void printUIDToLCD(byte *uid) {
  for (byte i = 0; i < 4; i++) {
    if (uid[i] < 0x10) lcd.print("0");
    lcd.print(uid[i], HEX);
    lcd.print(" ");
  }
}