#ifndef COMMANDPARSERS_H_INCLUDED
#define COMMANDPARSERS_H_INCLUDED

#include <stdio.h>
#include "selfreset.h"

void cmdM118(String s){
    Serial.println(s);
}

void cmdG0(double x, double y){
    goTo(x, y, SPEED_FAST);
    cmdM118(String("OK"));
}

void cmdG1(double x, double y){
    goTo(x, y, SPEED_NORMAL);
    cmdM118("OK");
}

void cmdG4(double p){
    delay(p);
    cmdM118("OK");
}

void cmdG28(){
    if(XHOME){
        while(digitalRead(XHOME)!=true){
            stepperX.step(-STEP_INCREMENT);
        }
    }
    if(YHOME){
        while(digitalRead(YHOME)!=true){
            stepperY.step(-STEP_INCREMENT);
        }
    }
    posX=0;
    posY=0;
    cmdM118("OK");
}

void cmdG90(){
    relative=false;
    cmdM118("OK");
}

void cmdG91(){
    relative=true;
    cmdM118("OK");
}

void cmdT(int t){
    if(tool){
        penUp(tool, true);
    }
    if(t<=PENCOUNT){
        penUp(t, false);
        tool=t;
    }
    else{
        tool=0;
    }
    cmdM118("OK");
}

void cmdM2(){
    Serial.println("PROGRAM END");
}

void cmdM112(){
    selfReset();
}

void cmdM114(){
    char s[50];
    sprintf(s, "X:%f Y:%f", posXmm, posYmm);
    Serial.println(s);
}

void cmdM115(){
    char s[500];
    sprintf(s, "{\"software\":\"%s\", \"power\":%b, \"length\":%i, \"width\":%i", FIRMWARE, true, MAX_Y, MAX_X);
    Serial.println(s);
}

#endif // COMMANDPARSERS_H_INCLUDED