/* ================= REQUIRED LIBRARIES =============== */
/*
 *
 *
 */
#include <SerialCommand.h>
#include <SoftwareSerial.h>
#include "FocheggiatoreLib.h"
#include "config.h"

SerialCommand SCmd;

/* ====================================================== */
/* ===================  CLASS DEFINE  =================== */
/* ====================================================== */

//  FOC_LIB_INI

Focheggiatore   foc = Focheggiatore(   PIN_FOC_ENABLE, PIN_FOC_DIR, PIN_FOC_STEP, PIN_FOC_SELECT, PIN_FOC_FC_UP, PIN_FOC_FC_DOWN, PIN_FOC_HOME, PIN_MOSI, PIN_MISO, PIN_SCK );


void setup()
{

  // Inizializzo Seriale
  Serial.begin( SERIAL_BOUND_RATE );


  // Inizializzo Focheggiatore
  foc.begin();


  /* ===================  SERIAL COMANDS  =================== */

  // MOTORI
  SCmd.addCommand("CMI",infinito);

/*
*
* MAIN LOOP FUNC
*
*/
void loop()
{
   // Handler comandi seriale
   SCmd.readSerial();

   // Routine per Focheggiatore
   foc.singleSteps();

}




/* ================================================================== */
/* ======================== CAMANDS CALLBACKS  ====================== */
/* ================================================================== */



/* ===========================  MOTORI  ============================= */
/*
 *
 *
 *
 */

/* ===============   INFINITY MOVE  ============ */

/*
 *
 *
  */
void infinito(){

    char* id = SCmd.next();
    char* dir = SCmd.next();
    if( *id == FOC_MOT_ID ){
      foc.move( *dir, 200 , 1 );
    }
    Serial.println("ok-CMI");
}


/* ====== HELPERS ====== */

long int convertToInt( char* vett ) {
  String appoString = vett;
  long int num =  appoString.toInt();
  return num;
}