#include "umacuir.h"

//  **************************************************************  
//
//  Module UMAC
//  This module contains I/O functions for the Delta TAU controller
//                   
//
//  Seva Kaplounenko, Stanford Linear Accelerator Center 
 //  6/14/06
 // 
static char module_name[] = {"UMAC"};  
//  **************************************************************  

//  INCLUDES  							
// To avoid compiling errors when using local copy of repository
// locate local copy of repository.h file and change all pathes there
// to your local copies location. Do not forget change location of repository.h
// file in the project
//#include "repository.h"  //04/24/2014
//02/28/2014 #include DELAY //adds most commonly used functions
#include <delay.h>  
#include <utility.h>
#include <formatio.h>
#include <ansi_c.h>
#include "umac.h"
#include <userint.h> 
#include "UmacLib.h"



//--------------- //  PRIVATE GLOBAL VARIABLES ------------------
//---------------------------------------------------------------
//  This table allows several devices of the same type to be
//  used in the system.
//  dev_addr, contains the addresses of opened devices
//  dev_descr, contains the device descriptors of opened devices
//  dev_count, contains the number of devices open of this type 
	
	static DWORD dev_addr[UMAC_MAX_NUM_DEV] = {-1};
	static int   dev_descr[UMAC_MAX_NUM_DEV ];
	static int   dev_count =0;
	
//----------------------------------------------------------------

	//umac driver variables
	static int m_iPmacLibID = -1000;
	
	//structure  to keep private variables
	static UMAC_PARAM_STRUCT g_umac_param[UMAC_NUM_AXIS]; //should be extended 
	//to two dimensional array for use with more then one UMAC device 
	

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

//  PRIVATE FUNCTIONS DECLARATION //

//library functions
BOOL umac_get_lib_functions(void);
BOOL OpenRuntimeLink(void);
BOOL CloseRuntimeLink(void);

//umac registration functions
DWORD umac_open_dev          (DWORD addr                        );
void  umac_close_dev         (DWORD dev_ID                      );
BOOL  umac_get_position_scale(int dev_ID, int axis, UINT *pScale);

static char log_file[128] = {"Positions.txt"};


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





//--------------------------   UMAC.C FUNCTIONS  -----------------------------


 //
//****************************************************************************
//                    --- umac.c::umac_init ---
//
//!\brief  Initializes driver and check it status
//!\param  DWORD  [in]
//!\param  int * [out]  for compatibility only
//!\param  UMAC_PARAM_STRUCT[]    [in]
//!\retval DWORD
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center
//****************************************************************************
DWORD umac_init(DWORD dev_id, int * ID, UMAC_PARAM_STRUCT pParam[])
{
    int axis = -1;
    long int scale[UMAC_NUM_AXIS+1];
    char buf[256];
    int i = 0,j = 0, n = 0;
    int retVal;
    char str[128], vs[30],ds[30];
    
    unsigned long status_x, status_y;
	BOOL res1,res2, res3;
	unsigned int mask = 1L;
	int confirm = 0;
	char result[64];
	int umac_axes_sequence[UMAC_NUM_AXIS] = {1,2,0};
   
    message("");
    message("Initializing the UMAC...");
    
    
    
    // Exit here if testing without hardware 
    #ifdef DUMMY_DEVICES
        *ID = 0;
        return 1;
    #endif 
 //add for loop for multiple UMAC devices for(){
 
    //check against device table  whether drivers are open
    if(umac_is_dev_open(dev_id))
    	stop_error_msg(module_name,"Tries to open same device");
    
    
    
    //apply default parameters to private structure
    for(i = 0; i < UMAC_NUM_AXIS; i++)
        umac_setParameters(&pParam[i],i+1);//explicitly do real copy of parameters, not a pointers of source structure,
                                            //thus we do not need to keep source structure in the memory
											
		 // Get the PMAC library functions (only 3)        
    if(OpenRuntimeLink())  
    {
    	if( (*OpenPmacDevicePtr)(dev_id) )
    	{
    		//register drive in table   //check whether the com is open      
    		dev_id = umac_open_dev(dev_id);  
    		if(dev_id == -1)
    			stop_error_msg(module_name,"currently no more than one driver allowed");
    	}
    	else
    	{
    		stop_error_msg(module_name,"Failed to open UMAC driver");
    	}
    }
    else
    {
    	stop_error_msg(module_name,"DLL library is missed!");
    }
    
    //verify library by requesting ROM version 
	retVal = (*PmacGetResponseAPtr)(dev_id,vs,10,"ver");//returns number of read chars
	vs[retVal-1] = '\0';
	printf("ROM version:%s\n",vs);
	
	//slow but safe to take from controller directly
	for(i = 0; i < UMAC_NUM_AXIS; i++)
        umac_get_position_scale( dev_id, i+1, &g_umac_param[i].position_scale_factor);
        
     //do settings from initialisation structures
     for(i = 0; i < UMAC_NUM_AXIS ; i++)
     {
     	umac_set_velocity( dev_id, i+1, g_umac_param[i].velocity); //do not convert to mm/s
     	umac_set_acceleration( dev_id, i+1, g_umac_param[i].acceleration); //do not convert to mm/s*s
     }
     
     //check whether all motors are in close loop mode 
    for(i = 0; i < UMAC_NUM_AXIS; i++)
    {
		umac_get_mot_status(dev_id, i+1, &status_x, &status_y);
			
		res1 = (status_x & (mask << MOTOR_ACTIVATED_X));
			
		res2 = !(status_x & (mask << MOTOR_OPEN_LOOP_X));
			
		res3 = (status_x & (mask << MOTOR_AMPLIFIER_ENABLED_X));
		
		if(res1 && res2 && res3)
		{
			//ready to use
			sprintf(result,"UMAC:motor:%i is ready to use",(i+1));
			message(result);
		}
		else
		{
			//do initialization
			confirm = ConfirmPopup ("ATTENTION!", "Umac is about to perform initialization\n All motors will move!");
			if(confirm)
			{
				/*//disable all plcs
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"displc2,3,4,5");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");  */
				//cvi_delay(3.0);
				delay(3.0);   //02/28/2014
				
				//03/05/2014 enable plc 10
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc10");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");
				//cvi_delay(3.0);
				delay(3.0);   //02/28/2014
				//03/05/2014 enable plc 5
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc5");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");
				//cvi_delay(3.0);
				delay(3.0);   //02/28/2014
				//04/01/2014 Set the homing mask to home z, x and y(1+2+4)
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"p810=7");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");
				//cvi_delay(3.0);
				delay(3.0);   //02/28/2014
					//03/05/2014 home
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"b1200r");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");
				//cvi_delay(3.0);
				delay(130);   //02/28/2014	/*	*/
				 
				
			/* 03/05/2014
			//run plc3 to synchronize z and w axes
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc3");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");
					//cvi_delay(5.0);
					delay(5.0); //20/28/2014
				//run plc2 (safety) and plc 4 to manage switches
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc2,4");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");
					// cvi_delay(3.0); 
					delay(3.0);   //02/28/2014
				//and finally run stages to close feedback loop
				retVal = (*PmacGetResponseAPtr)(dev_id,result,0,"enaplc5");//returns number of read chars
				if(retVal <0)
					message("initialization is failed");   */
			}
			else
			{
			}
			
			umac_exit(dev_id);
			stop_error_msg(module_name,"Please restart program after all motors stop");
			
			
		}
	}
   
    *ID = (int)dev_id;
    //cvi_delay(10.);
    
    // Move to initial positions
    for(j = 0; j < UMAC_NUM_AXIS; j++)
    {
		i = umac_axes_sequence[j]; //set a sequence for axes to move  
    	umac_abs_move(*ID,i+1, g_umac_param[i].acceleration,g_umac_param[i].velocity,g_umac_param[i].init_offset);
    	do
    	{
    		cvi_delay(2.);
    	}while(umac_AxisIsMoving(*ID,i+1));
    }
    return dev_id;
}


//
//****************************************************************************
//                    --- umac.c::umac_go_home ---
//
//!\brief  Moves  motor to home position. The velocity and acceleration are specified.
//!\param  DWORD  [in]
//!\param  int    [in]  for compatibility only
//!\param  double [in]
//!\param  double [in]
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center
//****************************************************************************
void umac_go_home(DWORD dev_ID, int axis, double acc, double vel)
{
    /* Declare variables */
    //int dev_open;
    message("The umac_go_home function is not implemented!");
    /* Done */
    return;
}

 //
//****************************************************************************
//                    --- umac.c::umac_exit ---
//
//!\brief  Closes driver
//!\param  DWORD  [in]
//!\retval  none
//!\author  Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_exit(DWORD dev_ID)
{
	BOOL res =TRUE;
	int status =0;
    
    /* Exit here if testing without hardware */
    #ifdef DUMMY_DEVICES
        return;
    #endif
    
    res = (*ClosePmacDevicePtr)(dev_ID); //chec all umac devices if use more then one driver
    umac_close_dev(dev_ID);
    
    status = ReleaseExternalModule (m_iPmacLibID);
    if(status < 0 )
    	printf("DLL unload problem:%i\n",status);
    
    /* Done */
    return;
}

//
//****************************************************************************
//                    --- umac.c::umac_open_dev ---
//
//!\brief  Checks whether device is open, and let it open if it is not opened
//!\param  DWORD
//!\retval DWORD
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center 
//**************************************************************************** 

DWORD umac_open_dev(DWORD addr)
{
    /* Declare variables */
    int i;
    int dev_ID = -1;
    int descr =1;
    int err;
    
    /* Initialization */
    dev_ID = -1;

    /* Check to see if the device is already in the device table */
    for (i = 0; i < UMAC_MAX_NUM_DEV; i++)			   //UMAC_MAX_NUM_DEV=1 usually 03/28/2014
    {												   
        if (dev_addr[i] == addr)
        {
            dev_ID = addr;
            return dev_ID;
        }
    }
    
    /* Find an ID for the device */
    for (i = 0; i < UMAC_MAX_NUM_DEV; i++)
    {
        if (dev_addr[i] == -1)
        {
            dev_addr[i] = addr;
            dev_ID = addr; 
            dev_descr[i] = descr;
    		dev_count++;
            break;
        }
    }
     
    /* If an entry could not be opened in the device table, return an error */
    if (dev_ID < 0)
    {
        stop_error_msg(module_name,"The umac device could not be opened");
        return  -1;
    }
    
    /* Return device ID */
    return dev_ID;
}

//
//****************************************************************************
//                    --- umac.c::umac_close_dev ---
//
//!\brief  Does record at device table upon closing
//!\param  DWORD
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center 
//****************************************************************************
void umac_close_dev(DWORD addr)
{
	int i = 0;
    /* Close the device */
    for (i = 0; i < UMAC_MAX_NUM_DEV; i++)
    {
        if (dev_addr[i] == addr)
        {
            dev_addr[i] = -1;
            break;
        }
    }
    return;
}


//
//****************************************************************************
//                    --- umac4.c::umac_is_dev_open ---
//
//!\brief   Checks whether device is open using address
//!\param  int
//!\retval  int
//!\author  Seva Kaplounenko, Stanford Linear Accelerator Center 
//**************************************************************************** 
int umac_is_dev_open(DWORD dev_address)
{
    int i = 0;
    for (i = 0; i < UMAC_MAX_NUM_DEV; i++)
    {
        if (dev_addr[i] == dev_address)
        {
            return 1;
        }
    }
    return 0;
}

//
//****************************************************************************
//                    --- umac.c::umac_homing ---
//
//!\brief  returns 0 if not moving, othewise returns 1 
//!\param  int - device id      [in]
//!\param  int - axis           [in] 
//!\param  int - direction      [in] 
//!\retval int    - 1 negative limit hit, 2 - positive, 0 - none
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center 
//****************************************************************************
int umac_homing(DWORD dev_ID, int axis,int homing_direction)
{
	int retVal =0;
    /* Declare variables */
   message("The function umac_homing is not impemented, motor moves to zero position instead");
   umac_abs_move( dev_ID, axis,g_umac_param[axis-1].acceleration,g_umac_param[axis-1].home_velocity,0.0);
    
        
   /* Check for command errors */
    //umac_read_error_status(dev_ID);
    return retVal;
}


//
//****************************************************************************
//                    --- umac.c::umac_moveToLimit ---
//
//!\brief   returns 0 if not moving, othewise returns 1 
//!\param  int - device id      [in]
//!\param  int - axis           [in]
//!\param  int - direction      [in]
//!\param  int - acceleration   [in]
//!\param  int - velocity       [in]
//!\retval int    
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center 
//****************************************************************************
int umac_moveToLimit(DWORD dev_ID, int axis,int moving_direction, double acc, double vel, double step, double offset)
{
    int retVal =0;
    /* Declare variables */
   message("The function umac_moveToLimit is not impemented, motor moves to zero position instead");
   umac_abs_move( dev_ID, axis,g_umac_param[axis-1].acceleration,g_umac_param[axis-1].home_velocity,0.0);
    
        
   /* Check for command errors */
    //umac_read_error_status(dev_ID);
    return retVal;
}


//
//****************************************************************************
//                    --- umac.c::umac_LimitSwitchesStatus ---
//
//!\brief  Returns limit switches status
//!\param  int - device id      [in]
//!\param  int - axis           [in] 
//!\retval int    - 1 negative limit hit, 2 - positive, 0 - none
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center 
//****************************************************************************
int umac_LimitSwitchesStatus(DWORD dev_ID, int axis)
{
    /* Declare variables */
    unsigned long status_x, status_y;
    int retVal = 0;
    unsigned int temp = 0;
    unsigned int mask = 1L;
    
    umac_get_mot_status(dev_ID, axis, &status_x, &status_y);
    
	if((status_x & (mask << MOTOR_POSITIVE_LIMIT_SET_X)))
		return 2;
	if((status_x & (mask << MOTOR_NEGATIVE_LIMIT_SET_X)))
		return 1;
    
    return 0;
}

//
//****************************************************************************
//                    --- umac.c::umac_getParameters ---
//
//!\brief   Get parameters from private umac_param_struct
//!\param   LPUMAC_PARAM_STRUCT  [out]
//!\retval  none
//!\author  Seva Kaplounenko, Stanford Linear Accelerator Center 
//****************************************************************************
void umac_getParameters(LPUMAC_PARAM_STRUCT pParam, int axis)
{
    int idx = 0;
    if(axis >  UMAC_NUM_AXIS || axis <  1)
    {
        stop_error_msg(module_name,"axis number out of range");
    }
    idx = axis - 1;
    pParam->cs_counts_per_mm			= g_umac_param[idx].cs_counts_per_mm			;
    pParam->position_scale_factor		= g_umac_param[idx].position_scale_factor		;
    pParam->velocity               		= g_umac_param[idx].velocity               		;
    pParam->acceleration           		= g_umac_param[idx].acceleration           		;
    
    
    pParam->home_acceleration           = g_umac_param[idx].home_acceleration           ;
    pParam->axis_on                     = g_umac_param[idx].axis_on                     ;
    pParam->home_velocity               = g_umac_param[idx].home_velocity               ;
    pParam->home_acceleration           = g_umac_param[idx].home_acceleration           ;  
    pParam->home_direction              = g_umac_param[idx].home_direction              ;  
}

//
//****************************************************************************
//                    --- umac.c::umac_setParameters ---
//
//!\brief  Set parameters of umac_param_struct
//!\param  LPUMAC_PARAM_STRUCT  [in]
//!\retval none
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_setParameters(LPUMAC_PARAM_STRUCT pParam, int axis)
{
int idx = 0;
    if(axis >  UMAC_NUM_AXIS || axis <  1)
    {
        stop_error_msg(module_name,"axis number out of range");
    }
    idx = axis - 1;
    g_umac_param[idx].cs_counts_per_mm            = pParam->cs_counts_per_mm            ;
    g_umac_param[idx].position_scale_factor       = pParam->position_scale_factor       ;
    g_umac_param[idx].velocity               	  = pParam->velocity               		;
    g_umac_param[idx].acceleration           	  = pParam->acceleration           		;
    g_umac_param[idx].init_offset           	  = pParam->init_offset          		;
    
    g_umac_param[idx].home_acceleration           = pParam->home_acceleration           ;
    g_umac_param[idx].axis_on                     = pParam->axis_on                     ;
    g_umac_param[idx].home_velocity               = pParam->home_velocity               ;
    g_umac_param[idx].home_acceleration           = pParam->home_acceleration           ;  
    g_umac_param[idx].home_direction              = pParam->home_direction              ; 
}


//
//****************************************************************************
//                    --- umac.c::umac_get_lib_functions ---
//
//!\brief   Gets pointers to DLL functions
//!\param   none
//!\retval  BOOL
//!\author  Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_get_lib_functions(void)
{
	int status =0;											 //take this function name from manual (PCOMMPRO2 Software reference manual 03/28/2014)
	OpenPmacDevicePtr = GetExternalModuleAddr (m_iPmacLibID, "OpenPmacDevice", &status);
	 
	 if (OpenPmacDevicePtr == NULL)
	 {
	 	printf("Error at %s:status=%i\n", "OpenPmacDevice",status);
	 	return FALSE;
	 }
	 
	 //PmacGetResponseAPtr = GetExternalModuleAddr (m_iPmacLibID, "PmacGetResponseA", &status);
	 PmacGetResponseAPtr = GetExternalModuleAddr (m_iPmacLibID, "PmacGetResponseA", &status);   
	 if (PmacGetResponseAPtr == NULL)
	 {
	 	printf("Error at %s:status=%i\n", "PmacGetResponseA",status);
	 	return FALSE;
	 }
	 
	 ClosePmacDevicePtr = GetExternalModuleAddr (m_iPmacLibID, "ClosePmacDevice", &status);
	 
	 if (ClosePmacDevicePtr == NULL)
	 {
	 	printf("Error at %s:status=%i\n", "ClosePmacDevice",status);
	 	return FALSE;
	 }
	 
	 
	return TRUE;
}

//
//****************************************************************************
//                    --- umac.c::OpenRuntimeLink ---
//
//!\brief  Opens link to dll library
//!\param  none
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL OpenRuntimeLink(void)
{
	m_iPmacLibID = LoadExternalModuleEx (DRIVERNAME, __CVIUserHInst);
	if(m_iPmacLibID > 0)
	{
		return umac_get_lib_functions(); 
		//return TRUE;
	}
	else
		return FALSE;
}

//
//****************************************************************************
//                    --- umac.c::CloseRuntimeLink ---
//
//!\brief   Closes link to dll library 
//!\param  none
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL CloseRuntimeLink(void)
{
	int status = 0;
	status = ReleaseExternalModule (m_iPmacLibID);
	if(status < 0)
		return FALSE;
	else
		return TRUE;
}

//
//****************************************************************************
//                    --- umac.c::umac_command ---
//
//!\brief  Sends text command an get reply
//!\param  DWORD  [in]
//!\param  char * [out] 
//!\param  int	  [in]
//!\param  char*  [in]
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_command(DWORD id,char * result , int rlength, char * command )
{
	int retVal = -1;

	retVal = (*PmacGetResponseAPtr)(id,result,rlength,command);//returns number of read chars
	if(retVal <=0)
		return FALSE;
	result[retVal-1] = '\0';
	//for debug purposes only
	//printf("%s\n",result);
	return TRUE;
}

//
//****************************************************************************
//                    --- umac.c::umac_get_axis ---
//
//!\brief  Returns current motor number
//!\param  DWORD  [in]
//!\retval int
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
int umac_get_axis( DWORD id)
{
	char result[10];
	int retVal = -1;
#ifdef DUMMY_DEVICES
	return 1;
#endif

	retVal = (*PmacGetResponseAPtr)(id,result,10,"#");//returns number of read chars
	if(retVal <=0)
		return -1;
	return atoi(result);
}


//
//****************************************************************************
//                    --- umac.c::umac_set_axis ---
//
//!\brief  Sets current motor number
//!\param  DWORD  [in]
//!\param  int  [in] 
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_set_axis( DWORD id , int axis)
{
	char result[10];
	char command[20];
	int retVal = -1;

	if(axis > UMAC_NUM_AXIS || axis < 1)
	{
		message("umac: axis number out of range");
		return ;
	}
	sprintf(command,"# %i",axis);
	retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars
	if(retVal <0)
		message("umac_set_axis failed!");
	
}


//
//****************************************************************************
//                    --- umac.c::umac_get_velocity ---
//
//!\brief  Returns motor velocity
//!\param  DWORD  [in]
//!\param  int    [in]  
//!\retval double
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
double umac_get_velocity( DWORD id, int motor)
{
	char result[16];
	int retVal = -1;
	char command[32];
#ifdef DUMMY_DEVICES
	return 1.;
#endif	
	switch(motor)
	{
		case umac_z: 
			strcpy(command,"p203");
			break;
		case umac_x:
			strcpy(command,"p204");
			break;
		case umac_y:
			strcpy(command,"p205");
			break;
		default:
			message("get_velocity:No such axis!");
			return 0;
			break;
	}

	retVal = (*PmacGetResponseAPtr)(id,result,16,command);//returns number of read chars
	if(retVal <=0)
		return -1;
	return atof(result)/1000.;
}


//
//****************************************************************************
//                    --- umac.c::umac_set_velocity ---
//
//!\brief  Sets motor velocity [mm/s]
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double    [in] 
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_set_velocity( DWORD id, int motor, double velocity)
{
	char result[16];
	int retVal = -1;
	char command[32];
	char abs_command[32];
	double abs_velocity;
	int iabs_velocity;
	
	abs_velocity = velocity*g_umac_param[motor-1].cs_counts_per_mm; // counts/ms
	iabs_velocity =  (int)abs_velocity;
	sprintf(abs_command,"i%i22=%i",motor,iabs_velocity);
	
	velocity*=1000.;//convert to mm
	
	switch(motor)
	{
		case umac_z:
			sprintf(command,"p203=%.3f",velocity);
			break;
		case umac_x:
			sprintf(command,"p204=%.3f",velocity); 
			break;
		case umac_y:
			sprintf(command,"p205=%.3f",velocity); 
			break;
		default:
			message("set_velocity:No such axis!");
			break;
	}

	retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars
	if(retVal <0)
		message("umac_set_velocity failed");
		
	retVal = (*PmacGetResponseAPtr)(id,result,0,abs_command);//returns number of read chars
	if(retVal <0)
		message("umac_set_velocity failed"); 
	
}

//
//****************************************************************************
//                    --- umac.c::umac_get_acceleration ---
//
//!\brief   Returns motor acceleration
//!\param  DWORD  [in]
//!\param  int    [in]  
//!\retval double
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
double umac_get_acceleration( DWORD id, int motor)
{
	char result[16];
	int retVal = -1;
	char command[32];
	
	#ifdef DUMMY_DEVICES
	return 0.001;
	#endif
	
	switch(motor)
	{
		case umac_z: 
			strcpy(command,"p206");
			break;
		case umac_x:
			strcpy(command,"p207");
			break;
		case umac_y:
			strcpy(command,"p208");
			break;
		default:
			message("get_velocity:No such axis!");
			return 0;
			break;
	}

	retVal = (*PmacGetResponseAPtr)(id,result,16,command);//returns number of read chars
	if(retVal <=0)
		return -1;
	return atof(result)/1000.;
}


//
//****************************************************************************
//                    --- umac.c::umac_set_acceleration ---
//
//!\brief  Sets motor acceleration
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double [in] 
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_set_acceleration( DWORD id, int motor, double acceleration)
{
	char result[16];
	int retVal = -1;
	char command[32];
	char abs_command[32];
	double abs_acceleration;
	int iabs_acceleration;
	
	abs_acceleration = acceleration*g_umac_param[motor-1].cs_counts_per_mm; 
	abs_acceleration/=1000.;  // counts/ms^2

	sprintf(abs_command,"i%i19=%f",motor,abs_acceleration);
	
	acceleration*=1000.;//convert to mm
	switch(motor)
	{
		case umac_z:
			sprintf(command,"p206=%.3f",acceleration);
			break;
		case umac_x:
			sprintf(command,"p207=%.3f",acceleration); 
			break;
		case umac_y:
			sprintf(command,"p208=%.3f",acceleration); 
			break;
		default:
			message("set_velocity:No such axis!");
			break;
	}

	retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars
	if(retVal <0)
		message("umac_set_acceleration failed");
		
	retVal = (*PmacGetResponseAPtr)(id,result,0,abs_command);//returns number of read chars
	if(retVal <0)
		message("umac_set_acceleration failed");
	
}


//
//****************************************************************************
//                    --- umac.c::umac_get_distance ---
//
//!\brief  Returns motor relative distance to move
//!\param  DWORD  [in]
//!\param  int    [in]  
//!\retval double
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
double umac_get_distance( DWORD id, int motor)
{
	char result[16];
	int retVal = -1;
	char command[32];
	
	#ifdef DUMMY_DEVICES
	return 0.011;
#endif
	
	switch(motor)
	{
		case umac_z: 
			strcpy(command,"p200");
			break;
		case umac_x:
			strcpy(command,"p201");
			break;
		case umac_y:
			strcpy(command,"p202");
			break;
		default:
			message("get_velocity:No such axis!");
			return 0;
			break;
	}

	retVal = (*PmacGetResponseAPtr)(id,result,16,command);//returns number of read chars
	if(retVal <=0)
		return -1;
	return atoi(result)/1000.;
}

//
//****************************************************************************
//                    --- umac.c::umac_set_distance ---
//
//!\brief   Sets corresponding motor relative distance to move
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double [in] 
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_set_distance( DWORD id, int motor, double distance)
{
	char result[16];
	int retVal = -1;
	char command[32];
	
	distance*=1000.;//convert to mm
	switch(motor)
	{
		case umac_z:
			sprintf(command,"p200=%.3f",distance);
			break;
		case umac_x:
			sprintf(command,"p201=%.3f",distance); 
			break;
		case umac_y:
			sprintf(command,"p202=%.3f",distance); 
			break;
		default:
			message("set_distance:No such axis!");
			break;
	}

	retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars
	if(retVal <0)
		message("umac_set_distance failed");
	
}


//
//****************************************************************************
//                    --- umac.c::umac_get_mot_position ---
//
//!\brief   Returns actual motor position
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double [out] 
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_get_mot_position(DWORD dev_ID, int axis, double* pos)
{
    char result[16];
	int retVal = -1;
	char command[32];
	double position=0;
	
#ifdef DUMMY_DEVICES
	return 1;
#endif
	
//	distance*=1000.;//convert to mm
	switch(axis)
	{
		case umac_z:
			sprintf(command,"#1p");
			break;
		case umac_x:
			sprintf(command,"#3p"); 
			break;
		case umac_y:
			sprintf(command,"#2p"); 
			break;
		default:
			message("set_distance:No such axis!");
			return FALSE;
			break;
	}

	retVal = (*PmacGetResponseAPtr)(dev_ID,result,10,command);//returns number of read chars
	if(retVal <0)
	{
		message("umac_get_position is failed");
		return FALSE;
	}
	
	*pos = atof(result); // [counts]
	(*pos) /= g_umac_param[axis-1].cs_counts_per_mm; //[mm]
	(*pos) /= 1000.; // [m ]
	return TRUE;
}


//
//****************************************************************************
//                    --- umac.c::umac_get_position_scale ---
//
//!\brief   Finds scale applied to axis
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  UINT [out] 
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_get_position_scale(int dev_ID, int axis, UINT *pScale)
{
    char result[16];
	int retVal = -1;
	char command[32];
	double distance=0;
	
//	distance*=1000.;//convert to mm
	switch(axis)
	{
		case umac_z:
			sprintf(command,"i108");
			break;
		case umac_x:
			sprintf(command,"i208"); 
			break;
		case umac_y:
			sprintf(command,"i308"); 
			break;
		default:
			message("set_distance:No such axis!");
			return FALSE;
			break;
	}

	retVal = (*PmacGetResponseAPtr)(dev_ID,result,10,command);//returns number of read chars
	if(retVal <0)
	{
		message("umac_get_position_scale is failed");
		return FALSE;
	}
	
	*pScale = atoi(result) * 32; //5 bits offset
	return TRUE;
		
}




//
//****************************************************************************
//                    --- umac.c::umac_rel_move ---
//
//!\brief  Moves axis to specified distance [m]
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double [in] 
//!\param  double [in]
//!\param  double [in]
//!\retval BOOL 
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_rel_move(DWORD dev_ID, int axis, double acceleration, double velocity, double distance)
{
	char result[16];
	int retVal = -1;
	char command[32];
	BOOL res = FALSE;
	double position = 0.0;
	
	
	umac_set_distance( dev_ID, axis,distance); 
	umac_set_velocity( dev_ID, axis,velocity);
	umac_set_acceleration( dev_ID, axis,acceleration);
	switch(axis)
	{
		case umac_z:
			sprintf(command,"b4r");
			break;
		case umac_x:
			sprintf(command,"b5r"); 
			break;
		case umac_y:
			sprintf(command,"b6r"); 
			break;
		default:
			message("set_distance:No such axis!");
			return FALSE;
			break;
	}

	retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars
	if(retVal <0)
	{
		message("umac_rel_move is failed");
		return FALSE;
	}
	
	return TRUE;
}

//
//****************************************************************************
//                    --- umac.c::umac_AxisIsMoving ---
//
//!\brief  returns 0 if not moving, othewise returns 1 
//!\param  int - device id      [in]
//!\param  int - axis           [in] 
//!\retval int
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center 
//****************************************************************************
BOOL umac_AxisIsMoving(DWORD dev_ID, int axis)
{
    unsigned long status_x, status_y;
	
	BOOL res1,res2, res3;
	unsigned int mask = 1L;
	
	umac_get_mot_status(dev_ID, axis, &status_x, &status_y);
			
	res1 = !(status_y & (mask << MOTOR_INPOSITION_Y));
			
	res2 = !(status_x & (mask << MOTOR_DESIRED_VELOCITY0_X));
			
	res3 = (status_x & (mask << MOTOR_MOVE_TIMER_ACTIVE_X));
		
	return res1 && res2 && res3;
}




//
//****************************************************************************
//                    --- umac.c::umac_abs_move ---
//
//!\brief  Moves axis to specified position [m]
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double [in] 
//!\param  double [in]
//!\param  double [in]
//!\retval BOOL 
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_abs_move(DWORD dev_ID, int axis, double acceleration, double velocity, double position)
{
    char result[16];
	int retVal = -1;
	char command[32];
	BOOL res = FALSE;
	
	
	umac_set_velocity( dev_ID, axis,velocity);
	umac_set_acceleration( dev_ID, axis,acceleration);
	
	sprintf(command,"#%i j=%f",axis, (position*1000.)*g_umac_param[axis-1].cs_counts_per_mm);
	

	retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars
	if(retVal <0)
	{
		message("umac_abs_move is failed");
		return FALSE;
	}
	
	return TRUE;
}

//
//****************************************************************************
//                    --- umac.c::umac_get_mot_status ---
//
//!\brief  Returns motor status
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  unsigned long * [out]
//!\param  unsigned long * [out]
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_get_mot_status(DWORD dev_ID, int axis, unsigned long * status_x, unsigned long * status_y)
{
    char result[16];
	int retVal = -1;
	char command[32];
	unsigned long mask = 1L;
	unsigned long test = 0L;
	
	BOOL res;
	
	unsigned long xreg = 0L, yreg = 0L; //corresponds to  registers X and Y on DELTA TAU controller
	unsigned char sxreg[14], syreg[14]; //and corresponding to them strings
	unsigned char shxreg[14], shyreg[14]; //and corresponding to them strings

	


	sprintf(command,"#%i?",axis);
		
	//03/19/2014
	//retVal = (*PmacGetResponseAPtr)(dev_ID,result,16,command);//returns number of read chars
	retVal = (*PmacGetResponseAPtr)(dev_ID,result,16,command);//returns number of read chars
	result[retVal-1] = '\0';
	if(retVal <0)
	{
		message("umac_get_mot_status is failed");
		return FALSE;
	}
	
	 strncpy(sxreg,result,6);
	 sxreg[6] = '\0';
	 strcpy(syreg,&result[6]);
	 syreg[6] = '\0';
	 
	 sprintf(shxreg,"0x%s",sxreg);
	 sprintf(shyreg,"0x%s",syreg);
	 
	 sscanf(shxreg,"%x",&xreg);
	 sscanf(shyreg,"%x",&yreg);
	 
	 *status_x = xreg;
	 *status_y = yreg;
	return TRUE;
	
}


//
//****************************************************************************
//                    --- umac.c::umac_get_cs_mot_status ---
//
//!\brief  Returns coordinate system status
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  unsigned long * [out]
//!\param  unsigned long * [out]
//!\param  unsigned long * [out]
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_get_cs_status(DWORD dev_ID, int cs, unsigned long * status_x, unsigned long * status_y1, unsigned long * status_y2)
{
    char result[24];
	int retVal = -1;
	char command[32];
	unsigned long mask = 1L;
	unsigned long test = 0L;
	
	BOOL res;
	
	unsigned long xreg = 0L, y1reg = 0L,y2reg = 0L ; //corresponds to  registers X and Y on DELTA TAU controller
	unsigned char sxreg[14], sy1reg[14], sy2reg[14]; //and corresponding to them strings
	unsigned char shxreg[14], shy1reg[14],shy2reg[14]; //and corresponding to them strings

	


	sprintf(command,"&%i??",cs);
		

	retVal = (*PmacGetResponseAPtr)(dev_ID,result,20,command);//returns number of read chars
	if(retVal <0)
	{
		message("umac_get_cs_status is failed");
		return FALSE;
	}
	
	 strncpy(sxreg,result,6);
	 sxreg[6] = '\0';
	 strncpy(sy1reg,&result[6],6);
	 sy1reg[6] = '\0';
	 strncpy(sy2reg,&result[12],6);
	 sy2reg[6] = '\0';
	 
	 sprintf(shxreg,"0x%s",sxreg);
	 sprintf(shy1reg,"0x%s",sy1reg);
	 sprintf(shy2reg,"0x%s",sy2reg);
	 
	 sscanf(shxreg,"%x",&xreg);
	 sscanf(shy1reg,"%x",&y1reg);
	 sscanf(shy2reg,"%x",&y2reg);
	 
	 *status_x = xreg;
	 *status_y1 = y1reg;
	 *status_y2 = y2reg;
	return TRUE;
	
}

//
//****************************************************************************
//                    --- umac.c::umac_set_z_break ---
//
//!\brief  Sets Z break in manual mode
//!\param  DWORD  [in]
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_set_z_break_manual( DWORD id, BOOL state)
{
	char result[16];
	int retVal = -1;
	char command[32];
	
	if(state)
	{
		strcpy(command,"q2=1");
	}
	else
	{
		strcpy(command,"q2=0");
	}
			

	retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars
	if(retVal <=0)
		return FALSE;
	return TRUE;
}



//
//****************************************************************************
//                    --- umac.c::umac_zero ---
//
//!\brief  This function sets the indexer position counter to zero.
//!\param  DWORD  [in]
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_zero(DWORD dev_ID, int axis)
{
    char result[16];
	int retVal = -1;
	char command[32];
	
	BOOL confirmation = TRUE;
	
	confirmation = ConfirmPopup ("Reset axis zero position!", "Are you sure to set current position to be zero?");
	
	if(confirmation)
	{
		sprintf(command,"#%ihmz",axis);
	
		retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars
		if(retVal <=0)
			return FALSE;
	}
	return TRUE;
}


//
//****************************************************************************
//                    --- umac.c::umac_set_position ---
//
//!\brief  This function sets the indexer position counter to given position
//!\param  DWORD  [in]
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_set_position(DWORD dev_ID, int axis, double position)
{
    /* Declare variables */
    int err;
    int dev_open;

    /* Check input parameters */
    stop_error_msg(module_name,"umac_set_position is not implemented yet");
    
    /* Done */
    return;
}

//
//****************************************************************************
//                    --- umac.c::umac_set_controller_zero ---
//
//!\brief  Sets zero offset at controller and request reboot if position value changes for more than 1 micron
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double [in]
//!\retval BOOL
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
BOOL umac_set_controller_zero (DWORD dev_ID, int axis,double position)
{
	int retVal = 0;
	BOOL needRestart = FALSE;
	
	char result[16];
	
	char command[32];
	char command1[32];
	double cur_position; //in mm
	
	
	position*=1000.; //convert to mm 
	
	switch(axis)
	{
		case umac_z:
			sprintf(command,"q9=%.3f",position);
			sprintf(command1,"q9");
			break;
		case umac_x:
			sprintf(command,"q10=%.3f",position);
			sprintf(command1,"q10");
			break;
		case umac_y:
			sprintf(command,"q11=%.3f",position);
			sprintf(command1,"q11");
			break;
		default:
			message("set_distance:No such axis!");
			break;
	}

	retVal = (*PmacGetResponseAPtr)(dev_ID,result,15,command1);//returns number of read chars
	if(retVal <0)
		message("umac_move_position_set_zero failed");
		
	cur_position = atof(result); //mm /g_umac_param[axis-1].cs_counts_per_mm;
	if( fabs(cur_position - position) > 0.001)
	{
		needRestart = TRUE;
		retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars
		if(retVal <0)
			message("umac_move_position_set_zero failed");
			
		//kill motor	
		sprintf(command,"#%ik",axis);
		retVal = (*PmacGetResponseAPtr)(dev_ID,result,0,command);//returns number of read chars
		if(retVal <0)
			message("umac_move_position_set_zero failed");
	}
	return needRestart;
}


//
//****************************************************************************
//                    --- umac.c::add_log ---
//
//!\brief   Adds string to log file
//!\param  char[]
//!\retval  none
//!\author   Seva
//****************************************************************************
void umac_add_log(char buf[], BOOL newline)
{
    char header[] = {"UMAC: "};
    char bufOut[512];
    FILE* file_ptr;
    
    file_ptr = fopen(log_file, "a");
    if (file_ptr == NULL)
    {
        message("Unable to open log file");
        return;
    }
    
    // Write the values to the log file 
    if(newline)
        fprintf(file_ptr, "\n%s: %s",header, buf);
    else
        fprintf(file_ptr, "\t %s", buf);
    
    fclose(file_ptr);
}


//
//****************************************************************************
//                    --- umac.c::umac_set_soft_positive_limit ---
//
//!\brief   Sets software positive limit switch in counts, set 0 to disable it
//!\param  DWORD  [in]
//!\param  int    [in]
//!\param  double [in] - mm
//!\retval void
//!\author Seva Kaplounenko, Stanford Linear Accelerator Center  
//****************************************************************************
void umac_set_soft_positive_limit( DWORD id, int motor, double limit)
{
	char result[32];
	int retVal = -1;
	char command[64];
	int nCounts = 0;
	
	switch(motor)
	{
		case umac_z:
			nCounts = g_umac_param[0].cs_counts_per_mm * limit;
			sprintf(command,"i0113=%I",nCounts);
			break;
		case umac_x:
			nCounts = g_umac_param[1].cs_counts_per_mm * limit;
			sprintf(command,"i0213=%i",nCounts); 
			break;
		case umac_y:
			nCounts = g_umac_param[2].cs_counts_per_mm * limit;
			sprintf(command,"i0313=%I",nCounts); 
			break;
		default:
			message("set_distance:No such axis!");
			break;
	}

	retVal = (*PmacGetResponseAPtr)(id,result,0,command);//returns number of read chars
	if(retVal <0)
		message("umac_set_soft limit swich is failed");
	
}