/*
 * Drillcon Version 1.0
 * Copyright (C) 2005 Nicholas Redgrave
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#include <pthread.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
#include <time.h>
#include <errno.h>
#include <comedilib.h>



/* Commands */
#define IDLE            0   /* idle command */
#define ACTIVATE        1   /* activate command sequence */

#define EOT             1   /* end of command table sequence */
#define MOVE_TO         2   /* move to */
#define LINEAR_TO       3   /* move linearly to */
#define DRILL_OFF       4   /* drill off */
#define DRILL_ON        5   /* drill on */
#define DRILL_RETRACT   6   /* drill retract */
#define DRILL_PLUNGE    7   /* drill plunge */
#define ABORT           8   /* abort and turn everything off */
#define X_MOVE          9   /* move x axis */
#define Y_MOVE         10   /* move y axis */
#define Z_MOVE         11   /* move z axis */

/* Command Statuses */
#define STATUS_IDLE     0   /* no command */
#define STATUS_FAIL     1   /* command failed */
#define STATUS_OKAY     2   /* command successful */
#define STATUS_PENDING  3   /* command pending */

/* Additional statuses */
#define DRILL_MODE      1   /* drill mode */
#define ROUTE_MODE      2   /* route mode */

/* Axis statuses */
#define AXIS_IDLE       0   /* axis inactive */
#define AXIS_SEEK       1   /* axis in motion */

#define X_AXIS          0
#define Y_AXIS          1
#define Z_AXIS          2


#define FILE_CMD_SIZE   81  /* number of chars in file command line */

/* axis drive parameter structure */
typedef struct Axis_Parameters
{
  double dXDist_per_Step;     /* x-axis mm distance moved per half-step */
  double dYDist_per_Step;     /* y-axis mm distance moved per half-step */
  double dZDist_per_Step;     /* z-axis mm distance moved per half-step */
  int iZUp;                   /* z-axis up step count */
  int iZDown;                 /* z-axis down step count */

} Axis_Parameters;


typedef struct axis_control
{
  int iAxis_Status;           /* status of axis control */
  int iRef_Position;          /* reference position of axis in half-steps */
  int iCommand_Steps;         /* requested movement in half-steps */
  int iStep_Table_Pos;        /* position in stepping table */
  unsigned int uiControlData; /* coil activation data */
  double dRef_Position;       /* reference position of axis in mm */

} axis_control;


typedef struct Drill_Status
{
  int iCurrentX;              /* current x axis position */
  int iCurrentY;              /* current y axis position */
  int iCurrentZ;              /* current z axis position */
  double dCurrentX;           /* current x axis position in mm */
  double dCurrentY;           /* current y axis position in mm */
  double dCurrentZ;           /* current z axis position in mm */
  int iDrillMode;             /* drilling or routing */
  int iDrillPower;            /* drill on or off */
  int iDrillPosition;         /* drill retracted or plunged */

} Drill_Status;


typedef struct Route_Control
{
  int iXStart;                /* X axis start position */
  int iYStart;                /* Y axis start position */
  int iXEnd;                  /* X axis end position */
  int iYEnd;                  /* Y axis end position */
  int iStep;                  /* X step (+1 or -1) */
  double dm;                  /* gradient (as in y=mx+c) */
  double dc;                  /* intercept (as in y=mx+c) */

} Route_Control;


typedef struct Drill_Control
{
  int iCommand;               /* command */
  int iStatus;                /* status of command */
  int iCurrentCmd;            /* current command in stack */
  int iStackPtr;              /* pointer to next command */
  int iCmdStack[10];          /* command stack */

} Drill_Control;



/* Global variables */
comedi_t *device;
const int subdevice = 0;
pthread_t control_loop_thread_id;
pthread_t input_loop_thread_id;
struct sched_param scp;

Axis_Parameters axis_parameters;
Drill_Status drill_status;
Drill_Control drill_control;

axis_control axis_control_x;
axis_control axis_control_y;
axis_control axis_control_z;
char chDrillFile[FILE_CMD_SIZE];
char chFileCommand[FILE_CMD_SIZE];
char chCmd[FILE_CMD_SIZE];


int iCThread_Terminate = 0;
int iIThread_Terminate = 0;
unsigned int uiStepTable[8]={9,8,10,2,6,4,5,1};
int iTool = 0;
int iDrillFile = 0;
FILE * fpCmd = NULL;

/* Prototypes */
void * Control_Loop(void * unused);
void Move_Axis(int iAxis);
int Analyse_Command(char * chCmd);
int Open_Command_File(char * chCmdFile);
void Close_Command_File(void);
int Read_Command_File(char * chCmd);
void strupr(char * c);
void * Input_Loop(void * unused);
int main (void);




/* ============================================================================ */



/* Control Loop Thread Function */

void * Control_Loop(void * unused)
{
  static unsigned int mask = 0xFFF;
  static int iLocalStatus = STATUS_IDLE;
  static int iLocalCommand = IDLE;
  static Route_Control route_control;
  axis_control * ptr_axis_control = NULL;
  unsigned int data;
  int iAbsPos;
  struct timespec timewait;
  struct timespec timerem;

  /* Set up time wait for nanosleep */
  timewait.tv_sec = 0;
  timewait.tv_nsec = 2100000; /* okay - go no lower! */


  while( iCThread_Terminate == 0 )
  {

    /* Check for new command sequence */
    if( drill_control.iCommand == ACTIVATE )
    {
      /* process new command sequence */
      drill_control.iStackPtr = -1;
      drill_control.iCurrentCmd = IDLE;
      drill_control.iStatus = STATUS_PENDING;
      drill_control.iCommand = IDLE;
      iLocalStatus = STATUS_IDLE;
    }


    /* Is a command sequence in progress? */
    if( drill_control.iStatus == STATUS_PENDING )
    {
      /* monitor and update command sequence */
      if( iLocalStatus == STATUS_IDLE )
      {
        drill_control.iStackPtr++;

        switch( drill_control.iCmdStack[drill_control.iStackPtr] )
        {
          case( EOT ):
          {
            /* command sequence complete */
            iLocalCommand = EOT;
            drill_control.iStatus = STATUS_OKAY;
printf("\nEOT\n");
            break;
          }

          case( ABORT ):
          {
            /* stop drill and cancel all moves */
#ifndef TESTING
            comedi_dio_write(device,subdevice,12,0);
#endif
            drill_status.iDrillPower = DRILL_OFF;
            axis_control_x.iCommand_Steps = 0;
            axis_control_x.iAxis_Status = AXIS_IDLE;
            axis_control_y.iCommand_Steps = 0;
            axis_control_y.iAxis_Status = AXIS_IDLE;
            axis_control_z.iCommand_Steps = 0;
            axis_control_z.iAxis_Status = AXIS_IDLE;
            drill_control.iStatus = STATUS_OKAY;
            iLocalStatus = STATUS_IDLE;
printf("\nABORT\n");
            break;
          }

          case( DRILL_ON ):
          {
#ifndef TESTING
            comedi_dio_write(device,subdevice,12,1);
#endif
            drill_status.iDrillPower = DRILL_ON;
printf("\nDRILL_ON\n");
            break;
          }

          case( DRILL_OFF ):
          {
#ifndef TESTING
            comedi_dio_write(device,subdevice,12,0);
#endif
            drill_status.iDrillPower = DRILL_OFF;
printf("\nDRILL_OFF\n");
            break;
          }

          case( DRILL_PLUNGE ):
          {
            iLocalCommand = DRILL_PLUNGE;
            iLocalStatus = STATUS_PENDING;
            axis_control_z.iCommand_Steps = - (drill_status.iCurrentZ - axis_parameters.iZDown);
            axis_control_z.iAxis_Status = AXIS_SEEK;
printf("\nDRILL_PLUNGE\n");
            break;
          }

          case( DRILL_RETRACT ):
          {
            iLocalCommand = DRILL_RETRACT;
            iLocalStatus = STATUS_PENDING;
            axis_control_z.iCommand_Steps = axis_parameters.iZUp - drill_status.iCurrentZ;
            axis_control_z.iAxis_Status = AXIS_SEEK;
printf("\nDRILL_RETRACT\n");
            break;
          }

          case( X_MOVE ):
          {
            iLocalCommand = X_MOVE;
            iLocalStatus = STATUS_PENDING;
            /* Get x coordinate */
            drill_control.iStackPtr++;
            axis_control_x.iCommand_Steps = drill_control.iCmdStack[drill_control.iStackPtr];
            axis_control_x.iAxis_Status = AXIS_SEEK;
printf("\nX_MOVE\n");
            break;
          }

          case( Y_MOVE ):
          {
            iLocalCommand = Y_MOVE;
            iLocalStatus = STATUS_PENDING;
            /* Get y coordinate */
            drill_control.iStackPtr++;
            axis_control_y.iCommand_Steps = drill_control.iCmdStack[drill_control.iStackPtr];
            axis_control_y.iAxis_Status = AXIS_SEEK;
printf("\nY_MOVE\n");
            break;
          }

          case( Z_MOVE ):
          {
            iLocalCommand = Z_MOVE;
            iLocalStatus = STATUS_PENDING;
            /* Get z coordinate */
            drill_control.iStackPtr++;
            axis_control_z.iCommand_Steps = drill_control.iCmdStack[drill_control.iStackPtr];
            axis_control_z.iAxis_Status = AXIS_SEEK;
printf("\nZ_MOVE\n");
            break;
          }

          case( MOVE_TO ):
          {
            iLocalCommand = MOVE_TO;
            iLocalStatus = STATUS_PENDING;
            /* Get x and y coordinates */
            drill_control.iStackPtr++;
            iAbsPos = drill_control.iCmdStack[drill_control.iStackPtr];
            axis_control_x.iCommand_Steps = iAbsPos - drill_status.iCurrentX;
            axis_control_x.iAxis_Status = AXIS_SEEK;
            drill_control.iStackPtr++;
            iAbsPos = drill_control.iCmdStack[drill_control.iStackPtr];
            axis_control_y.iCommand_Steps = iAbsPos - drill_status.iCurrentY;
            axis_control_y.iAxis_Status = AXIS_SEEK;
printf("\nMOVE_TO\n");
printf("x steps: %d\n",axis_control_x.iCommand_Steps);
printf("y steps: %d\n",axis_control_y.iCommand_Steps);
            break;
          }

          case( LINEAR_TO ):
          {
            iLocalCommand = LINEAR_TO;
            iLocalStatus = STATUS_PENDING;
            /* Get x and y coordinates */
            drill_control.iStackPtr++;
            iAbsPos = drill_control.iCmdStack[drill_control.iStackPtr];
            route_control.iXStart = drill_status.iCurrentX;
            route_control.iXEnd = iAbsPos;
            drill_control.iStackPtr++;
            iAbsPos = drill_control.iCmdStack[drill_control.iStackPtr];
            route_control.iYStart = drill_status.iCurrentY;
            route_control.iYEnd = iAbsPos;

            /* Can we convert to a single axis move? */
            if( route_control.iXStart == route_control.iXEnd )
            {
              axis_control_y.iCommand_Steps = route_control.iYEnd - route_control.iYStart;
              axis_control_y.iAxis_Status = AXIS_SEEK;
              iLocalCommand = MOVE_TO;
printf("\nLINEAR_TO -> MOVE_TO\n");
printf("y steps: %d\n",axis_control_y.iCommand_Steps);
            }
            else if( route_control.iYStart == route_control.iYEnd )
            {
              axis_control_x.iCommand_Steps = route_control.iXEnd - route_control.iXStart;
              axis_control_x.iAxis_Status = AXIS_SEEK;
              iLocalCommand = MOVE_TO;
printf("\nLINEAR_TO -> MOVE_TO\n");
printf("x steps: %d\n",axis_control_x.iCommand_Steps);
            }
            else
            {
              /* Interpolated linear move required */
              route_control.dm = ( (double)(route_control.iYEnd - route_control.iYStart) /
                                   (double)(route_control.iXEnd - route_control.iXStart) );
              route_control.dc = (double)route_control.iYStart - (route_control.dm * (double)route_control.iXStart);
              if( route_control.iXEnd > route_control.iXStart )
              {
                route_control.iStep = 1;
              }
              else
              {
                route_control.iStep = -1;
              }
printf("\nLINEAR_TO\n");
printf("m: %f  c:%f  step: %d\n",route_control.dm, route_control.dc, route_control.iStep);
            }
            break;
          }


        } /* end of switch(... */
        drill_control.iCurrentCmd = iLocalCommand;

      } /* end of if( iLocalStatus... */

    } /* end of if( drill_control.iStatus... */




    /* Control axes if required */
    if( axis_control_x.iAxis_Status != AXIS_IDLE )
    {
      Move_Axis(X_AXIS);
//      printf("Coil data: %u\n", axis_control_x.uiControlData);
    }
    data = axis_control_x.uiControlData;

    if( axis_control_y.iAxis_Status != AXIS_IDLE )
    {
      Move_Axis(Y_AXIS);
//      printf("Coil data: %u\n", axis_control_y.uiControlData);
    }
    data += (axis_control_y.uiControlData << 4);

    if( axis_control_z.iAxis_Status != AXIS_IDLE )
    {
      Move_Axis(Z_AXIS);
//      printf("Coil data: %u\n", axis_control_z.uiControlData);
    }
    data += (axis_control_z.uiControlData << 8);


    /* Apply stepping data */
#ifndef TESTING
    comedi_dio_bitfield(device,subdevice,mask,&data);
#endif
    if( nanosleep(&timewait, &timerem) == EINTR )
    {
      /* Wait was interrupted - try again */
      nanosleep(&timerem, &timerem);
    }




    /* Is the move complete? */
    if( iLocalStatus == STATUS_PENDING )
    {
      switch( iLocalCommand )
      {
        case( DRILL_PLUNGE ):
        {
          if( axis_control_z.iAxis_Status == AXIS_IDLE )
          {
            drill_status.iDrillPosition = DRILL_PLUNGE;
            iLocalStatus = STATUS_IDLE;
printf("\nPLUNGE complete\n");
          }
          break;
        }

        case( DRILL_RETRACT ):
        {
          if( axis_control_z.iAxis_Status == AXIS_IDLE )
          {
            drill_status.iDrillPosition = DRILL_RETRACT;
            iLocalStatus = STATUS_IDLE;
printf("\nRETRACT complete\n");
          }
          break;
        }

        case( X_MOVE ):
        {
          if( axis_control_x.iAxis_Status == AXIS_IDLE )
          {
            iLocalStatus = STATUS_IDLE;
printf("\nX_MOVE complete\n");
          }
          break;
        }

        case( Y_MOVE ):
        {
          if( axis_control_y.iAxis_Status == AXIS_IDLE )
          {
            iLocalStatus = STATUS_IDLE;
printf("\nY_MOVE complete\n");
          }
          break;
        }

        case( Z_MOVE ):
        {
          if( axis_control_z.iAxis_Status == AXIS_IDLE )
          {
            drill_status.iDrillPosition = DRILL_RETRACT;
            iLocalStatus = STATUS_IDLE;
printf("\nZ_MOVE complete\n");
          }
          break;
        }

        case( MOVE_TO ):
        {
          if( (axis_control_x.iAxis_Status == AXIS_IDLE) &&
              (axis_control_y.iAxis_Status == AXIS_IDLE) )
          {
            iLocalStatus = STATUS_IDLE;
printf("\nMOVE_TO complete\n");
          }
          break;
        }

        case( LINEAR_TO ):
        {
          if( (axis_control_x.iAxis_Status == AXIS_IDLE) &&
              (axis_control_y.iAxis_Status == AXIS_IDLE) )
          {
            if( drill_status.iCurrentX == route_control.iXEnd )
            {
              /* X Move complete  - is Y move complete? */
              if( drill_status.iCurrentY == route_control.iYEnd )
              {
                iLocalStatus = STATUS_IDLE;
printf("\nLINEAR_TO complete\n");
              }
              else
              {
                /* We have a rounding error - correct it */
                axis_control_y.iCommand_Steps = route_control.iYEnd - drill_status.iCurrentY;
                axis_control_y.iAxis_Status = AXIS_SEEK;
printf("\nLINEAR_TO correct rounding error\n");
              }
            }
            else
            {
              /* Calculate new move position */
              axis_control_x.iCommand_Steps = route_control.iStep;
              axis_control_x.iAxis_Status = AXIS_SEEK;
              iAbsPos = (int)((route_control.dm * (double)drill_status.iCurrentX) + route_control.dc);
              axis_control_y.iCommand_Steps = iAbsPos - drill_status.iCurrentY;
              axis_control_y.iAxis_Status = AXIS_SEEK;
printf("\nLINEAR_TO new segment\n");
printf("x steps: %d  y steps: %d\n",axis_control_x.iCommand_Steps, axis_control_y.iCommand_Steps);
            }
          }
          break;
        }

      } /* end of switch... */

    } /* end of if( iLocalStatus == STATUS_PENDING ) */

    if( drill_control.iStatus == STATUS_OKAY )
    {
      /* We're not doing anything - reschedule */
      (void)sched_yield();
      usleep(20);
    }


  } /* end of while( iCThread_Terminate == 0 ) */


  return NULL;
}
/* ---------------------------------------------------------------------------- */


void Move_Axis(int iAxis)
{
  axis_control * ptr_axis_control;
  int iStep = 0;


  switch( iAxis )
  {
    case(X_AXIS):
    {
      /* x axis */
      ptr_axis_control = &axis_control_x;
      break;
    }
    case(Y_AXIS):
    {
      /* y axis */
      ptr_axis_control = &axis_control_y;
      break;
    }
    case(Z_AXIS):
    {
      /* z axis */
      ptr_axis_control = &axis_control_z;
      break;
    }
  }



  /* Is movement complete? */
  if( ptr_axis_control->iCommand_Steps == 0 )
  {
    ptr_axis_control->iAxis_Status = AXIS_IDLE;
    return;
  }


  /* Clockwise or anti-clockwise? */
  if( ptr_axis_control->iCommand_Steps > 0 )
  {
    ptr_axis_control->iStep_Table_Pos++;
    ptr_axis_control->iStep_Table_Pos %= 8;
    ptr_axis_control->uiControlData = uiStepTable[ptr_axis_control->iStep_Table_Pos];
    ptr_axis_control->iCommand_Steps--;
    iStep = 1;
  }
  else
  {
    ptr_axis_control->iStep_Table_Pos--;
    ptr_axis_control->iStep_Table_Pos += 8;
    ptr_axis_control->iStep_Table_Pos %= 8;
    ptr_axis_control->uiControlData = uiStepTable[ptr_axis_control->iStep_Table_Pos];
    ptr_axis_control->iCommand_Steps++;
    iStep = -1;
  }



  switch( iAxis )
  {
    case(X_AXIS):
    {
      /* x axis */
      drill_status.iCurrentX += iStep;
      drill_status.dCurrentX = (double)drill_status.iCurrentX * axis_parameters.dXDist_per_Step;
      break;
    }
    case(Y_AXIS):
    {
      /* y axis */
      drill_status.iCurrentY += iStep;
      drill_status.dCurrentY = (double)drill_status.iCurrentY * axis_parameters.dYDist_per_Step;
      break;
    }
    case(Z_AXIS):
    {
      /* z axis */
      drill_status.iCurrentZ += iStep;
      drill_status.dCurrentZ = (double)drill_status.iCurrentZ * axis_parameters.dZDist_per_Step;
      break;
    }
  }



}
/* ---------------------------------------------------------------------------- */


int Analyse_Command(char * chCmd)
{
  static int iHeader = 0;
  float ftmp1, ftmp2;
  double dXPos, dYPos;
  int iXPos, iYPos, iZPos;


  /* Non-Excellon commands */
  if( strncasecmp("PCP", chCmd, 3) == 0 )
  {
    /* Print current position */
    printf( "Cmd:%d x:%d y:%d z:%d x:%1.3fmm y:%1.3fmm z:%1.3fmm Route:%d Power:%d Plunged:%d  \n",
            drill_control.iCurrentCmd,
            drill_status.iCurrentX,
            drill_status.iCurrentY,
            drill_status.iCurrentZ,
            drill_status.dCurrentX,
            drill_status.dCurrentY,
            drill_status.dCurrentZ,
            drill_status.iDrillMode,
            drill_status.iDrillPower - DRILL_OFF,
            drill_status.iDrillPosition - DRILL_RETRACT);
    return 0;
  }

  if( strncasecmp("XMV", chCmd, 3) == 0 )
  {
    /* Move x axis by mm */
    sscanf(chCmd, "XMV%f", &ftmp1);
    printf("Move x axis by %fmm\n",ftmp1);
    iXPos = (int)((double)ftmp1 / axis_parameters.dXDist_per_Step);
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = X_MOVE;
    drill_control.iCmdStack[1] = iXPos;
    drill_control.iCmdStack[2] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  if( strncasecmp("YMV", chCmd, 3) == 0 )
  {
    /* Move y axis by mm */
    sscanf(chCmd, "YMV%f", &ftmp1);
    printf("Move y axis by %fmm\n",ftmp1);
    iYPos = (int)((double)ftmp1 / axis_parameters.dYDist_per_Step);
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = Y_MOVE;
    drill_control.iCmdStack[1] = iYPos;
    drill_control.iCmdStack[2] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  if( strncasecmp("ZMV", chCmd, 3) == 0 )
  {
    /* Raise/Lower drill by mm */
    sscanf(chCmd, "ZMV%f", &ftmp1);
    printf("Raise/Lower Drill by %fmm\n",ftmp1);
    iZPos = (int)((double)ftmp1 / axis_parameters.dZDist_per_Step);
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = Z_MOVE;
    drill_control.iCmdStack[1] = iZPos;
    drill_control.iCmdStack[2] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  if( strncasecmp("SPD", chCmd, 3) == 0 )
  {
    sscanf(chCmd, "SPDX%fY%f", &ftmp1, &ftmp2);
    printf("Set x,y position in drill mode (mm) to %f, %f\n", ftmp1, ftmp2);
    drill_status.dCurrentX = (double)ftmp1;
    drill_status.iCurrentX = (int)((double)ftmp1 / axis_parameters.dXDist_per_Step);
    drill_status.dCurrentY = (double)ftmp2;
    drill_status.iCurrentY = (int)((double)ftmp2 / axis_parameters.dYDist_per_Step);
    return 0;
  }

  if( strncasecmp("SPR", chCmd, 3) == 0 )
  {
    sscanf(chCmd, "SPRX%dY%d", &iXPos, &iYPos);
    printf("Set x,y position in route mode (steps) to %d, %d\n",iXPos, iYPos);
    drill_status.iCurrentX = iXPos;
    drill_status.dCurrentX = (double)iXPos * axis_parameters.dXDist_per_Step;
    drill_status.iCurrentY = iYPos;
    drill_status.dCurrentY = (double)iYPos * axis_parameters.dYDist_per_Step;
    return 0;
  }

  if( strncasecmp("DON", chCmd, 3) == 0 )
  {
    /* Drill power on */
    printf("Drill power on\n");
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = DRILL_ON;
    drill_control.iCmdStack[1] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  if( strncasecmp("DOFF", chCmd, 4) == 0 )
  {
    /* Drill power off */
    printf("Drill power off\n");
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = DRILL_OFF;
    drill_control.iCmdStack[1] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  if( strncasecmp("ABORT", chCmd, 5) == 0 )
  {
    /* Abort current command */
    printf("Abort\n");
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = ABORT;
    drill_control.iCmdStack[1] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  /* Zero x axis only */
  if( strncasecmp("ZX", chCmd, 2) == 0 )
  {
    printf("Set x zero point\n");
    drill_status.iCurrentX = 0;
    drill_status.dCurrentX = 0.0;
    return 0;
  }

  /* Zero y axis only */
  if( strncasecmp("ZY", chCmd, 2) == 0 )
  {
    printf("Set y zero point\n");
    drill_status.iCurrentY = 0;
    drill_status.dCurrentY = 0.0;
    return 0;
  }

  /* Zero z axis only */
  if( strncasecmp("ZZ", chCmd, 2) == 0 )
  {
    printf("Set z zero point\n");
    drill_status.iCurrentZ = 0;
    drill_status.dCurrentZ = 0.0;
    return 0;
  }


  /* Excellon commands */

  /* Header start */
  if( strncasecmp("M48", chCmd, 3) == 0 )
  {
    printf("Header start\n");
    iHeader = 1;
    return 0;
  }

  /* Header end */
  if( strncasecmp("%", chCmd, 1) == 0 )
  {
    printf("Header end\n");
    iHeader = 0;
    return 0;
  }

  /* Program end */
  if( strncasecmp("M30", chCmd, 3) == 0 )
  {
    printf("Program end\n");
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = DRILL_RETRACT;
    drill_control.iCmdStack[1] = DRILL_OFF;
    drill_control.iCmdStack[2] = MOVE_TO;
    drill_control.iCmdStack[3] = 0;
    drill_control.iCmdStack[4] = 0;
    drill_control.iCmdStack[5] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  /* Comment */
  if( strncasecmp(";", chCmd, 1) == 0 )
  {
    printf("Comment\n");
    return 0;
  }

  /* Metric */
  if( strncasecmp("METRIC", chCmd, 6) == 0 )
  {
    printf("Metric\n");
    return 0;
  }
  /* Metric */
  if( strncasecmp("M71", chCmd, 3) == 0 )
  {
    printf("Metric2\n");
    return 0;
  }

  /* Format */
  if( strncasecmp("FMAT,2", chCmd, 6) == 0 )
  {
    printf("Format\n");
    return 0;
  }

  /* Z axis up limit */
  if( strncasecmp("UP,", chCmd, 3) == 0 )
  {
    sscanf(&chCmd[3], "%f", &ftmp1);
    axis_parameters.iZUp = (int)((double)ftmp1 / axis_parameters.dZDist_per_Step);
    printf("Z Up Limit: %f mm  %d steps\n", ftmp1, axis_parameters.iZUp);
    return 0;
  }

  /* Z axis down limit */
  if( strncasecmp("DN,", chCmd, 3) == 0 )
  {
    sscanf(&chCmd[3], "%f", &ftmp1);
    axis_parameters.iZDown = (int)((double)ftmp1 / axis_parameters.dZDist_per_Step);
    printf("Z Down Limit: %f mm  %d steps\n", ftmp1, axis_parameters.iZDown);
    return 0;
  }

  /* Tool */
  if( strncasecmp("T", chCmd, 1) == 0 )
  {
    if( iHeader == 1 )
    {
      /* In header - ignore */
      printf("Tool\n");
      return 0;
    }
    else
    {
      /* Change tool - do nothing but wait for now */
      sscanf(&chCmd[1], "%d", &iTool);
      printf("Tool Change: %d\n",iTool);
      return 2;
    }
  }

  /* Drill plunge */
  if( strncasecmp("M15", chCmd, 3) == 0 )
  {
    printf("Drill plunge\n");
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = DRILL_ON;
    drill_control.iCmdStack[1] = DRILL_PLUNGE;
    drill_control.iCmdStack[2] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  /* Drill retract */
  if( strncasecmp("M16", chCmd, 3) == 0 )
  {
    printf("Drill retract\n");
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = DRILL_RETRACT;
    drill_control.iCmdStack[1] = DRILL_OFF;
    drill_control.iCmdStack[2] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  /* Drill position */
  if( strncasecmp("X", chCmd, 1) == 0 )
  {
    sscanf(chCmd, "X%fY%f", &ftmp1, &ftmp2);
    printf("Drill at X: %f Y: %f\n",ftmp1, ftmp2);
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = MOVE_TO;
    drill_control.iCmdStack[1] = (int)((double)ftmp1 / axis_parameters.dXDist_per_Step);
    drill_control.iCmdStack[2] = (int)((double)ftmp2 / axis_parameters.dYDist_per_Step);
    drill_control.iCmdStack[3] = DRILL_ON;
    drill_control.iCmdStack[4] = DRILL_PLUNGE;
    drill_control.iCmdStack[5] = DRILL_RETRACT;
    drill_control.iCmdStack[6] = DRILL_OFF;
    drill_control.iCmdStack[7] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }

  /* Set zero point */
  if( strncasecmp("Z", chCmd, 1) == 0 )
  {
    printf("Set zero point\n");
    drill_status.iCurrentX = 0;
    drill_status.iCurrentY = 0;
    drill_status.iCurrentZ = 0;
    drill_status.dCurrentX = 0.0;
    drill_status.dCurrentY = 0.0;
    drill_status.dCurrentZ = 0.0;
    return 0;
  }

  /* Enter drilling mode */
  if( strncasecmp("G05", chCmd, 3) == 0 )
  {
    printf("Drilling Mode\n");
    drill_status.iDrillMode = DRILL_MODE;
    return 0;
  }

  /* Move to Routing position */
  if( strncasecmp("G00", chCmd, 3) == 0 )
  {
    drill_status.iDrillMode = ROUTE_MODE;
    if( iHeader == 1 )
    {
      /* Enter routing mode */
      printf("Routing Mode\n");
      return 0;
    }
    else
    {
      /* Move to position */
      if( drill_status.iDrillPosition == DRILL_PLUNGE )
      {
        /* non-linear move while plunged! don't do it! */
        return -1;
      }
      sscanf(chCmd, "G00X%dY%d", &iXPos, &iYPos);
      printf("Move to X: %d Y: %d\n",iXPos, iYPos);
      drill_control.iStatus = STATUS_IDLE;
      drill_control.iCmdStack[0] = MOVE_TO;
      drill_control.iCmdStack[1] = iXPos;
      drill_control.iCmdStack[2] = iYPos;
      drill_control.iCmdStack[3] = EOT;
      drill_control.iCommand = ACTIVATE;
      return 1;
    }
  }

  /* Route to position */
  if( strncasecmp("G01", chCmd, 3) == 0 )
  {
    /* Route to position */
    sscanf(chCmd, "G01X%dY%d", &iXPos, &iYPos);
    printf("Route to X: %d Y: %d\n",iXPos, iYPos);
    drill_control.iStatus = STATUS_IDLE;
    drill_control.iCmdStack[0] = LINEAR_TO;
    drill_control.iCmdStack[1] = iXPos;
    drill_control.iCmdStack[2] = iYPos;
    drill_control.iCmdStack[3] = EOT;
    drill_control.iCommand = ACTIVATE;
    return 1;
  }


  printf("Unknown Command: %s\n", chCmd);
  return -1;

}
/* ---------------------------------------------------------------------------- */


int Open_Command_File(char * chCmdFile)
{

  fpCmd = fopen(chCmdFile, "rt");

  if( fpCmd == NULL )
  {
    return 0;
  }
  else
  {
    return 1;
  }

}
/* ---------------------------------------------------------------------------- */


void Close_Command_File(void)
{
  fclose(fpCmd);
}
/* ---------------------------------------------------------------------------- */


int Read_Command_File(char * chCmd)
{

  if( fgets(chCmd, FILE_CMD_SIZE-1, fpCmd) == NULL)
  {
    return 0;
  }
  else
  {
    return 1;
  }

}
/* ---------------------------------------------------------------------------- */


void strupr(char * c)
{
  while (*c)
  {
    *c++ = (char)toupper(*c);
  }
}
/* ---------------------------------------------------------------------------- */



/* Keyboard Input Loop Thread Function */

void * Input_Loop(void * unused)
{
  int iret;

  /* Spin reading keyboard input */
  while(1)
  {

    fgets(chCmd, FILE_CMD_SIZE-1, stdin);

    /* Analyse command */
    if( strncasecmp("OPEN", chCmd, 4) == 0 )
    {
      /* Get control file and execute */
      sscanf(&chCmd[5], "%s", chDrillFile);
      if( Open_Command_File(chDrillFile) == 0 )
      {
        printf("Error: unable to open drill control file: %s\n",chDrillFile);
        continue;
      }
      iDrillFile = 1;
      continue;
    } /* end of if( strncasecmp("OPEN" ... */

    /* Now convert to upper case */
    strupr(chCmd);

    if( strncasecmp("?", chCmd, 1) == 0 )
    {
      printf("\nLocal Commands:\n");
      printf("OPEN <control_file>: open and perform control file\n");
      printf("ABORT:               abort current command\n");
      printf("SUSPEND:             suspend control file execution\n");
      printf("GO:                  continue control file execution\n");
      printf("STOP:                terminate control file execution\n");
      printf("QUIT:                quit\n");
      printf("\nNative/Excellon Commands:\n");
      printf("PCP:                 print current position\n");
      printf("XMV<n.n>:            move x by n.n mm\n");
      printf("YMV<n.n>:            move y by n.n mm\n");
      printf("ZMV<n.n>:            move z by n.n mm\n");
      printf("SPDX<n.n>Y<p.p>:     set current position as <n.n>,<p.p> mm\n");
      printf("SPRX<n>Y<p>:         set current position as <n>,<p> steps\n");
      printf("DON:                 drill power on\n");
      printf("DOFF:                drill power off\n");
      printf("ZX:                  zero x axis\n");
      printf("ZY:                  zero y axis\n");
      printf("ZZ:                  zero z axis\n");
      printf("M48:                 header start\n");
      printf("%%:                   header end\n");
      printf("M30:                 program end\n");
      printf(";:                   comment\n");
      printf("METRIC:              use mm metric measurements\n");
      printf("M71:                 use mm metric measurements\n");
      printf("FMAT,2:              Excellon format two\n");
      printf("UP,<n.n>:            z axis up limit in mm\n");
      printf("DN,<n.n>:            z axis down limit in mm\n");
      printf("T,<n>:               select/change to tool <n>\n");
      printf("M15:                 drill plunge\n");
      printf("M16:                 drill retract\n");
      printf("X<n.n>Y<p.p>:        drill at <n.n>,<p.p> mm\n");
      printf("Z:                   set zero point\n");
      printf("G05:                 drilling mode\n");
      printf("G00X<n>Y<p>:         routing mode - move to position <n>,<p> steps\n");
      printf("G01X<n>Y<p>:         route to position <n>,<p> steps\n\n");

      continue;
    }

    if( strncasecmp("ABORT", chCmd, 5) == 0 )
    {
      /* Abort current command */
      iDrillFile = 0;
      Analyse_Command("ABORT");
      Close_Command_File();
      printf("Command aborted\n");
      continue;
    } /* end of if( strncasecmp("ABORT" ... */

    if( strncasecmp("SUSPEND", chCmd, 7) == 0 )
    {
      /* Suspend control file execution */
      if( iDrillFile != 0 )
      {
        iDrillFile = 2;
        printf("Program suspended\n");
      }
      continue;
    } /* end of if( strncasecmp("SUSPEND" ... */

    if( strncasecmp("STOP", chCmd, 4) == 0 )
    {
      /* Stop and close control file execution */
      iDrillFile = 0;
      Close_Command_File();
      printf("Program terminated\n");
      continue;
    } /* end of if( strncasecmp("STOP" ... */

    if( strncasecmp("GO", chCmd, 2) == 0 )
    {
      /* Continue control file execution */
      if( iDrillFile != 0 )
      {
        if( drill_control.iStatus == STATUS_PENDING )
        {
          printf("Unable to comply - last command still pending\n");
        }
        else
        {
          printf("Program resumed\n");
          iDrillFile = 1;
        }
      }
      continue;
    } /* end of if( strncasecmp("GO" ... */

    if( strncasecmp("QUIT", chCmd, 4) == 0 )
    {
      /* Ask control loop thread to die */
      iCThread_Terminate = 1;
      /* Make sure the control thread has finished.  */
      pthread_join (control_loop_thread_id, NULL);

      /* Make sure drill is powered off */
      if( drill_status.iDrillPower == DRILL_ON )
      {
#ifndef TESTING
        comedi_dio_write(device,subdevice,12,0);
#endif
      }
      /* Now kill the input thread */
      iIThread_Terminate = 1;
      break;
    }

    /* Pass command to native/Excellon command handler */
    if( drill_control.iStatus == STATUS_PENDING )
    {
      printf("Unable to comply - last command still pending\n");
    }
    else
    {
      iret = Analyse_Command(chCmd);
      if( iret >= 1 )
      {
        /* Wait for command acknowledgement */
        while( drill_control.iCommand != IDLE )
        {
          usleep(250);
        }
      }
    }


  } /* end of while(1) */


  return NULL;

}
/* ---------------------------------------------------------------------------- */



/* The main program.  */

int main(void)
{
  int i,iret;
  int iLastTime = 0;
  struct timespec timewait2;
  struct timespec timerem2;


  /* Access the comedi device */
#ifndef TESTING
  device=comedi_open("/dev/comedi0");
  if(!device)
  {
    comedi_perror("/dev/comedi0");
    exit(0);
  }
#endif


#ifndef TESTING
  /* Configure I/O board */
  for(i=0; i<16; i++)
  {
    iret=comedi_dio_config(device,subdevice,i,COMEDI_OUTPUT);
  }
  for(i=16; i<24; i++)
  {
    iret=comedi_dio_config(device,subdevice,i,COMEDI_INPUT);
  }
  for(i=0; i<16; i++)
  {
    comedi_dio_write(device,subdevice,i,0);
    usleep(10000);
  }
#endif


  /* configure axis parameters */
  axis_parameters.dXDist_per_Step = 3.1115385/1000.0;
  axis_parameters.dYDist_per_Step = 6.490625/1000.0;
  axis_parameters.dZDist_per_Step = 3.1115385/1000.0;
  axis_parameters.iZUp = 0;
  axis_parameters.iZDown = 0;

  /* reset drill status */
  drill_status.iCurrentX = 0;
  drill_status.iCurrentY = 0;
  drill_status.iCurrentZ = 999;
  drill_status.dCurrentX = 0.0;
  drill_status.dCurrentY = 0.0;
  drill_status.dCurrentZ = 1.5;
  drill_status.iDrillMode = DRILL_MODE;
  drill_status.iDrillPower = DRILL_OFF;
  drill_status.iDrillPosition = DRILL_RETRACT;

  /* reset drill control */
  drill_control.iCommand = IDLE;
  drill_control.iStatus = STATUS_IDLE;
  drill_control.iStackPtr = 0;


  /* configure x axis */
  axis_control_x.iAxis_Status = AXIS_IDLE;
  axis_control_x.iRef_Position = 0;
  axis_control_x.iCommand_Steps = 0;
  axis_control_x.iStep_Table_Pos = 0;
  axis_control_x.uiControlData = uiStepTable[0];
  axis_control_x.dRef_Position = 0.0;

  /* configure y axis */
  memcpy(&axis_control_y, &axis_control_x, sizeof(axis_control));

  /* configure z axis */
  memcpy(&axis_control_z, &axis_control_x, sizeof(axis_control));

  /* Set up time wait for nanosleep */
  timewait2.tv_sec = 0;
  timewait2.tv_nsec = 62500000; /* okay */


  printf("Drillcon Version 1.0 (C) 2005 Nicholas Redgrave\n");


  /* Try to activate real-time scheduling */
  memset(&scp, 0 ,sizeof(scp));
  scp.sched_priority = sched_get_priority_min(SCHED_RR);
  if (sched_setscheduler(0, SCHED_RR, &scp) < 0)
  {
    printf("WARNING: Cannot set RR-scheduler\n");
  }
  else
  {
    printf("RR-scheduler activated\n");
  }




  /* Create the control loop thread */
  pthread_create (&control_loop_thread_id, NULL, &Control_Loop, NULL);

  /* Create the input loop thread */
  pthread_create (&input_loop_thread_id, NULL, &Input_Loop, NULL);


  /* Wait for command line input and monitor */
  while( iIThread_Terminate == 0 )
  {

    if( iDrillFile == 1 )
    {
      /* We are processing a drill control file */
      if( drill_control.iStatus != STATUS_PENDING )
      {
        if( Read_Command_File(chFileCommand) == 0 )
        {
          /* end of drill control file */
          iDrillFile = 0;
          Close_Command_File();
          continue;
        }
        strupr(chFileCommand);
        iret = Analyse_Command(chFileCommand);
        if( iret == 2 )
        {
          printf("Execution will pause after current command - GO to continue\n");
          iDrillFile = 2;
          iret = 1;
        }
        if( iret == 1 )
        {
          /* Wait for command acknowledgement */
          while( drill_control.iCommand != IDLE )
          {
            usleep(250);
          }
        }
      }
    }


    /* Monitor activity */
    if( drill_control.iStatus == STATUS_PENDING )
    {
      printf( "Cmd:%d x:%d y:%d z:%d x:%1.3fmm y:%1.3fmm z:%1.3fmm Route:%d Power:%d Plunged:%d  \r",
            drill_control.iCurrentCmd,
            drill_status.iCurrentX,
            drill_status.iCurrentY,
            drill_status.iCurrentZ,
            drill_status.dCurrentX,
            drill_status.dCurrentY,
            drill_status.dCurrentZ,
            drill_status.iDrillMode,
            drill_status.iDrillPower - DRILL_OFF,
            drill_status.iDrillPosition - DRILL_RETRACT);
      iLastTime = 1;
    }
    else
    {
      if( iLastTime == 1 )
      {
        printf( "Cmd:%d x:%d y:%d z:%d x:%1.3fmm y:%1.3fmm z:%1.3fmm Route:%d Power:%d Plunged:%d  \n",
              drill_control.iCurrentCmd,
              drill_status.iCurrentX,
              drill_status.iCurrentY,
              drill_status.iCurrentZ,
              drill_status.dCurrentX,
              drill_status.dCurrentY,
              drill_status.dCurrentZ,
              drill_status.iDrillMode,
              drill_status.iDrillPower - DRILL_OFF,
              drill_status.iDrillPosition - DRILL_RETRACT);
        iLastTime = 0;
      }
    }

    /* Kill some time */
    if( nanosleep(&timewait2, &timerem2) == EINTR )
    {
      /* Wait was interrupted - try again */
      nanosleep(&timerem2, &timerem2);
    }




  } /* end of while( iIThread_Terminate == 0 ) */





  /* Make sure the control loop thread has finished. */
  pthread_join (control_loop_thread_id, NULL);

  /* Make sure the input thread has finished. */
  pthread_join (input_loop_thread_id, NULL);

#ifndef TESTING
  /* Power down outputs and close comedi device */
  for(i=0; i<16; i++)
  {
    comedi_dio_write(device,subdevice,i,0);
    usleep(1);
  }
  (void)comedi_close(device);
#endif

  /* Now we can safely return.  */
  return 0;
}