/********************************************************************************/
/*    H   H Y   Y TTTTT EEEEE  CCC       HYTEC ELECTRONICS LTD                  */
/*    H   H  Y Y    T   E     C          5 Cradock Road,                        */
/*    HHHHH   Y     T   EEE   C          Reading, Berks.    Tel: 0118 9757770	*/
/*    H   H   Y     T   E     C          RG2 0JT            Fax: 0118 9757566	*/
/*    H   H   Y     T   EEEEE  CCC       Web: www.hytec-electronics.co.uk       */
/********************************************************************************/
/********************************************************************************/
/*    _____________________________________________________________________     */
/*    | H Y T E C   8 6 0 1   S T E P   M O T E R   A s y n   D r i v e r |     */
/*    ---------------------------------------------------------------------     */
/*                                                                              */
/*  Source file name      :-      HytecMotorDriver.c                            */
/*                                                                              */
/*  Initial creation date :-      29-Mar-2011                                   */
/*                                                                              */
/*  Original Developers   :-      Jim Chen. Hytec Electronics Ltd               */
/*                                                                              */
/********************************************************************************/
/*                                                                              */
/*  Description :- This is the "model 3" asyn motor driver for Hytec 8601  		*/
/*                 Stepper Motor IP module. The code is based on original   	*/
/*  			   Hytec drvHy8601asyn.c driver and also Mark Rivers' 			*/
/*				   ACRMotorDriver. 					                          	*/
/*                                                                              */
/*                                                                              */
/*  (C)2011 Hytec Electronics Ltd.                                              */
/*                                                                              */
/********************************************************************************/
/*                                                                              */
/*  Revision history: (comment and initial revisions)                           */
/*                                                                              */
/*  vers.       revised              modified by         date                   */
/*  -----       -----------        ----------------   ---------------           */
/*  2.0         Continued version  		Jim Chen       	 29/03/2011             */
/*  	        The main contents of this driver are the same as the original   */
/*  			Hytec drvHy8601asyn.c driver but in "model 3" form as defined   */
/*              by Mark Rivers. Hence it starts from version 2.0.				*/
/*  2.1         New interfaces 			Jim Chen       	 04/04/2011             */
/*  	        This version follows the asyn motor model 3 latest interface	*/
/*				changes that include:											*/
/*				a).New asynMotorAxis base class									*/
/*				b).Moves the axis specific functions from the motor controller 	*/
/*				   class to individual axis class								*/
/*				c).Changes asynMotorDriver.cpp name to asynMotorController.cpp.	*/
/*  2.2         Bugs fix 				Jim Chen       	 14/04/2011             */
/*  	        Fixed setPosition bug											*/
/*				Added firmware version parameter								*/
/*  2.3         Bugs fix 				Jim Chen       	 21/04/2011             */
/*  	        Fixed detecting encoder logic wrong bug							*/
/*  2.4         Bugs fix 				Jim Chen       	 24/06/2011             */
/*  	        Fixed wrong ioc shell command function name						*/
/*  2.5         Changes for 64bit Linux	Jim Chen, Mark Rivers 07/07/2011        */
/*  	        Changed the cast modifier for "this" pointer in ipmIntConnect 	*/
/*              function by (long) rather than (int) to suit 64bit Linux		*/
/*              build. Also corrected the PROM reading. Added more info in      */
/*              report when value > 0.                                          */
/*              				                                                */
/********************************************************************************/

#include <stddef.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>
#include <stdio.h>
#include <string.h>

#include <epicsFindSymbol.h>
#include <epicsTime.h>
#include <epicsThread.h>
#include <epicsMutex.h>
#include <ellLib.h>
#include <epicsMessageQueue.h>

#include <drvSup.h>
#include <devLib.h>
#include <drvIpac.h>
#include <iocsh.h>

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

static void ISR_8601(int pDrv);
static void intQueuedTask( void *pDrv );

#define MAX_MESSAGES  100           /* maximum number of messages */
#define ENCODER_HARD_RATIO    0.25 	/* encoder hardware ratio, it is 1/4 since the quadrature encoder */


// Constructor
HytecMotorController::HytecMotorController(const char *portName, int numAxes, 
			double movingPollPeriod, double idlePollPeriod, int cardnum, 
			int ip_carrier, int ipslot, int vector, int useencoder,
	        double encoderRatio0, double encoderRatio1, double encoderRatio2,
	        double encoderRatio3)
    :   asynMotorController(portName, numAxes, NUM_HYTEC_PARAMS, 
            asynInt32Mask | asynFloat64Mask | asynUInt32DigitalMask, 
            asynInt32Mask | asynFloat64Mask | asynUInt32DigitalMask,
            ASYN_CANBLOCK | ASYN_MULTIDEVICE, 
            1, // autoconnect
            0, 0)  // Default priority and stack size
{
    int axis;
	double encoderRatio[4];
    asynStatus status;
    HytecMotorAxis *pAxis;
    static const char *functionName = "HytecMotorController";

	encoderRatio[0] = encoderRatio0;
	encoderRatio[1] = encoderRatio1;
	encoderRatio[2] = encoderRatio2;
	encoderRatio[3] = encoderRatio3;

    // Check the validity of the arguments
    // Is Interrupt Vector Valid...
    if (vector <= 0 || vector > 255) 
    {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
            "%s:%s: illegal interrupt vector number. must be in range [1..255]\n",
            driverName, functionName);
//	   printf("ERROR! illegal interrupt vector number %d !\n", vector);
    }

    if (numAxes < 1 ) numAxes = 1;
    this->numAxes = numAxes;
    this->card  = cardnum;
    this->ip_carrier = ip_carrier;
    this->ipslot = ipslot;
    this->vector = vector & 0xFF;
    this->ip_carrier = ip_carrier;
	this->useencoder = useencoder;
    
    // Create controller-specific parameters
    createParam(HytecPowerControlString,	asynParamInt32,  &this->HytecPowerControl_);
    createParam(HytecBrakeControlString,    asynParamInt32,  &this->HytecBrakeControl_);
    createParam(HytecFirmwareString,   		asynParamInt32,  &this->HytecFWVersion_);
    createParam(HytecMoveAllString,    		asynParamInt32,  &this->HytecMoveAll_);

    if ((status = SetupCard()) != asynSuccess) 
    {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
            "%s:%s: setup motor IP card failed.\n",
            driverName, functionName);
    }

    for (axis=0; axis<numAxes && status == asynSuccess; axis++ ) 
    {
    	pAxis = new HytecMotorAxis(this, axis, encoderRatio[axis], vector & 0xFF);
    }
    
	// create a queue task for dealing interrupts
    this->intMsgQId = epicsMessageQueueCreate(MAX_MESSAGES, (this->numAxes + 2)*sizeof(int));
    if (epicsThreadCreate("drvHy8601intQueuedTask",
                         epicsThreadPriorityLow,
                         epicsThreadGetStackSize(epicsThreadStackMedium),
                         (EPICSTHREADFUNC)intQueuedTask,
                         (void *) this) == NULL)
    {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
            "%s:%s: drvHy8601intQueuedTask epicsThreadCreate failure.\n",
            driverName, functionName);
    }

  	startPoller(movingPollPeriod, idlePollPeriod, 2);
}

// set up IP card
asynStatus HytecMotorController::SetupCard()
{
    char *regbase;
	int st;
    static const char *functionName = "SetupCard";

    /* Register the card in A16 address space */
    regbase=(char*)ipmBaseAddr( this->ip_carrier,
			 this->ipslot,
			 ipac_addrIO);

    /* If failed to register card... */
    if (regbase == NULL) 
    {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
            "%s:%s: Cannot register Hy8601 A16 device.\n",
            driverName, functionName);
		return asynError;
    }

    /* Memory Check OK, so check PROM */
    /* JSC. checkprom returns 1 = ok, 0 = failed. */
    if (checkprom((char*)regbase+PROM_OFFS, PROM_MODEL)!=1) 
    {
	   return asynError;
    }

    /* Setup Interrupts. */
    st=ipmIntConnect(this->ip_carrier,
			 this->ipslot,
			 this->vector,
			 &ISR_8601,
			 (long)this);
      
    /* If Interrupts NOT Setup OK */
    if (st!=OK) 
    {
        asynPrint(this->pasynUserSelf, ASYN_TRACE_ERROR, 
            "%s:%s: intConnect ERROR.\n",
            driverName, functionName);
	   return asynError;
    }

    /* Setup Carrier Card */
    ipmIrqCmd(this->ip_carrier,
	      this->ipslot,
	      0,
	      ipac_irqEnable);

    this->regbase = regbase;
    return asynSuccess;
}

/**********************************************************

    NAME:	checkprom()

    Description:

    Function -	
	1. Check for "VITA4 " string. 
	2. Check for Hytec ID.
	3. Check for IP Model (0x8601).
	4. Display Debug Information.
	5. Return whether Check PROM Successful.
 
    Parameters:

    Inputs:         
	
	pr       - The PROM base address (1st address of PROM).
	expmodel - Expected IP Model as a Hex Value (i.e. 0x8601) 
	verbose  - TRUE = Display Debug Information as you go.

    Outputs:        None.
    Return:      (ishytec && ismodel)   

	TRUE  - Everything OK.
	FALSE - Either the Hytec ID or IP Model number is wrong.


    Notes:

    Programmer(s):  Darrell Nineham
                    Walter Scott            
                    Steve Hunt

**********************************************************

Revision History
----------------

Date            By      Version     Descripition
----            --      -------     ------------
29-MAR-2011     JSC     1.1.1.0     Intial Version.
    
**********************************************************/
int HytecMotorController::checkprom(char *pr,int expmodel)
{
	#define PR_ID    0x06
	#define PR_MODN  0x0a
	#define PR_REVN  0x0c
	#define PR_DRID1 0x10
	#define PR_DRID2 0x12
	#define PR_FLAGS 0x14
	#define PR_BYTES 0x16
	#define PR_SERN  0x1a
	#define HYTECID  0x8003

	char*	hytecstr="  (HyTec Electronics Ltd., Reading, UK)";
	char	lstr[7];
	int		manid;
	epicsUInt16	modelnum, word;
	int		ishytec,ismodel,strok, ver;
  
	/* Begin */

	/* Debug - Display the passed Address */ 
	printf("PROM CONTENTS AT: %p\n",pr);
	/* Read the first 6 Characters from the passed Address */ 
	word = *((epicsUInt16*)(pr+0));
	lstr[0] = (char) (word >> 8);
	lstr[1] = (char) (word & 0xFF);
	word = *((epicsUInt16*)(pr+2));
	lstr[2] = (char) (word >> 8);
	lstr[3] = (char) (word & 0xFF);
	word = *((epicsUInt16*)(pr+4));
	lstr[4] = (char) (word >> 8);
	lstr[5] = 0x20;
	lstr[6] = 0;

	/* Compare to Expected String (i.e. "VITA4 ") */
	strok = (strcmp(lstr, IP_DETECT_STR) == 0);

	printf("PROM header: '%6s'\n",lstr);
	/* Set manid to PROM Address of the Hytec ID */
	manid    = (*((epicsUInt16*)(pr+PR_ID)) << 8) + (*((epicsUInt16*)(pr+PR_ID+2)) >> 8);
	/* Set ishytec to TRUE if Hytec ID is as expected */
	ishytec  = (manid ==HYTECID);
	/* Set modelnum to PROM Address of the IP Model */
	modelnum = *((epicsUInt16*)(pr+PR_MODN));
	/* Set ismodel to TRUE if IP Model is as expected */
	ismodel  = (modelnum==expmodel);
	
	/* Display Assorted Information from Reading the PROM */
	printf("PROM manufacturer ID: 0x%04X",manid);
	if (ishytec) printf(hytecstr);

	printf("\nPROM model #: 0x%04hx, rev. 0x%04hx, serial # %hu\n",
	modelnum,
	*((short int*)(pr+PR_REVN)),
	*((short int*)(pr+PR_SERN)));
    
	printf("PROM driver ids: 0x%04hx, 0x%04hx\n",
	*((short int*)(pr+PR_DRID1)),
	*((short int*)(pr+PR_DRID2)));

	ver = *((short int*)(pr+PR_REVN));
	ver = ver/4096*1000 + (ver % 4096)/256*100 + ((ver % 4096) % 256)/16*10 + ((ver % 4096) % 256) % 16;
    setIntegerParam( HytecFWVersion_, ver);
   
	printf("PROM flags: 0x%04hx\nPROM number of bytes used: 0x%04hx (%hd)\n\n",
	*((short int*)(pr+PR_FLAGS)),
	*((short int*)(pr+PR_BYTES)),
	*((short int*)(pr+PR_BYTES)));
	

	/* Debug - Print error message if string is NOT as expected */
	if (!strok)
	{
		printf("PROM INVALID PROM HEADER; EXPECTED '%s'\n",
		       IP_DETECT_STR);
	}
	
	/* Debug - Print error message if Hytec ID is NOT as expected */
	if (!ishytec)
	{
		printf("PROM UNSUPPORTED MANUFACTURER ID;\nPROM EXPECTED 0x%08X, %s\n",
		HYTECID,hytecstr);
	}
		
	/* Debug - Print error message if IP Model is NOT as expected */
	if(!ismodel)
	{
		printf("PROM UNSUPPORTED BOARD MODEL NUMBER: EXPECTED 0x%04hx\n",
		expmodel);
	}
	
	return (/* SCO strok && */ ismodel);
}

// report
void HytecMotorController::report(FILE *fp, int level)
{
    int axis;
    HytecMotorAxis *pAxis;
	double p, encoderp;
	int dir, hasencoder, done, hlimit, home, fault, llimit;

    fprintf(fp, "Hytec motor driver %s, numAxes=%d, carrierNo=%d, ipslot=%d\n", 
			this->portName, this->numAxes, this->ip_carrier, this->ipslot);

    if (level > 0) {
        for (axis=0; axis<this->numAxes; axis++) {
            pAxis = getAxis(axis);
	    	getDoubleParam(pAxis->axisNo_, motorPosition_, &p);
	    	getDoubleParam(pAxis->axisNo_, motorEncoderPosition_, &encoderp);
			getIntegerParam(pAxis->axisNo_, motorStatusDirection_, &dir);
			getIntegerParam(pAxis->axisNo_, motorStatusHasEncoder_, &hasencoder);
			getIntegerParam(pAxis->axisNo_, motorStatusDone_, &done);
			getIntegerParam(pAxis->axisNo_, motorStatusHighLimit_, &hlimit);
			getIntegerParam(pAxis->axisNo_, motorStatusHome_, &home);
			getIntegerParam(pAxis->axisNo_, motorStatusProblem_, &fault);
			getIntegerParam(pAxis->axisNo_, motorStatusLowLimit_, &llimit);
            fprintf(fp, "  axis %d: encoder ratio = %f\n", pAxis->axisNo_, pAxis->encoderRatio);
            fprintf(fp, "          abs position = %f\n", p);
            fprintf(fp, "          encoder position = %f\n", encoderp);
            fprintf(fp, "          direction = %d\n", dir);
            fprintf(fp, "          encoder detected = %s\n", hasencoder == 0 ? "No" : "Yes");
            fprintf(fp, "          Done bit set = %s\n", done == 0 ? "No" : "Yes");
            fprintf(fp, "          High limit hit = %s\n", hlimit == 0 ? "No" : "Yes");
            fprintf(fp, "          Low limit hit = %s\n", llimit == 0 ? "No" : "Yes");
            fprintf(fp, "          Home limit hit = %s\n", home == 0 ? "No" : "Yes");
            fprintf(fp, "          Faulty = %s\n", fault == 0 ? "No" : "Yes");
        }
    }

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

// get axis
HytecMotorAxis * HytecMotorController::getAxis(asynUser *pasynUser)
{
  	return dynamic_cast<HytecMotorAxis*>(asynMotorController::getAxis(pasynUser));
}
    
HytecMotorAxis * HytecMotorController::getAxis(int axisNo)
{
  	return dynamic_cast<HytecMotorAxis*>(asynMotorController::getAxis(axisNo));
}
    
// Int32 read interface
asynStatus HytecMotorController::readInt32(asynUser *pasynUser, epicsInt32 *value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    HytecMotorAxis *pAxis = getAxis(pasynUser);
	double dvalue;
	bool moving;
//    static const char *functionName = "readInt32";
    
    if ((function == motorPosition_) || (function == motorEncoderPosition_))
    {
		//update all values and status
		pAxis->poll(&moving);		
	    getDoubleParam(pAxis->axisNo_, function, &dvalue);
	    *value = (int)dvalue;
	}else if(function == HytecFWVersion_)
	{
	    getIntegerParam( HytecFWVersion_, value);
	}else
        // Call base class call its method (if we have our parameters check this here)
   	    status = asynPortDriver::readInt32(pasynUser, value);    
    return status;	
}

// Int32 write interface
asynStatus HytecMotorController::writeInt32(asynUser *pasynUser, epicsInt32 value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    HytecMotorAxis *pAxis = getAxis(pasynUser);
//    static const char *functionName = "writeInt32";
    
    
    /* 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 = setIntegerParam(pAxis->axisNo_, function, value);
    
    if(function == HytecPowerControl_)
	{
		if(value)
	   		CSR_SET(pAxis->chanbase, CSR_AUX1);
		else
       		CSR_CLR(pAxis->chanbase, CSR_AUX1);
	} else if(function == HytecBrakeControl_)
	{
		if(value)
       		CSR_CLR(pAxis->chanbase, CSR_AUX2);
		else
	   		CSR_SET(pAxis->chanbase, CSR_AUX2);
	} else if(function == HytecMoveAll_)
	{
		
	} else
        /* Call base class call its method (if we have our parameters check this here) */
        status = asynMotorController::writeInt32(pasynUser, value);
    
    /* Do callbacks so higher layers see any changes */
    callParamCallbacks(pAxis->axisNo_);
    return status;
}

// Float64 write interface. This is currently useless.
asynStatus HytecMotorController::writeFloat64(asynUser *pasynUser, epicsFloat64 value)
{
    int function = pasynUser->reason;
    asynStatus status = asynSuccess;
    HytecMotorAxis *pAxis = getAxis(pasynUser);
//    static const char *functionName = "writeFloat64"; 
    
    /* 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 = setDoubleParam(pAxis->axisNo_, function, value);
	
	/*
	 * The following part are not implemented in this driver but is retained here
	 * in case of future requirements.
	 */
//    if(function == motorResolution_)
//	{
	    /* Calculate the resolution. it is initialised as 1.0. 
            previous_resolution = pAxis->resolution;
            pAxis->resolution = value;
            pAxis->softLowLimit = pAxis->resolution * pAxis->softLowLimit / previous_resolution;     
            pAxis->softHighLimit = pAxis->resolution * pAxis->softHighLimit / previous_resolution;  */
//	}
//    else if(function == motorEncRatio_)
//	{	
	    /* Calculate the encoder ratio and store. Note, encoder ratio is initialised as 1/4 since 
		 * the hardware design counts 4 of each encoder pulse. encoderRatio is not used for 8601 
		 * after consulting to Ron Sluiter on 21/01/2001 
            pAxis->encoderRatio = ENCODER_HARD_RATIO * value; */
//    }
//    else if(function == motorLowLimit_)
//	{
    	// Calculate the soft low limit to engineering unit. softLowLimit is not used for 8601 
        //pAxis->softLowLimit = pAxis->resolution * value;
//	}
//    else if(function == motorHighLimit_)
//	{
    	// Calculate the soft high limit to engineering unit. softHighLimit is not used for 8601 
        //pAxis->softHighLimit = pAxis->resolution * value;
//	}
//	else
	    /* Call base class call its method (if we have our parameters check this here) */
    	status = asynMotorController::writeFloat64(pasynUser, value);
    
    /* Do callbacks so higher layers see any changes */
    pAxis->callParamCallbacks();
    return status;
}

// ISR8601 Routines to handle interrupts
static void ISR_8601(int pDrv)
{
    HytecMotorController *pController = (HytecMotorController*) pDrv;
	HytecMotorAxis * pAxis;
	volatile char * chanbase;
	epicsMessageQueueId	qID;
    int data[4];
    int axis;
	int numOfAxes;

	numOfAxes = pController->getNumAxes();
    for (axis = 0; axis < numOfAxes; axis++) 
    {	
		pAxis = pController->getAxis(axis);
		chanbase = pAxis->getChanbase();
        data[axis] = GET_REG(chanbase, REG_CSR);
        SET_REG(chanbase, REG_INTMASK, 0);             /* disable interrupt */
    }

    /* wake up INT queue */
	qID = pController->getINTMsgQID();
    if (epicsMessageQueueTrySend(qID, data, sizeof(data)) == 0)
        pController->increaseMsgSent();
    else
        pController->increaseMsgFail();
}

// Interrupt queue task
static void intQueuedTask( void *pDrv )
{
    HytecMotorController *pController = (HytecMotorController*) pDrv;
	HytecMotorAxis * pAxis;
	epicsMessageQueueId	qID;
    int data[4];
    int axis;
	int numOfAxes, ipslot, ipcarrier;

	qID = pController->getINTMsgQID();
	numOfAxes = pController->getNumAxes();
	ipslot = pController->getIPSlot();
	ipcarrier = pController->getIPCarrier();

    while(1) 
    {
        /* Wait for event from interrupt routine */
        epicsMessageQueueReceive(qID, data, sizeof(data));
                 
        for (axis = 0; axis < numOfAxes; axis++) 
        {
			pAxis = pController->getAxis(axis);
           	pController->drvHy8601GetAxisStatus( pAxis, data[axis] );
        }

        /* re-enable interrupt. for Linux IOCs */
        ipmIrqCmd(ipcarrier, ipslot, 0, ipac_irqEnable);
    }
}

// Interrupt queue task updates 
void HytecMotorController::drvHy8601GetAxisStatus( HytecMotorAxis *pAxis, int csr_data )
{
    this->lock();

    setIntegerParam( pAxis->axisNo_, motorStatusDone_, 		((csr_data & CSR_DONE) != 0 ) );
    setIntegerParam( pAxis->axisNo_, motorStatusHighLimit_, ((csr_data & CSR_MAXLMT) != 0 ) );
    setIntegerParam( pAxis->axisNo_, motorStatusHome_,    	((csr_data & CSR_HOMELMT) != 0 ) );

    if (csr_data & CSR_HOMELMT) CSR_CLR(pAxis->chanbase, CSR_HOMESTOP);                     /* after home limit is reported, clear Stop at home */

    setIntegerParam( pAxis->axisNo_, motorStatusProblem_,   ((csr_data & CSR_DRVSTAT) != 0) );
    setIntegerParam( pAxis->axisNo_, motorStatusLowLimit_,  ((csr_data & CSR_MINLMT) != 0) );
    callParamCallbacks(pAxis->axisNo_);

    this->unlock();
}

int HytecMotorController::getNumAxes()
{
	return numAxes;
}

int HytecMotorController::getIPCarrier()
{
	return ip_carrier;
}

int HytecMotorController::getIPSlot()
{
	return ipslot;
}

epicsMessageQueueId HytecMotorController::getINTMsgQID()
{
	return intMsgQId;
}

void HytecMotorController::increaseMsgSent()
{
	messagesSent++;
}

void HytecMotorController::increaseMsgFail()
{
	messagesFailed++;
}


/**************************************************************************************
 *
 *    							Motor Axis Methods
 *
 *************************************************************************************/
// Motor Axis methods
HytecMotorAxis::HytecMotorAxis(HytecMotorController *pC, int axisNo, double ratio, int vector)
  : asynMotorAxis(pC, axisNo),
    pC_(pC), vector(vector), encoderRatio(ratio)
{
	InitialiseAxis();
}

int HytecMotorAxis::getVector()
{
	return vector;
}

volatile char *HytecMotorAxis::getChanbase()
{
	return chanbase;
}


// initialise axis
asynStatus HytecMotorAxis::InitialiseAxis()
{
    volatile char *chanbase;

    // Required for the motor axis infrastructure
	this->absPosition = 0;											// initialise the software counter to 0 to be in line with the reset which clears the absolute position 
	this->desiredMove = 0;											// clear desired position 
    this->resolution = 1.0;                             			// initial resolution is 1 
    this->softLowLimit = 0.0;                           			// initialise soft low limit to 0 
    this->softHighLimit = 0.0;                          			// initialise soft high limit to 0 
	this->times = 0;

    // Initialisation specific to this model of controller
    chanbase = this->chanbase = pC_->regbase + REG_BANK_OFFS + this->axisNo_ * REG_BANK_SZ;
                        
    // Reset Card 
    CSR_SET(chanbase, CSR_RESET);
    CSR_CLR(chanbase, CSR_RESET);

	/* Note: the useencoder in configure overrides the encoder detection. So if the user decides not 
	 * to use encoder by setting the useencoder param to 0 in the configure call, even there is 
	 * an encoder present, the software will not use the encoder. If the useencoder is 1, then
	 * driver will use encoder but if the encoder is not present, no value will be written to the position register */

	this->useencoder = ((pC_->useencoder & (1<<this->axisNo_)) == 0) ? 0 : 1;
	
   /* check use encoder set by the configure */
    if(this->useencoder)
        CSR_SET(chanbase, CSR_ENCODUSE);

    /* enable interrupt, set vector */
    /* Note, all 4 axis of 8601 have been set the same vector. 
     * It is the ISR's responsibility to check which axis generates the interrupt at runtime.
     * If the mask of individual axis needs to be set differently, extra interface should 
     * be created, but they should still share the same ISR routine. */
    CSR_SET(chanbase, CSR_INTEN);
    SET_REG(chanbase,REG_INTVECTOR,this->vector);

    CSR_SET(chanbase, CSR_CRASHSTOP);
    epicsThreadSleep( 0.1 );
    CSR_CLR(chanbase, CSR_CRASHSTOP);

    return asynSuccess;
}	

// Move axis
asynStatus HytecMotorAxis::move(double position, int relative, double min_velocity, double max_velocity, double acceleration)
{
//    static const char *functionName = "moveAxis";
    asynStatus status = asynSuccess;
    double result;
    int currpos;

	if (min_velocity != 0) 
   	{
       	/* Set start speed register, min_velocity is steps/s */
       	SET_REG(this->chanbase, REG_STARTSTOPSPD, (int)floor(min_velocity));
   	}
   	if (max_velocity != 0) 
   	{
       	/* Set high speed register, max_velocity is steps/s */
       	SET_REG(this->chanbase, REG_HIGHSPD, (int)floor(max_velocity));
   	}
   	if (acceleration >= 64) 
   	{
       	/* Set ramp register, acceleration is steps/s/s */
       	SET_REG(this->chanbase, REG_RAMPRATE, (int)floor(acceleration));
   	}

   	if (relative) 
   	{
       	/* Write to step counter register abs(position)*/
       	SET_REG(this->chanbase, REG_STEPCNTLO, (abs((int)floor(position))) & 0xFFFF);
       	SET_REG(this->chanbase, REG_STEPCNTHI, (abs((int)floor(position))) >> 16); 
       	/* Write dir flag based on relative position */
       	if (position >= 0) 
       	{
	      	CSR_SET(this->chanbase, CSR_DIRECTION);
       	}else 
       	{
	      	CSR_CLR(this->chanbase, CSR_DIRECTION);
       	}
	   	this->desiredMove = position;												/* store desired position */
   	}else 
   	{
       	if(!this->useencoder) 									/* if encoder is not used, read the absolute position from register */
	   	{
	       	currpos = GET_REG(this->chanbase, REG_CURRPOSHI) << 16;
	       	currpos += GET_REG(this->chanbase, REG_CURRPOSLO);
	   	}
       	else                     								/* otherwise, current position is from the software variable */
           	currpos = (int)this->absPosition; 

	   	/* calculate steps to move */
      	result = position - currpos;
	   	this->desiredMove = result;												/* store desired position */

       	/* Write to step counter abs(result) */
       	SET_REG(this->chanbase, REG_STEPCNTLO, (abs((int)floor(result))) & 0xFFFF);
       	SET_REG(this->chanbase, REG_STEPCNTHI, (abs((int)floor(result))) >> 16); 
       	/* Write dir flag based on result */
       	if (result >= 0) 
       	{
	      	CSR_SET(this->chanbase, CSR_DIRECTION);
       	}else 
       	{
	      	CSR_CLR(this->chanbase, CSR_DIRECTION);
       	}
   	}

/* int csr = GET_REG(this->chanbase, REG_CSR); 
 printf("rel=%d askp=%d currp=%d, absp=%d  res=%d, csr=%x acce=%d minsp=%d maxsp=%d\n", relative, (int)position, (int)currpos, (int)this->absPosition, (int)result, csr, (int)floor(acceleration),  (int)floor(min_velocity),  (int)floor(max_velocity));*/  /* for debug  */

   	/* Write GO bit */
   	this->times = 0;											/* clear done bit flag at the beginning of every move */
   	CSR_SET(this->chanbase, CSR_GO);
   	SET_REG(this->chanbase,REG_INTMASK,DONE_INT);             	/* to enable interrupt */

	pC_->lock();
   	setIntegerParam(pC_->motorStatusDone_, 0);
   	callParamCallbacks();
	pC_->unlock();

    return status;
}

// Move home
asynStatus HytecMotorAxis::home(double min_velocity, double max_velocity, double acceleration, int forwards)
{
    asynStatus status = asynSuccess;
//    static const char *functionName = "homeAxis";

   	if (min_velocity != 0)  
   	{
       	/* Set start speed register, min_velocity is steps/s */
       	SET_REG(this->chanbase, REG_STARTSTOPSPD, (int)floor(min_velocity));
   	}
   	if (max_velocity != 0) 
   	{
       	/* Set high speed register, max_velocity is steps/s */
       	SET_REG(this->chanbase, REG_HIGHSPD, (int)floor(max_velocity));
   	}
   	if (acceleration != 0) 
   	{
       	/* Set ramp register, acceleration is steps/s/s */
       	SET_REG(this->chanbase, REG_RAMPRATE, (int)floor(acceleration));
   	}
   	/* Write direction bit */
   	if (forwards) 
   	{
       	CSR_SET(this->chanbase, CSR_DIRECTION);
   	}else 
   	{
       	CSR_CLR(this->chanbase, CSR_DIRECTION);
   	}
   	/* Write SH bit and start moving towards home */
   	CSR_SET(this->chanbase, CSR_HOMESTOP | CSR_JOG);
   	SET_REG(this->chanbase,REG_INTMASK,DONE_INT);             	/* to enable interrupt */

	pC_->lock();
   	setIntegerParam(pC_->motorStatusDone_, 0);
   	callParamCallbacks();
	pC_->unlock();

    return status;
}

// move velocity
asynStatus HytecMotorAxis::moveVelocity(double min_velocity, double max_velocity, double acceleration)
{
    asynStatus status = asynSuccess;
//    static const char *functionName = "moveVelocityAxis";

   	if (min_velocity != 0) 
   	{
       	/* Set start speed register, min_velocity is steps/s */
       	SET_REG(this->chanbase, REG_STARTSTOPSPD, (int)floor(min_velocity));
   	}
   	if (max_velocity != 0) 
   	{
       	/* Set high speed register, velocity is steps/s */
       	SET_REG(this->chanbase, REG_HIGHSPD, (int)floor(max_velocity));
   	}
   	if (acceleration != 0) 
   	{
       	/* XXXX: set ramp register, acceleration is steps/s/s */
       	SET_REG(this->chanbase, REG_RAMPRATE, (int)floor(acceleration));
   	}
   	/* Write direction bit */
   	if (max_velocity >= 0) 
   	{
       	CSR_SET(this->chanbase, CSR_DIRECTION);
   	} else 
   	{
       	CSR_CLR(this->chanbase, CSR_DIRECTION);
   	}
   	/* Set Jog flag */
   	CSR_SET(this->chanbase, CSR_JOG);
   	SET_REG(this->chanbase,REG_INTMASK,DONE_INT);             /* to enable interrupt */

	pC_->lock();
   	setIntegerParam(pC_->motorStatusDone_, 0);
   	callParamCallbacks();
	pC_->unlock();

    return status;
}

// stop axis
asynStatus HytecMotorAxis::stop(double acceleration )
{
    asynStatus status = asynSuccess;
//    static const char *functionName = "stopAxis";
    int value;
	unsigned int acce;

   	acce = ((unsigned int)floor(acceleration));
   	if (acce != 0) 
    {
       	value = GET_REG(this->chanbase, REG_RAMPRATE);
       	if((acce > 64) || (acce <= 0xFFFF)) 
           	value = acce;

    	/* Set ramp register, acceleration is steps/s/s */
       	SET_REG(this->chanbase, REG_RAMPRATE, value);
       	/* Clear JOG and GO bits */
       	CSR_CLR(this->chanbase, CSR_JOG | CSR_GO);
    }else
	{
   	    /* Write abort bit to 1 */
   	   	CSR_SET(this->chanbase, CSR_CRASHSTOP);
       	CSR_CLR(this->chanbase, CSR_CRASHSTOP);
	}

    return status;
}

// timer task updates
asynStatus HytecMotorAxis::poll(bool *moving)
{
    double position, error=0.0, stepMoved;
    epicsUInt16 csr;
    asynStatus status = asynSuccess;

    pC_->lock();

    /* Get position register reading */
   	position = GET_REG(this->chanbase, REG_CURRPOSHI) << 16;
   	position += GET_REG(this->chanbase, REG_CURRPOSLO);

	/* For calculating absolute position if encoder is used */
   	csr = GET_REG(this->chanbase, REG_CSR);
   	error = GET_REG(this->chanbase, REG_STEPCNTHI) << 16;
   	error += GET_REG(this->chanbase, REG_STEPCNTLO);
	
	if(this->desiredMove < 0)										/* move reversely */
	{
		if(error >= 0)
		{
			stepMoved = (this->desiredMove + error) > 0 ? this->desiredMove + error : -(this->desiredMove + error);
			this->desiredMove = -error;
		}
		else
		{
			stepMoved = (this->desiredMove) > 0 ? this->desiredMove : -(this->desiredMove);
			this->desiredMove = 0;
		}
	}
	else															/* move forward */
	{
		if(error >= 0)
		{
			stepMoved = this->desiredMove - error;
			this->desiredMove = error;
		}
		else
		{
			stepMoved = this->desiredMove;
			this->desiredMove = 0;
		}
	}

    if((csr & CSR_ENCODDET) && this->useencoder) 														/* use encoder */
	{
       	setDoubleParam( pC_->motorEncoderPosition_, floor(position*this->encoderRatio+0.5) );
		if(csr & CSR_DIRECTION)													/* forward */
		{
			if(!(csr & CSR_DONE)) 
				this->absPosition += stepMoved;
			else
			{
				if(this->times == 0)
				{
					this->absPosition += stepMoved;
					this->times = 1;											/* set done bit hit the first time */
				}
   				SET_REG(this->chanbase, REG_STEPCNTHI, 0);
   				SET_REG(this->chanbase, REG_STEPCNTLO, 0);
			}
	        setDoubleParam( pC_->motorPosition_, floor(this->absPosition+0.5) );
		}else																	/* reversed */
		{
			if(!(csr & CSR_DONE)) 
				this->absPosition -= stepMoved;
			else
			{
				if(this->times == 0)
				{
					this->absPosition -= stepMoved;
					this->times = 1;											/* set done bit hit the first time */
				}
   				SET_REG(this->chanbase, REG_STEPCNTHI, 0);
   				SET_REG(this->chanbase, REG_STEPCNTLO, 0);
			}
	        setDoubleParam( pC_->motorPosition_, floor(this->absPosition-0.5) );			
		}
	}else																		/* do not use encoder */
	{
       	setDoubleParam( pC_->motorEncoderPosition_, floor(position+0.5) );
        setDoubleParam( pC_->motorPosition_, floor(position+0.5) );
	}

/* //debug
int acc= GET_REG(this->chanbase, REG_RAMPRATE);
int lsp= GET_REG(this->chanbase, REG_STARTSTOPSPD);
int hsp=GET_REG(this->chanbase, REG_HIGHSPD);
if(this->axisNo_ == 0)
printf("%d: moved=%d, error=%d, desired=%d absp=%d csr=%x\n",this->axisNo_, (int)stepMoved, (int)error , (int)this->desiredMove, (int)this->absPosition, (int)csr); 
*/
    setIntegerParam( pC_->motorStatusDirection_,     ((csr & CSR_DIRECTION) != 0 ) );
    setIntegerParam( pC_->motorStatusHasEncoder_,    ((csr & CSR_ENCODDET) != 0) );

	*moving = (csr & CSR_DONE) != 0 ? true : false;
    setIntegerParam( pC_->motorStatusDone_,          ((csr & CSR_DONE) != 0 ) );			
    setIntegerParam( pC_->motorStatusHighLimit_, 	((csr & CSR_MAXLMT) != 0 ) );
    setIntegerParam( pC_->motorStatusHome_,    		((csr & CSR_HOMELMT) != 0 ) );
    setIntegerParam( pC_->motorStatusProblem_,       ((csr & CSR_DRVSTAT) != 0) );
    setIntegerParam( pC_->motorStatusLowLimit_,  	((csr & CSR_MINLMT) != 0) );

    callParamCallbacks();
    pC_->unlock();
    return status;
}

asynStatus HytecMotorAxis::setPosition(double position)
{
  	asynStatus status = asynSuccess;

    if(this->useencoder) 																/* use encoder */
		this->absPosition = position;												  	/* update the software absolute position, note if encoder is not present, this is not used */
	else
	{
  		SET_REG(this->chanbase, REG_CURRPOSLO, ((int)floor(position)) & 0xFFFF);
  		SET_REG(this->chanbase, REG_CURRPOSHI, ((int)floor(position)) >> 16);
	}

  	return status;
}



/**
 * Configure the Hy8601 card
 * 
 * This simply creates a HytecMotorController object and 
 * the constructor does everything for initialisation.
 * 
 * @param portName   asyn port name
 * @param numAxes    number of axis on the IP card
 * @param movingPollPeriod  The time in ms between polls when any axis is moving
 * @param idlePollPeriod    The time in ms between polls when no axis is moving 
 * @param cardnum    Arbitrary card number to assign to this controller
 * @param ip_carrier which previously configured IP carrier in the IOC
 * @param ipslot     which IP Slot on carrier card (0=A etc.) 
 * @param vector     which Interrupt Vector (0 - Find One ?) 
 *                   8601 interrupt mask is always set to 0x2000. Even though the logical AND of bits in this 
 *                   register and those in the CSR will cause the channel and then the IP module 
 *                   to generate an interrupt on IRQ0 as the graph below
 *
 *                   bit 15  14  13   12  11  10  9   8   7   6   5    4     3      2      1     0
 *                       * | * | DN | * | * | * | * | * | * | * | * | FLT | HLIM | +LIM | -LIM | * | 
 *
 *                   we only set Done bit to generate interrupt since no matter which bit of FLT, HLIM, +LIM
 *                   or -LIM is set, the Done bit is set. As such, the interrupt source is identified by the 
 *                   ISR. Note, there is no interrupt clear concept. To stop the interrupt, we need to clear 
 *                   the mask bit. This means we need to enable the mask bit to enable the interrupt every time 
 *                   after a GO command which will also knock down the Done bit.
 *
 * @param useencoder - the least 4 bits determine to the 4 axes to use encoder when set (=1) 
 * @param encoderRatio0 ~ encoderRatio3  - hardware encoder ratio. For example, a quardrature encoder would 
 * 					 have its ratio of 0.25 since every line generates 4 counts.
 * 
 */
extern "C" int Hytec8601Configure(const char *portName, 
				int numAxes,
				int movingPollPeriod, 
				int idlePollPeriod,
				int cardnum,
	          	int ip_carrier,
	          	int ipslot,
	          	int vector,
	          	int useencoder,
	          	double encoderRatio0,
	          	double encoderRatio1,
	          	double encoderRatio2,
	          	double encoderRatio3)
{
    HytecMotorController *pHytecMotorController
        = new HytecMotorController(portName, numAxes, movingPollPeriod/1000., idlePollPeriod/1000.,
				cardnum, ip_carrier, ipslot, vector, useencoder, encoderRatio0, 
				encoderRatio1, encoderRatio2, encoderRatio3);
    pHytecMotorController = NULL;
    return(asynSuccess);
}

static const iocshArg Hy8601ConfigArg0 = {"portName", iocshArgString};
static const iocshArg Hy8601ConfigArg1 = {"numAxes", iocshArgInt};
static const iocshArg Hy8601ConfigArg2 = {"movingPollPeriod", iocshArgInt};
static const iocshArg Hy8601ConfigArg3 = {"idlePollPeriod", iocshArgInt};
static const iocshArg Hy8601ConfigArg4 = {"cardnum", iocshArgInt};
static const iocshArg Hy8601ConfigArg5 = {"carrier", iocshArgInt};
static const iocshArg Hy8601ConfigArg6 = {"ipslot", iocshArgInt};
static const iocshArg Hy8601ConfigArg7 = {"vector", iocshArgInt};
static const iocshArg Hy8601ConfigArg8 = {"useencoder", iocshArgInt};
static const iocshArg Hy8601ConfigArg9 = {"encoderRatio0", iocshArgDouble};
static const iocshArg Hy8601ConfigArg10 = {"encoderRatio1", iocshArgDouble};
static const iocshArg Hy8601ConfigArg11 = {"encoderRatio2", iocshArgDouble};
static const iocshArg Hy8601ConfigArg12 = {"encoderRatio3", iocshArgDouble};

static const iocshArg * const Hy8601ConfigArgs[] =  {&Hy8601ConfigArg0,
                                                       &Hy8601ConfigArg1,
                                                       &Hy8601ConfigArg2,
                                                       &Hy8601ConfigArg3,
                                                       &Hy8601ConfigArg4,
                                                       &Hy8601ConfigArg5,
													   &Hy8601ConfigArg6,
													   &Hy8601ConfigArg7,
													   &Hy8601ConfigArg8,
													   &Hy8601ConfigArg9,
													   &Hy8601ConfigArg10,
													   &Hy8601ConfigArg11,
													   &Hy8601ConfigArg12};

static const iocshFuncDef configHy8601 = {"Hytec8601Configure", 13, Hy8601ConfigArgs};
static void configHy8601CallFunc(const iocshArgBuf *args)
{
    Hytec8601Configure(args[0].sval, args[1].ival, args[2].ival, args[3].ival, args[4].ival, 
			args[5].ival, args[6].ival, args[7].ival, args[8].ival, args[9].dval, 
			args[10].dval, args[11].dval, args[12].dval);
}

static void Hytec8601Register(void)
{
    iocshRegister(&configHy8601, configHy8601CallFunc);
}

epicsExportRegistrar(Hytec8601Register);


