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

//
//  Module XPS - C8
//  This module contains functions to work with Newport XPS controller.
//
//  Yurii Levashov $Seva
//  11/06/2005
 

//  **************************************************************  
 static char module_name[] = {"XPC C8"};  
//  **************************************************************  //

//  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"
///#include DELAY //adds most commonly used functions
 
//#include <delay.h> 

#include "delay.h" 

#include <analysis.h>
#include <userint.h>
#include <formatio.h>
#include <utility.h>
#include <ansi_c.h>


#include "XPS_Q8_drivers.h"
#include "XPS_C8.h"
#include "XPS_C8param.h"


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

//  PRIVATE GLOBAL VARIABLES  
static char msg[300];
static int server_connect = FALSE;
static char log_file[512];

//  *****************************************************************************************************************  
 
static XPS_PARAM    gXPC_param;//       [XPS_MAX_NUM_DEV  + 1];
static XPS_2DGROUP  g_XPS2Dparam    /*[XPS_MAX_NUM_DEV]*/ [XPS_NUM_OF_2D_GROUPS];  //can be extended for multiple devices
static int dev_addr                 [XPS_MAX_NUM_DEV  + 1] = {-1};
static int dev_descr                [XPS_MAX_NUM_DEV  + 1] = {-1};
static int dev_count = 0;

static int group_mode[2] = {0};
static double gUfintEntryScaleAdjustment = 1.;
static double gUfintExitScaleAdjustment =  1.; 


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

//  PRIVATE FUNCTIONS 
int xps_c8_connect(char IPAddress[], int iPort);
int xps_c8_get_version(int SocketID, char buffer[]);
void xps_c8_disconnect(int SocketID);
void DisplayErrorAndClose(int error, int SocketID, char *APIName);
void xps_c8_kill_group(int SocketID, char *pGroup);
void xps_c8_move_group_init(int SocketID, char *pGroup);
void xps_c8_stop_motion(int SocketID, char *pGroup);
int xps_c8_connect(char IPAddress[], int iPort);
double xps_c8_GroupHomeSearch (int SocketID, int group);
int xps_c8_GetParameters (int SocketID, char * pGroup, double * vel, double * acc, double * minJerk, double * maxJerk);
int xps_c8_SetParameters (int SocketID, int group);
int xps_c8_SetParameters1 (int SocketID, int nGroup);

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


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

//                    --- xps_c8.c::xps_c8_checkID ---
//
//!\brief   Verifies whether device open
//!\param  int  [in]
//!\retval  int
//!\author  Seva & Yurii 
//  *****************************************************************************************************************  

int xps_c8_checkID(int id)
{
    int i = 0;
    for(i = 0; i < dev_count; i++)
        if(dev_addr[i] == id)
            return i;
    return -1;
}

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

//                    --- xps_c8.c::xps_c8_init ---
//
//!\brief   Initializes motion controller.
//!\param  int *  [out]
//!\retval  int
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

int xps_c8_init(int * dev_id)  // dev_number is the param structure number in array for future use
{
    int err = 0;
    char buffer[256] = {'\0'};
    int n = 0;
    int result = -1;
	
	XPS_PARAM      xps_param;
    XPS_2DGROUP    xps_group_param[2];

    
    
    // Exit here if testing without hardware
    #ifdef DUMMY_DEVICES
        dev_addr[dev_count]  = 1;
        dev_descr[dev_count] = dev_count;
        dev_count++;
        *dev_id = 0;
        return dev_count;
    #endif
		
		
		xps_c8_fill_param_struct(&xps_param, xps_group_param);
    	//and send them to global variable at driver
    	xps_c8_set_param( xps_param, xps_group_param);
		
    
    // if using multiple controllers should check for available index in array(the one that have dev_addr=-1 in array)
    // check, if communication with server established
    dev_addr[dev_count] = xps_c8_connect(gXPC_param.pIPAddress, gXPC_param.nPort); //dev_addr = tcp id of controller
    if(dev_addr[dev_count] == -1) 
    {
        stop_error_msg(module_name, "Unable initialize communication port.");
        return -1;
    }
    // Check if communication still works 
    else if(xps_c8_get_version(dev_addr[dev_count], buffer) != 0)  // If problem, try to connect again
    {
        dev_addr[dev_count] = xps_c8_connect(gXPC_param.pIPAddress, gXPC_param.nPort);
        if(dev_addr[dev_count] == -1) 
        {
            stop_error_msg(module_name,"Unable initialize communication port.");
            return -1;
        }
     }
        
     //for multiple devices should check al descriptors before adding the new one
     dev_descr[dev_count] = dev_count; //dev_descr(iptor) shows device id location in array 
     *dev_id = dev_addr[dev_count];//return new id to application
     dev_count++;
     message("XPS C8 initialized.");
     message("XPS C8 Homing...");
     
     for(n = 0; n < XPS_NUM_OF_2D_GROUPS; n++)
     {
        result = xps_c8_SetParameters(*dev_id,n);
        //do home search
        g_XPS2Dparam[n].position =   xps_c8_GroupHomeSearch (*dev_id, n);
        //Set moving parameters
        //result = xps_c8_SetParameters(*dev_id,n);
     }
     return dev_count;
}
    
    
//
//  *****************************************************************************************************************  

//                    --- xps_c8.c::xps_c8_connect ---
//
//!\brief   Opens connection to server.
//!\param  char *  [in]
//!\param  int  [in] 
//!\retval  int
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

int xps_c8_connect(char IPAddress[], int iPort)
{
    double dTimeOut = 600;
    int SocketID = -1;
    
    SocketID = TCP_ConnectToServer(IPAddress, iPort, dTimeOut);
    xps_c8_get_version(SocketID, msg);
    if (SocketID == -1)
     {
      sprintf (msg, "Connection to @ %s, port = %ld failed\n", IPAddress, iPort);
      stop_error_msg(module_name,msg);
     }
     else  
      {
       server_connect = TRUE;
       message("Connected to XPS server.");
      }
 return SocketID;
    
} 

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

//                    --- xps_c8.c::xps_c8_get_version ---
//
//!\brief   Get hardware version
//!\param  int  [in]
//!\param  char *  [in] 
//!\retval  int
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

int xps_c8_get_version(int SocketID, char buffer[])
{
    int err = 0;
    
    // Get and return controller version
    err = FirmwareVersionGet (SocketID, buffer);
    if(err != 0)
       DisplayErrorAndClose(err, SocketID, "FirmwareVersionGet");
       
  return err;
}


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

//                    --- xps_c8.c::DisplayErrorAndClose ---
//
//!\brief   Displays errors
//!\param  int  [in]
//!\param  int  [in] 
//!\param  char *  [in] 
//!\retval  void
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

void DisplayErrorAndClose(int error, int SocketID, char *APIName)
{
    int err = 0;
    char err_string[260];
    
    // If error occurred other than Timeout error
    if (error != -2)
     {
    // Error => Get error description
      err = ErrorStringGet(SocketID, error, err_string);

    // If error occurred with the API ErrorStringGet
       if (err != 0)
        {
    // Display API name, error code and ErrorStringGet error code
         Fmt(msg,"%s< ErrorStringGet ERROR = %d", err);
         message(msg);
        }
        else
         {
       // Display API name, number and description of the error
          Fmt(msg, "%s< ERROR in %s: %d %s", APIName, error, err_string);
          message(msg);
         }
     } 
    else
     {
    // Timeout error
      Fmt(msg,"%s < ERROR => %s: TCP timeout %d", APIName, error);
      stop_error_msg(module_name,msg);
     }
    // Close TCP socket
    xps_c8_disconnect(SocketID);
    stop_error_msg(module_name,"Disconnected from server");
}

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

//                    --- xps_c8.c::xps_c8_kill_group ---
//
//!\brief   Kill motion of specified group of stages. 
//!\param  int
//!\param  char *
//!\retval  void
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

void xps_c8_kill_group(int SocketID, char *pGroup)
{
    int err;
    err = GroupKill(SocketID, pGroup);
     if(err != 0)  DisplayErrorAndClose(err, SocketID, "GroupKill");    
 return;
}

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

//                    --- xps_c8.c::xps_c8_stop_motion ---
//
//!\brief   Stop motion of specified group of stages. 
//!\param  int
//!\param  char *
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  

void xps_c8_stop_motion(int SocketID, char *pGroup)
{
    int err;
    
    err = GroupMoveAbort(SocketID, pGroup);
    if(err != 0)  DisplayErrorAndClose(err, SocketID, "GroupMoveAbort");
 return;
}

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

//                    --- xps_c8.c::xps_c8_move_abs ---
//
//!\brief   Move group to specified position
//!\param  int
//!\param  int
//!\param  double 
//!\param  double[2]
//!\retval  int
//!\author  Seva
//  *****************************************************************************************************************  

int xps_c8_move_abs(int SocketID, int group, double position , double positions[2]) 
{
   int err = 0;
    double d[2];
    
    //convert distance to mm and use right direction
    position *= -1000.;
    
    d[0] = position*g_XPS2Dparam[group].nScale1;
    d[1] = position*g_XPS2Dparam[group].nScale2;
    err = GroupMoveAbsolute(SocketID, g_XPS2Dparam[group].Group, 2, d);
    if(err != 0)  DisplayErrorAndClose(err, SocketID, "GroupMoveAbsolute");
    
    xps_c8_get_position(SocketID, group, positions);
    return err;
}



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

//                    --- xps_c8.c::xps_c8_move_rel ---
//
//!\brief   Move group relative
//!\param  int
//!\param  int
//!\param  double
//!\param  double[2]
//!\retval  int 
//!\author  Seva
//  *****************************************************************************************************************  

int xps_c8_move_rel(int SocketID, int group, double distance, double positions[2])
{
    int err = 0;
    double d[2];
    
    //convert distance to mm
    distance *= -1000.;
    
    d[0] = distance*g_XPS2Dparam[group].nScale1;
    d[1] = distance*g_XPS2Dparam[group].nScale2;
    
    err = GroupMoveRelative(SocketID, g_XPS2Dparam[group].Group, 2, d);
    if(err != 0) DisplayErrorAndClose(err, SocketID, "GroupMoveReletive");
    
  //return position
   xps_c8_get_position(SocketID, group, positions);
   // d[0]distance *= -1000.;
   // d[1]distance *= -1000.;
   return err;
}

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

//                    --- xps_c8.c::xps_c8_move_group_init ---
//
//!\brief   Kill motion group of stages and do home search
//!\param  int
//!\param  char * 
//!\retval  void
//!\author  Seva & Yurii
//  *****************************************************************************************************************  
void xps_c8_move_group_init(int SocketID, char *pGroup)
{
   int err = 0;
   int status = 0;
   
   // kill group
//   xps_c8_kill_group(SocketID, pGroup);
    err = GroupKill(SocketID, pGroup);
     if(err != 0)  DisplayErrorAndClose(err, SocketID, "GroupKill");
               
   //initialize group
//   xps_c8_init_group(SocketID, pGroup);
    err = GroupInitialize (SocketID, pGroup);
     if(err != 0) DisplayErrorAndClose(err, SocketID, "FirmwareVersionGet");
               
   // search group home
   err = GroupHomeSearch(SocketID, pGroup);
    if(err != 0) DisplayErrorAndClose(-1, SocketID, "GroupHomeSearch");
    
   // wait until home search done
   while(status != 11)     // homing done
    {
     if(status != -1)
        err = GroupStatusGet(SocketID, pGroup, &status);
    
    cvi_delay(2.0);//08/20/2014 AM
	 //delay(2.0);  
    }
    return;
}

//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_disconnect ---
//
//!\brief   Release server
//!\param  int
//!\retval  void
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

void xps_c8_disconnect(int SocketID)
{
    // TCP / IP disconnection
    TCP_CloseSocket(SocketID); // Close Socket
    server_connect = FALSE;
}

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

//                    --- xps_c8.c::xps_c8_get_position ---
//
//!\brief   Get current stage position
//!\param  int
//!\param  int 
//!\param  double[] 
//!\retval  int
//!\author  Seva 
//  *****************************************************************************************************************  

int xps_c8_get_position(int SocketID, int group, double pos[2])
{
    int err = 0;
#ifndef DUMMY_DEVICES
    err = GroupPositionCurrentGet(SocketID, g_XPS2Dparam[group].Group, 2, pos);
    if(err != 0) DisplayErrorAndClose(err, SocketID, "GroupPositionCurrentGet");
    
    //convert back to meters
    pos[0]/=(-1000.);
    pos[1]/=(-1000.);
#else
    pos[0] = 0.;
    pos[1] = 0.;
#endif
    return err;
}

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

//                    --- xps_c8.c::xps_c8_exit ---
//
//!\brief   Release device
//!\param  int
//!\retval  void
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

void xps_c8_exit(int devID)
{
    int i = 0;
    // disconnect from server
    i=xps_c8_checkID(devID);
    if(i>=0) 
    {
        xps_c8_disconnect(devID);
        dev_count--;
        dev_addr[i] = -1;
        dev_descr[i] = -1;
        message("XPS C8 Disconnected");
    }
    return;
}

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

//                    --- xps_c8.c::xps_c8_GroupHomeSearch ---
//
//!\brief   Do home search
//!\param  int
//!\param  int
//!\retval  double
//!\author  Seva & Yurii
//  *****************************************************************************************************************  

double xps_c8_GroupHomeSearch (int SocketID, int group)
{
    int err = 0;
   int status = 0;
   char buf[128];
   double p[2];
   
   // kill group
    err = GroupKill(SocketID, g_XPS2Dparam[group].Group);
     if(err != 0)  DisplayErrorAndClose(err, SocketID, "GroupKill");
               
   //initialize group
    err = GroupInitialize (SocketID, g_XPS2Dparam[group].Group);
     if(err != 0) DisplayErrorAndClose(err, SocketID, "FirmwareVersionGet");
               
   // search group home, should run in separate thread
   err = GroupHomeSearch(SocketID, g_XPS2Dparam[group].Group);
    if(err != 0) DisplayErrorAndClose(-1, SocketID, "GroupHomeSearch");
  //TODO move these two lines to the command thread  
    
    
   // and then wait until home search done by checking status
   //but here we just check errors
   err = GroupStatusGet(SocketID, g_XPS2Dparam[group].Group, &status);
   switch(status)
   {
        case 11:
            sprintf(buf,"%s homing is completed",g_XPS2Dparam[group].Group);
            message(buf); 
            break;
        case -1:
            message("Error:Socket was busy");
        default:
            message("Undocumented error");
    }
   
    xps_c8_get_position(SocketID, group, p);
    return p[0];
}


//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_GetParameters ---
//
//!\brief   Set group parameters
//!\param  int
//!\param  char *
//!\param  double
//!\param  double 
//!\param  double 
//!\param  double 
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  

int xps_c8_GetParameters (int SocketID, char * pGroup, double * pVel, double * pAcc, double * pMinJerk, double * pMaxJerk)
{
    int result =0;
    int n = 0;
    result = PositionerSGammaParametersGet(SocketID, pGroup,pVel,pAcc, pMinJerk,pMaxJerk);
    if(result == 0)
    {
        for(n = 0; n < XPS_NUM_OF_2D_GROUPS; n++)
        {
            if(strcmp(g_XPS2Dparam [n].Group ,pGroup) == 0)
            {
                g_XPS2Dparam [n].acceleration   =   *pAcc       ;
                g_XPS2Dparam [n].velocity       =   *pVel       ;
                g_XPS2Dparam [n].min_j_time     =   *pMinJerk   ;
                g_XPS2Dparam [n].max_j_time     =   *pMaxJerk   ; 
            }
        }
    }
    return result;
}

//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_SetParameters ---
//
//!\brief   Set group parameters
//!\param  int
//!\param  char *
//!\param  double
//!\param  double 
//!\param  double 
//!\param  double 
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  
int xps_c8_SetParameters (int SocketID, int nGroup)
{
    
    double vel,acc,minJerk,maxJerk;
    int res = 0;
    
    int result1 = 0, result2, result = 0;
 
    result1 = PositionerSGammaParametersSet
    (
        SocketID,
        g_XPS2Dparam [nGroup].Axis1,
        g_XPS2Dparam [nGroup].velocity,
        g_XPS2Dparam [nGroup].acceleration,
        g_XPS2Dparam [nGroup].min_j_time,
        g_XPS2Dparam [nGroup].max_j_time
    );
    
    res =xps_c8_GetParameters (SocketID, g_XPS2Dparam [nGroup].Axis1, &vel, &acc,  &minJerk, &maxJerk);
    
    result2 = PositionerSGammaParametersSet
    (
        SocketID,
        g_XPS2Dparam [nGroup].Axis2,
        g_XPS2Dparam [nGroup].velocity,
        g_XPS2Dparam [nGroup].acceleration,
        g_XPS2Dparam [nGroup].min_j_time,
        g_XPS2Dparam [nGroup].max_j_time
    );
    
    res =xps_c8_GetParameters (SocketID, g_XPS2Dparam [nGroup].Axis2, &vel, &acc,  &minJerk, &maxJerk);
    
    result = result1&&result2;
    if(result)
        stop_error_msg(module_name,"Parameters Error!");
    return result;
}



//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_SetParameters1 ---
//
//!\brief   Set group parameters
//!\param  int
//!\param  char *
//!\param  double
//!\param  double 
//!\param  double 
//!\param  double 
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  
int xps_c8_SetParameters1 (int SocketID, int nGroup)
{
    
    double vel,acc,minJerk,maxJerk;
    int res = 0;
    
    int result1 = 0, result2, result = 0;
 
    result1 = PositionerSGammaParametersSet
    (
        SocketID,
        g_XPS2Dparam [nGroup].Axis1,
        g_XPS2Dparam [nGroup].velocity1,
        g_XPS2Dparam [nGroup].acceleration1,
        g_XPS2Dparam [nGroup].min_j_time1,
        g_XPS2Dparam [nGroup].max_j_time1
    );
    
    res =xps_c8_GetParameters (SocketID, g_XPS2Dparam [nGroup].Axis1, &vel, &acc,  &minJerk, &maxJerk);
    
    result2 = PositionerSGammaParametersSet
    (
        SocketID,
        g_XPS2Dparam [nGroup].Axis2,
        g_XPS2Dparam [nGroup].velocity1,
        g_XPS2Dparam [nGroup].acceleration1,
        g_XPS2Dparam [nGroup].min_j_time1,
        g_XPS2Dparam [nGroup].max_j_time1
    );
    
    res =xps_c8_GetParameters (SocketID, g_XPS2Dparam [nGroup].Axis2, &vel, &acc,  &minJerk, &maxJerk);
    
    result = result1&&result2;
    if(result)
        stop_error_msg(module_name,"Parameters Error!");
    return result;
}



//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_set_param ---
//
//!\brief   Set parameters
//!\param  XPS_PARAM        [in]
//!\param  XPS_2DGROUP []   [in] 
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  
void xps_c8_set_param( XPS_PARAM nParam, XPS_2DGROUP group_param[])
{
    int n = -1;
    gXPC_param.pIPAddress       =       nParam.pIPAddress;
    gXPC_param.nPort            =       nParam.nPort;
 
    //Groups definitions

    //XY:XX Group ---------
    for(n = 0; n < XPS_NUM_OF_2D_GROUPS; n++)
    {
        strcpy(g_XPS2Dparam [n].Group   ,           group_param[n].Group)       ;
        g_XPS2Dparam        [n].acceleration    =   group_param[n].acceleration ;
        g_XPS2Dparam        [n].velocity        =   group_param[n].velocity     ;
        g_XPS2Dparam        [n].min_j_time      =   group_param[n].min_j_time   ;
        g_XPS2Dparam        [n].max_j_time      =   group_param[n].max_j_time   ; 
        
        g_XPS2Dparam        [n].acceleration1   =   group_param[n].acceleration1;
        g_XPS2Dparam        [n].velocity1       =   group_param[n].velocity1    ;
        g_XPS2Dparam        [n].min_j_time1     =   group_param[n].min_j_time1  ;
        g_XPS2Dparam        [n].max_j_time1     =   group_param[n].max_j_time1  ; 
        
        g_XPS2Dparam        [n].offset          =   group_param[n].offset       ;

        //First axis definitions
        strcpy(g_XPS2Dparam [n].Axis1           ,   group_param[n].Axis1)       ;
        g_XPS2Dparam        [n].nScale1         =   group_param[n].nScale1      ;
        g_XPS2Dparam        [n].nScale1_copy    =   group_param[n].nScale1_copy ;

        //Second axis definitions
        strcpy(g_XPS2Dparam [n].Axis2           ,   group_param[n].Axis2)       ;
        g_XPS2Dparam        [n].nScale2         =   group_param[n].nScale2      ;
        g_XPS2Dparam        [n].nScale2_copy    =   group_param[n].nScale2_copy ;
		g_XPS2Dparam        [n].group_mode		=	group_param[n].group_mode	;
     } 
}

//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_set_param ---
//
//!\brief   Get parameters
//!\param  XPS_PARAM        [out]
//!\param  XPS_2DGROUP []   [out] 
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  
void xps_c8_get_param( LPXPS_PARAM pParam, XPS_2DGROUP pGroup_param[])
{
    int n = -1;
    pParam->pIPAddress      =       gXPC_param.pIPAddress;
    pParam->nPort           =       gXPC_param.nPort;
 
    //Groups definitions

    //XY:XX Group ---------
    for(n = 0; n < XPS_NUM_OF_2D_GROUPS; n++)
    {
        strcpy(pGroup_param [n].Group   ,           g_XPS2Dparam[n].Group)      ;
        pGroup_param        [n].acceleration    =   g_XPS2Dparam[n].acceleration;
        pGroup_param        [n].velocity        =   g_XPS2Dparam[n].velocity    ;
        pGroup_param        [n].min_j_time      =   g_XPS2Dparam[n].min_j_time  ;
        pGroup_param        [n].max_j_time      =   g_XPS2Dparam[n].max_j_time  ; 
        
        pGroup_param        [n].acceleration1   =   g_XPS2Dparam[n].acceleration1;
        pGroup_param        [n].velocity1       =   g_XPS2Dparam[n].velocity1    ;
        pGroup_param        [n].min_j_time1     =   g_XPS2Dparam[n].min_j_time1 ;
        pGroup_param        [n].max_j_time1     =   g_XPS2Dparam[n].max_j_time1  ; 
        
        pGroup_param        [n].offset          =   g_XPS2Dparam[n].offset      ;

        //First axis definitions
        strcpy(pGroup_param [n].Axis1           ,   g_XPS2Dparam[n].Axis1)      ;
        pGroup_param        [n].nScale1         =   g_XPS2Dparam[n].nScale1     ;
        pGroup_param        [n].nScale1_copy    =   g_XPS2Dparam[n].nScale1_copy;

        //Second axis definitions
        strcpy(pGroup_param [n].Axis2           ,   g_XPS2Dparam[n].Axis2)      ;
        pGroup_param        [n].nScale2         =   g_XPS2Dparam[n].nScale2     ;
        pGroup_param        [n].nScale2_copy    =   g_XPS2Dparam[n].nScale2_copy;
     } 
}

//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_getMovingTime ---
//
//!\brief   Calculates moving time for given distance to move
//!\param  int       [in]
//!\param  int       [in]
//!\param  double    [in] 
//!\retval  double
//!\author  Seva
//  *****************************************************************************************************************  
double xps_c8_getMovingTime(int SocketID, int group, double distance )
{
    double vel;
    double acc;
    double time = 0;
    double dist;
    double retVal = 0;
    
    if(fabs(g_XPS2Dparam[group].nScale1) >= fabs(g_XPS2Dparam[group].nScale2))
    {
        vel = fabs(g_XPS2Dparam[group].velocity*g_XPS2Dparam[group].nScale1);
        acc = fabs(g_XPS2Dparam[group].acceleration*g_XPS2Dparam[group].nScale1);
        dist= fabs(distance*g_XPS2Dparam[group].nScale1);
    }
    else
    {
        vel = fabs(g_XPS2Dparam[group].velocity*g_XPS2Dparam[group].nScale2);
        acc = fabs(g_XPS2Dparam[group].acceleration*g_XPS2Dparam[group].nScale2);
        dist= fabs(distance*g_XPS2Dparam[group].nScale2);
    }
    
    time = (2.*vel)/acc + ((dist*1000. - (vel*vel)/acc))/vel;
    time += 2.*g_XPS2Dparam[group].max_j_time;
    return time;
}

/* 
//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_set_group_mode ---
//
//!\brief   Set axes motion mode in group. Do not change axes scale
//!\param  int       [in]
//!\param  int       [in]
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  
void xps_c8_set_group_mode(int group,int mode)
{
    if(group >= XPS_NUM_OF_2D_GROUPS)
        stop_error_msg(module_name,"Group number exceed max value");
    switch(mode)
    {
        case MODE_1P2P: //both moving positive
            g_XPS2Dparam[group].nScale1 =   fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1N2N: //both moving negative
            g_XPS2Dparam[group].nScale1 =   -fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   -fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1P2N: //first moving positive second moving negative direction
            g_XPS2Dparam[group].nScale1 =   fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   -fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1N2P: //first moving negative second moving positive direction
            g_XPS2Dparam[group].nScale1 =   -fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1P2S: //first moving positive second do not move
            g_XPS2Dparam[group].nScale1 =   fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   0.0;
            break;
        case MODE_1N2S: //first moving negative second do not move
            g_XPS2Dparam[group].nScale1 =   -fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   0.0;
            break;
        case MODE_1S2P: //first do not move second moving positive direction
            g_XPS2Dparam[group].nScale1 =   0.0;
            g_XPS2Dparam[group].nScale2 =   fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1S2N:  //first do not move second moving negative direction
            g_XPS2Dparam[group].nScale1 =   0.0;
            g_XPS2Dparam[group].nScale2 =   -fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        default:
            g_XPS2Dparam[group].nScale1 =   0.0; //something wrong
            g_XPS2Dparam[group].nScale2 =   0.0; //do not move
        break;
    }
    group_mode[group] = mode;
}

 */



 

					   //To test second integral
//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_set_group_mode ---
//
//!\brief   Set axes motion mode in group. Do not change axes scale
//!\param  int       [in]
//!\param  int       [in]
//!\retval  void
//!\author  Seva
//  *****************************************************************************************************************  
void xps_c8_set_group_mode(int group,int mode)
{
    if(group >= XPS_NUM_OF_2D_GROUPS)
        stop_error_msg(module_name,"Group number exceed max value");
    switch(mode)
    {
        case MODE_1P2P: //both moving positive
            g_XPS2Dparam[group].nScale1 =   fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1N2N: //both moving negative
            g_XPS2Dparam[group].nScale1 =   -fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   -fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1P2N: //first moving positive second moving negative direction
            g_XPS2Dparam[group].nScale1 =   fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   -fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1N2P: //first moving negative second moving positive direction
            g_XPS2Dparam[group].nScale1 =   -fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
			
        case MODE_1P2S: //first moving positive second do not move
            g_XPS2Dparam[group].nScale1 =   gUfintEntryScaleAdjustment * fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =   -gUfintExitScaleAdjustment * fabs(g_XPS2Dparam[group].nScale1_copy); 
            break;
        case MODE_1N2S: //first moving negative second do not move
            g_XPS2Dparam[group].nScale1 =   -gUfintEntryScaleAdjustment * fabs(g_XPS2Dparam[group].nScale1_copy);
            g_XPS2Dparam[group].nScale2 =    gUfintExitScaleAdjustment*fabs(g_XPS2Dparam[group].nScale1_copy); 
            break;
        case MODE_1S2P: //first do not move second moving positive direction
            g_XPS2Dparam[group].nScale1 =   -gUfintExitScaleAdjustment * fabs(g_XPS2Dparam[group].nScale2_copy); 
            g_XPS2Dparam[group].nScale2 =   gUfintEntryScaleAdjustment * fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        case MODE_1S2N:  //first do not move second moving negative direction
            g_XPS2Dparam[group].nScale1 =  gUfintExitScaleAdjustment * fabs(g_XPS2Dparam[group].nScale2_copy); 
            g_XPS2Dparam[group].nScale2 = -gUfintEntryScaleAdjustment * fabs(g_XPS2Dparam[group].nScale2_copy);
            break;
        default:
            g_XPS2Dparam[group].nScale1 =   0.0; //something wrong
            g_XPS2Dparam[group].nScale2 =   0.0; //do not move
        break;
    }
    group_mode[group] = mode;
}

 
 












//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_get_group_mode ---
//
//!\brief   Get axes motion mode for group
//!\param  int       [in]
//!\retval  int
//!\author  Seva
//  *****************************************************************************************************************  
int xps_c8_get_group_mode(int group)
{
    if(group >= XPS_NUM_OF_2D_GROUPS)
        stop_error_msg(module_name,"Group number exceed max value");
    return group_mode[group];
}

//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_move_axis_abs ---
//
//!\brief   Move specified axis in group to specified position
//!\param  int
//!\param  int
//!\param  int 
//!\param  double 
//!\param  double*
//!\retval  int
//!\author  Seva
//  *****************************************************************************************************************  
int xps_c8_move_axis_abs(int SocketID, int group, int axis, double position , double * pPosition) 
{
   int err;
    double d[2];
    int axis1 = 0;
    double positions[2];
    double scale = 0.;
    
    if(axis == 0)
    {
        axis1 = 1;
        g_XPS2Dparam[group].nScale1;
    }
    else if(axis == 1)
    {
        axis1 = 0;
        scale = g_XPS2Dparam[group].nScale2;
    }
    else
        stop_error_msg(module_name,"Wrong axis number in group1");
    
    //convert distance to mm and use right direction
    position *= -1000.;
    
    xps_c8_get_position(SocketID, group, positions);
    
    d[axis1] = positions[axis1];;
    d[axis] = position*scale;
    err = GroupMoveAbsolute(SocketID, g_XPS2Dparam[group].Group, 2, d);
    if(err != 0)  DisplayErrorAndClose(err, SocketID, "GroupMoveAbsolute");
    
    xps_c8_get_position(SocketID, group, positions);
    *pPosition = positions[axis];
    return err;
}

//
//  *****************************************************************************************************************  
//                    --- xps_c8.c::xps_c8_move_rel_both ---
//
//!\brief   Move relative both axes in group regardless default configuration 
//!\param  int
//!\param  int
//!\param  double
//!\param  double[2]
//!\retval  int 
//!\author  Seva
//  *****************************************************************************************************************  
int xps_c8_move_rel_both(int SocketID, int group, double distance, double positions[2])
{
    int err = 0;
    double d[2];
    int modes[2];
    
    //convert distance to mm
    distance *= -1000.;
    
     //save group settings
    modes[0] = xps_c8_get_group_mode(group); //move both axes in group
    //modes[1] = xps_c8_get_group_mode(1); //move both axes in group
    
    //set both axes moving
    xps_c8_set_group_mode(group,g_XPS2Dparam[group].group_mode/*MODE_1P2P*/); //move both axes in group
    //xps_c8_set_group_mode(1,MODE_1P2P); //move both axes in group
    
    d[0] = distance*g_XPS2Dparam[group].nScale1;
    d[1] = distance*g_XPS2Dparam[group].nScale2;
    
    err = GroupMoveRelative(SocketID, g_XPS2Dparam[group].Group, 2, d);
    if(err != 0) DisplayErrorAndClose(err, SocketID, "GroupMoveReletive");
    
  //return position
   xps_c8_get_position(SocketID, group, positions);
   
   //set both axes states back
    xps_c8_set_group_mode(group,modes[0]); //move both axes in group
    //xps_c8_set_group_mode(1,modes[1]); //move both axes in group
   
   return err;
}


void xps_set_slow(BOOL slow, int dev_id)
{
    int n = 0; 
    int result = 0;
    for(n = 0; n < XPS_NUM_OF_2D_GROUPS; n++)
     {
         if(slow)
            result = xps_c8_SetParameters1(dev_id,n);
         else
             result = xps_c8_SetParameters(dev_id,n);
     }
}

void xps_c8_set_ufint_scales(double entry_scale, double exit_scale)
{
	gUfintEntryScaleAdjustment = entry_scale; // 1./entry_scale;
	gUfintExitScaleAdjustment  = exit_scale;
}