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

#define SS_PIN 5
#define RST_PIN 21
#define SERVO_PIN 13

MFRC522 rfid(SS_PIN, RST_PIN);
Servo door;

String masterCard = "";
String authorized[10];

int cardCount = 0;
bool managementMode = false;

void setup() {

  Serial.begin(115200);

  SPI.begin();
  rfid.PCD_Init();

  door.attach(SERVO_PIN);
  door.write(0);

  Serial.println("RFID Door Lock Ready");
}

void loop() {

  if (!rfid.PICC_IsNewCardPresent()) return;
  if (!rfid.PICC_ReadCardSerial()) return;

  String uid = "";

  for (byte i = 0; i < rfid.uid.size; i++) {
    uid += String(rfid.uid.uidByte[i], HEX);
  }

  uid.toUpperCase();

  Serial.print("Card UID: ");
  Serial.println(uid);

  if (masterCard == "") {

    masterCard = uid;
    Serial.println("Master card set");
    return;
  }

  if (uid == masterCard) {

    managementMode = true;
    Serial.println("Management mode");
    return;
  }

  if (managementMode) {

    int index = findCard(uid);

    if (index >= 0) {

      removeCard(index);
      Serial.println("Card removed");

    } else {

      addCard(uid);
      Serial.println("Card added");
    }

    managementMode = false;
    return;
  }

  if (findCard(uid) >= 0) {

    Serial.println("Access granted");
    openDoor();

  } else {

    Serial.println("Access denied");
  }
}

int findCard(String uid) {

  for (int i = 0; i < cardCount; i++) {

    if (authorized[i] == uid)
      return i;
  }

  return -1;
}

void addCard(String uid) {

  authorized[cardCount] = uid;
  cardCount++;
}

void removeCard(int index) {

  for (int i = index; i < cardCount - 1; i++) {

    authorized[i] = authorized[i + 1];
  }

  cardCount--;
}

void openDoor() {

  door.write(90);
  delay(3000);
  door.write(0);
}