/*
 * This ESP32 code is created by esp32io.com
 *
 * This ESP32 code is released in the public domain
 *
 * For more detail (instruction and wiring diagram), visit https://esp32io.com/tutorials/esp32-keypad
 */

#include <Keypad.h>
#include <ESP32Servo.h>

#define ROW_NUM     4 // four rows
#define COLUMN_NUM  4 // three columns

#define pinServoA 13 //Servo A is a 9g servo motor
#define pinServoB 14 //Servo B is a 20kg servo motor

Servo myServoA;
Servo myServoB;

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

byte pin_rows[ROW_NUM] = {1, 2, 42, 41}; // GIOP18, GIOP5, GIOP17, GIOP16 connect to the row pins
byte pin_column[COLUMN_NUM] = {40, 39, 38, 37};  // GIOP4, GIOP0, GIOP2 connect to the column pins

Keypad keypad = Keypad( makeKeymap(keys), pin_rows, pin_column, ROW_NUM, COLUMN_NUM );

const String password = "7890"; // change your password here
String input_password;
int redLED = 4; // define LED output as GPIO pin 4

void setup() {
  Serial.begin(9600);
  input_password.reserve(32); // maximum input characters is 33, change if needed
  pinMode(redLED, OUTPUT);

  myServoA.attach(pinServoA);
  myServoB.attach(pinServoB);

  myServoA.write(0);
  myServoB.write(0);
}

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

  if (key) {
    Serial.println(key);

    if (key == '*') {
      input_password = ""; // clear input password
    } else if (key == '#') {
      if (password == input_password) {
        Serial.println("The password is correct, ACCESS GRANTED!");
        digitalWrite(redLED, HIGH);
        delay(2000);
        digitalWrite(redLED, LOW);

        myServoA.write(30);
        delay(1000);
        myServoB.write(30);
        delay(5000);
        myServoB.write(0);
        delay(2000);
        myServoA.write(0);

      } else {
        Serial.println("The password is incorrect, ACCESS DENIED!");
      }

      input_password = ""; // clear input password
    } else {
      input_password += key; // append new character to input password string
    }
  }
}