#include "FPS_GT511C3.h"
#include "SoftwareSerial.h"
#include <LiquidCrystal.h>
#include <Keypad.h>
#include <Servo.h> 

FPS_GT511C3 fps(A3, A4);
Servo locker;
LiquidCrystal lcd(13, 12, 11, 9, A1, A0);
char secretCode[4] = {'1', '2', '3', '4'};
char input[4];
char key;
int position = 0;
int wrong = 0;
int i = 0;
int j = 0;
int pos = 0;
int a=0;
int b=0;
const byte rows = 4;
const byte cols = 4;
boolean c=false;

char keys[rows][cols] =
{
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte rowPins[rows] = {8, 7, 6, 5};
byte colPins[cols] = {4, 3, 2, 1};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, rows, cols);

void setup() 
{
  Serial.begin(9600);
  lcd.begin(16, 2);
  fps.Open();
  fps.SetLED(true);
  locker.attach(10);
  locker.write(90);
  pinMode(10,INPUT);
}

void loop() 
{
  while(a==0)
  {
    input_key();            // Password Input Mode
    password();             // Confirm Password
    password_check();       // Display current state
  }
  while(b==0)
  {
    fingerscan();           // Fingerprint Input Mode, Check Fingerprint
  }
  int sum = a + b;          // State Flag
  if(sum==2)                // Correct Password & registered Fingerprint
  {
    lcd.clear();
    lcd.setCursor(0, 0);
    pinMode(10,OUTPUT);
    
    locker.write(180);      // OPENING
    lcd.print("perfect complete");
    delay(2000);
    pinMode(10,INPUT);
    key = keypad.waitForKey();
    if(key == '*')
    {  
      a=0;                  // Clear Flag
      b=0;                  // Clear Flag
      lcd.clear();
      lcd.setCursor(0,0);
      pinMode(10,OUTPUT);
      locker.write(90);     // Closing
      lcd.print("    Lock ON");
      delay(2000);
      pinMode(10,INPUT);  
    }
  }
}

void input_key()
{
  lcd.clear();                      
  lcd.setCursor(0, 0);
  lcd.print("    Password");
  for (i = 0; i < 4; i++)
  {
    key = keypad.waitForKey();
    input[i] = key;
    lcd.setCursor(i + 6, 1);
    lcd.print("*");
    Serial.print(input[i], DEC);
    Serial.print("*");
    if (i == 3)
    {
      Serial.println("");
    }
  }
}

void password()
{
  wrong = 0;
  for (j = 0; j < 4; j++)
  {
    if (secretCode[j] == input[j])
    {
    }
    else if (secretCode[j] != input[j])
    {
      wrong += 1;
    }
  }
}

void password_check()
{
  if (wrong == 0)
  {
    Serial.println("correct");
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("      PASS");
    delay(2000);
    a=1;
    lcd.clear();
  }
  else if (wrong != 0)
  {
    Serial.println("wrong");
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("     wrong");
    delay(2000);
    a=0;
    lcd.clear();
    wrong = 0;
  }
}

void fingerscan()
{   
  pinMode(10, INPUT); //test
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("press finger");

  while(!fps.IsPressFinger()){}
  if (fps.IsPressFinger())
  {
    fps.CaptureFinger(false);
    int id = fps.Identify1_N();
    if (id < 200)
    {
      if (id == 5 || id==6)
      {
        lcd.setCursor(0, 0);
        lcd.clear();
        lcd.print("lock off");;
        delay(1000);
        Serial.println(id);
        b=1;
      }
    }
    else
    {
      lcd.clear();
      lcd.setCursor(0, 0);  
      lcd.print("Finger not found");
      lcd.setCursor(0, 1);
      lcd.print("Press again");
      b=0;
      delay(1000);
    }
  }
  else
  {
  }
  
  delay(1000);
}