#include <Servo.h>  // Include the Servo library to control servos

// Initialize servo objects
Servo servoUpDown1;   // Servo for up-down movement of arm 1
Servo servoUpDown2;   // Servo for up-down movement of arm 2
Servo servoLeftRight; // Servo for left-right movement
Servo servoGripper;   // Servo for gripper control (MG996)

// Joystick pins
const int joyXPin = A2;  // Joystick X-axis pin (controls left-right movement)
const int joyYPin = A3;  // Joystick Y-axis pin (controls up-down movement)

// Servo pins
const int servoUpDown1Pin = 9;   // Servo 1 pin for up-down movement on arm 1
const int servoUpDown2Pin = 10;  // Servo 2 pin for up-down movement on arm 2
const int servoLeftRightPin = 6; // Servo pin for left-right movement
const int servoGripperPin = 11;  // Servo pin for controlling the gripper

// Store the current position of each servo
float currentUpDownAngleLeft = 90.0;     // Initial position for up-down servo on the left side of the robot arm
float currentUpDownAngleRight = 90.0;    // Initial position for up-down servo on the right side of the robot arm
float currentLeftRightAngle = 90.0;      // Initial position for the left-right servo
int currentGripperAngle = 0;             // Initial position for the gripper (closed)

// Threshold for joystick movement to avoid jitter
const int joystickThreshold = 50;  // Joystick movement has to be greater than this value to be considered valid input

// Limits for up-down movement (80-degree range)
const int upDownMinAngle = 50;    // Minimum angle for up-down servo movement
const int upDownMaxAngle = 130;   // Maximum angle for up-down servo movement

// Define variables for any repeated values
const int joystickCenter = 512;   // Center position of the joystick (for detecting the joysticks eutral state)
const float servoSpeed = 0.5;     // Step size for servo angle adjustments (controls the speed of servo movement)
const int movementDelay = 20;     // Delay in milliseconds for slower movement to make sure control is stabalized

void setup() {
  // Attach the servos to its pins
  servoUpDown1.attach(servoUpDown1Pin);  // Attach servoUpDown1 to the pin for up-down movement on arm 1
  servoUpDown2.attach(servoUpDown2Pin);  // Attach servoUpDown2 to the pin for up-down movement on arm 2
  servoLeftRight.attach(servoLeftRightPin); // Attach servoLeftRight to the pin for left-right movement
  servoGripper.attach(servoGripperPin);   // Attach servoGripper to the pin for controlling the gripper

  // Initialize all servos to their initial positions
  servoUpDown1.write(currentUpDownAngleLeft);  // Set servoUpDown1 to initial position
  servoUpDown2.write(currentUpDownAngleRight); // Set servoUpDown2 to initial position
  servoLeftRight.write(currentLeftRightAngle); // Set servoLeftRight to initial position
  servoGripper.write(currentGripperAngle);     // Set gripper to initial closed position

  Serial.begin(9600); 
}

void loop() {
  // Read joystick values for X-axis (left-right) and Y-axis (up-down)
  int joyX = analogRead(joyXPin);  // Read joystick X-axis input
  int joyY = analogRead(joyYPin);  // Read joystick Y-axis input

  // Control up and down movement of the arm
  if (abs(joyY - joystickCenter) > joystickThreshold) {  // If joystick Y-axis is outside the dead zone
    if (joyY < joystickCenter - joystickThreshold) {  // Joystick moved up
      currentUpDownAngleRight = min(upDownMaxAngle, currentUpDownAngleRight + servoSpeed);  // Increase angle of right servo
      currentUpDownAngleLeft = max(upDownMinAngle, currentUpDownAngleLeft - servoSpeed);    // Decrease angle of left servo
    } else if (joyY > joystickCenter + joystickThreshold) {  // Joystick moved down
      currentUpDownAngleRight = max(upDownMinAngle, currentUpDownAngleRight - servoSpeed);  // Decrease angle of right servo
      currentUpDownAngleLeft = min(upDownMaxAngle, currentUpDownAngleLeft + servoSpeed);    // Increase angle of left servo
    }
    servoUpDown1.write(currentUpDownAngleRight);  // Update the position of the right arm servo
    servoUpDown2.write(currentUpDownAngleLeft);   // Update the position of the left arm servo
  } else {
    // If joystick is centered, stop servo movement (hold position)
    servoUpDown1.write(currentUpDownAngleRight);  // Hold the current position of the right arm servo
    servoUpDown2.write(currentUpDownAngleLeft);   // Hold the current position of the left arm servo
  }

  // Control left-right movement of the arm
  if (abs(joyX - joystickCenter) > joystickThreshold) {  // If joystick X-axis is outside the dead zone
    if (joyX < joystickCenter - joystickThreshold) {  // Joystick moved left
      currentLeftRightAngle = max(0.0f, currentLeftRightAngle - servoSpeed);  // Decrease left-right angle slowly
    } else if (joyX > joystickCenter + joystickThreshold) {  // Joystick moved right
      currentLeftRightAngle = min(180.0f, currentLeftRightAngle + servoSpeed);  // Increase left-right angle slowly
    }
    servoLeftRight.write(currentLeftRightAngle);  // Update the left-right movement servo
  } else {
    // If joystick is centered, stop servo movement (hold position)
    servoLeftRight.write(currentLeftRightAngle);  // Hold the current left-right position
  }

  // Gripper control from Serial Monitor input
  if (Serial.available() > 0) {  // If data is available on the Serial Monitor
    String userInput = Serial.readStringUntil('\n');  // Read the user input sent from Serial Monitor until newline

    if (userInput == "open") {  // If the userInput is "open"
      currentGripperAngle = 45;  // Set gripper to open position (45 degrees)
      servoGripper.write(currentGripperAngle);  // Send new angle to the gripper servo
    } else if (userInput == "close") {  // If the userInput is "close"
      currentGripperAngle = 0;   // Set gripper to closed position (0 degrees)
      servoGripper.write(currentGripperAngle);  // Send new angle to the gripper servo
    }
  }

  delay(movementDelay);  // Add a small delay to make the movement and controls smoother 
}