#ifndef _RCCLDEFS_H_
#define _RCCLDEFS_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: RCCL structure definitions, macros, and external entries
 *		for use at the user level.
 *
 * AUTHOR:	Vincent Hayward, Purdue University, 1983.
 *		John Lloyd, McGill University 1988.
 *
 * NOTES:
 *
 *-------------------------------------------------------------------------*/

#include <stdio.h>

#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/namesize.h"
#include "../h/rciTask.h"
#include "../h/rciMacros.h"
#include "../h/robot_jls.h"

/* flags for default values */

#define F_UNDEF		-1.0		/* undefined or unspecified */
#define F_DEFAULT	-2.0		/* default value */

#define DEFAULT_TRANS_VEL	200.	/* translational velocity (mm/sec) */
#define DEFAULT_ROT_VEL		50.	/* rotational velocity (deg/sec) */
#define DEFAULT_TRANS_ACCEL    100.0	/* Default trans. accel (mm/sec^2) */
#define DEFAULT_ROT_ACCEL      100.0	/* Default rot. accel (deg/sec^2) */

/* Synchronization flag definitions */

#define NUM_USRMFLAGS   256	/* no. of usr definable motion flags */
#define NUM_SYSMFLAGS	256	/* no. motion flags available to system */
#define MSTAT_ERR      -256	/* error return for motionStatus(), etc .. */

/* MFLAG status values */

#define M_PENDING   0x01	/* Motion is on the queue */
#define M_RUNNING   0x02 	/* Motion is being serviced */
#define M_PAUSED    0x04 	/* Motion has been paused */
#define M_FINISHED  0x08	/* Motion is finished */

/* Start and stop code values */

#define ON_FORCE    0x8000	/* stop on force condition */
#define ON_DIFF	    0x8001	/* stop on diff motion condition */
#define ON_NORMAL   0x8002	/* normal start/stop condition */
#define ON_ABORT    0x8003	/* request was aborted before start */

/* Definitions for the synchronization stuff */

typedef struct {
	int startRequest;	/* motion execution go-ahead flag */
	int startValue;		/* go-ahead code once motion starts */
	int startTime;		/* time when the motion began */
	int stopRequest;	/* motion execution stop flag */
	int stopValue;		/* stop code once motion has stopped */
	int stopTime;		/* time when motion completed */
	int status;		/* motion status (see rccl.h) */
	float scale;		/* path parameter (0.0 to 1.0) */
	int stopSig;		/* signal to be delivered on completion */
	int timeCmd;		/* motion timing command */
#define MF_ADJUST_SPEED	     1		/* change current speed */
#define MF_ADJUST_STOPTIME   2		/* change the next stop time */
	float timeCmdVal;	/* timing command value */
	float timeCmdTrans;	/* transition time associated with time cmd */
} MFLAG, *MFLAG_PTR;

extern TRSF *EQ;		/* Tokens used to define position eqns. */
extern TRSF *TL;		/*   in the arg list of "makeposition()"*/
extern TRSF *T6;
extern TRSF *TRANS;

extern int constTrsf();		/* Tokens describing transform function */
extern int varbTrsf();		/* types */

/* RCCL OPTIONS (set with rcclSetOptions()) */

#define RCCL_SIMULATE		0x01	/* run to simulator mode */
#define RCCL_ERROR_EXIT		0x02	/* exit or abort on error */
#define RCCL_LEAVE_POWER_ON     0x04	/* leave power on when exiting */

/* Definitions specifying degrees of freedom */

#define ALONG_X		0x01
#define ALONG_Y		0x02
#define ALONG_Z		0x04

#define ABOUT_X		0x08
#define ABOUT_Y		0x10
#define ABOUT_Z		0x20

/* Data type for describing a set of joint axis values */

#define NUM_JNTS	6
typedef struct {
	float v[NUM_JNTS];	/* joint values */
	char type[NUM_JNTS+1];	/* joint type ('r' or 'p') */
	int num;		/* number of actual joints */
} JNTS, *JNTS_PTR;

/* Position data structure definitions */

typedef struct _POS_TERM {
	/* Individual terms in a kinematic loop equation. A circular
	   linked list of these comprises a position data structure. */

	struct _POS_TERM *next;	/* links with which to build circular */
	struct _POS_TERM *prev; /*   linked list ... */
	TRSF *trsf;		/* pointer to the transform itself */
	int t6Side;		/* transform is defined on same side as T6 */
	int leftSide;		/* transform is defined on LHS of equation */
} POS_TERM;

typedef struct _POS {
	/* Header for kinematic loop equation. For description, see
	   header in the file rccl/makeposition.c. */

	/* The user should not be too concerned with the contents of
	   this data structure */

	POS_TERM *startTerm;	/* starting term in equation */
	POS_TERM *t6Term;	/* pointer to T6 term of equation */
	POS_TERM *toolTerm;	/* pointer to tool frame term of equation */
	POS_TERM *coordTerm;	/* pointer to coord frame term of equation */

	int coordfn;		/* TRUE if coord part of eqn. not constant */
	int toolfn;		/* TRUE if tool part of eqn. not constant */
	int numTerms;		/* number of terms in the eqn. */

	struct _POS *addr;	/* Original allocated address */
	short global;	        /*   made with makePosition() */
} POS, *POS_PTR;

/* ----- MANIP data structure ----- */

#define MAX_MANIPS	8	/* max. no. manips per cpu */
#define CONFSTR_SIZE	6	/* no. bytes in config string */

typedef struct _MANIP {
	/* Manipulator data structure. Returned by rcclCreate() and
	   used as a handle when controlling the manipulator. Contains
	   state information about the manipulator. */

	struct _MANIP *next;
	struct _MANIP *prev;

	char name[RCI_NAMESIZE+1]; /* name of the manipulator */
	int num;		/* number assigned by system (internal use) */
	int taskNum;		/* number of controlling trajectory task */
	int rbtNum;		/* number of attached robot (if any) */
	int cpu;		/* number of CPU of trajectory task */

	int objectFlag;		/* TRUE if no robot is attached */

	/* These fields only maintained if a robot is connected */
	JNTS *j6;		/* output joint values */
	JNTS *j6o;		/* actual joint values */
	JNTS *jbias;		/* current value of the joint offset */
	JNTS *jvel;		/* output joint velocity */

	TRSF *t6;		/* output T6 value */
	TRSF *t6o;		/* actual T6 value */
	TRSF *here;		/* output T6 value at end of last motion */
	TRSF *tool;		/* current value of the tool transform */
	TRSF *rest;		/* T6 corresponding to the "park" position */
	TRSF *lastTC;		/* target T6 at end of last motion */

	POS *last;		/* built-in position eqn: T6 = lastTC */
	POS *park;		/* built-in position eqn: T6 = rest */

	int conf;		/* robot kinematic configuartion */

	int completed;		/* all motions requests have been completed */
	int state;		/* not used at the moment */
	int condition;		/* kinematic condition of the robot */
	int handPos;		/* current manipulator hand setting */
	int setHandPos;		/* not used at the moment */

	int mid;		/* current motion ID */

	RCI_RBT *rbt;		/* robot reference structure */
	HOW *how;		/* robot input structure */
	KYN *kyn;		/* pointers to the manipulator KYN, */
	JLS *jls;		/* JLS, and VAR structures. These are */
	void *var;		/* NULL if there is no robot */

	/* The following fields are used internally by the system */
	
	void *mvb;		/* motion value block */
	void *mr;		/* motion request template (not used) */
	void *mq;		/* motion queue */
	void *mcb;		/* motion control block */
	void *mrp;		/* motion request parameters (not used) */

} MANIP, *MANIP_PTR;

/* Mode bits for the MANIP data structure */

#define TRACKING_MODE		0x01	/* Track last target when idle */ 
#define INTEGRATE_MODE		0x02	/* Turn on joint servo integration */
#define T6O_EVAL		0x04	/* Evaluate mnp->j6o and mnp->t6o */
#define COMPLETE_AT_TB		0x08	/* Motions end at trans. midpoint */
#define RANGE_LIMITING		0x20	/* (not implemented) */ 
#define GRAVITY_FEEDFORWARD	0x40	/* Compute gravity term for servos */

#define DEFAULT_MANIP_MODES	(COMPLETE_AT_TB)

/* Argument interface to the 'setTransBinding' function */

typedef struct {
	int code;	    /* type of binding */
#define TRANS_CONST	    1	/* transform is constant */
#define TRANS_VARB	    2	/* transform value might change */
#define TRANS_EVAL	    3	/* transform bound to permanent fxn. */
#define TRANS_MOTION_EVAL   4	/* transform bound to motion activated fxn. */

	int (*fxn)();	    /* pointer to function */
	int arg;	    /* argument to give to function */
	MANIP *mnp;	    /* manipulator (for motion activated fxns) */
} TRANS_BINDING;

/* Get the RCI_RBT structure associated with a manipulator */
#define MANIP_ROBOT(m)	\
	((m)->rbtNum != UNDEF ? TASKROBOT(MANIP_TASK(m),(m)->rbtNum) : NULL)

/* Get the trajectory task CPU associated with a manipulator */
#define MANIP_CPU(m)	((m)->cpu)

/* Get the trajectory task associated with a manipulator */
#define MANIP_TASK(m)	findRciTask(m->taskNum)

/* Get the current motion ID */
#define getActiveMotionId(mnp)	(mnp)->mid

/* arguments passed to transform evaluation functions (this is an old
   interface, and used only if OLD_TFXN_ARGS is defined true
   in rccl/transCtrl.c) */

typedef struct {
	int arg;
	float scale;
	float time;
	MANIP *manip;
} TRANS_ARGS;

/* Maximum number of monitor functions */
#define MAX_MONITOR_FXNS	32

/* RCCL constants (defined in constants.c) */

extern VECT *xunit;
extern VECT *yunit;
extern VECT *zunit;

/* Macros to wait while a condition (p) is true, and
   until a condition (p) is true */

extern int rciTaskId;

#define waitWhile(p) do { if (rciTaskId == 0) rcclBlock(); } while(p);
#define waitFor(p)   do { if (rciTaskId == 0) rcclBlock(); } while(!(p));

/* Macros to wait for the finish and start of a motion controlled by (e) */

#define waitForStop(e)		waitfor (motionStopCode(e) >= 0)
#define waitForStart(e) 	waitfor (motionStartCode(e) >= 0)
#define waitForCompleted(mnp)	if (rciTaskId == 0) rcclBlock(); \
				waitfor ((mnp)->completed)

/* Old function names kept around for compatibility */

#define makeposition	 makePosition
#define freeposition(p)	 freePosition(p)
#define getTrans(n)	 getTransByName(n)
#define findTrans(a)	 getTransByAddr(a)
#define getManip(n)	 getManipByName(n)
#define findManip(a)	 getManipByAddr(a)
#define rcclGetRate()	 rcclGetInterval()
#define rcclSetRate()	 rcclSetInterval()
#define waitfor(e)	 waitFor(e)
#define waitwhile(e)	 waitWhile(e)
#define gentr_eul	 allocTransEul
#define gentr_pao	 allocTransPao
#define gentr_rot	 allocTransRot
#define gentr_rpy	 allocTransRpy
#define gentr_trsl	 allocTransXyz

#define move	movep

/*  Function definitions */

#ifndef PROTO
#ifndef NO_PROTOTYPES
#define PROTO(x) x
#else
#define PROTO(x) ()
#endif
#endif

extern JNTS *copyJnts PROTO((JNTS*, JNTS*));
extern JNTS *addJnts PROTO((JNTS*, JNTS*, JNTS*));
extern JNTS *subtractJnts PROTO((JNTS*, JNTS*, JNTS*));
extern JNTS *scaleJnts PROTO((JNTS*, double, JNTS*));
extern JNTS *combineJnts PROTO((JNTS*, double, JNTS*, double, JNTS*));
extern JNTS *zeroJnts PROTO((JNTS*));
extern JNTS *extrapJnts PROTO((JNTS*, JNTS*, JNTS*, double));
extern JNTS *degreeToJnts PROTO((JNTS*, float*));
extern float *jntsToDegree PROTO((float*, JNTS*));
extern JNTS *initJntsByName PROTO((JNTS*, const char*));
extern JNTS *initJntsByManip PROTO((JNTS*, MANIP*));

extern TRSF *allocTrans PROTO((const char*, int));
extern freeTrans PROTO((TRSF*));
extern TRSF *getTransByName PROTO((const char*));
extern TRSF *getTransByAddr PROTO((TRSF*));
extern TRSF *readTrans PROTO((TRSF*, TRSF*));
extern TRSF *writeTrans PROTO((TRSF*, TRSF*));
extern char *getTransName PROTO((TRSF*));

extern TRSF *allocTransEul PROTO((const char*, int, double, double, double, 
		double, double, double));
extern TRSF *allocTransPao PROTO((const char*, int, double, double, double, 
		double, double, double, double, double, double));
extern TRSF *allocTransRot PROTO((const char*, int, double, double, double, 
		VECT*, double));
extern TRSF *allocTransRpy PROTO((const char*, int, double, double, double, 
		double, double, double));
extern TRSF *allocTransXyz PROTO((const char*, int, double, double, double));

/* extern comply PROTO((MANIP*,...)); */
extern lock PROTO((MANIP*, const char*));
extern long setComply PROTO((MANIP*, unsigned long, FORCE*));
extern getComply PROTO((MANIP*, unsigned long*, FORCE*));
/* extern limit PROTO((MANIP*,...)); */
extern setForceLimit PROTO((MANIP*, unsigned long, FORCE*));
extern setDispLimit PROTO((MANIP*, unsigned long, DIFF*));
/* extern distance PROTO((MANIP*,...)); */
extern setDistance PROTO((MANIP*, TRSF*));

extern POS *getPositionByName PROTO((const char*));
extern POS *getPositionByAddr PROTO((POS*));
extern POS *copyPosition PROTO((POS*));
extern freePosition PROTO((POS*));

/* extern POS *makePosition PROTO((const char*,...)); */
extern POS *makePosition();

extern getCartAccel PROTO((MANIP*, float*, float*));
extern getJointAccel PROTO((MANIP*, JNTS*));
extern float getAccelScale PROTO((MANIP*));
extern float getSpeed PROTO((MANIP*));
extern getCartVel PROTO((MANIP*, float*, float*));
extern getJointVel PROTO((MANIP*, JNTS*));
extern float getJvelScale PROTO((MANIP*));
extern getDefaultCartVel PROTO((MANIP*, float*, float*));
extern getDefaultJointVel PROTO((MANIP*, JNTS*));

extern getActiveCartAccel PROTO((MANIP*, float*, float*));
extern getActiveJointAccel PROTO((MANIP*, JNTS*));
extern getActiveComply PROTO((MANIP*, unsigned long*, FORCE*));
extern getActiveConf PROTO((MANIP*, char*));
extern getActiveMotionCounts PROTO((MANIP*, int*, int*));
extern getActiveCount PROTO((MANIP*));
extern getActiveDispLimit PROTO((MANIP*, unsigned long*, DIFF*));
extern getActiveForceLimit PROTO((MANIP*, unsigned long*, FORCE*));
extern getActiveMod PROTO((MANIP*));
extern getActiveCartVel PROTO((MANIP*, float*, float*));
extern getActiveJointVel PROTO((MANIP*, JNTS*));

extern MANIP *getManipByName PROTO((const char*));
extern MANIP *getManipByAddr PROTO((MANIP*));

extern jbiasSelect PROTO((MANIP*, int));
extern jbiasMotionEval PROTO((int (*)(), int, MANIP*));
extern setJbiasConst PROTO((MANIP*));
extern setJbiasVarb PROTO((MANIP*));

extern motionStatus PROTO((int));
extern motionStopCode PROTO((int));
extern motionStartCode PROTO((int));
extern float motionScale PROTO((int));

extern move PROTO((MANIP*, POS*));
extern movej PROTO((MANIP*, JNTS*));
extern maintain PROTO((MANIP*, POS*));
extern numMotionsQueued PROTO((MANIP*));
extern checkMotionQueue PROTO((MANIP*, POS*));

extern rcclAbortReset PROTO(());
extern rcclActive PROTO(());
extern rcclBlock PROTO(());
extern MANIP *rcclCreate PROTO((const char*, unsigned long));
extern rcclDelete PROTO((MANIP*));
extern unsigned long rcclSetOptions PROTO((unsigned long));
extern unsigned long rcclClearOptions PROTO((unsigned long));
extern unsigned long rcclPutOptions PROTO((unsigned long));
extern unsigned long rcclGetOptions PROTO((unsigned long));
extern rcclSetSimulator PROTO((const char *));
extern char *rcclGetSimulator PROTO(());
extern rcclPause PROTO((double, double));
extern rcclResume PROTO(());
extern rcclIsPaused PROTO(());
extern rcclWasPaused PROTO(());
extern rcclScheduling PROTO((int));
extern rcclSetInterval PROTO((int));
extern rcclGetInterval PROTO(());
extern unsigned long rcclSetModes PROTO((MANIP*, unsigned long));
extern unsigned long rcclClearModes PROTO((MANIP*, unsigned long));
extern unsigned long rcclPutModes PROTO((MANIP*, unsigned long));
extern unsigned long rcclGetModes PROTO((MANIP*, unsigned long));
extern unsigned long rcclGetActiveModes PROTO((MANIP*, unsigned long));
extern rcclStart PROTO(());
extern rcclRelease PROTO((int));
float rcclSysTime PROTO(());
extern rcclTeach PROTO((MANIP*, const char*, TRSF*));

extern runMonitorFxn PROTO((MANIP*, int (*)(), int));
extern deleteMonitorFxn PROTO((MANIP*, int));
extern runMotionFxn PROTO((MANIP*, int (*)(), int));

extern setCompletedSig PROTO((MANIP*, int));
extern setStopSig PROTO((MANIP*, int, int));

extern setCartAccel PROTO((MANIP*, double, double));
extern setJointAccel PROTO((MANIP*, JNTS*));
extern float setAccelScale PROTO((MANIP*, double));

extern setConf PROTO((MANIP*, const char*));

extern setMod PROTO((MANIP*,int));
extern getMod PROTO((MANIP*));
extern setMotionFlag PROTO((MANIP*, int));
extern setMotionHold PROTO((MANIP*));
extern setMotionQueueSize PROTO((MANIP*, int));
extern getMotionQueueSize PROTO((MANIP*));
extern float setSpeed PROTO((MANIP*, double));
extern setCartVel PROTO((MANIP*, double, double));
extern setJointVel PROTO((MANIP*, JNTS*));
extern float setJvelScale PROTO((MANIP*, double));
extern setTime PROTO((MANIP*, double, double));
extern setTransBinding PROTO((TRSF*, TRANS_BINDING*));
extern getTransBinding PROTO((TRSF*, TRANS_BINDING*));
extern setTransConst PROTO((TRSF*));
extern setTransVarb PROTO((TRSF*));
extern transEval PROTO((TRSF*, int (*)(), int));
extern transMotionEval PROTO((TRSF*, int (*)(), int, MANIP*));
extern TRSF *solveTrans PROTO((TRSF*, POS*, TRSF*, TRSF*));
extern TRSF *solveChain PROTO((TRSF*, POS*, TRSF*, TRSF*, TRSF*));
extern TRSF *solveInvChain PROTO((TRSF*, POS*, TRSF*, TRSF*, TRSF*));
extern stop PROTO((MANIP*, double));
extern stopCurrentMotion PROTO((MANIP*, int));
extern flushMotionQueue PROTO((MANIP*));
extern stopMotion PROTO((int, int));
extern startMotion PROTO((int, int));
extern updateTrans PROTO((MANIP*, TRSF*, POS*, TRSF*, TRSF*));
extern delay PROTO((double));

extern float motionStartTime PROTO((int));
extern float motionStopTime PROTO((int));
extern float rcclGetMotionDelay PROTO(());
extern char *rcclGetSimulator PROTO(());

#endif
