#include <Keypad.h>
int buzzer_pin = 13;
#include <ESP32Servo.h>

const int servo_pin = 21;

Servo myServo;

const int rows = 4;
const int cols = 4;

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

uint8_t rowPins[rows] = {8, 18, 17, 16}; 
uint8_t colPins[cols] = {39, 40, 41, 42};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, rows, cols);

int servoPos = 0;
int step = 1;
unsigned long previousMillis = 0;
const long interval = 10;
int servoMode = 0;
const int openDoorThreshold = 110;
const int lockDoorThreshold = 40;

String password = "123D";
String keyInput = "";

char key;
bool enableRead = true;

int button = 45;

void setup() {
  // put your setup code here, to run once:
  // pinMode(buzzer_pin, OUTPUT);
  Serial.begin(115200);
  myServo.attach(servo_pin);
  myServo.write(lockDoorThreshold);
  servoPos = lockDoorThreshold;
  pinMode(buzzer_pin, OUTPUT);
  pinMode(button, INPUT);
}

void readButton() {
  // Serial.println("button is waiting");
  int buttonState = digitalRead(button);
  if (buttonState == HIGH && servoPos == openDoorThreshold) {
    Serial.println("Button was pressed!");
    servoMode = 2;
  }
}

void servoAction() {
  unsigned long currentMillis = millis();
  if (servoMode == 1) /* open */ {
    Serial.println("Door is opening");
    if (currentMillis - previousMillis >= interval && servoPos < openDoorThreshold) {
      Serial.println(servoPos, openDoorThreshold);
      previousMillis = currentMillis;

      myServo.write(servoPos);

      servoPos += step;
      Serial.println(servoPos);
    } 
    if (servoPos == openDoorThreshold) {
      servoMode = 0;
      enableRead = true;
    }
  } else if (servoMode == 2) /* close */ {
    if (currentMillis - previousMillis >= interval && servoPos > lockDoorThreshold) {
      previousMillis = currentMillis;

      myServo.write(servoPos);

      servoPos -= step;
    }
    if (servoPos == lockDoorThreshold) {
      servoMode = 0;
      enableRead = true;
    }
  }
}

void readKeyInput() {
  key = keypad.getKey();
  unsigned long currentMillis = millis();
  if (key != NO_KEY) {
    Serial.println(key);
    tone(buzzer_pin, 432, 50);
    if (key == '#') {
      if (keyInput == password) {
        Serial.println("Success!");
        servoMode = 1;
        enableRead = false;
      } else {
        Serial.println("Wrong password!");
      }
      keyInput = "";
    } else if (key == '*') {
      keyInput = "";
      Serial.println("Input cleared");
    } else {
      keyInput += key;
    }
  }
}


void loop() {
  // put your main code here, to run repeatedly:
  if (enableRead) {
    readKeyInput();
    readButton();
  }

  servoAction();
}