#ifndef _SETPOINT_H_
#define _SETPOINT_H_

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

/*---------------------------------------------------------------------------
 *
 * DESCRIPTION:	Defines structures used internally by the trajectory 
 *		generator.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

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

#include "../h/rciConfig.h"

#include "transform.h"
#include "cartBlend.h"
#include "jointBlend.h"
#include "motionQueue.h"
#include "solveEqn.h"
#include "teach.h"
#include "motionRequest.h"

/* the basic setpoint control functions */

extern int setpStartup();
extern int setpCtrl();
extern int setpRelease();

#define JOINT	  1			/* joint path interpolation mode */
#define CARTESIAN 2			/* Cartesian path interpolation mode */

/* Default parameter settings */

#define DEFAULT_ARM_VEL		10.0	/* joint velocity of lower arm */
#define DEFAULT_WRIST_VEL	20.0	/* joint velocity of wrist */

#define DEFAULT_INTERMODE	JOINT	/* def. path interpolation mode */
#define DEFAULT_QUEUE_SIZE	5	/* def. motion queue size */

/* Command codes for transform management functions */
#define MANIP_TRANS_PICKUP	1
#define MANIP_TRANS_DROP	2
#define TASK_TRANS_PICKUP	3
#define TASK_TRANS_DROP		4

/* Command codes for jbias management functions */
#define JBIAS_FXN_PICKUP	1
#define JBIAS_FXN_DROP		2

typedef struct {
	float s;		/* path scale (nominally 0.0 to 1.0) */
	float ds;		/* path scale increment for last cycle */
	float a;		/* scale acceleration factor */
	int k_a;		/* acceleration counter */
	int c;			/* path counter (nominally 0 to sigma) */
	int sigma;		/* nominal length of path, in cycles */
	int sigma_0;		/* initial value of sigma */
} MOTION_COUNT_INFO;

typedef struct {
	/* per-program global information structure. Allocated out of
	   global memory on the arbiter CPU. */

	int leadCpu;			/* CPU no. of lead tgen task */
	int wasPaused;			/* system was paused last cycle */
	int taskActive[MAX_CPUS];	/* is trajectory task active ? */
	int rendezvous[MAX_CPUS];	/* stamp used by setpRendezvous() */
	MFLAG mflagBuf[NUM_USRMFLAGS + NUM_SYSMFLAGS]; /* MFLAGS live here */
	int interval;			/* control cycle interval (msec.) */
	float speed;			/* global speed scale (not used) */
	MANIP *manipPool[MAX_CPUS];	/* planning level addr. of MANIPs */

	/* Data fields used for pausing motions. This is a bit hacky
	   right now ... if you don't belive me, just check out
	   the function checkOnPauseState().
	*/

	float sysTime[MAX_CPUS]; 	/* system time kept by each task */

	float pauseTimeLimit;	/* pause time (msec) */
	float pauseTransTime;	/* transition time (msec) */
	int pauseReq;		/* pause request variable */
	int pauseCtrl;		/* pause state control variable */
#define TASK_RUNNING		1	/* task running as normal */
#define TASK_READY_TO_PAUSE	2	/* saw pause req; ready to pause */ 
#define TASK_PAUSING		3	/* in the process of pausing */
#define TASK_PAUSED		4	/* task is paused */
#define TASK_READY_TO_RESUME	5	/* saw resume req; ready to resume */
	int pauseState[MAX_CPUS]; /* pause state for each trajectory task */
	int pauseCount[MAX_CPUS]; /* pause count kept by each traj. task */

	int powerOff[MAX_CPUS];	/* arm power off record for each task */
	int powerOffFreeze;	/* arm power off, freeze trajectory task */

	int motionFlagNumber;	/* current system allocated motion flag */
	unsigned long optionFlags; /* set with rcclOptions() */
	MUTEX optionsMutex;	/* mutex structure for writing optionFlags */

	float serviceDelay;	/* delay for servicing motion reqs. (msec.) */
	int tightControlLoop;	/* send cmds same cycle they're computed */
} RCCL_GMEM;

#define MAX_GOAL_POSITION	1024	/* max target pos. eqn size (bytes) */

typedef struct {	
	/* structure defining things that have to be done when a  motion
	   terminates (flags to be set, signals to be sent, transforms
	   to update, etc) */

	MFLAG *mflag;		/* MFLAG to set status values in */
	TRSF *t6;		/* place to put current T6 value (not used)*/
	int transListSize;	/* size of pos. eqn list to evaluate */
	POS_TRSF *transList;	/*  and eqn. terms. used by updateTrans()*/
	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 */
	int code;		/* Completion code value */
} COMPLETION_BLOCK;

typedef struct _mpb {
	/* Motion parameter block. Per-manipulator structure allocated out
	   of local memory in the control task. This is the repository
	   of all the local information used by the trajectory generator
	   to control a single robot. */	

	struct _mpb *next;	/* so trajectory task can link these into */
	struct _mpb *prev;	/*  a list (not used at the moment) */
	int num;		/* manipulator number */
	
	/* --- System-type variables --- */

	RCI_DESC *td;		/* Descriptor for RCI trajectory task */
	RCI_RBT *rbt;		/* RCI robot information (if present) */
	MOTION_QUEUE *mq;	/* pointer to the motion queue */
	MOTION_CONTROL_BLOCK *mcb; /* planning task shared control memory */
	MANIP *mnps[MAX_CPUS];	/* pointer to MANIPS on all CPUS */
	MANIP *hostMan;		/* address of MANIP seen by planning level */
	int state;		/* current state of trajectory generator */
#define TGEN_IDLE		1	/* idle (not servicing any motions) */
#define TGEN_SERVICE		2	/* servicing a motion */
#define TGEN_PAUSE		3	/* paused (activity suspended) */
	int noMotionsYet;	/* no motions have yet been queued */
	int tracking;		/* manipulator is in tracking idle state */
	float sampleTime;	/* trajectory task sample interval (msec.) */
	float sysTime;		/* system time (sampleTime*cycleCount) */
	int systemPause;	/* RCCL system is suspended (paused) */
	int controlMode;	/* teach or normal trajectory generator */
		
	/* --- Current motion targets --- */

	POS *goalPos;		/* Pointer to target position equation */
	char goalPosBuf1[MAX_GOAL_POSITION]; /* buffer for position eqn. */
	char goalPosBuf2[MAX_GOAL_POSITION]; /* buffer for position eqn. */
	JNTS jointPosition;	/* target joint angles */

	/* --- Basic state variables --- */

	TRSF t6;		/* per-cycle output value of T6 */
	JNTS j6;		/* per-cycle output value of J6 */
	TRSF oldT6;		/* previous cycle value of T6 */
	JNTS oldJ6;		/* previous cycle value of J6 */
	JNTS j6vel;		/* per-cycle output joint velocity */

	/* --- Parameters controlling the current motion --- */

	/* (Note that the velocity/acceleration parameters are contained
	    int the motion control block) */

	int interMode;		/* path interpolation mode */
	int newMotion;		/* should compute fresh drive params. */
	int targetType;		/* move request, stop request, or idling */
	char motionHasTimeLimit;/* motion completes after motionCountLimit */
	int targetFixed;	/* Entire motion target is fixed */
	int goalPosFixed;	/* Target goal position is fixed */
	int jbiasFixed;		/* joint bias values (if any) are fixed */
	unsigned long config;	/* actual robot configuration value */
	unsigned long configTarget; /* desired robot configuration value */
	unsigned long configUsed; /* last config vale used to compute tC */
	float objectLoad;	/* estimated object mass (not used) */
	int motionNumber;	/* number of most recent motion serviced */
	char doDrive;		/* motion needs a drive transform computed */
	char stopOnNextRequest; /* motion stops when next request appears */
	unsigned long mode;	/* control mode bits (set by rcclSetModes())*/
	int relativePosition;	/* target position equation is mnp->last */
	int stopMotion;		/* motion is a stop request */
	int targetInRange;	/* jC in range (used by pendingMotion.c) */

	/* --- Motion timing counters --- */
	
	int cycle;		/* cycle counter for each motion (0->???) */
	int tau;		/* 1/2 number of samples for transition */
	int motionTau;		/* explicitly given tau for motion */
	int currentTau;		/* tau currently in used */
	float tauSkew;		/* transition symmetry factor (0.0 to 1.0) */
	MOTION_COUNT_INFO ci;   /* path counters and scale parameters */
	float lastScaleTrans;	/* trans. time for last scale change (sec.) */
	int endCount;		/* no. of counts ahead to start next motion */

	/* --- Variables associated with the trajectory computation --- */

	DRIVEP drivePar;	/* drive parameters for Cartesian motion */
	JNTS driveJnts;		/* drive parameters for joint motion */	
	char shiftSet;		/* target offset Xform is present */
	TRSF shiftTransform;	/* value of target offset Xform */

	TRSF tC;		/* target position in end effector coords */
	JNTS jC;		/* target position in joint coordinates */
	TRSF tC0;		/* value (or estimate) of tC for cycle 0 */
	JNTS jC0;		/* value (or estimate) of jC for cycle 0 */
	JNTS jCp0;		/* jC excluding joint bias for cycle 0 */
	TRSF t60;		/* Initial T6 value (cycle 0) */
	TRSF t61;		/* Extrapolated T6 value for cycle 1 */
	TRSF t62;		/* Extrapolated T6 value for cycle 2 */
	TRSF tB_est;		/* estimated value of tB */
	JNTS jB_est;		/* estimated value of jB */

	TRSF t6del0;		/* Delta T6 at cycle 0 */
	DSPL t6v0;		/* Cartesian velocity for cycle 0 */	

	DRIVEP t6vel0;		/* Cartesian velocity for cycle 0 */
	int blendInT6;		/* determines if we blend if T6 or not */
	JOINT_BLEND_PARAMS jbp;	/* joint transitioning params */
	CARTESIAN_BLEND_PARAMS cbp; /* cartesian transitioning params */

	int toolTerm;		/* tool term is present */
	int coordTerm;		/* coord term is present */

	TRSF COORD;		/* solved value of the COORD term */
	TRSF COORD_I;		/* solved value of the inverse COORD term */
	TRSF TOOL;		/* solved value of the TOOL term */
	TRSF TOOL_I;		/* solved value of the inverse TOOL term */
	TRSF *coord;		/* set if COORD has been solved */
	TRSF *invCoord;		/* set if inv COORD has been solved */
	TRSF *tool;		/* set if TOOL has been solved */
	TRSF *invTool;		/* set if inv TOOL has been solved */

	TRSF COORD1;		/* COORD term computed during cycle 0 */
	TRSF TOOL1;		/* TOOL term computed during cycle 0 */
	TRSF COORD1_I;		/* inverse of COORD1 */
	TRSF TOOL1_I;		/* inverse of TOOL1 */

	TRSF DRIVE1;		/* Drive estimate computed during cycle 0 */
	TRSF DRIVE;		/* Drive computed in normal execution */

	JNTS j61;		/* Extrapolated j6 value for cycle 1 */
	JNTS j62;		/* Extrapolated j6 value for cycle 2 */
	JNTS jdrive1;		/* estimate of jdrive computed for cycle 0 */
	JNTS jdrive;		/* drive jnts computed in normal execution */

	/* --- Motion completeion information --- */

	COMPLETION_BLOCK cbk;	/* information used when motion completes */
	COMPLETION_BLOCK expiringCbk; /* cbk info for completeing motion */
	int expiringCycles;	/* cycles until motion completes */
	MOTION_COUNT_INFO expiringCi; /* count info for completing motion */

	/* --- Variables associated with the next motion request --- */

	MOTION_REQUEST *nextMotionRequest; /* Request packet for next motion*/
	POS *nextGoalPos;	/* goal position for next motion request */
	TRSF nextTC0;		/* estimated tC for next motion */
	JNTS nextJC0;		/* estimated jC for next motion */
	JNTS nextJCp0;		/* estimated jC (sans bias) for next motion */
	int nextCount;		/* No. of cycles motion has been pending */
	MFLAG *nextMflag;	/* motion control flag for next motion */
	int nextMotionReady;	/* next motion is ready to be serviced */
/*XXX*/	int nextMotionInRange;	/* next motion's target is in range */
	DIFF nextCvel;		/* estimated Cartesian velocity for motion */
	JNTS nextJvel;		/* estimated joint velocity for motion */
  	unsigned long nextUsedConfig; /* last config used to solve for 'nextJC0' */

	/* --- Variables associated with the last motion --- */

	int lastInterMode;	/* path interpolation mode of last motion */
	unsigned long lastConfigTarget;	/* desired configuration for last motion */
	int lastTargetFixed;	/* entire last motion target was fixed */
	int lastGoalPosFixed;	/* last target goal position was fixed */
	int lastJbiasFixed;	/* last joint bias (if any) was fixed */
	int finishCode;		/* how previous motion request terminated */
	int lastFinishCode;	/* used to check conditional motions */
	int completedOnSchedule;/* last motion finished when planned */
	TRSF lastTC0;		/* tC0 for the last motion */
	JNTS lastJC0;		/* jC0 for the last motion */
	int lastStopMotion;	/* last motion was a stop request */

	/* --- Monitor, transform function, and update information --- */

	int numMotionFxns;	/* number monitors for current motion */
	FXN_BLOCK motionFxns[MAX_MONITOR_FXNS]; /* current motion monitors */
	int jbiasEnable;	/* joint biasing specified for motion */
	JNTS jbias;		/* joint bias values */
	FXN_BLOCK jbiasFxn;	/* joint biasing function */
	FXN_BLOCK *monitorFxnHead; /* head of general monitor fxn list */
	FXN_BLOCK *monitorFxnTail; /* tail of general monitor fxn list */
	FXN_BLOCK *freeFxnBlocks; /* monitor function block free list */
	FXN_BLOCK fxnBlockPool[MAX_MONITOR_FXNS]; /* mon. fxn block pool */
	POS_TRSF updatePosList2[MAX_UPDATE_POS];/* xforms to be multiplied*/
	POS_TRSF updatePosList1[MAX_UPDATE_POS];/* xforms to be multiplied*/
	TRSF_DESC *evalListHead; /* list of functional xforms to be evaled */
	TRSF_DESC *evalListTail; /* list of functional xforms to be evaled */

	/* --- Variables controlling the hand --- */

	int handPos;		/* desired hand position (not used) */
	int handPosSet;		/* hand position has been set (not used) */

	/* --- Miscelaneous things --- */

	MFLAG *mflagBuf;	/* pointer to global motion flag pool */
	int flushedMotionNumber; /* motion number of deleted motion */
	TEACH_BLOCK tbk;	/* variables used by the teach routine */
	int updateAcceleration; /* recompute transition count */

	/* --- Stored motion parameters for paused motions --- */

	int SAVEstate;			/* All of these variables have */
	int SAVEtracking;		/* counterparts above ...      */
	POS *SAVEgoalPosition;		
	int SAVEnumMotionFxns;		
	int SAVEendCount;
	int SAVEmotionTau;
	float SAVEtauSkew;
	char SAVEmotionHasTimeLimit;
	int SAVEcomplyMask;
	int SAVEforceLimitMask;
	int SAVEdiffLimitMask;
	TRSF_DESC *SAVEevalListHead;
	TRSF_DESC *SAVEevalListTail;
	MOTION_COUNT_INFO SAVEci;
	char SAVEdoDrive;
	char SAVEstopOnNextRequest;
	int SAVEtargetFixed;
	int SAVEgoalPosFixed;
	int SAVEjbiasFixed;
	JNTS SAVEjC0;			/* not used */
	TRSF SAVEtC0;			/* not used */
	int SAVEjbiasEnable;
	COMPLETION_BLOCK SAVEcbk;
	int SAVEstopMotion;
	int SAVEtau;
	TRSF SAVEDRIVE;
	JNTS SAVEjdrive;
	DRIVEP SAVEdrivePar;
	JNTS SAVEdriveJnts;

	int spare[16];			/* spare data fields */
} MOTION_PARAMETER_BLOCK;

typedef struct {
	/* basic component of the manipTable[], which describes to 
	   each control task what manipulators are present on the system.
	   The manipTable is part of the 'RCCL_SMEM' shared memory 
	   between the planning task and each control task.	  
	 */

	int cpu;		/* CPU the manipulator is assigned to */
	int allocated;		/* this field has been taken */
	void *sharedMem;	/* address of shared mem. for this MANIP */
	void *localMem;	/* address of local mem. for this MANIP */
	MANIP *mnp;		/* planning addr. of MANIP structure */
} MANIP_RECORD;

typedef struct {
	/* per-control task control structure, used to communicate between
	   the planning level and the control task. Allocated from shared
	   memory between the planning and control tasks. */

/*KILL*/MANIP *manips[MAX_MANIPS];	/* manipulator list for this task */
	MOTION_PARAMETER_BLOCK *mpb[MAX_MANIPS]; /* mpb list for this task */
	MANIP_RECORD manipTable[MAX_MANIPS]; /* used to init above 2 fields */

	/* arguments for transCtrl() */

	TRSF_DESC *transPtr;
	int (*transFxn)();
	int transArg;
	int manipNum;
	MANIP *man;

} RCCL_SMEM;

extern RCCL_GMEM *globalMemoryCheck();

#endif
