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

const byte numRows = 4; 
const byte numCols = 4; 

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

byte rowPins[numRows] = {32, 33, 12, 13}; 
byte colPins[numCols] = {25, 26, 27, 23}; 

Keypad myKeypad = Keypad(makeKeymap(keyMap), rowPins, colPins, numRows, numCols);

const int servoControlPin = 4; 
Servo servoMotor;
int servoPosition = 0;
bool isCustomAngleRequested = false; 
int customServoAngle = 0; 

void moveServoToAngle(int angle);
void stopServoMotor();
void resetServoPosition();

void setup() {
  Serial.begin(115200);
  servoMotor.attach(servoControlPin);
  servoMotor.write(servoPosition); 
  Serial.println("Servo Control System Initialized");
  Serial.println("Press 'C' to enter a custom angle in the serial monitor.");
}

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

  if (keyPressed) {
    switch (keyPressed) {
      case '1': moveServoToAngle(30); break;
      case '2': moveServoToAngle(45); break;
      case '3': moveServoToAngle(60); break;
      case '4': moveServoToAngle(90); break;
      case '5': moveServoToAngle(120); break;
      case '6': moveServoToAngle(180); break;
      case 'A': stopServoMotor(); break;
      case 'B': resetServoPosition(); break;
      case 'C': {
        Serial.println("Enter custom angle (0-180):");
        isCustomAngleRequested = true; 
        break;
      }
      default: Serial.println("Invalid key!"); break;
    }
  }

  if (isCustomAngleRequested && Serial.available() > 0) {
    customServoAngle = Serial.parseInt();
    if (customServoAngle >= 0 && customServoAngle <= 180) {
      moveServoToAngle(customServoAngle);
    } else {
      Serial.println("Invalid angle! Please enter an angle between 0 and 180.");
    }
    isCustomAngleRequested = false;
  }
}

void moveServoToAngle(int angle) {
  servoPosition = angle;
  servoMotor.write(angle);
  Serial.print("Moved to ");
  Serial.print(angle);
  Serial.println(" degrees");
}

void stopServoMotor() {
  servoMotor.detach();
  Serial.println("Servo stopped");
}

void resetServoPosition() {
  servoPosition = 0;
  servoMotor.attach(servoControlPin);
  servoMotor.write(servoPosition);
  Serial.println("Servo reset to 0 degrees");
}