#include <Servo.h>

// Pin Definitions
const int ledPin = 13;
const int joystickX = A0;
const int joystickY = A1;
const int servoPin1 = 9;
const int servoPin2 = 10;
const int buttonPin = 2; // Pin for the button

// Create Servo objects
Servo servo1;
Servo servo2;

// Variables for LED and button control
int buttonState = 0;
int lastButtonState = 0;
bool ledState = false; // Track the LED state (on/off)

// Variables for servo control
int servo1Pos = 90; // Initial position for servo 1
int servo2Pos = 90; // Initial position for servo 2
int lastServo1Pos = -1; // Last reported position for servo 1
int lastServo2Pos = -1; // Last reported position for servo 2
int joystickNeutralZone = 50; // Neutral zone threshold for joystick

// Default speed percentage (0% to 100%)
int speedPercentage = 50; // Default speed is 50%

// Servo angle lock variables
bool isLocked = false; // Lock state
int servo1Min = 0, servo1Max = 180; // Servo 1 range
int servo2Min = 0, servo2Max = 180; // Servo 2 range

void setup() {
  // Initialize LED and button pins
  pinMode(ledPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP); // Button with pull-up resistor
  
  // Initialize servo motors
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);

  // Reset servos to neutral position
  servo1Pos = 90;
  servo2Pos = 90;
  servo1.write(servo1Pos);
  servo2.write(servo2Pos);

  // Start with LED off
  digitalWrite(ledPin, LOW);

  // Begin serial communication
  Serial.begin(9600);
  Serial.println("Enter 'reset' to set servos to 90 degrees.");
  Serial.println("Enter 'speed <value>' to set servo speed (0-100%).");
  Serial.println("Enter 'angle lock' to lock servos to specific angles.");
  Serial.println("Enter 'unlock' to remove angle locks.");
}

void loop() {
  // Button logic to toggle LED
  buttonState = digitalRead(buttonPin);
  if (buttonState == LOW && lastButtonState == HIGH) {
    ledState = !ledState;
    digitalWrite(ledPin, ledState ? HIGH : LOW);
    delay(200); // Debounce delay
  }
  lastButtonState = buttonState;

  // Check for Serial input to set speed, reset servos, or move by angle
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n'); // Read input until newline
    input.trim(); // Remove any extra whitespace

    // Check for "reset" command
    if (input == "reset") {
      proportionalResetServos(); // Reset servos proportionally to speed
      Serial.println("Servos reset to 90 degrees.");
    }

    // Check for "speed" command
    else if (input.startsWith("speed ")) {
      int newSpeed = input.substring(6).toInt(); // Extract the value
      if (newSpeed >= 0 && newSpeed <= 100) {
        speedPercentage = newSpeed;
        Serial.print("Speed set to ");
        Serial.print(speedPercentage);
        Serial.println("%.");
      } else {
        Serial.println("Invalid speed value. Enter a value between 0 and 100.");
      }
    }

    // Check for "angle lock" command
    else if (input == "angle lock") {
      isLocked = true;
      Serial.println("Enter minimum angle for Servo 1:");
      while (!Serial.available()); // Wait for input
      servo1Min = Serial.readStringUntil('\n').toInt();
      Serial.print("Servo 1 Min set to ");
      Serial.println(servo1Min);

      Serial.println("Enter maximum angle for Servo 1:");
      while (!Serial.available()); // Wait for input
      servo1Max = Serial.readStringUntil('\n').toInt();
      Serial.print("Servo 1 Max set to ");
      Serial.println(servo1Max);

      Serial.println("Enter minimum angle for Servo 2:");
      while (!Serial.available()); // Wait for input
      servo2Min = Serial.readStringUntil('\n').toInt();
      Serial.print("Servo 2 Min set to ");
      Serial.println(servo2Min);

      Serial.println("Enter maximum angle for Servo 2:");
      while (!Serial.available()); // Wait for input
      servo2Max = Serial.readStringUntil('\n').toInt();
      Serial.print("Servo 2 Max set to ");
      Serial.println(servo2Max);

      if (isValidRange(servo1Min, servo1Max) && isValidRange(servo2Min, servo2Max)) {
        Serial.println("Angle lock applied successfully.");
      } else {
        Serial.println("Invalid angle range entered. Removing lock.");
        isLocked = false;
      }
    }

    // Check for "unlock" command
    else if (input == "unlock") {
      isLocked = false;
      Serial.println("Angle lock removed. Servos restored to full range.");
    }
  }

  // Calculate step size based on speed percentage
  int servoVelocity = map(speedPercentage, 0, 100, 0, 10); // Speed 0-10 degrees per step

  // Read joystick positions (X and Y axis)
  int xPosition = analogRead(joystickX);
  int yPosition = analogRead(joystickY);

  // Determine target positions for the servos
  int targetServo1Pos = map(xPosition, 0, 1023, 0, 180);
  int targetServo2Pos = map(yPosition, 0, 1023, 0, 180);

  // If locked, constrain target positions to specified ranges
  if (isLocked) {
    targetServo1Pos = constrain(targetServo1Pos, servo1Min, servo1Max);
    targetServo2Pos = constrain(targetServo2Pos, servo2Min, servo2Max);
  }

  // Check if joystick is within the neutral zone
  bool isJoystickNeutral = true;
  if (abs(xPosition - 512) > joystickNeutralZone) {
    servo1Pos = moveTowards(servo1Pos, targetServo1Pos, servoVelocity); // Adjusted for speed
    isJoystickNeutral = false;
  }

  if (abs(yPosition - 512) > joystickNeutralZone) {
    servo2Pos = moveTowards(servo2Pos, targetServo2Pos, servoVelocity); // Adjusted for speed
    isJoystickNeutral = false;
  }

  // Set servo positions
  servo1.write(servo1Pos);
  servo2.write(servo2Pos);

  // If joystick is in the neutral zone and positions have changed, print to Serial Monitor
  if (isJoystickNeutral && (servo1Pos != lastServo1Pos || servo2Pos != lastServo2Pos)) {
    Serial.print("Joystick in neutral zone. Servo 1 Position: ");
    Serial.print(servo1Pos);
    Serial.print(" | Servo 2 Position: ");
    Serial.println(servo2Pos);

    // Update last reported positions
    lastServo1Pos = servo1Pos;
    lastServo2Pos = servo2Pos;
  }

  // Delay for smooth operation
  delay(15);
}

// Function to reset the servos to 90 degrees proportionally to speed
void proportionalResetServos() {
  int step = map(speedPercentage, 0, 100, 1, 10); // Step size proportional to speed
  while (servo1Pos != 90 || servo2Pos != 90) {
    if (servo1Pos != 90) {
      servo1Pos = moveTowards(servo1Pos, 90, step);
      servo1.write(servo1Pos);
    }
    if (servo2Pos != 90) {
      servo2Pos = moveTowards(servo2Pos, 90, step);
      servo2.write(servo2Pos);
    }
    delay(15); // Smooth movement
  }
}

// Function to move servo position gradually
int moveTowards(int current, int target, int step) {
  if (step == 0) return current; // If speed is 0%, don't move
  if (current < target) {
    return min(current + step, target); // Move closer to target
  } else if (current > target) {
    return max(current - step, target); // Move closer to target
  }
  return current; // If already at target
}

// Function to validate angle range
bool isValidRange(int min, int max) {
  return min >= 0 && max <= 180 && max > min;
}