const int XdirPin = 2;
const int XstepPin = 5;
const int YdirPin = 3;
const int YstepPin = 6;
const int ZdirPin = 4;
const int ZstepPin = 7;
const int enablePin = 8; //to enable the stealth chop drivers

float oldXcircle;
float oldYcircle;
float newXcircle;
float newYcircle;
float oldXmagnet;
float oldYmagnet;
float newXmagnet;
float newYmagnet;
int XnumberOfSteps;
int YnumberOfSteps;
float multiplier;

int stepPin;
int dirPin;
int XendStop = 15;
int YendStop = 15;
int XmotorSpeed = 750;
int YmotorSpeed = 750;
int ZmotorSpeed = 750;
int motorSpeed = 1000;

void setup()
{
  Serial.begin(115200);
  Serial.println("testing 1 2 3");
  delay(2000);
  pinMode(XstepPin, OUTPUT);
  pinMode(XdirPin, OUTPUT);
  pinMode(YstepPin, OUTPUT);
  pinMode(YdirPin, OUTPUT);
  pinMode(ZstepPin, OUTPUT);
  pinMode(ZdirPin, OUTPUT);
  pinMode(enablePin, OUTPUT);
  digitalWrite(enablePin, LOW);
  homeMotors();
  delay(2000);
  yAxisShift(-2000, 'A', 200);
  straight('X', 'A', 7000, 125, 125);
  straight('Y', 'A', 800, 125, 125);
}

void moveMotor(char axis, char directiony, int numberOfSteps, int motorSpeed) {

  if (axis == 'X') {
    stepPin = XstepPin;
    dirPin = XdirPin;
  } else if (axis == 'Y') {
    stepPin = YstepPin;
    dirPin = YdirPin;
  } else if (axis == 'Z') {
    stepPin = ZstepPin;
    dirPin = ZdirPin;
  }

  if (directiony == 'C') {
    digitalWrite(dirPin, LOW);
  } else {
    digitalWrite(dirPin, HIGH);
  }

  for (int x = 0; x < numberOfSteps; x++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(motorSpeed);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(motorSpeed);
  }
}

void powerSlide(int totalDistance, char dir, int theSpeed) {
  if (dir == 'C') {
    for (int x = 0; x < 400; x++) {
      moveMotor('X', 'C', 1, theSpeed / 2);
      moveMotor('Z', 'C', 1, theSpeed / 2);
    }
    moveMotor('X', 'C', (totalDistance - 800), theSpeed);
    for (int x = 0; x < 400; x++) {
      moveMotor('X', 'C', 1, theSpeed / 2);
      moveMotor('Z', 'A', 1, theSpeed / 2);
    }
  } else {
    for (int x = 0; x < 400; x++) {
      moveMotor('X', 'A', 1, theSpeed / 2);
      moveMotor('Z', 'C', 1, theSpeed / 2);
    }
    moveMotor('X', 'A', (totalDistance - 800), theSpeed);
    for (int x = 0; x < 400; x++) {
      moveMotor('X', 'A', 1, theSpeed / 2);
      moveMotor('Z', 'A', 1, theSpeed / 2);
    }
  }
}

void powerSlideQuick(int totalDistance, char dir, int theSpeed) {
  if (dir == 'C') {
    for (int x = 0; x < 200; x++) {
      moveMotor('X', 'C', 1, theSpeed / 3);
      moveMotor('Z', 'C', 2, theSpeed / 3);
    }
    moveMotor('X', 'C', (totalDistance - 400), theSpeed);
    for (int x = 0; x < 200; x++) {
      moveMotor('X', 'C', 1, theSpeed / 3);
      moveMotor('Z', 'A', 2, theSpeed / 3);
    }
  } else {
    for (int x = 0; x < 200; x++) {
      moveMotor('X', 'A', 1, theSpeed / 3);
      moveMotor('Z', 'A', 2, theSpeed / 3);
    }
    moveMotor('X', 'A', (totalDistance - 400), theSpeed);
    for (int x = 0; x < 200; x++) {
      moveMotor('X', 'A', 1, theSpeed / 3);
      moveMotor('Z', 'C', 2, theSpeed / 3);
    }
  }
}

void turn(int radius, int XdistanceToCentre, int YdistanceToCentre, char dir, float percentage, int startSpeed, int finishSpeed) {

  int oldXmagnet = -XdistanceToCentre;
  int oldYmagnet = -YdistanceToCentre;
  int oldXcircle = -XdistanceToCentre;
  float oldYcircle = -float(YdistanceToCentre);
  int newXcircle;
  float newYcircle;
  char Xdirection;
  char Ydirection;
  int circleMotorSpeed;
  int Xcounter = 0;
  int Ycounter = 0;
  float Zcounter = 0.0;
  float Zfigure = radius / 400.0;
  int mycounter = 0;

  for (int x = 0; x < ((float)radius * 4.0 * percentage); x++) {

    if (((oldYcircle > 0) and (oldXcircle < 0)) or ((oldXcircle == -radius) and (dir == 'C')) or ((oldYcircle == radius) and (dir == 'A'))) { //if you're in the top left quarter of circle
      if (dir == 'C') {
        newXcircle = oldXcircle + 1.0;
      } else if (dir == 'A') {
        newXcircle = oldXcircle - 1.0;
      }
      newYcircle = sqrt(sq(float(radius)) - sq(float(newXcircle)));
    } else if (((oldYcircle > 0) and (oldXcircle > 0)) or ((oldXcircle == radius) and (dir == 'A')) or ((oldYcircle == radius) and (dir == 'C'))) { //if you're in the top right quarter of circle
      if (dir == 'C') {
        newXcircle = oldXcircle + 1;
      } else if (dir == 'A') {
        newXcircle = oldXcircle - 1;
      }
      newYcircle = sqrt(sq(float(radius)) - sq(float(newXcircle)));
    } else if (((oldYcircle < 0) and (oldXcircle < 0)) or ((oldXcircle == -radius) and (dir == 'A')) or ((oldYcircle == -radius) and (dir == 'C'))) { //if you're in the bottom left quarter of circle

      if (dir == 'C') {
        newXcircle = oldXcircle - 1;
      } else if (dir == 'A') {
        newXcircle = oldXcircle + 1;
      }
      newYcircle = -sqrt(sq(float(radius)) - sq(float(newXcircle)));
    } else if (((oldYcircle < 0) and (oldXcircle > 0)) or ((oldXcircle == radius) and (dir == 'C')) or ((oldYcircle == -radius) and (dir == 'A'))) { //if you're in the bottom right quarter of circle

      if (dir == 'C') {
        newXcircle = oldXcircle - 1;
      } else if (dir == 'A') {
        newXcircle = oldXcircle + 1;
      }
      newYcircle = -sqrt(sq(float(radius)) - sq(float(newXcircle)));
    }

    int Xchange = (newXcircle - oldXmagnet);
    int Ychange = (newYcircle - oldYmagnet);

    if (Xchange > 0) {
      Xdirection = 'C';
    } else {
      Xdirection = 'A';
    }
    if (Ychange > 0) {
      Ydirection = 'C';
    } else {
      Ydirection = 'A';
    }

    int Xsteps = abs(Xchange);
    int Ysteps = abs(Ychange);

    //    if ((Xsteps <1) or (Ysteps <1)){
    //      circleMotorSpeed = regularMotorSpeed;
    //    } else if ((Xsteps ==1) and (Ysteps ==1)){
    //      circleMotorSpeed = regularMotorSpeed*0.71;
    //    } else {
    //      circleMotorSpeed = ((min(Xsteps,Ysteps)*(regularMotorSpeed*0.71))  + ((abs(Xsteps - Ysteps)*regularMotorSpeed))/(Xsteps + Ysteps));
    //    }
    //      float motorSpeedChange = float(finishMotorSpeed - startMotorSpeed)*percentageXstepsElapsed;


    float percentageXstepsElapsed = (float)Xcounter / ((float)(radius * 4.0 * percentage));
    //float percentageYstepsElapsed = float(Ycounter/(float(radius)*4.0*percentage));
    float percentageZstepsElapsed = (float)Zcounter / ((float)(1600 * percentage));

    int circleMotorSpeed = startSpeed + round((finishSpeed - startSpeed) * percentageXstepsElapsed);

    if (percentageXstepsElapsed > percentageZstepsElapsed) {
      moveMotor('Z', dir, 1, 5);
      Zcounter = Zcounter + 1.0;
    }

    moveMotor('X', Xdirection, Xsteps, circleMotorSpeed);
    moveMotor('Y', Ydirection, Ysteps, circleMotorSpeed);
    Xcounter = Xcounter + Xsteps;
    Ycounter = Ycounter + Ysteps;

    if ((Zcounter * Zfigure) < Xcounter) {
      moveMotor('Z', dir, 1, 5);
      Zcounter = Zcounter + 1;
    }

    oldXmagnet = oldXmagnet + (Xchange);
    oldYmagnet = oldYmagnet + (Ychange);
    oldXcircle = newXcircle;
    oldYcircle = newYcircle;
  }

}

void homeMotors() {

  int XendSwitch = 9;
  int YendSwitch = 10;
  pinMode(XendSwitch, INPUT_PULLUP);
  pinMode(YendSwitch, INPUT_PULLUP);

  for (int x = 0; x < 15000; x++) {
    if (digitalRead(XendSwitch) == HIGH) {
      moveMotor('X', 'C', 1, 200);
    } else {
      break;
    }
  }

  for (int x = 0; x < 15000; x++) {
    if (digitalRead(YendSwitch) == HIGH) {
      moveMotor('Y', 'C', 1, 200);
    } else {
      break;
    }
  }
}

void yAxisShift(int newYlocation, char Xdir, int theSpeed) {

  int Ycounter = 0;
  float Ypercentage = 0.0;
  int Zcounter = 0;
  float Zpercentage = 0.0;
  char Ydir;
  char Zdir1;
  char Zdir2;

  if (((Xdir == 'C') and (newYlocation > 0)) or ((Xdir == 'A') and (newYlocation < 0))) {
    Zdir1 = 'A';
    Zdir2 = 'C';
  } else {
    Zdir1 = 'C';
    Zdir2 = 'A';
  }

  if (newYlocation > 0) {
    Ydir = 'C';
  } else {
    Ydir = 'A';
  }


  for (int x = 0; x < abs(newYlocation / 2); x++) {
    moveMotor('X', Xdir, 2, theSpeed * 0.75);
    moveMotor('Y', Ydir, 1, theSpeed * 0.75);
    Ycounter = Ycounter + 1;
    Ypercentage = (float)Ycounter / ((float)abs(newYlocation / 2.0));
    Zpercentage = (float)Zcounter / 133.0;
    if (Zpercentage < Ypercentage) {
      moveMotor('Z', Zdir1, 1, theSpeed * 0.75); //because the length of the hypothenuse on triange 2 x 1 is 2.24
      Zcounter = Zcounter + 1;
    }
  }

  for (int x = 0; x < abs(newYlocation / 2); x++) {

    moveMotor('X', Xdir, 2, theSpeed * 0.75);
    moveMotor('Y', Ydir, 1, theSpeed * 0.75);
    Ycounter = Ycounter + 1;
    Ypercentage = (float)Ycounter / ((float)abs(newYlocation / 2.0));
    Zpercentage = (float)Zcounter / 133.0;
    if (Zpercentage < Ypercentage) {
      moveMotor('Z', Zdir2, 1, theSpeed * 0.75); //because the length of the hypothenuse on triange 2 x 1 is 2.24
      Zcounter = Zcounter + 1;
    }
  }
}

void straight(char axis, char dir, int distance, int startSpeed, int finishSpeed) {

  int straightMotorSpeed = startSpeed;
  int counter = 0;
  for (int x = 0; x < distance; x++) {
    counter = counter + 1;
    moveMotor(axis, dir, 1, straightMotorSpeed);
    straightMotorSpeed = round(float(startSpeed) + (float(finishSpeed - startSpeed) * (float(counter) / float(distance))));
  }
}

void loop()
{

straight('X', 'C', 10000, 200, 400);
delay(1000);
straight('X', 'A', 10000, 200, 400);
delay(1000);

//  yAxisShift(-3000, 'A', 200);
//  straight('X', 'C', 1700, 100, 80);
//  for (int x = 0; x < 400; x++) {
//    moveMotor('X', 'C', 1, 40);
//    moveMotor('Z', 'C', 1, 40);
//  }
//  straight('X', 'C', 550, 80, 80);
//  turn(748.0, 0, -748, 'C', 0.25, 80, 60);
//  straight('Y', 'A', 250, 60, 60);
//  turn(748.0, 748, 0, 'A', 0.25, 60, 40);
//  straight('X', 'C', 550, 40, 40);
//  for (int x = 0; x < 400; x++) {
//    moveMotor('X', 'C', 1, 25);
//    moveMotor('Z', 'A', 1, 25);
//  }
//  yAxisShift(500, 'C', 50);
//  straight('X', 'C', 3000, 50, 20);
//  straight('X', 'A', 2000, 60, 30);
//  yAxisShift(2000, 'A', 30);
//  yAxisShift(-2000, 'A', 35);
//  straight('X', 'A', 2000, 35, 45);
//  
// straight('X', 'C', 1000, 60, 50);
// yAxisShift(-2125, 'C', 50);
// straight('X', 'C', 2000, 60, 50);
// yAxisShift(-1200, 'C', 50);
// straight('X', 'C', 2000, 60, 50);
//
//   
//      straight('X', 'A', 2000, 60, 50);
//      yAxisShift(1200, 'A', 50);
//      straight('X', 'A', 2000, 50, 50);
//      yAxisShift(-1200, 'A', 50);
//            straight('X', 'A', 3350, 50, 50);
//
//                        straight('X', 'C', 12150, 50, 50);
//
//                          straight('X', 'A', 2000, 60, 50);
//      yAxisShift(1200, 'A', 50);
//      straight('X', 'A', 2000, 50, 50);
//      yAxisShift(-1200, 'A', 50);
//            straight('X', 'A', 3350, 50, 50);
//
//            straight('X', 'C', 12150, 50, 50);
//
//                                     straight('X', 'A', 2000, 60, 50);
//      yAxisShift(1200, 'A', 50);
//      straight('X', 'A', 2000, 50, 50);
//      yAxisShift(-1200, 'A', 50);
//            straight('X', 'A', 3350, 50, 50);
//
// straight('X', 'C', 12150, 50, 50);
//
//  delay(9999999);
//
//  for (int x = 0; x <400; x++) {
//    moveMotor('Y', 'A', 100, 100);
//    delay(1000);
  }


  //straight('X', 'C', 2000, 140, 140);
  //yAxisShift(-750, 'C', 140);
  //straight('X', 'C', 1300, 140, 140);
  //
  //for (int x = 0; x <400; x++) {
  //   moveMotor('X', 'C', 1, 70);
  //   moveMotor('Z', 'C', 1, 70);
  //  }
  //turn(782.0, 0, -782, 'C', 0.25, 450, 450);
  //turn(782.0, 782, 0, 'A', 0.25, 450, 450);
  //for (int x = 0; x <400; x++) {
  //   moveMotor('X', 'C', 1, 70);
  //   moveMotor('Z', 'C', 1, 70);
  // }
  //straight('X', 'C', 2000, 140, 140);
  //
  //delay(1000);
  //
  //
  //straight('X', 'A', 2000, 140, 140);
  //
  //for (int x = 0; x <400; x++) {
  //   moveMotor('X', 'A', 1, 70);
  //   moveMotor('Z', 'A', 1, 70);
  //}
  //turn(782.0, 0, 782, 'C', 0.25, 450, 450);
  //turn(782.0, -782, 0, 'A', 0.25, 450, 450);
  //for (int x = 0; x <400; x++) {
  //   moveMotor('X', 'A', 1, 70);
  //   moveMotor('Z', 'A', 1, 70);
  //}
  //
  //straight('X', 'A', 1300, 140, 140);
  //yAxisShift(750, 'A', 140);
  //straight('X', 'A', 2000, 140, 140);

//}


//  int counter = 0;
//  for (int x = 0; x <800; x++) {
//    moveMotor('X', 'C', 1, 65);
//    moveMotor('Y', 'A', 2, 65);
//    if ((counter % 4)==1 ){
//    moveMotor('Z', 'C', 1, 65);
//    }
//    counter = counter + 1;
//  }
//  counter = 0;
//  for (int x = 0; x <800; x++) {
//    moveMotor('X', 'C', 2, 65);
//    moveMotor('Y', 'A', 1, 65);
//    if ((counter % 4)==1){
//    moveMotor('Z', 'A', 1, 65);
//    }
//    counter = counter + 1;
//  }
//
//  moveMotor('X', 'C', 2000, 100);
//  powerSlide(4000, 'C');
//  moveMotor('X', 'C', 4000, 100);
//  delay(250);
//  moveMotor('X', 'A', 4000, 100);
//  powerSlide(4000, 'A');
//  moveMotor('X', 'A', 2000, 100);
//  for (int x = 0; x <800; x++) {
//    moveMotor('X', 'A', 1, 65);
//    moveMotor('Y', 'C', 2, 65);
//    if ((counter % 4)==1 ){
//    moveMotor('Z', 'C', 1, 65);
//    }
//    counter = counter + 1;
//  }
//  counter = 0;
//  for (int x = 0; x <800; x++) {
//    moveMotor('X', 'A', 2, 65);
//    moveMotor('Y', 'C', 1, 65);
//    if ((counter % 4)==1){
//    moveMotor('Z', 'A', 1, 65);
//    }
//    counter = counter + 1;
//  }
//  moveMotor('X', 'A', 1000, 100);
//  delay(1000);
//}}

//void curvedRail(float radius, int oldXcircle, int oldYcircle) {
//
//    int oldXmagnet = -oldXcircle;
//    int oldYmagnet = -oldYcircle;
//    float oldXcircle = -oldXcircle;
//    float oldYcircle = -oldYcircle;
//    char Xdirection;
//    char Ydirection;
//    int circleMotorSpeed;
//    int regularMotorSpeed = 100;
//
//    // SECTION 1
//
//    for (int x = 0; x <radius; x++) {
//
//    float newXcircle = oldXcircle + 1.0;
//    float newYcircle = -sqrt(sq(radius) - sq(newXcircle));
//
//    Serial.print("new xcircle value is ");
//    Serial.println(newXcircle);
//    Serial.print("new ycircle value is ");
//    Serial.println(newYcircle);
//
//
//    float Xchange = (newXcircle - oldXmagnet);
//    float Ychange = (newYcircle - oldYmagnet);
//    Serial.print("X change value is ");
//    Serial.println(Xchange);
//    Serial.print("Y change value is ");
//    Serial.println(Ychange);
//    if (Xchange>0.0){
//      Xdirection = 'C';
//    } else {
//      Xdirection = 'A';
//    }
//    if (Ychange>0.0){
//      Ydirection = 'C';
//    } else {
//      Ydirection = 'A';
//    }
//
//    Serial.print("X direction is ");
//    Serial.println(Xdirection);
//    Serial.print("Y direction is ");
//    Serial.println(Ydirection);
//
//
//    float XchangeAbs = abs(newXcircle - oldXmagnet);
//    float YchangeAbs = abs(newYcircle - oldYmagnet);
//
//    int Xsteps = round(XchangeAbs);
//    int Ysteps = round(YchangeAbs);
//
//    if ((Xsteps <1) or (Ysteps <1)){
//      circleMotorSpeed = regularMotorSpeed;
//    } else if ((Xsteps ==1) and (Ysteps ==1)){
//      circleMotorSpeed = regularMotorSpeed*0.71;
//    } else {
//      circleMotorSpeed = ((min(Xsteps,Ysteps)*(regularMotorSpeed*0.71))  + ((abs(Xsteps - Ysteps)*regularMotorSpeed))/(Xsteps + Ysteps));
//    }
//    moveMotor('X', Xdirection, Xsteps*8, circleMotorSpeed);
//    Serial.print("moving X axis by ");
//    Serial.println(Xsteps);
//    moveMotor('Y', Ydirection, Ysteps*8, circleMotorSpeed);
//    Serial.print("moving Y axis by ");
//    Serial.println(Ysteps);
//    Serial.println("");
//
//    oldXmagnet = oldXmagnet + Xsteps;
//    oldYmagnet = oldYmagnet + Ysteps;
//
//    oldXcircle = newXcircle;
//    oldYcircle = newYcircle;
//    delay(5);
//  }
//}