#include <AccelStepper.h>

#define BAUD (9600) // How fast is the Arduino talking?
#define MAX_BUF (64) // What is the longest message Arduino can store?

//------------------------------------------------------------------------------
// GLOBALS
//------------------------------------------------------------------------------

char  serialBuffer[MAX_BUF];  // where we store the message until we get a newline
int   sofar;            // how much is in the serialBuffer
float px, py, pz;      // location
float px_mm, py_mm, pz_mm; // location_mm

// speeds
float fr =     0;  // human version
//long  step_delay;  // machine version

// settings
char mode_abs = 1; // absolute mode?

typedef struct {
  int step_pin;
  int dir_pin;
  int enable_pin;
  int limit_switch_pin;
} Motor;

#define NUM_AXIES 3
Motor stepperMotor[NUM_AXIES];

//***** Stepper X
#define X_MicroStep 16
#define X_StepPerRound 200
byte X_PUL = 8;
byte X_DIR = 9;
byte X_EN = 10;
byte X_ENDSTOP_MIN = 2 ;
byte X_ENDSTOP_MAX = 3 ;
float X_STEPS_PER_MM = 100;

byte Y_PUL = 11;
byte Y_DIR = 12;
byte Y_EN = 13;
byte Y_ENDSTOP_MIN = 4 ;
byte Y_ENDSTOP_MAX = 5 ;
float Y_STEPS_PER_MM = 100;

byte Z_PUL = 14;
byte Z_DIR = 15;
byte Z_EN = 16;
byte Z_ENDSTOP_MIN = 6 ;
byte Z_ENDSTOP_MAX = 7 ;
float Z_STEPS_PER_MM = 100;

AccelStepper X_motor(1,X_PUL,X_DIR); //1 driver driven mode
AccelStepper Y_motor(1,Y_PUL,Y_DIR);
AccelStepper Z_motor(1,Z_PUL,Z_DIR);

int X_direction = 1;
int X_Target_Speed = 0;

const unsigned int MAX_SPEED = 400; //to be set - step/s
const unsigned int MAX_ACCELERATION = 800; //to be set - step/s2

unsigned int SPEED = MAX_SPEED; // calculated and updated from feedrate
unsigned int X_SPEED = MAX_SPEED; // calculated and updated from feedrate
unsigned int Y_SPEED = MAX_SPEED; // calculated and updated from feedrate
unsigned int Z_SPEED = MAX_SPEED; // calculated and updated from feedrate


//RotateControl Stepper_Controller;
//boolean Set_Rotate_Controller = false;

#define MM_MODE (1)

//------------------------------------------------------------------------------
// METHODS
//------------------------------------------------------------------------------
/**
   parse G-CODE message
   @ input code - char to find, next to the required value val
   @ input 
*/
float parseNumber(char code, float val) {
  char *ptr = serialBuffer; // start at the beginning of buffer
  while ((long)ptr > 1 && (*ptr) && (long)ptr < (long)serialBuffer + sofar) { // walk to the end
    if (*ptr == code) { // if you find code on your walk,
      return atof(ptr + 1); // convert the digits that follow into a float and return it
    }
    ptr = strchr(ptr, ' ') + 1; // take a step from here to the letter after the next space
  }
  return val;  // end reached, nothing found, return default val.
}

/**
   delay for the appropriate number of microseconds
   @input ms how many milliseconds to wait
*/
void pause(long ms) {
  delay(ms / 1000);
  delayMicroseconds(ms % 1000); // delayMicroseconds doesn't work for values > ~16k.
}

/**
   Start the required G-CODE process
*/
void processCommand() {
  int cmd = parseNumber('G', -1);

  switch (cmd) {
    case  0:
      Serial.println("Caso G0");
      break;
    case  1: { // line
        feedrate(parseNumber('F', fr));
        
        float old_pz = pz;
        #ifdef MM_MODE
          old_pz = pz_mm;
        #endif
        float pz_new = parseNumber('Z', (mode_abs ? old_pz : 0)) + (mode_abs ? 0 : old_pz);
        #ifdef MM_MODE
          pz_new = mm_to_steps(pz_new, Z_STEPS_PER_MM);
        #endif

        float old_px = px;
        float old_py = py;
        #ifdef MM_MODE
          old_px = px_mm;
          old_py = py_mm;
        #endif
        float px_new = parseNumber('X', (mode_abs ? old_px : 0)) + (mode_abs ? 0 : old_px);
        float py_new = parseNumber('Y', (mode_abs ? old_py : 0)) + (mode_abs ? 0 : old_py);
        #ifdef MM_MODE
          px_new = mm_to_steps(px_new, X_STEPS_PER_MM);
          py_new = mm_to_steps(py_new, Y_STEPS_PER_MM);
        #endif
        
        // verticalLine() and orizontalLine() requires values in steps
        verticalLine(pz_new);
        horizontalLine(px_new, py_new);
        //horizontalLine( parseNumber('X', (mode_abs ? px : 0)) + (mode_abs ? 0 : px),
        //                parseNumber('Y', (mode_abs ? py : 0)) + (mode_abs ? 0 : py) );

        break;
      }
    case 28:  { // homing
      motor_homing(X_motor, X_ENDSTOP_MIN);
      motor_homing(Y_motor, Y_ENDSTOP_MIN);
      motor_homing(Z_motor, Z_ENDSTOP_MIN);
      motor_updatePosition(); //update px, py and pz values
      }
    case 90:
      mode_abs = 1;
      break;  // absolute mode
    case 91:
      mode_abs = 0;
      break;  // relative mode
    default:
      break;
  }

  cmd = parseNumber('M', -1);
  switch (cmd) {
    case 18:  // disable motors
    /*
      disable();
      break;
    */
    case 92:  {/* help();  break; */
      X_STEPS_PER_MM = parseNumber('X', X_STEPS_PER_MM);
      Y_STEPS_PER_MM = parseNumber('Y', Y_STEPS_PER_MM);
      Z_STEPS_PER_MM = parseNumber('Z', Z_STEPS_PER_MM);
    }
    case 100:  /* help();  break; */
    case 114:  /* where();  break; */
    default:
      break;
  }
}
void ready() {
  sofar = 0; // clear input buffer
  Serial.print(F(">"));  // signal ready to receive input
}


//------------------------------------------------------------------------------
// SETUP AND LOOP
//------------------------------------------------------------------------------

void setup() {
  Serial.begin(BAUD); // open coms
  motor_setup();
  //help(); // say hello
  //set_feedrate(200); // set default speed
  //ready();
  Serial.println("Test Comunicazione GCODE");
  Serial.print(">");
}

void loop() {
  // listen for commands
  if ( Serial.available() ) { // if something is available
    char c = Serial.read(); // get it
    Serial.print(c); // optional: repeat back what I got for debugging

    // store the byte as long as there's room in the buffer.
    // if the buffer is full some data might get lost
    if (sofar < MAX_BUF) serialBuffer[sofar++] = c;
    // if we got a return character (\n) the message is done.
    if (c == '\n') {
      //Serial.print(F("\r\n")); // optional: send back a return for debugging

      // strings must end with a \0.
      serialBuffer[sofar] = 0;
      processCommand(); // do something with the command
      ready();
    }
  }
}