/** \file motor_dmc2280.c
 * \brief Driver for Galil DMC2280 motor controller.
 *
 * Implements a SICS motor object with a MotorDriver interface.
 *
 * Copyright: see file Copyright.txt
 *
 * Ferdi Franceschini November 2005
 *
 * TODO
 * - check for motors enabled on plc
 * - Check error bit, see Dan's email
 */
#include <stdlib.h>
/* ISO C Standard:  7.16  Boolean type and values  <stdbool.h> */
#include <stdbool.h>
#include <math.h>
#include <float.h>
#include <assert.h>
#include <string.h>
#include <stdarg.h>
#include <sys/time.h>
#include <fortify.h>
#include <sics.h>
#include <asyncqueue.h>
#include <nwatch.h>
#include <modriv.h>
#include <motor.h>
#include <dynstring.h>
#include <time.h>
#include "anstoutil.h"

#define UNITSLEN 256
#define TEXTPARLEN 1024
#define CMDLEN 1024
/** \brief Used to ensure that the getDMCSetting function is called
 * with valid values.
 * \see getDMCSetting
 */
enum dmcsetting {dmcspeed, dmcacceleration, dmcdeceleration};
enum commandtype {CMD_RUN=1, CMD_HALT=2};

typedef struct __MoDriv DMC2280Driv, *pDMC2280Driv;

enum eventtype {eTimerEvent, eMessageEvent, eCommandEvent, eTimeoutEvent};
typedef struct EvtEvent_s EvtEvent, *pEvtEvent;

typedef void (*StateFunc)(pDMC2280Driv self, pEvtEvent event);

typedef struct EvtTimer_s { } EvtTimer;

typedef struct EvtMessage_s {
  pAsyncTxn cmd;
} EvtMessage;

typedef struct EvtCommand_s {
  enum commandtype cmd_type;
} EvtCommand;

typedef struct EvtTimeout_s { } EvtTimeout;

struct EvtEvent_s {
  enum eventtype event_type;
  union {
    EvtTimer tmr;
    EvtMessage msg;
    EvtCommand cmd;
    EvtTimeout tmo;
  } event;
};

static pAsyncProtocol DMC2280_Protocol = NULL;

/*-----------------------------------------------------------------------
  The motor driver structure. Please note that the first set of fields has
  be identical with the fields of AbstractModriv in  ../modriv.h
  ------------------------------------------------------------------------*/
struct __MoDriv {
  /* general motor driver interface
     fields. _REQUIRED!
     */
  float fUpper; /**< hard upper limit */
  float fLower; /**< hard lower limit */
  char *name; /**< motor name */
  int (*GetPosition)(void *self, float *fPos);
  int   (*RunTo)(void *self, float fNewVal);
  int   (*GetStatus)(void *self);
  void  (*GetError)(void *self, int *iCode, char *buffer, int iBufLen);
  int   (*TryAndFixIt)(void *self,int iError, float fNew);
  int   (*Halt)(void *self);
  int   (*GetDriverPar)(void *self, char *name,
      float *value);
  int    (*SetDriverPar)(void *self,SConnection *pCon,
      char *name, float newValue);
  void   (*ListDriverPar)(void *self, char *motorName,
      SConnection *pCon);
  void   (*KillPrivate)(/*@only@*/void *self);
  int   (*GetDriverTextPar)(void *self, char *name,
                                  char *textPar);


  /* DMC-2280 specific fields */
  pAsyncUnit asyncUnit;
  pMotor pMot; /**< Points to logical motor object */
  int errorCode;
  char *errorMsg; /**< Points to memory for error messages */
  char units[256]; /**<  physical units for axis */
  char long_name[256]; /**<  long name of motor */
  char part[256]; /**<  assembly which motor belongs to */
  float speed; /**<  physical units per second */
  float maxSpeed; /**<  physical units per second */
  float accel; /**<  physical units per second^2 */
  float maxAccel; /**<  physical units per second^2 */
  float decel; /**<  physical units per second^2 */
  float maxDecel; /**<  physical units per second^2 */
  char axisLabel;
  char lastCmd[CMDLEN];
  char dmc2280Error[CMDLEN];
  float home; /**<  home position for axis, default=0 */
  int motorHome; /**<  motor home position in steps */
  int noPowerSave; /**< Flag = 1 to leave motors on after a move */
  float stepsPerX; /**<  steps per physical unit */
  int abs_encoder; /**<  Flag = 1 if there is an abs enc */
  int absEncHome; /**<  Home position in counts for abs enc */
  float cntsPerX; /**<  absolute encoder counts per physical unit */
  int motOffDelay; /**< number of msec to wait before switching motor off, default=0 */
  int currFlags;
  int currSteps;
  int currCounts;
  float currPosition; /**< Position at last position check */
  float lastPosition; /**< Position at last position check */
  float lastSteps;
  float lastCounts;
  struct timeval time_lastPos_set; /**< Time when lastPosition was set */
  float blockage_ckInterval; /**< Interval for checking blocked motors, seconds */
  float blockage_thresh; /**< motion threshold for blockage checking */
  float blockage_ratio; /**< ratio steps/counts must be between 1/this and this */
  int blockage_fail; /**< flag =1 if we should fail the motor */
  int has_airpads; /**< Flag = 1 if there is are airpads for this motor */
  float backlash_offset; /**< signed offset to drive from */
  float fTarget; /**< target passed from SICS to timer callback */
  int settle; /**< motor settling time in seconds */
  struct timeval time_settle_done; /**< time when settling will be over */
  int airpad_state; /**< state of the airpads finite state machine */
  int airpad_counter; /**< airpad timer retry counter */
  pNWTimer airpad_timer; /**< timer waiting for airpad action to complete */
  pNWTimer motoff_timer; /**< motor off timer */
  int debug;
  int stepCount; /**< number of step operations for this move cycle */
  float creep_offset; /**< last little bit to creep in units */
  float creep_precision;
  int creep_val;
  int preseek; /**< Flag = 1 if current move is a backlash preseek */
  int has_fsm; /**< Flag = 1 if motor has finite state machine driver model */
  int driver_status;
  StateFunc myState;
  int subState;
  pNWTimer state_timer; /**< motor state timer */
  SConnection *trace;
};

int DMC2280MotionControl = 1; /* defaults to enabled */

#define AIR_POLL_TIMER  1000
#define MOTOR_POLL_TIMER 50
#define ON_SETTLE_TIMER 1000
#define MAX_CREEP_STEPS 100

#define AIRPADS_DOWN     0
#define AIRPADS_RAISE    1
#define AIRPADS_UP       2
#define AIRPADS_LOWER    3
/*------------------- error codes ----------------------------------*/
#define BADADR     -1 // NOT SET: Unknown host/port?
#define BADBSY     -2
#define BADCMD     -3
#define BADPAR     -4 // NOT SET: Does SICS already check parameter types?
#define BADUNKNOWN -5
#define BADSTP     -6 // NOT SET
#define BADEMERG   -7 // NOT SET: ESTOP
#define RVRSLIM    -8
#define FWDLIM     -9
#define POSFAULT   -11 // NOT SET
#define BADCUSHION -12 // Air Cushion Fault
#define ERRORLIM   -13
#define IMPOSSIBLE_LIM_SW -14
#define BGFAIL     -15 // NOT SET
#define BLOCKED    -16
#define MOTIONCONTROLOFF -17
#define MOTIONCONTROLUNK -18
#define MOTCMDTMO  -19 /* Motor Command Timeout */
#define STATEERROR -20
#define THREADZERO -21 /* Thread zero not running */
/*--------------------------------------------------------------------*/
#define STATUSMOVING 128 /* Motor is moving */
#define STATUSERRORLIMIT 64 /* Number of errors exceed limit */
#define STATUSOFF 32 /* Motor off */
#define STATUSFWDLIMIT 8 /* Forward limit switch active */
#define STATUSRVRSLIMIT 4 /* Reverse limit switch active */

#define INIT_STR_SIZE 256
#define STR_RESIZE_LENGTH   256
#define BUFFLEN 512
#define _SAVEPOWER 0

#define SETPOS "setpos"
#define HOME "home"
#define HARDLOWERLIM "hardlowerlim"
#define HARDUPPERLIM "hardupperlim"
#define UNITS "units"
#define SPEED "speed"
#define MAXSPEED "maxspeed"
#define ACCEL "accel"
#define MAXACCEL "maxaccel"
#define DECEL "decel"
#define MAXDECEL "maxdecel"
#define MOTOFFDELAY "motoffdelay"
#define AIRPADS "airpads"
#define SETTLE "settle"
#define LONG_NAME "long_name"
#define BLOCKAGE_CHECK_INTERVAL "blockage_check_interval"

static int DMC2280Halt(void *pData);
static int DMC2280SetPar(void *pData, SConnection *pCon,
    char *name, float newValue);

/** \brief Convert axis speed in physical units to
 * motor speed in steps/sec.
 * \param self (r) provides access to the motor's data structure
 * \param speed in physical units, eg mm/sec degrees/sec
 * \return the speed in motor steps/sec
 */
static int motSpeed(pDMC2280Driv self, float axisSpeed) {
  int speed;
  speed = (int) (fabs(axisSpeed * self->stepsPerX) + 0.5);
  return speed;
}

/** \brief Convert axis acceleration in physical units to
 * to motor speed in steps/sec^2
 * \param self (r) provides access to the motor's data structure
 * \param acceleration in physical units, eg mm/sec^2 degrees/sec^2
 * \return the acceleration in motor steps/sec^2
 */
static int motAccel(pDMC2280Driv self, float axisAccel) {
  int accel;
  accel = (int) (fabs(axisAccel * self->stepsPerX) + 0.5);
  return accel;
}

/** \brief Convert axis deceleration in physical units to
 * motor deceleration in steps/sec^2
 * \param self (r) provides access to the motor's data structure
 * \param deceleration in physical units, eg mm/sec^2 degrees/sec^2
 * \return the deceleration in motor steps/sec^2
 */
static int motDecel(pDMC2280Driv self, float axisDecel) {
  int decel;
  decel = (int) (fabs(axisDecel * self->stepsPerX) + 0.5);
  return decel;
}

/** \brief Convert motor position to physical units
 * using the current motor steps or encoder counts
 * \param self (r) provides access to the motor's data structure
 * \return the motor position in physical units
 */
static float motPosit(pDMC2280Driv self) {
  float fPos;
  if (self->abs_encoder)
    fPos = (self->currCounts - self->absEncHome) / self->cntsPerX + self->home;
  else
    fPos = (self->currSteps - self->motorHome) / self->stepsPerX + self->home;
  return fPos;
}

/** \brief Convert motor target in physical units to
 * motor absolute position in steps
 * \param self (r) provides access to the motor's data structure
 * \param target in physical units, eg mm, degrees
 * \return the absolute position in motor steps
 */
static int motAbsol(pDMC2280Driv self, float target) {
  double absolute;
  int result;
  if (self->abs_encoder) {
#if 1
    /* distance of target from home in units */
    absolute = target - self->home;
    /* subtract current encoder position in units */
    absolute -= (self->currCounts - self->absEncHome) / self->cntsPerX;
    /* convert to motor steps */
    absolute *= self->stepsPerX;
    /* add current position in steps */
    absolute += (double) self->currSteps;
#else
    absolute = ((self->absEncHome - self->currCounts) +
        (self->cntsPerX * (target - self->home))) *
      (self->stepsPerX / self->cntsPerX) +
      self->currSteps;
#endif
  }
  else {
    absolute = (target - self->home) * self->stepsPerX + self->motorHome;
  }
  if (absolute > 0)
    result = (int) (absolute + 0.5);
  else if (absolute < 0)
    result = (int) (absolute - 0.5);
  else
    result = (int) absolute;
  if (self->debug) {
    char line[CMDLEN];
    snprintf(line, CMDLEN, "Rounding %f to %d", absolute, result);
    SICSLogWrite(line, eStatus);
  }
  return result;
}

/** \brief Convert motor target in physical units to
 * motor absolute position in steps
 * \param self (r) provides access to the motor's data structure
 * \param target in physical units, eg mm, degrees
 * \return the absolute position in motor steps
 */
static int motCreep(pDMC2280Driv self, float target) {
  int target_steps = motAbsol(self, target);
  if (!self->abs_encoder ||     /* no absolute encoder */
      self->preseek ||          /* backlash preseek active */
      self->creep_offset == 0)  /* not creeping anyway */
    return target_steps;
  else {
    int offset = target_steps - self->currSteps;
    if (self->creep_val == 0) { /* initial creep step */
      if (offset > 0) /* moving up */
        self->creep_val = -1;
      else if (offset < 0) /* moving down */
        self->creep_val = +1;
    }
    else {
      if (self->creep_val > 0) /* moving down */
        ++self->creep_val;
      else
        --self->creep_val;
      if (abs(self->creep_val) > MAX_CREEP_STEPS &&
          abs(self->creep_val) > 2.0 * fabs(self->stepsPerX / self->cntsPerX)) {
        if (self->debug) {
          char line[CMDLEN];
          snprintf(line, CMDLEN, "Motor=%s creep stopped, stepcount = %d",
              self->name, self->stepCount);
          SICSLogWrite(line, eStatus);
        }
        return target_steps;
      }
    }

    if (self->creep_val > 0) /* moving down handle as positive */
      offset = -offset;
    if (offset > fabs(self->stepsPerX * self->creep_precision)) {
      if (offset - (int) fabs(self->stepsPerX * self->creep_offset) > (int) fabs(self->stepsPerX / self->cntsPerX))
        offset = offset - fabs(self->stepsPerX * self->creep_offset);
      else {
        if (offset <= fabs(self->stepsPerX / self->cntsPerX))
          offset = 1;
        else
          offset = offset / 2;
      }
      self->preseek = 1;
    }
    if (self->creep_val > 0) /* moving down restore to negative */
      offset = - offset;

    if (self->debug) {
      char text[CMDLEN];
      snprintf(text, CMDLEN, "CREEP: cur=%d, target=%d, offset=%d, new=%d",
          self->currSteps, target_steps, offset, self->currSteps + offset);
      SICSLogWrite(text, eStatus);
    }
    target_steps = self->currSteps + offset;
  }
  return target_steps;
}

static int DMC_Tx(pAsyncProtocol p, pAsyncTxn ctx)
{
  int iRet = 1;
  pAsyncTxn myCmd = (pAsyncTxn) ctx;

  if (myCmd) {
    myCmd->txn_status = ATX_ACTIVE;
    iRet = AsyncUnitWrite(myCmd->unit, myCmd->out_buf, myCmd->out_len);
    /* TODO handle errors */
    if (iRet < 0) { /* TODO: EOF */
      /*
      iRet = AsyncUnitReconnect(myCmd->unit);
      if (iRet == 0)
      */
        return 0;
    }
  }
  return 1;
}

static int DMC_Rx(pAsyncProtocol p, pAsyncTxn ctx, int rxchar) {
  int iRet = 1;
  pAsyncTxn myCmd = (pAsyncTxn) ctx;

  switch (myCmd->txn_state) {
    case 0: /* first character */
      if (rxchar == ':') {
        /* normal prompt */
        myCmd->txn_state = 99;
        myCmd->txn_status = ATX_COMPLETE;
      }
      else if (rxchar == '?') {
        /* error prompt, send TC1 ahead of any queued commands */
        iRet = AsyncUnitWrite(myCmd->unit, "TC1\r\n", 5);
        myCmd->txn_state = 1;
      }
      else {
        /* normal data */
        myCmd->txn_state = 1;
      }
      /* note fallthrough */
    case 1: /* receiving reply */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0D)
        myCmd->txn_state = 2;
      break;
    case 2: /* received CR and looking for LF */
      if (myCmd->inp_idx < myCmd->inp_len)
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
      if (rxchar == 0x0A) {
        /* end of line */
/*
        myCmd->inp_idx -= 2;
        myCmd->inp_buf[myCmd->inp_idx++] = rxchar;
 */
        myCmd->txn_state = 0;
      }
      else
        myCmd->txn_state = 1;
      break;
  }
  if (myCmd->txn_state == 99) {
    myCmd->inp_buf[myCmd->inp_idx] = '\0';
    if (strncmp(myCmd->inp_buf, myCmd->out_buf, myCmd->out_len) == 0) {
      int i;
      SICSLogWrite("Line echo detected", eStatus);
      for (i = myCmd->out_len; i <= myCmd->inp_idx; ++i) {
        myCmd->inp_buf[i - myCmd->out_len] = myCmd->inp_buf[i];
      }
    }
    iRet = 0;
  }
  if (iRet == 0) { /* end of command */
    return AQU_POP_CMD;
  }
  return iRet;
}

static int DMC_Ev(pAsyncProtocol p, pAsyncTxn pTxn, int event) {
  if (event == AQU_TIMEOUT) {
    /* handle command timeout */
    pTxn->txn_status = ATX_TIMEOUT;
    return AQU_POP_CMD;
  }
  return AQU_POP_CMD;
}

static int DMC_SendCmd(pDMC2280Driv self,
    char* command,
    AsyncTxnHandler callback)
{
  return AsyncUnitSendTxn(self->asyncUnit,
      command, strlen(command),
      callback, self, CMDLEN);
}

/**
 * \brief SendCallback is the callback for the general command.
 */
static int SendCallback(pAsyncTxn pCmd)
{
  char* cmnd = pCmd->out_buf;
  char* resp = pCmd->inp_buf;
  pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;

  if (pCmd->txn_status == ATX_TIMEOUT) {
    if (self->debug) {
      SICSLogWrite(pCmd->out_buf, eStatus);
      SICSLogWrite("<TIMEOUT>", eStatus);
    }
    strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
    self->errorCode = MOTCMDTMO;
  }
  else {
    switch (resp[0]) {
      case ':':
      case ' ':
        if (self->debug) {
          SICSLogWrite(cmnd, eStatus);
          SICSLogWrite(resp, eStatus);
        }
        break;
      case '?':
        strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
        strncpy(self->dmc2280Error, &resp[1], CMDLEN);
        SICSLogWrite(cmnd, eError);
        SICSLogWrite(resp, eError);
        self->errorCode = BADCMD;
        break;
      default:
        SICSLogWrite(cmnd, eError);
        SICSLogWrite(resp, eError);
        self->errorCode = BADUNKNOWN;
        break;
    }
  }

  return 0;
}

static int DMC2280Queue(pDMC2280Driv self, char *cmd, AsyncTxnHandler cb) {
  if (cb == NULL)
    cb = SendCallback;
  return DMC_SendCmd(self, cmd, cb);
}

/** \brief Sends a DMC2280 command to the motor controller.
 *
 * If the command fails it displays the DMC2280 error message to the client
 * and writes it to the log file, also sets errorCode field in motor's
 * data structure.
 *
 * \param self (rw) provides access to the motor's data structure
 * \param *command (r) DMC2280 command
 * \return
 * - SUCCESS
 * - FAILURE
 * \see SUCCESS FAILURE
 */
 /* First character returned by controller is
   '?' for an invalid command or
   ':' for a valid command with no response
   ' ' for a valid command with a response (':' follows)
  */
static int DMC2280Send(pDMC2280Driv self, char *command) {
  return DMC2280Queue(self, command, SendCallback);
}

/**
 * \brief Sends a command and waits for a response
 *
 * \param self motor data
 * \param cmd command to send
 * \param reply space to return response
 * \return
 */
static int DMC2280SendReceive(pDMC2280Driv self, char *cmd, char* reply) {
  int status;

  status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, CMDLEN);

  if (status != 1) {
    if (self->debug)
      SICSLogWrite(cmd, eStatus);
    if (status == -1)
      self->errorCode = MOTCMDTMO;
    else
      self->errorCode = BADUNKNOWN;
    return FAILURE;
  }

  switch (reply[0]) {
    case ':':
      if (self->debug) {
        SICSLogWrite(cmd, eStatus);
        SICSLogWrite(reply, eStatus);
      }
      return SUCCESS;
    case ' ':
      if (self->debug) {
        SICSLogWrite(cmd, eStatus);
        SICSLogWrite(reply, eStatus);
      }
      return SUCCESS;
    case '?':
      strncpy(self->lastCmd, cmd, CMDLEN);
      strncpy(self->dmc2280Error, &reply[1], CMDLEN);
      SICSLogWrite(cmd, eError);
      SICSLogWrite(reply, eError);
      self->errorCode = BADCMD;
      return FAILURE;
    default:
      strncpy(self->lastCmd, cmd, CMDLEN);
      strncpy(self->dmc2280Error, &reply[0], CMDLEN);
      SICSLogWrite(cmd, eError);
      SICSLogWrite(reply, eError);
      self->errorCode = BADUNKNOWN;
      return FAILURE;
  }
  return OKOK;
}

/** \brief Record the given posn and timestamp it.
 *
 * \param *pData provides access to a motor's data
 * \param posn, the axis position which you want to remember.
 * */
static void set_lastMotion(pDMC2280Driv self, float steps, float counts) {
  assert(self != NULL);
  self->lastSteps = steps;
  self->lastCounts = counts;
  gettimeofday(&(self->time_lastPos_set), NULL);
}

static int set_currMotion(pDMC2280Driv self, const char* text) {
  int iRet, iFlags;
  double steps, counts, flags;
  iRet = sscanf(text, "%lf %lf %lf", &steps, &counts, &flags);
  if (iRet != 3)
    return 0;
  iFlags = (int)(flags + 0.1);
  self->currFlags = iFlags;
  self->currSteps = steps;
  self->currCounts = counts;
  self->currPosition = motPosit(self);
  return 1;
}

/** \brief Reads motion.
 *
 * \param *steps motor steps
 * \param *counts encoder counts
 * \return
 * SUCCESS
 * FAILURE
 */
static int readMotion(pDMC2280Driv self, float *steps, float *counts) {
  char reply[CMDLEN];
  char cmd[CMDLEN];

  snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c", self->axisLabel, self->axisLabel);
  if (FAILURE == DMC2280SendReceive(self, cmd, reply))
    return FAILURE;

  if (2 != sscanf(reply, "%f %f", steps, counts))
    return FAILURE;
  return SUCCESS;
}

/** \brief Reads absolute encoder.
 *
 * \param *pos is the absolute encoder reading on SUCCESS.
 * \return
 * SUCCESS
 * FAILURE
 */
static int readAbsEnc(pDMC2280Driv self, float *pos) {
  char reply[CMDLEN];
  char cmd[CMDLEN];

  snprintf(cmd, CMDLEN, "TP%c", self->axisLabel);
  if (FAILURE == DMC2280SendReceive(self, cmd, reply))
    return FAILURE;

  *pos = (float) atoi(reply);
  return SUCCESS;
}

/** \brief Reads Thread variable
 *
 * \param *pos is the thread variable value on SUCCESS.
 * \return
 * SUCCESS
 * FAILURE
 */
static int ReadThread(pDMC2280Driv self, int thread, float *pos) {
  char reply[CMDLEN];
  char cmd[CMDLEN];

  snprintf(cmd, CMDLEN, "MG _XQ%d", thread);
  if (FAILURE == DMC2280SendReceive(self, cmd, reply))
    return 0;

  *pos = (float) atoi(reply);
  return 1;
}

/** \brief Reads HOMERUN variable
 *
 * \param *pos is the homerun variable value on SUCCESS.
 * \return
 * SUCCESS
 * FAILURE
 */
static int readHomeRun(pDMC2280Driv self, float *pos) {
  char reply[CMDLEN];
  char cmd[CMDLEN];

  snprintf(cmd, CMDLEN, "MG HOMERUN");
  if (FAILURE == DMC2280SendReceive(self, cmd, reply))
    return FAILURE;

  *pos = (float) atoi(reply);
  return SUCCESS;
}

/** \brief Runs the HOMERUN routine on the MC
 *
 * \return
 * SUCCESS
 * FAILURE
 */
static int RunHomeRoutine(pDMC2280Driv self, float newValue) {
  float fValue;
  char cmd[CMDLEN];

  /* Read HOMERUN should get error if it does not have one */
  if (FAILURE == readHomeRun(self, &fValue))
    return FAILURE;

  /* 0 => reset homerun */
  if (newValue < 0.5) {
    snprintf(cmd, CMDLEN, "HOMERUN=0");
    if (FAILURE == DMC2280Send(self, cmd))
      return FAILURE;
    return SUCCESS;
  }
  snprintf(cmd, CMDLEN, "XQ #HOME,1");
  if (FAILURE == DMC2280Send(self, cmd))
    return FAILURE;

  return SUCCESS;
}

/**
 * \brief calculate and issue the motion commands
 *
 * \param self provides access to a motor's data
 * \param fValue target position in physical units, software zeros
 * have already been applied.
 * \return
 * - OKOK request succeeded
 * - HWFault request failed

 */
static int DMC2280RunCommon(pDMC2280Driv self,float fValue){
  char axis;
  char SHx[CMDLEN], BGx[CMDLEN], absPosCmd[CMDLEN];
  int absEncHome, motorHome, newAbsPosn;
  float stepsPerX, cntsPerX;
  float target;

  axis=self->axisLabel;
  motorHome = self->motorHome;
  stepsPerX=self->stepsPerX;
  snprintf(SHx, CMDLEN, "SH%c", axis);
  snprintf(BGx, CMDLEN, "BG%c", axis);
  target = fValue - self->home;

  if (self->preseek)
    self->preseek = 0;
  else {
    if (self->backlash_offset > FLT_EPSILON) {
      if (target > self->lastPosition) {
        self->preseek = 1;
        target += self->backlash_offset;
        if (target > self->fUpper)
          target = self->fUpper;
      }
    }
    else if (self->backlash_offset < -FLT_EPSILON) {
      if (target < self->lastPosition) {
        self->preseek = 1;
        target += self->backlash_offset;
        if (target < self->fLower)
          target = self->fLower;
      }
    }
  }

  if (1 == self->abs_encoder) {
    absEncHome = self->absEncHome;
    cntsPerX = self->cntsPerX;
#if 0
    /* PAF=-((absEncHome-_TPF)/-cntsPerX + target)*stepsPerX + _TDF */
    snprintf(absPosCmd, CMDLEN,
        "PA%c=(((%d-_TP%c)/%f)+%f)*%f + _TD%c",
        axis,
        absEncHome,
        axis,
        cntsPerX,
        target,
        stepsPerX,
        axis);
#else
    /* PAZ=((absEncHome-_TPZ) + (cntsPerX * target)) * stepsPerX / cntsPerX + _TDZ */
    char s_cnts[20];
    char s_trgt[20];
    char s_stps[20];
    int i;
    snprintf(s_cnts, sizeof(s_cnts), "%.4f", cntsPerX);
    for (i = strlen(s_cnts); i > 0; --i)
      if (s_cnts[i-1] == '.') {
        s_cnts[i-1] = '\0';
        break;
      }
      else if (s_cnts[i-1] == '0')
        s_cnts[i-1] = '\0';
      else
        break;
    snprintf(s_trgt, sizeof(s_trgt), "%.4f", target);
    for (i = strlen(s_trgt); i > 0; --i)
      if (s_trgt[i-1] == '.') {
        s_trgt[i-1] = '\0';
        break;
      }
      else if (s_trgt[i-1] == '0')
        s_trgt[i-1] = '\0';
      else
        break;
    snprintf(s_stps, sizeof(s_stps), "%.4f", stepsPerX);
    for (i = strlen(s_stps); i > 0; --i)
      if (s_stps[i-1] == '.') {
        s_stps[i-1] = '\0';
        break;
      }
      else if (s_stps[i-1] == '0')
        s_stps[i-1] = '\0';
      else
        break;
    snprintf(absPosCmd, CMDLEN,
        "PA%c=((%d-_TP%c)+(%s*%s))*%s/%s+_TD%c",
        axis,
        absEncHome,
        axis,
        s_cnts,
        s_trgt,
        s_stps,
        s_cnts,
        axis);
#endif
#ifdef BACKLASHFIX
    do {
      char cmd[CMDLEN];
      snprintf(cmd, CMDLEN, "%cQTARGET=%d", axis,
          (int) (target * cntsPerX + absEncHome + 0.5));
      if (FAILURE == DMC2280Send(self, cmd))
        return HWFault;
    } while (0);
#endif
  } else {
    newAbsPosn = (int)(target * stepsPerX + motorHome + 0.5);
    snprintf(absPosCmd, CMDLEN, "PA%c=%d",axis, newAbsPosn);
  }

  if (FAILURE == DMC2280Send(self, SHx))
    return HWFault;
  if (FAILURE == DMC2280Send(self, absPosCmd))
      return HWFault;
  if (FAILURE == DMC2280Send(self, BGx))
    return HWFault;
  return OKOK;
}

/**
 * \brief process the airpad status response
 */
static int airpad_callback(pAsyncTxn pCmd) {
  char* resp = pCmd->inp_buf;
  pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;

  if (pCmd->txn_status == ATX_TIMEOUT) {
    if (self->debug) {
      SICSLogWrite(pCmd->out_buf, eStatus);
      SICSLogWrite("<TIMEOUT>", eStatus);
    }
    strncpy(self->lastCmd, pCmd->out_buf, CMDLEN);
    self->errorCode = MOTCMDTMO;
  }
  else {
    float fReply;
    if (self->debug) {
      SICSLogWrite(pCmd->inp_buf, eStatus);
      SICSLogWrite(pCmd->out_buf, eStatus);
    }
    fReply = (float) atof(resp);
    if (self->airpad_state == AIRPADS_RAISE && fReply > 0) {
      int iRet;
      self->airpad_state = AIRPADS_UP;
      iRet = DMC2280RunCommon(self, self->fTarget);
      if (iRet != OKOK)
        self->errorCode = BADCUSHION;
      return 0;
    }
    if (self->airpad_state == AIRPADS_LOWER && fReply == 0) {
      self->airpad_state = AIRPADS_DOWN;
      return 0;
    }
  }
  return 0;
}

/**
 * \brief request the airpad status periodically
 */
static int airpad_timeout(void* ctx, int mode) {
  pDMC2280Driv self = (pDMC2280Driv) ctx;

  self->airpad_timer = NULL;

  if (self->airpad_state == AIRPADS_UP ||
      self->airpad_state == AIRPADS_DOWN)
    return 0;

  if (self->airpad_counter <= 0) {
    self->errorCode = BADCUSHION;
    self->airpad_state = AIRPADS_DOWN;
    return 0;
  }
  --self->airpad_counter;
  NetWatchRegisterTimer(&self->airpad_timer,
      AIR_POLL_TIMER,
      airpad_timeout, self);

  if (FAILURE == DMC2280Queue(self, "MG APDONE", airpad_callback)) {
    self->errorCode = BADCUSHION;
    self->airpad_state = AIRPADS_DOWN;
    return 0;
  }
  return 0;
}

/**
 * \brief initiate the raising or lowering of the airpads
 *
 * \param self motor data
 * \param flag 1 for raise and 0 for lower
 * \return 1 for SUCCESS or 0 for FAILURE
 */
static int DMC_AirPads(pDMC2280Driv self, int flag) {
  char *cmd = NULL;

  if (self->airpad_timer)
    NetWatchRemoveTimer(self->airpad_timer);
  self->airpad_timer = NULL;
  if (flag) {
    cmd = "FTUBE=1";
    self->airpad_state = AIRPADS_RAISE;
  }
  else {
    cmd = "FTUBE=0";
    self->airpad_state = AIRPADS_LOWER;
  }
  if (FAILURE == DMC2280Send(self, cmd)) {
    self->airpad_state = AIRPADS_DOWN;
    self->errorCode = BADCUSHION;
    return 0;
  }
  self->airpad_counter = 10;
  NetWatchRegisterTimer(&self->airpad_timer,
      AIR_POLL_TIMER,
      airpad_timeout, self);
  return 1;
}

/**
 * \brief turn the motor off after a delay
 *
 * \param context motor data
 * \param mode
 */
static int motoff_timeout(void* context, int mode) {
  pDMC2280Driv self = (pDMC2280Driv) context;
  char cmd[CMDLEN];

  self->motoff_timer = NULL;
  if (self->has_airpads) {
    DMC_AirPads(self, 0);
    return 0;
  }
  snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
  DMC2280Send(self, cmd);
  return 0;
}

/** \brief Check if the axis has moved significantly since
 * the last check.
 *
 * The motion is checked against the expected at intervals of
 * pDMC2280Driv->blockage_ckInterval.
 *
 * \param *pData provides access to a motor's data
 * \return
 * - 1 MOTOR OK, position has changed significantly during move
 * - 0 MOTOR BLOCKED, no significant change in position detected.
 */
static int checkMotion(void *pData) {
  float steps, counts, ratio_obs, ratio_exp, ratio_cmp;
  long int usec_TimeDiff;
  struct timeval now;

  pDMC2280Driv self;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);
  /* we can only test if there is an absolute encoder */
  if (!self->abs_encoder)
    return 1;
  if (self->time_lastPos_set.tv_sec == 0) {
    /* first time - initialise the data */
    if (self->has_fsm) {
      steps = self->currSteps;
      counts = self->currCounts;
    }
    else {
      if (FAILURE == readMotion(self, &steps, &counts))
      return 0;
    }
    set_lastMotion(pData, steps, counts);
    return 1;
  }
  gettimeofday(&now, NULL);
  usec_TimeDiff = now.tv_sec - self->time_lastPos_set.tv_sec;
  usec_TimeDiff *= 1000000;
  usec_TimeDiff += now.tv_usec;
  usec_TimeDiff -= self->time_lastPos_set.tv_usec;
  if (usec_TimeDiff < (long int)(1e6*self->blockage_ckInterval))
    return 1;
  if (self->has_fsm) {
    steps = self->currSteps;
    counts = self->currCounts;
  }
  else {
    if (FAILURE == readMotion(self, &steps, &counts))
      return 0;
  }
  /* If not stepping, then not blocked */
  if (fabs(steps - self->lastSteps) < fabs(self->blockage_thresh * self->stepsPerX)) {
    /* just update the timestamp */
    set_lastMotion(pData, self->lastSteps, self->lastCounts);
    return 1;
  }
  /* calculate observed and expected steps per count ratios */
  if (counts == self->lastCounts) /* prevent divide by zero */
    ratio_obs = (steps - self->lastSteps);
  else
    ratio_obs = (steps - self->lastSteps) / (counts - self->lastCounts);
  ratio_exp = (float) self->stepsPerX / (float) self->cntsPerX;
  ratio_cmp = ratio_obs / ratio_exp;
  /* wrong signs, less than half, or more than double is trouble */
  if (ratio_cmp < 0.0 ||
      ratio_cmp > self->blockage_ratio ||
      (1.0 / ratio_cmp) > self->blockage_ratio) {
    char msg[132];
    float cmp = ratio_cmp;
    if (fabs(cmp) > 0 && fabs(cmp) < 1)
      cmp = 1.0 / cmp;
    snprintf(msg, sizeof(msg), "Motor %s fail: obs=%f, exp=%f, cmp=%f",
        self->name, ratio_obs, ratio_exp, cmp);
    SICSLogWrite(msg, eError);
    snprintf(msg, sizeof(msg), "steps=%f-%f, counts=%f-%f, exp=%f/%f",
        steps, self->lastSteps, counts, self->lastCounts,
        self->stepsPerX, self->cntsPerX);
    SICSLogWrite(msg, eError);
    if (self->blockage_fail)
      return 0;
    set_lastMotion(pData, steps, counts);
    return 1;
  } else {
    if (self->debug) {
      char msg[132];
      float cmp = ratio_cmp;
      if (fabs(cmp) > 0 && fabs(cmp) < 1)
        cmp = 1.0 / cmp;
      snprintf(msg, sizeof(msg), "Motor %s pass: obs=%f, exp=%f, cmp=%f",
          self->name, ratio_obs, ratio_exp, cmp);
      SICSLogWrite(msg, eError);
    }
    set_lastMotion(pData, steps, counts);
    return 1;
  }
  return 1;
}

/**
 * \brief handle the run command for a motor with airpads
 *
 * \param self motor data
 * \param fValue new motor position
 */
static int DMC2280RunAir(pDMC2280Driv self, float fValue) {
  if (!DMC_AirPads(self, 1))
    return HWFault;
  return OKOK;
}

/* State Functions */

static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event);
static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event);
static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event);
static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event);
static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event);
static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event);

static char* state_name(StateFunc func) {
  if (func == DMCState_Unknown) return "DMCState_Unknown";
  if (func == DMCState_Idle) return "DMCState_Idle";
  if (func == DMCState_AirOn) return "DMCState_AirOn";
  if (func == DMCState_MotorOn) return "DMCState_MotorOn";
  if (func == DMCState_Moving) return "DMCState_Moving";
  if (func == DMCState_MotorHalt) return "DMCState_MotorHalt";
  if (func == DMCState_OffTimer) return "DMCState_OffTimer";
  if (func == DMCState_AirOff) return "DMCState_AirOff";
  if (func == DMCState_MotorOff) return "DMCState_MotorOff";
  return "<unknown_state>";
}

void str_n_cat(char* s1, int len, const char* s2) {
  int i = strlen(s1);
  const char* p = s2;
  while (i < len - 3 && *p) {
    if (*p == '\r') {
      s1[i++] = '\\';
      s1[i++] = 'r';
      ++p;
    }
    else if (*p == '\n') {
      s1[i++] = '\\';
      s1[i++] = 'n';
      ++p;
    }
    else
      s1[i++] = *p++;
  }
  s1[i] = '\0';
}

static char* event_name(pEvtEvent event, char* text, int length) {
  switch (event->event_type) {
    case eTimerEvent:
      snprintf(text, length, "eTimerEvent");
      return text;
    case eMessageEvent:
      snprintf(text, length, "eMessageEvent:");
      str_n_cat(text, length, event->event.msg.cmd->out_buf);
      str_n_cat(text, length, "|");
      str_n_cat(text, length, event->event.msg.cmd->inp_buf);
      return text;
    case eCommandEvent:
      switch (event->event.cmd.cmd_type) {
        case CMD_RUN:
        snprintf(text, length, "eCommandEvent:RUN");
        return text;
        case CMD_HALT:
        snprintf(text, length, "eCommandEvent:HALT");
        return text;
      }
      snprintf(text, length, "eCommandEvent:unknown");
      return text;
    case eTimeoutEvent:
      snprintf(text, length, "eTimeoutEvent");
      return text;
    default:
      snprintf(text, length, "<unknown_event>");
      return text;
  }
}

static void report_event(pDMC2280Driv self, pEvtEvent event) {
  if (self->debug || self->trace) {
    char line[CMDLEN];
    char text[CMDLEN];
    snprintf(line, CMDLEN, "Motor=%s, State=%s, event=%s", self->name,
        state_name(self->myState), event_name(event, text, CMDLEN));
    if (self->debug)
      SICSLogWrite(line, eStatus);
    if (self->trace)
      SCWrite(self->trace, line, eStatus);
  }
}

static void change_state(pDMC2280Driv self, StateFunc func) {
  if (self->debug || self->trace) {
    char line[CMDLEN];
    snprintf(line, CMDLEN, "Motor=%s, OldState=%s, NewState=%s", self->name,
        state_name(self->myState), state_name(func));
    if (self->debug)
      SICSLogWrite(line, eStatus);
    if (self->trace)
      SCWrite(self->trace, line, eStatus);
  }
  self->myState = func;
  self->subState = 0;
}

static void unhandled_event(pDMC2280Driv self, pEvtEvent event) {
    char line[CMDLEN];
    char text[CMDLEN];
    snprintf(line, CMDLEN, "Motor=%s, State=%s, unhandled event=%s",
        self->name,
        state_name(self->myState),
        event_name(event, text, CMDLEN));
    SICSLogWrite(line, eStatus);
}

static int state_msg_callback(pAsyncTxn pCmd)
{
  pDMC2280Driv self = (pDMC2280Driv) pCmd->cntx;
  EvtEvent event;

  if (pCmd->txn_status == ATX_TIMEOUT) {
    if (self->debug) {
      SICSLogWrite(pCmd->out_buf, eStatus);
      SICSLogWrite("<TIMEOUT>", eStatus);
    }
    event.event_type = eTimeoutEvent;
    event.event.msg.cmd = pCmd;
  }
  else {
    if (self->debug) {
      SICSLogWrite(pCmd->out_buf, eStatus);
      SICSLogWrite(pCmd->inp_buf, eStatus);
    }
    event.event_type = eMessageEvent;
    event.event.msg.cmd = pCmd;
  }
  if (self->debug || self->trace)
    report_event(self, &event);
  self->myState(self, &event);
  return 0;
}

static int state_tmr_callback(void* ctx, int mode)
{
  pDMC2280Driv self = (pDMC2280Driv) ctx;
  EvtEvent event;
  self->state_timer = 0;
  event.event_type = eTimerEvent;
  if (self->debug || self->trace)
    report_event(self, &event);
  self->myState(self, &event);
  return 0;
}

static int state_cmd_execute(pDMC2280Driv self, enum commandtype cmd) {
  EvtEvent event;
  event.event_type = eCommandEvent;
  event.event.cmd.cmd_type = cmd;
  if (self->debug || self->trace)
    report_event(self, &event);
  self->myState(self, &event);
  return 0;
}

static void DMCState_Unknown(pDMC2280Driv self, pEvtEvent event) {
  char cmd[CMDLEN];
  int value;

  switch (event->event_type) {
    case eTimerEvent:
      /* Set speed */
      value =  motSpeed(self, self->speed);
      snprintf(cmd, CMDLEN, "SP%c=%d", self->axisLabel, value);
      if (FAILURE == DMC2280Send(self, cmd))
        break;
      /* Set acceleration */
      value = motAccel(self, self->accel);
      snprintf(cmd, CMDLEN, "AC%c=%d", self->axisLabel, value);
      if (FAILURE == DMC2280Send(self, cmd))
        break;
      /* Set deceleration */
      value = motDecel(self, self->decel);
      snprintf(cmd, CMDLEN, "DC%c=%d", self->axisLabel, value);
      if (FAILURE == DMC2280Send(self, cmd))
        break;
      snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
          self->axisLabel,
          self->axisLabel,
          self->axisLabel);
      DMC_SendCmd(self, cmd, state_msg_callback);
      return;
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        if (pCmd->out_buf[0] == 'M') { /* MG */
          int iRet;
          iRet = set_currMotion(self, pCmd->inp_buf);
          if (iRet == 0)
            break;
          set_lastMotion(self, self->currSteps, self->currCounts);
          if (self->abs_encoder == 0) {
            change_state(self, DMCState_Idle);
            return;
          }
#if 0
          value = ((self->currCounts  - self->absEncHome) / self->cntsPerX) * self->stepsPerX;
          self->currSteps = value;
          snprintf(cmd, CMDLEN, "DP%c=%d", self->axisLabel, value);
#else
          snprintf(cmd, CMDLEN, "LV");
#endif
          DMC_SendCmd(self, cmd, state_msg_callback);
          return;
        }
        if (pCmd->out_buf[0] == 'D') { /* DP */
          change_state(self, DMCState_Idle);
          return;
        }
        if (pCmd->out_buf[0] == 'L') { /* LV */
          char req[12];
          char rdy[12];
          snprintf(req, 12, "REQ%c=", self->axisLabel);
          snprintf(rdy, 12, "RSP%c=", self->axisLabel);
          if (strstr(pCmd->inp_buf, req) &&
              strstr(pCmd->inp_buf, rdy)) {
            self->has_airpads = 2;
          }
          change_state(self, DMCState_Idle);
          return;
        }
      }
      while (0);
      break;
    case eTimeoutEvent:
      /* TODO: handle timeout */
      break;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_Idle(pDMC2280Driv self, pEvtEvent event) {
  char cmd[CMDLEN];
  switch (event->event_type) {
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        if (pCmd->out_buf[0] == 'M') { /* MG _XQ0,_TSx */
          float fReply;
          int iRet, iFlags;
          int fwd_limit_active, rvrs_limit_active, errorlimit;
          iRet = sscanf(pCmd->inp_buf, "%f %d", &fReply, &iFlags);
          if (iRet != 2)
            break;
          if (fReply < 0) {
            if (self->subState > 0) {
              self->driver_status = HWFault;
              self->errorCode = THREADZERO;
              return;
            }
            DMC_SendCmd(self, "XQ #AUTO,0", state_msg_callback);
            return;
          }
          /* Handle limit switches */
          fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
          rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
          errorlimit = (iFlags & STATUSERRORLIMIT);
          if (fwd_limit_active && rvrs_limit_active) {
            self->errorCode = IMPOSSIBLE_LIM_SW;
            self->driver_status = HWFault;
          } else if (errorlimit) {
            self->errorCode = ERRORLIM;
            self->driver_status = HWFault;
          }
          if (self->driver_status == HWFault) {
            return;
          }
          if (self->has_airpads == 1) {
            snprintf(cmd, CMDLEN, "FTUBE=1");
            DMC_SendCmd(self, cmd, state_msg_callback);
            change_state(self, DMCState_AirOn);
          }
          else if (self->has_airpads == 2) {
            snprintf(cmd, CMDLEN, "REQ%c=1", self->axisLabel);
            DMC_SendCmd(self, cmd, state_msg_callback);
            change_state(self, DMCState_AirOn);
          }
          else {
            snprintf(cmd, CMDLEN, "SH%c", self->axisLabel);
            DMC_SendCmd(self, cmd, state_msg_callback);
            change_state(self, DMCState_MotorOn);
          }
          return;
        }
        if (pCmd->out_buf[0] == 'X') { /* XQ #AUTO,0 */
          snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel);
          DMC_SendCmd(self, cmd, state_msg_callback);
          self->subState = 1;
        }
      } while (0);
      break;
    case eCommandEvent:
      switch (event->event.cmd.cmd_type) {
        case CMD_RUN:
          if (self->pMot == NULL)
            self->pMot = FindMotor(pServ->pSics, self->name);
          self->driver_status = HWBusy;
          snprintf(cmd, CMDLEN, "MG _XQ0,_TS%c", self->axisLabel);
          DMC_SendCmd(self, cmd, state_msg_callback);
          self->subState = 0;
          return;
        case CMD_HALT:
          /* TODO: handle halt command */
          return;
      }
      break;
    case eTimeoutEvent:
      /* TODO handle message timeout */
      break;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_AirOn(pDMC2280Driv self, pEvtEvent event) {
  char cmd[CMDLEN];
  switch (event->event_type) {
    case eTimerEvent:
      if (self->has_airpads == 2)
        snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
      else
        snprintf(cmd, CMDLEN, "MG APDONE");
      DMC_SendCmd(self, cmd, state_msg_callback);
      return;
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        if (pCmd->out_buf[0] == 'F' || /* FTUBE */
            pCmd->out_buf[0] == 'R') { /* REQx= */
          NetWatchRegisterTimer(&self->state_timer,
              AIR_POLL_TIMER,
              state_tmr_callback, self);
          return;
        }
        else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */
          float fReply;
          fReply = (float) atof(pCmd->inp_buf);
          if (fReply > 0) {
            NetWatchRegisterTimer(&self->state_timer,
                ON_SETTLE_TIMER,
                state_tmr_callback, self);
            change_state(self, DMCState_MotorOn);
            return;
          }
          /* TODO handle airpad timeout */
          NetWatchRegisterTimer(&self->state_timer,
              AIR_POLL_TIMER,
              state_tmr_callback, self);
          return;
        }
      } while (0);
      break;
      /* TODO: handle halt command */
    case eTimeoutEvent:
      /* TODO handle message timeout */
      break;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_MotorOn(pDMC2280Driv self, pEvtEvent event) {
  char cmd[CMDLEN];
  switch (event->event_type) {
    case eTimerEvent:
      snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
          self->axisLabel,
          self->axisLabel,
          self->axisLabel);
      DMC_SendCmd(self, cmd, state_msg_callback);
      return;
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        if (pCmd->out_buf[0] == 'S') { /* SH */
          NetWatchRegisterTimer(&self->state_timer,
              ON_SETTLE_TIMER,
              state_tmr_callback, self);
          return;
        }
        else if (pCmd->out_buf[0] == 'M') { /* MG */
          int iRet, absolute;
          float target;
          iRet = set_currMotion(self, pCmd->inp_buf);
          if (iRet == 0)
            break;
          set_lastMotion(self, self->currSteps, self->currCounts);
          /* compute position for PA command */
          target = self->fTarget;
          self->preseek = 0;
          if (self->backlash_offset) {
            if (self->backlash_offset > 0) {
              /*
               * We want to be moving from high to low,
               * if the target is higher than current
               * we must pre-seek to the higher side
               */
              if (target > self->currPosition) {
                self->preseek = 1;
                target += self->backlash_offset;
                if (target > self->fUpper)
                  target = self->fUpper;
              }
            }
            else if (self->backlash_offset < 0) {
              /*
               * We want to be moving from to low high ,
               * if the target is lower than current
               * we must pre-seek to the lower side
               */
              if (target < self->currPosition) {
                self->preseek = 1;
                target += self->backlash_offset;
                if (target < self->fLower)
                  target = self->fLower;
              }
            }
          }
          self->creep_val = 0;
          absolute = motCreep(self, target);
          snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute);
          DMC_SendCmd(self, cmd, state_msg_callback);
          return;
        }
        else if (pCmd->out_buf[0] == 'P') { /* PA */
          self->stepCount = 1;
          snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
          DMC_SendCmd(self, cmd, state_msg_callback);
          change_state(self, DMCState_Moving);
          return;
        }
      } while (0);
      break;
      /* TODO: handle halt command */
    case eTimeoutEvent:
      /* TODO handle message timeout */
      break;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_Moving(pDMC2280Driv self, pEvtEvent event) {
  char cmd[CMDLEN];
  switch (event->event_type) {
    case eTimerEvent:
      snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
          self->axisLabel,
          self->axisLabel,
          self->axisLabel);
      DMC_SendCmd(self, cmd, state_msg_callback);
      return;
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        if (pCmd->out_buf[0] == 'B') { /* BG */
          NetWatchRegisterTimer(&self->state_timer,
              MOTOR_POLL_TIMER,
              state_tmr_callback, self);
          return;
        }
        else if (pCmd->out_buf[0] == 'P') { /* PA */
          snprintf(cmd, CMDLEN, "BG%c", self->axisLabel);
          DMC_SendCmd(self, cmd, state_msg_callback);
          self->stepCount++;
          return;
        }
        else if (pCmd->out_buf[0] == 'M') { /* MG */
          int iRet, iFlags;
          bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;
          iRet = set_currMotion(self, pCmd->inp_buf);
          if (iRet == 0)
            break;
          iFlags = self->currFlags;
          moving = (iFlags & STATUSMOVING);
          if (moving) {
            /* If Motion Control is off, report HWFault */
            if (DMC2280MotionControl != 1) {
              state_cmd_execute(self, CMD_HALT);
              if (DMC2280MotionControl == 0)
                self->errorCode = MOTIONCONTROLOFF;
              else
                self->errorCode = MOTIONCONTROLUNK;
              self->driver_status = HWFault;
              return;
            }
            if (self->abs_encoder && checkMotion(self) == 0)
            {
              /* handle blocked */
              state_cmd_execute(self, CMD_HALT);
              self->errorCode = BLOCKED;
              self->driver_status = HWFault;
              return;
            }
            NetWatchRegisterTimer(&self->state_timer,
                MOTOR_POLL_TIMER,
                state_tmr_callback, self);
            return;
          }
          /* Motor has stopped */
          /* Handle limit switches */
          fwd_limit_active = !(iFlags & STATUSFWDLIMIT);
          rvrs_limit_active = !(iFlags & STATUSRVRSLIMIT);
          errorlimit = (iFlags & STATUSERRORLIMIT);
          if (fwd_limit_active && rvrs_limit_active) {
            self->errorCode = IMPOSSIBLE_LIM_SW;
            self->driver_status = HWFault;
          } else if (fwd_limit_active) {
            self->errorCode = FWDLIM;
            self->driver_status = HWFault;
          } else if (rvrs_limit_active) {
            self->errorCode = RVRSLIM;
            self->driver_status = HWFault;
          } else if (errorlimit) {
            self->errorCode = ERRORLIM;
            self->driver_status = HWFault;
          }
          if (self->driver_status == HWFault) {
            state_cmd_execute(self, CMD_HALT);
            return;
          }
          /*
           * If this was a pre-seek then compute the next iteration
           */
          if (self->preseek) {
            float target;
            int absolute;
            self->preseek = 0;
            target = self->fTarget;
            /* if we are not creeping it is backlash correction */
            /* TODO: ensure minimum progress so it doesn't stall */
            /* TODO: handle edge conditions like limits */
            if (self->creep_val == 0) {
              float precision;
              MotorGetPar(self->pMot, "precision", &precision);
              /* TODO: take precision into account */
              if (self->backlash_offset > 0) {
                if (target + self->backlash_offset > self->currPosition + precision) {
                  self->preseek = 1;
                  target += self->backlash_offset + precision;
                  if (target > self->fUpper)
                    target = self->fUpper;
                }
              }
              else if (self->backlash_offset < 0) {
                if (target + self->backlash_offset < self->currPosition - precision) {
                  self->preseek = 1;
                  target += self->backlash_offset - precision;
                  if (target < self->fLower)
                    target = self->fLower;
                }
              }

              /* limit the maximum number of tries */
              if (self->preseek && self->stepCount > 10) {
                if (self->debug) {
                  char line[CMDLEN];
                  snprintf(line, CMDLEN, "Motor=%s preseek stopped stepcount = %d",
                      self->name, self->stepCount);
                  SICSLogWrite(line, eStatus);
                }
                self->preseek = 0;
              }
              else if (self->preseek) {
                absolute = motCreep(self, target);
              }
              else {
                /* change of direction, reset motion check */
                set_lastMotion(self, self->currSteps, self->currCounts);
                absolute = motCreep(self, target);
              }
            }
            else
              absolute = motCreep(self, target);
            /*
             * If we are still iterating, continue
             */
            if (self->preseek) {
              if (absolute == self->currSteps) {
                if (self->debug) {
                  char line[CMDLEN];
                  snprintf(line, CMDLEN, "Motor=%s motion stopped absolute = %d",
                      self->name, absolute);
                  SICSLogWrite(line, eStatus);
                }
                self->preseek = 0;
              }
              else {
                snprintf(cmd, CMDLEN, "PA%c=%d", self->axisLabel, absolute);
                DMC_SendCmd(self, cmd, state_msg_callback);
                return;
              }
            }
            /*
             * We are no longer iterating, so fall through
             */
          }
          /* handle settle time */
          if (self->settle && self->time_settle_done.tv_sec == 0) {
            gettimeofday(&self->time_settle_done, NULL);
            self->time_settle_done.tv_sec += self->settle;
            NetWatchRegisterTimer(&self->state_timer,
                self->settle,
                state_tmr_callback, self);
            return;
          }
          if (self->motOffDelay)
            NetWatchRegisterTimer(&self->state_timer,
                self->motOffDelay,
                state_tmr_callback, self);
          else
            NetWatchRegisterTimer(&self->state_timer,
                MOTOR_POLL_TIMER,
                state_tmr_callback, self);
          self->driver_status = HWIdle;
          change_state(self, DMCState_OffTimer);
          return;
        }
      } while (0);
      break;
    case eCommandEvent:
      switch (event->event.cmd.cmd_type) {
        case CMD_HALT:
          /* handle halt command, send message */
          snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
          DMC_SendCmd(self, cmd, state_msg_callback);
          change_state(self, DMCState_MotorHalt);
          return;
      }
      break;
    case eTimeoutEvent:
      strncpy(self->lastCmd, event->event.msg.cmd->out_buf, CMDLEN);
      self->errorCode = MOTCMDTMO;
      self->driver_status = HWFault;
      state_cmd_execute(self, CMD_HALT);
      return;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_MotorHalt(pDMC2280Driv self, pEvtEvent event)
{
  char cmd[CMDLEN];
  switch (event->event_type) {
    case eTimerEvent:
      snprintf(cmd, CMDLEN, "MG _TD%c,_TP%c,_TS%c",
          self->axisLabel,
          self->axisLabel,
          self->axisLabel);
      DMC_SendCmd(self, cmd, state_msg_callback);
      return;
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        if (pCmd->out_buf[0] == 'S') { /* ST */
          NetWatchRegisterTimer(&self->state_timer,
              MOTOR_POLL_TIMER,
              state_tmr_callback, self);
          return;
        }
        else if (pCmd->out_buf[0] == 'M') { /* MG */
          int iRet, iFlags;
          iRet = set_currMotion(self, pCmd->inp_buf);
          if (iRet == 0)
            break;
          iFlags = self->currFlags;
          if (iFlags & STATUSMOVING) {
            NetWatchRegisterTimer(&self->state_timer,
                MOTOR_POLL_TIMER,
                state_tmr_callback, self);
            return;
          }
          if (self->has_airpads == 1) {
            snprintf(cmd, CMDLEN, "FTUBE=0");
            DMC_SendCmd(self, cmd, state_msg_callback);
            change_state(self, DMCState_AirOff);
            return;
          }
          else if (self->has_airpads == 2) {
            snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
            DMC_SendCmd(self, cmd, state_msg_callback);
            change_state(self, DMCState_AirOff);
            return;
          }
          else {
            snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
            DMC_SendCmd(self, cmd, state_msg_callback);
            change_state(self, DMCState_MotorOff);
            return;
          }
        }
      } while (0);
      break;
    case eTimeoutEvent:
      /* TODO handle message timeout */
      break;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_OffTimer(pDMC2280Driv self, pEvtEvent event) {
  char cmd[CMDLEN];
  switch (event->event_type) {
    case eTimerEvent:
      if (self->has_airpads == 1) {
        snprintf(cmd, CMDLEN, "FTUBE=0");
        DMC_SendCmd(self, cmd, state_msg_callback);
        change_state(self, DMCState_AirOff);
      }
      else if (self->has_airpads == 2) {
        snprintf(cmd, CMDLEN, "REQ%c=0", self->axisLabel);
        DMC_SendCmd(self, cmd, state_msg_callback);
        change_state(self, DMCState_AirOff);
        return;
      }
      else {
        snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
        DMC_SendCmd(self, cmd, state_msg_callback);
        change_state(self, DMCState_MotorOff);
      }
      return;
    case eCommandEvent:
      switch (event->event.cmd.cmd_type) {
        case CMD_RUN:
          /* handle run command, convert to motor on timer expired */
          NetWatchRemoveTimer(self->state_timer);
          self->state_timer = 0;
          self->driver_status = HWBusy;
          change_state(self, DMCState_MotorOn);
          event->event_type = eTimerEvent;
          self->myState(self, event);
          return;
        case CMD_HALT:
          /* handle halt command, convert to motor off timer expired */
          NetWatchRemoveTimer(self->state_timer);
          self->state_timer = 0;
          event->event_type = eTimerEvent;
          self->myState(self, event);
          return;
      }
      return;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_AirOff(pDMC2280Driv self, pEvtEvent event) {
  char cmd[CMDLEN];
  switch (event->event_type) {
    case eTimerEvent:
      if (self->has_airpads == 2)
        snprintf(cmd, CMDLEN, "MG RSP%c", self->axisLabel);
      else
        snprintf(cmd, CMDLEN, "MG APDONE");
      DMC_SendCmd(self, cmd, state_msg_callback);
      return;
    case eMessageEvent:
      do {
        pAsyncTxn pCmd = event->event.msg.cmd;
        if (pCmd->out_buf[0] == 'F' || /* FTUBE */
            pCmd->out_buf[0] == 'R') { /* REQx= */
        }
        else if (pCmd->out_buf[0] == 'M') { /* MG APDONE/RSPx */
          float fReply;
          fReply = (float) atof(pCmd->inp_buf);
          if (fReply == 0) {
            NetWatchRegisterTimer(&self->state_timer,
                ON_SETTLE_TIMER,
                state_tmr_callback, self);
            change_state(self, DMCState_MotorOff);
            return;
          }
        }
        NetWatchRegisterTimer(&self->state_timer,
            AIR_POLL_TIMER,
            state_tmr_callback, self);
      } while (0);
      return;
    case eCommandEvent:
      switch (event->event.cmd.cmd_type) {
        case CMD_RUN:
          /* TODO: handle run command */
          return;
        case CMD_HALT:
          /* TODO: handle halt command */
          return;
      }
      break;
    case eTimeoutEvent:
      /* TODO handle message timeout */
      break;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

static void DMCState_MotorOff(pDMC2280Driv self, pEvtEvent event) {
  switch (event->event_type) {
    case eTimerEvent:
    case eMessageEvent:
      /* progress to IDLE */
      change_state(self, DMCState_Idle);
      if (self->driver_status == HWBusy)
        self->driver_status = HWIdle;
      return;
    case eCommandEvent:
      switch (event->event.cmd.cmd_type) {
        case CMD_RUN:
          /* TODO: handle run command */
          return;
        case CMD_HALT:
          /* TODO: handle halt command */
          return;
      }
      break;
    case eTimeoutEvent:
      /* TODO handle message timeout */
      break;
  }
  unhandled_event(self, event);
  self->errorCode = STATEERROR;
}

/** \brief Reads motor position, implements the GetPosition
 * method in the MotorDriver interface.
 *
 * \param *pData provides access to a motor's data
 * \param *fPos contains the motor position in physical units after a call.
 * \return
 * - OKOK request succeeded
 * - HWFault request failed
 * */
static int DMC2280GetPos(void *pData, float *fPos){
  pDMC2280Driv self = NULL;
  char reply[CMDLEN];
  char cmd[CMDLEN];
  float absEncPos, motorPos;

  reply[0]='\0';
  self = (pDMC2280Driv)pData;
  assert(self != NULL);

#if 0
  if (self->has_fsm) {
    if (self->myState == DMCState_Unknown)
      SicsWait(1);
    if (self->abs_encoder)
      *fPos = (self->currCounts - self->absEncHome)/self->cntsPerX + self->home;
    else
      *fPos = (self->currSteps - self->motorHome)/self->stepsPerX + self->home;
    return OKOK;
  }
#endif

  if (1 == self->abs_encoder) {
    if (readAbsEnc(self, &absEncPos) == FAILURE)
      return HWFault;
    *fPos = (absEncPos - self->absEncHome)/self->cntsPerX + self->home;
  } else {
    snprintf(cmd, ERRLEN, "TD%c", self->axisLabel);
    if (FAILURE == DMC2280SendReceive(self, cmd, reply))
      return HWFault;
    motorPos =(float)atof(reply);
    *fPos = (motorPos - self->motorHome)/self->stepsPerX + self->home;
  }
  return OKOK;
}

/** \brief DMC2280 implementation of the RunTo
 * method in the MotorDriver interface.
 *
 * \param *pData provides access to a motor's data
 * \param fValue target position in physical units, software zeros
 * have already been applied.
 * \return
 * - OKOK request succeeded
 * - HWFault request failed
 * */
static int DMC2280Run(void *pData,float fValue){
  pDMC2280Driv self = NULL;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);

  /* If Motion Control is off, report HWFault */
  if (DMC2280MotionControl != 1) {
    if (DMC2280MotionControl == 0)
      self->errorCode = MOTIONCONTROLOFF;
    else
      self->errorCode = MOTIONCONTROLUNK;
    return HWFault;
  }

  if (self->motoff_timer)
    NetWatchRemoveTimer(self->motoff_timer);
  self->motoff_timer = NULL;

  if (self->settle)
    self->time_settle_done.tv_sec = 0;

  /*
   * Note: this will read the current position which will block
   */
  do {
    float currPos;
    DMC2280GetPos(pData, &currPos);
    self->lastPosition = currPos;
    self->time_lastPos_set.tv_sec = 0;
  } while (0);

  self->fTarget = fValue;

  if (self->has_fsm) {
    while (self->myState == DMCState_AirOff
        || self->myState == DMCState_MotorOff) {
      SicsWait(1);
    }
    state_cmd_execute(self, CMD_RUN);
    return 1;
  }
  
  /*
   * Note: this passes control to a timer routine
   */
  if (self->has_airpads)
    if (self->airpad_state != AIRPADS_UP)
      return DMC2280RunAir(self, fValue);

  return DMC2280RunCommon(self, fValue);
}


/** \brief Returns the motor status while it's moving,
 * implements the GetStatus method in the MotorDriver interface.
 *
 * \param *pData provides access to a motor's data
 * \return
 * - HWFault hardware fault or status cannot be read.
 * - HWPosFault controller was unable to position the motor.
 * - HWBusy The motor is still driving.
 * - HWWarn There is a warning from the controller.
 * - HWIdle The motor has finished driving and is idle.
 */
static int DMC2280Status(void *pData){
  pDMC2280Driv self = NULL;
  char cmd[CMDLEN];
  int switches;
  char switchesAscii[CMDLEN];
#ifdef BACKLASHFIX
  char reply[CMDLEN];
  int SERVO_LOOP_NOT_RUNNING = -1, servoLoopStatus;
  int SHOULD_FIXPOS=1, should_fixpos;
#endif
  bool moving, fwd_limit_active, rvrs_limit_active, errorlimit;

  self = (pDMC2280Driv)pData;
  assert(self != NULL);

  if (self->has_fsm) {
    return self->driver_status;
  }
  /*
   * If we are waiting for the motor or airpads then we
   * are busy
   */
  if (self->motoff_timer || self->airpad_timer)
    return HWBusy;

  /* Make sure that speed, accel and decel are set correctly */
  /* ckSpeedAccelDecel(self); */
  /* Get status of switches
   * see TS (Tell Switches) in Galil manc2xx.pdf */
  snprintf(cmd, CMDLEN, "TS%c", self->axisLabel);
  if (FAILURE == DMC2280SendReceive(self, cmd, switchesAscii))
    return HWFault;
  sscanf(switchesAscii, "%d", &switches);
  moving = (switches & STATUSMOVING)>0;
  fwd_limit_active = !((switches & STATUSFWDLIMIT)>0);
  rvrs_limit_active = !((switches & STATUSRVRSLIMIT)>0);
  errorlimit = (switches & STATUSERRORLIMIT)>0;

  if (fwd_limit_active && rvrs_limit_active) {
    self->errorCode = IMPOSSIBLE_LIM_SW;
    return HWFault;
  }
  if (moving) {
    int iRet;
    /* If Motion Control is off, report HWFault */
    if (DMC2280MotionControl != 1) {
      if (DMC2280MotionControl == 0)
        self->errorCode = MOTIONCONTROLOFF;
      else
        self->errorCode = MOTIONCONTROLUNK;
      return HWFault;
    }
    /* If pos hasn't changed since last
     * check then stop and scream */
#if 0
    iRet = checkPosition(pData);
#else
    iRet = checkMotion(pData);
#endif
    if (iRet == 0) {
      DMC2280Halt(pData);
      self->errorCode = BLOCKED;
      return HWFault;
    } else {
      self->errorCode = BADBSY;
      return HWBusy;
    }
  } else {
    /* If motor stopped check limits and error status */
    if (fwd_limit_active) {
      self->errorCode = FWDLIM;
      return HWFault;
    } else if (rvrs_limit_active) {
      self->errorCode = RVRSLIM;
      return HWFault;
    } else if (errorlimit) {
      self->errorCode = ERRORLIM;
      return HWFault;
    }
#ifdef BACKLASHFIX
    if (self->abs_encoder == 1) {
      /* Make sure that the servo loop is closed by checking if
       * the CLSLOOP thread is running on the controller.*/
      if (FAILURE == DMC2280SendReceive(self, "MG _XQ1", reply))
        return HWFault;
      sscanf(reply, "%d", &servoLoopStatus);
      if (servoLoopStatus == SERVO_LOOP_NOT_RUNNING) {
        /* Start subroutine on controller to close the servo loop */
        if (FAILURE == DMC2280Send(self, "XQ#CLSLOOP"))
          return HWFault;
      }
      snprintf(cmd, CMDLEN, "MG %cSHLDFIX", self->axisLabel);
      if (FAILURE == DMC2280SendReceive(self, cmd, reply))
        return HWFault;
      sscanf(reply, "%d", &should_fixpos);
      if (should_fixpos == SHOULD_FIXPOS) {
        snprintf(cmd, CMDLEN, "%cFIXPOS=1", self->axisLabel);
        if (FAILURE == DMC2280Send(self, cmd))
          return HWFault;
        self->errorCode=BADBSY;
        return HWBusy;
      }
    }
#endif
    /*
     * When we get here, the motion has completed
     */
    if (self->preseek) {
      DMC2280RunCommon(self, self->fTarget);
      return HWBusy;
    }
    /* and we
     * must determine when and how to shut off the motor
     */
    if (self->settle) {
      struct timeval *then = &self->time_settle_done;
      struct timeval now;
      gettimeofday(&now, NULL);
      if (then->tv_sec == 0 ||
          (then->tv_sec - now.tv_sec) > self->settle) {
        gettimeofday(then, NULL);
        then->tv_sec += self->settle;
        return HWBusy;
      } else {
        if ((now.tv_sec > then->tv_sec) ||
            ((now.tv_sec == then->tv_sec) &&
            (now.tv_usec >= then->tv_usec))) {
          /* it's finished, fall through */
        } else {
          return HWBusy;
        }
      }
    }
    if (self->has_airpads) {
      if (self->airpad_state == AIRPADS_DOWN)
        return HWIdle;
      if (self->airpad_state == AIRPADS_LOWER)
        return HWBusy;
      if (self->motOffDelay > 0 ) {
        NetWatchRegisterTimer(&self->motoff_timer,
            self->motOffDelay,
            motoff_timeout, self);
        return HWIdle;
      }
      if (!DMC_AirPads(self, 0))
        return HWFault;
      return HWIdle;
    }
    if (self->noPowerSave == _SAVEPOWER) {
      if (self->motOffDelay > 0 ) {
#if 0
        snprintf(cmd, CMDLEN, "AT %d; MO%c", self->motOffDelay, self->axisLabel);
#else
      NetWatchRegisterTimer(&self->motoff_timer,
          self->motOffDelay,
          motoff_timeout, self);
      return HWIdle;
#endif
      } else {
        snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
      }
      DMC2280Send(self, cmd);
    }
    return HWIdle;
  }
}
/** \brief DMC2280 implementation of the GetError
 * method in the MotorDriver interface.
 *
 * \param *pData provides access to a motor's data
 * \param *iCode error code returned to abstract motor
 * \param *error error message
 * \param errLen maximum error message length allowed by abstract motor
 */
static void DMC2280Error(void *pData, int *iCode, char *error, int errLen){
  pDMC2280Driv self = NULL;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);

  /* Allocate errLen bytes for error messages */
  if (self->errorMsg == NULL) {
    self->errorMsg = (char *) malloc(errLen);
    if (self->errorMsg == NULL) {
      *iCode = 0;
      return;
    }
  }
  *iCode = self->errorCode;
  switch(*iCode){
#ifdef HAS_RS232
    case NOTCONNECTED:
    case BADSEND:
    case BADMEMORY:
    case INCOMPLETE:
      strncpy(error, "General Error(TODO)", (size_t)errLen); /* TODO */
      break;
#endif
    case MOTCMDTMO:
      strncpy(error, "Command Timeout", (size_t)errLen);
      break;
    case BADADR:
      strncpy(error, "Bad address", (size_t)errLen);
      break;
    case BADBSY:
      strncpy(error, "Motor still busy", (size_t)errLen);
      break;
    case BADCMD:
      snprintf(error, (size_t)errLen, "Bad command: '%s'\ndmcError: ", self->lastCmd);
      strncat(error, self->dmc2280Error, (size_t)errLen);
      break;
    case BADPAR:
      strncpy(error, "Bad parameter", (size_t)errLen);
      break;
    case BADUNKNOWN:
      strncpy(error, "Unknown error condition", (size_t)errLen);
      break;
    case BADSTP:
      strncpy(error, "Motor is stopped", (size_t)errLen);
      break;
    case BADEMERG:
      strncpy(error, "Emergency stop is engaged", (size_t)errLen);
      break;
    case BGFAIL:
      strncpy(error, "Begin not possible due to limit switch", (size_t)errLen);
      break;
    case RVRSLIM:
      strncpy(error, "Crashed into limit switch", (size_t)errLen);
      break;
    case FWDLIM:
      strncpy(error, "Crashed into limit switch", (size_t)errLen);
      break;
    case POSFAULT:
      strncpy(error, "Positioning fault detected", (size_t)errLen);
      break;
    case BADCUSHION:
      strncpy(error, "Air cushion problem", (size_t)errLen);
      break;
    case ERRORLIM:
      strncpy(error, "Axis error exceeds error limit", (size_t)errLen);
      break;
    case IMPOSSIBLE_LIM_SW:
      strncpy(error, "Both limit switches seem active, maybe the polarity is set 'active low'. You should configure the controller with CN 1,-1,-1,0", (size_t)errLen);
      break;
    case BLOCKED:
      strncpy(error, "STOPPING MOTOR, MOTION SEEMS TO BE BLOCKED", (size_t)errLen);
      break;
    case MOTIONCONTROLOFF:
      strncpy(error, "MOTION CONTROL SEEMS TO BE DISABLED", (size_t)errLen);
      break;
    case MOTIONCONTROLUNK:
      strncpy(error, "MOTION CONTROL SEEMS TO BE UNKNOWN", (size_t)errLen);
      break;
    case STATEERROR:
      strncpy(error, "ERROR IN DMC2280 FINITE STATE MACHINE", (size_t)errLen);
      break;
    case THREADZERO:
      strncpy(error, "THREAD ZERO NOT RUNNING ON CONTROLLER", (size_t)errLen);
      break;
    default:
      /* FIXME What's the default */
      snprintf(error, (size_t)errLen, "Unknown Motor Error: %d", self->errorCode);
      break;
  }
  strncpy(self->errorMsg, error, (size_t)errLen);
}

/** \brief Attempts to recover from an error. Implements the TryAndFixIt
 * method in the MotorDriver interface.
 *
 * \param *pData provides access to a motor's data
 * \param iCode error code returned by DMC2280Error
 * \param fValue unused, target position
 * \return A return code which informs the abstract motors next action.
 * - MOTREDO try to redo the last move.
 * - MOTFAIL move failed, give up.
 */
static int DMC2280Fix(void *pData, int iCode,/*@unused@*/ float fValue){
  pDMC2280Driv self = NULL;

  self = (pDMC2280Driv)pData;
  assert(self != NULL);

  switch(iCode){
    case BADADR:
      return MOTFAIL;
    case BADCMD:
    case BADPAR:
    case BLOCKED:
    case MOTIONCONTROLOFF:
    case MOTIONCONTROLUNK:
    case MOTCMDTMO:
    case THREADZERO:
      return MOTFAIL;
    case POSFAULT:
#if HAS_RS232
    case BADSEND:
    case TIMEOUT:
    case BADMEMORY: /* Won't happen if MonConnect sets the send terminator */
    case INCOMPLETE:
      return MOTREDO;
    case NOTCONNECTED:
#endif
      return MOTREDO;
      break;
    case STATEERROR:
      /* TODO: recover state error */
      if (self->has_fsm) {
        if (self->state_timer)
          NetWatchRemoveTimer(self->state_timer);
        self->state_timer = 0;
        change_state(self, DMCState_Unknown);
        /* Schedule a timer event as soon as possible */
        NetWatchRegisterTimer(&self->state_timer,
            0,
            state_tmr_callback, self);
      }
      return MOTFAIL;
    default:
      return MOTFAIL;
      break;
  }
}
/** \brief Emergency halt. Implements the Halt
 * method in the MotorDriver interface.
 *
 * Cannot set maximum deceleration because the DC command
 * is not valid for absolute (ie PA) moves. See DC description
 * in DMC-2xxx command ref (ie manc2xxx.pdf)
 * \param *pData provides access to a motor's data
 *
 * XXX Does abstract motor use the return values?
 */
static int DMC2280Halt(void *pData){
  pDMC2280Driv self = NULL;
  char cmd[CMDLEN];

  self = (pDMC2280Driv)pData;
  assert(self != NULL);

  if (self->has_fsm) {
    state_cmd_execute(self, CMD_HALT);
    return 1;
  }
  /* Stop motor */
  snprintf(cmd, CMDLEN, "ST%c", self->axisLabel);
  if (FAILURE == DMC2280Send(self, cmd))
    return HWFault;
  /* TODO: FIXME cannot do this until motor stops */
  snprintf(cmd, CMDLEN, "MO%c", self->axisLabel);
  if (FAILURE == DMC2280Send(self, cmd))
    return HWFault;
  else
    return 1;
}

/** \brief Fetches the value of the named parameter,
 * implements the GetDriverPar method in the MotorDriver interface.
 *
 * Note: The GetDriverPar method in the MotorDriver interface only
 * allows float values to be returned.
 *
 * If the speed, acceleration or deceleration is requested then
 * this compares the setting on the controller to the required setting,
 * if they don't match then the controller is set to the required value.
 *
 * Note: Doesn't warn if the speed, acceleration, or deceleration set on
 * the controller differ from the required settings.
 *
 * \param *pData (r) provides access to a motor's data
 * \param *name (r) the name of the parameter to fetch.
 * \param *fValue (w) the parameter's value.
 * \return
 * - 1 request succeeded
 * - 0 request failed
 * */
static int DMC2280GetPar(void *pData, char *name,
    float *fValue){
  pDMC2280Driv self = NULL;

  self = (pDMC2280Driv)pData;

  /* XXX Maybe move this to a configuration parameter.*/
  if(strcasecmp(name,HOME) == 0) {
    *fValue = self->home;
    return 1;
  }
  if(strcasecmp(name,HARDLOWERLIM) == 0) {
    *fValue = self->fLower;
    return 1;
  }
  if(strcasecmp(name,HARDUPPERLIM) == 0) {
    *fValue = self->fUpper;
    return 1;
  }
  if(strcasecmp(name,SPEED) == 0) {
    *fValue = self->speed;
    return 1;
  }
  if(strcasecmp(name,MAXSPEED) == 0) {
    *fValue = self->maxSpeed;
    return 1;
  }
  if(strcasecmp(name,ACCEL) == 0) {
    *fValue = self->accel;
    return 1;
  }
  if(strcasecmp(name,MAXACCEL) == 0) {
    *fValue = self->maxAccel;
    return 1;
  }
  if(strcasecmp(name,DECEL) == 0) {
    *fValue = self->decel;
    return 1;
  }
  if(strcasecmp(name,MAXDECEL) == 0) {
    *fValue = self->maxDecel;
    return 1;
  }
  if(strcasecmp(name,MOTOFFDELAY) == 0) {
    *fValue = self->motOffDelay;
    return 1;
  }
  if(strcasecmp(name,"debug") == 0) {
    *fValue = self->debug;
    return 1;
  }
  if(strcasecmp(name,SETTLE) == 0) {
    *fValue = self->settle;
    return 1;
  }
  if(strcasecmp(name,AIRPADS) == 0) {
    *fValue = self->has_airpads;
    return 1;
  }
  if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
    *fValue = self->blockage_ckInterval;
    return 1;
  }
  if(strcasecmp(name,"blockage_thresh") == 0) {
    *fValue = self->blockage_thresh;
    return 1;
  }
  if(strcasecmp(name,"blockage_ratio") == 0) {
    *fValue = self->blockage_ratio;
    return 1;
  }
  if(strcasecmp(name,"blockage_fail") == 0) {
    *fValue = self->blockage_fail;
    return 1;
  }
  if(strcasecmp(name,"backlash_offset") == 0) {
    *fValue = self->backlash_offset;
    return 1;
  }
  if (self->abs_encoder != 0) {
    if (strcasecmp(name,"absenc") == 0) {
      if (readAbsEnc(self, fValue) == SUCCESS)
        return 1;
      else
        return 0;
    }
    if (strcasecmp(name,"absenchome") == 0) {
      *fValue = self->absEncHome;
      return 1;
    }
    if (self->has_fsm) {
      if(strcasecmp(name,"creep_offset") == 0) {
        *fValue = self->creep_offset;
        return 1;
      }
      if(strcasecmp(name,"creep_precision") == 0) {
        *fValue = self->creep_precision;
        return 1;
      }
    }
  }
  else {
    if (strcasecmp(name,"homerun") == 0) {
      if (readHomeRun(self, fValue) == SUCCESS)
        return 1;
      else
        return 0;
    }
  }
  if(strcasecmp(name,"thread0") == 0)
    return ReadThread(self, 0, fValue);
  if(strcasecmp(name,"thread1") == 0)
    return ReadThread(self, 1, fValue);
  if(strcasecmp(name,"thread2") == 0)
    return ReadThread(self, 2, fValue);
  if(strcasecmp(name,"thread3") == 0)
    return ReadThread(self, 3, fValue);
  if(strcasecmp(name,"thread4") == 0)
    return ReadThread(self, 4, fValue);
  if(strcasecmp(name,"thread5") == 0)
    return ReadThread(self, 5, fValue);
  if(strcasecmp(name,"thread6") == 0)
    return ReadThread(self, 6, fValue);
  if(strcasecmp(name,"thread7") == 0)
    return ReadThread(self, 7, fValue);
  return 0;
}

/** \brief Sets the named parameter, implements the SetDriverPar
 * method in the MotorDriver interface.
 *
 * Note: The SetDriverPar method in the MotorDriver interface only
 * allows float values to be set.
 * \param *pData (rw) provides access to a motor's data
 * \param *pCon (r) connection object.
 * \param *name (r) of the parameter to set.
 * \param *newValue (r) new value.
 * \return
 * - 1 request succeeded
 * - 0 request failed
 * */
static int DMC2280SetPar(void *pData, SConnection *pCon,
    char *name, float newValue){
  pDMC2280Driv self = NULL;
  char pError[ERRLEN];
  char cmd[CMDLEN];

  self = (pDMC2280Driv)pData;

  /* XXX Maybe move this to a configuration parameter.*/
  /* Set home, managers only. Users should set softposition */
  if(strcasecmp(name,HOME) == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      if ( (self->fLower - newValue) > FLT_EPSILON) {
        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, HOME, self->fLower);
        SCWrite(pCon, pError, eError);
        return 1;
      }
      if ( (newValue - self->fUpper) > FLT_EPSILON) {
        snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, HOME, self->fUpper);
        SCWrite(pCon, pError, eError);
        return 1;
      }
      self->home = newValue;
      return 1;
    }
  }

  if(strcasecmp(name,SETPOS) == 0) {
    float oldZero, newZero;
    if (self->pMot == NULL)
      self->pMot = FindMotor(pServ->pSics, self->name);
    MotorGetPar(self->pMot,"softzero",&oldZero);
    newZero = (self->pMot->fPosition - newValue) + oldZero;
    MotorSetPar(self->pMot,pCon,"softzero",newZero);
    return 1;
  }

  /* Set motor off delay, managers only */
  if(strcasecmp(name,MOTOFFDELAY) == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->motOffDelay = newValue;
      return 1;
    }
  }

  /* Debug, managers only */
  if(strcasecmp(name,"debug") == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->debug = newValue;
      return 1;
    }
  }

  /* Setttle, managers only */
  if(strcasecmp(name,SETTLE) == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->settle = newValue;
      return 1;
    }
  }

  /* Set airpads, managers only */
  if(strcasecmp(name,AIRPADS) == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->has_airpads = newValue;
      return 1;
    }
  }

  /* Set interval between blocked motor checks,
   * managers only */
  if(strcasecmp(name,BLOCKAGE_CHECK_INTERVAL) == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->blockage_ckInterval = newValue;
      return 1;
    }
  }

  /* Set movement threshold for blocked motor checks,
   * managers only */
  if(strcasecmp(name,"blockage_thresh") == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->blockage_thresh = newValue;
      return 1;
    }
  }

  /* Set ratio for blocked motor checks,
   * managers only */
  if(strcasecmp(name,"blockage_ratio") == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->blockage_ratio = newValue;
      return 1;
    }
  }

  /* Set blocked motor checks failure mode,
   * managers only */
  if(strcasecmp(name,"blockage_fail") == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->blockage_fail = newValue;
      return 1;
    }
  }

  /* Set backlash offset,
   * managers only */
  if(strcasecmp(name,"backlash_offset") == 0) {
    if(!SCMatchRights(pCon,usMugger))
      return 1;
    else {
      self->backlash_offset = newValue;
      return 1;
    }
  }

  if (self->abs_encoder) { /* If we DO have an absolute encoder */
    if (self->has_fsm) { /* If we DO have a finite state machine */
      /* Set creep offset */
      if (strcasecmp(name,"creep_offset") == 0) {
        if(!SCMatchRights(pCon,usMugger)) /* managers only */
          return 1;
        else {
          self->creep_offset = fabs(newValue);
          return 1;
        }
      }

      /* Set creep_precision */
      if (strcasecmp(name,"creep_precision") == 0) {
        if(!SCMatchRights(pCon,usMugger)) /* managers only */
          return 1;
        else {
          self->creep_precision = fabs(newValue);
          return 1;
        }
      }
    }
  }
  else { /* If we do NOT have an absolute encoder */

    /* Invoke Home Run routine in controller */
    if(strcasecmp(name,"homerun") == 0) {
      if(!SCMatchRights(pCon,usMugger)) /* managers only */
        return 1;
      else {
        if (DMC2280MotionControl != 1 && newValue > 0.5) {
          snprintf(pError, ERRLEN,"ERROR: Motion Control must be on");
          SCWrite(pCon, pError, eError);
        }
        RunHomeRoutine(self, newValue);
        return 1;
      }
    }
  }

  /* Set speed */
  if(strcasecmp(name,SPEED) == 0) {
    if ((0.0 - newValue) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, SPEED, 0.0);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    if ((newValue - self->maxSpeed ) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, SPEED, self->maxSpeed);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    self->speed = newValue;
    snprintf(cmd,CMDLEN,"SP%c=%d", self->axisLabel, motSpeed(self, self->speed));
    if (FAILURE == DMC2280Send(self, cmd))
      return 0; /* FIXME should signal a HWFault */
    return 1;
  }

  /* Set Acceleration */
  if(strcasecmp(name,ACCEL) == 0) {
    if ((0.0 - newValue) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, ACCEL, 0.0);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    if ((newValue - self->maxAccel ) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, ACCEL, self->maxAccel);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    self->accel = newValue;
    snprintf(cmd,CMDLEN,"AC%c=%d", self->axisLabel, motAccel(self, self->accel));
    if (FAILURE == DMC2280Send(self, cmd))
      return 0; /* FIXME should signal a HWFault */
    return 1;
  }

  /* Set Deceleration */
  if(strcasecmp(name,DECEL) == 0) {
    if ((0.0 - newValue) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be greater than or equal to %f", self->name, DECEL, 0.0);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    if ((newValue - self->maxDecel ) > FLT_EPSILON) {
      snprintf(pError, ERRLEN,"ERROR:'%s %s' must be less than or equal to %f", self->name, DECEL, self->maxDecel);
      SCWrite(pCon, pError, eError);
      return 1;
    }
    self->decel = newValue;
    snprintf(cmd,CMDLEN,"DC%c=%d", self->axisLabel, motDecel(self, self->decel));
    if (FAILURE == DMC2280Send(self, cmd))
      return 0; /* FIXME should signal a HWFault */
    return 1;
  }

  return 0;
}
/** \brief List the motor parameters to the client.
 * \param self (r) provides access to the motor's data structure
 * \param *name (r) name of motor.
 * \param *pCon (r) connection object.
 */
static void DMC2280StrList(pDMC2280Driv self, char *name, SConnection *pCon){
  char buffer[BUFFLEN];

  snprintf(buffer, BUFFLEN, "%s.part = %s\n", name, self->part);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.long_name = %s\n", name, self->long_name);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.axis = %c\n", name, self->axisLabel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.units = %s\n", name, self->units);
  SCWrite(pCon, buffer, eStatus);
  return;
}
/** \brief List the motor parameters to the client.
 * \param self (r) provides access to the motor's data structure
 * \param *name (r) name of motor.
 * \param *pCon (r) connection object.
 */
static void DMC2280List(void *pData, char *name, SConnection *pCon){
  pDMC2280Driv self = (pDMC2280Driv) pData;
  char buffer[BUFFLEN];

  snprintf(buffer, BUFFLEN, "%s.home = %f\n", name, self->home);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.speed = %f\n", name, self->speed);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.maxSpeed = %f\n", name, self->maxSpeed);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.accel = %f\n", name, self->accel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.maxAccel = %f\n", name, self->maxAccel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.decel = %f\n", name, self->decel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.maxDecel = %f\n", name, self->maxDecel);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.motOffDelay = %d\n", name, self->motOffDelay);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.Debug = %d\n", name, self->debug);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.Settle = %d\n", name, self->settle);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.Blockage_Check_Interval = %f\n", name, self->blockage_ckInterval);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.Blockage_Thresh = %f\n", name, self->blockage_thresh);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.Blockage_Ratio = %f\n", name, self->blockage_ratio);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.Blockage_Fail = %d\n", name, self->blockage_fail);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.Backlash_offset = %f\n", name, self->backlash_offset);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.AirPads = %d\n", name, self->has_airpads);
  SCWrite(pCon, buffer, eStatus);
  snprintf(buffer, BUFFLEN, "%s.absEnc = %d\n", name, self->abs_encoder);
  SCWrite(pCon, buffer, eStatus);
  if (self->abs_encoder) {
    snprintf(buffer, BUFFLEN, "%s.absEncHome = %d\n", name, self->absEncHome);
    SCWrite(pCon, buffer, eStatus);
    if (self->has_fsm) {
      snprintf(buffer, BUFFLEN, "%s.cntsPerX = %f\n", name, self->cntsPerX);
      SCWrite(pCon, buffer, eStatus);
      snprintf(buffer, BUFFLEN, "%s.Creep_Offset = %f\n", name, self->creep_offset);
      SCWrite(pCon, buffer, eStatus);
      snprintf(buffer, BUFFLEN, "%s.Creep_Precision = %f\n", name, self->creep_precision);
      SCWrite(pCon, buffer, eStatus);
    }
  }
  snprintf(buffer, BUFFLEN, "%s.stepsPerX = %f\n", name, self->stepsPerX);
  SCWrite(pCon, buffer, eStatus);
  return;
}

static void DMC_Notify(void* context, int event)
{
  pDMC2280Driv self = (pDMC2280Driv) context;
  char line[132];

  switch (event) {
    case AQU_DISCONNECT:
      snprintf(line, 132, "Disconnect on Motor '%s'", self->name);
      SICSLogWrite(line, eStatus);
      /* TODO: disconnect */
      break;
    case AQU_RECONNECT:
      snprintf(line, 132, "Reconnect on Motor '%s'", self->name);
      SICSLogWrite(line, eStatus);
      /* TODO: reconnect */
      if (self->has_fsm) {
        /* Reset the state machine */
        if (self->state_timer)
          NetWatchRemoveTimer(self->state_timer);
        self->state_timer = 0;
        change_state(self, DMCState_Unknown);
        /* Schedule a timer event as soon as possible */
        NetWatchRegisterTimer(&self->state_timer,
            0,
            state_tmr_callback, self);
      }
      break;
  }
  return;
}

/** \brief Free memory if motor is removed
 * \param *pData (rw) provides access to the motor's data structure
 */
static void KillDMC2280(/*@only@*/void *pData){
  pDMC2280Driv self = NULL;
  self = (pDMC2280Driv)pData;
  assert(self != NULL);
  if (self->name) {
    free(self->name);
    self->name = NULL;
  }
  if (self->errorMsg) {
    free(self->errorMsg);
    self->errorMsg = NULL;
  }
  if (self->asyncUnit) {
    AsyncUnitDestroy(self->asyncUnit);
    self->asyncUnit = NULL;
  }
/* Not required as performed in caller
 * free(self);
 */
  return;
}

/** \brief Create a driver for the DMC2280 Galil controller.
 *
 * This is called by the Motor configuration command in the
 * SICS configuration file when you create a DMC2280 motor.
 *
 * Usage:\n
 * Motor stth DMC2280 paramArray\n
 * - stth is the motor name
 * - DMC2280 is the motor type that will lead to calling this function.
 * - paramArray is a Tcl array of the motor parameters.
 *
 * \param *pCon (r) connection object.
 * \param *motor (r) motor name
 * \param *params (r) configuration parameter array.
 * \return a reference to Motordriver structure
 *
 * NOTES:\n
 * -Adding parameters
 *  - Add a field for the parameter to the DMC2280Driv struct
 *  - Get the parameter from the parameter array, see PARAMETERS: below
 *  - If the parameter requires an abs enc then add it after ABSENC:
 */
MotorDriver *CreateDMC2280(SConnection *pCon, char *motor, char *params) {
  pDMC2280Driv pNew = NULL;
  char *pPtr = NULL;
  char buffer[132];
  char pError[ERRLEN];
  Tcl_Interp *interp;

  buffer[0]='\0';

  interp = InterpGetTcl(pServ->pSics);

  /*
     allocate and initialize data structure
     */
  pNew = (pDMC2280Driv)malloc(sizeof(DMC2280Driv));
  if(NULL == pNew){
    snprintf(pError, ERRLEN, "ERROR: no memory when creating DMC2280 motor '%s'", motor);
    SCWrite(pCon, pError, eError);
    return NULL;
  }
  memset(pNew, 0, sizeof(DMC2280Driv));

  /* Get AsyncQueue from the list of named parameters */
  if ((pPtr=getParam(pCon, interp, params, "multichan", _OPTIONAL)) != NULL ||
      (pPtr=getParam(pCon, interp, params, "asyncqueue", _OPTIONAL)) != NULL ||
      (pPtr=getParam(pCon, interp, params, "asyncunit", _OPTIONAL)) != NULL) {
    if (!AsyncUnitCreate(pPtr, &pNew->asyncUnit)) {
      snprintf(pError, ERRLEN, "Cannot find AsyncQueue '%s' when creating DMC2280 motor '%s'",
          pPtr, motor);
      SCWrite(pCon,pError,eError);
      KillDMC2280(pNew);
      free(pNew);
      return NULL;
    }
    AsyncUnitSetNotify(pNew->asyncUnit, pNew, DMC_Notify);
  }
  else if ((pPtr=getParam(pCon, interp, params, "host", _OPTIONAL)) != NULL) {
    char* host = pPtr;
    if ((pPtr=getParam(pCon, interp, params,"port",_REQUIRED)) == NULL) {
      snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
      SCWrite(pCon,pError,eError);
      KillDMC2280(pNew);
      free(pNew);
      return NULL;
    }
    /* AsyncUnit */
    if (!AsyncUnitCreateHost(host, pPtr, &pNew->asyncUnit)) {
      snprintf(pError, ERRLEN,
          "Cannot create AsyncUnit '%s:%s' for DMC2280 motor '%s'",
          host, pPtr, motor);
      SCWrite(pCon,pError,eError);
      KillDMC2280(pNew);
      free(pNew);
      return NULL;
    }
    AsyncUnitSetNotify(pNew->asyncUnit, pNew, DMC_Notify);
  }
  else {
    snprintf(pError, ERRLEN, "\tError occurred when creating DMC2280 motor '%s'", motor);
    SCWrite(pCon,pError,eError);
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }

  pNew->name = (char *)malloc(sizeof(char)*(strlen(motor)+1));
  if (pNew->name == NULL) {
    (void) SCWrite(pCon,"ERROR: no memory to allocate motor driver",
                   eError);
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }

  pNew->pMot = NULL;
  strcpy(pNew->name, motor);
  pNew->home = 0.0;
  pNew->fLower = 0.0;//(float)atof(argv[2]);
  pNew->fUpper = 0.0;//(float)atof(argv[3]);
  pNew->GetPosition = DMC2280GetPos;
  pNew->RunTo = DMC2280Run;
  pNew->GetStatus = DMC2280Status;
  pNew->GetError = DMC2280Error;
  pNew->TryAndFixIt = DMC2280Fix;
  pNew->Halt = DMC2280Halt;
  pNew->GetDriverPar = DMC2280GetPar;
  pNew->SetDriverPar = DMC2280SetPar;
  pNew->ListDriverPar = DMC2280List;
  pNew->KillPrivate = KillDMC2280;
  pNew->GetDriverTextPar = NULL;
  pNew->blockage_ckInterval = 0.5;
  pNew->blockage_thresh = 0.5; /* minimum distance in units */
  pNew->blockage_ratio = 2.0; /* maximum ratio observed/expected */
  pNew->blockage_fail = 1; /* fail the motor */
  pNew->backlash_offset = 0.0;
  pNew->myState = DMCState_Unknown;

  /* PARAMETERS: Fetch parameter values */

  /* Debug: this motor driver logs exchanges */
  if ((pPtr=getParam(pCon, interp, params,"debug",_OPTIONAL)) == NULL)
    pNew->debug=0;
  else {
    sscanf(pPtr,"%d",&(pNew->debug));
  }

  /* FSM: this driver uses the finite state machine model */
  if ((pPtr=getParam(pCon, interp, params,"fsm",_OPTIONAL)) == NULL)
    pNew->has_fsm=1;
  else {
    sscanf(pPtr,"%d",&(pNew->has_fsm));
  }

  if ((pPtr=getParam(pCon, interp, params, LONG_NAME, _OPTIONAL)) != NULL) {
    strncpy(pNew->long_name, pPtr, sizeof(pNew->long_name));
    pNew->long_name[sizeof(pNew->long_name) - 1] = '\0';
  }
  if ((pPtr=getParam(pCon, interp, params,HARDLOWERLIM,_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%f",&(pNew->fLower));
  if ((pPtr=getParam(pCon, interp, params,HARDUPPERLIM,_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%f",&(pNew->fUpper));
  if ((pPtr=getParam(pCon, interp, params,UNITS,_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%s",pNew->units);
  if ((pPtr=getParam(pCon, interp, params,MAXSPEED,_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%f",&(pNew->speed));
  pNew->maxSpeed = pNew->speed;
  if ((pPtr=getParam(pCon, interp, params,MAXACCEL,_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%f",&(pNew->accel));
  pNew->maxAccel = pNew->accel;
  if ((pPtr=getParam(pCon, interp, params,MAXDECEL,_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%f",&(pNew->decel));
  pNew->maxDecel = pNew->decel;
  if ((pPtr=getParam(pCon, interp, params,"axis",_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%c",&(pNew->axisLabel));
  if ((pPtr=getParam(pCon, interp, params,"stepsperx",_REQUIRED)) == NULL) {
    KillDMC2280(pNew);
    free(pNew);
    return NULL;
  }
  sscanf(pPtr,"%f",&(pNew->stepsPerX));
  if ((pPtr=getParam(pCon, interp, params,"motorhome",_OPTIONAL)) == NULL)
    pNew->motorHome=0;
  else
    sscanf(pPtr,"%d",&(pNew->motorHome));
  if ((pPtr=getParam(pCon, interp, params,"nopowersave",_OPTIONAL)) == NULL)
    pNew->noPowerSave=_SAVEPOWER;
  else
    sscanf(pPtr,"%d",&(pNew->noPowerSave));
  if ((pPtr=getParam(pCon, interp, params,"motoffdelay",_OPTIONAL)) == NULL)
    pNew->motOffDelay=0;
  else
    sscanf(pPtr,"%d",&(pNew->motOffDelay));

  /* SETTLE: this motor need time to settle */
  if ((pPtr=getParam(pCon, interp, params,"settle",_OPTIONAL)) == NULL)
    pNew->settle=0;
  else {
    sscanf(pPtr,"%d",&(pNew->settle));
  }

  /* BACKLASH: this controls unidirectional driving */
  if ((pPtr=getParam(pCon, interp, params,"backlash_offset",_OPTIONAL)) == NULL)
    pNew->backlash_offset=0.0;
  else {
    sscanf(pPtr,"%f",&(pNew->backlash_offset));
  }

  /* AIRPADS: this motor need airpads */
  if ((pPtr=getParam(pCon, interp, params,"airpads",_OPTIONAL)) == NULL)
    pNew->has_airpads=0;
  else {
    sscanf(pPtr,"%d",&(pNew->has_airpads));
  }

  /* ABSENC: If the parameter requires an abs enc add it to the else block */
  if ((pPtr=getParam(pCon, interp, params,"absenc",_OPTIONAL)) == NULL)
    pNew->abs_encoder = 0;
  else {
    sscanf(pPtr,"%d",&(pNew->abs_encoder));
    if ((pPtr=getParam(pCon, interp, params,"absenchome",_REQUIRED)) == NULL)
      pNew->absEncHome = 0;
    else
      sscanf(pPtr,"%d",&(pNew->absEncHome));
    if ((pPtr=getParam(pCon, interp, params,"cntsperx",_REQUIRED)) == NULL)
      pNew->cntsPerX = 1.0;
    else
      sscanf(pPtr,"%f",&(pNew->cntsPerX));
    if (pNew->has_fsm) {
      /* CREEP_OFFSET: this controls unidirectional driving */
      if ((pPtr=getParam(pCon, interp, params,"creep_offset",_OPTIONAL)) == NULL)
        pNew->creep_offset = 0.0;
      else {
        sscanf(pPtr, "%f", &(pNew->creep_offset));
        if (pNew->creep_offset < 0)
          pNew->creep_offset = -pNew->creep_offset;
      }

      /* CREEP_PRECISION: this controls unidirectional driving */
      if ((pPtr=getParam(pCon, interp, params,"creep_precision",_OPTIONAL)) == NULL)
        pNew->creep_precision = 0.0;
      else {
        sscanf(pPtr, "%f", &(pNew->creep_precision));
        if (pNew->creep_precision < 0)
          pNew->creep_precision = -pNew->creep_precision;
      }
    }
  }

  if (pNew->has_fsm) {
    /* Schedule a timer event as soon as possible */
    NetWatchRegisterTimer(&pNew->state_timer,
        0,
        state_tmr_callback, pNew);
  }
  else {
    char cmd[CMDLEN];
    /* Set speed */
    snprintf(cmd,CMDLEN,"SP%c=%d", pNew->axisLabel, motSpeed(pNew, pNew->speed));
    if (FAILURE == DMC2280Send(pNew, cmd))
      return NULL;
    /* Set acceleration */
    snprintf(cmd,CMDLEN,"AC%c=%d", pNew->axisLabel, motAccel(pNew, pNew->accel));
    if (FAILURE == DMC2280Send(pNew, cmd))
      return NULL;
    /* Set deceleration */
    snprintf(cmd,CMDLEN,"DC%c=%d", pNew->axisLabel, motDecel(pNew, pNew->decel));
    if (FAILURE == DMC2280Send(pNew, cmd))
      return NULL;
    /* TODO Initialise current position and target
     * to get a sensible initial list output
     */
  }

  return (MotorDriver *)pNew;
}

int DMC2280Action(SConnection *pCon, SicsInterp *pSics, void *pData,
    int argc, char *argv[])
{
  pMotor pm = (pMotor) pData;
  pDMC2280Driv self = (pDMC2280Driv) pm->pDriver;

  if (argc > 1) {
    if (strcasecmp("send", argv[1]) == 0) {
      char cmd[CMDLEN];
      char rsp[CMDLEN];
      int idx = 0;
      int i, j;
      cmd[0] = '\0';
      for (i = 2; i < argc; ++i) {
        j = snprintf(&cmd[idx], CMDLEN - idx, "%s%s",
            (i > 2) ? " " : "",
            argv[i]);
        if (j < 0)
          break;
        idx += j;
      }
      DMC2280SendReceive(self, cmd, rsp);
      SCWrite(pCon, rsp, eValue);
      return 1;
    }
    else if(strcasecmp("data", argv[1]) == 0) {
      char line[132];
      snprintf(line, 132, "%s.motor_step = %f %s", self->name,
          1.0 / self->stepsPerX, self->units);
      SCWrite(pCon, line, eValue);
      if (self->abs_encoder) {
        snprintf(line, 132, "%s.encoder_count = %f %s", self->name,
            1.0 / self->cntsPerX, self->units);
        SCWrite(pCon, line, eValue);
        snprintf(line, 132, "%s.steps_per_count = %f steps", self->name,
            self->stepsPerX / self->cntsPerX);
        SCWrite(pCon, line, eValue);
      }
      return 1;
    }
    else if (strcasecmp("units", argv[1]) == 0) {
      if (argc > 2) {
        strncpy(self->units, argv[2], sizeof(self->units));
        self->units[sizeof(self->units) - 1] = '\0';
      }
      else {
        char line[132];
        snprintf(line, 132, "%s.units = %s", self->name, self->units);
        SCWrite(pCon, line, eValue);
      }
      return 1;
    }
    else if (strcasecmp("long_name", argv[1]) == 0) {
      if (argc > 2) {
        strncpy(self->long_name, argv[2], sizeof(self->long_name));
        self->long_name[sizeof(self->long_name) - 1] = '\0';
      }
      else {
        char line[132];
        snprintf(line, 132, "%s.long_name = %s", self->name, self->long_name);
        SCWrite(pCon, line, eValue);
      }
      return 1;
    }
    else if (strcasecmp("part", argv[1]) == 0) {
      if (argc > 2) {
        strncpy(self->part, argv[2], sizeof(self->part));
        self->part[sizeof(self->part) - 1] = '\0';
      }
      else {
        char line[132];
        snprintf(line, 132, "%s.part = %s", self->name, self->part);
        SCWrite(pCon, line, eValue);
      }
      return 1;
    }
    else if (strcasecmp("list", argv[1]) == 0) {
      /* Handle in generic motor driver */
    }
    else if (strcasecmp("slist", argv[1]) == 0) {
      DMC2280StrList(self, argv[0], pCon);
      return 1;
    }
    else if(strcasecmp(SETPOS, argv[1]) == 0) {
      float oldZero, newZero, currPos, newValue;
      if (self->pMot == NULL)
        self->pMot = FindMotor(pServ->pSics, self->name);
      MotorGetPar(self->pMot, "softzero", &oldZero);
      if (argc > 3) {
        sscanf(argv[2], "%f", &currPos);
        currPos += oldZero;
        sscanf(argv[3], "%f", &newValue);
      }
      else if (argc > 2 ){
        sscanf(argv[2], "%f", &newValue);
        currPos = self->pMot->fPosition;
      }
      else {
        char buffer[BUFFLEN];
        snprintf(buffer, BUFFLEN, "%s.setPos = %f\n", self->name, oldZero);
        SCWrite(pCon, buffer, eStatus);
        return 1;
      }
      newZero = (currPos - newValue);
      return MotorSetPar(self->pMot, pCon, "softzero", newZero);
    }
    else if(strcasecmp("reset", argv[1]) == 0) {
      if (self->has_fsm) {
        /* Reset the state machine */
        if (self->state_timer)
          NetWatchRemoveTimer(self->state_timer);
        self->state_timer = 0;
        change_state(self, DMCState_Unknown);
        /* Schedule a timer event as soon as possible */
        NetWatchRegisterTimer(&self->state_timer,
            0,
            state_tmr_callback, self);
      }
      /* Handle further in generic motor driver */
      return MotorAction(pCon, pSics, pData, argc, argv);
    }
    else if(self->has_fsm && strcasecmp("state", argv[1]) == 0) {
      char line[132];
      snprintf(line, 132, "%s.state = %s (timer=%s)",
          self->name, state_name(self->myState),
          self->state_timer ? "active" : "inactive");
      SCWrite(pCon, line, eValue);
      return 1;
    }
    else if(self->has_fsm && strcasecmp("trace", argv[1]) == 0) {
      if (argc > 2 && strcasecmp("on", argv[2]) == 0) {
        self->trace = pCon;
        SCWrite(pCon, "TRACE ON", eValue);
      }
      else {
        self->trace = 0;
      SCWrite(pCon, "TRACE OFF", eValue);
      }
      return 1;
    }
  }
  return MotorAction(pCon, pSics, pData, argc, argv);
}

void DMC2280InitProtocol(SicsInterp *pSics) {
  if (DMC2280_Protocol == NULL) {
    DMC2280_Protocol = AsyncProtocolCreate(pSics, "DMC2280", NULL, NULL);
    DMC2280_Protocol->sendCommand = DMC_Tx;
    DMC2280_Protocol->handleInput = DMC_Rx;
    DMC2280_Protocol->handleEvent = DMC_Ev;
    DMC2280_Protocol->prepareTxn = NULL;
    DMC2280_Protocol->killPrivate = NULL;
  }
}