/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*----------McGill-Computer-Vision-and-Robotics-Laboratory--C-File-----------
 *
 * FILE NAME:	h/rccl.h
 *
 * DESCRIPTION: RCCL structure definitions, macros, and external entries
 *		for use at the user level.
 *
 * AUTHOR:	Vincent Hayward, Purdue University, 1983.
 *		CVaRL installation and modification by John Lloyd.
 *
 * NOTES:
 *
 *-------------McGill-Computer-Vision-and-Robotics-Laboratory--------------*/

#ifndef RCCLH
#define RCCLH

#ifndef FILE
#include <stdio.h>              /* included here for safety             */
#endif
#ifndef HUGE
#include <math.h>               /* also included for safety             */
#endif

/* Conversion code typedefs, to use with some of the read functions */

#ifndef CONVERTH
#include "convert.h"
#endif

#define YES     	1	/* Our particular flavour of TRUE ...	*/
#define NO      	0	/*   and our flavour of FALSE ...	*/
#define UNDEFINED      -1	/*   and undefined.			*/

/* ----- Synchronization definitions ----- */

#define NUM_EFTS   256	/* number of available event flag triples */

/* EFT 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 M_SYS	    0x80 	/* start/stop code reserved for RCCL system */

#define ON_FORCE	(M_SYS | 1)	/* stop on forc condition */
#define ON_DIFF		(M_SYS | 2)	/* stop on diff motion condition */
#define ON_NORMAL	(M_SYS | 3)	/* normal start/stop condition */
#define ON_ABORT	(M_SYS | 4)	/* request was aborted before start */

/* Configuration codes, for the 'conf' field of a TRSF. This is a CVaRL */
/* local thing; not used very much yet				        */

#define CONF_RIGHT	0x01
#define CONF_UP		0x02
#define CONF_FLIP	0x04

#define EQ              1       /* Tokens used to define position eqns. */
#define TL              2       /*   in the arg list of "makeposition()"*/

/* ----- Typedefs for RCCL (User level only) ----- */

typedef int bool;		/* Logical type				*/
typedef float real;		/* A more familiar name for "floats"    */
typedef int event;		/* A semaphore				*/

typedef struct vector {		/* Standard three component vector	*/
		real x, y, z;
} VECT, *VECT_PTR;

typedef int(* TRFN)();		/* A transform manipulating function	*/

typedef struct transform {	/* 4X4 homogeneous transform structure: */
		char *name;	/*   Name of the transform		*/
		TRFN fn;	/*   Function that (may) modify it 	*/
		VECT n, o, a, p;/*   4 columns of the matrix		*/
		int timeval;	/*   Time function was last evaluated	*/
		long conf;	/*   Configuration (optional)		*/
} TRSF, *TRSF_PTR;

typedef struct jns {		/* Values for the manipulator joint	*/
		char *conf;	/*   String describing the orientation  */
		real th1, th2, th3, th4, th5, th6;	/* Joint values */
} JNTS, *JNTS_PTR;

typedef struct {		/* User defined position structure	*/
		char *name;	/*   Name of the position		*/

				/* 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 */
} POS, *POS_PTR;

typedef struct force {		/* Force descriptor with 6 components:	*/
		VECT f, m;	/*   'f' is 3 linear forces, and	*/
} FORCE, *FORCE_PTR;		/*   'm' is 3 moments			*/

typedef struct diff {		/* Descriptor for differential motion:	*/
		VECT t, r;	/*   't' is 3 linear displacements, and */
} DIFF, *DIFF_PTR;		/*   'r' is 3 differential rotations.	*/

typedef struct {		/* Quaternion descriptor		*/
		float c;	/*   'c' - scaler part ( cos(theta/2) ) */
		VECT v;		/*   'v' - vector part ( u * sin(theta/2) )*/
} QTRN;

typedef struct {               	/* drive parameters:            	*/
	real dx, dy, dz;	/*  translations			*/
	real dthe, dphi, dpsi;	/*  angles				*/
} DRIVEP;

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

extern POS_PTR  makeposition();

extern TRSF_PTR assigntr(),	copyTrsfXyz(),	copyTrsfNoa(),
		multTrsf(),	trmultinp(),	trmultinv(),
		invertTrsf(),	invertinp(),	xyzToTrsf(),
		vaoToTrsf(),		rotToTrsf(),		eulToTrsf(),
		rpyToTrsf(),		multTrsfXyz(),	multTrsfVao(),
		multTrsfRot(),		multTrsfEul(),		multTrsfRpy(),
		diffToTrsf();

extern DIFF_PTR copyDiff(),	trsfToDiff(),	transDiff();

extern FORCE_PTR copyForce(),	 transForce();

extern VECT_PTR copyVectt(),	crossVect(),	unitVect();

extern float    dotVect(),		motion_scale();

/* ----- RCCL global variables (defined in shared.c) ----- */

extern JNTS	rcl_j6,		
		rcl_j6delta;

extern TRSF	rcl_t6,
		rcl_rest,
		rcl_here,
		rcl_tool;

extern POS	*rcl_goalPosition,
		*rcl_lastPosition,
		*rcl_there,
		*rcl_park;

extern event 	rcl_completed;
extern float	rcl_time,
		rcl_sampleTime,
		rcl_scale;
extern int	rcl_state;

extern int	rcl_handPosition;

extern char	rcl_flushQueue,
		rcl_terminateMotion,
		rcl_pauseMotion,
		rcl_resumeMotion;

extern int	rcl_pauseTime,
		rcl_pauseTransTime,
		rcl_setHandPosition;

extern TRSF 	rcl_identity;
extern VECT	rcl_xunit,
		rcl_yunit,
		rcl_zunit;

extern FILE	*rcl_fpi,
		*rcl_fpo,
		*rcl_fpg[];

extern int	rcl_fddb,
		rcl_printFlag,
		rcl_forceControl;

/* ----- "slang" definitions for global variables ----- */

#define t6		(&rcl_t6)
#define tl		(&rcl_tool)
#define j6		(&rcl_j6)
#define jd		(&rcl_j6delta)
#define here		(&rcl_here)
#define rest		(&rcl_rest)
#define unitr		(&rcl_identity)
#define xunit		(&rcl_xunit)
#define yunit		(&rcl_yunit)
#define zunit		(&rcl_zunit)

#define goalpos		rcl_goalPosition
#define lastpos		rcl_lastPosition
#define there		rcl_there
#define park		rcl_park
#define completed	rcl_completed

#define rtime		rcl_time
#define stime		rcl_sampleTime

#define dbf		rcl_fddb

/* ----- Macros ----- */

/* macros to wait until a condition (p) is true, while a condition (p) */
/* is true, and for an event (e) to occur */

#define waituntil(p)	       {while(!(p)) nap(stime/1000.);}
#define waitwhile(p)	       {while(p) nap(stime/1000.);}
#define waitfor(e)	       {++(e); \
				while((e) > 0) \
				   nap (stime/1000.);}
#define waitas(p)	       {while(!(p)) nap(stime/1000.);}

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

#define waitForStop(e)	waituntil (motion_stop_value(e) != 0)
#define waitForStart(e) 	waituntil (motion_start_value(e) != 0)

/* Macros to specify a motion in either cartesian or joint mode, and,	*/
/* in joint mode, to optionally change the configuartion.		*/
/* The segment transist time is given by 'ts', the acceleration time    */
/* for transitioning to that path segment is 'ta', the position to move */
/* to is 'p', and the new configuration is 'cf'.			*/

#define movecart(p, ta, ts)\
      {setmod('c'); setime(ta, ts); move(p);}

#define movejnts(p, ta, ts)\
      {setmod('j'); setime(ta, ts); move(p);}	
							
#define moveconf(p, ta, ts, cf)\
      {setconf(cf); setmod('j'); setime(ta, ts); move(p);}

/* Macros to free memory space for transforms */

#define freetrans(t)            {freetr((char *)t); t = NULL;}

/* Macro to free the memory space associated with a position and then	*/
/* NULL the pointer. Component transform space is not freed.		*/

#define freeposition(p)         {freepos(p); p = NULL;}

#endif
