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

// Stepper Motor Connections
#define DIR_PIN 12  // Direction pin for A4988
#define STEP_PIN 11 // Step pin for A4988

// Create stepper instance (AccelStepper library)
AccelStepper stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);

// Servo objects
Servo servo1;
Servo servo2;
Servo servo3;

// Potentiometer pins
int pot1Pin = A0;
int pot2Pin = A1;
int pot3Pin = A2;

void setup() {
  // Setup for stepper motor
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);

  // Initialize stepper motor with max speed and acceleration
  stepper.setMaxSpeed(1000);      // Set max speed
  stepper.setAcceleration(500);   // Set acceleration

  // Attach servos to corresponding pins
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  
  // Initialize Serial Monitor for debugging
  Serial.begin(9600);
}

void loop() {
  // Reading potentiometer values for servo positions (0-1023)
  int pot1Val = analogRead(pot1Pin);
  int pot2Val = analogRead(pot2Pin);
  int pot3Val = analogRead(pot3Pin);

  // Map potentiometer values to servo angles (0-180)
  int angle1 = map(pot1Val, 0, 1023, 0, 180);
  int angle2 = map(pot2Val, 0, 1023, 0, 180);
  int angle3 = map(pot3Val, 0, 1023, 0, 180);

  // Set servo positions
  servo1.write(angle1);
  servo2.write(angle2);
  servo3.write(angle3);

  // Debugging information
  Serial.print("Servo1 Angle: ");
  Serial.println(angle1);
  Serial.print("Servo2 Angle: ");
  Serial.println(angle2);
  Serial.print("Servo3 Angle: ");
  Serial.println(angle3);

  // Stepper motor control
  // Set direction based on potentiometer 1
  if (pot1Val > 512) {
    stepper.setSpeed(500);  // Positive speed for one direction
  } else {
    stepper.setSpeed(-500); // Negative speed for reverse direction
  }

  // Move the stepper motor based on speed set above
  stepper.runSpeed();

  // Small delay to avoid too rapid changes
  delay(50);
}