#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("press angle to define angle (0-180):");
        inputCustomAngle();
        delay(500);
        break;
      }
      default: Serial.println("Invalid key!"); break;
    }
  }

}

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");
}
void inputCustomAngle(){
   Serial.println("Press angle (0-180) using keypad, and then press '#':");
  
  String inputAngle = ""; // To store the input angle
  char inputKey;

  while (true) {
    inputKey = myKeypad.getKey(); // Get a key press
    if (inputKey) { // If a key is pressed
      if (inputKey >= '0' && inputKey <= '9') {
        inputAngle += inputKey; // Append the digit to the input string
        Serial.print(inputKey); // Display the input digit on the Serial Monitor
      } 
      else if (inputKey == '#') { // Use '#' to finish entering the angle
        if (inputAngle.length() > 0) { // Ensure something was entered
          int angle = inputAngle.toInt(); // Convert to integer
          if (angle >= 0 && angle <= 180) {
            servoMotor.write(angle); // Move to the custom angle
            delay(1000); // Wait for the servo to reach the position
            Serial.print("\nServo moved to: ");
            Serial.println(angle); // Print the custom angle
          } else {
            Serial.println("\nInvalid angle. ");
          }
        } else {
          Serial.println("\nNo angle entered.");
        }
        break; // Exit the while loop after processing
      }
    }
  }
}