#ifndef UTILS_H_INCLUDED
#define UTILS_H_INCLUDED

unsigned long int mod(long int a){
    return a>0?a:-a;
}

double toAbsoluteX(double x){
    return posX+x;
}

double toAbsoluteY(double y){
    return posY+y;
}

String removeComments(String code){
    if(code.indexOf(";")!=-1){
        return code.substring(0, code.indexOf(";"));
    }
    else if(code.indexOf("(")!=-1){
        return code.substring(0, code.indexOf("("))+code.substring(code.indexOf(")"));
    }
    else{
        return code;
    }
}

double getParameter(String code, char param){
    char b[50];
    code.substring(code.indexOf(param)+1, code.indexOf(" ", code.indexOf(param))).toCharArray(b, 50);
    return atof(b);
}

String getCommand(String code){
    return code.substring(0, code.indexOf(" "));
}

void goTo(double x, double y, long speed){
    /**
    Makes use of Bresenham algorithm
    @link https://www.marginallyclever.com/blog/2013/08/how-to-build-an-2-axis-arduino-cnc-gcode-interpreter/
    **/

    //set speed
    stepperX.setSpeed(speed);
    stepperY.setSpeed(speed);

    //convert to something "normal"
    if(relative){
        x=toAbsoluteX(x);
        y=toAbsoluteY(y);
    }

    //maintain limits
    if(MAX_X!=-1 && x>MAX_X){
        x=MAX_X;
    }
    if(MAX_Y !=-1 && y>MAX_Y){
        y=MAX_Y;
    }

    //coords to steps
    int xsteps=x*STEPX;
    int ysteps=y*STEPY;

    //calculate deltas
    int dx=abs(xsteps-posX);
    int dy=abs(ysteps-posY);
    int moveX=x>posX?STEP_INCREMENT:-STEP_INCREMENT;
    int moveY=y>posY?STEP_INCREMENT:-STEP_INCREMENT;

    unsigned long int over=0;

    if(dx>dy){
        for(unsigned long int i=0; i<dx; i++){
            stepperX.step(moveX);
            over+=dy;
            if(over>=dx){
                over-=dx;
                stepperY.step(moveY);
            }
            delay(STEP_DELAY);
        }
    }
    else{
        for(unsigned long int i=0; i<dy; i++){
            stepperY.step(moveY);
            over+=dx;
            if(over>=dy){
                over-=dy;
                stepperX.step(moveX);
            }
            delay(STEP_DELAY);
        }
    }

    //update coords
    posX=x;
    posY=y;
    posXmm=x;
    posYmm=y;
}

void penUp(int pen, bool up=true){
    if(up){
        digitalWrite(pen_pins[pen], HIGH);
    }
    else{
        digitalWrite(pen_pins[pen], LOW);
    }
}

#endif // UTILS_H_INCLUDED