#include <AccelStepper.h>

// Motor Pins
const int stepPin1 = 3;
const int dirPin1 = 2;
const int stepPin2 = 5;
const int dirPin2 = 4;
const int stepPin3 = 6;
const int dirPin3 = 7;

// Constants
const int speed = 20;
const int waveSteps = 60;
const int maxSpeed = 1000;
const int acceleration = 1000;
unsigned long previousTime = 0UL;
const unsigned long timeDelay = 1000000UL;

// Define Motor Objects
AccelStepper stepper1(1, stepPin1, dirPin1);
AccelStepper stepper2(1, stepPin2, dirPin2);
AccelStepper stepper3(1, stepPin3, dirPin3);

// Define Motor Structures
struct motorStructure{
  bool main;
  bool start;
  bool direction;};

motorStructure motor1;
motorStructure motor2;
motorStructure motor3;

void setup() {
  Serial.begin(9600);
  // Initialize Motors
  motorInit(stepper1);
  motorInit(stepper2);
  motorInit(stepper3);

  // Fill Motor Structures
  motor1 = {false, true, true};
  motor2 = {false, true, true};
  motor3 = {false, true, true};

  // Delay for better timing
  delay(1000);
}

void loop() {
  // Get Current Time
  unsigned long currentTime = micros();

  // Start Motor 1
  if(currentTime - previousTime >= timeDelay){
    motor1.main = true;}

  // Start Motor 2
  if(currentTime - previousTime >= 2*timeDelay){
    motor2.main = true;}

  // Start Motor 3
  if(currentTime - previousTime >= 3*timeDelay){
    motor3.main = true;}

  // Execute Main Motion Function
  motor1 = moveMotor(stepper1, motor1);
  motor2 = moveMotor(stepper2, motor2);
  motor3 = moveMotor(stepper3, motor3);
}



/// FUNCTIONS ///
void motorInit(AccelStepper &stepperMotor){
  // Initialize Motors w/ Max Speed and Acceleration
  stepperMotor.setMaxSpeed(maxSpeed);
  stepperMotor.setAcceleration(acceleration);
}

motorStructure moveMotor(AccelStepper &stepperMotor, struct motorStructure x){
  bool main = x.main;
  bool start = x.start;
  bool direction = x.direction;

  // Only Execute if 'main' boolean is triggered
  if(main){
    // Start First Motion as its own Action
    if(start){
      int directionSign = plusMinus(direction);
      stepperMotor.setSpeed(speed);
      stepperMotor.moveTo(directionSign * waveSteps);
      stepperMotor.runSpeedToPosition();
      x.start = false;
      }
    else{
      // Check to See if Motor is at End of Motion Before Re-Assigning New Position
      // [non-blocking workaround]
      if(stepperMotor.distanceToGo() == 0){
        direction = !direction;
        x.direction = direction;
        int directionSign = plusMinus(direction);
        stepperMotor.moveTo(directionSign * waveSteps);
      }
      stepperMotor.setSpeed(speed);
      stepperMotor.runSpeedToPosition();
    }
  }
  return x;
}

int plusMinus(bool direction){
  // Convert Boolean (0,1) to integers (-1,+1)
  if(direction){
    return +1;}
  else{
    return -1;}
}