#include <Wire.h>
#include <SparkFun_APDS9960.h>
#include <Servo.h>

#define SERVO_PIN 9
#define APDS 2
#define RED_LED A1
#define GREEN_LED A2

const int passwordLen = 6;

Servo servo;

int pageState = 0;
int passCounter = 0;
String password[passwordLen] = {"UP", "DOWN", "RIGHT", "LEFT", "UP", "DOWN"};
String recognizedGesture = "";
uint8_t proximityData = 0;

SparkFun_APDS9960 apds = SparkFun_APDS9960();
int flag = 0;

void setup() {
  pinMode(APDS, INPUT);
  servo.attach(SERVO_PIN);
  servo.write(0);
  servo.detach();

  Serial.begin(9600);
  Serial.print(passwordLen);
  Serial.println();
  Serial.println("APDS-9960 Lock");

  attachInterrupt(0, interrupRoutine, FALLING);

  if (apds.init()){
    Serial.println(("APDS-9960: Initilization APDS OK!"));
  } else{
    Serial.println(("APDS-9960: Initilization APDS ERROR!"));
  }
  if (apds.setProximityGain(PGAIN_1X)){
    Serial.println(("PGAIN_1X: Initilization APDS OK!"));
  } else{
    Serial.println(("PGAIN_1X: Initilization APDS ERROR!"));
  }
  if (apds.enableProximitySensor(true)){
    Serial.println(("Proximity Sensor: Initilization APDS OK!"));
  } else{
    Serial.println(("Proximity Sensor: Initilization APDS ERROR!"));
  }
  if (apds.enableGestureSensor(true)){
    Serial.println(("Gesture Sensor: Initilization APDS OK!"));
  } else{
    Serial.println(("Gesture Sensor: Initilization APDS ERROR!"));
  }

}

void loop() {
  if (pageState == 0){
      if(!apds.readProximity(proximityData)){
        Serial.println(("Proximity Error: error reading proximity!"));
      } else{
        Serial.println((proximityData));
        if (proximityData >= 40 && (pageState == 0)){
          pageState = 1;
          delay(2000);
        }
      }
      switch (pageState){
        case 0:
        blinkRed();
        break;

        case 1:
        passCounter = 0;
        for (int x = 0; x < passwordLen; x++){
          Serial.print(x);
          while (apds.isGestureAvailable() == false){
          }
          if (flag == 1){
            detachInterrupt(0);
            handleGesture();
            flag = 0;
            attachInterrupt(0, interrupRoutine, FALLING);
            if(recognizedGesture == password[x]){
              analogRead(GREEN_LED, 255);
              passCounter += 1;
              turnOff();
            } else if (recognizedGesutre != password[x]){
              analogRead(RED_LED, 255);
              pageState = 0;
              delay(1000);
              turnOff();
              break;
            }
            if(passCounter == passwordLen){
              pageState = 2;
              break;
            }
          }
          else if (recognizedGesture == "NONE") {
            x -= 1;
          }
        }
      }
      break;
    case 2:
    pageState = 0;
    servo.attach(SERVO_PIN);
    servo.write(90);
    enter();
    servo.write(0);
    break;
   }
} 

void interrupRoutine(){
  flag = 1 ;
}

void handleGesture(){
  if (apds.isGestureAvailable()){
    switch(apds.readGesture()){
      case DIR_UP:
        Serial.println("UP");
        recognizedGesture = "UP";
        break;
      case DIR_DOWN:
        Serial.println("DONW");
        recognizedGesture = "DONW";
        break;
      case DIR_LEFT:
        Serial.println("LEFT");
        recognizedGesture = "LEFT";
        break;
      case DIR_RIGHT:
        Serial.println("RIGHT");
        recognizedGesture = "RIGHT";
        break;
      case DIR_NEAR:
        Serial.println("NEAR");
        recognizedGesture = "NEAR";
        break;
      case DIR_FAR:
        Serial.println("FAR");
        recognizedGesture = "FAR";
        break;
      default:
        Serial.println("NONE");
        recognizedGesture = "NONE";
    }
  }
}

void turnOff(){
  analogRead(RED_LED, 0);
  analogRead(GREEN_LED, 0);
}

void blinkRed(){
  analogRead(RED_LED, 255);
  delay(250);
  turnOff();
  delay(250);
}

void enter(){
  for (int x = 0; x < 30; x++){
    analogRead(GREEN_LED, 255);
    delay(100);
    turnOff();
    delay(100);
  }
  for (int x = 9; x > 0; x--){
    delay(1000);
  }
}