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

// Pin assignments (3 motors)
const byte enPins[] = { 8, 8, 8 };
const byte dirPins[] = { 25, 29, 33 };
const byte stepPins[] = { 23, 27, 31 };

// Motor parameters
#define MAX_SPEED 1200
#define ACCELERATION 1000
const long downSteps = 300;
const long upSteps = 6000;
const long center = (upSteps + downSteps) / 2;
const long amplitude = 1000;  // Amplitude for wave motion

#define motorInterfaceType 1
#define NUM_MOTORS 3

AccelStepper steppers[NUM_MOTORS] = {
  AccelStepper(motorInterfaceType, stepPins[0], dirPins[0]),
  AccelStepper(motorInterfaceType, stepPins[1], dirPins[1]),
  AccelStepper(motorInterfaceType, stepPins[2], dirPins[2])
};

void setup() {
  for (int i = 0; i < NUM_MOTORS; i++) {
    pinMode(enPins[i], OUTPUT);
    digitalWrite(enPins[i], LOW);
    steppers[i].setMaxSpeed(MAX_SPEED);
    steppers[i].setAcceleration(ACCELERATION);
    steppers[i].setCurrentPosition(center);
  }
}

void loop() {
  static unsigned long startTime = millis();
  unsigned long elapsedTime = millis() - startTime;
  const unsigned long cycleTime = 5000;                           // 5-second cycle
  float t = (float)(elapsedTime % cycleTime) / cycleTime * (PI);  // Full wave every 5 sec

  // Motor 0 & 2 smoothly swap positions
  steppers[0].moveTo(constrain(center + amplitude * sin(t), downSteps, upSteps));
  steppers[2].moveTo(constrain(center - amplitude * sin(t), downSteps, upSteps));

  // Motor 1 oscillates up and down continuously
  steppers[1].moveTo(constrain(center + amplitude * cos(t), downSteps, upSteps));

  // Update motors
  for (int i = 0; i < NUM_MOTORS; i++) {
    steppers[i].run();
  }
}