#include <AccelStepper.h>

// Motor Pins
#define stepPin1 3
#define dirPin1 2
#define stepPin2 5
#define dirPin2 4
#define stepPin3 6
#define dirPin3 7
#define stepPin4 10
#define dirPin4 11

// Pin Arrays
int stepPins[] = {stepPin1, stepPin2, stepPin3, stepPin4};
int dirPins[] = {dirPin1, dirPin2, dirPin3, dirPin4};
int numMotors = sizeof(stepPins) / sizeof(int);

// Button and LED Pins
const int startPin = A1;
const int stopPin = A0;
const int onLED = 8;
const int offLED = 9;

// Constants
int previousStart = HIGH;         
int previousStop = HIGH; 
const int maxSpeed = 1000;
const int acceleration = 1000;


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

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

motorStructure motor1;
motorStructure motor2;
motorStructure motor3;
motorStructure motor4;

// Motion
int pullSpeed = 10;
int pushSpeed = 500;

int pullStep = 50;
int pushStep = -5;

int go;

// Initial Settings
bool initMain = false;
bool initStart = true;
bool initDone = false;
bool initFinish = false;

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

  // Set Button and LED Pin Modes
  pinMode(startPin, INPUT);
  pinMode(stopPin, INPUT);
  pinMode(onLED, OUTPUT);
  pinMode(offLED, OUTPUT);

  // Set Motor Pin Modes
  for(int i = 0; i < numMotors; i++){
    pinMode(stepPins[i], OUTPUT);
    pinMode(dirPins[i], OUTPUT);}
  
  // Initialize LEDs
  digitalWrite(offLED, HIGH);
  digitalWrite(onLED, LOW);

  // Initialize Motors
  motorInit(stepper1);
  motorInit(stepper2);
  motorInit(stepper3);
  motorInit(stepper4);

  motor1 = {initMain, initStart, initDone};
  motor2 = {initMain, initStart, initDone};
  motor3 = {initMain, initStart, initDone};
  motor4 = {initMain, initStart, initDone};
}

void loop() {
  // Check if Start Button has Been Pressed
  int currentStart = digitalRead(startPin);
  if((currentStart != previousStart) && (currentStart == HIGH)){
    // Start Motion
    go = 1;
    Serial.println("Go");

    // Turn on LEDs
    digitalWrite(offLED, LOW);
    digitalWrite(onLED, HIGH);    

    // Begin Motor Motion Loop
    while(go == 1){
      // Start Motors
      motor1.main = true;
      motor2.main = true;
      motor3.main = true;
      motor4.main = true;

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

      // Wait Until All Motors Are Done
      if(motor1.finish && motor2.finish && motor3.finish && motor4.finish){
        Serial.println("Done");

        // Reset Motors
        motor1 = {initMain, initStart, initDone};
        motor2 = {initMain, initStart, initDone};
        motor3 = {initMain, initStart, initDone};
        motor4 = {initMain, initStart, initDone};

        // Turn on OFF LED
        digitalWrite(offLED, HIGH);
        digitalWrite(onLED, LOW);

        // Exit Motion Loop
        go = 0;
      }
    }
  }
  previousStart = currentStart;
}



/// 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){

  // Only Execute if 'main' boolean is triggered
  if(x.main){
    x.finish = false;
    // Start First Motion as its own Action
    if(x.start){
      setPositionSpeed(stepperMotor, pullStep, pullSpeed);
      stepperMotor.runSpeedToPosition();
      x.start = false;
    }
    else{
      // Check to See if Motor is at End of Motion
      if(stepperMotor.distanceToGo() == 0){
        if(!x.done){
          setPositionSpeed(stepperMotor, pushStep, pushSpeed);
          x.done = true;
        }
        else{
          setPositionSpeed(stepperMotor, 0, pullSpeed);
          if(stepperMotor.distanceToGo() == 0){
            x.finish = true;
          }
        }
      }  
      stepperMotor.runSpeedToPosition();
    }
  }
  return x;
}

void setPositionSpeed(AccelStepper &stepperMotor, int position, int speed){
  stepperMotor.moveTo(position);
  stepperMotor.setSpeed(speed);
}

void homeMotor(AccelStepper &stepperMotor){
  if(stepperMotor.distanceToGo() == 0){
    setPositionSpeed(stepperMotor, 0, pullSpeed);
  }  
  stepperMotor.runSpeedToPosition();
}