#include <Servo.h>
//#include <EEPROM.h>

Servo lockServo;

const int piezoAnalog = 0;
const int piezoDigital = 21;

const int openPin = 1;
const int pirPin = 12;
const int trainingPin = 2;

const int ledPin = 11;

int prevPiezo;
boolean knocking;
int numKnocks;
int notDetectedCount;

long int knockTimes[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
double patternTimes[] = {0,1,2,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int patternLength = 3;

boolean sensedMotion;

void resetDetection() {
  knocking = true;
  numKnocks = 0;
  notDetectedCount = 0;
  prevPiezo = analogRead(piezoAnalog);
}

void setup() {
  pinMode(piezoDigital, INPUT);
  pinMode(openPin, INPUT_PULLUP);
  pinMode(trainingPin, INPUT_PULLUP);
  
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, LOW);
  
  lockServo.attach(4);
  
  sensedMotion = false;
  resetDetection();
  lock();
}

boolean foundPattern()
{
  if(numKnocks != patternLength) return false;
  
  long int baseTime = knockTimes[0];
  double factor = 1 / (double)(knockTimes[1] - baseTime);
  
  for(int i=0; i < patternLength; i++)
  {
    double converted = (double)(knockTimes[i] - baseTime) * factor;
    double convDiff = converted - patternTimes[i];
    Serial.println(convDiff);
    
    if(convDiff > 0.4 || convDiff < -0.4) return false;
  }
  
  return true;
}

void detectKnocks()
{
  int piezo = analogRead(piezoAnalog);
  
  int diff = piezo - prevPiezo;
  //Serial.println(diff);
  //if(diff > 1 || diff < -1) Serial.println(diff);
  if(diff > 15 || diff < -15)
  {
    //Serial.println(diff);
    notDetectedCount = 0;
    if(!knocking) 
    {
      //digitalWrite(ledPin, HIGH);
      knockTimes[numKnocks] = millis();
      Serial.println("Num");
      Serial.println(numKnocks);
      //Serial.println(knockTimes[numKnocks]);
      numKnocks++;
      knocking = true;
    }
  }
  else
  {
    //digitalWrite(ledPin, LOW);
    if(notDetectedCount > 5) knocking = false;
    else notDetectedCount++;
    
    if((numKnocks > 0) && (millis() - knockTimes[numKnocks-1] > 1000))
    {
      if(foundPattern())
      {
        unlock();
        digitalWrite(ledPin, HIGH);
        delay(5000);
      }
      else
      {
        lock();
        digitalWrite(ledPin, LOW);
      }
      resetDetection();
    }
  }
  prevPiezo = piezo;
}

void learnKnocks()
{
  int piezo = analogRead(piezoAnalog);
  
  int diff = piezo - prevPiezo;
  //Serial.println(diff);
  //if(diff > 1 || diff < -1) Serial.println(diff);
  if(diff > 15 || diff < -15)
  {
    notDetectedCount = 0;
    if(!knocking) 
    {
      digitalWrite(ledPin, HIGH);
      knockTimes[numKnocks] = millis();
      Serial.println("Num");
      Serial.println(numKnocks);
      //Serial.println(knockTimes[numKnocks]);
      numKnocks++;
      knocking = true;
      
      long int baseTime = knockTimes[0];
      double factor = 1 / (double)(knockTimes[1] - baseTime);
      
      for(int i=0; i < numKnocks; i++)
      {
        double converted = (double)(knockTimes[i] - baseTime) * factor;
        Serial.println(converted);
        patternTimes[i] = converted;
      }
      patternLength = numKnocks;
    }
  }
  else
  {
    if(notDetectedCount > 5)
    {
      digitalWrite(ledPin, LOW);
      knocking = false;
    }
    else notDetectedCount++;
    
    if((numKnocks > 0) && (millis() - knockTimes[numKnocks-1] > 1000)) resetDetection();
  }
  prevPiezo = piezo;
}

void lock()
{
  lockServo.write(10);
}

void unlock()
{
  lockServo.write(105);
}

void loop() {
  
  boolean isOpen = digitalRead(openPin);
  //digitalWrite(ledPin, isOpen);
  //Serial.println(isOpen);
  
  if(sensedMotion && !digitalRead(pirPin)) lock();
  sensedMotion = digitalRead(pirPin);
  boolean isTraining = !digitalRead(trainingPin);
  
  if(isTraining)
  {
    learnKnocks();
  }
  else if(isOpen)
  {
    unlock();
    resetDetection();
  }
  else if(sensedMotion)
  {
    unlock();
    resetDetection();
  }
  else
  {
    detectKnocks();
  }
  
  delay(5);
}

/*
void EEPROM_writeDouble(int ee, double value)
{
    byte* p = (byte*)(void*)&value;
    for (int i = 0; i < sizeof(value); i++)
        EEPROM.write(ee++, *p++);
}

double EEPROM_readDouble(int ee)
{
    double value = 0.0;
    byte* p = (byte*)(void*)&value;
    for (int i = 0; i < sizeof(value); i++)
        *p++ = EEPROM.read(ee++);
    return value;
}
*/