long unsigned int numsteps_y = 800; //800*4;
long unsigned int numsteps_x = 600; //1500;
long unsigned int numsteps_z = 100; //800/2;
long unsigned int numsteps_yaw = 200;//200;//500;

float d_time = 1000; //(min val is 1)

const int xPins[] = {54, 55, 38}; // step, dir, enable //uno: {2,5}
const int yPins[] = {60, 61, 56}; // step, dir, enable //uno: {3,6}
const int zPins[] = {46, 48, 62}; // step, dir, enable //uno: {4,7}
const int yawPins[] = {26, 20, 24}; // (E0) step, dir, enable //uno: {12,13} 

void stepping(int stepX, int stepY, int stepZ, int stepYaw, long unsigned int nsteps_x, long unsigned int nsteps_y, long unsigned int nsteps_z, long unsigned int nsteps_yaw, float delayTime);

void steps(int stepPin, float delayTime);


void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  pinMode(xPins[0], OUTPUT); // step pins
  pinMode(xPins[1], OUTPUT); // dir pins
  pinMode(xPins[2], OUTPUT); // en pins

  pinMode(yPins[0], OUTPUT); // step pins
  pinMode(yPins[1], OUTPUT); // dir pins
  pinMode(yPins[2], OUTPUT); // en pins

  pinMode(zPins[0], OUTPUT); // step pins
  pinMode(zPins[1], OUTPUT); // dir pins
  pinMode(zPins[2], OUTPUT); // en pins

  pinMode(yawPins[0], OUTPUT); // step pins
  pinMode(yawPins[1], OUTPUT); // dir pins
  pinMode(yawPins[2], OUTPUT); // en pins

  digitalWrite(xPins[2],LOW);
  digitalWrite(yPins[2],LOW);
  digitalWrite(zPins[2],LOW);
  digitalWrite(yawPins[2],LOW);

  Serial.println("CNC Shield Initialized");

}

void loop() {
  //put your main code here, to run repeatedly:

  delay(1000);
  
  Serial.println("Counterclockwise");
  digitalWrite(xPins[1], LOW); // makes the x motor direction counterclockwise (away from motor)
  digitalWrite(yPins[1], LOW); // makes the y motor direction counterclockwise (away from motor)
  digitalWrite(zPins[1], LOW); // makes the z motor direction counterclockwise
  digitalWrite(yawPins[1], LOW); // makes the yaw motor direction counterclockwise

  stepping(xPins[0], yPins[0], zPins[0], yawPins[0], numsteps_x, numsteps_y, numsteps_z, numsteps_yaw, d_time);

 delay(1000);

 Serial.println("Clockwise");
 // move all the motors clockwise
 digitalWrite(xPins[1], HIGH); // makes the x motor direction clockwise
 digitalWrite(yPins[1], HIGH); // makes the y motor direction clockwise (toward motor)
 digitalWrite(zPins[1], HIGH); // makes the z motor direction clockwise
 digitalWrite(yawPins[1], HIGH); // makes the yaw motor direction counterclockwise

 stepping(xPins[0], yPins[0], zPins[0], yawPins[0], numsteps_x, numsteps_y, numsteps_z, numsteps_yaw, d_time);


}

void stepping(int stepX, int stepY, int stepZ, int stepYaw, long unsigned int nsteps_x, long unsigned int nsteps_y, long unsigned int nsteps_z, long unsigned int nsteps_yaw, float delayTime){
  Serial.println("Running motors");
  int temp1 = max(nsteps_x, nsteps_y);
  int temp2 = max(nsteps_z, nsteps_yaw);
  int maxsteps = max(temp1,temp2);

  for(int i = 0; i < maxsteps; i++) {
    if( i < nsteps_z ){
      steps(stepZ, delayTime);
    }
    if( i < nsteps_x ){
      steps(stepX, delayTime);
    }
    if( i < nsteps_y ){
      steps(stepY, delayTime);
    }
    if( i < nsteps_yaw ){
      steps(stepYaw, delayTime);
    }
  }
}

void steps(int stepPin, float delayTime){
  digitalWrite(stepPin, LOW);
  delayMicroseconds(delayTime); 
  digitalWrite(stepPin, HIGH);
}