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

const int pinButtonForward = 2;
const int pinButtonBackward = 3;
const int pinLed = 13;

#define NUM_STEPPERS 3

// Create three AccelStepper objects.
// These three will be combined in the MultiStepper.
AccelStepper stepper[NUM_STEPPERS] =
{
  AccelStepper(AccelStepper::DRIVER, 9, 8), // step, dir
  AccelStepper(AccelStepper::DRIVER, 7, 6), // step, dir
  AccelStepper(AccelStepper::DRIVER, 5, 4), // step, dir
};

// Two extra stepper motors that are not part of the MultiStepper.
AccelStepper stepper1(AccelStepper::DRIVER, A4, A3);
AccelStepper stepper2(AccelStepper::DRIVER, A2, A1);

// MultiStepper object for maximum 10 steppers.
MultiStepper multi;

long position[NUM_STEPPERS] = {0};

Bounce bounceForward = Bounce();
Bounce bounceBackward = Bounce();

unsigned long previousMillis;
const unsigned long interval = 300UL;
bool led = false;

void setup() 
{
  pinMode(pinButtonForward, INPUT_PULLUP);
  pinMode(pinButtonBackward, INPUT_PULLUP);
  pinMode(pinLed, OUTPUT);

  // Initialize the MultiStepper
  for (int i = 0; i < NUM_STEPPERS; i++)
  {
    stepper[i].setMaxSpeed(300);
    // Acceleration does not apply to MultiStepper
    // stepper[i].setAcceleration(1);

    multi.addStepper(stepper[i]);
  }

  // Initialize the AccelStepper motors (not in MultiStepper)
  stepper1.setMaxSpeed(200);
  stepper1.setAcceleration(20);
  stepper2.setMaxSpeed(200);
  stepper2.setAcceleration(20);

  // Initialize the buttons with the Bounce2 library.
  bounceForward.attach(pinButtonForward, INPUT_PULLUP);
  bounceForward.interval(25);
  bounceBackward.attach(pinButtonBackward, INPUT_PULLUP);
  bounceBackward.interval(25);
}

void loop() 
{
  unsigned long currentMillis = millis();

  multi.run();      // keep the multisteppers running
  stepper1.run();   // keep stepper1 running
  stepper2.run();   // keep stepper2 running

  bounceForward.update(); // update debounced buttons
  bounceBackward.update();

  if (bounceForward.fell())
  {
    position[0] += 600;
    position[1] += 400;
    position[2] += 200;
    multi.moveTo(position);
  }

  if (bounceBackward.fell())
  {
    position[0] -= 600;
    position[1] -= 400;
    position[2] -= 200;
    multi.moveTo(position);
  }

  // Blink LED without blocking
  if (currentMillis - previousMillis >= interval)
  {
    previousMillis = currentMillis;

    digitalWrite(pinLed, led ? HIGH : LOW);
    led = !led;

    // Random movement for stepper1
    if (random(0, 5) == 0)
    {
      long newPos = random(-400, 400);
      if (random(0, 20) == 0)
        newPos = 0;
      stepper1.moveTo(newPos);
    }

    // Random movement for stepper2
    if (random(0, 5) == 0)
    {
      long newPos = random(-400, 400);
      if (random(0, 20) == 0)
        newPos = 0;
      stepper2.moveTo(newPos);
    }
  }
}