#include <Servo.h>
#include <Adafruit_Fingerprint.h>
#include <SoftwareSerial.h> // Use this library for SoftwareSerial on modern Arduino

int getFingerprintIDez();
int ledaccess = 7;
int leddeny = 8;
int servoPin = 9;

Servo doorLock;
SoftwareSerial mySerial(2, 3); // Define RX and TX pins for fingerprint sensor
Adafruit_Fingerprint finger = Adafruit_Fingerprint(&mySerial);

void setup() {
  doorLock.attach(servoPin);  // Attach the servo motor
  pinMode(ledaccess, OUTPUT);
  pinMode(leddeny, OUTPUT);
  Serial.begin(9600);
  Serial.println("fingertest");
  
  finger.begin(57600); // Initialize the fingerprint sensor
  
  if (finger.verifyPassword()) {
    Serial.println("Found fingerprint sensor!");
  } else {
    Serial.println("Did not find fingerprint sensor :");
    while (1); // Halt the program if the sensor is not found
  }
  
  Serial.println("Waiting for valid finger...");
}

void loop() {
  int ID = getFingerprintIDez();  // Fetch fingerprint ID

  digitalWrite(ledaccess, HIGH);
  digitalWrite(leddeny, HIGH);
  doorLock.write(0);  // Initial position: Lock the door
  
  if (ID >= 0) {
    // Access granted
    digitalWrite(ledaccess, HIGH);
    digitalWrite(leddeny, LOW);
    doorLock.write(180);  // Unlock the door
  } else if (ID == -3) {
    // Access denied
    digitalWrite(ledaccess, LOW);
    digitalWrite(leddeny, HIGH);
  }
  
  delay(5000);  // Wait 5 seconds before next loop
}

uint8_t getFingerprintID() {
  uint8_t p = finger.getImage();
  switch (p) {
    case FINGERPRINT_OK:
      Serial.println("Image taken");
      break;
    case FINGERPRINT_NOFINGER:
      Serial.println("No finger detected");
      return p;
    case FINGERPRINT_PACKETRECIEVEERR:
      Serial.println("Communication error");
      return p;
    case FINGERPRINT_IMAGEFAIL:
      Serial.println("Imaging error");
      return p;
    default:
      Serial.println("Unknown error");
      return p;
  }

  p = finger.image2Tz();
  switch (p) {
    case FINGERPRINT_OK:
      Serial.println("Image converted");
      break;
    case FINGERPRINT_IMAGEMESS:
      Serial.println("Image too messy");
      return p;
    case FINGERPRINT_PACKETRECIEVEERR:
      Serial.println("Communication error");
      return p;
    case FINGERPRINT_FEATUREFAIL:
    case FINGERPRINT_INVALIDIMAGE:
      Serial.println("Could not find fingerprint features");
      return p;
    default:
      Serial.println("Unknown error");
      return p;
  }

  p = finger.fingerFastSearch();
  if (p == FINGERPRINT_OK) {
    Serial.println("Found a print match!");
  } else if (p == FINGERPRINT_PACKETRECIEVEERR) {
    return p;
  } else if (p == FINGERPRINT_NOTFOUND) {
    Serial.println("Did not find a match");
    return p;
  } else {
    Serial.println("Unknown error");
    return p;
  }

  Serial.print("Found ID #"); 
  Serial.print(finger.fingerID);
  Serial.print(" with confidence of "); 
  Serial.println(finger.confidence);
  return finger.fingerID;
}

int getFingerprintIDez() {
  int p = finger.getImage();
  if (p != FINGERPRINT_OK) return -1;

  p = finger.image2Tz();
  if (p != FINGERPRINT_OK) return -2;
    
  p = finger.fingerFastSearch();
  if (p != FINGERPRINT_OK) {
    Serial.println("No match found");
    return -3;
  }
  
  Serial.print("Found ID #"); 
  Serial.print(finger.fingerID);
  Serial.print(" with confidence of "); 
  Serial.println(finger.confidence);
  return finger.fingerID;
}