/*
 Driver for the MasterMACS motor controller used at SINQ
 For documentation see the standard SINQ place for hardware documentation or 
 Marcel Schildt

 Mark Koennecke, March-August 2023
*/


#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <time.h>
#include <unistd.h>

#include <iocsh.h>
#include <epicsThread.h>
#include <errlog.h>

#include <asynOctetSyncIO.h>

#include "MasterMACSDriver.h"
#include <epicsExport.h>

#define CHECK_BIT(var,pos) ((var) & (1 << pos))
#define ABS(x) (x < 0 ? -(x) : (x))

#define debug 0

/** Creates a new MasterMACSController object.
  * \param[in] portName          The name of the asyn port that will be created for this driver
  * \param[in] MasterMACSPortName     The name of the drvAsynSerialPort that was created previously to connect to the MasterMACS controller 
  * \param[in] numAxes           The number of axes that this controller supports 
  */
MasterMACSController::MasterMACSController(const char *portName,
					   const char *MasterMACSPortName,
					   int numAxes,
					   int idlePoll, int busyPoll):SINQController
    (portName, MasterMACSPortName, numAxes)
{
    asynStatus status;
    static const char *functionName =
	"MasterMACSController::MasterMACSController";
    char terminator[2] = "\x03";


    /* Connect to MasterMACS controller */
    status =
	pasynOctetSyncIO->connect(MasterMACSPortName, 0,
				  &pasynUserController_, NULL);
    if (status) {
	asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR,
		  "%s: cannot connect to MasterMACS controller\n",
		  functionName);
    }
    pasynOctetSyncIO->setOutputEos(pasynUserController_, terminator,
				   strlen(terminator));
    pasynOctetSyncIO->setInputEos(pasynUserController_, terminator,
				  strlen(terminator));


    pAxes_ = (MasterMACSAxis **) (asynMotorController::pAxes_);
    createParam(EnableAxisString, asynParamInt32, &enableAxis_);
    createParam(AxisEnabledString, asynParamInt32, &axisEnabled_);
    callParamCallbacks();
	    
    startPoller(busyPoll/1000., idlePoll/1000., 1);
    setIdlePollPeriod(idlePoll/1000.);
    setMovingPollPeriod(busyPoll/1000.);
}


/** Creates a new MasterMACSController object.
  * Configuration command, called directly or from iocsh
  * \param[in] portName          The name of the asyn port that will be created for this driver
  * \param[in] MasterMACSPortName       The name of the drvAsynIPPPort that was created previously to connect to the MasterMACS controller 
  * \param[in] numAxes           The number of axes that this controller supports 
  * \param[in] idlePoll Poll interval when idle in microseconds
  * \param[in] busyPoll Poll interval when moving in microSeconds
  */
extern "C" int
MasterMACSCreateController(const char *portName,
			   const char *MasterMACSPortName, int numAxes, int idlePoll, int busyPoll)
{
    MasterMACSController *pMasterMACSController
	= new MasterMACSController(portName, MasterMACSPortName, numAxes, idlePoll, busyPoll);
    pMasterMACSController = NULL;
    return (asynSuccess);
}

/** Reports on status of the driver
  * \param[in] fp The file pointer on which report information will be written
  * \param[in] level The level of report detail desired
  *
  * If details > 0 then information is printed about each axis.
  * After printing controller-specific information it calls asynMotorController::report()
  */
void
 MasterMACSController::report(FILE * fp, int level)
{
    fprintf(fp, "MasterMACS motor driver %s, numAxes=%d\n",
	    this->portName, numAxes_);

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

/** Returns a pointer to an MasterMACSAxis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] pasynUser asynUser structure that encodes the axis index number. */
MasterMACSAxis *MasterMACSController::getAxis(asynUser * pasynUser)
{
    return static_cast <
	MasterMACSAxis * >(asynMotorController::getAxis(pasynUser));
}

/** Returns a pointer to an MasterMACSAxis object.
  * Returns NULL if the axis number encoded in pasynUser is invalid.
  * \param[in] axisNo Axis index number. */
MasterMACSAxis *MasterMACSController::getAxis(int axisNo)
{
    return static_cast <
	MasterMACSAxis * >(asynMotorController::getAxis(axisNo));
}

/**
 * send a command to the MasterMACS and read the reply. Do some error and controller 
 * issue fixing on the way
 * \param[in] command The command to send
 * \param[out] reply The controllers reply
 */

asynStatus
    MasterMACSController::transactController(int axisNo,
					     char command[COMLEN],
					     char reply[COMLEN])
{
    asynStatus status;
    size_t in, out;
    int reason, len, idx, lenPayload;
    unsigned int i;
    char *mmacsData =
	NULL, ackchar, mmacsResponse[COMLEN], hexResponse[256];
    SINQAxis *axis = getAxis(axisNo);

    pasynOctetSyncIO->flush(pasynUserController_);

    /* read with a short timeout in order to remove duplicate messages 
     * from the line. This also serves to slow down communication
     */
    pasynOctetSyncIO->read(pasynUserController_, mmacsResponse, 35, .05, &in, &reason); 

    /* pack data for MasterMACS */
    len = strlen(command) + 6;
    mmacsData = (char *) malloc(len * sizeof(char));
    if (!mmacsData) {
	errlogSevPrintf(errlogMajor,
			"Failed to allocate memory in MasterMACSController::transactController");
	return asynError;
    }
    mmacsData[0] = 0x05;
    mmacsData[1] = (char) (len - 2);
    mmacsData[2] = 0;
    mmacsData[3] = 0x19;
    memcpy(mmacsData + 4, command, strlen(command) * sizeof(char));
    mmacsData[len - 2] = 0x0D;
    /* 0x03 is appended by asyn */

    /* send the stuff away ... */
    if(debug) {
    	errlogSevPrintf(errlogMajor,"Sending command: %s\n", command);
    }

    status =
	pasynOctetSyncIO->writeRead(pasynUserController_, mmacsData,
				    len - 1, mmacsResponse, 35, 5., &out,
				    &in, &reason);
    if (status != asynSuccess) {
	if (axis != NULL) {
	    errlogSevPrintf(errlogMajor,
			    "Lost connection to motor controller,a axis %d, reason %d",
			    axisNo, reason);
	    axis->updateMsgTxtFromDriver
		("Lost connection to motor controller");
	    return status;
	}
	errlogSevPrintf(errlogMajor,
			"Lost connection to motor controller without axis, reason %d",
			reason);
	return status;
    }
    free(mmacsData);

    /* format and print the response in hex for debugging purposes */
    if(debug) {       
       for(i = 0, idx = 0; i < in; i++){
       sprintf(hexResponse + idx, "%02x ", (unsigned int)mmacsResponse[i]);
       idx = strlen(hexResponse);
       }        
       errlogSevPrintf(errlogMajor,"Reply in hex: %s\n", hexResponse);
    }

    /* Here we have read the data from the MasterMACS. We proceed to extract
     * the payload and the state of the ACK byte and place that reply into data
     */
    if ((in < 33)) {
	errlogSevPrintf(errlogMajor,
			"MasterMACS only sent %d bytes, 34 expected",
			(int) in);
	return asynError;
    }
    /*
     * Unpack the MasterMACS message, start by finding <CR>
     */
    for (i = 4, idx = 0; i < 34; i++) {
	if (mmacsResponse[i] == 0x0d) {
	    idx = i;
	    break;
	}
    }
    //errlogSevPrintf(errlogMajor, "Found <CR> at %d", idx);

    /* one character before <CR> is the ACK, if it is there */
    ackchar = mmacsResponse[idx - 1];
    if (ackchar == 0x06) {
	strcpy(reply, "ACK:");
    } else if (ackchar == 0x15) {
	strcpy(reply, "NAK:");
	errlogSevPrintf(errlogMajor,
			"MasterMACS responded with NAK to %s\n", command);
	status = asynError;
    } else {
	/* the MasterMACS does not always send a ACK/NAK on read */
	strcpy(reply, "NON:");
    }
    //errlogSevPrintf(errlogMajor, "Reply after testing ACK byte: %s ", reply);

    /* copy data */
    lenPayload = idx - 4;
    memcpy(reply + 4, mmacsResponse + 4, lenPayload);
    reply[4 + lenPayload] = (char) 0;
    if(debug) {
   	 errlogSevPrintf(errlogMajor, "Completed reply at end of transactController: %s ", reply);
    }
    return status;
}

asynStatus
    MasterMACSController::writeInt32(asynUser * pasynUser,
				     epicsInt32 value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    MasterMACSAxis *pAxis = NULL;
    char command[64] = { 0 };
    char response[64] = { 0 };
    int devStatus;
    bool moving;
    time_t startTime;

    pAxis = (MasterMACSAxis *) 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 == motorStop_){
        errlogPrintf("Stop called with value %d\n", value);
        double accel;
        getDoubleParam(pAxis->axisNo_, motorAccel_, &accel);
        status = pAxis->stop(accel);
        return status;
    }
    if (function == enableAxis_) {
        /*
        * Read the status in order to prevent execssive commands
        */
        devStatus = pAxis->readStatus();
        if (value == 1 && !pAxis->isOn(devStatus) ) {
	        /* download parameters, does not work as of now */
	        /*
	        sprintf(command, "%dS85=1.", pAxis->axisNo_);
	        status = transactController(pAxis->axisNo_, command, response);
	        */

	        /*  actually enable */
	        sprintf(command, "%dS04=1", pAxis->axisNo_);
	        status = transactController(pAxis->axisNo_, command, response);
        } else if(pAxis->isOn(devStatus)) {
        	        // only send command when necessary
	        	sprintf(command, "%dS04=0", pAxis->axisNo_);
	            	status = transactController(pAxis->axisNo_, command, response);
	} else {
		// nothing to do
		return status;
	}
	if (status == asynSuccess) {
	    pAxis->updateMsgTxtFromDriver("");
	} else {
	    errlogPrintf("MMACS: Failure to enable or disable axis %d",
			 pAxis->axisNo_);
	}
	startTime = time(NULL);
	while(time(NULL) < startTime + 2.){
		// wait for the change to happen
		devStatus = pAxis->readStatus();
		if(pAxis->isOn(devStatus) == value){
            pAxis->active = true;
			pAxis->poll(&moving); // to update the Enable_RBV field
            pAxis->active = false;
			return asynSuccess;
		}
                usleep(200);
	}
	errlogPrintf("MMACS: Failed to enable motor %d within 2 seconds\n", pAxis->axisNo_); 
    }
    return asynMotorController::writeInt32(pasynUser, value);
}


// These are the MasterMACSAxis methods

/** Creates a new MasterMACSAxis object.
  * \param[in] pC Pointer to the MasterMACSController to which this axis belongs. 
  * \param[in] axisNo Index number of this axis, range 0 to pC->numAxes_-1.
  * 
  * Initializes register numbers, etc.
  */
MasterMACSAxis::MasterMACSAxis(MasterMACSController * pC, int axisNo):SINQAxis(pC, axisNo),
pC_
(pC)
{
    hasStarted = false;
    active = false;
}

/** Reports on status of the axis
  * \param[in] fp The file pointer on which report information will be written
  * \param[in] level The level of report detail desired
  *
  * After printing device-specific information calls asynMotorAxis::report()
  */
void MasterMACSAxis::report(FILE * fp, int level)
{
    if (level > 0) {
	fprintf(fp, "  axis %d\n", axisNo_);
    }
}

int MasterMACSAxis::readStatus()
{ 
    char command[COMLEN], reply[COMLEN], *pPtr;
    float fval;
    int status;

    /*
     * The MasterMACS sends invalid responses with a low frequency.
     * Therefore I send cached status responses in such a case in order 
     * to help the logic evrywhere else in the code.
     */
    sprintf(command, "%dR10", axisNo_);
    status = pC_->transactController(axisNo_, command, reply);
    if (status == asynError) {
	    return oldStatus;
    }
    pPtr = strstr(reply, "=");
    if(pPtr) {
        sscanf(pPtr + 1, "%f", &fval);
        oldStatus = (int) fval;
        return oldStatus;
    } else { 
        errlogPrintf("MMACS: invalid status reponse %s on axis %d\n", reply, axisNo_);
        return oldStatus;
    }
}


int MasterMACSAxis::isOn(int axisStatus)
{
    if (CHECK_BIT(axisStatus, 0) && CHECK_BIT(axisStatus, 1)) {
	return 1;
    }
    return 0;
}

asynStatus
    MasterMACSAxis::move(double position, int relative, double minVelocity,
			 double maxVelocity, double acceleration)
{
    asynStatus status = asynSuccess;
    char command[COMLEN], reply[COMLEN];
    int devStatus;

    //errlogPrintf("minVelocity = %f, maxVelocity = %f\n", minVelocity, maxVelocity);
    memset(command, 0, COMLEN * sizeof(char));

    /* clear motor error message */
    updateMsgTxtFromDriver("");

    /*
     * reset error code
     */
    if(errorCodeFound){
        sprintf(command, "%dS17=0", axisNo_);
        status = pC_->transactController(axisNo_, command, reply);
        errorCodeFound = 0;
    }

    /*
     * only start if the thing is On
     */
    devStatus = readStatus();

    /*
     * set speed
     */
    /* temporarily disabled in order to get the basic logic right
    sprintf(command, "%dS05=%f", axisNo_, maxVelocity / 1000);
    status = pC_->transactController(axisNo_, command, reply);
    */

    if (relative) {
	    position += this->position;
    }

    if(isOn(devStatus) && active == false) {
        errlogPrintf("Starting axis %d with destination %f\n", axisNo_,
		             position / 1000.);

        /* send target position */
        sprintf(command, "%dS02= %.3f", axisNo_, position / 1000.);
        status = pC_->transactController(axisNo_, command, reply);
        if (status == asynError) {
            errlogPrintf("MMACS: failed to set target on %d\n", axisNo_);
	        updateMsgTxtFromDriver("Failed to send target position");
	        return status;
        }

        /* send move command */
        sprintf(command, "%dS00=1", axisNo_);
        status = pC_->transactController(axisNo_, command, reply);
        if (status == asynError) {
	        errlogPrintf("MMACS: failed to start axis %d\n", axisNo_);
	        updateMsgTxtFromDriver("Failed to start axis");
	        return status;
        }
        hasStarted = true;
        homing = 0;
        active = true;
        usleep(500);
        lastPositionUpdate = time(NULL);
    } else {
        errlogPrintf("MMACS: axis %d disabled, cannot start\n", axisNo_);
    }
    return status;
}

asynStatus
    MasterMACSAxis::home(double minVelocity, double maxVelocity,
			 double acceleration, int forwards)
{
    asynStatus status;
    char command[COMLEN], reply[COMLEN];
    int devStatus;

    memset(command, 0, COMLEN * sizeof(char));

    /*
     * test if the thing is On
     */
    devStatus = readStatus();
    updateMsgTxtFromDriver("");

    if(isOn(devStatus)){
        sprintf(command, "%dS00=9", axisNo_);
        homing = 1;
        status = pC_->transactController(axisNo_, command, reply);
        hasStarted = true;
        active = true;
        lastPositionUpdate = time(NULL);
        return status;
    } else {
        errlogPrintf("MMACS: cannot home disabled axis %d\n", axisNo_);
        return asynError;
    }
}

asynStatus
    MasterMACSAxis::moveVelocity(double minVelocity, double maxVelocity,
				 double acceleration)
{
    setIntegerParam(pC_->motorStatusProblem_, true);
    errlogSevPrintf(errlogMajor,
		    "This controller does not support the jog feature");
    return asynError;
}

asynStatus MasterMACSAxis::stop(double acceleration)
{
    asynStatus status = asynSuccess;
    char command[COMLEN], reply[COMLEN];
    int devStatus;

    memset(command, 0, COMLEN * sizeof(char));

    devStatus = readStatus();
    if(!CHECK_BIT(devStatus, 10)) {
        // only try to stop when running ...    
	    sprintf(command, "%dS00=8", axisNo_);
	    status = pC_->transactController(axisNo_, command, reply);
	    errlogPrintf("Sent STOP on Axis %d\n", axisNo_);
	    updateMsgTxtFromDriver("Axis interrupted");
    }
    return status;
}

asynStatus MasterMACSAxis::setPosition(double position)
{
    setIntegerParam(pC_->motorStatusProblem_, true);
    errlogSevPrintf(errlogMajor,
		    "This controller does not support setting position");
    updateMsgTxtFromDriver("Controller does not support setPosition");
    return asynError;
}

asynStatus MasterMACSAxis::setClosedLoop(bool closedLoop)
{
    /*
       This belongs into the Kingdom of Electronics.
       We do not do this.
     */

    return asynError;
}

/** Polls the axis.
 * This function reads the motor position, the limit status, the home status, the moving status, 
 * and the drive power-on status. 
 * It calls setIntegerParam() and setDoubleParam() for each item that it polls,
 * and then calls callParamCallbacks() at the end.
 * \param[out] moving A flag that is set indicating that the axis is moving (true) or done (false). */
asynStatus MasterMACSAxis::poll(bool * moving)
{
    asynStatus comStatus = asynSuccess;
    char command[COMLEN], reply[COMLEN], *pPtr, buffer[80];
    unsigned int errCode, derCode, devStatus;
    float errStatus;
    struct tm* tm_info;
    time_t timer;

    /* 
     * EPICS always polls all motors on a controller when one motor is active.
     * In order to reduce load on the controller we check if we are active and
     * if not return old stuff
     */
    if(active == false && time(NULL) < lastPoll + pC_->idlePollPeriod_) {
        *moving = false;
        return asynSuccess;
    }

    timer = time(NULL);
    tm_info = localtime(&timer);

    strftime(buffer, 26, "%Y-%m-%d %H:%M:%S", tm_info);
    errlogPrintf("poll called at %s on axis %d \n", 
		 buffer, axisNo_ );
    lastPoll = time(NULL);

    setIntegerParam(pC_->motorStatusProblem_, false);
    memset(command, 0, COMLEN * sizeof(char));

    // Read the current motor position
    sprintf(command, "%dR12", axisNo_);
    comStatus = pC_->transactController(axisNo_, command, reply);
    if(comStatus != asynError) {
        pPtr = strstr(reply, "=");
        if(pPtr) {
	        sscanf(pPtr + 1, "%lf", &position);
            setDoubleParam(pC_->motorPosition_, position * 1000.);
            setDoubleParam(pC_->motorEncoderPosition_, position * 1000.);
            /* 
            * keep track of position in order to check for a stuck motor later
            */
            if(ABS(position - oldPosition) > 1){
                oldPosition = position;
                lastPositionUpdate = time(NULL);
            }
       } else {
            errlogPrintf("MMACS: Invalid response asking position on %d\n", axisNo_); 
       }
    } else {
        errlogPrintf("MMACS: communication problem reading axis %d position\n", axisNo_);
    }

    // Read the overall  status of this motor */
    devStatus = readStatus();
    if(debug) {
    	errlogPrintf("Axis %d, position %lf, devStatus %d\n", axisNo_,
	             position, devStatus);
    }

    // Check for the thing being in a bad state
    if(CHECK_BIT(devStatus, 6)) {
        *moving = false;
        active = false;
 	    setIntegerParam(pC_->motorStatusDone_, true);
        updateMsgTxtFromDriver("AXIS dead");
        goto skip;
    }


    setIntegerParam(pC_->enableAxis_, isOn(devStatus));
    setIntegerParam(pC_->axisEnabled_, isOn(devStatus));

    // Check if the motor is disabled
    if (!isOn(devStatus)) {
	    updateMsgTxtFromDriver("Axis disabled");
	    *moving = false;
        active = false;
	    setIntegerParam(pC_->motorStatusDone_, true);
	    goto skip;
    }

    /*
     * if the motor has never run, the status bit 10 is invalid
     */
    if (!hasStarted) {
    	*moving = false;
	    setIntegerParam(pC_->motorStatusDone_, true);
        active = false;
	    goto skip;
    }

    /*
     * We may have a valid status bit...
     */
    if (!CHECK_BIT(devStatus, 10)) {
    	/* we are still creeping along .... */
	    *moving = true;
	    setIntegerParam(pC_->motorStatusDone_, false);
        if(time(NULL) > lastPositionUpdate + 120) {
            // we are detecting a stuck motor
            errlogPrintf("MMACS: axis %d is STUCK!!\n", axisNo_);
            updateMsgTxtFromDriver("Axis STUCK!!");
    	    setIntegerParam(pC_->motorStatusDone_, true);
           *moving = false;
            active = false;
        }
	    goto skip;
    }


    /*we are done moving */
    *moving = false;
    active = false;
    setIntegerParam(pC_->motorStatusDone_, true);
    updateMsgTxtFromDriver("");

    /* when homing, set the proper flag */
    if (homing) {
	    setIntegerParam(pC_->motorStatusAtHome_, true);
    }

    /* check for limit switches*/
    setIntegerParam(pC_->motorStatusLowLimit_, false);
    setIntegerParam(pC_->motorStatusHighLimit_, false);
    if (CHECK_BIT(devStatus, 11)) {
	    errlogSevPrintf(errlogMajor, "Limit bit in status code %d", axisNo_);
	    /* guessing which limit has been hit ... */
	    if (position > 0) {
		    updateMsgTxtFromDriver("Hit positive limit switch");
		    setIntegerParam(pC_->motorStatusHighLimit_, true);
	    } else {
		    updateMsgTxtFromDriver("Hit negative limit switch");
		    setIntegerParam(pC_->motorStatusLowLimit_, true);
	    }
	    goto skip;
	}
    
    /* check for error conditions */
	if (CHECK_BIT(devStatus, 3)) {
        /* read error codes */
        sprintf(command, "%dR11", axisNo_);
        comStatus = pC_->transactController(axisNo_, command, reply);
        if (comStatus != asynError) {
         pPtr = strstr(reply, "=");
            if(pPtr) {
                sscanf(pPtr + 1, "%f", &errStatus);
                errCode = (unsigned int) errStatus;
            
            } else {
                errlogPrintf("MMACS: axis %d received invalid reply asking for error code \n", axisNo_);
                errCode = 0;
                goto skip;
            }
        } else {
            errlogPrintf("MMACS: axis %d failed reading error code \n", axisNo_);
            goto skip;
        }

        sprintf(command, "%dR18", axisNo_);
        comStatus = pC_->transactController(axisNo_, command, reply);
        if (comStatus != asynError) {
            pPtr = strstr(reply, "=");
            if(pPtr) {
                sscanf(pPtr + 1, "%f", &errStatus);
                derCode = (unsigned int) errStatus;
            } else {
                errlogPrintf("MMACS: malformed reply for R18: %s, on axis %d\n", reply, axisNo_);
                derCode = 0;
                goto skip;
            }
        } else {
            errlogPrintf("MMACS: axis %d failed reading extended error code R18 \n", axisNo_);
            goto skip;
        }

        if(debug) {
		errlogPrintf("Axis %d, errCode(R11) %d, derCode(R18) %d\n", axisNo_,
		             errCode, derCode);
    	}

        if (errCode == 0) {
	        errlogSevPrintf(errlogMajor,
			    "Fault bit in status code, but no error code on %d\n", axisNo_);
	        updateMsgTxtFromDriver ("Fault bit in status code without error code");
            goto skip;
	    }
        errorCodeFound = 1;

        if (CHECK_BIT(errCode, 0)) {
	        errlogSevPrintf(errlogMajor, "CAN error on %d", axisNo_);
	        updateMsgTxtFromDriver("CAN error");
        } else if (CHECK_BIT(errCode, 1)) {
	        errlogSevPrintf(errlogMajor, "Short circuit on %d", axisNo_);
	        updateMsgTxtFromDriver("Short circuit");
        } else if (CHECK_BIT(errCode, 2)) {
	        errlogSevPrintf(errlogMajor, "Invalide Setup on %d", axisNo_);
	        updateMsgTxtFromDriver("Invalid Setup");
        } else if (CHECK_BIT(errCode, 3)) {
	        errlogSevPrintf(errlogMajor, "Control error on %d", axisNo_);
	        updateMsgTxtFromDriver("Control error");
        } else if (CHECK_BIT(errCode, 4)) {
	        errlogSevPrintf(errlogMajor, "CAN communication error on %d",
			                axisNo_);
	        updateMsgTxtFromDriver("CAN communication error");
        } else if (CHECK_BIT(errCode, 5)) {
	        errlogSevPrintf(errlogMajor, "Feedback error on %d", axisNo_);
	        updateMsgTxtFromDriver("Feedback error");
        } else if (CHECK_BIT(errCode, 6)) {
	        updateMsgTxtFromDriver("Hit positive limit switch");
	        setIntegerParam(pC_->motorStatusHighLimit_, true);
	        setIntegerParam(pC_->motorStatusProblem_, false);
        } else if (CHECK_BIT(errCode, 7)) {
	        updateMsgTxtFromDriver("Hit negative limit switch");
	        setIntegerParam(pC_->motorStatusLowLimit_, true);
	        setIntegerParam(pC_->motorStatusProblem_, false);
        } else if (CHECK_BIT(errCode, 8)) {
	        errlogSevPrintf(errlogMajor, "Over current %d", axisNo_);
	        updateMsgTxtFromDriver("Over current");
        } else if (CHECK_BIT(errCode, 9)) {
	        errlogSevPrintf(errlogMajor, "I2T protection on %d", axisNo_);
	        updateMsgTxtFromDriver("I2t protection");
        } else if (CHECK_BIT(errCode, 10)) {
	        errlogSevPrintf(errlogMajor, "Over heated motor on  %d", axisNo_);
	        updateMsgTxtFromDriver("Motor overheated");
        } else if (CHECK_BIT(errCode, 11)) {
	        errlogSevPrintf(errlogMajor, "Over temperature drive on %d",
			axisNo_);
	        updateMsgTxtFromDriver("Over temperature drive");
        } else if (CHECK_BIT(errCode, 12)) {
	        errlogSevPrintf(errlogMajor, "Over voltage on %d", axisNo_);
	        updateMsgTxtFromDriver("Over voltage");
        } else if (CHECK_BIT(errCode, 13)) {
	        errlogSevPrintf(errlogMajor, "Under voltage on %d", axisNo_);
	        updateMsgTxtFromDriver("Under voltage");
        } else if (CHECK_BIT(errCode, 14)) {
	        errlogSevPrintf(errlogMajor, "Command error on %d", axisNo_);
	        updateMsgTxtFromDriver("Command error");
        } else if (CHECK_BIT(errCode, 15)) {
	        errlogSevPrintf(errlogMajor, "Motor disabled on %d", axisNo_);
	        updateMsgTxtFromDriver("Motor disabled");
        }
  }

  skip:
    callParamCallbacks();
    return comStatus;
}

/** Code for iocsh registration */
static const iocshArg
    MasterMACSCreateControllerArg0 = { "Port name", iocshArgString };

static const iocshArg
    MasterMACSCreateControllerArg1 =
    { "MasterMACS port name", iocshArgString };

static const iocshArg
    MasterMACSCreateControllerArg2 = { "Number of axes", iocshArgInt };

static const iocshArg
    MasterMACSCreateControllerArg3 = { "idlePoll", iocshArgInt };

static const iocshArg
    MasterMACSCreateControllerArg4 = { "busyPoll", iocshArgInt };


static const iocshArg *const
    MasterMACSCreateControllerArgs[] = { &MasterMACSCreateControllerArg0,
    &MasterMACSCreateControllerArg1,
    &MasterMACSCreateControllerArg2,
    &MasterMACSCreateControllerArg3,
    &MasterMACSCreateControllerArg4
};

static const iocshFuncDef
    MasterMACSCreateControllerDef =
    { "MasterMACSCreateController", 5, MasterMACSCreateControllerArgs };
static void MasterMACSCreateContollerCallFunc(const iocshArgBuf * args)
{
    MasterMACSCreateController(args[0].sval, args[1].sval, args[2].ival, 
		    	       args[3].ival, args[4].ival);
}

/**
  * C wrapper for the MasterMACSAxis constructor.
  *  See MasterMAXSAxis::MasterMACSAxis.
  *
  */
asynStatus MasterMACSCreateAxis(const char *MasterMACSPort,	/* specify which controller by port name */
				int axis)
{				/* axis number (start from 1). */
    MasterMACSController *pC;
    MasterMACSAxis *pAxis;

    static const char *functionName = "MasterMACSCreateAxis";

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

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

/* MasterMACSCreateAxis */
static const iocshArg
    MasterMACSCreateAxisArg0 = { "Controller port name", iocshArgString };

static const iocshArg
    MasterMACSCreateAxisArg1 = { "Axis number", iocshArgInt };

static const iocshArg *const
    MasterMACSCreateAxisArgs[] = { &MasterMACSCreateAxisArg0,
    &MasterMACSCreateAxisArg1
};

static const iocshFuncDef
    configMasterMACSAxis =
    { "MasterMACSCreateAxis", 2, MasterMACSCreateAxisArgs };

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

static void MasterMACSRegister(void)
{
    iocshRegister(&MasterMACSCreateControllerDef,
		  MasterMACSCreateContollerCallFunc);
    iocshRegister(&configMasterMACSAxis, configMasterMACSAxisCallFunc);
}

extern "C" {
    epicsExportRegistrar(MasterMACSRegister);
}