#include <Servo.h>
#include <AccelStepper.h>
#include <Stepper.h>

// Servo definitions
Servo servo1; // Create servo object for servo 1
Servo servo2; // Create servo object for servo 2
Servo servo3; // Create servo object for servo 3

// Stepper motor definitions
#define motorPin1 0
#define motorPin2 1
#define motorPin3 2
#define motorPin4 3

AccelStepper stepper(AccelStepper::FULL4WIRE, motorPin1, motorPin2, motorPin3, motorPin4);

int joyX = A0; // Analog input pin for joystick X-axis
int joyY = A1; // Analog input pin for joystick Y-axis
int joy2Y = A3; // Analog input pin for joystick 2 Y-axis

int centerValue = 512; // Assuming 10 bit ADC (0-1023), the center is around 512
int lastServoAngle = 90; // start position of servo
int threshold = 10; // ignore minor movements

void setup() 
{
  servo1.attach(9); // Attach servo 1 to digital pin 9
  servo2.attach(10); // Attach servo 2 to digital pin 10
  servo3.attach(11); // Attach servo 3 to digital pin 11

  servo1.write(lastServoAngle);

  stepper.setMaxSpeed(1000); // Set max speed for the stepper motor
}

void loop() 
{
  int valX = analogRead(joyX); // Read joystick X-axis
  int valY = analogRead(joyY); // Read joystick Y-axis

  // Map the joystick 1 values to servo angles (0 to 180 degrees)
  int servo2Angle = map(valX, 0, 1023, 0, 180);

  // Map the joystick 2 values to servo angles (0 to 180 degrees)
  int servo3Angle = map(valY, 0, 1023, 0, 180);

  // Write the mapped angle to the servos
  servo2.write(servo2Angle);
  servo3.write(servo3Angle);

  if(abs(valY - centerValue) > threshold)
  {
    int change = valY - centerValue;
    int servoChange = map(change, -512, 512, -90, 90);
    int newServoAngle = lastServoAngle + servoChange;
    newServoAngle = constrain(newServoAngle, 0, 180);
    servo1.write(newServoAngle);
    lastServoAngle = newServoAngle;
  }


  // Control stepper motor based on joystick X-axis
  int stepperSpeed = map(valX, 0, 1023, -1000, 1000);
  stepper.setSpeed(stepperSpeed);
  stepper.runSpeed(); // Run the motor at the set speed

  delay(15);
}