/*


 */

#include <Stepper.h>

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

// initial position of end effector (relative to motor A) - length of motor A wire (mm)
int lA0=500;
// initial position of end effector (relative to motor B) - length of motor B wire (mm)
int lB0=500;

// final length of motor A (mm)
int lA1=550;
// final length of motor B (mm)
int lB1=525;

//calibration
float DlA=30./200; //périmètre poulie moteur/nombre de pas pour faire un tour
float DlB=30./200;


int nstepA=int((lA1-lA0)/DlA);
int nstepB=int((lB1-lB0)/DlB);

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


int x0=0;
int y0=0;
int x1=0;
int y1=0;

boolean start=true;

void setup() {
  // set the speed at 60 rpm:
  myStepper1.setSpeed(60);
  myStepper2.setSpeed(60);
  // initialize the serial port:
  Serial.begin(9600);
  //Serial.println("Hello World");
  Serial.print("lA0=");
  Serial.println(lA0);
  Serial.print("DlA=");
  Serial.println(DlA);
  Serial.print("nstepA=");
  Serial.println(nstepA);
  Serial.print("lB0=");
  Serial.println(lB0);
  Serial.print("DlB=");
  Serial.println(DlB);
  Serial.print("nstepB=");
  Serial.println(nstepB);
}


void loop() {
  if(start == true){
  traceligne(0,0,300,100);  
  traceligne(0,0,100,300);
  }
  //start = false;
}

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;
     
     if(deltax>=deltay){
       for (int x=x0; x<=x1; x++){ 
           //return (x,y)
           myStepper1.step(1);
           Serial.print("(");
           Serial.print(x);
           Serial.print(",");
           Serial.print(y);
           Serial.println(")");
           error1 = error1 + deltaerr1;
           if (error1 >= 0.5) {
               y = y + 1;
               myStepper2.step(1);
               error1 = error1 - 1.0;
           }
       }
     }

     if(deltay>deltax){
       for (int y=y0; y<=y1; y++){ 
           //return (x,y)
           myStepper2.step(1);
           Serial.print("(");
           Serial.print(x);
           Serial.print(",");
           Serial.print(y);
           Serial.println(")");
           error2 = error2 + deltaerr2;
           if (error2 >= 0.5) {
               x = x + 1;
               myStepper1.step(1);
               error2 = error2 - 1.0;
           }
       }
     }
     
  }