#include <LiquidCrystal.h>
#include<Servo.h>
#include <Adafruit_Fingerprint.h>
const int rs = 12, en = 11, d4 = 5, d5 = 4, d6 = 3, d7 = 2;
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
// On Leonardo/Micro or others with hardware serial, use those! #0 is green wire, #1 is white
// uncomment this line:
// #define mySerial Serial1
Servo lock;
// For UNO and others without hardware serial, we must use software serial...
// pin #2 is IN from sensor (GREEN wire)
// pin #3 is OUT from arduino  (WHITE wire)
// comment these two lines if using hardware serial
SoftwareSerial mySerial(6, 7);
int resetState = 0;

Adafruit_Fingerprint finger = Adafruit_Fingerprint(&mySerial);

int firstsuccess = 0;

void setup()
{
  //lock.attach(9);
  //lock.write(0);
  pinMode(13, INPUT);
  //digitalWrite(8, LOW);
  Serial.begin(9600);
  lcd.begin(16, 2);
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Scan 1st Finger");
  while (!Serial);  // For Yun/Leo/Micro/Zero/...
  delay(100);
  Serial.println("\n\nAdafruit finger detect test");

  // set the data rate for the sensor serial port
  finger.begin(57600);
  delay(5);
  if (finger.verifyPassword()) {
    Serial.println("Found fingerprint sensor!");
  } else {
    Serial.println("Did not find fingerprint sensor :(");
    while (1) { delay(1); }
  }

  finger.getTemplateCount();
  Serial.print("Sensor contains "); Serial.print(finger.templateCount); Serial.println(" templates");
  Serial.println("Waiting for valid finger...");
}

void(* resetFunc) (void) = 0;

void loop()                     // run over and over again
{
  getFingerprintIDez();
  //delay(50);            //don't ned to run this at full speed.
  Serial.print(digitalRead(13));
  resetState = digitalRead(13);
  if (resetState == HIGH) {
    Serial.print("On");
  } else {
    Serial.print("Off");
  }
}

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;
  }

  // OK success!

  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:
      Serial.println("Could not find fingerprint features");
      return p;
    case FINGERPRINT_INVALIDIMAGE:
      Serial.println("Could not find fingerprint features");
      return p;
    default:
      Serial.println("Unknown error");
      return p;
  }

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

  // found a match!
  Serial.print("Found ID #"); Serial.print(finger.fingerID);
  Serial.print(" with confidence of "); Serial.println(finger.confidence);

  return finger.fingerID;
}

// returns -1 if failed, otherwise returns ID #
int getFingerprintIDez() {
  uint8_t p = finger.getImage();
  if (p != FINGERPRINT_OK)  return -1;

  p = finger.image2Tz();
  if (p != FINGERPRINT_OK)  return -1;

  p = finger.fingerFastSearch();
  if (p != FINGERPRINT_OK)  return -1;

  // found a match!
  if(finger.fingerID == 3 && firstsuccess == 0) {
    Serial.print("Found first fingerprint, do the other one now");
    firstsuccess = 1;
    Serial.print(firstsuccess);
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Finger 1 OK!");
    delay(1000);
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Scan 2nd Finger");
  }

  if(finger.fingerID == 4 && firstsuccess == 1) {
    Serial.print("Found second fingerprint, success.");
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Success!");
    lock.attach(9);
    lock.write(90);
    while(true){
      Serial.print("Looping");
      resetState = digitalRead(13);
      if (resetState == HIGH) {
         lock.write(0);
         break;
      }
    }
    delay(1000);
    firstsuccess = 0;
    resetFunc();
  }
  Serial.print("Found ID #"); Serial.print(finger.fingerID);
  //Serial.print(" with confidence of "); Serial.println(finger.confidence);
  return finger.fingerID;
}