/*=-=-=-=-=-=-=-=-=-=-  RCCL/RCI Notice of Copyright  -=-=-=-=-=-=-=-=-=-=-=*
 *									    *
 *	Copyright (C) 1987 by all authors described below. Permission to    *
 *	use, distribute and copy is granted, but only for non-commercial    *
 *	purposes and providing the authors are acknowledged and this 	    *
 *	copyright notice is displayed prominently.			    *
 *									    *
 *	The authors provide this software in hope that it will be useful,   *
 *	but do not assume responsibility for its use.			    *
 *									    *
 *=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-*/

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	moves.h
 *
 * DESCRIPTION:	Include file containing definitions internal to the 
 *		implementation of rccl which do not usually have to be seen 
 *		by the user.
 * 
 * AUTHOR/DATE:	John Lloyd, December, 1986
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#ifndef RCCLH
#include "rccl.h"
#endif
#ifndef KINEH
#include "kine.h"
#endif

#ifndef MOVESH
#define MOVESH

/* ----- Motion Queue stuff ----- */

#define MAX_Q_ENTRIES	0x100
#define Q_BUF_SIZE	0x8000

typedef struct _q_descr {
	char *addr;
	unsigned long size;
} Q_DESCR;

extern char 	rcl_queueBuf [];
extern Q_DESCR 	rcl_queueBlocks [];
extern int 	rcl_Qhead,
		rcl_Qtail;

/* ------ Definitions for the synchronization stuff ----- */

typedef struct {
	unsigned char startRequest;	/* motion execution go-ahead flag */
	unsigned char stopRequest;	/* motion execution stop flag */
	unsigned char startValue;	/* go-ahead code once motion starts */
	unsigned char stopValue;	/* stop code once motion has stopped */
	unsigned char status;		/* motion status (see rccl.h) */
	float scale;			/* path parameter (0.0 to 1.0) */
} EFT;

extern EFT rcl_eftBuf[];

/* ----- Definitions for the trajectory generator ----- */

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

/* Default parameter settings */

#define DEFAULT_TRANS_VEL	200.	/* translational velocity */
#define DEFAULT_ROT_VEL		50.	/* rotational velocity */
#define DEFAULT_J1_VEL		10.0	/* joint 1 velocity (deg/sec) */
#define DEFAULT_J2_VEL		10.0	/* joint 2 velocity */
#define DEFAULT_J3_VEL		10.0	/* joint 3 velocity */
#define DEFAULT_J4_VEL		20.0	/* joint 4 velocity */
#define DEFAULT_J5_VEL		20.0	/* joint 5 velocity */
#define DEFAULT_J6_VEL		20.0	/* joint 6 velocity */
#define DEFAULT_CARTESIAN_SPEED	 1.0	/* cartesian velocity scale */
#define DEFAULT_JOINT_SPEED	 1.0	/* joint velocity scale */

#define DEFAULT_SAMPLE_RATE	28
#define DEFAULT_INTERMODE	JOINT
#define DEFAULT_QUEUE_SIZE	5
#define DEFAULT_TRANS_TIME	500
#define MIN_SAMPLE_RATE		7
#define MAX_SAMPLE_RATE		112

/* Selection bits for compliance and limit checking */

#define SELFX           01	/* select force in x */
#define SELFY           02	/* select force in y */
#define SELFZ           04	/* select force in z */
#define SELMX           010	/* select moment in x */
#define SELMY           020	/* select moment in y */
#define SELMZ           040	/* select moment in z */

#define SELTX           01	/* select translation in x */
#define SELTY           02	/* select translation in y */
#define SELTZ           04	/* select translation in z */
#define SELRX           010	/* select rotation in x */
#define SELRY           020	/* select rotation in y */
#define SELRZ           040	/* select rotation in z */

#define SELJ1           01	/* select joint 1 */
#define SELJ2           02	/* select joint 2 */
#define SELJ3           04	/* select joint 3 */
#define SELJ4           010	/* select joint 4 */
#define SELJ5           020	/* select joint 5 */
#define SELJ6           040	/* select joint 6 */

/* ------ basic data structure definitions ------ */

typedef struct _term {	               /* Position equation term:       */
		TRSF *trsf;		/*  user's transform             */
		TRSF *altr;		/*  alternate system's transform */
		TRSF *oldt;		/*  old value of transform       */
		bool rhs;               /*  right hand side predicate    */
		long dummy[2];
		struct _term *next;  /*  pointer to the next term     */
		struct _term *prev;  /*  pointer to the previous term */
} TERM, *TERM_PTR;
				/* Internal position structure   	*/
				/* that maps exactly on user's   	*/
typedef struct {	        /* POS structure:                	*/
		char *name;     /*  name pointer        	 	*/

				/* Old sync stuff; not really used now: */
		int code;	/*   Motion completion code		*/
		real scal;	/*   Goes from 0 to 1 during motion	*/
		event end;	/*   Semaphore set at the end of motion */

					/* What follows not known to user*/
		bool cfnsp, tfnsp;      /*  coord fns , tool fns pred.   */
		bool coorp, toolp;      /*  coord , tool pred.           */
		TERM_PTR t6ptr;         /*  pointer to t6 term           */
		TERM_PTR tlptr;         /*  pointer to tool term         */
		TERM_PTR pos;           /*  pointer to pos term          */
} PST, *PST_PTR;

typedef struct {
	PST *goalPosition;
	EFT *eft;

	char defaultInterModeSet;
	int defaultInterMode;

	int checkingDiffLimits;
	DIFF diffLimitValues;
	int checkingForceLimits;
	FORCE forceLimitValues;
	int compliantAxes;
	FORCE compliantForceValues;

	char transitionTimeSet;
	int transitionTime;
	char segmentTimeSet;
	int segmentTime;

	char velocitySet;
	float translationalVelocity;
	float rotationalVelocity;
	char jointVelocitySet;
	JNTS jointVelocity;
	char cartesianSpeedSet;
	float cartesianSpeed;
	char jointSpeedSet;
	float jointSpeed;

	char configurationSet;
	int configuration;
	int (*monitorFunction)();
	
	TRSF *updateTransform;
	PST *updatePosition;

	char sampleRateSet;
	int sampleRate;
	float objectLoad;
	
	char shiftSet;
	TRSF shiftTransform;

	int numHoldXforms;
	TRSF tC0;

} MOTION_REQUEST;	

/* ----- Function definitions ----- */

extern int rcl_setpoint(),
	   rcl_checkstate();	   

/* ----- Trajectory generator variables ----- */

extern char	rcl_setpointInit;
extern SNCS     rcl_sncs;	/* precomputed arm jacobian and trig terms */

struct _opsw {				/* option switches:                */
	int graphics,			/*  -g  graphic files (unformated) */
	    numerics,			/*  -d  numeric output (formated)  */
	    angles,			/*  -a  angles in solution frame   */
	    t6butnotj6,			/*  -k  t6 but not j6              */
	    encoders,			/*  -e  encoder values output      */
	    setp_debug;			/*  -s  set point debug switch	   */
};

extern struct _opsw rcl_options;
extern JNTS rcl_maxv;

#endif
