/*
FILENAME...     drvANC150Asyn.cc
USAGE...        asyn motor driver support for attocube systems AG ANC150
                Piezo Step Controller.

Version:        $Revision: 1.7 $
Modified By:    $Author: sluiter $
Last Modified:  $Date: 2009-02-18 21:39:00 $

*/

/*
 *      Original Author: Ron Sluiter
 *      Date: 07/24/2008
 *
 *      Experimental Physics and Industrial Control System (EPICS)
 *
 *      Copyright 1991, the Regents of the University of California,
 *      and the University of Chicago Board of Governors.
 *
 *      This software was produced under  U.S. Government contracts:
 *      (W-7405-ENG-36) at the Los Alamos National Laboratory,
 *      and (W-31-109-ENG-38) at Argonne National Laboratory.
 *
 *      Initial development by:
 *            The Controls and Automation Group (AT-8)
 *            Ground Test Accelerator
 *            Accelerator Technology Division
 *            Los Alamos National Laboratory
 *
 *      Co-developed with
 *            The Controls and Computing Group
 *            Accelerator Systems Division
 *            Advanced Photon Source
 *            Argonne National Laboratory
 *
 * Modification Log:
 * -----------------
 * .01 11-09-07 rls copied from drvMM4000Asyn.cc
 * .02 10-29-08 rls - added frequency and step mode to polling data.
 *                  - support for CNEN field.
 *                  - simulate trajectory positon so RDBL link updates.
 *                  - allow zero moves so motor record does not get stuck.
 * .03 11-19-08 rls - change input EOS to prompt ">".
 *                  - support enable/disable "torque".
 *                  - zero move bug fix.
 *                  - set firmwareVersion.
 * .04 02-18-09 rls Copied Matthew Pearson's (Diamond) fix on XPS for;
 *                  - idle polling interfering with setting position.
 *                  - auto save/restore not working.
 *
 */


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

#include "epicsThread.h"
#include "epicsExport.h"
#include "epicsEvent.h"
#include "epicsMutex.h"
#include "epicsTime.h"
#include "epicsTimer.h"
#include "epicsString.h"
#include "iocsh.h"

#include "drvSup.h"
#include "asynOctetSyncIO.h"
#define DEFINE_MOTOR_PROTOTYPES 1
#include "motor_interface.h"
#include "paramLib.h"

/* End-of-string defines */
#define ANC150_OUT_EOS   "\r\n" /* Command */
#define ANC150_IN_EOS    "> "   /* Reply   */

#define NINT(f)	(long)((f)>0 ? (f)+0.5 : (f)-0.5)	/* Nearest integer. */

motorAxisDrvSET_t motorANC150 =
{
    14,
    motorAxisReport,        /**< Standard EPICS driver report function (optional) */
    motorAxisInit,          /**< Standard EPICS dirver initialisation function (optional) */
    motorAxisSetLog,        /**< Defines an external logging function (optional) */
    motorAxisOpen,          /**< Driver open function */
    motorAxisClose,         /**< Driver close function */
    motorAxisSetCallback,   /**< Provides a callback function the driver can call when the status updates */
    motorAxisSetDouble,     /**< Pointer to function to set a double value */
    motorAxisSetInteger,    /**< Pointer to function to set an integer value */
    motorAxisGetDouble,     /**< Pointer to function to get a double value */
    motorAxisGetInteger,    /**< Pointer to function to get an integer value */
    motorAxisHome,          /**< Pointer to function to execute a more to reference or home */
    motorAxisMove,          /**< Pointer to function to execute a position move */
    motorAxisVelocityMove,  /**< Pointer to function to execute a velocity mode move */
    motorAxisStop,          /**< Pointer to function to stop motion */
    motorAxisforceCallback  /**< Pointer to function to request a poller status update */
};

extern "C" {epicsExportAddress(drvet, motorANC150);}

typedef struct
{
    asynUser *pasynUser;
    int numAxes;
    char firmwareVersion[100];
    double movingPollPeriod;
    double idlePollPeriod;
    epicsEventId pollEventId;
    AXIS_HDL pAxis;             /* array of axes */
} ANC150Controller;

typedef struct motorAxisHandle
{
    ANC150Controller *pController;
    PARAMS params;
    double targetPosition;
    double currentPosition;
    double highLimit;
    double lowLimit;
    double homePreset;
    int axisStatus;
    int card;
    int axis;
    int maxDigits;
    motorAxisLogFunc print;
    void *logParam;
    bool moving_ind;        /* Moving indicator. */
    epicsMutexId mutexId;
    epicsTime *movetimer;   /* Moving timer. */
    double moveinterval;    /* Moving delta time (sec). */
    int frequency;
} motorAxis;

typedef struct
{
    AXIS_HDL pFirst;
    epicsThreadId motorThread;
    motorAxisLogFunc print;
    void *logParam;
    epicsTimeStamp now;
} motorANC150_t;

extern "C" {
static int motorANC150LogMsg(void *, const motorAxisLogMask_t, const char *, ...);
}
static int sendOnly(ANC150Controller *, char *);
static asynStatus sendAndReceive(ANC150Controller *, char *, char *, int);
static asynStatus getFreq(ANC150Controller *, int);
static bool stpMode(ANC150Controller *, int);

#define PRINT   (drv.print)
#define FLOW    motorAxisTraceFlow
#define IODRIVER  motorAxisTraceIODriver

#define ANC150_MAX_AXES 6
#define BUFFER_SIZE 100         /* Size of input and output buffers */
#define TIMEOUT 2.0             /* Timeout for I/O in seconds */

#define ANC150_HOME       0x20  /* Home LS. */
#define ANC150_LOW_LIMIT  0x10  /* Minus Travel Limit. */
#define ANC150_HIGH_LIMIT 0x08  /* Plus Travel Limit. */
#define ANC150_DIRECTION  0x04  /* Motor direction: 0 - minus; 1 - plus. */
#define ANC150_POWER_ON   0x02  /* Motor power 0 - ON; 1 - OFF. */
#define ANC150_MOVING     0x01  /* In-motion indicator. */


#define TCP_TIMEOUT 2.0
static motorANC150_t drv = {NULL, NULL, motorANC150LogMsg, 0, {0, 0}};
static int numANC150Controllers;
/* Pointer to array of controller strutures */
static ANC150Controller *pANC150Controller = NULL;

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

static void motorAxisReportAxis(AXIS_HDL pAxis, int level)
{
    if (level > 0)
    {
        printf("Axis %d\n", pAxis->axis);
        printf("   axisStatus:  0x%x\n", pAxis->axisStatus);
        printf("   high limit:  %f\n", pAxis->highLimit);
        printf("   low limit:   %f\n", pAxis->lowLimit);
        printf("   home preset: %f\n", pAxis->homePreset);
        printf("   max digits:  %d\n", pAxis->maxDigits);
    }
}

static void motorAxisReport(int level)
{
    int i, j;

    for (i = 0; i < numANC150Controllers; i++)
    {
        printf("Controller %d firmware version: %s\n", i, pANC150Controller[i].firmwareVersion);
        if (level)
        {
            printf("    model: attocube ANC 150\n");
            printf("    moving poll period: %f\n", pANC150Controller[i].movingPollPeriod);
            printf("    idle poll period: %f\n", pANC150Controller[i].idlePollPeriod);
            printf("Controller %d firmware version: %s\n", i, pANC150Controller[i].firmwareVersion);
        }
        for (j = 0; j < pANC150Controller[i].numAxes; j++)
        {
            motorAxisReportAxis(&pANC150Controller[i].pAxis[j], level);
        }
    }
}


static int motorAxisInit(void)
{
    return(MOTOR_AXIS_OK);
}

static int motorAxisSetLog(AXIS_HDL pAxis, motorAxisLogFunc logFunc, void *param)
{
    if (pAxis == NULL)
    {
        if (logFunc == NULL)
        {
            drv.print = motorANC150LogMsg;
            drv.logParam = NULL;
        }
        else
        {
            drv.print = logFunc;
            drv.logParam = param;
        }
    }
    else
    {
        if (logFunc == NULL)
        {
            pAxis->print = motorANC150LogMsg;
            pAxis->logParam = NULL;
        }
        else
        {
            pAxis->print = logFunc;
            pAxis->logParam = param;
        }
    }
    return(MOTOR_AXIS_OK);
}

static AXIS_HDL motorAxisOpen(int card, int axis, char *param)
{
    AXIS_HDL pAxis;

    if (card > numANC150Controllers)
        return(NULL);
    if (axis > pANC150Controller[card].numAxes)
        return(NULL);
    pAxis = &pANC150Controller[card].pAxis[axis];
    return(pAxis);
}

static int motorAxisClose(AXIS_HDL pAxis)
{
    return(MOTOR_AXIS_OK);
}

static int motorAxisGetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int *value)
{
    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);
    else
    {
        return(motorParam->getInteger(pAxis->params, (paramIndex) function, value));
    }
}

static int motorAxisGetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double *value)
{
    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);
    else
    {
        return(motorParam->getDouble(pAxis->params, (paramIndex) function, value));
    }
}

static int motorAxisSetCallback(AXIS_HDL pAxis, motorAxisCallbackFunc callback, void *param)
{
    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);
    else
    {
        return(motorParam->setCallback(pAxis->params, callback, param));
    }
}

static int motorAxisSetDouble(AXIS_HDL pAxis, motorAxisParam_t function, double value)
{
    int ret_status = MOTOR_AXIS_ERROR;
    int status;

    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);
    else
    {
        epicsMutexLock(pAxis->mutexId);
        switch (function)
        {
        case motorAxisPosition:
        {
            pAxis->currentPosition = pAxis->targetPosition = value;
            break;
        }
        case motorAxisEncoderRatio:
        {
            PRINT(pAxis->logParam, motorAxisTraceError,
                  "motorAxisSetDouble: ANC150 does not support setting encoder ratio\n");
            break;
        }
        case motorAxisResolution:
        {
            PRINT(pAxis->logParam, motorAxisTraceError,
                  "motorAxisSetDouble: ANC150 does not support setting resolution\n");
            break;
        }
        case motorAxisLowLimit:
        case motorAxisHighLimit:
            break;
        case motorAxisPGain:
        {
            PRINT(pAxis->logParam, motorAxisTraceError,
                  "ANC150 does not support setting proportional gain\n");
            break;
        }
        case motorAxisIGain:
        {
            PRINT(pAxis->logParam, motorAxisTraceError,
                  "ANC150 does not support setting integral gain\n");
            break;
        }
        case motorAxisDGain:
        {
            PRINT(pAxis->logParam, motorAxisTraceError,
                  "ANC150 does not support setting derivative gain\n");
            break;
        }
        default:
            PRINT(pAxis->logParam, motorAxisTraceError,
                  "motorAxisSetDouble: unknown function %d\n", function);
            break;
        }
        if (ret_status == MOTOR_AXIS_OK )
        {
            motorParam->setDouble(pAxis->params, function, value);
            motorParam->callCallback(pAxis->params);
        }
        epicsMutexUnlock(pAxis->mutexId);
    }
    return(ret_status);
}

static int motorAxisSetInteger(AXIS_HDL pAxis, motorAxisParam_t function, int value)
{
    int ret_status = MOTOR_AXIS_ERROR;
    int status;
    char buff[20];

    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);

    switch (function)
    {
    case motorAxisClosedLoop:
        if (value == 0.0)
            sprintf(buff, "setm %d gnd", pAxis->axis + 1);
        else
            sprintf(buff, "setm %d stp", pAxis->axis + 1);

        ret_status = sendOnly(pAxis->pController, buff);
        break;

    default:
        PRINT(pAxis->logParam, motorAxisTraceError,
              "motorAxisSetInteger: unknown function %d\n", function);
        break;
    }
    if (ret_status != MOTOR_AXIS_ERROR)
        status = motorParam->setInteger(pAxis->params, function, value);
    return(ret_status);
}


static int motorAxisMove(AXIS_HDL pAxis, double position, int relative,
                         double min_velocity, double max_velocity, double acceleration)
{
    int status;
    long imove;
    char buff[100];
    const char *moveCommand;
    bool posdir;
    double fmove, ffrequency;

    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);

    PRINT(pAxis->logParam, FLOW,
          "Set card %d, axis %d move to %f, min vel=%f, max_vel=%f, accel=%f\n",
          pAxis->card, pAxis->axis, position, min_velocity, max_velocity, acceleration);

    if (relative)
    {
        if (position >= 0.0)
            posdir = true;
        else
            posdir = false;
        pAxis->targetPosition += position;
        imove = NINT(position);
    }
    else
    {
        imove = NINT(position - pAxis->currentPosition);
        if (imove >= 0)
            posdir = true;
        else
            posdir = false;
        pAxis->targetPosition = position;
    }
    if (posdir == true)
        moveCommand = "stepu";
    else
        moveCommand = "stepd";

    pAxis->moving_ind = true;
    imove = abs(imove);

    fmove = (double) imove;
    ffrequency = (double) pAxis->frequency;
    pAxis->moveinterval = fmove / ffrequency;
    if (pAxis->moveinterval <= 0.0)
        pAxis->moveinterval = epicsThreadSleepQuantum();
    *pAxis->movetimer = epicsTime::getCurrent() + pAxis->moveinterval;

    sprintf(buff, "%s %d %ld", moveCommand, pAxis->axis + 1, imove);
    status = sendOnly(pAxis->pController, buff);
    if (status)
        return(MOTOR_AXIS_ERROR);

    /* Set direction indicator. */
    motorParam->setInteger(pAxis->params, motorAxisDirection, posdir);

    /* Send a signal to the poller task which will make it do a poll, and
       switch to the moving poll rate. */
    epicsEventSignal(pAxis->pController->pollEventId);

    return(MOTOR_AXIS_OK);
}

static int motorAxisHome(AXIS_HDL pAxis, double min_velocity,
                         double max_velocity, double acceleration, int forwards)
{
    return(MOTOR_AXIS_ERROR);
}


static int motorAxisVelocityMove(AXIS_HDL pAxis, double min_velocity,
                                 double velocity, double acceleration)
{
    int status;

    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);

    /*
     * ANC150 does not have a jog command.  Simulate with move absolute to the
     * appropriate software limit. We can move to ANC150 soft limits. If the
     * record soft limits are set tighter than the ANC150 limits the record
     * will prevent JOG motion beyond its soft limits
     */
    if (velocity > 0.)
        status = motorAxisMove(pAxis, pAxis->highLimit, 0, min_velocity,
                               velocity, acceleration);
    else
        status = motorAxisMove(pAxis, pAxis->lowLimit, 0, min_velocity,
                               -velocity, acceleration);

    return(status);
}

static int motorAxisProfileMove(AXIS_HDL pAxis, int npoints, double positions[],
                                double times[], int relative, int trigger)
{
    return(MOTOR_AXIS_ERROR);
}

static int motorAxisTriggerProfile(AXIS_HDL pAxis)
{
    return(MOTOR_AXIS_ERROR);
}

static int motorAxisStop(AXIS_HDL pAxis, double acceleration)
{
    int status;
    char buff[100];

    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);

    PRINT(pAxis->logParam, FLOW, "Set card %d, axis %d to stop with accel=%f\n",
          pAxis->card, pAxis->axis, acceleration);

    sprintf(buff, "stop %d", pAxis->axis + 1);
    status = sendOnly(pAxis->pController, buff);
    if (status)
        return(MOTOR_AXIS_ERROR);
    
    /* Reset timer; indicating move is done. */
    *pAxis->movetimer = epicsTime::getCurrent();

    return(MOTOR_AXIS_OK);
}

static int motorAxisforceCallback(AXIS_HDL pAxis)
{
    if (pAxis == NULL)
        return(MOTOR_AXIS_ERROR);

    PRINT(pAxis->logParam, FLOW,
          "motorAxisforceCallback: request card %d, axis %d status update\n",
          pAxis->card, pAxis->axis);

    /* Force a status update. */
    motorParam->forceCallback(pAxis->params);

    /* Send a signal to the poller task which will make it do a status update */
    epicsEventSignal(pAxis->pController->pollEventId);
    return(MOTOR_AXIS_OK);
}


static void ANC150Poller(ANC150Controller *pController)
{
    /* This is the task that polls the ANC150 */
    double timeout;
    AXIS_HDL pAxis;
    asynStatus astatus;
    int status;
    int itera;
    int axisDone;
    int anyMoving;
    int forcedFastPolls = 0;

    timeout = pController->idlePollPeriod;
    epicsEventSignal(pController->pollEventId); /* Force on poll at startup */

    while (1)
    {
        if (timeout != 0.)
            status = epicsEventWaitWithTimeout(pController->pollEventId, timeout);
        else
            status = epicsEventWait(pController->pollEventId);

        if (status == epicsEventWaitOK)
        {
            /*
             * We got an event, rather than a timeout.  This is because other
             * software knows that an axis should have changed state (started
             * moving, etc.). Force a minimum number of fast polls, because the
             * controller status might not have changed the first few polls
             */
            forcedFastPolls = 0;
        }

        anyMoving = 0;
        for (itera = 0; itera < pController->numAxes; itera++)
        {
            double slewposition, proportion;

            pAxis = &pController->pAxis[itera];
            if (!pAxis->mutexId)
                break;
            epicsMutexLock(pAxis->mutexId);
            
            if (pAxis->moving_ind == true)
            {
                double time_remain = *pAxis->movetimer - epicsTime::getCurrent();
                double delta = pAxis->targetPosition - pAxis->currentPosition;

                axisDone = 0;
                anyMoving = 1;
                if (time_remain < 0.0)
                {
                    pAxis->moving_ind = false;
                    slewposition = pAxis->currentPosition = pAxis->targetPosition;
                }
                else
                {
                    proportion = 1.0 - (time_remain / pAxis->moveinterval);
                    slewposition = pAxis->currentPosition + (delta * proportion);
                }
            }
            else
            {
                axisDone = 1;
                slewposition = pAxis->currentPosition = pAxis->targetPosition;
            }

            motorParam->setInteger(pAxis->params, motorAxisDone, axisDone);
            if (pAxis->axisStatus & ANC150_HOME)
                motorParam->setInteger(pAxis->params, motorAxisHomeSignal, 1);
            else
                motorParam->setInteger(pAxis->params, motorAxisHomeSignal, 0);
            if (pAxis->axisStatus & ANC150_HIGH_LIMIT)
                motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, 1);
            else
                motorParam->setInteger(pAxis->params, motorAxisHighHardLimit, 0);
            if (pAxis->axisStatus & ANC150_LOW_LIMIT)
                motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, 1);
            else
                motorParam->setInteger(pAxis->params, motorAxisLowHardLimit, 0);

            motorParam->setDouble(pAxis->params, motorAxisPosition, slewposition);
            motorParam->setDouble(pAxis->params, motorAxisEncoderPosn, slewposition);
            PRINT(pAxis->logParam, IODRIVER, "ANC150Poller: axis %d axisStatus=%x, position=%f\n",
                  pAxis->axis, pAxis->axisStatus, slewposition);

            /* Update frequency. */
            astatus = getFreq(pController, itera);
            if (astatus == asynSuccess)
                motorParam->setInteger(pAxis->params, motorAxisCommError, 0);
            else
                motorParam->setInteger(pAxis->params, motorAxisCommError, 1);

            /* Check power on. */
            if (stpMode(pController, itera))
                motorParam->setInteger(pAxis->params, motorAxisPowerOn, 1);
            else
                motorParam->setInteger(pAxis->params, motorAxisPowerOn, 0);

            motorParam->callCallback(pAxis->params);
            epicsMutexUnlock(pAxis->mutexId);

        }           /* Next axis */

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

static int motorANC150LogMsg(void *param, const motorAxisLogMask_t mask,
                             const char *pFormat,...)
{

    va_list pvar;
    int nchar;

    va_start(pvar, pFormat);
    nchar = vfprintf(stdout, pFormat, pvar);
    va_end(pvar);
    printf("\n");
    return(nchar);
}


int ANC150AsynSetup(int num_controllers)    /* number of ANC150 controllers in system.  */
{

    if (num_controllers < 1)
    {
        printf("ANC150Setup, num_controllers must be > 0\n");
        return(MOTOR_AXIS_ERROR);
    }
    numANC150Controllers = num_controllers;
    pANC150Controller = (ANC150Controller *) calloc(numANC150Controllers,
                                                    sizeof(ANC150Controller));
    return(MOTOR_AXIS_OK);
}


int ANC150AsynConfig(int card,              /* Controller number */
                     const char *portName,  /* asyn serial port name */
                     int numAxes,           /* Number of axes this controller supports */
                     int movingPollPeriod,  /* Time to poll (msec) when an axis is in motion */
                     int idlePollPeriod)    /* Time to poll (msec) when an axis is idle. 0 for no polling */

{
    AXIS_HDL pAxis;
    int axis;
    ANC150Controller *pController;
    char threadName[20];
    int status;
    int retry = 0;
    char inputBuff[BUFFER_SIZE];
    char outputBuff[BUFFER_SIZE];

    if (numANC150Controllers < 1)
    {
        printf("ANC150Config: no ANC150 controllers allocated, call ANC150Setup first\n");
        return(MOTOR_AXIS_ERROR);
    }
    if ((card < 0) || (card >= numANC150Controllers))
    {
        printf("ANC150Config: card must in range 0 to %d\n", numANC150Controllers - 1);
        return(MOTOR_AXIS_ERROR);
    }
    if ((numAxes < 1) || (numAxes > ANC150_MAX_AXES))
    {
        printf("ANC150Config: numAxes must in range 1 to %d\n", ANC150_MAX_AXES);
        return(MOTOR_AXIS_ERROR);
    }

    pController = &pANC150Controller[card];
    pController->numAxes = numAxes;
    pController->movingPollPeriod = movingPollPeriod / 1000.;
    pController->idlePollPeriod = idlePollPeriod / 1000.;

    status = pasynOctetSyncIO->connect(portName, 0, &pController->pasynUser, NULL);

    if (status != asynSuccess)
    {
        printf("ANC150AsynConfig: cannot connect to asyn port %s\n", portName);
        return(MOTOR_AXIS_ERROR);
    }

    /* Set command End-of-string */
    pasynOctetSyncIO->setInputEos(pController->pasynUser,  ANC150_IN_EOS,  strlen(ANC150_IN_EOS));
    pasynOctetSyncIO->setOutputEos(pController->pasynUser, ANC150_OUT_EOS, strlen(ANC150_OUT_EOS));

    do
    {
        pasynOctetSyncIO->flush(pController->pasynUser);
        status = sendAndReceive(pController, (char *) "ver", inputBuff, sizeof(inputBuff));
        if (status == asynSuccess && strncmp(inputBuff, "attocube", 8) == 0)
            strncpy(pController->firmwareVersion, &inputBuff[19], sizeof(inputBuff));
        else
            status = asynError;
        retry++;
        /* Return value is length of response string */
    } while (status != asynSuccess && retry < 3);

    if (status != asynSuccess)
        return(MOTOR_AXIS_ERROR);


    /* Don't initialize pAxis until all the error checks have passed;
     * prevents drvAsynMotorConfigure from crashing. */
    pController->pAxis = (AXIS_HDL) calloc(numAxes, sizeof(motorAxis));

    for (axis = 0; axis < numAxes; axis++)
    {
        pAxis = &pController->pAxis[axis];
        pAxis->pController = pController;
        pAxis->card = card;
        pAxis->axis = axis;
        pAxis->mutexId = epicsMutexMustCreate();
        pAxis->params = motorParam->create(0, MOTOR_AXIS_NUM_PARAMS);
        motorParam->setInteger(pAxis->params, motorAxisClosedLoop, 1);
        /* Set motorAxisHasClosedLoop on so the CNEN field works. */
        motorParam->setInteger(pAxis->params, motorAxisHasClosedLoop, 1);
        pAxis->currentPosition = 0.0;
        pAxis->movetimer = new epicsTime();
        pAxis->moving_ind = false;
        getFreq(pController, axis);
        sprintf(outputBuff, "setm %d stp", pAxis->axis + 1);
        status = sendOnly(pAxis->pController, outputBuff);
    }

    pController->pollEventId = epicsEventMustCreate(epicsEventEmpty);

    /* Create the poller thread for this controller */
    epicsSnprintf(threadName, sizeof(threadName), "ANC150:%d", card);
    epicsThreadCreate(threadName,
                      epicsThreadPriorityMedium,
                      epicsThreadGetStackSize(epicsThreadStackMedium),
                      (EPICSTHREADFUNC) ANC150Poller, (void *) pController);

    return(MOTOR_AXIS_OK);
}

static int sendOnly(ANC150Controller * pController, char *outputBuff)
{
    char inputBuff[BUFFER_SIZE];
    int nRequested = strlen(outputBuff);
    size_t nActual, nRead;
    asynStatus status;
    int eomReason;

    status = pasynOctetSyncIO->writeRead(pController->pasynUser, outputBuff, nRequested,
                                         inputBuff, sizeof(inputBuff), TIMEOUT, &nActual,
                                         &nRead, &eomReason);
    
    if (nActual != (size_t) nRequested)
        status = asynError;

    if (status != asynSuccess)
    {
        asynPrint(pController->pasynUser, ASYN_TRACE_ERROR,
                  "drvANC150Asyn:sendOnly: error sending command %d, sent=%d, status=%d\n",
                  outputBuff, nActual, status);
    }

    return(status);
}


static asynStatus sendAndReceive(ANC150Controller *pController, char *outputBuff,
                                 char *inputBuff, int inputSize)
{
    int nWriteRequested = strlen(outputBuff);
    size_t nWrite, nRead;
    int eomReason;
    asynStatus status;
    char localbuf[BUFFER_SIZE];

    status = pasynOctetSyncIO->writeRead(pController->pasynUser, outputBuff,
                                         nWriteRequested, localbuf, BUFFER_SIZE,
                                         TIMEOUT, &nWrite, &nRead, &eomReason);

    if (status == asynSuccess)
    {
        if (nWrite != (size_t) nWriteRequested ||
            strncmp(outputBuff, localbuf, nWrite) != 0)
            status = asynError;
    }

    if (status == asynSuccess)
    {
        char *echo, *reply, *ack;

        if ((echo = strstr(localbuf, "\r\n")) == NULL)
            status = asynError;
        else if ((reply = strstr(&echo[2], "\r\n")) == NULL)
            status = asynError;
        else if ((ack = strstr(&reply[2], "\r\n")) == NULL)
            status = asynError;
        else
        {
            *reply = 0;        /* Terminate reply at \r. */
            strcpy(inputBuff, &echo[2]);
        }
    }

    if (status != asynSuccess)
    {
        asynPrint(pController->pasynUser, ASYN_TRACE_ERROR,
                  "drvANC150Asyn:sendAndReceive error calling writeRead, output=%s status=%d, error=%s\n",
                  outputBuff, status, pController->pasynUser->errorMessage);
    }

    return(status);
}

        
static asynStatus getFreq(ANC150Controller *pController, int axis)
{
    asynStatus status;
    char inputBuff[BUFFER_SIZE];
    char outputBuff[BUFFER_SIZE];
    AXIS_HDL pAxis;
    int savedfreq;

    sprintf(outputBuff, "getf %d", axis + 1);
    status = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff));

    if (status != asynSuccess)
        return(status);
    else if (strncmp(inputBuff, "Axis not in computer control mode", 34) == 0)
        return(status);
    else
    {
        pAxis = pController->pAxis;
        savedfreq = pAxis->frequency;
        if (sscanf(inputBuff, "frequency = %d", &pAxis->frequency) != 1)
        {
            pAxis->frequency = savedfreq;
            pasynOctetSyncIO->flush(pController->pasynUser);
            return(asynError);
        }
        return(asynSuccess);
    }
}

        
static bool stpMode(ANC150Controller *pController, int axis)
{
    asynStatus status;
    char inputBuff[BUFFER_SIZE];
    char outputBuff[BUFFER_SIZE];
    size_t nRead;
    int eomReason;
    bool rtnstatus;

    sprintf(outputBuff, "getm %d", axis + 1);
    status = sendAndReceive(pController, outputBuff, inputBuff, sizeof(inputBuff));

    if (strncmp(inputBuff, "mode = stp", 11) == 0)
        rtnstatus = true;
    else if (strncmp(inputBuff, "mode = gnd", 11) == 0)
        rtnstatus = false;
    else if (strncmp(inputBuff, "Axis not in computer control mode", 34) == 0)
    {
        /* Eat the ERROR msg. */
        status = pasynOctetSyncIO->read(pController->pasynUser, inputBuff,
                                        sizeof(inputBuff), TIMEOUT, &nRead, &eomReason);
        rtnstatus = false;
    }
    else
    {
        pasynOctetSyncIO->flush(pController->pasynUser);
        rtnstatus = true;
    }
    return(rtnstatus);
}


extern "C"
{

// Setup arguments
    static const iocshArg setupArg0 = {"Maximum # of controllers", iocshArgInt};
// Config arguments
    static const iocshArg configArg0 = {"Card# being configured", iocshArgInt};
    static const iocshArg configArg1 = {"asyn port name", iocshArgString};
    static const iocshArg configArg2 = {"Number of Axes", iocshArgInt};
    static const iocshArg configArg3 = {"Moving poll rate", iocshArgInt};
    static const iocshArg configArg4 = {"Idle poll rate", iocshArgInt};

    static const iocshArg *const SetupArgs[1]  = {&setupArg0};
    static const iocshArg *const ConfigArgs[5] = {&configArg0, &configArg1, &configArg2,
                                                    &configArg3, &configArg4};

    static const iocshFuncDef setupANC150  = {"ANC150AsynSetup",  1, SetupArgs};
    static const iocshFuncDef configANC150 = {"ANC150AsynConfig", 5, ConfigArgs};

    static void setupANC150CallFunc(const iocshArgBuf *args)
    {
        ANC150AsynSetup(args[0].ival);
    }
    static void configANC150CallFunc (const iocshArgBuf *args)
    {
        ANC150AsynConfig(args[0].ival, args[1].sval, args[2].ival, args[3].ival, args[4].ival);
    }

    static void ANC150Register(void)
    {
        iocshRegister(&setupANC150, setupANC150CallFunc);
        iocshRegister(&configANC150, configANC150CallFunc);
    }

    epicsExportRegistrar(ANC150Register);

} // extern "C"


