// I was wondering what the MultiStepper is about.
// Wokwi is the perfect tool to test that.
//
// The MultiStepper moves several stepper motors at the same time.
// But the number of steps does not have to be the same.
// The library adjusts the speed, and all stepper motors
// reach the endpoint at the same time.

#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() 
{
  // Serial.begin(115200);

  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);
    
    // It seems that the acceleration setting
    // does not work ? Something to investigate.
    // stepper[i].setAcceleration(1);

    // Assign the stepper to the MultiStepper
    multi.addStepper(stepper[i]);
  }

  // Initialize the AccelStepper motors
  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 stepper motors going (all the multisteppers).
  stepper1.run();   // keep the stepper motor going.
  stepper2.run();   // keep the stepper motor going.

  bounceForward.update(); // keep the Bounce2 library going
  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 a led.
  // Is it proof that the sketch does not
  // use delays and does not wait.
  if(currentMillis - previousMillis >= interval)
  {
    previousMillis = currentMillis;

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

    // The millis-timer to blink the leds
    // is also used for the normal stepper motors,
    // as if they have a mind of their own.
    if(random(0,5) == 0)
    {
      long newPos = random(-400,400);
      if(random(0,20) == 0)
        newPos = 0;
      stepper1.moveTo(newPos);
    }
    if(random(0,5) == 0)
    {
      long newPos = random(-400,400);
      if(random(0,20) == 0)
        newPos = 0;
      stepper2.moveTo(newPos);
    }
  }
}