#include <Wire.h>
#include <ESP32Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Keypad.h>
// include library to read and write from flash memory
#include <EEPROM.h>
#include "SafeState.h"

#define EEPROM_SIZE 12

// Our custom icon numbers
#define ICON_LOCKED_CHAR   (byte)0
#define ICON_UNLOCKED_CHAR (byte)1
#define ICON_RIGHT_ARROW   (byte)126

LiquidCrystal_I2C lcd(0x27, 20, 4);

const unsigned long eventInterval = 10000;
unsigned long previousTime = 0;
unsigned long currentTime = 0;

byte iconUnlocked[] = {
  0b01110,
  0b10000,
  0b10000,
  0b11111,
  0b11011,
  0b11011,
  0b11111,
};

byte iconLocked[] = {
  0b01110,
  0b10001,
  0b10001,
  0b11111,
  0b11011,
  0b11011,
  0b11111,
};

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

#define SERVO_PIN 6
#define SERVO_LOCK_POS   20
#define SERVO_UNLOCK_POS 90
Servo lockServo;

uint8_t colPins[COLS] = { 16, 4, 0, 2 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 19, 18, 5, 17 }; // Pins connected to R1, R2, R3, R4

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

SafeState safeState;
bool bckl = true;
String pass = "";

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  EEPROM.begin(EEPROM_SIZE);
  //EEPROM.writeString(0, pass);
  //EEPROM.commit();

  if (safeState.locked()) {
    lock();
  } else {
    unlock();
  }
  
  lcd.init();
  lcd.clear();         
  lcd.begin(20,4);
  lcd.createChar(ICON_LOCKED_CHAR, iconLocked);
  lcd.createChar(ICON_UNLOCKED_CHAR, iconUnlocked);   
}

void loop() {
  // put your main code here, to run repeatedly:
  if (safeState.locked()) {
    lcd.noBacklight();
    lcd.clear();
    lcd.setCursor(2, 0);
    lcd.write(ICON_LOCKED_CHAR);
    lcd.print(" Safe Locked! ");
    lcd.write(ICON_LOCKED_CHAR);
    while (bckl){
      char key = keypad.getKey();
      if (key == '*') {
        lcd.backlight();
        bckl = false;
        previousTime = millis();
        Serial.println("PT:");
        Serial.println(previousTime);
      } 
    }
    safeLockedLogic();
  } else {
    lcd.backlight();
    safeUnlockedLogic();
  }
}

void safeLockedLogic() {
  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.write(ICON_LOCKED_CHAR);
  lcd.print(" Safe Locked! ");
  lcd.write(ICON_LOCKED_CHAR);
  if (previousTime == 0) {
    previousTime = millis();
  }
  bool unlockedSuccessfully = false;
  String userCode = inputSecretCode();
  if (!userCode.equals("")){ 
    unlockedSuccessfully = safeState.unlock(userCode);
    showWaitScreen(200);
  }else{
    //unlockedSuccessfully = false;
  }
  if (unlockedSuccessfully) {
    showUnlockMessage();
    unlock();
  } else {
    lcd.clear();
    lcd.setCursor(4, 0);
    lcd.print("Access Denied!");
    //delay(1000);
    showWaitScreen(1000);
    bckl = true;
  }
}

void safeUnlockedLogic() {
  lcd.clear();
  
  lcd.setCursor(2, 0);
  lcd.write(ICON_UNLOCKED_CHAR);
  lcd.setCursor(4, 0);
  lcd.print(" # to lock");
  lcd.setCursor(17, 0);
  lcd.write(ICON_UNLOCKED_CHAR);

  bool newCodeNeeded = true;

  if (safeState.hasCode()) {
    lcd.setCursor(2, 1);
    lcd.print("  A = new code");
    newCodeNeeded = false;
  }

  auto key = keypad.getKey();
  while (key != 'A' && key != '#') {
    key = keypad.getKey();
  }

  bool readyToLock = true;
  if (key == 'A' || newCodeNeeded) {
    readyToLock = setNewCode();
  }

  if (readyToLock) {
    lcd.clear();
    lcd.setCursor(8, 0);
    lcd.write(ICON_UNLOCKED_CHAR);
    lcd.print(" ");
    lcd.write(ICON_RIGHT_ARROW);
    lcd.print(" ");
    lcd.write(ICON_LOCKED_CHAR);

    safeState.lock();
    lock();
    showWaitScreen(100);
  }
}

bool setNewCode() {
  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.print("Enter new code:");
  String newCode = inputSecretCode();
  if (newCode == ""){
    return false;
  }
  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.print("Confirm new code");
  String confirmCode = inputSecretCode();

  if (newCode.equals(confirmCode)) {
    safeState.setCode(newCode);
    return true;
  } else {
    lcd.clear();
    lcd.setCursor(1, 0);
    lcd.print("Code mismatch");
    lcd.setCursor(0, 1);
    lcd.print("Safe not locked!");
    delay(2000);
    return false;
  }
}

String inputSecretCode() {
  lcd.setCursor(7, 1);
  lcd.print("[____]");
  lcd.setCursor(8, 1);
  String result = "";
  while (result.length() < 4) {
    currentTime = millis();
    if (currentTime - previousTime >= eventInterval) {
      break;
      bckl = true;
    }
    char key = keypad.getKey();
    if (key >= '0' && key <= '9') {
      lcd.print('*');
      result += key;
    }
    
  }
  return result;
}

void showUnlockMessage() {
  lcd.clear();
  lcd.setCursor(2, 0);
  lcd.write(ICON_UNLOCKED_CHAR);
  lcd.setCursor(4, 0);
  lcd.print("Unlocked!");
  lcd.setCursor(17, 0);
  lcd.write(ICON_UNLOCKED_CHAR);
  delay(1000);
}

void showWaitScreen(int delayMillis) {
  lcd.setCursor(5, 1);
  lcd.print("[..........]");
  lcd.setCursor(6, 1);
  for (byte i = 0; i < 10; i++) {
    delay(delayMillis);
    lcd.print("=");
  }
}

void lock() {
  lockServo.write(SERVO_LOCK_POS);
  safeState.lock();
}

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