Minimalist XY firmware for arduino leonardo + CNC Shield

everything is hardcoded
the commands contain relative movements
there's no units or you could say the units are motor steps

only XY are taken in considerations

to move the pen by ( 12,23 ) steps the command is

G0 X12 Y23 Z0

//the basic hardware control was inspired HC_CNC_Shield_Test by Andrew Davies

#define EN 8

#define X_STEP 2
#define X_DIR 5 
#define Y_STEP 3
#define Y_DIR 6
#define Z_STEP 4
#define Z_DIR 7

#define A_DIR 13  /* Direction pin for Aux driver. Requires D13 and A-DIR pins to be shorted */
#define A_STEP 12 /* Direction pin for Aux driver. Requires D12 and A-STEP pins to be shorted */

#define X_ENDSTOP 9   /* X axis endstop input pin */
#define Y_ENDSTOP 10  /* Y axis endstop input pin */
#define Z_ENDSTOP 11  /* Z axis endstop input pin */
#define ABORT A0  /* Abort input pin */
#define HOLD A1   /* Hold input pin */
#define RESUME A2 /* Resume input pin */

#define COMMAND_SIZE 128
char command[COMMAND_SIZE];
byte serial_count = 0;
boolean bytes_received = false;

void setup()
{
    /* Configure the steper drive pins as outputs */ 
  pinMode(EN, OUTPUT); 
  pinMode(X_DIR, OUTPUT); 
  pinMode(X_STEP, OUTPUT); 
  pinMode(Y_DIR, OUTPUT); 
  pinMode(Y_STEP, OUTPUT); 
  pinMode(Z_DIR, OUTPUT); 
  pinMode(Z_STEP, OUTPUT); 
  pinMode(A_DIR, OUTPUT); 
  pinMode(A_STEP, OUTPUT); 
  
  /* Configure the control pins as inputs with pullups */
  pinMode(X_ENDSTOP, INPUT_PULLUP);
  pinMode(Y_ENDSTOP, INPUT_PULLUP);
  pinMode(Z_ENDSTOP, INPUT_PULLUP);

  pinMode(ABORT, INPUT_PULLUP);
  pinMode(HOLD, INPUT_PULLUP);
  pinMode(RESUME, INPUT_PULLUP);

  /* Enable the X, Y, Z & Aux stepper outputs */
  digitalWrite(EN, HIGH); //Low to enable
  Serial.begin(9600);
}

void loop()
{
  char c;
  if( Serial.available() > 0 )
  {
    c = Serial.read();
    if( c != '\n' )
    {
        command[serial_count] = c;
        serial_count++;
    }
    else
    {
        command[serial_count] = 0;
        serial_count++;
    }
    bytes_received = true;
  }
  if( bytes_received && ( c == '\n' ) )
  {
    doit( command );
    serial_count = 0;
    bytes_received = false;
  }
}

void doit( char *input )
{
  char* command = strtok( input, " \n" );
  int G_val = -1;
  long X_val = 0;
  long Y_val = 0;
  long Z_val = 0;
  while( command != 0 )
  {
    switch( command[0] )
    {
      case 'G':
        G_val = atoi( command + 1 );
      break;
      case 'X':
        X_val = atol( command + 1 );
      break;
      case 'Y':
        Y_val = atol( command + 1 );
      break;
      case 'Z':
        Z_val = atol( command + 1 );
      break;
    }
    command = strtok(0, " \n" );
  }
  char mesg[32];
  sprintf( mesg, "%d %ld %ld %ld", G_val, X_val, Y_val, Z_val );
  Serial.println( mesg );
  digitalWrite(EN, LOW); //Low to enable
  motorizin( G_val, X_val, Y_val, Z_val );
  digitalWrite(EN, HIGH); //Low to enable
}

//------------------------------------------------------------------------------
void motorizin( int G_val, long X_val, long Y_val, long Z_val )
{
  switch( G_val )
  {
    case 0:
    case 1:
      line( X_val, Y_val, Z_val );
    break;
  }
}

//------------------------------------------------------------------------------
//first version of this program - only relative moves supported
//------------------------------------------------------------------------------
void line( long dx, long dy, long dz )
{
  digitalWrite( X_DIR, dx >= 0?LOW:HIGH );  
  digitalWrite( Y_DIR, dy >= 0?LOW:HIGH ); 
  long adx = abs( dx );
  long ady = abs( dy );
  if( adx >= ady ) 
  {
    subplot( 0, adx, ady );
  }
  else
  {
    subplot( 1, ady, adx );
  }
}

//------------------------------------------------------------------------------
void subplot( bool horizVert, long dx, long dy )
{  
  long D = 2 * dy - dx;
  bool stepy = false;
  for( long x = 1; x <= dx; x++ )
  {
    if( D > 0 )
    {
       stepy = true;
       D = D - dx;
    }
    D = D + dy;
    if( horizVert == 0 )
    {
      subsubplot( true, stepy );
  }
  else
  {
      subsubplot( stepy, true );
  }    
  stepy = false;
  }
}

//------------------------------------------------------------------------------
void subsubplot( long stepx, long stepy )
{
  if( stepx > 0 )
  {
    digitalWrite( X_STEP, HIGH ); 
  }
  if( stepy > 0 )
  {
    digitalWrite( Y_STEP, HIGH ); 
  }
  delay( 10 );
  digitalWrite( X_STEP, LOW );  
  digitalWrite( Y_STEP, LOW ); 
}