// Two RFID Readers - Robot Compatible //

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

// Main Insert Reader
#define RST_PIN_Main 8
#define SS_PIN_Main 10

// Rear Insert Reader
#define RST_PIN_Rear 7
#define SS_PIN_Rear 9

MFRC522 rfidMain(SS_PIN_Main, RST_PIN_Main);
MFRC522 rfidRear(SS_PIN_Rear, RST_PIN_Rear);

// State Tracking to Detect Insert Connections
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;  // Time Delay before considering Insert Removal

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

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

  Serial.println("Please Connect Insert");
}

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

void checkReader(MFRC522 &rfid, const char* readerLabel, bool &present, unsigned long &lastSeen, byte *lastUID) {
  rfid.PCD_Init();  // Helps Reset betewen Cycles

  String content = "";

  if (rfid.PICC_IsNewCardPresent() || rfid.PICC_ReadCardSerial()) {
    if (rfid.PICC_ReadCardSerial()) {
      if (!present || !sameUID(rfid.uid.uidByte, lastUID)) {
        for (byte i = 0; i < rfid.uid.size; i++) {
          content.concat(String(rfid.uid.uidByte[i] < 0x10 ? " 0" : " "));
          content.concat(String(rfid.uid.uidByte[i], HEX));
        }

        content.toUpperCase();

        // Mowing Insert Connected
        if (content.substring(1) == "32 AB CC 05") {
          Serial.print("Mowing Insert Detected: ");
          printUID(rfid.uid.uidByte);
          memcpy(lastUID, rfid.uid.uidByte, 4);
          present = true;
        }

        // Leaf Collector Insert Connected
        if (content.substring(1) == "2C AD B1 05") {
          Serial.print("Leaf Collecting Insert Detected:");
          printUID(rfid.uid.uidByte);
          memcpy(lastUID, rfid.uid.uidByte, 4);
          present = true;
        }
      }
    }

    lastSeen = millis();

    rfid.PICC_HaltA();
    rfid.PCD_StopCrypto1();
  }

  // Card removal check
  if (present && (millis() - lastSeen > timeout)) {
    Serial.print(readerLabel);
    Serial.print(" Insert Removed: ");
    printUID(lastUID);
    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();
}