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

const byte ROWS = 4;
const byte COLS = 3;

char keys[ROWS][COLS] = {
  {'1', '2', '3'},
  {'4', '5', '6'},
  {'7', '8', '9'},
  {'*', '0', '#'}
};

byte rowPins[ROWS] = {13, 12, 11, 10}; // Connect to the row pin of the keypad
byte colPins[COLS] = {9, 8, 7}; // Connect to the column pin of the keypad

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

const int passwordLength = 4;
const char correctPassword[passwordLength] = {'1', '2', '3', '4'};
char inputPassword[passwordLength];
int passwordIndex = 0;

void setup() {
  servo1.attach(5); // Pin for the first servo
  servo2.attach(3); // Pin for the second servo
  
  Serial.begin(9600);
  
  // Initialize servos to the middle position
  goToMiddle();
}

void loop() {
  char key = keypad.getKey();
  
  if (key) {
    Serial.print(key);
    
    if (key >= '0' && key <= '9') {
      inputPassword[passwordIndex] = key;
      passwordIndex++;
      
      if (passwordIndex >= passwordLength) {
        passwordIndex = 0;
        
        // Check if the input password is correct
        bool correct = true;
        for (int i = 0; i < passwordLength; i++) {
          if (inputPassword[i] != correctPassword[i]) {
            correct = false;
            break;
          }
        }
        
        if (correct) {
          moveServos(); // Move the servos
        } else {
          Serial.println("Incorrect password");
        }
        
        // Clear the input password
        for (int i = 0; i < passwordLength; i++) {
          inputPassword[i] = '\0';
        }
      }
    } 
    else if (key == '#') {
      // Move servos back to the middle position
      goToMiddle();
    }
  }
}

void moveServos() {
  int startPosition = 90;
  int endPosition = 180;
  
  // Move servos from middle (90 degrees) to 180 degrees and back to 90 degrees
  for (int pos = startPosition; pos <= endPosition; pos++) {
    servo1.write(pos);
    servo2.write(pos);
    delay(15); // Adjust the delay for speed control
  }
  
  for (int pos = endPosition; pos >= startPosition; pos--) {
    servo1.write(pos);
    servo2.write(pos);
    delay(15); // Adjust the delay for speed control
  }
  
  // Ensure servos end at middle position
  goToMiddle();
}

void goToMiddle() {
  servo1.write(90);
  servo2.write(90);
  delay(500); // Short delay to ensure servos have time to reach the position
}