/*
FILENAME...     omsBaseController.cpp
USAGE...        Pro-Dex OMS asyn motor base controller support

Version:        $Revision$
Modified By:    $Author$
Last Modified:  $Date$
HeadURL:        $URL$
*/

/*
 *  Created on: 06/2012
 *      Author: eden
 *
 */

#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <epicsExit.h>

#include "omsBaseController.h"

#define MIN(a,b) ((a)<(b)? (a): (b))

#define motorOmsStringSendString        "OMS_STRING_SEND"
#define motorOmsStringSendRecvString    "OMS_STRING_SENDRECV"
#define motorOmsStringRecvString        "OMS_STRING_RECV"
#define motorOmsPollString              "OMS_POLL"
#define MOTOR_OMS_PARAMS_COUNT          4

ELLLIST omsBaseController::omsControllerList;
int omsBaseController::omsTotalControllerNumber = 0;

static const char *driverName = "omsBaseDriver";

#ifdef __GNUG__
    #ifdef      DEBUG
        #define Debug(l, f, args...) {if (l <= motorOMSBASEdebug) \
                                  errlogPrintf(f, ## args);}
    #else
        #define Debug(l, f, args...)
    #endif
#else
    #define Debug
#endif
volatile int motorOMSBASEdebug = 0;
extern "C" {epicsExportAddress(int, motorOMSBASEdebug);}



omsBaseController::omsBaseController(const char *portName, int maxAxes, int prio, int stackSz, int extMotorParams=0)
    :   asynMotorController(portName, maxAxes, extMotorParams + MOTOR_OMS_PARAMS_COUNT,
            asynInt32Mask | asynFloat64Mask | asynOctetMask,
            asynInt32Mask | asynFloat64Mask | asynOctetMask,
            ASYN_CANBLOCK | ASYN_MULTIDEVICE,
            1, // autoconnect
            prio, stackSz)
{

    if (omsTotalControllerNumber == 0) {
        ellInit(&omsControllerList);
    }

    // check if portName is in use */
    if (findController(portName) != NULL)
        errlogPrintf("omsBaseController: ERROR: asynPort %s already in use\n", portName);

    omsBaseNode* pNode = new omsBaseNode;
    pNode->portName = epicsStrDup(portName);
    pNode->pController = this;
    ellAdd(&omsControllerList, (ELLNODE *)pNode);

    this->portName = epicsStrDup(portName);
    controllerNumber = omsTotalControllerNumber;
    ++omsTotalControllerNumber;

    sanityCounter = 0;
    useWatchdog = false;
    enabled = true;
    numAxes = maxAxes;
    controllerType = NULL;
    baseMutex = new epicsMutex;

    if (prio == 0)
        priority = epicsThreadPriorityLow;
    else
        priority = prio;

    if (stackSz == 0)
        stackSize = epicsThreadGetStackSize(epicsThreadStackMedium);
    else
        stackSize = stackSz;

    createParam(0, motorOmsPollString, asynParamInt32, &pollIndex);
    createParam(0, motorOmsStringSendString, asynParamOctet, &sendIndex);
    createParam(0, motorOmsStringSendRecvString, asynParamOctet, &sendReceiveIndex);
    createParam(0, motorOmsStringRecvString, asynParamOctet, &receiveIndex);
    setStringParam(0, sendIndex, (char *) "");
    setStringParam(0, sendReceiveIndex, (char *) "");
    setStringParam(0, receiveIndex, (char *) "");

    /* Set an EPICS exit handler */
    epicsAtExit(omsBaseController::callShutdown, this);

}

asynStatus omsBaseController::startPoller(double movingPollPeriod, double idlePollPeriod, int forcedFastPolls)
{
    char threadName[20];

    movingPollPeriod_ = movingPollPeriod/1000.;
    idlePollPeriod_   = idlePollPeriod/1000.;
    forcedFastPolls_  = forcedFastPolls;

    epicsSnprintf(threadName, sizeof(threadName), "OMSPoller-%d", controllerNumber);
    this->motorThread = epicsThreadCreate(threadName,
            priority, stackSize,
            (EPICSTHREADFUNC) &omsBaseController::callPoller, (void *) this);
  return asynSuccess;
}


void omsBaseController::shutdown(){
      lock();
      shuttingDown_ = 1;
      unlock();
}

void omsBaseController::report(FILE *fp, int level)
{
    int axis;
    double velocity, position, encoderPosition;
    int haveEncoder=0;

    fprintf(fp, "Oms %s motor driver %s, numAxes=%d\n",
        controllerType, portName, numAxes);

    for (axis=0; axis < numAxes; axis++) {
        omsBaseAxis *pAxis = pAxes[axis];
        fprintf(fp, "  axis %d\n", pAxis->axisNo_);

        if (level > 0)
        {
            lock();
            getDoubleParam(pAxis->axisNo_, motorVelocity_, &velocity);
            getDoubleParam(pAxis->axisNo_, motorPosition_, &position);
            getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
            if (haveEncoder) getDoubleParam(pAxis->axisNo_, motorEncoderPosition_, &encoderPosition);
            unlock();
            fprintf(fp, "    Current position = %f, velocity = %f\n",
                   position,
                   velocity);

             if (haveEncoder) fprintf(fp, "    Encoder position %f\n", encoderPosition );
            if (pAxis->homing) fprintf(fp, "    Currently homing axis\n" );
        }
    }
    // Call the base class method
    asynMotorController::report(fp, level);
}

omsBaseAxis * omsBaseController::getAxis(asynUser *pasynUser)
{
    int axisNo;

    getAddress(pasynUser, &axisNo);
    return pAxes[axisNo];
}

/** Returns a pointer to an omsBaseAxis object.
  * Returns NULL if the axis number is invalid.
  * \param[in] axisNo Axis index number. */
omsBaseAxis* omsBaseController::getAxis(int axisNo)
{
    if ((axisNo < 0) || (axisNo >= numAxes)) return NULL;
    return pAxes[axisNo];
}

asynStatus omsBaseController::writeOctet(asynUser *pasynUser, const char *value,
                                    size_t nChars, size_t *nActual)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    const char *functionName = "writeOctet";
    char inBuffer[MAX_STRING_SIZE];
    if (strlen(value) > nChars) return asynError;

    omsBaseAxis *pAxis = getAxis(pasynUser);
    if (!pAxis) return asynError;

    if (function == sendReceiveIndex)
    {
        status = sendReceiveLock(value, inBuffer, sizeof(inBuffer));
        if (status == asynSuccess){
            /* Set the parameter in the parameter library. */
            status = (asynStatus)setStringParam(pAxis->axisNo_, receiveIndex, (char *)inBuffer);
            status = (asynStatus)callParamCallbacks(pAxis->axisNo_);
            asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: answer is %s\n",
                    driverName, functionName, portName, inBuffer);
            *nActual = nChars;
        }
        else {
            *nActual = 0;
            asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: sendReceive Error\n",
                    driverName, functionName, portName);

        }
    }
    else if (function == sendIndex)
    {
        status = sendOnlyLock(value);
        if (status == asynSuccess){
            *nActual = nChars;
        }
        else {
            *nActual = 0;
            asynPrint(pasynUser, ASYN_TRACE_ERROR, "%s:%s:%s: send Error\n",
                    driverName, functionName, portName);
        }
    }
    return status;
}

asynStatus omsBaseController::readInt32(asynUser *pasynUser, epicsInt32 *value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    omsBaseAxis *pAxis = getAxis(pasynUser);
    static const char *functionName = "readInt32";
    static char outputBuffer[8];

    if (!pAxis) return asynError;

    if (function == motorPosition_) {
        strcpy(outputBuffer,"A? RP");
        sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
        *value = strtol(inputBuffer, NULL, 10);
     } else if (function == motorEncoderPosition_) {
         int haveEncoder;
         getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
         if (haveEncoder){
             strcpy(outputBuffer,"A? RE");
             sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
             *value = strtol(inputBuffer, NULL, 10);
         }
     } else {
          // Call base class
   	      status = asynMotorController::readInt32(pasynUser, value);
     }


    return status;
}

asynStatus omsBaseController::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    static const char *functionName = "writeInt32";
    omsBaseAxis *pAxis = getAxis(pasynUser);

    if (!pAxis) return asynError;

    status = pAxis->setIntegerParam(function, value);

    if (function == motorDeferMoves_)
    {
        asynPrint(pasynUser, ASYN_TRACE_ERROR,
            "%s:%s:%s Deferred Move: not yet implemented %s\n",
            driverName, functionName, portName);
    }
    else if (function == motorClosedLoop_)
    {
        if (value) {
            asynPrint(pasynUser, ASYN_TRACE_FLOW, "%s:%s:%s axis %d closed loop enable\n",
                  driverName, functionName, portName, pAxis->axisNo_);
            if (firmwareMin(1,30,0))
                status = sendReplace(pAxis, (char*) "A? CL1");
            else
                status = sendReplace(pAxis, (char*) "A? HN");
        } else {
            asynPrint(pasynUser, ASYN_TRACE_FLOW, "%s:%s:%s SetInteger axis %d closed loop disable\n",
                  driverName, functionName, portName, pAxis->axisNo_);
            if (firmwareMin(1,30,0))
                status = sendReplace(pAxis, (char*) "A? CL0");
            else
                status = sendReplace(pAxis, (char*) "A? HF");
        }
    }
    else if (function == motorMoveToHome_) {
        /* avoid  asynMotorController::writeInt32 to handle this*/
    }
    else if (function == pollIndex)
    {
    	 if (value) {
    		 wakeupPoller();
    	 }
    }
    else {
        return asynMotorController::writeInt32(pasynUser, value);
    }

    pAxis->callParamCallbacks();

    if (status)
        asynPrint(pasynUser, ASYN_TRACE_ERROR,
              "%s:%s: error, status=%d function=%d, value=%d\n",
              driverName, functionName, status, function, value);
    else
        asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
              "%s:%s: function=%d, value=%d\n",
              driverName, functionName, function, value);
    return status;
}

asynStatus omsBaseController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    static const char *functionName = "writeFloat64";
    omsBaseAxis *pAxis = getAxis(pasynUser);

    if (!pAxis) return asynError;


    /* Set the parameter and readback in the parameter library.  This may be overwritten when we read back the
     * status at the end, but that's OK */
    status = pAxis->setDoubleParam(function, value);

    if (function == motorEncoderPosition_){
        int haveEncoder;
        getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
        if (haveEncoder){
            asynPrint(pasynUser, ASYN_TRACE_FLOW,
                  "%s:%s:%s axis %d set encoder position to %f\n",
                  driverName, functionName, portName, pAxis->axisNo_, value);
            sprintf(inputBuffer,"A? LPE%d;",(int)(value));
            status = sendReplace(pAxis, inputBuffer);
        }
     }
/*
 *   Encoder Mode is currently not supported in motor record
     else if (function == motorEncoderRatio_) {
         int haveEncoder;
         getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &haveEncoder);
         if (haveEncoder){
            asynPrint(pasynUser, ASYN_TRACE_FLOW,
                  "%s:%s:%s axis %d set encoder ratio to %f\n",
                  driverName, functionName, portName, pAxis->axisNo_, value);
            sprintf(inputBuffer,"A? ER%d,100000;",(int)(value*100000));
            status = sendReplace(pAxis, inputBuffer);
        }
     }
*/
     else if ((function == motorResolution_) || (function == motorLowLimit_) || (function == motorHighLimit_)) {
            // we do nothing
            status = asynSuccess;
     }
     else if (function == motorPGain_) {
         if ((0.00 >= value) && (value < 32768.00)){
             asynPrint(pasynUser, ASYN_TRACE_FLOW,
                      "%s:%s:%s axis %d set proportional gain to %f\n",
                      driverName, functionName, portName, pAxis->axisNo_, value);
              sprintf(inputBuffer,"A? KP%f;", value);
              status = sendReplace(pAxis, inputBuffer);
          }
          else {
                asynPrint(pasynUser, ASYN_TRACE_ERROR,
                      "%s:%s:%s axis %d proportional gain %f not in range 0.0 -> 32768.0\n",
                      driverName, functionName, portName, pAxis->axisNo_, value);
            }
        }
     else if (function == motorIGain_) {
         if ((0.00 >= value) && (value < 32768.00)){
                asynPrint(pasynUser, ASYN_TRACE_FLOW,
                      "%s:%s:%s axis %d set integral gain to %f\n",
                      driverName, functionName, portName, pAxis->axisNo_, value);
                sprintf(inputBuffer,"A? KI%f;", value);
                status = sendReplace(pAxis, inputBuffer);
         }
         else {
                asynPrint(pasynUser, ASYN_TRACE_ERROR,
                      "%s:%s:%s axis %d integral gain %f not in range 0.0 -> 32768.0\n",
                      driverName, functionName, portName, pAxis->axisNo_, value);
         }
        }
     else if (function == motorDGain_)
    {
         if ((0.00 >= value) && (value < 32768.00)){
                 asynPrint(pasynUser, ASYN_TRACE_FLOW,
                       "%s:%s:%s axis %d set derivative gain to %f\n",
                       driverName, functionName, portName, pAxis->axisNo_, value);
                sprintf(inputBuffer,"A? KD%f;", value);
                status = sendReplace(pAxis, inputBuffer);
          }
          else {
                asynPrint(pasynUser, ASYN_TRACE_ERROR,
                      "%s:%s:%s axis %d derivative gain %f not in range 0.0 -> 32768.0\n",
                      driverName, functionName, portName, pAxis->axisNo_, value);
          }
    }
     else {
         /* Call base classes method (if we have our parameters check this here) */
         status = asynMotorController::writeFloat64(pasynUser, value);
    }

    pAxis->callParamCallbacks();
    if (status)
        asynPrint(pasynUser, ASYN_TRACE_ERROR,
              "%s:%s: error, status=%d function=%d, value=%f\n",
              driverName, functionName, status, function, value);
    else
        asynPrint(pasynUser, ASYN_TRACEIO_DRIVER,
              "%s:%s: function=%d, value=%f\n",
              driverName, functionName, function, value);
    return status;
}



asynStatus omsBaseController::getFirmwareVersion()
{
    /* get FirmwareVersion, try 3 times */
    int count = 0;
    asynStatus status = asynError;
    char *p;

    while (status != asynSuccess && count < 3){
        epicsThreadSleep(1.0);
        status = sendReceiveLock((char*) "WY", inputBuffer, sizeof(inputBuffer));
        count++;
        errlogPrintf("OMS Firmware Version: %s\n", inputBuffer);
    }
    //
    if ((p = strstr(inputBuffer, "ver:"))){
        count = sscanf(p, "ver:%d.%d.%d,", &fwMajor, &fwMinor, &fwRevision);
    }
    if ((p == NULL) || (count < 2)) {
        errlogPrintf("omsBaseController::getFirmwareVersion: unable to retrieve Firmware version\n");
        status = asynError;
    }

    return status;
}

asynStatus omsBaseController::Init(const char* initString, int multiple){

    char *p, *tokSave;
    int totalAxes;
    char axisChrArr[OMS_MAX_AXES] = {'X','Y','Z','T','U','V','R','S'};
    char outputBuffer[10];
    epicsInt32  axisPosArr[OMS_MAX_AXES];

    /* Interrupt clear */
    sendOnlyLock("IC;");

    /* Stop all axes */
    sendOnlyLock("AM SA;");

    /* send InitString */
    if ((initString != NULL) && (strlen(initString) > 0)) {
        if (multiple){
            char* inittmp = epicsStrDup(initString);
            for (tokSave = NULL, p = epicsStrtok_r(inittmp, ";", &tokSave);
                    p != NULL; p = epicsStrtok_r(NULL, ";", &tokSave)) {
                sendOnlyLock(p);
            }
            free(inittmp);
        }
        else {
            sendOnlyLock(initString);
        }
    }

    /* get Positions of all axes */
    inputBuffer[0] = '\0';
    sendReceiveLock((char*) "AA RP;", inputBuffer, sizeof(inputBuffer));

    if (numAxes > OMS_MAX_AXES) {
        errlogPrintf("omsBaseController:Init: number of axes=%d exceeds allowed maximum\n", numAxes);
        return asynError;
    }

    /* The return string will tell us how many axes this controller has */
    for (totalAxes = 0, tokSave = NULL, p = epicsStrtok_r(inputBuffer, ",", &tokSave);
             p != 0; p = epicsStrtok_r(NULL, ",", &tokSave), totalAxes++);
    if ((totalAxes > numAxes) || (totalAxes > OMS_MAX_AXES)) {
        errlogPrintf("omsBaseController:Init: actual number of axes=%d > MIN(numAxes=%d, OMS_MAX_AXES)\n", totalAxes, numAxes);
        return asynError;
    }
    numAxes = totalAxes;

    pAxes = new omsBaseAxis*[numAxes];
    lock();
    for (int axis=0; axis < numAxes; axis++) {

        omsBaseAxis* pAxis  = new omsBaseAxis(this, axis, axisChrArr[axis]);
        pAxes[axis] = pAxis;

        pAxis->setIntegerParam(motorStatusDone_, 1);
        pAxis->setIntegerParam(motorStatusMoving_, 0);
        pAxis->setIntegerParam(motorStatusHomed_, 0);
        pAxis->setIntegerParam(motorStatusHome_, 0);
        pAxis->setIntegerParam(motorStatusAtHome_, 0);
        pAxis->setIntegerParam(motorStatusProblem_, 0);
        pAxis->setIntegerParam(motorStatusCommsError_, 0);

        /* Determine if encoder is present and if mode is stepper or servo. */
        if (firmwareMin(1,1,30))
            strcpy(outputBuffer,"A? PS?");
        else
            strcpy(outputBuffer,"A? ?PS");
        sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
        /*we expect any of "=O","=E","=M"  */
        if (inputBuffer[1] == 'O'){
            pAxis->setStepper(1);
            pAxis->setIntegerParam(motorStatusHasEncoder_, 0);
            pAxis->setDoubleParam(motorEncoderPosition_, 0.0); // reset if not available
        }
        else if (inputBuffer[1] == 'M'){
            pAxis->setStepper(0);
            pAxis->setIntegerParam(motorStatusHasEncoder_, 1);
        }
        else if (inputBuffer[1] == 'E'){
            pAxis->setStepper(1);
            pAxis->setIntegerParam(motorStatusHasEncoder_, 1);
        }
        else
            errlogPrintf("omsBaseController:Init: error: unknown axis type! (%s)\n", inputBuffer);

        /* Determine limit true state high or low */
        /* CAUTION you need firmware version 1.30 or higher to do this */
        if (firmwareMin(1,1,30))
            strcpy(outputBuffer,"A? LT?");
        else
            strcpy(outputBuffer,"A? ?LS");
        sendReceiveReplace(pAxis, outputBuffer, inputBuffer, sizeof(inputBuffer));
        /*we expect any of "=l" or "=h"  */
        if (inputBuffer[1] == 'l'){
            pAxis->setLimitInvert(1);
        }
        else if (inputBuffer[1] == 'h'){
            pAxis->setLimitInvert(0);
        }
        else
            errlogPrintf("omsBaseController:Init: error: unknown limit true state!\n");

    }
    if (getAxesPositions(axisPosArr) == asynSuccess){
    	for (int axis=0; axis < numAxes; axis++)
    		(pAxes[axis])->setDoubleParam(motorPosition_, (double) axisPosArr[axis]);
    }
    unlock();
    return asynSuccess;
}

void omsBaseController::callPoller(void *drvPvt)
{
    omsBaseController *pController = (omsBaseController*)drvPvt;
    pController->omsPoller();
}


#define DELTA 0.1
void omsBaseController::omsPoller()
{

    static const char *functionName = "BasePoller";

#define STATUSSTRINGLEN 5
    /* This is the task that polls the MAX card */
    double timeout, timeToWait;
    double movingPollPeriod;
    double idlePollPeriod;
    omsBaseAxis* pAxis;
    int retry_count, loopBreakCount=0, haveEncoder;
    // asynStatus status;
    int axisMoving, anyMoving=0;
    int forcedFastPolls, fastPolls=0;
    epicsInt32  axisPosArr[OMS_MAX_AXES], encPosArr[OMS_MAX_AXES], veloArr[OMS_MAX_AXES];
    char statusBuffer[OMS_MAX_AXES*STATUSSTRINGLEN+2];
    char encStatusBuffer[OMS_MAX_AXES*6+2];
    int closedLoopStatus[OMS_MAX_AXES];
    char encBuffer[7];
    unsigned int limitFlags;
    epicsTimeStamp now, loopStart;
    bool haveCLStatus, haveVeloArray, haveEncStatus, haveLimits, useEncoder=false, moveDone;

    lock();
    movingPollPeriod = movingPollPeriod_;
    idlePollPeriod = idlePollPeriod_;
    forcedFastPolls = forcedFastPolls_;
    for (int i=0; (i < numAxes); i++) {
        getIntegerParam(i, motorStatusHasEncoder_, &haveEncoder);
        if (haveEncoder) useEncoder = true;
    }
    unlock();


    while(1) {

        lock();
        if (shuttingDown_) {
          unlock();
          break;
        }
        unlock();

        if (loopBreakCount > 10){
            errlogPrintf("%s:%s:%s: Error: %d consecutive unsuccessful attempts to read from motor card\n"
                    , driverName, functionName, this->portName, loopBreakCount);
            loopBreakCount = 0;
        }

        epicsTimeGetCurrent(&loopStart);

        retry_count = 0;
        while ((getAxesPositions(axisPosArr) != asynSuccess) && (retry_count < 5)){
            epicsThreadSleepQuantum();
            ++retry_count;
        }
        if (retry_count > 4){
            errlogPrintf("%s:%s:%s: error reading axis position after %d attempts\n",
                    driverName, functionName, this->portName, retry_count);
            ++loopBreakCount;
            continue;
        }

        if (useEncoder && (getEncoderPositions(encPosArr) != asynSuccess)){
            Debug(1, "%s:%s:%s: error executing get Encoder Positions\n", driverName, functionName, this->portName);
            ++loopBreakCount;
            continue;
        }

        /* read all axis status values and reset done-field
         * MDNN,MDNN,PNLN,PNNN,PNLN,PNNN,PNNN,PNNN*/
        if (getAxesStatus(statusBuffer, sizeof(statusBuffer), &moveDone) != asynSuccess){
             Debug(1, "%s:%s:%s: error reading axes status\n", driverName, functionName, this->portName);
            ++loopBreakCount;
            continue;
        }
        loopBreakCount = 0;
/*
        if (sanityCheck() != asynSuccess){
            errlogPrintf("%s:%s:%s: error during sanity check\n", driverName, functionName, this->portName);
        }
*/

        if (anyMoving)
            haveCLStatus = false;
        else
            haveCLStatus = true;
        if (haveCLStatus && getClosedLoopStatus(closedLoopStatus) != asynSuccess){
            haveCLStatus = false;
            Debug(1, "%s:%s:%s: error executing get Closed Loop Status\n", driverName, functionName, this->portName);
        }

        haveVeloArray = true;
        if (getAxesArray((char*) "AM;RV;", veloArr) != asynSuccess){
            haveVeloArray = false;
            Debug(1,"%s:%s:%s: Error executing command Report Velocity (RV)\n", driverName, functionName, this->portName);
        }
        haveEncStatus = true;
        if (sendReceiveLock((char*) "AM;EA;", encStatusBuffer, sizeof(encStatusBuffer)) != asynSuccess){
            haveEncStatus = false;
            Debug(1,"%s:%s:%s: Error reading encoder status buffer >%s<\n", driverName, functionName, this->portName, encStatusBuffer);
        }

        /* read all limits */
        if (moveDone || (anyMoving == 0))
            haveLimits = true;
        else
              haveLimits = false;
        limitFlags =0;
        if (haveLimits){
            if ((sendReceiveLock((char*) "AM;QL;", pollInputBuffer, sizeof(pollInputBuffer)) == asynSuccess)){
                if (1 != sscanf(pollInputBuffer, "%x", &limitFlags)){
                    Debug(1,"%s:%s:%s: error converting limits: %s\n", driverName, functionName, this->portName, pollInputBuffer);
                    haveLimits = false;
                }
            }
            else {
                haveLimits = false;
                Debug(1,"%s:%s:%s: error reading limits %s\n", driverName, functionName, this->portName, pollInputBuffer);
            }
        }
        if (enabled) watchdogOK();

        anyMoving = 0;
        lock();
        for (int i=0; (i < numAxes) && enabled && (shuttingDown_ == 0); i++) {
            pAxis = pAxes[i];

            if (useEncoder){
                /* if this axis has an encoder, read encoder status */
                getIntegerParam(i, motorStatusHasEncoder_, &haveEncoder);
                if (haveEncoder){
                    if (haveEncStatus){
                        if (getSubstring(i, encStatusBuffer, encBuffer, sizeof(encBuffer)) == asynSuccess){
                            if (encBuffer[2] == 'S')
                                pAxis->setIntegerParam(motorStatusFollowingError_, 1);
                            else
                                pAxis->setIntegerParam(motorStatusFollowingError_, 0);
                        }
                        else{
                            errlogPrintf("%s:%s:%s: error parsing encoder status string %s\n",
                                    driverName, functionName, this->portName, encStatusBuffer);
                        }
                    }
                    pAxis->setDoubleParam(motorEncoderPosition_, (double) encPosArr[i]);
                }
            }

            /* check the done flag or current velocity */
            if (statusBuffer[i*STATUSSTRINGLEN + 1] == 'D'){
                Debug(3, "%s:%s:%s: found Done Flag axis %d\n", driverName, functionName, portName, i);
                pAxis->setIntegerParam(motorStatusProblem_, 0);
                pAxis->moveDelay=0;
                pAxis->setIntegerParam(motorStatusDone_, 1);
                pAxis->setIntegerParam(motorStatusMoving_, 0);
                if (pAxis->homing) pAxis->homing = 0;
                Debug(1, "%s:%s:%s: done axis %d \n", driverName, functionName,
                        this->portName, i );
            }
            else if (haveVeloArray && (veloArr[i] == 0)){
                /* keep a small delay here, to make sure that a motorAxisDone cycle is performed
                   even if the motor is on hard limits */
                getIntegerParam(pAxis->axisNo_, motorStatusMoving_, &axisMoving);
                if (axisMoving){
                    pAxis->moveDelay++ ;
                    if (pAxis->moveDelay >= 5) {
                        Debug(4, "%s:%s:%s: setting Problem Flag axis %d\n", driverName, functionName, portName, i);
                        pAxis->setIntegerParam(motorStatusDone_, 1);
                        pAxis->setIntegerParam(motorStatusMoving_, 0);
                        pAxis->setIntegerParam(motorStatusProblem_, 1);
                        pAxis->moveDelay = 0;
                        if (pAxis->homing) pAxis->homing = 0;
                        Debug(1, "%s:%s:%s: stop axis %d, moveDelay count %d\n", driverName, functionName,
                                this->portName, i, pAxis->moveDelay );
                    }
                    Debug(2, "%s:%s:%s: moveDelay axis %d, count %d\n", driverName, functionName,
                            this->portName, i, pAxis->moveDelay );
               }
            }
            else {
                Debug(4, "%s:%s:%s: poller loop: axis %d still moving\n", driverName, functionName, this->portName, i);
                pAxis->moveDelay = 0;
                pAxis->setIntegerParam(motorStatusProblem_, 0);
                pAxis->setIntegerParam(motorStatusDone_, 0);
                pAxis->setIntegerParam(motorStatusMoving_, 1);
            }
            getIntegerParam(pAxis->axisNo_, motorStatusMoving_, &axisMoving);
            anyMoving += axisMoving;

            /* check limits */
            if (haveLimits){
                if (((limitFlags & (1 << i)) > 0) ^ (pAxis->getLimitInvert()))
                    pAxis->setIntegerParam(motorStatusLowLimit_, 1);
                else
                    pAxis->setIntegerParam(motorStatusLowLimit_, 0);
                if (((limitFlags & (1 << (i+8))) > 0) ^ (pAxis->getLimitInvert()))
                    pAxis->setIntegerParam(motorStatusHighLimit_, 1);
                else
                    pAxis->setIntegerParam(motorStatusHighLimit_, 0);
            }

            /* check home switch */
            if (statusBuffer[i*STATUSSTRINGLEN + 3] == 'H')
                pAxis->setIntegerParam(motorStatusAtHome_, 1);
            else
                pAxis->setIntegerParam(motorStatusAtHome_, 0);

            /* check direction */
            if (statusBuffer[i*STATUSSTRINGLEN] == 'P')
                pAxis->setIntegerParam(motorStatusDirection_, 1);
            else
                pAxis->setIntegerParam(motorStatusDirection_, 0);

            /* set positions */
            pAxis->setDoubleParam(motorPosition_, (double) axisPosArr[i]);

            /* set closed loop status */
            if (haveCLStatus) pAxis->setIntegerParam(motorStatusGainSupport_, closedLoopStatus[i]);

            // callParamCallbacks(pAxis->axisNo_, pAxis->axisNo_);
            pAxis->callParamCallbacks();

        } /* Next axis */

        if (shuttingDown_) {
          unlock();
          break;
        }
        unlock();

        if (fastPolls > 0) {
            timeout = movingPollPeriod;
            fastPolls--;
        }
        else if (anyMoving) {
            timeout = movingPollPeriod;
        }
        else {
            timeout = idlePollPeriod;
        }

        /* wait here for the next poll
           waiting may be interrupted by pollEvent or interrupt messages*/
        epicsTimeGetCurrent(&now);
        timeToWait = timeout - epicsTimeDiffInSeconds(&now, &loopStart);
        Debug(5, "%s:%s:%s: poller loop: waiting %f s\n", driverName, functionName, this->portName, timeToWait);
        if (waitInterruptible(timeToWait) == epicsEventWaitOK) {
            fastPolls = forcedFastPolls;
        }
    } /* End while */
    Debug(1, "%s:%s:%s: omsPoller shutdown\n", driverName, functionName, portName);
}

asynStatus omsBaseController::getEncoderPositions(epicsInt32 encPosArr[OMS_MAX_AXES])
{
    return getAxesArray((char*) "AM PE;", encPosArr);
}

asynStatus omsBaseController::getClosedLoopStatus(int clstatus[OMS_MAX_AXES])
{
    asynStatus status = asynSuccess;
    char clBuffer[9];

    if (firmwareMin(1,30,0)){
        pollInputBuffer[0] = '\0';
        status = sendReceiveLock((char*) "AM;CL?;", pollInputBuffer, sizeof(pollInputBuffer));
        if (status == asynSuccess) {
            for (int i=0; i < numAxes; ++i) {
                status = getSubstring(i, pollInputBuffer, clBuffer, sizeof(clBuffer));
                if ( status == asynSuccess){
                    if (strncmp(clBuffer, "on", 2))
                        clstatus[i] = 1;
                    else
                        clstatus[i] = 0;
                }
            }
        }
    }
    else {
        for (int i=0; i < numAxes; ++i) {
            strcpy(clBuffer,"A? ?PM");
            status = sendReceiveReplace(pAxes[i], clBuffer, pollInputBuffer, sizeof(pollInputBuffer));
            if (status != asynSuccess){
                Debug(1, "%s:getClosedLoopStatus:%s: Error getting closed loop status %s\n",
                        driverName, portName, pollInputBuffer);
            }
            else {
                if (strncmp(pollInputBuffer, "=on", 3))
                    clstatus[i] = 1;
                else
                    clstatus[i] = 0;
            }
        }
    }
    return status;
}

asynStatus omsBaseController::sanityCheck()
{
    const char* functionName="sanityCheck";
    asynStatus status = asynSuccess;

    ++ sanityCounter;

    if (sanityCounter > 100){
        int commandBufferSize[OMS_MAX_AXES];
        char outputBuffer[10];
        // check the remaining command buffer size, which must not overflow
        sanityCounter = 0;
        if (getAxesArray((char*) "AM;RQC", commandBufferSize) != asynSuccess){
            errlogPrintf("%s:%s:%s: Error executing command: Report Command Queue (RCQ)\n", driverName, functionName, this->portName);
        }
        for (int i=0; i < numAxes; i++) {
            omsBaseAxis* pAxis = pAxes[i];
            // flush command buffer if size is below 100
            if (commandBufferSize[i] < 100) {
                errlogPrintf("%s:%s:%s: Caution: flushing command queue axis %d,remaining size %d\n",
                                    driverName, functionName, portName, pAxis->axisNo_, commandBufferSize[i]);
                strcpy(outputBuffer,"A?;FL;");
                sendReplace(pAxis, (char*) outputBuffer);
                status = asynError;
            }
        }
    }
    return status;
}

bool omsBaseController::firmwareMin(int major, int minor, int revision){
    if (major < fwMajor) return true;
    if (major == fwMajor){
        if (minor < fwMinor) return true;
        if (minor == fwMinor){
            if (revision <= fwRevision) return true;
        }
    }
    return false;
}

omsBaseController* omsBaseController::findController(const char* asynPort){

    omsBaseNode* pNode = (omsBaseNode*) ellFirst(&omsControllerList);
    while (pNode) {
        if (strcmp(pNode->portName, asynPort) == 0){
            return (pNode->pController);
        }
        pNode = (omsBaseNode *)ellNext(&pNode->node);
    }
    return (omsBaseController *) NULL;
}

epicsEventWaitStatus omsBaseController::waitInterruptible(double timeout)
{
    return epicsEventWaitWithTimeout(pollEventId_, timeout);
}

/*
    insert axis character and send to Controller
    If insertChar != 0 and 2nd Character is a "?", then replace it with axisChar
*/
asynStatus omsBaseController::sendReplace(omsBaseAxis* pAxis, char *outputBuff)
{
    asynStatus status;

    if (strncmp(outputBuff,"A? ",3) == 0 )
        outputBuff[1] = pAxis->axisChar;
    status = sendOnlyLock(outputBuff);
    return status;
}

/*
    insert axis character, send to Controller and wait for the answer
    If insertChar != 0 and 2nd Character is a "?", then replace it with axisChar
*/
asynStatus omsBaseController::sendReceiveReplace(omsBaseAxis* pAxis, char *outputBuff, char *inputBuff, int inputSize)
{
    asynStatus status;
    if (strncmp(outputBuff,"A? ",3) == 0 )
        outputBuff[1] = pAxis->axisChar;
    status = sendReceiveLock(outputBuff, inputBuff, inputSize);
    return status;
}

asynStatus omsBaseController::sendOnlyLock(const char *outputBuff)
{
    asynStatus status;
    baseMutex->lock();
    status = sendOnly(outputBuff);
    baseMutex->unlock();
    return status;
}

asynStatus omsBaseController::sendReceiveLock(const char *outputBuff, char *inputBuff, unsigned int inputSize)
{
    asynStatus status;
    baseMutex->lock();
    status = sendReceive(outputBuff, inputBuff, inputSize);
    baseMutex->unlock();
    return status;
}

asynStatus omsBaseController::getAxesStatus(char *inputBuff, int inputSize, bool *done)
{
    char *outputBuff = (char*) "AM;RI;";
    asynStatus status;

    *done=false;

    status = sendReceiveLock(outputBuff, inputBuff, inputSize);

    if (status == asynSuccess){
        if (strchr(inputBuff, 'D') != NULL) *done=true;
        if (!((inputBuff[0] == 'P') || (inputBuff[0] == 'M')))
            status = asynError;
        if (strlen(inputBuff) < (unsigned int)(numAxes * 5 -1))
            status = asynError;
        if (status == asynError) Debug(1, "%s:getAxesStatus:%s: corrupted status string %s\n",
                driverName, portName, inputBuff);
    }
    return status;
}

asynStatus omsBaseController::getAxesPositions(int positions[OMS_MAX_AXES] )
{
    return getAxesArray((char*) "AM PP;", positions);
}

asynStatus omsBaseController::getAxesArray(char* cmd, int positions[OMS_MAX_AXES] )
{
    //  maximum length of the ascii position / velocity values is 11 chars + comma
    //  -2147483648 <-> 2147483647
    // we expect numAxes values separated with commas
    // possible answers are "0,5000,0" ",,,," "0" ",,," (3 commas for 4 axes)

    const char* functionName="getAxesArray";
    asynStatus status = asynSuccess;
    char inputBuff[OMSINPUTBUFFERLEN] = "";
    char *start, *end, *stop;
    int i, intVal, again = 1;
    int count =0;

    status = sendReceiveLock(cmd, inputBuff, sizeof(inputBuff));
    if ((status == asynSuccess) && (strlen(inputBuff) >= (unsigned int)numAxes -1)) {
        start = inputBuff;
        stop = start + MIN(strlen(inputBuff), sizeof(inputBuff));
        for (i = 0; ((i < OMS_MAX_AXES) && again); ++i){
            if (*start == ','){
                positions[i] = 0;
                ++count;
                start += 1;
                if (*start == '\0'){
                    /* comma was last character */
                    ++count;
                    again=0;
                    if (i < OMS_MAX_AXES-1) positions[i+1] = 0;
                }
                continue;
            }
            intVal = strtol(start, &end, 10);
            if ((intVal == 0) && (start == end)) {
                again = 0;
            }
            else {
                positions[i] = intVal;
                ++count;
            }
            if (end >= stop) again = 0;
            start = end + 1;
        }
        if (count != numAxes) {
            errlogPrintf("%s:%s:%s: array string conversion error, count: %d, axes: %d, input: >%s<\n",
                                driverName, functionName, portName, count, numAxes, inputBuff);
            return asynError;
        }
    }
    else {
        if (status == asynSuccess){
            errlogPrintf("%s:%s:%s: read string too short %d\n",
                                driverName, functionName, portName, (int)strlen(inputBuff));
            return asynError;
        }
    }
    return status;
}

asynStatus omsBaseController::getSubstring(unsigned int number, char* inputBuffer, char *outBuffer, unsigned int outBufferLen)
{
    /* get the numbered substring from a comma-separated string */
    /* numbering starts with 0 */
    /* other as strtok accept a sequence of commas ,,, as empty substrings */
    char *start, *end, *pos, *tmpBuffer;
    unsigned int i, count;
    int doloop = 1;
    asynStatus status = asynError;


    if (strlen(inputBuffer) < number) return status;

    tmpBuffer = epicsStrDup(inputBuffer);
    start = tmpBuffer;
    end = start + strlen(start);
    for (i=0; i<=number && doloop==1; ++i){
        pos = strchr(start, ',');
        if (pos == NULL) {
            doloop = 0;
            count = MIN(strlen(start), outBufferLen-1);
        }
        else {
            count = MIN((unsigned int)(pos-start), outBufferLen-1);
            *pos = '\0';
        }
        if (i==number){
               strncpy(outBuffer, start, count);
                outBuffer[count]='\0';
                status = asynSuccess;
                break;
            }
        start = pos+1;
        if (start > end) doloop =0;
    }
    free (tmpBuffer);
    return status;
}

bool omsBaseController::watchdogOK()
{
    char inputBuff[10] = "";
    const char* functionName = "watchdogOK";

    if (useWatchdog && (fwMinor >= 33)) {
        sendReceiveLock((char*) "#WS", (char*) inputBuff, sizeof(inputBuff));
        if ((inputBuff[0] == '=') && (inputBuff[1] != '0')) {
            errlogPrintf("%s:%s:%s: *** CAUTION watchdog not running, disabling card ***\n",
                    driverName, functionName, portName);
            enabled = false;
            return false;
        }
    }
    return true;
}

