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

// Pin definitions for the button and servo
const int buttonPin = 2;
const int servoPin = 11;

// Servo object
Servo myServo;
bool buttonPressed = false;
int buttonState = 0;
int lastButtonState = 0;
bool servoPosition = false; // false = start position (0°), true = end position (180°)

// Stepper motor pin definitions
const int stepPin1 = 3; 
const int dirPin1 = 4;
const int potentiometerPin1 = A0;  // Potentiometer for Motor 1

const int stepPin2 = 5; 
const int dirPin2 = 6;
const int potentiometerPin2 = A1;  // Potentiometer for Motor 2

const int stepPin3 = 7; 
const int dirPin3 = 8;
const int potentiometerPin3 = A2;  // Potentiometer for Motor 3

const int stepPin4 = 9; 
const int dirPin4 = 10;
const int potentiometerPin4 = A3;  // Potentiometer for Motor 4

// AccelStepper objects for each stepper motor
AccelStepper stepper1(AccelStepper::DRIVER, stepPin1, dirPin1);
AccelStepper stepper2(AccelStepper::DRIVER, stepPin2, dirPin2);
AccelStepper stepper3(AccelStepper::DRIVER, stepPin3, dirPin3);
AccelStepper stepper4(AccelStepper::DRIVER, stepPin4, dirPin4);

// Max angles for each motor
float maxAngle1 = 90.0;  // Max angle for motor 1
float maxAngle2 = 180.0; // Max angle for motor 2
float maxAngle3 = 45.0;  // Max angle for motor 3
float maxAngle4 = 360.0; // Max angle for motor 4

// Calculating max steps for each motor
int maxSteps1 = maxAngle1 / 1.8;  // Steps for motor 1
int maxSteps2 = maxAngle2 / 1.8;  // Steps for motor 2
int maxSteps3 = maxAngle3 / 1.8;  // Steps for motor 3
int maxSteps4 = maxAngle4 / 1.8;  // Steps for motor 4

void setup() {
  // Initialize button pin and servo
  pinMode(buttonPin, INPUT);
  myServo.attach(servoPin);
  myServo.write(0);  // Start at 0 degrees

  // Initialize stepper motors
  stepper1.setMaxSpeed(1000);
  stepper1.setAcceleration(500);
  
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(500);
  
  stepper3.setMaxSpeed(1000);
  stepper3.setAcceleration(500);
  
  stepper4.setMaxSpeed(1000);
  stepper4.setAcceleration(500);

  Serial.begin(9600);
}

void loop() {
  // Read button state
  buttonState = digitalRead(buttonPin);

  // Detect a button press (simple state change detection)
  if (buttonState != lastButtonState) {
    if (buttonState == HIGH) {
      buttonPressed = true;
    }
  }
  lastButtonState = buttonState;

  // Toggle servo position on button press
  if (buttonPressed) {
    servoPosition = !servoPosition; // Toggle the position
    if (servoPosition) {
      myServo.write(180);  // Move to full position
    } else {
      myServo.write(0);    // Return to start position
    }
    delay(100);  // Debounce delay
    buttonPressed = false;  // Reset the button press flag
  }

  // Potentiometer control for each stepper motor
  int potentiometerValue1 = analogRead(potentiometerPin1);
  int targetPosition1 = map(potentiometerValue1, 0, 1023, -maxSteps1, maxSteps1);
  stepper1.moveTo(targetPosition1);
  stepper1.run();  // Non-blocking call to step the motor

  int potentiometerValue2 = analogRead(potentiometerPin2);
  int targetPosition2 = map(potentiometerValue2, 0, 1023, -maxSteps2, maxSteps2);
  stepper2.moveTo(targetPosition2);
  stepper2.run();  // Non-blocking call to step the motor

  int potentiometerValue3 = analogRead(potentiometerPin3);
  int targetPosition3 = map(potentiometerValue3, 0, 1023, -maxSteps3, maxSteps3);
  stepper3.moveTo(targetPosition3);
  stepper3.run();  // Non-blocking call to step the motor

  int potentiometerValue4 = analogRead(potentiometerPin4);
  int targetPosition4 = map(potentiometerValue4, 0, 1023, -maxSteps4, maxSteps4);
  stepper4.moveTo(targetPosition4);
  stepper4.run();  // Non-blocking call to step the motor

  delay(50);  // Small delay to allow potentiometer reading
}