#include <Keypad.h>
#include <ESP32Servo.h>  // Use ESP32Servo for ESP32 compatibility

#define SERVO_PIN 18  // PWM pin connected to the servo motor

Servo myServo;

// Keypad configuration
const byte ROWS = 4;  // Four rows
const byte COLS = 4;  // Four columns
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {5, 17, 16, 4};  // Connect to the row pins of the keypad
byte colPins[COLS] = {0, 2, 15, 13};  // Connect to the column pins of the keypad

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

void setup() {
  Serial.begin(115200);
  myServo.attach(SERVO_PIN);
  myServo.write(0);  // Initialize servo at 0°
  Serial.println("Servo Controller Initialized");
}

void loop() {
  char key = keypad.getKey();
  
  if (key) {
    switch (key) {
      case '1':
        moveServo(30);
        break;
      case '2':
        moveServo(45);
        break;
      case '3':
        moveServo(60);
        break;
      case '4':
        moveServo(90);
        break;
      case '5':
        moveServo(120);
        break;
      case '6':
        moveServo(150);
        break;
      case '7':
        moveServo(180);
        break;
      case 'A':  // Reset button
        moveServo(0);
        break;
      case 'B':  // Stop button
        stopServo();
        break;
      case 'C':  // Custom angle input via serial monitor
        customAngleInput();
        break;
      default:
        break;
    }
  }
}

void moveServo(int angle) {
  myServo.write(angle);
  Serial.print("Moving servo to ");
  Serial.print(angle);
  Serial.println(" degrees");
}

void stopServo() {
  int currentPos = myServo.read();  // Get the current position of the servo
  myServo.detach();  // Detach servo to stop movement
  Serial.print("Servo stopped at ");
  Serial.print(currentPos);
  Serial.println(" degrees");
}

void customAngleInput() {
  Serial.println("Enter custom angle (0° to 180°): ");
  while (!Serial.available());
  int customAngle = Serial.parseInt();
  if (customAngle >= 0 && customAngle <= 180) {
    myServo.attach(SERVO_PIN);  // Reattach servo if it was detached
    moveServo(customAngle);
  } else {
    Serial.println("Invalid angle. Please enter a value between 0° and 180°.");
  }
}