#include <Servo.h>

// Define pins for the servos and joysticks
const int servoZPin = 9;            // Pin for Z-axis servo
const int servoYPin = 10;           // Pin for Y-axis servo
const int servoExtensionPin = 11;   // Pin for extension servo
const int servo4Pin = 12;           // Pin for 4th servo

const int joystickZPin = A0;        // Joystick for Z-axis (X-axis)
const int joystickYPin = A1;        // Joystick for Y-axis (Y-axis)
const int joystickExtensionPin = A2;// Joystick for extension (X-axis)
const int joystick4Pin = A3;        // Joystick for 4th servo (Y-axis)

// Create Servo objects
Servo servoZ;
Servo servoY;
Servo servoExtension;
Servo servo4; // 4th Servo

// Variables to store the servo positions
int servoZPos = 90;           // Start at the middle position
int servoYPos = 90;
int servoExtensionPos = 90;
int servo4Pos = 90;           // 4th Servo starting position

// Define a threshold for joystick's neutral position
const int centerThreshold = 20;

// Define the step size for precise movement
const int stepSize = 1;  // Move 1 degree at a time

void setup() {
  // Attach servos to their respective pins
  servoZ.attach(servoZPin);
  servoY.attach(servoYPin);
  servoExtension.attach(servoExtensionPin);
  servo4.attach(servo4Pin);   // Attach 4th servo

  // Initialize servo positions
  servoZ.write(servoZPos);
  servoY.write(servoYPos);
  servoExtension.write(servoExtensionPos);
  servo4.write(servo4Pos);    // Initialize 4th servo position
}

void loop() {
  // Read joystick values
  int joystickZVal = analogRead(joystickZPin);
  int joystickYVal = analogRead(joystickYPin);
  int joystickExtensionVal = analogRead(joystickExtensionPin);
  int joystick4Val = analogRead(joystick4Pin);  // Read value for 4th joystick

  // Calculate deviations from center
  int deviationZ = joystickZVal - 512;
  int deviationY = joystickYVal - 512;
  int deviationExtension = joystickExtensionVal - 512;
  int deviation4 = joystick4Val - 512; // Deviation for 4th servo

  // Control Z-axis servo
  if (abs(deviationZ) > centerThreshold) {
    if (deviationZ > 0 && servoZPos < 180) {
      servoZPos += stepSize;  // Move clockwise
    } else if (deviationZ < 0 && servoZPos > 0) {
      servoZPos -= stepSize;  // Move counterclockwise
    }
  }

  // Control Y-axis servo
  if (abs(deviationY) > centerThreshold) {
    if (deviationY > 0 && servoYPos < 180) {
      servoYPos += stepSize;  // Move clockwise
    } else if (deviationY < 0 && servoYPos > 0) {
      servoYPos -= stepSize;  // Move counterclockwise
    }
  }

  // Control extension servo
  if (abs(deviationExtension) > centerThreshold) {
    if (deviationExtension > 0 && servoExtensionPos < 180) {
      servoExtensionPos += stepSize;  // Move clockwise
    } else if (deviationExtension < 0 && servoExtensionPos > 0) {
      servoExtensionPos -= stepSize;  // Move counterclockwise
    }
  }

  // Control 4th servo
  if (abs(deviation4) > centerThreshold) {
    if (deviation4 > 0 && servo4Pos < 180) {
      servo4Pos += stepSize;  // Move clockwise
    } else if (deviation4 < 0 && servo4Pos > 0) {
      servo4Pos -= stepSize;  // Move counterclockwise
    }
  }

  // Write positions to servos
  servoZ.write(servoZPos);
  servoY.write(servoYPos);
  servoExtension.write(servoExtensionPos);
  servo4.write(servo4Pos); // Update 4th servo

  // Small delay for smooth movement
  delay(15);
}