#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("..");
}