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

#define SERVO_PIN 6
#define SERVO_LOCK_POS 20
#define SERVO_UNLOCK_POS 90

const byte KEY_ROWS = 4;
const byte KEY_COLS = 4;
byte rowPins[4] = {A0, A1, A2, A3};
byte colPins[4] = {2, 3, 4, 5};

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

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, KEY_ROWS, KEY_COLS);
LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo;

bool locked = false;
char combo[4];
char input[4];
byte digitCount = 0;

void lock() {
  locked = true;
  servo.write(SERVO_LOCK_POS);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("LOCKED");
  lcd.setCursor(0, 1);
  lcd.print("Enter combo:");
  digitCount = 0;
}

void unlock() {
  locked = false;
  servo.write(SERVO_UNLOCK_POS);
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("UNLOCKED");
  lcd.setCursor(0, 1);
  lcd.print("Set new combo:");
  digitCount = 0;
}

void setup() {
  lcd.init();
  lcd.backlight();
  servo.attach(SERVO_PIN);
  servo.write(SERVO_UNLOCK_POS);
  lcd.setCursor(0, 0);
  lcd.print("UNLOCKED");
  lcd.setCursor(0, 1);
  lcd.print("Set new combo:");
}

void loop() {
  char key = keypad.getKey();

  // only accept digit keys 0-9
  if (key && key >= '0' && key <= '9') {
    input[digitCount] = key;
    digitCount++;

    // show * for each digit entered
    lcd.setCursor(digitCount - 1, 1);
    lcd.print("*");

    if (digitCount == 4) {
      if (locked) {
        // check if combo matches
        bool match = true;
        for (int i = 0; i < 4; i++) {
          if (input[i] != combo[i]) match = false;
        }
        if (match) {
          unlock();
        } else {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Wrong combo!");
          delay(1000);
          lock();
        }
      } else {
        // save as new combo and lock
        for (int i = 0; i < 4; i++) combo[i] = input[i];
        lock();
      }
    }
  }
}