/* 
 *  V0 : Benoit Sobrie
 *  - Convert X,Y coordinate to A,B belt length.
 *  - Can draw lines with provided (absolute) coordinates.
 *  - Write the letter F and A.
 *  - Use RAMPS as controler.
*/

#define A_STEP_PIN         54 // X
#define A_DIR_PIN          55
#define A_ENABLE_PIN       38

#define B_STEP_PIN         60 // Y
#define B_DIR_PIN          61
#define B_ENABLE_PIN       56

#define STEP_DELAY         250 // Motor speed
#define STEP_ENDDELAY      250 // Motor speed

//Distance between stepper motor shafts
#define W 728

#define DIR_REVERSE_A 
//#define DIR_REVERSE_B

#define STEPS_PER_mm_A 80 //80 with 1/16 microstep, GT2 belt and 20 tooth
#define STEPS_PER_mm_B 80 //80 with 1/16 microstep, GT2 belt and 20 tooth

#define DEBUG

#ifdef DEBUG
  #define DEBUG_PRINTLN(x)  Serial.println (x);
  #define DEBUG_PRINT(x)  Serial.print (x);
#else
  #define DEBUG_PRINTLN(x)
  #define DEBUG_PRINT(x)
#endif

#ifdef DEBUGSTEPPER
  #define DEBUGSTEPPER_PRINTLN(x)  Serial.println (x);
  #define DEBUGSTEPPER_PRINT(x)  Serial.print (x);
#else
  #define DEBUGSTEPPER_PRINTLN(x)
  #define DEBUGSTEPPER_PRINT(x)
#endif

bool start = true;
float xpen0=100.f;
float ypen0=100.f;

typedef struct point
{
  float x;
  float y;
} point;

point origin;
point currentPosition;


void setup() {
  Serial.begin(115200);
  origin.x = 100.0f;
  origin.y = 100.0f;
  currentPosition.x = origin.x;
  currentPosition.y = origin.y;
  SetupStepper();
}

void SetupStepper() {
  DEBUG_PRINT("# Stepper A  : (E:")
  DEBUG_PRINT(A_ENABLE_PIN)
  DEBUG_PRINT(",S:")
  DEBUG_PRINT(A_STEP_PIN)
  DEBUG_PRINT(",D:")
  DEBUG_PRINT(A_DIR_PIN)
  DEBUG_PRINTLN(")")
  pinMode(A_ENABLE_PIN,OUTPUT); // Enable
  pinMode(A_STEP_PIN,OUTPUT); // Step
  pinMode(A_DIR_PIN,OUTPUT); // Dir
  digitalWrite(A_ENABLE_PIN,HIGH); // Set !Enable high  

  DEBUG_PRINT("# Stepper B  : (E:")
  DEBUG_PRINT(B_ENABLE_PIN)
  DEBUG_PRINT(",S:")
  DEBUG_PRINT(B_STEP_PIN)
  DEBUG_PRINT(",D:")
  DEBUG_PRINT(B_DIR_PIN)
  DEBUG_PRINTLN(")")
  pinMode(B_ENABLE_PIN,OUTPUT); // Enable
  pinMode(B_STEP_PIN,OUTPUT); // Step
  pinMode(B_DIR_PIN,OUTPUT); // Dir
  digitalWrite(B_ENABLE_PIN,HIGH); // Set !Enable high  
}

void loop() {
  DEBUG_PRINT("# Ready waiting Serial input. Current Position : (")
  DEBUG_PRINT(currentPosition.x);
  DEBUG_PRINT(",");
  DEBUG_PRINT(currentPosition.y);
  DEBUG_PRINTLN(")");
  if(Serial.read() != -1){
    letterF(1.0f); 
    gotoXY(currentPosition.x+65,currentPosition.y);
    letterA(1.0f); 
    gotoXY(currentPosition.x+15,currentPosition.y);
    letterB(1.f);
    gotoXY(currentPosition.x+65,currentPosition.y);
    letterL(1.f);
    letterA(1.0f); 
    gotoXY(currentPosition.x+15,currentPosition.y);
    letterB(1.f);
    gotoXY(origin.x, origin.y);
  }
  start = false;
}

void letterF(float ratio) {
  point current = currentPosition;
  gotoXY(current.x+(50*ratio),current.y);
  gotoXY(current.x,current.y);
  gotoXY(current.x,current.y+(50*ratio));
  gotoXY(current.x+(50*ratio),current.y+(50*ratio));
  gotoXY(current.x,current.y+(50*ratio));
  gotoXY(current.x,current.y+(100*ratio));
}

void letterA(float ratio) {
  point current = currentPosition;
  gotoXY(current.x+(12.5*ratio),current.y-(50*ratio));
  gotoXY(current.x+(37.5*ratio),current.y-(50*ratio));
  gotoXY(current.x+(25*ratio),current.y-(100*ratio));
  gotoXY(current.x+(12.5*ratio),current.y-(50*ratio));
  gotoXY(current.x+(37.5*ratio),current.y-(50*ratio));
  gotoXY(current.x+(50*ratio),current.y);
}

void letterB(float ratio) {
  point current = currentPosition;
  gotoXY(current.x,current.y-(100*ratio));
  gotoXY(current.x+(25*ratio),current.y-(90*ratio));
  gotoXY(current.x+(37.5*ratio),current.y-(75*ratio));
  gotoXY(current.x+(25*ratio),current.y-(65*ratio));
  gotoXY(current.x,current.y-(65*ratio));
  gotoXY(current.x+(25*ratio),current.y-(65*ratio));
  gotoXY(current.x+(37.5*ratio),current.y-(50*ratio));
  gotoXY(current.x+(50*ratio),current.y-(37.5*ratio));
  gotoXY(current.x+(45*ratio),current.y-(27*ratio));
  gotoXY(current.x+(25*ratio),current.y-(12.5*ratio));
  gotoXY(current.x,current.y);
}

void letterL(float ratio) {
  point current = currentPosition;
  gotoXY(current.x,current.y-(100*ratio));
  gotoXY(current.x,current.y);
  gotoXY(current.x+(50*ratio),current.y);
}

void gotoXY(float x1, float y1) {
  traceline(currentPosition.x, currentPosition.y, x1, y1);
  currentPosition.x = x1;
  currentPosition.y = y1;
}

void traceline(float x0, float y0, float x1, float y1) {
  point origin;
  point destination;
  point originAB;
  point destinationAB;

  DEBUG_PRINT("# TraceLigne : (")
  DEBUG_PRINT(x0)
  DEBUG_PRINT(",")
  DEBUG_PRINT(y0)
  DEBUG_PRINT(")(");
  DEBUG_PRINT(x1)
  DEBUG_PRINT(",")
  DEBUG_PRINT(y1)
  DEBUG_PRINTLN(")");  
  
  origin.x = x0;
  origin.y = y0;
  destination.x = x1;
  destination.y = y1;

  originAB = convert2ABcoord(origin);
  destinationAB = convert2ABcoord(destination);

  moveAB(originAB,destinationAB);
}

void moveAB(point p0, point p1) {
  DEBUG_PRINT("# Move AB    : (")
  DEBUG_PRINT(p0.x)
  DEBUG_PRINT(",")
  DEBUG_PRINT(p0.y)
  DEBUG_PRINT(")(");
  DEBUG_PRINT(p1.x)
  DEBUG_PRINT(",")
  DEBUG_PRINT(p1.y)
  DEBUG_PRINTLN(")");
  
  float dA = p1.x - p0.x;
  float dB = p1.y - p0.y;
  bool Adir = (dA >= 0) ? true : false;
  bool Bdir = (dB >= 0) ? true : false;
  unsigned long Astep = abs(dA) * STEPS_PER_mm_A;
  unsigned long Bstep = abs(dB) * STEPS_PER_mm_B;
  DEBUG_PRINT("# AB Steps : ")
  DEBUG_PRINT(Astep)
  DEBUG_PRINT(",")
  DEBUG_PRINTLN(Bstep)
          
  if(Astep == 0) StepB(Bdir, Bstep);
  else {
    if(Bstep == 0) StepA(Adir, Astep);
    else {
      float k = (float) Bstep / (float)Astep;
      unsigned long AstepStatus = 0;
      unsigned long BstepStatus = 0;
      float cerror = 0;
      float error = 0;
      unsigned long nstep = 0;
      if(k>=1) {
        nstep = k;
        error = k - nstep;
        while(AstepStatus < Astep) {
          StepA(Adir, 1);
          cerror += error;
          if(cerror >= 1) {
            int missingsteps = cerror;
            cerror -= missingsteps;
            StepB(Bdir, nstep + missingsteps);
          }
          else {
            StepB(Bdir, nstep);
          }
          AstepStatus ++;
        }
      }
      else {
        k = 1 / k;
        nstep = k;
        error = k - nstep;
        while(BstepStatus < Bstep) {
          StepB(Bdir, 1);
          cerror += error;
          if(cerror >= 1) {
            int missingsteps = cerror;
            cerror -= missingsteps;
            StepA(Adir, nstep + missingsteps);
          }
          else {
            StepA(Adir, nstep);
          }
          BstepStatus ++;
        }        
      }
    }
  }
}

point convert2ABcoord(point coord) {
  point res;
  
  float px0 = pow(coord.x,2);
  float py0 = pow(coord.y,2);
  float pwx0 = pow((W-coord.x),2);
  
  res.x = sqrt(px0 + py0);
  res.y = sqrt(pwx0 + py0);

  return res;
}

void StepA(bool dir, unsigned long nStep) {
  #ifdef DIR_REVERSE_A
    DEBUGSTEPPER_PRINTLN("# Stepper A  : Reverse direction")
    Step(!dir, A_DIR_PIN, A_ENABLE_PIN, A_STEP_PIN, nStep);
  #else
    DEBUGSTEPPER_PRINTLN("# Stepper A  : Normal direction")
    Step(dir, A_DIR_PIN, A_ENABLE_PIN, A_STEP_PIN, nStep);
  #endif
}

void StepB(bool dir, unsigned long nStep) {
  #ifdef DIR_REVERSE_B
    DEBUGSTEPPER_PRINTLN("# Stepper B  : Reverse direction")
    Step(!dir, B_DIR_PIN, B_ENABLE_PIN, B_STEP_PIN, nStep);
  #else
    DEBUGSTEPPER_PRINTLN("# Stepper B  : Normal direction")
    Step(dir, B_DIR_PIN, B_ENABLE_PIN, B_STEP_PIN, nStep);
  #endif
}

void Step(bool dir, int dirPin, int enablePin, int stepPin, unsigned long nStep) {
  DEBUGSTEPPER_PRINT("# Stepper    : (")
  DEBUGSTEPPER_PRINT(dir)
  DEBUGSTEPPER_PRINT(",")
  DEBUGSTEPPER_PRINT(nStep)
  DEBUGSTEPPER_PRINTLN(")")
  digitalWrite(enablePin,LOW);
  if(dir) {
    digitalWrite(dirPin, LOW);
  }
  else {
    digitalWrite(dirPin, HIGH);
  }
  for(unsigned long i=0; i < nStep; i++) {
    digitalWrite(stepPin,HIGH);       // Output high
    delayMicroseconds(STEP_DELAY);    //delay(STEP_DELAY);            // Wait
    digitalWrite(stepPin,LOW);    	  // Output low
    delayMicroseconds(STEP_ENDDELAY); //delay(STEP_ENDDELAY);         // Wait   
  }
  digitalWrite(enablePin,HIGH);
}