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

Servo myServo; 

const int greenLedPin = 2;  // Pin for the green LED
const int redLedPin = 3;    // Pin for the red LED

// Define the keypad
const byte ROW_NUM    = 4; // four rows
const byte COLUMN_NUM = 4; // four columns

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]    = {9, 8, 7, 6}; 
byte pin_column[COLUMN_NUM] = {5, 4, 11, 10};

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

// Define variables for setup phase
bool setupPhase = true;
char setupCode[4];
int setupIndex = 0;

// Define the correct 4-digit code
char correctCode[] = {'0', '0', '0', '0'}; // Initialize with a default code
char enteredCode[4];
int codeIndex = 0;

bool servoRotated = false;

void setup() {
  Serial.begin(9600);
  myServo.attach(12);   // Attach the servo to pin 12
  pinMode(greenLedPin, OUTPUT);
  pinMode(redLedPin, OUTPUT);

  // Enter setup phase
  enterSetup();
}

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

  if (key) {
    if (setupPhase) {
      handleSetup(key);
    } else {
      handleOperation(key);
    }
  }
}

void enterSetup() {
  Serial.println("Enter a new 4-digit code:");

  while (setupIndex < 4) {
    char key = myKeypad.getKey();
    if (key) {
      setupCode[setupIndex] = key;
      setupIndex++;
      Serial.print('*'); // Mask the entered digit during setup
      delay(300);  // Delay for debouncing
    }
  }

  Serial.println("\nCode set successfully. Confirm the code:");

  while (true) {
    for (int i = 0; i < 4; i++) {
      char key = myKeypad.getKey();
      if (key) {
        Serial.print('*'); // Mask the entered digit during setup
        delay(300);
        if (key != setupCode[i]) {
          Serial.println("\nCodes do not match. Retry the setup.");
          setupIndex = 0;
          enterSetup();
          return;
        }
      }
    }

    // Codes match, exit setup phase
    Serial.println("\nCodes match. Setup complete.");
    delay(1000);
    setupPhase = false;
    // Save the setup code
    for (int i = 0; i < 4; i++) {
      correctCode[i] = setupCode[i];
    }
    resetCode();
    break;
  }
}

void handleSetup(char key) {
  // During setup, ignore key presses
}

void handleOperation(char key) {
  if (key == '*' || key == '#') {
    // Reset the entered code if '*' or '#' is pressed
    resetCode();
  } else {
    // Add the pressed key to the entered code
    enteredCode[codeIndex] = key;
    codeIndex++;

    if (codeIndex == 4) {
      // Check the entered code when 4 digits are entered
      checkCode();
      delay(1000); // Delay to prevent rapid key presses
      resetCode();
    }
  }
}

void checkCode() {
  // Check if the entered code is correct
  for (int i = 0; i < 4; i++) {
    if (enteredCode[i] != correctCode[i]) {
      // Incorrect code
      digitalWrite(redLedPin, HIGH);
      delay(1000);  // Display red LED for 1 second
      digitalWrite(redLedPin, LOW);
      return;
    }
  }

  // Correct code
  digitalWrite(greenLedPin, HIGH); // Turn on green LED
  if (servoRotated) {
    myServo.write(0);  // Rotate the servo motor back to initial position
    servoRotated = false;
  } else {
    myServo.write(90);  // Rotate the servo motor by 90 degrees
    servoRotated = true;
  }
  delay(200);  // Display green LED and servo motor for 2 seconds
  digitalWrite(greenLedPin, LOW);
}

void resetCode() {
  // Reset the entered code and index
  for (int i = 0; i < 4; i++) {
    enteredCode[i] = '\0';
  }
  codeIndex = 0;
  digitalWrite(greenLedPin, LOW);
  digitalWrite(redLedPin, LOW);
}