#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
#include <Servo.h>
#include <Adafruit_Fingerprint.h>

Servo lockServo;
SoftwareSerial bluetoothSerial(10, 11);

const int passwordLength = 4;
char password[passwordLength + 1] = "1234";
char enteredPassword[passwordLength + 1];

const byte ROWS = 4;
const byte COLS = 4;
char hexaKeys[ROWS][COLS] = {
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'}
};
const byte row_pins[ROWS] = {2, 3, 4, 5};
const byte col_pins[COLS] = {A0, A1, A2, A3};
Keypad customKeypad = Keypad(makeKeymap(hexaKeys), row_pins, col_pins, ROWS, COLS);

LiquidCrystal_I2C lcd(0x27, 16, 2);
Adafruit_Fingerprint finger = Adafruit_Fingerprint(&bluetoothSerial);

void setup() {
  Serial.begin(9600);
  bluetoothSerial.begin(9600);
  lockServo.attach(9);

  if (finger.begin(57600)) {
    Serial.println("Could not find a fingerprint sensor");
    while (1);
  }

  if (finger.verifyPassword()) {
    Serial.println("Found fingerprint sensor!");
  } else {
    Serial.println("Did not find fingerprint sensor :(");
    while (1);
  }

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Smart Lock");
}

void loop() {
  int verificationResult = getFingerprint();

  if (verificationResult == FINGERPRINT_OK) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Fingerprint OK");
    unlockDoor();
  } else if (verificationResult == -1) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Password OK");
    unlockDoor();
  }

  char key = customKeypad.getKey();
  if (key) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Key entered: ");
    lcd.print(key);
    delay(1000);
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Smart Lock");
    if (key == '#') {
      enteredPassword[0] = '\0';
    } else if (strlen(enteredPassword) < passwordLength) {
      strncat(enteredPassword, &key, 1);
    }

    if (strlen(enteredPassword) == passwordLength) {
      if (strcmp(enteredPassword, password) == 0) {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Password OK");
        unlockDoor();
      } else {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Incorrect Password");
        delay(1000);
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Smart Lock");
        enteredPassword[0] = '\0';
      }
    }
  }
}

int getFingerprint() {
  char key = customKeypad.getKey();

  if (key) {
    Serial.print("Key entered: ");
    Serial.println(key);

    if (key == '#') {
      enteredPassword[0] = '\0';
      return -1;
    } else if (strlen(enteredPassword) < passwordLength) {
      strncat(enteredPassword, &key, 1);
    }

    if (strlen(enteredPassword) == passwordLength) {
      if (strcmp(enteredPassword, password) == 0) {
        Serial.println("Password correct!");
        return -1;
      } else {
        Serial.println("Incorrect password!");
        enteredPassword[0] = '\0';
      }
    }
  }

  uint8_t p = finger.getImage();
  switch (p) {
    case FINGERPRINT_OK:
      Serial.println("Image taken");
      break;
    case FINGERPRINT_NOFINGER:
      return p;
    case FINGERPRINT_PACKETRECIEVEERR:
      return p;
    case FINGERPRINT_IMAGEFAIL:
      return p;
    default:
      return p;
  }

  p = finger.image2Tz();
  switch (p) {
    case FINGERPRINT_OK:
      Serial.println("Image converted");
      break;
    case FINGERPRINT_IMAGEMESS:
      return p;
    case FINGERPRINT_PACKETRECIEVEERR:
      return p;
    case FINGERPRINT_FEATUREFAIL:
      return p;
    case FINGERPRINT_INVALIDIMAGE:
      return p;
    default:
      return p;
  }

  p = finger.createModel();
  switch (p) {
    case FINGERPRINT_OK:
      Serial.println("Model created");
      break;
    case FINGERPRINT_PACKETRECIEVEERR:
      return p;
    case FINGERPRINT_ENROLLMISMATCH:
      return p;
    case FINGERPRINT_BADLOCATION:
      return p;
    case FINGERPRINT_FLASHERR:
      return p;
    default:
      return p;
  }

  p = finger.storeTemplate();
  switch (p) {
    case FINGERPRINT_OK:
      Serial.println("Template stored");
      break;
    case FINGERPRINT_PACKETRECIEVEERR:
      return p;
    case FINGERPRINT_BADLOCATION:
      return p;
    case FINGERPRINT_FLASHERR:
      return p;
    default:
      return p;
  }

  delay(1000);
  return FINGERPRINT_OK;
}

void unlockDoor() {
  lockServo.write(90);
  delay(2000);
  lockServo.write(0);
  delay(1000);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Smart Lock");
}