/********************************************
*  pmacController.cpp
* 
*  PMAC Asyn motor based on the 
*  asynMotorController class.
* 
*  Matthew Pearson
*  23 May 2012
* 
* 
* Substantially modified for use at SINQ at PSI. 
* The thing with the PMACS is that they can be programmed.
* This affects also the commands they understand. 
*
* Mark Koennecke, February 2013 
*
* Updated to use the MsgTxt field for error messages as 
* used at ESS and SINQ 
*
* Mark Koennecke, January 2019
********************************************/



#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <errno.h>
#include <iostream>
#include <unistd.h>
using std::cout;
using std::endl;

#include "epicsTime.h"
#include <epicsThread.h>
#include <epicsExport.h>
#include <epicsString.h>
#include <iocsh.h>
#include <drvSup.h>
#include <registryFunction.h>
#include <errlog.h>

#include "asynOctetSyncIO.h"

#include "pmacController.h"
#include "pmacAxis.h"

#define MULT 1000.

static const char *driverName = "pmacController";

const epicsUInt32 pmacController::PMAC_MAXBUF_ = 1024;
const epicsFloat64 pmacController::PMAC_TIMEOUT_ = 5.0;

const epicsUInt32 pmacController::PMAC_STATUS1_MAXRAPID_SPEED    = (0x1<<0);
const epicsUInt32 pmacController::PMAC_STATUS1_ALT_CMNDOUT_MODE  = (0x1<<1);
const epicsUInt32 pmacController::PMAC_STATUS1_SOFT_POS_CAPTURE  = (0x1<<2);
const epicsUInt32 pmacController::PMAC_STATUS1_ERROR_TRIGGER     = (0x1<<3);
const epicsUInt32 pmacController::PMAC_STATUS1_FOLLOW_ENABLE     = (0x1<<4);
const epicsUInt32 pmacController::PMAC_STATUS1_FOLLOW_OFFSET     = (0x1<<5);
const epicsUInt32 pmacController::PMAC_STATUS1_PHASED_MOTOR      = (0x1<<6);
const epicsUInt32 pmacController::PMAC_STATUS1_ALT_SRC_DEST      = (0x1<<7);
const epicsUInt32 pmacController::PMAC_STATUS1_USER_SERVO        = (0x1<<8);
const epicsUInt32 pmacController::PMAC_STATUS1_USER_PHASE        = (0x1<<9);
const epicsUInt32 pmacController::PMAC_STATUS1_HOMING            = (0x1<<10);
const epicsUInt32 pmacController::PMAC_STATUS1_BLOCK_REQUEST     = (0x1<<11);
const epicsUInt32 pmacController::PMAC_STATUS1_DECEL_ABORT       = (0x1<<12);
const epicsUInt32 pmacController::PMAC_STATUS1_DESIRED_VELOCITY_ZERO = (0x1<<13);
const epicsUInt32 pmacController::PMAC_STATUS1_DATABLKERR        = (0x1<<14);
const epicsUInt32 pmacController::PMAC_STATUS1_DWELL             = (0x1<<15);
const epicsUInt32 pmacController::PMAC_STATUS1_INTEGRATE_MODE    = (0x1<<16);
const epicsUInt32 pmacController::PMAC_STATUS1_MOVE_TIME_ON      = (0x1<<17);
const epicsUInt32 pmacController::PMAC_STATUS1_OPEN_LOOP         = (0x1<<18);
const epicsUInt32 pmacController::PMAC_STATUS1_AMP_ENABLED       = (0x1<<19);
const epicsUInt32 pmacController::PMAC_STATUS1_X_SERVO_ON        = (0x1<<20);
const epicsUInt32 pmacController::PMAC_STATUS1_POS_LIMIT_SET     = (0x1<<21);
const epicsUInt32 pmacController::PMAC_STATUS1_NEG_LIMIT_SET     = (0x1<<22);
const epicsUInt32 pmacController::PMAC_STATUS1_MOTOR_ON          = (0x1<<23);

const epicsUInt32 pmacController::PMAC_STATUS2_IN_POSITION       = (0x1<<0);
const epicsUInt32 pmacController::PMAC_STATUS2_WARN_FOLLOW_ERR   = (0x1<<1);
const epicsUInt32 pmacController::PMAC_STATUS2_ERR_FOLLOW_ERR    = (0x1<<2);
const epicsUInt32 pmacController::PMAC_STATUS2_AMP_FAULT         = (0x1<<3);
const epicsUInt32 pmacController::PMAC_STATUS2_NEG_BACKLASH      = (0x1<<4);
const epicsUInt32 pmacController::PMAC_STATUS2_I2T_AMP_FAULT     = (0x1<<5);
const epicsUInt32 pmacController::PMAC_STATUS2_I2_FOLLOW_ERR     = (0x1<<6);
const epicsUInt32 pmacController::PMAC_STATUS2_TRIGGER_MOVE      = (0x1<<7);
const epicsUInt32 pmacController::PMAC_STATUS2_PHASE_REF_ERR     = (0x1<<8);
const epicsUInt32 pmacController::PMAC_STATUS2_PHASE_SEARCH      = (0x1<<9);
const epicsUInt32 pmacController::PMAC_STATUS2_HOME_COMPLETE     = (0x1<<10);
const epicsUInt32 pmacController::PMAC_STATUS2_POS_LIMIT_STOP    = (0x1<<11);
const epicsUInt32 pmacController::PMAC_STATUS2_DESIRED_STOP      = (0x1<<12);
const epicsUInt32 pmacController::PMAC_STATUS2_FORE_IN_POS       = (0x1<<13);
const epicsUInt32 pmacController::PMAC_STATUS2_NA14              = (0x1<<14);
const epicsUInt32 pmacController::PMAC_STATUS2_ASSIGNED_CS       = (0x1<<15);

/*Global status ???*/
const epicsUInt32 pmacController::PMAC_GSTATUS_CARD_ADDR             = (0x1<<0);
const epicsUInt32 pmacController::PMAC_GSTATUS_ALL_CARD_ADDR         = (0x1<<1);
const epicsUInt32 pmacController::PMAC_GSTATUS_RESERVED              = (0x1<<2);
const epicsUInt32 pmacController::PMAC_GSTATUS_PHASE_CLK_MISS        = (0x1<<3);
const epicsUInt32 pmacController::PMAC_GSTATUS_MACRO_RING_ERRORCHECK = (0x1<<4);
const epicsUInt32 pmacController::PMAC_GSTATUS_MACRO_RING_COMMS      = (0x1<<5);
const epicsUInt32 pmacController::PMAC_GSTATUS_TWS_PARITY_ERROR      = (0x1<<6);
const epicsUInt32 pmacController::PMAC_GSTATUS_CONFIG_ERROR          = (0x1<<7);
const epicsUInt32 pmacController::PMAC_GSTATUS_ILLEGAL_LVAR          = (0x1<<8);
const epicsUInt32 pmacController::PMAC_GSTATUS_REALTIME_INTR         = (0x1<<9);
const epicsUInt32 pmacController::PMAC_GSTATUS_FLASH_ERROR           = (0x1<<10);
const epicsUInt32 pmacController::PMAC_GSTATUS_DPRAM_ERROR           = (0x1<<11);
const epicsUInt32 pmacController::PMAC_GSTATUS_CKSUM_ACTIVE          = (0x1<<12);
const epicsUInt32 pmacController::PMAC_GSTATUS_CKSUM_ERROR           = (0x1<<13);
const epicsUInt32 pmacController::PMAC_GSTATUS_LEADSCREW_COMP        = (0x1<<14);
const epicsUInt32 pmacController::PMAC_GSTATUS_WATCHDOG              = (0x1<<15);
const epicsUInt32 pmacController::PMAC_GSTATUS_SERVO_REQ             = (0x1<<16);
const epicsUInt32 pmacController::PMAC_GSTATUS_DATA_GATHER_START     = (0x1<<17);
const epicsUInt32 pmacController::PMAC_GSTATUS_RESERVED2             = (0x1<<18);
const epicsUInt32 pmacController::PMAC_GSTATUS_DATA_GATHER_ON        = (0x1<<19);
const epicsUInt32 pmacController::PMAC_GSTATUS_SERVO_ERROR           = (0x1<<20);
const epicsUInt32 pmacController::PMAC_GSTATUS_CPUTYPE               = (0x1<<21);
const epicsUInt32 pmacController::PMAC_GSTATUS_REALTIME_INTR_RE      = (0x1<<22);
const epicsUInt32 pmacController::PMAC_GSTATUS_RESERVED3             = (0x1<<23);

const epicsUInt32 pmacController::PMAC_HARDWARE_PROB = (PMAC_GSTATUS_MACRO_RING_ERRORCHECK | PMAC_GSTATUS_MACRO_RING_COMMS | PMAC_GSTATUS_REALTIME_INTR | PMAC_GSTATUS_FLASH_ERROR | PMAC_GSTATUS_DPRAM_ERROR | PMAC_GSTATUS_CKSUM_ERROR | PMAC_GSTATUS_WATCHDOG | PMAC_GSTATUS_SERVO_ERROR);

const epicsUInt32 pmacController::PMAX_AXIS_GENERAL_PROB1 = 0;
const epicsUInt32 pmacController::PMAX_AXIS_GENERAL_PROB2 = (PMAC_STATUS2_DESIRED_STOP | PMAC_STATUS2_AMP_FAULT);


//C function prototypes, for the functions that can be called on IOC shell
extern "C" {
asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
				 int numAxes, int movingPollPeriod, int idlePollPeriod);
asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
				 int numAxes, int movingPollPeriod, int idlePollPeriod);
asynStatus pmacV3CreateController(const char *portName,
			  const char *lowLevelPortName,
			  int lowLevelPortAddress, int numAxes,
			  int movingPollPeriod, int idlePollPeriod);
asynStatus pmacCreateAxis(const char *pmacName, int axis);
asynStatus pmacCreateAxis(const char *pmacName, int numAxis);

}

pmacController::pmacController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
		       int numAxes, double movingPollPeriod, double idlePollPeriod, const int& extraParams)
: SINQController(portName, lowLevelPortName, numAxes+1, extraParams)
{
static const char *functionName = "pmacController::pmacController";

//Initialize non static data members
lowLevelPortUser_ = NULL;
debugFlag_ = 0;

pAxes_ = (pmacAxis **)(asynMotorController::pAxes_);

// Create controller-specific parameters
createParam(PMAC_C_CommsErrorString,         asynParamInt32, &PMAC_C_CommsError_);

// Connect our Asyn user to the low level port that is a parameter to this constructor
if (lowLevelPortConnect(lowLevelPortName, lowLevelPortAddress, &lowLevelPortUser_, "\006", (char *)"\r") != asynSuccess) {
printf("%s: Failed to connect to low level asynOctetSyncIO port %s\n", functionName, lowLevelPortName);
setIntegerParam(PMAC_C_CommsError_, 1);
} else {
/* Create the poller thread for this controller
* NOTE: at this point the axis objects don't yet exist, but the poller tolerates this */
setIntegerParam(PMAC_C_CommsError_, 0);
}
startPoller(movingPollPeriod, idlePollPeriod, 10);

callParamCallbacks();

}


pmacController::~pmacController(void) 
{
//Destructor
}


/**
* Connect to the underlying low level Asyn port that is used for comms.
* This uses the asynOctetSyncIO interface, and also sets the input and output terminators.
*/
int pmacController::lowLevelPortConnect(const char *port, int addr, asynUser **ppasynUser, char *inputEos, char *outputEos)
{
asynStatus status = asynSuccess;

static const char *functionName = "pmacController::lowLevelPortConnect";

debugFlow(functionName);

status = pasynOctetSyncIO->connect( port, addr, ppasynUser, NULL);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
      "pmacController::motorAxisAsynConnect: unable to connect to port %s\n", 
      port);
return status;
}

//Do I want to disconnect below? If the IP address comes up, will the driver recover
//if the poller functions are running? Might have to use asynManager->isConnected to
//test connection status of low level port (in the pollers). But then autosave 
//restore doesn't work (and we would save wrong positions). So I need to 
//have a seperate function(s) to deal with connecting after IOC init.

status = pasynOctetSyncIO->setInputEos(*ppasynUser, inputEos, strlen(inputEos) );
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
      "pmacController: unable to set input EOS on %s: %s\n", 
      port, (*ppasynUser)->errorMessage);
pasynOctetSyncIO->disconnect(*ppasynUser);
//Set my low level pasynUser pointer to NULL
*ppasynUser = NULL;
return status;
}

status = pasynOctetSyncIO->setOutputEos(*ppasynUser, outputEos, strlen(outputEos));
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
      "pmacController: unable to set output EOS on %s: %s\n", 
      port, (*ppasynUser)->errorMessage);
pasynOctetSyncIO->disconnect(*ppasynUser);
//Set my low level pasynUser pointer to NULL
*ppasynUser = NULL;
return status;
}

return status;
}

/**
* Utilty function to print the connected status of the low level asyn port.
*/
asynStatus pmacController::printConnectedStatus()
{
asynStatus status = asynSuccess;
int asynManagerConnected = 0;

if (lowLevelPortUser_) {
status = pasynManager->isConnected(lowLevelPortUser_, &asynManagerConnected);
if (status) {
asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
      "pmacController: Error calling pasynManager::isConnected.\n");
return asynError;
} else {
printf("pmacController::printConnectedStatus: isConnected: %d\n", asynManagerConnected);
}
}
return asynSuccess;
}

/**
* Wrapper for asynOctetSyncIO write/read functions.
* @param command - String command to send.
* @response response - String response back.
*/
asynStatus pmacController::lowLevelWriteRead(int axisNo, const char *command, char *response)
{
asynStatus status = asynSuccess;

int eomReason;
size_t nwrite = 0;
size_t nread = 0;
int commsError = 0;
static const char *functionName = "pmacController::lowLevelWriteRead";
pmacAxis *axis = getAxis(axisNo);

debugFlow(functionName);

if (!lowLevelPortUser_) {
setIntegerParam(this->motorStatusCommsError_, 1);
return asynError;
}

asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: command: %s\n", functionName, command);
debugFlow("Sending: ");
debugFlow(command);

//Make sure the low level port is connected before we attempt comms
//Use the controller-wide param PMAC_C_CommsError_
getIntegerParam(PMAC_C_CommsError_, &commsError);

if (!commsError) {
status = pasynOctetSyncIO->writeRead(lowLevelPortUser_ ,
				  command, strlen(command),
				  response, PMAC_MAXBUF_,
				  PMAC_TIMEOUT_,
				  &nwrite, &nread, &eomReason );

if (status) {
asynPrint(lowLevelPortUser_, ASYN_TRACE_ERROR, "%s: Error from pasynOctetSyncIO->writeRead. command: %s\n", functionName, command);
setIntegerParam(this->motorStatusCommsError_, 1);
if(axis!= NULL){
 axis->updateMsgTxtFromDriver("Lost connection to motor controller");
}
} else {
setIntegerParam(this->motorStatusCommsError_, 0);
}
}

usleep(20); // slow down communication somewhat 

asynPrint(lowLevelPortUser_, ASYN_TRACEIO_DRIVER, "%s: response: %s\n", functionName, response); 
debugFlow("Received: ");
debugFlow(response);

return status;
}

void pmacController::debugFlow(const char *message)
{
if (debugFlag_ == 1) {
printf("  %s\n", message);
}

asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, "%s\n", message);

}


void pmacController::report(FILE *fp, int level)
{
int axis = 0;
pmacAxis *pAxis = NULL;

fprintf(fp, "pmac motor driver %s, numAxes=%d, moving poll period=%f, idle poll period=%f\n", 
  this->portName, numAxes_, movingPollPeriod_, idlePollPeriod_);

if (level > 0) {
for (axis=0; axis<numAxes_; axis++) {
pAxis = getAxis(axis);
fprintf(fp, "  axis %d\n"
	  "    scale = %lf\n", 
      pAxis->axisNo_,
      pAxis->scale_);
}
}

// Call the base class method
asynMotorController::report(fp, level);
}


asynStatus pmacController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
pmacAxis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
char message[132];

static const char *functionName = "pmacController::writeFloat64";

sprintf(message,"%s, reason %d",  functionName, function);
debugFlow(message);
//debugFlow(functionName);

pAxis = this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}


/* Set the parameter and readback in the parameter library. */
status = pAxis->setDoubleParam(function, value);

// if (function == motorPosition_) {
//   /*Set position on motor axis.*/            
//   epicsInt32 position = (epicsInt32) floor(value*32/pAxis->scale_ + 0.5);

//   sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
// 	     pAxis->axisNo_,
// 	     pAxis->axisNo_, position, pAxis->axisNo_, 
// 	     pAxis->axisNo_, position, pAxis->axisNo_ );

//   asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, 
// 	      "%s: Set axis %d on controller %s to position %f\n", 
// 	      functionName, pAxis->axisNo_, portName, value);

//   if ( command[0] != 0 && status == asynSuccess) {
//     status = lowLevelWriteRead(command, response);
//   }
	
//   sprintf(command, "#%dJ/", pAxis->axisNo_);

//   if (command[0] != 0 && status == asynSuccess) {
//     status = lowLevelWriteRead(command, response);
//   }

//   /*Now set position on encoder axis, if one is in use.*/
	
//   if (pAxis->encoder_axis_) {
//     getDoubleParam(motorEncRatio_,  &encRatio);
//     encposition = (epicsInt32) floor((position*encRatio) + 0.5);
	  
//     sprintf(command, "#%dK M%d61=%d*I%d08 M%d62=%d*I%d08",
// 	      pAxis->encoder_axis_,
// 	      pAxis->encoder_axis_, encposition, pAxis->encoder_axis_, 
// 	      pAxis->encoder_axis_, encposition, pAxis->encoder_axis_ );
	  
//     asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW, 
// 		"%s: Set encoder axis %d on controller %s to position %f\n", 
// 		functionName, pAxis->axisNo_, portName, value);
	  
//     if (command[0] != 0 && status == asynSuccess) {
// 	status = lowLevelWriteRead(command, response);
//     }
	  
//     sprintf(command, "#%dJ/", pAxis->encoder_axis_);
//     //The lowLevelWriteRead will be done at the end of this function.
//   }

//   /*Now do an update, to get the new position from the controller.*/
//   bool moving = true;
//   pAxis->getAxisStatus(&moving);
// } else 
/*
if (function == motorLowLimit_) {
sprintf(command, "I%d14=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
      "%s: Setting low limit on controller %s, axis %d to %f\n",
      functionName, portName, pAxis->axisNo_, value);
} else if (function == motorHighLimit_) {
sprintf(command, "I%d13=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
      "%s: Setting high limit on controller %s, axis %d to %f\n",
      functionName, portName, pAxis->axisNo_, value);
} 

//Execute the command.
if (command[0] != 0 && status == asynSuccess) {
status = lowLevelWriteRead(pAxis->axisNo_,command, response);
}
*/

//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeFloat64(pasynUser, value);

return status;

}


asynStatus pmacController::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
pmacAxis *pAxis = NULL;
static const char *functionName = "pmacController::writeInt32";

debugFlow(functionName);

pAxis = this->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->setIntegerParam(function, value);


//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeInt32(pasynUser, value);

return status;

}

/** Returns a pointer to an pmacAxis object.
* Returns NULL if the axis number encoded in pasynUser is invalid.
* \param[in] pasynUser asynUser structure that encodes the axis index number. */
pmacAxis* pmacController::getAxis(asynUser *pasynUser)
{
int axisNo = 0;

getAddress(pasynUser, &axisNo);
return getAxis(axisNo);
}



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


/** Polls the controller, rather than individual axis.*/
asynStatus pmacController::poll()
{
static const char *functionName = "pmacController::poll";

debugFlow(functionName);

if (!lowLevelPortUser_) {
setIntegerParam(this->motorStatusCommsError_, 1);
return asynError;
}

asynMotorController::poll();

callParamCallbacks();

return asynSuccess;
}



/*************************************************************************************/
/** The following functions have C linkage, and can be called directly or from iocsh */

extern "C" {

/**
* C wrapper for the pmacController constructor.
* See pmacController::pmacController.
*
*/
asynStatus pmacCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
			int numAxes, int movingPollPeriod, int idlePollPeriod)
{

pmacController *ppmacController
= new pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
ppmacController = NULL;

return asynSuccess;
}

asynStatus SeleneCreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress, 
			int numAxes, int movingPollPeriod, int idlePollPeriod)
{

SeleneController *ppmacController
= new SeleneController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
ppmacController = NULL;

return asynSuccess;
}

asynStatus pmacV3CreateController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
			  int numAxes, int movingPollPeriod, int idlePollPeriod)
{

pmacController *ppmacController = new pmacV3Controller(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.);
ppmacController = NULL;

return asynSuccess;
}

/*---------------------------------------------------------------------------------------------------------------------*/
SeleneController::SeleneController(const char *portName, const char *lowLevelPortName, int lowLevelPortAddress,
			     int numAxes, double movingPollPeriod, double idlePollPeriod) : pmacController(portName,
													   lowLevelPortName,
													   lowLevelPortAddress,
													   numAxes,
													   movingPollPeriod,
													   idlePollPeriod, 3) {
static const char *functionName = "seleneController::seleneController";
createParam(MotorSetPositionString,                asynParamFloat64,         &setMotorPosition_);
callParamCallbacks();
}

pmacV3Controller::pmacV3Controller(const char *portName,
			     const char *lowLevelPortName,
			     int lowLevelPortAddress, int numAxes,
			     double movingPollPeriod,
			     double idlePollPeriod,
			     const int &extraParams) :
pmacController(portName, lowLevelPortName, lowLevelPortAddress, numAxes, movingPollPeriod, idlePollPeriod, extraParams) {
static const char *functionName = "pmacV3Controller::pmacV3Controller";
createParam(EnableAxisString, asynParamInt32, &enableAxis_);
createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
callParamCallbacks();
}

/**
* C wrapper for the pmacAxis constructor.
* See pmacAxis::pmacAxis.
*
*/
asynStatus pmacCreateAxis(
const char *pmacName, /* specify which controller by port name */
int axis)             /* axis number (start from 1). */
{
pmacController *pC;
pmacAxis *pAxis;

static const char *functionName = "pmacCreateAxis";

pC = (pmacController *)findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n", driverName, functionName,
     pmacName);
return asynError;
}

pC->lock();
pAxis = new pmacAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}
/**
* C wrapper for the pmacHRPTAxis constructor.
* See pmacHRPTAxis::pmacHRPTAxis.
*
*/
asynStatus pmacCreateHRPTAxis(const char *pmacName,         /* specify which controller by port name */
		  int axis)                    /* axis number (start from 1). */
{
pmacController *pC;
pmacAxis *pAxis;

static const char *functionName = "pmacCreateHRPTAxis";

pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
   driverName, functionName, pmacName);
return asynError;
}

pC->lock();
pAxis = new pmacHRPTAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}

/**
* C wrapper for the SeleneAxis constructor.
* See SeleneAxis::SeleneAxis.
*
*/
asynStatus SeleneCreateAxis(const char *pmacName,         /* specify which controller by port name */
		    int axis,        /* axis number (start from 1). */
		    double limitTarget)
{
SeleneController *pC;
SeleneAxis *pAxis;

static const char *functionName = "SeleneCreateAxis";

pC = (SeleneController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
   driverName, functionName, pmacName);
return asynError;
}

pC->lock();
pAxis = new SeleneAxis(pC, axis, limitTarget);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}

/**
* C wrapper for the Selene LiftAxis constructor.
* See LiftAxis::LiftAxis.
*
*/
asynStatus LiftCreateAxis(const char *pmacName,         /* specify which controller by port name */
		  int axis)        /* axis number (start from 1). */
{
pmacController *pC;
LiftAxis *pAxis;

static const char *functionName = "LiftCreateAxis";

pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
   driverName, functionName, pmacName);
return asynError;
}

pC->lock();
pAxis = new LiftAxis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}

/**
* C wrapper for the pmacV3Axis constructor.
* See pmacAxis::pmacV3Axis.
*
*/
asynStatus pmacV3CreateAxis(
const char *pmacName, /* specify which controller by port name */
int axis)             /* axis number (start from 1). */
{
pmacController *pC;
pmacAxis *pAxis;

static const char *functionName = "pmacV3CreateAxis";

pC = (pmacController *)findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n", driverName, functionName,
     pmacName);
return asynError;
}

pC->lock();
pAxis = new pmacV3Axis(pC, axis);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}

/**
* C Wrapper function for pmacHRPTAxis constructor.
* See pmacAxis::pmacAxis.
* This function allows creation of multiple pmacAxis objects with axis numbers 1 to numAxes.
* @param pmacName Asyn port name for the controller (const char *)
* @param numAxes The number of axes to create, starting at 1.
*
*/
asynStatus pmacCreateAxes(const char *pmacName,        
		  int numAxes)                   
{
pmacController *pC;
pmacAxis *pAxis;

static const char *functionName = "pmacCreateAxis";

pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
   driverName, functionName, pmacName);
return asynError;
}

pC->lock();
for (int axis=1; axis<=numAxes; axis++) {
pAxis = new pmacAxis(pC, axis);
pAxis = NULL;
}
pC->unlock();
return asynSuccess;
}

/**
* C wrapper for the pmacAmorDetectorAxis constructor.
* See pmacAmorDetectorAxis::pmacAmorDetectorAxis.
*
*/
asynStatus pmacCreateAmorDetectorAxis(const char *pmacName,         /* specify which controller by port name */
					int axis,                      /* axis number (start from 1). */
			    int function_code)             /* AMOR detector axis function */
{     
pmacController *pC;
pmacAxis *pAxis;

static const char *functionName = "pmacCreateHRPTAxis";

pC = (pmacController*) findAsynPortDriver(pmacName);
if (!pC) {
printf("%s:%s: Error port %s not found\n",
   driverName, functionName, pmacName);
return asynError;
}

pC->lock();
pAxis = new AmorDetectorAxis(pC, axis, function_code);
pAxis = NULL;
pC->unlock();
return asynSuccess;
}

/*================================ SeleneController ===============================================*/

asynStatus SeleneController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
int function = pasynUser->reason;
asynStatus status = asynError;
SeleneAxis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
char message[132];

static const char *functionName = "SeleneController::writeFloat64";

sprintf(message,"%s, reason %d",  functionName, function);

pAxis = (SeleneAxis *)this->getAxis(pasynUser);
if (!pAxis) {
return asynError;
}


/* Set the parameter and readback in the parameter library. */
status = pAxis->setDoubleParam(function, value);

if (function == motorLowLimit_) {
sprintf(command, "Q%d54=%f", pAxis->axisNo_, (float)value/(float)MULT);
// sprintf(command, "Q%d54=%d", pAxis->axisNo_, (int)(value/pAxis->scale_/MULT));
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
      "%s: Setting low limit on controller %s, axis %d to %f\n",
      functionName, portName, pAxis->axisNo_, value);
errlogPrintf("Setting low limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
} else if (function == motorHighLimit_) {
sprintf(command, "Q%d53=%f", pAxis->axisNo_, (float)value/(float)MULT);
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
      "%s: Setting high limit on controller %s, axis %d to %f\n",
      functionName, portName, pAxis->axisNo_, value);
errlogPrintf("Setting high limit of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
} else if(function == setMotorPosition_){
snprintf(command,sizeof(command),"Q%d59=%f", pAxis->axisNo_, value);
asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
      "%s: Defining position of axis%d to %f\n",
      functionName, pAxis->axisNo_, value);
errlogPrintf("Defining position of axis %d to %f, command = %s\n", pAxis->axisNo_, value, command);
}

//Execute the command.
if (command[0] != 0 && status == asynSuccess) {
status = lowLevelWriteRead(pAxis->axisNo_,command, response);
}

// What if now status != asynSuccess

//Call base class method
//This will handle callCallbacks even if the function was handled here.
status = asynMotorController::writeFloat64(pasynUser, value);

return status;

}


asynStatus pmacV3Controller::writeInt32(asynUser *pasynUser, epicsInt32 value) {
int function = pasynUser->reason;
asynStatus status = asynSuccess;
pmacV3Axis *pAxis = NULL;
char command[64] = {0};
char response[64] = {0};
int isOn;
time_t startTime;
bool moving;
static const char *functionName = "pmacV3Controller::writeInt32";

  debugFlow(functionName);

  pAxis = (pmacV3Axis*)this->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 */
  pAxis->setIntegerParam(function, value);

  if (function == enableAxis_) {
    // only send commands when a state change is necessary      
    snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
    this->lowLevelWriteRead(pAxis->axisNo_, command, response);
    isOn = strtol(response, NULL, 10) != -3;
    if(value == 1 && isOn) {
      sprintf(command, "M%2.2d=15", pAxis->axisNo_);
      // lowLevelWriteRead(pAxis->axisNo_, command, response);
    }
    sprintf(command, "M%2.2d14=%d", pAxis->axisNo_, value);
    pAxis->updateMsgTxtFromDriver("");
    if(isOn != value) {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_FLOW,
              "%s: Enable axis on controller %s, axis %d enable=%d\n",
              functionName, portName, pAxis->axisNo_, value);
        lowLevelWriteRead(pAxis->axisNo_, command, response);
	startTime = time(NULL);
	while(time(NULL) < startTime + 2.){
	    snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
            this->lowLevelWriteRead(pAxis->axisNo_, command, response);
            isOn = strtol(response, NULL, 10) != -3;
	    if(value == isOn){
	    	pAxis->poll(&moving); // to update enable status
		return asynSuccess;
	    }
	    usleep(100);
       }
       errlogPrintf("PMAC: failed to switch axis %d enable status within 2 seconds\n", 
	            pAxis->axisNo_);
       return asynError;
    }
  } 

  return pmacController::writeInt32(pasynUser, value);
}

asynStatus pmacV3Controller::readInt32(asynUser *pasynUser, epicsInt32 *value) {

  int function = pasynUser->reason, axStat;
  asynStatus status = asynError;
  pmacV3Axis *pAxis = NULL;
  static const char *functionName = "pmacV3Controller::readInt32";
  char command[128];
  char response[128];

  debugFlow(functionName);

  pAxis = (pmacV3Axis*)(this->getAxis(pasynUser));
  if (!pAxis) {
    return asynError;
  }
  if (function == axisEnabled_) {
    snprintf(command, sizeof(command), "P%2.2d00", pAxis->axisNo_);
    status = this->lowLevelWriteRead(pAxis->axisNo_, command, response);
    *value = strtol(response, NULL, 10);
    axStat = *value != -3;
    int st = setIntegerParam(axisEnabled_, axStat);
    callParamCallbacks();
  }
  return pmacController::readInt32(pasynUser, value);
}


/* Code for iocsh registration */

#ifdef vxWorks
#else

/* pmacCreateController */
static const iocshArg pmacCreateControllerArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateControllerArg1 = {"Low level port name", iocshArgString};
static const iocshArg pmacCreateControllerArg2 = {"Low level port address", iocshArgInt};
static const iocshArg pmacCreateControllerArg3 = {"Number of axes", iocshArgInt};
static const iocshArg pmacCreateControllerArg4 = {"Moving poll rate (ms)", iocshArgInt};
static const iocshArg pmacCreateControllerArg5 = {"Idle poll rate (ms)", iocshArgInt};
static const iocshArg * const pmacCreateControllerArgs[] = {&pmacCreateControllerArg0,
							    &pmacCreateControllerArg1,
							    &pmacCreateControllerArg2,
							    &pmacCreateControllerArg3,
							    &pmacCreateControllerArg4,
							    &pmacCreateControllerArg5};
static const iocshFuncDef configpmacCreateController = {"pmacCreateController", 6, pmacCreateControllerArgs};
static void configpmacCreateControllerCallFunc(const iocshArgBuf *args)
{
  pmacCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}

static const iocshFuncDef configSeleneCreateController = {"SeleneCreateController", 6, pmacCreateControllerArgs};
static void configSeleneCreateControllerCallFunc(const iocshArgBuf *args)
{
  SeleneCreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival, args[4].ival, args[5].ival);
}
static const iocshFuncDef configpmacV3CreateController = {
    "pmacV3CreateController", 6, pmacCreateControllerArgs
};
static void configpmacV3CreateControllerCallFunc(const iocshArgBuf *args) {
  pmacV3CreateController(args[0].sval, args[1].sval, args[2].ival, args[3].ival,
                         args[4].ival, args[5].ival);
}

/* pmacCreateAxis */
static const iocshArg pmacCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg * const pmacCreateAxisArgs[] = {&pmacCreateAxisArg0,
                                                     &pmacCreateAxisArg1};
static const iocshFuncDef configpmacAxis = {"pmacCreateAxis", 2, pmacCreateAxisArgs};

static void configpmacAxisCallFunc(const iocshArgBuf *args)
{
  pmacCreateAxis(args[0].sval, args[1].ival);
}

/* pmacCreateHRPTAxis */
static const iocshArg pmacCreateHRPTAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateHRPTAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg * const pmacCreateHRPTAxisArgs[] = {&pmacCreateAxisArg0,
                                                     &pmacCreateAxisArg1};
static const iocshFuncDef configpmacHRPTAxis = {"pmacCreateHRPTAxis", 2, pmacCreateHRPTAxisArgs};

static void configpmacHRPTAxisCallFunc(const iocshArgBuf *args)
{
  pmacCreateHRPTAxis(args[0].sval, args[1].ival);
}


/* SeleneCreateAxis */
static const iocshArg SeleneCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg SeleneCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg SeleneCreateAxisArg2 = {"limitTraget", iocshArgDouble};
  static const iocshArg * const SeleneCreateAxisArgs[] = {&SeleneCreateAxisArg0,
							  &SeleneCreateAxisArg1,
                                                          &SeleneCreateAxisArg2};
static const iocshFuncDef configSeleneCreateAxis = {"SeleneCreateAxis", 3, SeleneCreateAxisArgs};

static void configSeleneCreateAxisCallFunc(const iocshArgBuf *args)
{
  SeleneCreateAxis(args[0].sval, args[1].ival,args[2].dval);
}

/* LiftCreateAxis */
static const iocshArg LiftCreateAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg LiftCreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg * const LiftCreateAxisArgs[] = {&LiftCreateAxisArg0,
                                                     &LiftCreateAxisArg1};
static const iocshFuncDef configLiftAxis = {"LiftCreateAxis", 2, LiftCreateAxisArgs};

static void configLiftAxisCallFunc(const iocshArgBuf *args)
{
  LiftCreateAxis(args[0].sval, args[1].ival);
}

/* pmacV3CreateAxis */
static const iocshArg pmacV3CreateAxisArg0 = {"Controller port name",
                                              iocshArgString};
static const iocshArg pmacV3CreateAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg *const pmacV3CreateAxisArgs[] = {&pmacV3CreateAxisArg0,
                                                       &pmacV3CreateAxisArg1};
static const iocshFuncDef configpmacV3Axis = {"pmacV3CreateAxis", 2,
                                              pmacV3CreateAxisArgs};

static void configpmacV3AxisCallFunc(const iocshArgBuf *args) {
  pmacV3CreateAxis(args[0].sval, args[1].ival);
}

/* pmacCreateAxes */
static const iocshArg pmacCreateAxesArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAxesArg1 = {"Num Axes", iocshArgInt};
static const iocshArg * const pmacCreateAxesArgs[] = {&pmacCreateAxesArg0,
                                                     &pmacCreateAxesArg1};
static const iocshFuncDef configpmacAxes = {"pmacCreateAxes", 2, pmacCreateAxesArgs};

static void configpmacAxesCallFunc(const iocshArgBuf *args)
{
  pmacCreateAxes(args[0].sval, args[1].ival);
}

/* pmacCraeteAmorDetectorAxis */
static const iocshArg pmacCreateAmorDetectorAxisArg0 = {"Controller port name", iocshArgString};
static const iocshArg pmacCreateAmorDetectorAxisArg1 = {"Axis number", iocshArgInt};
static const iocshArg pmacCreateAmorDetectorAxisArg2 = {"Function code", iocshArgInt};
static const iocshArg * const pmacCreateAmorDetectorAxisArgs[] = {&pmacCreateAmorDetectorAxisArg0,
                                                     &pmacCreateAmorDetectorAxisArg1, &pmacCreateAmorDetectorAxisArg2};
static const iocshFuncDef configpmacAmorDetectorAxis = {"pmacCreateAmorDetectorAxis", 3, pmacCreateAmorDetectorAxisArgs};

static void configpmacAmorDetectorAxisCallFunc(const iocshArgBuf *args)
{
  pmacCreateAmorDetectorAxis(args[0].sval, args[1].ival, args[2].ival);
}



static void pmacControllerRegister(void)
{
  iocshRegister(&configpmacCreateController,   configpmacCreateControllerCallFunc);
  iocshRegister(&configSeleneCreateController, configSeleneCreateControllerCallFunc);
  iocshRegister(&configpmacV3CreateController, configpmacV3CreateControllerCallFunc);
  iocshRegister(&configpmacAxis,               configpmacAxisCallFunc);
  iocshRegister(&configpmacHRPTAxis,           configpmacHRPTAxisCallFunc);
  iocshRegister(&configpmacAmorDetectorAxis, configpmacAmorDetectorAxisCallFunc);
  iocshRegister(&configSeleneCreateAxis,       configSeleneCreateAxisCallFunc);
  iocshRegister(&configLiftAxis,               configLiftAxisCallFunc);
  iocshRegister(&configpmacV3Axis,             configpmacV3AxisCallFunc);
  iocshRegister(&configpmacAxes,               configpmacAxesCallFunc);
}
epicsExportRegistrar(pmacControllerRegister);

#endif

} // extern "C"