const int STEPPER1_STEP_Pin = 2;
const int STEPPER1_DIR_Pin  = 5;

const int STEPPER2_STEP_Pin = 3;
const int STEPPER2_DIR_Pin  = 6;

const int STEPPER3_STEP_Pin = 4;
const int STEPPER3_DIR_Pin  = 7;

const int STEPPER4_STEP_Pin = 11;
const int STEPPER4_DIR_Pin  = 12;

typedef struct {
  int pos;
  int coords[3]; //XYZ format
  int stepPin;
  int dirPin;
} StepperData;
#define STEPPER_TIMING 3000
#define STEPS_PER_REV 200
#define DIAMETER_MM (40.0)
#define CIRCUMFERENCE_MM (M_PI * DIAMETER_MM)
#define DIST_PER_STEP_MM (CIRCUMFERENCE_MM / STEPS_PER_REV)
#define STEPPER_NUM 4
double distance3D(int a[3], int b[3]) {
    int dx = a[0] - b[0];
    int dy = a[1] - b[1];
    int dz = a[2] - b[2];
    return round(sqrt(dx*dx + dy*dy + dz*dz) / DIST_PER_STEP_MM);
}
#define NEW_POS(varName, coords, stepperCoords)                          \
    varName = (int)distance3D((coords), (stepperCoords)) / DIST_PER_STEP_MM
StepperData dataSteppers[STEPPER_NUM] = {
    {0, {000, 000, 000}, STEPPER1_STEP_Pin, STEPPER1_DIR_Pin},  // stepper 1
    {0, {200, 000, 000}, STEPPER2_STEP_Pin, STEPPER2_DIR_Pin},  // stepper 2
    {0, {000, 200, 000}, STEPPER3_STEP_Pin, STEPPER3_DIR_Pin},  // stepper 3
    {0, {200, 200, 000}, STEPPER4_STEP_Pin, STEPPER4_DIR_Pin}   // stepper 4
};
void motorStep(StepperData *stepper, int newPos) {
  int newOldDistDif = stepper->pos - newPos;
  stepper->pos = newPos;
  digitalWrite(stepper->dirPin, newOldDistDif > 0);
  for(int i = 0; i < abs(newOldDistDif); i++) {
    digitalWrite(stepper->stepPin, HIGH);
    delayMicroseconds(STEPPER_TIMING);
    digitalWrite(stepper->stepPin, LOW);
    delayMicroseconds(STEPPER_TIMING);
  }
}
void motorStepAll(int coords[3]) {
  int newOldDistDif[STEPPER_NUM];
  int stepperDist[STEPPER_NUM];
  for(int i = 0; i < STEPPER_NUM; i++) {
    NEW_POS(stepperDist[i], coords, dataSteppers[i].coords);
    newOldDistDif[i] = dataSteppers[i].pos - stepperDist[i];
    digitalWrite(dataSteppers[i].dirPin, newOldDistDif[i] > 0);
    newOldDistDif[i] = abs(newOldDistDif[i]);
  }
  int done = 0;
  int stepsDone;
  while(done < STEPPER_NUM) {
    done = 0;
    for(int i = 0; i < STEPPER_NUM; i++) {
      if(stepsDone < newOldDistDif[i]) {
        digitalWrite(dataSteppers[i].stepPin, HIGH);
        delayMicroseconds(STEPPER_TIMING);
        digitalWrite(dataSteppers[i].stepPin, LOW);
        delayMicroseconds(STEPPER_TIMING);
        stepsDone++;
      } else if(stepsDone == newOldDistDif[i]) {
        done++;
        dataSteppers[i].pos = stepperDist[i];
      }
    }
  }
}

void setup() {
  Serial.begin(9600);
  for(int i = 0; i < STEPPER_NUM; i++) {
    pinMode(dataSteppers[i].dirPin,OUTPUT);
    pinMode(dataSteppers[i].stepPin,OUTPUT);
  }
}

void loop() {
  for(int i = 0; i < 200; i++) {
    motorStep(&dataSteppers[0],i);
    motorStep(&dataSteppers[1],i);
    motorStep(&dataSteppers[2],i);
    motorStep(&dataSteppers[3],i);
  }
  // motorStepAll({0,0,0});
}