#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
#include <ezButton.h>

#include "knock_lock.h"
#include "pinout.h"
#include "knock_detection.h"
#include "knock_delay_control.h"


const int16_t KNOCK_THRESHOLD = 10000u; // accerelation threshold to be interpreted as a knock
uint8_t knock_delay_level = 0u;
uint32_t last_buzz_millis = 0u;

UnlockResult unlock_result = UnlockResult::Success;
const uint8_t password = 0b10000000;
bool password_bit = false;
KnockLock lock(password);

Servo servo;
const uint16_t SERVO_UNLOCKED_POS = 0u;
const uint16_t SERVO_LOCKED_POS = 180u;
uint16_t servo_current_pos = 0u;

MPU6050 mpu;

ezButton delay_button(DELAY_BUTTON_PIN);
ezButton lock_button(LOCK_BUTTON_PIN);

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  servo.attach(SERVO_PIN);
  lock_button.setDebounceTime(50);
  delay_button.setDebounceTime(50);

  pinMode(DELAY_BUTTON_PIN, INPUT_PULLUP);
  pinMode(LOCK_BUTTON_PIN, INPUT_PULLUP);
  pinMode(LED_R_PIN, OUTPUT);
  pinMode(LED_G_PIN, OUTPUT);
  digitalWrite(LED_R_PIN, LOW);
  digitalWrite(LED_G_PIN, LOW);
}

void loop() {
  delay_button.loop();
  lock_button.loop();
  display_delay_level(knock_delay_level);

  if (delay_button.isPressed()) {
    if (++knock_delay_level > MAX_DELAY_LEVEL) {
      knock_delay_level = 0;
    }
  }

  bool knock_detected = is_knock_detected(mpu, KNOCK_THRESHOLD, AccelerationAxis::X);
  if (knock_detected) {
    password_bit = true;
  }

  switch(lock.get_lock_state()) {
    case LockState::Locked:
      digitalWrite(LED_G_PIN, HIGH);
      digitalWrite(LED_R_PIN, HIGH);
      servo.write(SERVO_LOCKED_POS);
      if (knock_detected) {
        Serial.println("Knock detected!");
        last_buzz_millis = millis();
        lock.set_lock_state(LockState::UnlockInProgress);
      }
      break;

    case LockState::UnlockInProgress:
      if (millis() - last_buzz_millis > knock_delay_levels[knock_delay_level]) {        
        unlock_result = lock.check_password(password_bit);
        Serial.println(password_bit);
        password_bit = false;
        last_buzz_millis = millis();

        if (unlock_result != UnlockResult::Success) {
          if (unlock_result == UnlockResult::PasswordCorrect) {
            // unlock the door
            servo.write(SERVO_UNLOCKED_POS);
            lock.set_lock_state(LockState::Unlocked);
            tone(BUZZER_PIN, 392, 250);
          }
          else {
            digitalWrite(LED_G_PIN, LOW);
            tone(BUZZER_PIN, 370, 250);
            delay(knock_delay_levels[knock_delay_level]);
            digitalWrite(LED_R_PIN, HIGH);
            lock.set_lock_state(LockState::Locked);
          }
        }
        else {
          tone(BUZZER_PIN, 262, 250);
        }
      }
      break;

    case LockState::Unlocked:
      // if button pressed, lock the door
      digitalWrite(LED_G_PIN, HIGH);
      digitalWrite(LED_R_PIN, LOW);
      if (lock_button.isPressed()) {
        lock.set_lock_state(LockState::Locked);
      }
      break;

    default:
      break;
  }
}