#include "../include/GRBL.h"

GRBL::GRBL(int grblAddr){
    this->grbl = new Module_GRBL(grblAddr);
}

void GRBL::init(int speed, double pas, int accel, String mode){
    char s[1024];

    this->grbl->Init(&Wire);
    this->grbl->setMode(mode);

    sprintf(s,"$0=%f", pas);  // step/mm
    this->grbl->sendGcode(s);
    Serial.println(s);

    sprintf(s,"$4=%d", speed);  // speed
    this->grbl->sendGcode(s);
    Serial.println(s);

    sprintf(s,"$8=%d", accel);  // acceleration, mm/sec^2
    this->grbl->sendGcode(s);
    Serial.println(s);

    sprintf(s,"$3=%d", 500);  // puse/µsec
    this->grbl->sendGcode(s);
    Serial.println(s);
}

void GRBL::mouveForward(int mm){
    char s[1024];
    sprintf(s, "G1 X%d", mm);
    this->grbl->sendGcode(s);
}

bool GRBL::isIddle(){
    bool sortie = false;

    if(this->grbl->readStatus().indexOf("IDLE") != -1){
        sortie = true;
    }

    return sortie;
}