/*


 */

#include <Stepper.h>

const int stepsPerRevolution1 = 200;  // change this to fit the number of steps per revolution
const int stepsPerRevolution2 = 200;  // change this to fit the number of steps per revolution
// for your motor

//Distance in between spools
#define W 740

// initial position of end effector in cartesian coordinates
int xpen0=400;
int ypen0=600;

//calibration
float DlA=48./200; //1.5*périmètre poulie /200 pas pour faire un tour avec le moteur (10.14*3.14*1.5)
float DlB=48./200;

// initialize the stepper library on pins 8 through 11:
Stepper myStepper1(stepsPerRevolution1, 6, 7, 8, 9);
Stepper myStepper2(stepsPerRevolution2, 10, 11, 12, 13);

boolean start=true;

void setup() {
  // set the speed of the motors at 60 rpm:
  myStepper1.setSpeed(60);
  myStepper2.setSpeed(60);
  // initialize the serial port:
  Serial.begin(9600);
}


void loop() {
  if(start == true){
  //trace les différentes sections d'un carré  
  traceligneAB(xpen0,ypen0,xpen0,ypen0+100);  
  traceligneAB(xpen0,ypen0+100,xpen0+100,ypen0+100);
  traceligneAB(xpen0+100,ypen0+100,xpen0+100,ypen0);
  traceligneAB(xpen0+100,ypen0,xpen0,ypen0);
  //traceligne(0,0,-300,-100);
  //traceligne(0,0,-50,-100);
  }
  start = false;
}

//From cartesian coordinates to wire coordinates
//Function that calculates the initial (a0,b0) and final (a1,b1) lengths of the wires based on the cartesian coordinates of the initial point (x0,y0) and final point (x1,y1)
void traceligneAB(int x0, int y0, int x1, int y1) {
  float a0;
  float b0;
  
  float px0 = pow(x0,2);
  float py0 = pow(y0,2);
  float pwx0 = pow((W-x0),2);
  
  a0 = sqrt(px0 + py0);
  b0 = sqrt(pwx0 + py0);

  float a1;
  float b1;
  
  float px1 = pow(x1,2);
  float py1 = pow(y1,2);
  float pwx1 = pow((W-x1),2);
  
  a1 = sqrt(px1 + py1);
  b1 = sqrt(pwx1 + py1);
           Serial.println("longueurs des fils a et b au point initial");
           Serial.print("(a0,b0)=(");
           Serial.print(a0);
           Serial.print(",");
           Serial.print(b0);
           Serial.println(")");
           Serial.println("longueurs des fils a et b au point final");
           Serial.print("(a1,b1)=(");
           Serial.print(a1);
           Serial.print(",");
           Serial.print(b1);
           Serial.println(")");

           Serial.println("nombre de steps correspondant aux longueurs des fils a et b au point initial");
           Serial.print("(a0/DlA,b0/DlB)=(");
           Serial.print(int(a0/DlA));
           Serial.print(",");
           Serial.print(int(b0/DlB));
           Serial.println(")");
           Serial.println("nombre de steps correspondant aux longueurs des fils a et b au point final");
           Serial.print("(a1/DlA,b1/DlB)=(");
           Serial.print(int(a1/DlA));
           Serial.print(",");
           Serial.print(int(b1/DlB));
           Serial.println(")");

//trace lign in between (a0,b0) and (a1,B1) (+ conversion in number of motor steps)
  traceligne(int(a0/DlA),int(b0/DlB),int(a1/DlA),int(b1/DlB));

}

//function that interpolates a lign in between (a0,b0) and (a1,b1), stepper motor moves accordinigly to follow the theoretical line.
void traceligne(int x0, int y0, int x1, int y1) {
     float deltax = x1 - x0;
     float deltay = y1 - y0;
     Serial.print("deltax=");
     Serial.println(deltax);
     Serial.print("deltay=");
     Serial.println(deltay);
     float deltaerr1 = abs((float)deltay/(float)deltax);    // Assume deltax != 0 (line is not vertical),
           // note that this division needs to be done in a way that preserves the fractional part
     float error1 = deltaerr1 - 0.5;
     float deltaerr2 = abs((float)deltax/(float)deltay);    // Assume deltax != 0 (line is not vertical),
           // note that this division needs to be done in a way that preserves the fractional part
     float error2 = deltaerr2 - 0.5;
     int y = y0;
     int x = x0;

      // Si delta x >=deltay, le moteur 1 incrémente par rapport à x et le moteur 2 corrige et incrémente quand nécessaire par rapport à la droite théorique
     if(abs(deltax)>=abs(deltay)){
         if((x1-x0)>=0){
            Serial.println("(x1-x0)>=0");    
             
             for (int x=x0; x<=x1; x++){ 
                 myStepper1.step(1);
                 Serial.print("(");
                 Serial.print(x);
                 Serial.print(",");
                 Serial.print(y);
                 Serial.println(")");
                 error1 = error1 + deltaerr1;
                 if (error1 >= 0.5) {
                                  if(y1>y0){
                                     //Serial.println("y1>y0");
                                     y = y + 1;
                                     myStepper2.step(1);
                                     error1 = error1 - 1.0;
                                  }
                                  else{
                                    //Serial.println("y1<y0");
                                     y = y - 1;
                                     myStepper2.step(-1);
                                     error1 = error1 - 1.0;
                                  }
                  }
             }
         }
          else{
              for (int x=x0; x>=x1; x--){ 
                   myStepper1.step(-1);
                   Serial.print("(");
                   Serial.print(x);
                   Serial.print(",");
                   Serial.print(y);
                   Serial.println(")");
                   error1 = error1 + deltaerr1;
                   if (error1 >= 0.5) {
                                    if(y1>y0){
                                       //Serial.println("y1>y0");
                                       y = y + 1;
                                       myStepper2.step(1);
                                       error1 = error1 - 1.0;
                                    }
                                    else{
                                      //Serial.println("y1<y0");
                                       y = y - 1;
                                       myStepper2.step(-1);
                                       error1 = error1 - 1.0;
                                    }
                    }
               }
          }
             
     }
     // Si delta x <deltay, le moteur 2 incrémente par rapport à x et le moteur 1 corrige et incrémente quand nécessaire par rapport à la droite théorique
     if(abs(deltay)>abs(deltax))
     {
      Serial.println("abs(deltay)>abs(deltax)");
          if((y1-y0)>=0){
            Serial.println("(y1-y0)>=0");
               for (int y=y0; y<=y1; y++){ 
                   myStepper2.step(1);
                   Serial.print("(");
                   Serial.print(x);
                   Serial.print(",");
                   Serial.print(y);
                   Serial.println(")");
                   error2 = error2 + deltaerr2;
                   if (error2 >= 0.5) {
                        if(x1>x0){
                           //Serial.println("x1>x0");
                           x = x + 1;
                           myStepper1.step(1);
                           error2 = error2 - 1.0;
                        }
                        else{
                          //Serial.println("x1<x0");
                           x = x - 1;
                           myStepper1.step(-1);
                           error2 = error2 - 1.0;
                        }
                    }
                       
               }
            }
          else{
            Serial.println("(y1-y0)<0");
                for (int y=y0; y>=y1; y--){ 
                       myStepper2.step(-1);
                       Serial.print("(");
                       Serial.print(x);
                       Serial.print(",");
                       Serial.print(y);
                       Serial.println(")");
                       error2 = error2 + deltaerr2;
                       if (error2 >= 0.5) {
                            if(x1>x0){
                               //Serial.println("x1>x0");
                               x = x + 1;
                               myStepper1.step(1);
                               error2 = error2 - 1.0;
                            }
                            else{
                              //Serial.println("x1<x0");
                               x = x - 1;
                               myStepper1.step(-1);
                               error2 = error2 - 1.0;
                            }
                      }
                   }
          }
          
      
     }
}