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

#define trigpin 16
#define echpin 17
#define servopin 18

int cm;

bool Bahaya = false;

int pos = 0;

String pass = "31A*B2";
char CKPASS[7];
const byte ROWS = 4;
const byte COLS = 4;
char keys[ROWS][COLS]={
  {'1','2','3','A'},
  {'4','5','6','B'},
  {'7','8','9','C'},
  {'*','0','#','D'},
};

byte rowPins[ROWS]={12,14,27,26};
byte colPins[COLS]={25,33,32,13};

Keypad keypad=Keypad(makeKeymap (keys), rowPins, colPins, ROWS, COLS);

Servo myservo;



int GetPassword(){
  for(int i=0;i<6;i++){
    char key=keypad.getKey();
    if (key){
      CKPASS[i] = key;
    }
  }
  CKPASS[6] = '.\0'; 

  CheckPass(String(CKPASS));
  return 0;
}

void CheckPass(String CKPASS){
  if(CKPASS == pass){
  }
  else{
    for(int i=0;i<2;i++){
    }
    GetPassword();
  }
}

void Lock(){
  if(pos==180){
    pos -= 180;
    myservo.write(pos);
    for(int i=0;i<3;i++){
      tone(5,4500,200);
      delay(200);
      noTone(4);
      delay(200);
    }
    delay(15);
  }
}

void Unlock(){
  if(pos==0){
    pos+=180;
    myservo.write(pos);
    delay(15);
    tone(5, 5000, 200);
    delay(1000);
  }
}


void setup() {
  Serial.begin(115200);
  pinMode(5, OUTPUT);
  myservo.attach(servopin);
  pos = myservo.read();
  if(pos != 0){
    myservo.write(0);
    delay(15);

    pos = myservo.read();
  }
  delay(1000);
}

void loop() {
  int PasswordSuccess = GetPassword();
  while(!keypad.getKey());

  if(PasswordSuccess == 0){
    Unlock();
    delay(1000);
    PasswordSuccess = 1;
  }
  Lock();
}