#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Keypad.h>
#include <Servo.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Servo servo;

const byte row = 4;
const byte col = 4;
const char arr[row][col] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
const byte pinRow[row] = {13, 12, 11, 10};
const byte pinCol[col] = {9, 8, 7, 6};
Keypad keypad = Keypad(makeKeymap(arr), pinRow, pinCol, row, col);

Servo lockServo;
const byte servoPin = 6;
const byte relayPin = 7;

String password = "1234";
String inputPassword = "";
bool passwordEntered = false;

void lock() {
  lockServo.write(20);
}

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

void setup() {
  Serial.begin(9600);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;) {}
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  servo.attach(5);
  servo.write(0);
  display.println("Enter password:");
  display.display();

  pinMode(relayPin, OUTPUT);
  digitalWrite(relayPin, LOW);

  lockServo.attach(servoPin);
}

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

  if (btn != NO_KEY && !passwordEntered) {
    if (btn == '#') {
      if (inputPassword == password) {
        display.clearDisplay();
        display.setCursor(0, 0);
        display.println("Password correct");
        display.display();
        delay(1000);
        passwordEntered = true;
        inputPassword = "";
        servo.write(90);
      } else {
        display.clearDisplay();
        display.setCursor(0, 0);
        display.println("Password incorrect");
        display.display();
        delay(1000);
        display.clearDisplay();
        display.setCursor(0, 0);
        display.println("Enter password:");
        display.display();
        inputPassword = "";
      }
    } else if (btn == '*') {
      inputPassword = "";
      display.clearDisplay();
      display.setCursor(0, 0);
      display.println("Enter password:");
      display.display();
    } else if (inputPassword.length() < 4) {
      inputPassword += btn;
      display.clearDisplay();
      display.setCursor(0, 0);
      display.print("Enter password:");
      for (int i = 0; i < inputPassword.length(); i++) {
        display.print("*");
      }
      display.display();
    }
  }

  if (passwordEntered) {
    if (btn != NO_KEY) {
      if (btn == '1') {
        lock();
        digitalWrite(relayPin, LOW);
      } else if (btn == '2') {
        unlock();
        digitalWrite(relayPin, HIGH);
      } else if (btn == '3') {
        lock();
        digitalWrite(relayPin, LOW);
      }
    }
  }
}