program XPS_trajectoryScan("P=13BMC:,R=traj1,IPADDR=164.54.160.34,PORT=5001,"
                           "USERNAME=Administrator,PASSWORD=Administrator,"
                           "M1=m1,M2=m2,M3=m3,M4=m4,M5=m5,M6=m6,M7=m7,M8=m8,"
                           "GROUP=g1,"
                           "P1=p1,P2=p2,P3=p3,P4=p4,P5=p5,P6=p6,P7=p7,P8=p8")

/*  
 *  This sequencer program works with trajectoryScan.db.  It implements
 *  coordinated trajectory motion with the Newport XPS-C8 motor controller.
 *  It can be used with the Newport General Purpose Diffractometer or with any
 *  other set of motors connected to that controller.
 *  
 *  Original author: Jon Kelly, based on Mark Rivers' version for the MM4000.
 *  Current author:  Mark Rivers
 *
 * Modifications:
*/

%% #include <string.h>
%% #include <stdio.h>
%% #include <math.h>
%% #include "XPS_C8_drivers.h"
%% #include "Socket.h"
%% #include "xps_ftp.h"


/* This program must be compiled with the recursive option */
option +r;

/* Maximum # of trajectory elements.  The number of points XPS can accept
 * is almost unlimited because the data is stored in a file but the channel 
 * access limit with a double data type is 2000 on R3.13 clients.
 */
#define MAX_ELEMENTS 2000

/* Maximum # of output pulses.  At the moment the pulses are defined by a
 * timer which is synchronised with the trajectory points so the max number
 * of pulses equals the max number of elements.
 */
#define MAX_PULSES 2000

/* Note that MAX_ELEMENTS, and MAX_PULSES must be defined before
 * including the trajectoryScan.h */
#include "trajectoryScan.h"

/* Buffer sizes */
#define NAME_LEN 100

/* Polling interval for waiting for motors to reach their targets */
#define POLL_INTERVAL 0.1

/* Socket timeouts */
#define POLL_TIMEOUT 1.0
#define DRIVE_TIMEOUT 100000.  /* Forever */
#define ABORT_TIMEOUT 10.

/* Used within the exec state as a timeout within the while loops which wait for
 * the ss xpsTrajectoryRun to catch up.  The delays are 0.1 second, so 100 loops
 * is 10 seconds. */
#define COUNT_TIMEOUT  100

/* The maximum size of the item names in gathering, e.g. "GROUP2.POSITIONER1.CurrentPosition" */
#define MAX_GATHERING_AXIS_STRING 60
/* Number of items per axis */
#define NUM_GATHERING_ITEMS 2
/* Total length of gathering string */
#define MAX_GATHERING_STRING MAX_GATHERING_AXIS_STRING * NUM_GATHERING_ITEMS * MAX_AXES

/* Constants used for FTP to the XPS */
#define TRAJECTORY_DIRECTORY "/Admin/public/Trajectories"
#define TRAJECTORY_FILE "TrajectoryScan.trj"
#define GATHERING_DIRECTORY "/Admin/public/"
#define GATHERING_FILE "Gathering.dat"

int status;
int i;
int j;
int k;
int anyMoving;
int ncomplete;
int nextra;
int npoints;
int dir;
int pollSocket;
int driveSocket;
int abortSocket;
int positionSocket;
int xpsStatus;
int count;
double dtime;
double posActual;
double posTheory;
double expectedTime;
double initialPos[MAX_AXES];
double trajVel;
double preDistance[MAX_AXES];
double postDistance[MAX_AXES];
double motorLowLimit[MAX_AXES];
double motorHighLimit[MAX_AXES];
double motorMinPos[MAX_AXES];
double motorMaxPos[MAX_AXES];
double pulseTime;
double pulsePeriod;
char groupName[NAME_LEN];
char xpsAddress[NAME_LEN];
char *axisName[MAX_AXES];
char macroBuf[NAME_LEN];
char motorName[NAME_LEN];
char userName[NAME_LEN];
char password[NAME_LEN];
int  xpsPort;

/* Define PVs */

/* All PVs which will be accessed in local C functions need to have their index
 * extracted with pvIndex() */
int motorCurrentIndex[MAX_AXES];
int epicsMotorDoneIndex[MAX_AXES];

/* Note, this should be time_t, but SNL doesn't understand that.  This is
 * the defininition in vxWorks. */
unsigned long startTime;  

/* Define escaped C functions at end of file */
%% static int getMotorPositions(SS_ID ssId, struct UserVar *pVar, double *pos);
%% static int getMotorMoving(SS_ID ssId, struct UserVar *pVar);
%% static int waitMotors(SS_ID ssId, struct UserVar *pVar);
%% static int getEpicsMotorMoving(SS_ID ssId, struct UserVar *pVar);
%% static int waitEpicsMotors(SS_ID ssId, struct UserVar *pVar);

%% static int getSocket(SS_ID ssId, struct UserVar *pVar, double timeout);
%% static void trajectoryExecute(SS_ID ssId, struct UserVar *pVar);
%% static void buildAndVerify(SS_ID ssId, struct UserVar *pVar);
%% static int currentElement(SS_ID ssId, struct UserVar *pVar);
%% static int getGroupStatus(SS_ID ssId, struct UserVar *pVar);
%% static void readGathering(SS_ID ssId, struct UserVar *pVar);
%% static int trajectoryAbort(SS_ID ssId, struct UserVar *pVar);


ss xpsTrajectoryScan {

    /* Initialize things when first starting */
    state init {
        when() {
            /* Get the values from the macro parameteters */ 
            strcpy(groupName, macValueGet("GROUP"));
            strcpy(xpsAddress, macValueGet("IPADDR"));
            strcpy(userName, macValueGet("USERNAME"));
            strcpy(password, macValueGet("PASSWORD"));
            xpsPort = atoi(macValueGet("PORT"));
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state init:"
                       " xpsAddress=%s groupName=%s\n",
                       xpsAddress, groupName);
            }
            for (i=0; i<numAxes; i++) {
                axisName[i] = malloc(NAME_LEN);
                sprintf(macroBuf, "P%d", i+1);
                sprintf(axisName[i], "%s.%s", groupName, macValueGet(macroBuf));
                sprintf(macroBuf, "M%d", i+1);
                sprintf(motorName, "%s%s.VAL", macValueGet("P"), macValueGet(macroBuf));
                pvAssign(epicsMotorPos[i], motorName);
                sprintf(motorName, "%s%s.DIR", macValueGet("P"), macValueGet(macroBuf));
                pvAssign(epicsMotorDir[i], motorName);
                sprintf(motorName, "%s%s.OFF", macValueGet("P"), macValueGet(macroBuf));
                pvAssign(epicsMotorOff[i], motorName);
                sprintf(motorName, "%s%s.DMOV", macValueGet("P"), macValueGet(macroBuf));
                pvAssign(epicsMotorDone[i], motorName);
            }
            xpsStatus = 0;
            ncomplete = 1;
        
            /* Set the arrays to zero */
            for (i=0; i<MAX_ELEMENTS; i++) {
                for (j=0; j<MAX_AXES; j++) {
                    motorTrajectory[j][i] = 0.0;
                }
            }

            /* Force numAxes to be <= MAX_AXES */
            if (numAxes > MAX_AXES) numAxes = MAX_AXES;
            
            /* Get sockets for communicating with XPS */
            %%pVar->pollSocket = getSocket(ssId, pVar, POLL_TIMEOUT);
            %%pVar->driveSocket = getSocket(ssId, pVar, DRIVE_TIMEOUT);
            %%pVar->abortSocket = getSocket(ssId, pVar, ABORT_TIMEOUT);
        
            for (j=0; j<numAxes; j++) {
                motorCurrentIndex[j]   =  pvIndex(motorCurrent[j]);
                epicsMotorDoneIndex[j] =  pvIndex(epicsMotorDone[j]);
            }

            /* Clear all event flags */
            efClear(buildMon);
            efClear(executeMon);
            efClear(abortMon);
            efClear(readbackMon);
            efClear(nelementsMon);
        } state monitor_inputs
    }


    /* Monitor inputs which control what to do (Build, Execute, Read) */
    state monitor_inputs {
        entry {
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state monitor_inputs:"
                       " entry\n");
            }
        }
        when(efTestAndClear(buildMon) && (build==1)) {
        } state build

        when(efTestAndClear(executeMon) && (execute==1) 
                && (buildStatus == STATUS_SUCCESS)){
        } state execute

        when(efTestAndClear(readbackMon) && (readback==1)
                && (execStatus == STATUS_SUCCESS)) {
        } state readback

        when(efTestAndClear(nelementsMon) && (nelements>=1)) {
            /* If nelements changes, then change endPulses to this value,
             * since this is what the user normally wants.  endPulses can be
             * changed again after changing nelements if this is desired. */
            if (moveMode == MOVE_MODE_RELATIVE)
                endPulses = nelements;
            else
                endPulses = nelements-1;
            pvPut(endPulses);
        } state monitor_inputs

    }


    /* Build and verify trajectory */
    state build {
        entry {
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state build:"
                       " entry\n");
            }
        }
        when() {
            /* Set busy flag while building */
            buildState = BUILD_STATE_BUSY;
            pvPut(buildState);
            buildStatus=STATUS_UNDEFINED;
            pvPut(buildStatus);
            
            if (moveMode == MOVE_MODE_RELATIVE)
                npoints = nelements;
            else
                npoints = nelements-1;
            /* If total time mode, calc time per element and write,
               else use the array timeTraj  */
            if (timeMode == TIME_MODE_TOTAL) {
                dtime = time/npoints;
                for (i=0; i<nelements; i++) timeTrajectory[i] = dtime;
                pvPut(timeTrajectory);
            }
        
            /* Compute expected time for trajectory & check element period */
            expectedTime=0;
            for (i=0; i<npoints; i++) {
                expectedTime += timeTrajectory[i];
            }
        
            %%buildAndVerify(ssId, pVar);
         
            /* Export values to PVs */
            for (j=0; j<numAxes; j++) {       
                pvPut(motorMVA[j]);
                pvPut(motorMAA[j]);
            } 
            /* Clear busy flag, post status */
            buildState = BUILD_STATE_DONE;
            pvPut(buildState);
            pvPut(buildStatus);
            pvPut(buildMessage);
            /* Clear build command, post.  This is a "busy" record, don't want
             * to do this until build is complete. */
            build=0;
            pvPut(build);
            /* Set exec status to UNDEFINED */
            execStatus = STATUS_UNDEFINED;
            pvPut(execStatus);
            strcpy(execMessage, " ");
 
        } state monitor_inputs
    }


    state execute {
        entry {
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state execute:"
                       " entry\n");
            }
            %%waitMotors(ssId, pVar);
        
            /* Get the initial positions of the motors */
            for (j=0; j<numAxes; j++) initialPos[j] = epicsMotorPos[j];
            /* Move to start position if required.
             * Subtract distance of initial acceleration element */
            if (moveMode == MOVE_MODE_ABSOLUTE) {
                for (j=0; j<numAxes; j++) {
                    if (moveAxis[j]) {
                        epicsMotorPos[j] = motorTrajectory[j][0] - preDistance[j];
                        pvPut(epicsMotorPos[j]);
                    }
                }
            } else {
                /* Backup by distance of initial acceleration element */
                for (j=0; j<numAxes; j++) {
                    if (moveAxis[j]) {
                        epicsMotorPos[j] = epicsMotorPos[j] - preDistance[j];
                        pvPut(epicsMotorPos[j]);
                    }
                }
            }
            %%waitEpicsMotors(ssId, pVar);
        
           /* Define pulse output for trajectory */
            if (npulses > 0) {
                /* Check validity, modify values if necessary */
                if (startPulses < 1) startPulses = 1;
                if (startPulses > nelements) startPulses = nelements;
                pvPut(startPulses);
                if (endPulses < startPulses) endPulses = startPulses;
                if (endPulses > nelements) endPulses = nelements;
                pvPut(endPulses);

                /* The XPS can only output pulses at a fixed period, not a fixed
                 * distance along the trajectory.  
                 * The trajectory elements where pulses start and stop are
                 * defined with the PVs startPulses and endPulses.
                 * Compute the time between pulses as the total time over which pulses 
                 * should be output divided by the number of pulses to be output. */
                pulseTime=0;
                for (i=startPulses; i<=endPulses; i++) {
                    pulseTime += timeTrajectory[i-1];
                }
                pulsePeriod = pulseTime/npulses;
            } else {
                pulsePeriod = 0.;
            }

            /* Check that the trajectory won't exceed the software limits */
            for (j=0; j<numAxes; j++) {
                if (moveAxis[j]) {
                    status = PositionerUserTravelLimitsGet(pollSocket,
                                                           axisName[j],
                                                           &motorLowLimit[j], 
                                                           &motorHighLimit[j]);
                    if ((epicsMotorPos[j] + motorMinPos[j]) < motorLowLimit[j]) {
                        execStatus = STATUS_FAILURE;
                        pvPut(execStatus);
                        sprintf(execMessage, "Low soft limit violation on motor %d", j);
                        pvPut(execMessage);
                    }
                    if ((epicsMotorPos[j] + motorMaxPos[j]) > motorHighLimit[j]) {
                        execStatus = STATUS_FAILURE;
                        pvPut(execStatus);
                        sprintf(execMessage, "High soft limit violation on motor %d", j);
                        pvPut(execMessage);
                    }
                }
            }
        }
 

        when (execStatus == STATUS_FAILURE) {
            /* Clear execute command, post.  This is a "busy" record, don't
             * want to do this until execution is complete. */
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state execute:"
                       " execStatus = STATUS_FAILURE\n");
            }
            execute=0;
            pvPut(execute);
        } state monitor_inputs


        when (execStatus != STATUS_FAILURE) {
            %%pVar->xpsStatus = getGroupStatus(ssId, pVar);
            /* Setting execState here will cause the xpsTrajectoryRun SS to wake up */
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state execute:"
                       " setting execState = EXECUTE_STATE_MOVE_START\n");
            }
            execState = EXECUTE_STATE_MOVE_START;
            pvPut(execState);
            count = 0;
            while (execState != EXECUTE_STATE_EXECUTING && 
                   count < COUNT_TIMEOUT ) {
                epicsThreadSleep(0.1);
                count++;
            }
            if (count == COUNT_TIMEOUT) {
                strcpy(execMessage, "Exec Timeout!");
                pvPut(execMessage);
                execStatus = STATUS_ABORT;
                pvPut(execStatus);
            }        
            count = 0;
            /* Wait until ss xpsTrajectoryRun has started the traj scan */
            while (xpsStatus != 45 && 
                   count < COUNT_TIMEOUT && 
                   execStatus != STATUS_FAILURE ) {
                epicsThreadSleep(0.1);
                count++;
                %%pVar->xpsStatus = getGroupStatus(ssId, pVar);
            }
            if (count == COUNT_TIMEOUT) {
                strcpy(execMessage, "Exec Timeout!");
                pvPut(execMessage);
                execStatus = STATUS_ABORT;
                pvPut(execStatus);
            }
            readStatus=STATUS_UNDEFINED;
            pvPut(readStatus);
        } state wait_execute
    }


    /* Wait for trajectory to complete */
    state wait_execute {
        entry {
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state wait_execute:"
                       " entry\n");
            }
        }
        when (execStatus == STATUS_ABORT) {
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state wait_execute:"
                       " execStatus = STATUS_ABORT\n");
            }
            /* The trajectory_abort state set has detected an abort. It has
             * already posted the status and message.  Don't execute flyback
             * return to top */
            execState = EXECUTE_STATE_DONE;
            pvPut(execState);
            /* Clear execute command, post.  This is a "busy" record, don't
             * want to do this until execution is complete. */
            execute=0;
            pvPut(execute);
        } state monitor_inputs

        when (delay(0.1) && execState==EXECUTE_STATE_EXECUTING) {
            /* Get the current motor positions, post them */
            %%getMotorPositions(ssId, pVar, pVar->motorCurrent);
            for (j=0; j<numAxes; j++) pvPut(motorCurrent[j]);

            %%pVar->xpsStatus = getGroupStatus(ssId, pVar);
            if (xpsStatus == 45) {
                %%pVar->ncomplete = currentElement(ssId, pVar);
                sprintf(execMessage, "Executing element %d/%d", 
                        ncomplete, nelements);
                pvPut(execMessage);
            }
            else if (xpsStatus == 12) {
                /* 12 = ready from move */
                execState = EXECUTE_STATE_FLYBACK;
                execStatus = STATUS_SUCCESS;
                strcpy(execMessage, " ");
            }
            else if (xpsStatus < 10) {
                /* The xps group status reflects an error. */
                execState = EXECUTE_STATE_FLYBACK;
                execStatus = STATUS_FAILURE;
                sprintf(execMessage,"XPS Status Error=%d", xpsStatus);
            }
            /* See if the elapsed time is more than expected, time out */
            if (difftime(time(0), startTime) > (expectedTime+10)) {
                execState = EXECUTE_STATE_FLYBACK;
                execStatus = STATUS_TIMEOUT;
                strcpy(execMessage, "Timeout");
            }
        } state wait_execute

        when (execState==EXECUTE_STATE_FLYBACK) {
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state wait_execute:"
                       " execStatus = EXECUTE_STATE_FLYBACK\n");
            }
            /* Stop the detector */
            detOff = 1;
            pvPut(detOff);
            pvPut(execState);
            pvPut(execStatus);
            pvPut(execMessage);

            /* Only do the following if the trajectory executed OK */
            if (execStatus == STATUS_SUCCESS) { 
                /* Make sure the motors have stopped */
                %%waitMotors(ssId, pVar);
                %%waitEpicsMotors(ssId, pVar);        
            
                /* Get the current motor positions, post them */
                %%getMotorPositions(ssId, pVar, pVar->motorCurrent);
                for (j=0; j<numAxes; j++) pvPut(motorCurrent[j]);
                for (j=0; j<numAxes; j++) {
                    epicsMotorPos[j] = motorCurrent[j];
                    pvPut(epicsMotorPos[j]);
                }
                %%waitEpicsMotors(ssId, pVar);
            }
            
            execState = EXECUTE_STATE_DONE;
            pvPut(execState);
            /* Clear execute command, post.  This is a "busy" record, don't
             * want to do this until execution is complete. */
            execute=0;
            pvPut(execute);
        } state monitor_inputs
    }


    /* Read back actual positions */
    state readback {
        entry {
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryScan: state readback:"
                       " entry\n");
            }
        }
        when() {
            /* Set busy flag */
            readState = READ_STATE_BUSY;
            pvPut(readState);
            readStatus=STATUS_UNDEFINED;
            pvPut(readStatus);
            /* Erase the readback and error arrays */
            for (j=0; j<numAxes; j++) {
                for (i=0; i<MAX_PULSES; i++) {
                    motorReadbacks[j][i] = 0.;
                    motorError[j][i] = 0.;
                }
            }
            
            %%readGathering(ssId, pVar);
           
            /* readGathering has set the actual number of positions read back */
            pvPut(nactual);
 
            /* Post the readback and error arrays */
            for (j=0; j<numAxes; j++) {
                pvPut(motorReadbacks[j]);
                pvPut(motorError[j]);
            }
            /* Clear busy flag */
            readState = READ_STATE_DONE;
            pvPut(readState);
            /* For now we are not handling read errors */
            readStatus = STATUS_SUCCESS;  
            pvPut(readStatus);
            strcpy(readMessage, " ");
            pvPut(readMessage);
            /* Clear readback command, post.  This is a "busy" record, don't
             * want to do this until readback is complete. */
            readback=0;
            pvPut(readback);
        } state monitor_inputs
    }
}



/* This state set simply monitors the abort input.  It is a separate state set
 * so that it is always active, no matter what the state of the trajectoryScan
 * state set. */
ss xpsTrajectoryAbort {
    state monitorAbort {
        when ((efTestAndClear(abortMon)) && (abort==1) &&
              (execState==EXECUTE_STATE_EXECUTING)) {

            execStatus = STATUS_ABORT;
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryAbort: state monitorAbort:"
                       " setting execStatus = STATUS_ABORT\n");
            }
            pvPut(execStatus);
            strcpy(execMessage, "Motion aborted");
            pvPut(execMessage);
        
            %%trajectoryAbort(ssId, pVar);
        
            /* Clear abort command, post.  This is a "busy" record, don't
             * want to do this until abort command has been sent. */
            abort=0;
            pvPut(abort);
        } state monitorAbort
    }
}

/* This state was required because the trajectory execute call does not return
 * until the end of the trajectory. So to poll during the scan we call the 
 * scan from a separate state set. */
ss xpsTrajectoryRun {
    state asyncExecute {
        when (efTestAndClear(execStateMon) && (execState == EXECUTE_STATE_MOVE_START)) {
            /*%%pVar->xpsStatus = getGroupStatus(ssId, pVar);*/
            if (debugLevel > 0) {
                printf("XPS_trajectoryScan: ss xpsTrajectoryRun: state asyncExecute:"
                       " setting execState = EXECUTE_STATE_EXECUTING\n");
            }
            execState = EXECUTE_STATE_EXECUTING;
            pvPut(execState);

            /* If ready to move */
            if (xpsStatus > 9 && xpsStatus < 20) {
                /* Start the detector */
                detOn = 1;
                pvPut(detOn);
                /* Get start time of execute */        
                startTime = time(0);
                /* Call the C function from here so that the main state set can poll */
                %%trajectoryExecute(ssId, pVar);
            } else {
                execStatus = STATUS_FAILURE;
                pvPut(execStatus);
            }
        } state asyncExecute
    }
}

/* This state set polls every second to keep the motor current position medm 
 * screen up to date when a scan is not running */
ss xpsTrajectoryPosition {
    state positionUpdate {
        when (delay (1) && (execState == EXECUTE_STATE_DONE)) {
            %%getMotorPositions(ssId, pVar, pVar->motorCurrent);
            for (j=0; j<numAxes; j++) pvPut(motorCurrent[j]);
        } state positionUpdate
    }
}


/*  C functions  */
%{

/* getMotorPositions returns the positions of each motor.
 * It reads the positions from the controller, and then converts to 
 * EPICS motor user coordinates */
static int getMotorPositions(SS_ID ssId, struct UserVar *pVar, double *pos)
{
    int status;
    int j;
    int dir;

    /* Read the current positions of all the axes */
    
    status = GroupPositionCurrentGet(pVar->positionSocket,
                                     pVar->groupName,pVar->numAxes,pos);
    if (status != 0)
        printf("Error performing GroupPositionCurrentGet%i\n", status);
    /* Convert from XPS units (which are EPICS motor dial units) to user units */
    for (j=0; j<pVar->numAxes; j++) {
        if (pVar->epicsMotorDir[j] == 0) dir=1; else dir=-1;
        pos[j] = pos[j]*dir + pVar->epicsMotorOff[j];
    }
  
    return(status);
}


/* Returns 0 when no motors are moving */
static int getMotorMoving(SS_ID ssId, struct UserVar *pVar)
{
    int status;
    int moving=0;
    int groupStatus;

    /* Read the current status of the group */

    status = GroupStatusGet(pVar->pollSocket,pVar->groupName,&groupStatus);
    if (status != 0)
        printf("Error performing GroupStatusGet %i\n",status);
    
    if (groupStatus > 42)
        moving = 1;

    return(moving);
}

/* getEpicsMotorMoving returns the EPICS moving status of each motor, packed into 
 * a single int.  Bit 0 = motor 1, bit 1 = motor 2, etc. 0=not moving, 1=moving.
 * If the entire int is 0 then no motors are moving */
static int getEpicsMotorMoving(SS_ID ssId, struct UserVar *pVar)
{
    int j;
    int result=0, mask=0x01;

    for (j=0; j<pVar->numAxes; j++) {
        seq_pvGet(ssId, pVar->epicsMotorDoneIndex[j], 0);
        if (pVar->epicsMotorDone[j] == 0) result |= mask;
        mask = mask << 1;
    }
    return(result);
}

/* waitMotors waits for all motors to stop moving.  It reads and posts the
 * motor positions during each loop. */
static int waitMotors(SS_ID ssId, struct UserVar *pVar)
{
    int j;

    /* Logic is that we always want to post position motor positions 
     * after the end of move is detected. */
    while(getMotorMoving(ssId, pVar)) {
        /* Get the current motor positions, post them */
        getMotorPositions(ssId, pVar, pVar->motorCurrent);
        for (j=0; j<pVar->numAxes; j++) seq_pvPut(ssId, pVar->motorCurrentIndex[j], 0);
        epicsThreadSleep(POLL_INTERVAL);
    }
    getMotorPositions(ssId, pVar, pVar->motorCurrent);
    for (j=0; j<pVar->numAxes; j++) seq_pvPut(ssId, pVar->motorCurrentIndex[j], 0);
    return(0);
}

/* waitEpicsMotors waits for all motors to stop moving using the EPICS motor
 * records..  It reads and posts the motor positions during each loop. */
static int waitEpicsMotors(SS_ID ssId, struct UserVar *pVar)
{
    int j;

    /* Logic is that we always want to post position motor positions 
     * after the end of move is detected. */
    while(getEpicsMotorMoving(ssId, pVar)) {
        /* Get the current motor positions, post them */
        for (j=0; j<pVar->numAxes; j++) {
            pVar->motorCurrent[j] = pVar->epicsMotorPos[j];
            seq_pvPut(ssId, pVar->motorCurrentIndex[j], 0);
        }
        epicsThreadSleep(POLL_INTERVAL);
    }
    for (j=0; j<pVar->numAxes; j++) {
        pVar->motorCurrent[j] = pVar->epicsMotorPos[j];
        seq_pvPut(ssId, pVar->motorCurrentIndex[j], 0);
    }
    return(0);
}

/* Function to ask the XPS for a socket this requires Socket.h */
static int getSocket(SS_ID ssId, struct UserVar *pVar, double timeout)
{
    int sock = 0;
    
    sock = TCP_ConnectToServer(pVar->xpsAddress, pVar->xpsPort, timeout);
    if (sock < 0) 
            printf("Error TCP_ConnectToServer, status=%d\n",sock);
    return (sock);
}


/* Function to setup the gathering, events and start the trajectory */
static void trajectoryExecute(SS_ID ssId, struct UserVar *pVar)
{
    int status;
    int j;
    char buffer[MAX_GATHERING_STRING];
    int eventId;
    
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " entry\n");
    }
    /* Configure Gathering */
    /* Reset gathering.  
     * This must be done because GatheringOneData just appends to in-memory list */    
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling GatheringReset(%d)\n", pVar->pollSocket);
    }
    status = GatheringReset(pVar->pollSocket);
    if (status != 0) {
        printf("Error performing GatheringReset, status=%d\n",status);
        return;
    }

    /* Write list of gathering parameters.
     * Note that there must be NUM_GATHERING_ITEMS per axis in this list. */
    strcpy(buffer, "");
    for (j=0; j<pVar->numAxes; j++) {
        strcat (buffer, pVar->axisName[j]);
        strcat (buffer, ".SetpointPosition;");
        strcat (buffer, pVar->axisName[j]);
        strcat (buffer, ".CurrentPosition;");
    }
    
    /* Define what is to be saved in the GatheringExternal.dat.  
     * 3 pieces of information per axis. */
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling GatheringConfigurationSet(%d, %d, %s)\n",
               pVar->pollSocket, pVar->numAxes*NUM_GATHERING_ITEMS, buffer);
    }
    status = GatheringConfigurationSet(pVar->pollSocket, 
                                       pVar->numAxes*NUM_GATHERING_ITEMS, buffer);
    if (status != 0)
            printf("Error performing GatheringConfigurationSet, status=%d, buffer=%p\n",
                   status, buffer);

    /* Define trajectory output pulses. 
     * startPulses and endPulses are defined as 1=first real element, need to add
     * 1 to each to skip the acceleration element.  
     * The XPS is told the element to stop outputting pulses, and it seems to stop
     * outputting at the start of that element.  So we need to have that element be
     * the decceleration endPulses is the element, which means adding another +1. */

    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling MultipleAxesPVTPulseOutputSet(%d, %s, %d, %d, %f)\n",
                                           pVar->pollSocket, pVar->groupName,
                                           pVar->startPulses+1, 
                                           pVar->endPulses+2, 
                                           pVar->pulsePeriod);
    }
    status = MultipleAxesPVTPulseOutputSet(pVar->pollSocket, pVar->groupName,
                                           pVar->startPulses+1, 
                                           pVar->endPulses+2, 
                                           pVar->pulsePeriod);

    /* Define trigger */
    sprintf(buffer, "Always;%s.PVT.TrajectoryPulse", pVar->groupName);
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling EventExtendedConfigurationTriggerSet(%d, %d, %s, %s, %s, %s, %s)\n",
                                                  pVar->pollSocket, 2, buffer, 
                                                  "", "", "", "");
    }
    status = EventExtendedConfigurationTriggerSet(pVar->pollSocket, 2, buffer, 
                                                  "", "", "", "");
    if (status != 0) {
        printf("Error performing EventExtendedConfigurationTriggerSet, status=%d, buffer=%s\n", 
               status, buffer);
        return;
    }

    /* Define action */
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling EventExtendedConfigurationActionSet(%d, %d, %s, %s, %s, %s, %s)\n",
                                                 pVar->pollSocket, 1, 
                                                 "GatheringOneData", 
                                                 "", "", "", "");
    }
    status = EventExtendedConfigurationActionSet(pVar->pollSocket, 1, 
                                                 "GatheringOneData", 
                                                 "", "", "", "");
    if (status != 0) {
        printf("Error performing EventExtendedConfigurationActionSet, status=%d\n",
               status);
        return;
    }

    /* Start gathering */
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling EventExtendedStart(%d, %p)\n",
                                            pVar->pollSocket, &eventId);
    }
    status= EventExtendedStart(pVar->pollSocket, &eventId);
    if (status != 0) {
        printf("Error performing EventExtendedStart, status=%d\n",status);
        return;
    }

    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling MultipleAxesPVTExecution(%d, %s, %s, %d)\n",
                                      pVar->driveSocket, pVar->groupName,
                                      TRAJECTORY_FILE, 1);
    }
    status = MultipleAxesPVTExecution(pVar->driveSocket, pVar->groupName,
                                      TRAJECTORY_FILE, 1);
    /* status -27 means the trajectory was aborted */
    if ((status != 0) && (status != -27))
        printf("Error performing MultipleAxesPVTExecution, status=%d\n", status);
    
    /* Remove the event */
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling EventExtendedRemove(%d, %d)\n", pVar->pollSocket, eventId);
    }
    status = EventExtendedRemove(pVar->pollSocket, eventId);
    if (status != 0) {
        printf("Error performing ExtendedEventRemove, status=%d\n",status);
        return;
    }
        
    /* Save the gathered data to a file */    
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: trajectoryExecute:"
               " calling GatheringStopAndSave(%d)\n", pVar->pollSocket);
    }
    status = GatheringStopAndSave(pVar->pollSocket);
    
    /* status -30 means gathering not started i.e. aborted before the end of
       1 trajectory element */
    if ((status != 0) && (status != -30))
        printf("Error performing GatheringStopAndSave, status=%d\n", status);
    
    return;
}


/* Function to build, install and verify trajectory */ 
static void buildAndVerify(SS_ID ssId, struct UserVar *pVar)
{
    FILE *trajFile;
    int i, j, status;
    int npoints;
    double trajVel;
    double D0, D1, T0, T1;
    int ftpSocket;
    char fileName[NAME_LEN];
    double distance;
    double maxVelocity[MAX_AXES], maxAcceleration[MAX_AXES];
    double minJerkTime[MAX_AXES], maxJerkTime[MAX_AXES];
    double preTimeMax, postTimeMax;
    double preVelocity[MAX_AXES], postVelocity[MAX_AXES];
    double time;
    int dir[MAX_AXES];
    
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: buildAndVerify:"
               " entry\n");
    }
    /* We create trajectories with an extra element at the beginning and at the end.
     * The distance and time of the first element is defined so that the motors will
     * accelerate from 0 to the velocity of the first "real" element at their 
     * maximum allowed acceleration.
     * Similarly, the distance and time of last element is defined so that the 
     * motors will decelerate from the velocity of the last "real" element to 0 
     * at the maximum allowed acceleration.

    /* Compute the velocity of each motor during the first real trajectory element, 
     * and the time required to reach this velocity. */
    preTimeMax = 0.;
    postTimeMax = 0.;
    /* Zero values since axes may not be used */
    for (j=0; j<pVar->numAxes; j++) {
        preVelocity[j] = 0.;
        postVelocity[j] = 0.;
    }
    for (j=0; j<pVar->numAxes; j++) {
        if (!pVar->moveAxis[j]) continue;
        status = PositionerSGammaParametersGet(pVar->pollSocket, pVar->axisName[j], 
                                               &maxVelocity[j], &maxAcceleration[j],
                                               &minJerkTime[j], &maxJerkTime[j]);
        if (status != 0) {
            printf("Error calling positionerSGammaParametersSet, status=%d\n", 
                   status);
        }
        /* The calculation using maxAcceleration read from controller below
         * is "correct" but subject to roundoff errors when sending ASCII commands
         * to XPS.  Reduce acceleration 10% to account for this. */
        maxAcceleration[j] = 0.9 * maxAcceleration[j];
        /* Note: the preDistance and postDistance numbers computed here are
         * in user coordinates, not XPS coordinates, because they are used for 
         * EPICS moves at the start and end of the scan */
        if (pVar->moveMode == MOVE_MODE_RELATIVE) {
            distance = pVar->motorTrajectory[j][0];
        } else {
            distance = pVar->motorTrajectory[j][1] - pVar->motorTrajectory[j][0];
        }
        preVelocity[j] = distance/pVar->timeTrajectory[0];
        time = fabs(preVelocity[j]) / maxAcceleration[j];
        if (time > preTimeMax) preTimeMax = time;
        if (pVar->moveMode == MOVE_MODE_RELATIVE) {
            distance = pVar->motorTrajectory[j][pVar->nelements-1];
        } else {
            distance = pVar->motorTrajectory[j][pVar->nelements-1] - 
                       pVar->motorTrajectory[j][pVar->nelements-2];
        }
        postVelocity[j] = distance/pVar->timeTrajectory[pVar->nelements-1];
        time = fabs(postVelocity[j]) / maxAcceleration[j];
        if (time > postTimeMax) postTimeMax = time;
    }

    /* Compute the distance that each motor moves during its acceleration phase.
     * Only move it this far. */
    for (j=0; j<pVar->numAxes; j++) {
        pVar->preDistance[j] =  0.5 * preVelocity[j] *  preTimeMax; 
        pVar->postDistance[j] = 0.5 * postVelocity[j] * postTimeMax;
    }

    /* Create the trajectory file */
    trajFile =  fopen (TRAJECTORY_FILE, "w");

    /* Compute the sign relationship of user coordinates to XPS coordinates for
     * each axis */
    for (j=0; j<pVar->numAxes; j++) {
        if (pVar->epicsMotorDir[j] == 0) dir[j]=1; else dir[j]=-1;
    }
    /* Create the initial acceleration element */
    fprintf(trajFile,"%f", preTimeMax);
    for (j=0; j<pVar->numAxes; j++) 
        fprintf(trajFile,", %f, %f", pVar->preDistance[j]*dir[j], preVelocity[j]*dir[j]);
    fprintf(trajFile,"\n");
 
    /* The number of points in the file is nelements for MOVE_MODE_RELATIVE and
     * nelements-1 for other modes */
    if (pVar->moveMode == MOVE_MODE_RELATIVE) 
        npoints = pVar->nelements;
    else 
        npoints = pVar->nelements-1;
    for (i=0; i<npoints; i++) {
        T0 = pVar->timeTrajectory[i];
        if (i < npoints-1)
            T1 = pVar->timeTrajectory[i+1];
        else
            T1 = T0;
        for (j=0; j<pVar->numAxes; j++) {
            if (pVar->moveMode == MOVE_MODE_RELATIVE) {
                D0 = pVar->motorTrajectory[j][i] * dir[j];
                if (i < npoints-1) 
                    D1 = pVar->motorTrajectory[j][i+1] * dir[j];
                else
                    D1 = D0;
            } else {
                D0 = pVar->motorTrajectory[j][i+1] *dir[j] - 
                           pVar->motorTrajectory[j][i] * dir[j];
                if (i < npoints-1) 
                    D1 = pVar->motorTrajectory[j][i+2] * dir[j] - 
                           pVar->motorTrajectory[j][i+1] * dir[j];
                else
                    D1 = D0;
            }
                
            /* Average either side of the point? */
            trajVel = ((D0 + D1) / (T0 + T1));
            if (!(pVar->moveAxis[j])) {
                D0 = 0.0;    /* Axis turned off*/
                trajVel = 0.0;
            }
        
            if (j == 0) fprintf(trajFile,"%f", pVar->timeTrajectory[i]);
            fprintf(trajFile,", %f, %f",D0,trajVel);
            if (j == (pVar->numAxes-1)) fprintf(trajFile,"\n");
        }    
    }

    /* Create the final acceleration element. Final velocity must be 0. */
    fprintf(trajFile,"%f", postTimeMax);
    for (j=0; j<pVar->numAxes; j++) 
        fprintf(trajFile,", %f, %f", pVar->postDistance[j]*dir[j], 0.);
    fprintf(trajFile,"\n");
    fclose (trajFile);
    
    /* FTP the trajectory file from the local directory to the XPS */
    status = ftpConnect(pVar->xpsAddress, pVar->userName, pVar->password, &ftpSocket);
    if (status != 0) {
        printf("Error calling ftpConnect, status=%d\n", status);
        return;
    }
    status = ftpChangeDir(ftpSocket, TRAJECTORY_DIRECTORY);
    if (status != 0) {
        printf("Error calling ftpChangeDir, status=%d\n", status);
        return;
    }
    status = ftpStoreFile(ftpSocket, TRAJECTORY_FILE);
    if (status != 0) {
        printf("Error calling ftpStoreFile, status=%d\n", status);
        return;
    }
    status = ftpDisconnect(ftpSocket);
    if (status != 0) {
        printf("Error calling ftpDisconnect, status=%d\n", status);
        return;
    }
       
    /* Verify trajectory */
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: buildAndVerify:"
               " calling MultipleAxesPVTVerification(%d, %s, %s)\n", 
               pVar->pollSocket, pVar->groupName, TRAJECTORY_FILE);
    }
    status = MultipleAxesPVTVerification(pVar->pollSocket, pVar->groupName,
                                         TRAJECTORY_FILE);
     
    pVar->buildStatus = STATUS_FAILURE;
    if (status == 0) {
        strcpy(pVar->buildMessage, " ");
        pVar->buildStatus = STATUS_SUCCESS;
    }        
    else if (status == -69)
        strcpy(pVar->buildMessage, "Acceleration Too High");
    else if (status == -68)
        strcpy(pVar->buildMessage, "Velocity Too High");
    else if (status == -70)
        strcpy(pVar->buildMessage, "Final Velocity Non Zero");         
    else if (status == -75)
        strcpy(pVar->buildMessage, "Negative or Null Delta Time");         
    else
        sprintf(pVar->buildMessage, "Unknown trajectory verify error=%d", status);  
     
    /* Read dynamic parameters*/
     
    if (status == 0){
        pVar->buildStatus = STATUS_SUCCESS;
    }    
    if (1) {  /* We may need to test for status here */
        for (j=0; j<pVar->numAxes; j++) {
            status = MultipleAxesPVTVerificationResultGet(pVar->pollSocket,
                         pVar->axisName[j], fileName, &pVar->motorMinPos[j], &pVar->motorMaxPos[j], 
                         &pVar->motorMVA[j], &pVar->motorMAA[j]);
            if (status != 0) {
                printf("Error performing MultipleAxesPVTVerificationResultGet for axis %s, status=%d\n",
                       pVar->axisName[j], status); 
            }
        }
    } else {
        for (j=0; j<pVar->numAxes; j++) {
            pVar->motorMVA[j] = 0;
            pVar->motorMAA[j] = 0;     
        }
    }
           
    return; 
}


/* Function returns the current trajectory element*/ 
static int currentElement(SS_ID ssId, struct UserVar *pVar)
{
    int status;
    int number;
    char fileName[100];
    
    status = MultipleAxesPVTParametersGet(pVar->pollSocket,
                 pVar->groupName, fileName, &number);
    if (status != 0)
        printf("Error performing MultipleAxesPVTParametersGet, status=%d\n", 
               status);
    return (number);
}

static int getGroupStatus(SS_ID ssId, struct UserVar *pVar)
{
    int status;
    int groupStatus;

    /* Read the current status of the group */

    status = GroupStatusGet(pVar->pollSocket,pVar->groupName,&groupStatus);
    if (status != 0)
        printf("Error performing GroupStatusGet, status=%d\n", status);
    return(groupStatus);
}


/* Function to load the GatheringExternal.dat file which was written
 * by the XPS after the trajectory was performed and read back the
 * actual motor positions and calculate the position errors */ 
static void readGathering(SS_ID ssId, struct UserVar *pVar)
{
    char buffer[MAX_GATHERING_STRING];
    FILE *gatheringFile;
    int i,j;
    int nitems;
    double setpointPosition, actualPosition;
    int ftpSocket;
    int status;
    int dir;
    
    if (pVar->debugLevel > 0) {
        printf("XPS_trajectoryScan: readGathering:"
               " entry\n");
    }
    /* FTP the gathering file from the XPS to the local directory */
    status = ftpConnect(pVar->xpsAddress, pVar->userName, pVar->password, &ftpSocket);
    if (status != 0) {
        printf("Error calling ftpConnect, status=%d\n", status);
        return;
    }
    status = ftpChangeDir(ftpSocket, GATHERING_DIRECTORY);
    if (status != 0) {
        printf("Error calling ftpChangeDir, status=%d\n", status);
        return;
    }
    status = ftpRetrieveFile(ftpSocket, GATHERING_FILE);
    if (status != 0) {
        printf("Error calling ftpRetrieveFile, status=%d\n", status);
        return;
    }
    status = ftpDisconnect(ftpSocket);
    if (status != 0) {
        printf("Error calling ftpDisconnect, status=%d\n", status);
        return;
    }

    gatheringFile = fopen(GATHERING_FILE, "r");
    
    /* Read 1st 2 lines which only contain the axis names */
    for (i=0; i<2; i++) {
        fgets (buffer, MAX_GATHERING_STRING, gatheringFile);
    }
    /* The next line is the motor positions at the start of the trajectory element
     * where pulses began to be output.  Skip this line because this pulse just
     * started the MCS counting, there are no data yet. */
    fgets (buffer, MAX_GATHERING_STRING, gatheringFile);
    
    for (i=0; i<pVar->npulses; i++) {
        for (j=0; j<pVar->numAxes; j++) {
            /* Note the trailing white space in this format is required to make
             * fscanf read the newline */
            nitems = fscanf(gatheringFile, "%lf %lf ", 
                            &setpointPosition, &actualPosition);
            if (nitems != NUM_GATHERING_ITEMS) {
                printf("Error reading Gathering.dat file, nitems=%d, should be %d\n",
                       nitems, NUM_GATHERING_ITEMS);
                goto done;
            }
            pVar->motorError[j][i] = actualPosition - setpointPosition;
            /* Convert from XPS units to EPICS motor user units */
            if (pVar->epicsMotorDir[j] == 0) dir=1; else dir=-1;
            pVar->motorReadbacks[j][i] = actualPosition * dir + pVar->epicsMotorOff[j];
        }
    }
    
    done:
    fclose (gatheringFile);
    pVar->nactual = i; 

    return;
}

/* Function aborts the trajectory/motion */ 
static int trajectoryAbort(SS_ID ssId, struct UserVar *pVar)
{
    int status;
    
    status = GroupMoveAbort(pVar->abortSocket,pVar->groupName);
    if (status != 0)
        printf("Error performing GroupMoveAbort, status=%d\n", status);
       
    return (0);
}

}%
