#include <AFMotor.h>

int LASER=9;


int x,y,in;

int nextchar() {
  while(Serial.available()==0) {
  } // wait till next char is available
  int ch=Serial.read();
  //Serial.print(ch,DEC);
  return ch;
}

AF_Stepper mX(48, 2);
AF_Stepper mY(48, 1);

void step_x(int i) {
  if(i>0) mX.step(i,BACKWARD, MICROSTEP); 
  else mX.step(-i,FORWARD, MICROSTEP); 
}
void step_y(int i) {
  if(i>0) mY.step(i,BACKWARD, MICROSTEP); 
  else mY.step(-i,FORWARD, MICROSTEP); 
}

void trace() {
  Serial.print("> ");
  Serial.print(x, DEC); 
  Serial.print(" "); 
  Serial.println( y, DEC);
}


void slow() {
  mX.setSpeed(1); 
  mY.setSpeed(4); 
}

void fast() {
  mX.setSpeed(400); 
  mY.setSpeed(650); 
}
void setup() {
  Serial.begin(9600);
  slow();
  analogWrite(LASER,5);
  //while(true) nextchar();
  // step_x(50); step_y(50);
}

void loop() {
  int x,y;
  // loop enters for each line of text
  int ox=-1,oy=0;
  do {
    int x=y=0;
    in=nextchar(); // first letter of command       
    while(in!=32) { 
      x=x*10+(in-48); 
      in=nextchar(); 
    }
    in=nextchar();
    while(in!=32 && in!=13) { 
      y=y*10+(in-48); 
      in=nextchar(); 
    }
    //    Serial.print(x, DEC); 
    //    Serial.print(" "); 
    //    Serial.println( y, DEC);
    if(ox>-1) line(ox,5.8*oy,x,5.8*y);
    ox=x; 
    oy=y;
  } 
  while(in!=13);
  // end of drawn line: pen up?
  Serial.println("OK");
}
// contrary to some Bresenham implementations, here 
// we need to start painting from x0,y0
int line(int x0, int y0, int x1, int y1) {
  int stpX,stpY,i,err,dx,dy;
  if(y1>y0) stpY=1; 
  else stpY=-1;
  if(x1>x0) stpX=1; 
  else stpX=-1;
  // move to start of drawing
  fast();
  if(x!=x0) step_x(x0-x);
  if(y!=y0) step_y(y0-y);
  slow();
  analogWrite(LASER,255);
  x=x0; 
  y=y0; // let's move form here on
  dx=abs(x1-x0);
  dy=abs(y1-y0);
  if(abs(y1-y0)>abs(x1-x0)) { // let's move along Y axis
    err=dy/2;
    while(y!=y1) { 
      //Serial.println(stpY);
      //trace();
      step_y(stpY); 
      y+=stpY; 
      err-=dx; 
      if (err<0) {
        step_x(stpX); 
        x+=stpX;
        err+=dy; 
      }
    }
  }
  else { //let's move along X axis
    err=dx/2;
    while(x!=x1) {
      //Serial.println(stpX); 
      //trace();
      step_x(stpX); 
      x+=stpX; 
      err-=dy; 
      if (err<0) {
        step_y(stpY); 
        y+=stpY;
        err+=dx; 
      }
    }
  }
  analogWrite(LASER,5); 
  Serial.print("..");
}