 OutputPin dirPinY(2);
 OutputPin stepPinY(3);
 
-
 long xPosition = 0;
 long yPosition = 0;
 
-double pulsePerMM = 170.2722;
+double pulsePerMM = (360 * 32 / 1.8) / 60; //106.90453618;
 
 float curve_section = 0.5;
 bool isRelative = false;

    }
}


void setup(){
    Serial.begin(115200);
    Timer1.initialize(500);
    Timer1.attachInterrupt(updateMotion);
    pinMode(7,OUTPUT);
    analogWrite(7,0);
    Serial.print("start\n");
    
}

void loop(){
    if(Serial.available()){
        char c = Serial.read();
        if(c=='\n'){
            parseGCode();
            gcode = "";
        }else{
            gcode += c;
        }
    }
}
float getX(){
    return xPosition/pulsePerMM;
}
float getY(){
    return yPosition/pulsePerMM;
}
void parseGCode(){
    gcode.toLowerCase();
    char c;
    int cmd,index,startIndex,endIndex;
    float val;
    if(gcode.length()>2){
        if(hasCommand('g')){
            cmd = getCommand('g');
            switch(cmd){
                case 0:
                case 1:{
                    if(hasCommand('p')){
                        analogWrite(7,getCommand('p'));
                    }
                    if(hasCommand('f')){
                        targetDuringTime = (long)(50000/getCommand('f'));
                    }
                    double x = hasCommand('x')?getCommand('x'):0;
                    double y = hasCommand('y')?getCommand('y'):0;
                    if(isRelative){
                        move(x*pulsePerMM,y*pulsePerMM,cmd);
                    }else{
                        moveTo(hasCommand('x')?(x*pulsePerMM):xPosition,hasCommand('y')?(y*pulsePerMM):yPosition,cmd);
                        targetX = x;
                        targetY = y;
                    }
                }
                break;
                case 2:
                case 3:{
                    if(hasCommand('p')){
                        analogWrite(7,getCommand('p'));
                    }
                    arc(cmd==3);
                }
                break;
                case 28:{
                    xPosition = 0;
                    yPosition = 0;
                }
                break;
                case 90:{
                    isRelative = false;
                }
                break;
                case 91:{
                    isRelative = true;
                }
                break;
            }
        }else if(hasCommand('m')){
            cmd = getCommand('m');
            switch(cmd){
                case 4:
                    if(hasCommand('p')){
                        analogWrite(7,getCommand('p'));
                    }else{
                        analogWrite(7,0);
                    }
                break;
            }
        }
    }else{
    }
    commandEnd();
}
void commandEnd(){
    Serial.print("oMGok\n");
}
long toDistanceX(double x){
    return (x - xPosition);
}
long toDistanceY(double y){
    return (y - yPosition);
}

void stepX(bool dir){
    dirPinX = dir;
    stepPinX = LOW;
    delayMicroseconds(1);
    stepPinX = HIGH;
    xPosition+=dir?1:-1;
}
void stepY(bool dir){
    dirPinY = dir;
    stepPinY = LOW;
    delayMicroseconds(1);
    stepPinY = HIGH;
    yPosition+=dir?1:-1;
}

void move(long dx,long dy,bool precision){
    long tx = xPosition+dx;
    long ty = yPosition+dy;
    moveTo(tx,ty,precision);
}
int errX = 1;
int errY = 1;
void moveTo(long tx, long ty, bool precision) {  
    isMoving = true;
    long x0 = xPosition;
    long y0 = yPosition;
    long x1 = tx;
    long y1 = ty;
    currentDuringTime = max(targetDuringTime+500,1000);
    motionDir = 1;
    motionAcc = 1;
    long dx,dy,sx,sy;
    if(precision){
        dx = abs(x1-x0);
        sx = x0<x1;
        dy = abs(y1-y0);
        sy = y0<y1; 
        long err = (dx>dy ? dx : -dy)/2, e2;
        long ttt = millis();
        while(true){
            long distX = toDistanceX(x1);
            long distY = toDistanceY(y1);
            if(labs(distX)<200&&labs(distY)<200){
                motionDir = -1;
                motionAcc = 1;
            }
            if (distX==0 && distY==0) break;
            e2 = err;
            if (e2 >-dx) { 
                err -= dy;
                stepX(sx);   
            }
            if (e2 < dy) { 
                err += dx;
                stepY(sy);
            }
            if(currentDuringTime>1000){
                delay((int)(currentDuringTime/1000));
                delayMicroseconds(currentDuringTime%1000);
            }else{
                delayMicroseconds(currentDuringTime);
            }
        }
    }else{
        motionDir = 1;
        while(true){
            dx = toDistanceX(tx);
            dy = toDistanceY(ty);
            if(labs(dx)<200&&labs(dy)<200){
                motionDir = -1;
                motionAcc = 1;
            }
            if (dx==0 && dy==0) break;
            if (dx!=0) { 
                stepX(dx>0);
            }
            if (dy!=0) { 
                stepY(dy>0);
            }
            if(currentDuringTime>1000){
                delay((int)(currentDuringTime/1000));
                delayMicroseconds(currentDuringTime%1000);
            }else{
                delayMicroseconds(currentDuringTime);
            }
        }
    }
    isMoving = false;
}
void arc(bool ccw){
    float cx = isRelative?(getX()+getCommand('i')):getCommand('i');
    float cy = isRelative?(getY()+getCommand('j')):getCommand('j');
    float endX = isRelative?(getX()+getCommand('x')):getCommand('x');
    float endY = isRelative?(getY()+getCommand('y')):getCommand('y');
    float angleA, angleB, angle, radius, length, aX, aY, bX, bY;
    aX = (getX() - cx);
    aY = (getY() - cy);
    bX = (endX - cx);
    bY = (endY - cy);
    if(ccw){
        angleA = atan2(aY, aX);
        angleB = atan2(bY, bX);
    }else{
        angleA = atan2(bY, bX);
        angleB = atan2(aY, aX);
    }
    if (angleB <= angleA) {
        angleB += 2 * M_PI;
    }
    angle = angleB - angleA;
    radius = sqrt(aX * aX + aY * aY);
    length = radius * angle;
    int steps, s, step;
    steps = (int) ceil(length / curve_section);
    for (s = 1; s <= steps; s++) {
        step = (ccw) ? s : (steps - s); 
        moveTo((cx + radius * cos(angleA + angle * ((float) step / steps)))*pulsePerMM,(cy + radius * sin(angleA + angle * ((float) step / steps)))*pulsePerMM,1);
    }
}
bool hasCommand(char cmd){
    int index = 0;
    char c = 0;
    while(c!=cmd){
        if(index>=gcode.length()){
            return false;
        }else{
            c = gcode.charAt(index);
        }
        index++;
    }
    return true;
}
double getCommand(char cmd){
    int index = gcode.indexOf(cmd);
    int startIndex = gcode.indexOf(cmd)+1;
    char c = 0;
    while(c!=' '){
        index++;
        if(index>=gcode.length()){
            break;
        }else{
            c = gcode.charAt(index);
        }
    }
    return gcode.substring(startIndex,index).toDouble();
}
int motionAcc = 1;
bool isMoving = false;

void updateMotion()
{
    if (isMoving)
    {
        if (motionDir > 0 && currentDuringTime > targetDuringTime)
        {
            if (motionDir > 150)
            {
                motionAcc = -1;
                motionDir = 150;
            }
            motionDir += motionAcc;
            if (motionDir < 0)
            {
                motionDir = 0;
            }
            currentDuringTime -= motionDir;
            if (currentDuringTime < targetDuringTime)
            {
                currentDuringTime = targetDuringTime;
                motionDir = 0;
            }
        }
        else if (motionDir < 0 && currentDuringTime < 50000)
        {

            if (motionDir < -150)
            {
                motionAcc = -1;
                motionDir = -150;
            }
            motionDir -= motionAcc;
            if (motionDir > 0)
            {
                motionDir = 0;
            }
            currentDuringTime -= motionDir;
