#include <Adafruit_Fingerprint.h>
#define mySerial Serial1
Adafruit_Fingerprint finger = Adafruit_Fingerprint(&mySerial);
#define ledPin 7
int state;          // state
int alarmPin = 8;       // Alarm Pin nummer
int tiltPin = 5;        // Tilt føler pin
int servoPin = 6;     // LED connected to digital pin 9
int alarm;
int Received;
int resetAlarm = 11;
uint8_t id;
int fingerprintID;


void setup() {
  // put your setup code here, to run once:
  pinMode(ledPin, OUTPUT);
  pinMode(alarmPin, OUTPUT);  // sets the pin as output
  pinMode(servoPin, OUTPUT);
  pinMode(tiltPin, INPUT);
  pinMode(resetAlarm, INPUT);
  Serial.begin(38400); // Default communication rate of the Bluetooth module

  Serial.begin(9600);   //Starter Serial monitor
  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);

  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");
  delay(500);
}

void loop()
// put your main code here, to run repeatedly:
{
  getFingerprintIDez();
  delay(50);            //don't ned to run this at full speed.

  if (Serial.available() > 0)
  {
    Received = Serial.read();

  }
  if (state == 0)
  {
    Serial.println("State 0");
    analogWrite(servoPin, 198);
  }
  if (digitalRead(tiltPin) == 1 && state == 0)
  {
    Serial.println("Tilt er høj");
    alarm = 1;
  }
  if (digitalRead(resetAlarm) == 1 && state == 0)
  {
    Serial.println("Reset Alarm");
    alarm = 0;
  }

  if (alarm == 1)
  {
    Serial.println("Alarm");
    delay (500);
    analogWrite(alarmPin, 250); //   1/(2/255) = 127,5, an analog out of 128 is the closest approximation to a PWM signal no shorter than 1ms being the lowest value the Servo accepts.
    delay (500);
    analogWrite(alarmPin, 0); //   2/(2/255) = 255, an analog out of 255 is the upper value for, a PWM signal 2ms wide, (when the signal generate is 500 Hz), the Servo accepts.
  }
  ////////////////LIGHT/////////////////////
  if (state == 0 && Received == '1')
  {
    Serial.println("State 1");
    analogWrite(servoPin, 128);
    state = 1;
  }
  if (state == 1)
  {
    Serial.println("State 1");
    Serial.println(digitalRead(tiltPin));
    Serial.println(Received);
  }
  ////////////////VENT///////////////////////
  if (state == 1 && Received == 51)
  {
    Serial.println("State 2");
    analogWrite(servoPin, 198);
    state = 2;
    delay (5000);
  }
  if (state == 2)
  {
    Serial.println("State 2");
    Serial.println(Received);
    Serial.print("Found ID #"); Serial.print(finger.fingerID);
    Serial.print(" with confidence of "); Serial.println(finger.confidence);
    }

  ///////////////////////////////////////////
  ////////////////pc/////////////////////////
  if (finger.fingerID == 2 && state == 2)
  {
    state = 3;

  }

  ////////////////VENT///////////////////////
  if (state == 3)
  {
    Serial.println("State 3");
    analogWrite(servoPin, 128);
    delay (1000);
    state = 4;
  }
  ///////////////////////////////////////////
  ////////////////VENT///////////////////////
  if (state == 4)
  {
    Serial.println("State 4");
    Serial.println("Venter på lukket låg");
    Serial.println(Received);
    if (Received == 50)
    {
    state = 5;
    }
  }
  if (state == 5)
  {
    Serial.println("Låget er lukket");
    delay (5000);
    analogWrite(servoPin, 198);
    state = 0;
    Received = 0;
    delay (5000);
    }
  ///////////////////////////////////////////
}
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;
}