// Include the AccelStepper library:
#include "AccelStepper.h"

// Define stepper motor connections and motor interface type. 
// Motor interface type must be set to 1 when using a driver
#define dirPin1 13
#define stepPin1 14

#define dirPin2 10
#define stepPin2 11

#define dirPin3 40
#define stepPin3 41

#define dirPin4 37
#define stepPin4 38

#define motorInterfaceType 1
String clockwise = "clockwise";
String counterclockwise = "counterclockwise";
String dir1 = "";
String dir2 = "";
String dir3 = "";
String dir4 = "";

bool start = false;

// adjust stepper configuration here:
#define stepper_max_speed 1000
#define stepper_acceleration 50
#define stepper_speed 200

// enter steps here:
double steps1 = 1;
double steps2 = 303;
double steps3 = -55;
double steps4 = -1237;

// Create a new instance of the AccelStepper class:
AccelStepper stepper1 = AccelStepper(motorInterfaceType, stepPin1, dirPin1);
AccelStepper stepper2 = AccelStepper(motorInterfaceType, stepPin2, dirPin2);
AccelStepper stepper3 = AccelStepper(motorInterfaceType, stepPin3, dirPin3);
AccelStepper stepper4 = AccelStepper(motorInterfaceType, stepPin4, dirPin4);

void setup() {
  Serial.begin(115200);
  stepper1.setMaxSpeed(stepper_max_speed);
  stepper1.setAcceleration(stepper_acceleration);
  
  stepper2.setMaxSpeed(stepper_max_speed);
  stepper2.setAcceleration(stepper_acceleration);

  stepper3.setMaxSpeed(stepper_max_speed);
  stepper3.setAcceleration(stepper_acceleration);

  stepper4.setMaxSpeed(stepper_max_speed);
  stepper4.setAcceleration(stepper_acceleration);
}

void loop() { 

  if (start == false) {
    // positive steps: stepper turn clockwise; negative steps: stepper turns counterclockwise
    if (steps1 >= 0) {
      dir1 = clockwise;
    } else {
      dir1 = counterclockwise;
    }
    if (steps2 >= 0) {
      dir2 = clockwise;
    } else {
      dir2 = counterclockwise;
    }
    if (steps3 >= 0) {
      dir3 = clockwise;
    } else {
      dir3 = counterclockwise;
    }
    if (steps4 >= 0) {
      dir4 = clockwise;
    } else {
      dir4 = counterclockwise;
    }
    Serial.print("stepper1 turning ");
    Serial.print(abs(int(steps1)));
    Serial.print(" steps ");
    Serial.println(dir1);
    
    Serial.print("stepper2 turning ");
    Serial.print(abs(int(steps2)));
    Serial.print(" steps ");
    Serial.println(dir2);

    Serial.print("stepper3 turning ");
    Serial.print(abs(int(steps3)));
    Serial.print(" steps ");
    Serial.println(dir3);

    Serial.print("stepper4 turning ");
    Serial.print(abs(int(steps4)));
    Serial.print(" steps ");
    Serial.println(dir4);
    move_stepper(abs(steps1), dir1, abs(steps2), dir2, abs(steps3), dir3, abs(steps4), dir4);
    start = true;
  }

}

void move_stepper(double steps1, String dir1, double steps2, String dir2, double steps3, String dir3, double steps4, String dir4) {

  double smallest_steps;
  double temp;

  // check if all values are zero
  if (steps1 == 0 && steps2 == 0 && steps3 == 0 && steps4 == 0) {
    Serial.println("all steps are zero");
    return;
  } 
      // determine min step number excluding zeros
      double array[4] = {steps1, steps2, steps3, steps4};
      while (array[3] > array[2] || array[2] > array[1] || array[1] > array[0]) {
        if (array[1] > array[0])  {
                    temp = array[0];
            array[0] = array[1];
            array[1] = temp;
            }

            if (array[2] > array[1]) {
            temp = array[1];
            array[1] = array[2];
            array[2] = temp;
            }

            if (array[3] > array[2]) {
          temp = array[2];
          array[2] = array[3];
          array[3] = temp;
            }

      }
      for (int i = 0; i < 4; i++) {
        if (array[i] != 0) {
          smallest_steps = array[i];
          break;
        }
      }

  double steps1_req_per_loop = steps1/smallest_steps;
  double steps2_req_per_loop = steps2/smallest_steps;
  double steps3_req_per_loop = steps3/smallest_steps;
  double steps4_req_per_loop = steps4/smallest_steps;

  double steps1_req_per_loop_total = 0;
  double steps2_req_per_loop_total = 0;
  double steps3_req_per_loop_total = 0;
  double steps4_req_per_loop_total = 0;

  int steps1_new = 0;
  int steps2_new = 0;
  int steps3_new = 0;
  int steps4_new = 0;

  double steps1_new_total = 0;
  double steps2_new_total = 0;
  double steps3_new_total = 0;
  double steps4_new_total = 0;

  // Set the current position to 0:
  stepper1.setCurrentPosition(0);
  stepper2.setCurrentPosition(0);
  stepper3.setCurrentPosition(0);
  stepper4.setCurrentPosition(0);

  for (int i = 1; i <= int(smallest_steps); i++) {
    // calculate needed steps
    steps1_req_per_loop_total = i*steps1_req_per_loop;
    steps2_req_per_loop_total = i*steps2_req_per_loop;
    steps3_req_per_loop_total = i*steps3_req_per_loop;
    steps4_req_per_loop_total = i*steps4_req_per_loop;

    steps1_new = int(steps1_req_per_loop_total-steps1_new_total);
    steps1_new_total = steps1_new_total + steps1_new;
    
    steps2_new = int(steps2_req_per_loop_total-steps2_new_total);
    steps2_new_total = steps2_new_total + steps2_new;
    
    steps3_new = int(steps3_req_per_loop_total-steps3_new_total);
    steps3_new_total = steps3_new_total + steps3_new;
    
    steps4_new = int(steps4_req_per_loop_total-steps4_new_total);
    steps4_new_total = steps4_new_total + steps4_new;

    // move stepper
    // stepper 1
    if (dir1 == clockwise) {
      while(stepper1.currentPosition() != steps1_new)
      {
        stepper1.setSpeed(stepper_speed);
        stepper1.runSpeed();  
      }
    } else {
        while(stepper1.currentPosition() != -steps1_new)
        {
          stepper1.setSpeed(-stepper_speed);
          stepper1.runSpeed();  
        }
    }
    stepper1.setCurrentPosition(0);
    
    // stepper 2
    if (dir2 == clockwise) {
      while(stepper2.currentPosition() != steps2_new)
      {
        stepper2.setSpeed(stepper_speed);
        stepper2.runSpeed();  
      }
    } else {
        while(stepper2.currentPosition() != -steps2_new)
        {
          stepper2.setSpeed(-stepper_speed);
          stepper2.runSpeed();  
        }
    }
    stepper2.setCurrentPosition(0);

    // stepper 3
    if (dir3 == clockwise) {
      while(stepper3.currentPosition() != steps3_new)
      {
        stepper3.setSpeed(stepper_speed);
        stepper3.runSpeed();  
      }
    } else {
        while(stepper3.currentPosition() != -steps3_new)
        {
          stepper3.setSpeed(-stepper_speed);
          stepper3.runSpeed();  
        }
    }
    stepper3.setCurrentPosition(0);

    // stepper 4
    if (dir4 == clockwise) {
      while(stepper4.currentPosition() != steps4_new)
      {
        stepper4.setSpeed(stepper_speed);
        stepper4.runSpeed();  
      }
    } else {
        while(stepper4.currentPosition() != -steps4_new)
        {
          stepper4.setSpeed(-stepper_speed);
          stepper4.runSpeed();  
        }
    }
    stepper4.setCurrentPosition(0);

  }


}