#include <Servo.h>

//These are the states
#define S_START 0
#define S_BEGIN 1
#define S_LOCK1 2
#define S_LOCK2 3
#define S_LOCK3 4
#define S_UNLOCK 5
#define S_SET 6

//button debounce
#define DEBOUNCE 150

//The servo will act as the "lock" of the safe
Servo unlockServo;

/* Password/Button key
0 - top
1 - left
2 - bottom
3 - right
4 - far right (not usable on password)
-1 - no button (not usable on password)
*/

int State = S_START; //the default state is START
int buttonval = 0; //used by loop_update to find button value
int password[] = {2, 0, 3}; //the safe's password
int next = 0; //variable used by loop update
int count = 0; //variable used to keep track of password set
int next_set = 0; //variable used to update password

void setup() {
  Serial.begin(9600);
  unlockServo.attach(10);
}

void loop() {
  next = loop_update();

  switch (State) {
    case S_START:
      //make sure the "lock" is in lock position and no tones are playing
      unlockServo.write(0);
      noTone(8);
      //if the light sensor is reading less than 100, we're good to start
      if (analogRead(A1) < 100) { State = S_BEGIN; }
    break;

    case S_BEGIN:
      if (next == 4) {
        //if the far right button is being pressed, enter setup mode.
        State = S_SET;
      }
      else {
        if (next != -1) {
          //if another button is being pressed and that button actually exists (not -1) start the lock combination
          State = S_LOCK1;
        }
      }
    break;

    case S_SET:
     count = 0;
     //reset the count and start the loop
     //pressing the far right button will go back to the BEGIN state, but will also keep any password changes
     //count < 3 is the limiter because the password is 3 digits long
     while (next_set != 4 && count < 3) {
      noTone(8);
      next_set = loop_update();
      if (next_set != 4 && count < 3) {
        if (next_set != -1) {
        password[count] = next_set;
        tone(8, 300); // play a short tone to indicate you updated something
        count = count + 1;
        }
      }
     }
     State = S_BEGIN; //go back to the BEGIN state when finished
    break;

    case S_LOCK1:
      if (next == password[0]) { //if the button pressed equals the password's respective digit go to the next lock state & play a short tone to indicate you were correct
        tone(8, 200);
        State = S_LOCK2;
      }
      else {
        if (next != -1) { //if the button pressed was incorrect (and a button was actually pressed) go back to the begin state and play a short tone to indicate you were wrong
          tone(8, 100);
          State = S_BEGIN;
        }
      }
    break;

    case S_LOCK2:
    noTone(8); //resets the previously played success tone
    if (next == password[1]) {
        tone(8, 200);
        State = S_LOCK3;
      }
      else {
        if (next != -1) {
          tone(8, 100);
          State = S_BEGIN;
        }
      }
    break;

    case S_LOCK3:
    noTone(8);
    if (next == password[2]) {
        tone(8, 200);
        State = S_UNLOCK; //by now all 3 digits are correct, so go to the unlock state
      }
      else {
        if (next != -1) {
          tone(8, 100);
          State = S_BEGIN;
        }
      }
    break;

    case S_UNLOCK:
      noTone(8);
      unlockServo.write(180); //switch the servo to unlock position

      if (next == 4) { //if the far left button is pressed, go back to the begin state, therefore locking the safe again
        State = S_BEGIN;
      }
    break;
  }
}

int loop_update() {
  buttonval = analogRead(A2);
  //Top
  if (buttonval >= 100 and buttonval < 200) {
    delay(DEBOUNCE);
    return 0;
  }
  // Left
  else if (buttonval >= 0 and buttonval <= 60) {
    delay(DEBOUNCE);
    return 1;
  }
  //Bottom
  else if (buttonval >= 300 and buttonval < 400) {
    delay(DEBOUNCE);
    return 2;
  }
  //Right
  else if (buttonval > 450 and buttonval < 580) {
    delay(DEBOUNCE);
    return 3;
  }
  //Far Right
  else if (buttonval > 650 and buttonval < 800) {
    delay(DEBOUNCE);
    return 4;
  }
  //nothing pressed
  else return -1;
}