#include <Keypad.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>

int servoPin = 11;
const int keypadRows = 4;
const int keypadColumns = 4;

const byte rowPins[keypadRows] = {9, 8, 7, 6};
const byte colPins[keypadColumns] = {5, 4, 3, 2};

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

Keypad keypad = Keypad(makeKeymap((char*)keys), rowPins, colPins, keypadRows, keypadColumns);
LiquidCrystal_I2C lcd(0x27, 20, 4);
Servo servo;

int servoUnlockPos = 90;
int servoLockPos = 0;

String password = "1234";
String input = "";

void setup() {
  lcd.begin(20, 4);
  lcd.init();
  lcd.setCursor(0, 0);
  lcd.backlight();

  servo.attach(servoPin);
  servo.write(servoLockPos);
}

void loop() {
  lcd.setCursor(0, 0);
  lcd.print("Enter password\n");
  input = inputPassword();

  if (input == password) {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Unlocked!\r");
    servo.write(servoUnlockPos);
    delay(1000);
  } else {
    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("WRONG PASSWORD!\n");
    lcd.setCursor(0, 1);
    lcd.print("Press # to reset\r");
    delay(1000);
  }

  while (true) {
    char key = keypad.getKey();
    if (key == '#') {
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Enter new password \n");
      String newPassword = inputPassword();
      lcd.clear();
      lcd.setCursor(0, 0);
      lcd.print("Confirm password \n");
      String confirmPassword = inputPassword();
      if (newPassword == confirmPassword) {
        password = newPassword;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Password changed!\r");
        delay(1000);
      } else {
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Password no match!\r");
        delay(1000);
      }

    }
  }
}

String inputPassword() {
  String input = "";
  while (input.length() < 4) {
    char key = keypad.getKey();
    if (key >= '0' && key <= '9') {
      lcd.print('*');
      input += key;
      Serial.print(input);
    }
  }
  String password = String(input);
  Serial.print(password);
  return password;
}