#include <AccelStepper.h>
#include <MultiStepper.h>
#include <math.h>

// -------------------------------------------------------------------------
// Adjustable Parameters
// -------------------------------------------------------------------------
const long amplitudeFull = 160;          // Full amplitude for left/right motors (in steps)
const float middleAmplitudeFactor = 0.85;  // Scale for middle motors (e.g., 80% of full amplitude)
const unsigned long cycleTime = 5000;     // Duration (in milliseconds) of one full wave cycle

int maxSpeed = 400;  // Maximum speed
int acceleration = 600;

// -------------------------------------------------------------------------
// Pin Assignments for 5 Motors
// -------------------------------------------------------------------------
const byte numMotors = 5;

// Example pin assignments for 5 motors (adjust these as needed)
const byte enPins[numMotors]   = {43, 45, 47, 49, 51};  // Enable pins
const byte dirPins[numMotors]  = {25, 29, 33, 37, 41};  // Direction pins
const byte stepPins[numMotors] = {23, 27, 31, 35, 39};  // Step pins// -------------------------------------------------------------------------
// Motor Configuration
// -------------------------------------------------------------------------
#define motorInterfaceType 1  // Typical driver interface

// Create individual AccelStepper objects for each motor
AccelStepper stepper0(motorInterfaceType, stepPins[0], dirPins[0]);
AccelStepper stepper1(motorInterfaceType, stepPins[1], dirPins[1]);
AccelStepper stepper2(motorInterfaceType, stepPins[2], dirPins[2]);
AccelStepper stepper3(motorInterfaceType, stepPins[3], dirPins[3]);
AccelStepper stepper4(motorInterfaceType, stepPins[4], dirPins[4]);

// Create a MultiStepper object to coordinate simultaneous moves
MultiStepper steppers;

void setup() {
  Serial.begin(9600);

  for (int i = 0; i < numMotors; i++) {
    pinMode(enPins[i], OUTPUT);
    digitalWrite(enPins[i], LOW);  // Assuming LOW enables the motor driver
  }
  // Set maximum speed and acceleration for each motor (adjust as needed)
  stepper0.setMaxSpeed(maxSpeed);
  stepper0.setAcceleration(acceleration);

  stepper1.setMaxSpeed(maxSpeed);
  stepper1.setAcceleration(acceleration);

  stepper2.setMaxSpeed(maxSpeed);
  stepper2.setAcceleration(acceleration);

  stepper3.setMaxSpeed(maxSpeed);
  stepper3.setAcceleration(acceleration);

  stepper4.setMaxSpeed(maxSpeed);
  stepper4.setAcceleration(acceleration);

  // Set the initial position of all motors to 0 (this is our reference)
  stepper0.setCurrentPosition(0);
  stepper1.setCurrentPosition(0);
  stepper2.setCurrentPosition(0);
  stepper3.setCurrentPosition(0);
  stepper4.setCurrentPosition(0);

  // Add all steppers to the MultiStepper object for simultaneous control.
  steppers.addStepper(stepper0);
  steppers.addStepper(stepper1);
  steppers.addStepper(stepper2);
  steppers.addStepper(stepper3);
  steppers.addStepper(stepper4);
}

void loop() {
  // Calculate the current phase of the wave based on cycleTime.
  // Phase varies between 0 and 2*PI.
  unsigned long currentTime = millis();
  float phase = (currentTime % cycleTime) / (float)cycleTime * 2.0 * PI;

  // Calculate target positions for each motor based on sine waves.
  // The left (motor 0) and right (motor 4) motors use the full amplitude.
  // The middle motors (1, 2, 3) use a reduced amplitude with slight phase offsets.
  long targetPositions[5];
  targetPositions[0] = amplitudeFull * sin(phase);
  targetPositions[1] = amplitudeFull * middleAmplitudeFactor * sin(phase + 0.3);
  targetPositions[2] = amplitudeFull * middleAmplitudeFactor * sin(phase + 0.6);
  targetPositions[3] = amplitudeFull * middleAmplitudeFactor * sin(phase + 0.9);
  targetPositions[4] = amplitudeFull * sin(phase);

  // Command the MultiStepper to move all motors simultaneously to the target positions.
  steppers.moveTo(targetPositions);

  // runSpeedToPosition() blocks until all motors reach their target positions.
  steppers.runSpeedToPosition();
}