/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1991 by John E. Lloyd.				    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
#ifndef _MOTIONREQUEST_H_
#define _MOTIONREQUEST_H_

/*---------------------------------------------------------------------------
 *
 * DESCRIPTION:	Structures used to communicate motion requests between the 
 * 		planning level and the trajectory generator.
 *
 * AUTHOR/DATE:	John Lloyd, Summer 1989
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/rcclDefs.h"
#include "../h/robotTypes.h"

#define MAX_UPDATE_POS		32	/* max. # terms in an update pos */

typedef struct {
	  /* Element of a position equation LIST (a cheap and dirty
	     rep. of a position equation used by updateTrans()) */
	  TRSF *trsf;
	  int leftSide;
} POS_TRSF;

typedef struct _fxnBlk {
	/* fxn and argument information for monitor functions */

	struct _fxnBlk *next;
	struct _fxnBlk *prev;
	int (*fxn)();
	int arg;
	int id;
	 } FXN_BLOCK;

typedef struct {
	/* Basic motion request packet that is placed on the motion
	   queue, one per motion request.
	 */

	/* --- Information about the motion target --- */

	POS *goalPosition;	/* target is a position loop */
	int jointPositionSet;   /* target is a set of joint angles */
	JNTS jointPosition;	/* target joint angles */
	int positionSize;	/* size (bytes) of position equation */
	POS *positionAddr;	/* planning level address of equation */
	TRSF tC0;		/* precomputed estimate of target t6 */
	JNTS jC0;		/* precomputed estimate of target j6 */

	/* --- motion parameters and options --- */

	int condition;		/* motion conditional on last motion */
	int motionHold;		/* hold start until startMotion() call */
	int interMode;	 	/* motion interpolation mode */

	char transitionTimeSet;	/* transition time set explicitly */
	char segmentTimeSet;	/* segment time set explicitly */
	char stopTimeSet;	/* explicit stop time set explicitly */
	char startTimeSet;	/* explicit start time set explicitly */
	int transitionTime;	/* explicit transition time value */
	int segmentTime;	/* explicit segment time value */
	int stopTime;		/* explicit stop time value */
	int startTime;		/* explicit start time value */
	float tauSkew;		/* transition symmetry factor (0.0 to 1.0) */

	float transVel;		/* desired translational velocity */
	float rotVel;		/* desired rotational velocity */
	JNTS jointVel;		/* desired joint velocities */
	char jointVelSet;	/* new joint velocities have been set */

	float transAccel;	/* desired translational accel. */
	float rotAccel;		/* desired rotational accel. */
	JNTS jointAccel;	/* desired joint accels. */
	char jointAccelSet;	/* new joint accels. have been set */

	/* included for optimization purposes */
	float transAccelDist;	/* (transVel * transVel) / transAccel */
	float rotAccelDist;	/* (rotVel * rotVel) / rotAccel */
	JNTS jointAccelDist;	/* (jointVel * jointVel) / jointAccel */

	/* included for optimization purposes */
	float transAccelTime;	/* (2.0 * transVel) / transAccel  */
	float rotAccelTime;	/* (2.0 * rotVel) / rotAccel  */
	JNTS jointAccelTime;	/* (2.0 * jointVel) / jointAccel  */

	unsigned long configSet;/* manipulator configuration code set */
	unsigned long config;	/* new manipulator configuration value */
	unsigned long configUsed;/* config used to solve for jC0 */

	char shiftSet;		/* shift (distance) transform present */
	TRSF shiftTransform; 	/* value of shift transform */

	unsigned long mode;	/* motion control mode */

	/* --- The next items are computed for every motion --- */

	int mflagNum;		/* event flag for motion request */
	float objectLoad;	/* load mass (not used) */
	int number;		/* motion number */
	char doDrive;		/* compute a drive Xform */
	char stopOnNextRequest;	/* stop when next motion request seen */
	int stopMotion;		/* this is a "stop" request */
	int targetFixed;	/* motion target is fixed (not changing)*/
	int goalPosFixed;	/* motion goal position is fixed */
	int jbiasFixed;	        /* motion joint bias (if any) is fixed */
	int relativePosition;	/* motion target is relative */
	int targetInRange;	/* sucessively solved for jC0 */

	/* --- Update commands and function information --- */

	int numMotionFxns;	/* number of motion fxns. specified */
	FXN_BLOCK motionFxns[MAX_MONITOR_FXNS]; /* motion fxn descriptions */
	int jbiasEnable;	/* enable joint biasing */
	JNTS jbias;		/* joint bias values */
	FXN_BLOCK jbiasFxn;	/* joint biasing function */

	POS *updatePos;		/* updateTrans(): equation to solve */
	TRSF *updateTr;		/* updateTrans(): result transform */
	TRSF *updateT1;		/* updateTrans(): transform to solve for */
	TRSF *updateTx;		/* updateTrans(): instantiate value for T6 */

	/* These next things are just passed thorugh and copied into the 
	   MANIP structure ... */

	int diffLimitMask;	/* mask giving diff limit directions */
	DIFF diffLimitValues;   /* values for each diff limit direction */
	int forceLimitMask;	/* mask giving force limit directions */
	FORCE forceLimitValues;	/* values for each force limit direction*/
	int complyMask;		/* mask giving compliance directions */
	FORCE complyValues;	/* values for each compliance direction */

	/* Last item ... at time stamp saying when the motion was queued,
	   so we can implement a motion service delay */

	float queueTime;	/* time motion was queued (msec.) */

	int spare[4];	    	/* spare data fields */
} MOTION_REQUEST;	

typedef struct {
	/* Per-manipulator structure that contains motion parameter
	   information that is private to the planning task.
	 */

	/* --- Information about the motion target --- */

	POS *goalPosition;	/* target is a position loop */
	int jointPositionSet;   /* target is a set of joint angles */
	JNTS jointPosition;	/* target joint angles */
	int positionSize;	/* size (bytes) of position equation */
	POS *positionAddr;	/* planning level address of equation */
	TRSF tC0;		/* precomputed estimate of target t6 */
	JNTS jC0;		/* precomputed estimate of target j6 */

	/* --- motion parameters and options --- */

	int condition;		/* motion conditional on last motion */
	int newInterModeSet;	/* new motion interp. mode specified */
	int interMode;	 	/* motion interpolation mode */
	int motionHold;		/* hold start until startMotion() call */

	char transitionTimeSet;	/* transition time set explicitly */
	char segmentTimeSet;	/* segment time set explicitly */
	char stopTimeSet;	/* explicit stop time set explicitly */
	char startTimeSet;	/* explicit start time set explicitly */
	int transitionTime;	/* explicit transition time value */
	int segmentTime;	/* explicit segment time value */
	int stopTime;		/* explicit stop time value */
	int startTime;		/* explicit start time value */
	float tauSkew;		/* transition symmetry factor (0.0 to 1.0)*/

	float transVel;		/* desired translational velocity */
	float rotVel;		/* desired rotational velocity */
	JNTS jointVel;		/* desired joint velocities */

	float transAccel;	/* desired translational accel. */
	float rotAccel;		/* desired rotational accel. */
	JNTS jointAccel;	/* desired joint accels. */

	unsigned long configSet;/* manipulator configuration code set */
	unsigned long config;	/* new manipulator configuration value */
	unsigned long configUsed;/* config used to solve for jC0 */

	char newShiftSet;	/* new shift (distance) transform set */
	TRSF newShiftTransform;	/* new value of shift transform */
	char shiftSet;	    	/* shift (distance) transform present */
	TRSF shiftTransform; 	/* value of shift transform */

	unsigned long mode;	/* motion control mode */
	float objectLoad;	/* load mass (not used) */
	char stopOnNextRequest;	/* stop when next motion request seen */

	/* The next items are computed for every motion and passed
	   straight through */

	int mflagNum;		/* event flag for motion request */
	int number;		/* motion number */
	char doDrive;		/* compute a drive Xform */
	int stopMotion;		/* this is a "stop" request */
	int targetFixed;	/* motion target is fixed (not changing)*/
	int goalPosFixed;	/* motion goal position is fixed */
	int jbiasFixed;	        /* motion joint bias (if any) is fixed */
	int relativePosition;	/* motion target is relative */
	int targetInRange;	/* sucessively solved for jC0 */

	/* --- Update commands and function information --- */

	int numMotionFxns;	/* number of motion fxns. specified */
	FXN_BLOCK motionFxns[MAX_MONITOR_FXNS]; /* motion fxn descriptions */
	int newJbiasSetting;	/* joint bias setting has changed */
	int newJbiasEnable;	/* new setting for joint biasing enable */
	JNTS newJbias;		/* new joint bias values (not used) */
	FXN_BLOCK newJbiasFxn;	/* new joint biasing function */
	int jbiasEnable;	/* enable joint biasing */
	JNTS jbias;		/* joint bias values */
	FXN_BLOCK jbiasFxn;	/* joint biasing function */
	
	POS *updatePos;		/* updateTrans(): equation to solve */
	TRSF *updateTr;		/* updateTrans(): result transform */
	TRSF *updateT1;		/* updateTrans(): transform to solve for */
	TRSF *updateTx;		/* updateTrans(): instantiate value for T6 */

	/* These next things are just passed thorugh and copied into the 
	   MANIP structure ... */

	int diffLimitMask;	/* mask giving diff limit directions */
	DIFF diffLimitValues;   /* values for each diff limit direction */
	int forceLimitMask;	/* mask giving force limit directions */
	FORCE forceLimitValues; /* values for each force limit direction*/

	int spare[4];	    	/* spare data fields */
} MOTION_VALUE_BLOCK;

typedef struct {
	/* per-manipulator structure used to send commands from the planning
	   task to the trajectory generator. 
	
	   This also contains some of the parameters set by the various
	   "set this and that" primitives; they are kept here (instead
	   of in, say, the MOTION_VALUE_BLOCK, so that they can read
	   back from anywhere in the program.

	   Allocated from shared memory between planning and control task. */

	unsigned long mode;	/* motion modes (see rcclSetModes())*/
	unsigned long activeMode; /* currently active motion modes */

	int flushReq;		/* req. to flush motion queue */
	int terminateReq;	/* req. to terminate current motion */
	int terminateCode;	/*    and associated terminate code */
	int pauseTime;		/* command to pause for X msec. */
	int pauseTransTime;	/* transition time associated with pause */

	int monitorFxnCmd;	/* mon. fxn control command */
#define RUN_MONITOR_FXN		0x1	/* start a monitor function */
#define DELETE_MONITOR_FXN	0x2	/* delete a monitor function */
	int (*monitorFxn)();	/* function to be RUN */
	int monitorArg;		/* argument to be provided to fxn. */
	int monitorRunId;	/* returned handle describing fxn. */
	int monitorDeleteId;	/* handle of function to delete */
	int monitorFxnsActive;	/* number of active monitor functions */

	int motionQueueSize;	/* max. no. of queued motion requests */
	void *goalPosBuf;	/* address of pos. buffer in task space */
	int headMotionNumber;	/* motion recent motion to be entered */
	int tailMotionNumber;	/* last motion finished */
	int waitingForStart;	/* traj task. waiting for motion start */
	int active;		/* RCCL task is active; info initialized */
	
	char controlModeReq;	/* change RCCL control mode */
#define IDLE_CTRL		1	/* used for test purposes only */
#define SETPOINT_CTRL		2	/* usual trajectory generator */
#define TEACH_CTRL		3	/* teach pendant control */

	int completedSig;	/* signal when queue is empty */

	/* Stuff for the teach routine interface: used only by
	   the code in teach.c and teachCtrl.c */

	int t_state;		/* teach pendant or keyboard mode ... */
	int t_motion_mode;	/* we are moving ... wait till stop */
	int t_stop;		/* stop command */
	float t_speed;		/* speed scale */
	JNTS t_angles;		/* target joint angles */
	TRSF t_dest_t6;		/* target t6 */

	/* Active motion parameter settings, for the getActive() calls */

	float activeTransVel[2];    /* translational vel. in use */
	float activeRotVel[2];	    /* rotational vel. in use */
	JNTS activeJointVel[2];	    /* joint velocities in use */
	float activeTransAccel[2];  /* translational accel. in use*/
	float activeRotAccel[2];    /* rotational accel. in use */
	JNTS activeJointAccel[2];   /* joint accelerations in use */

	int activeComplyMask[2];    /* compliance DIF mask in use */
	FORCE activeComplyVals[2];  /* compliance bias values in use */
	int activeForceLimMask[2];  /* motion force limit mask in use */
	FORCE activeForceLimVals[2];/* motion force limit values in use */
	int activeDiffLimMask[2];   /* motion diff limit mask in use */
	DIFF activeDiffLimVals[2];  /* motion diff limit values in use */

	int activeInterMode[2];	    /* motion interpolation mode in use */

	int pathsegIndex;	    /* buffer index for the 'active' fields */

	int activeSigma[2];	    /* motion segment time in use */
	int activeTau[2];	    /* motion transition time in use */
	int activeCount[2];	    /* motion count in use */

	int countIndex;		    /* buffer index for the 'count' fields */

	/* active set values, for the regular get() calls */

	float setTransVel[2];        /* translational vel. in use */
	float setRotVel[2];	     /* rotational vel. in use */
	int cartVelIndex;	     /*      buffer index */
	JNTS setJointVel[2];	     /* joint velocities in use */
	int jointVelIndex;	     /*      buffer index */
	float setTransAccel[2];      /* translational accel. in use*/
	float setRotAccel[2];        /* rotational accel. in use */
	int cartAccelIndex;	     /*      buffer index */
	JNTS setJointAccel[2];       /* joint accelerations in use */
	int jointAccelIndex;	     /*      buffer index */

	int setComplyMask[2];        /* compliance DIF mask in use */
	FORCE setComplyValues[2];    /* compliance bias values in use */
	int complyIndex;	     /*      buffer index */
	int setForceLimMask[2];      /* motion force limit mask in use */
	FORCE setForceLimVals[2];    /* motion force limit values in use */
	int forceLimIndex;	     /*      buffer index */
	int setDispLimMask[2];       /* motion diff limit mask in use */
	DIFF setDispLimVals[2];      /* motion diff limit values in use */
	int diffLimIndex;	     /*      buffer index */
	
	float setSpeed;		     /* speed scale factor */
	float setJointSpeed;	     /* joint speed scale factor */
	float defaultTransVel;	     /* default translational velocity */
	float defaultRotVel;	     /* default rotational velocity */
	JNTS defaultJointVel;	     /* default joint velocities */

	float setAccelScale;	     /* acceleration scale factor */
	float setJointAccelScale;    /* joint accel. scale (not used) */
	float defaultTransAccel;     /* default translational acceleration */
	float defaultRotAccel;	     /* default rotational acceleration */
	JNTS defaultJointAccel;	     /* default joint accelerations */

	int setInterMode; 	     /* motion interpolation mode */

	int mcbregs[16];	     /* spare data fields */
} MOTION_CONTROL_BLOCK;

#endif
