/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/* test some of the basic motion control primitives */

#ifdef VXWORKS_CFG
#define main testSet_
#endif

#include "../h/rccl.h"
#include "../h/errorCodes.h"

#include "../rccl/testSet.h"

extern int ptMonitor();

PT_BLK *cbk;

JNTS testJnts1 = { 1.1, 2.2, 3.3, 4.4, 5.5, 6.6 };
JNTS testJnts2 = { .001, .01, .1, 123.0, 456.0, 789.0 };
JNTS defaultJnts = { F_DEFAULT, F_DEFAULT, F_DEFAULT, 
		     F_DEFAULT, F_DEFAULT, F_DEFAULT };

FORCE testcp = {  1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
FORCE testfl = {  1.1, 2.1, 3.1, 4.1, 5.1, 6.1 };
DIFF testdl = {  1.9, 2.9, 3.9, 4.9, 5.9, 6.9 };

#define DONTCARE	-2
#define PLANNING	1
#define CTRL		0

void *mqcheck;

#define EPSILON 	1e-6

#define FEQ(a,b)	(FABS(2*((a)-(b))/((a)+(b)))<EPSILON)

cmpVf (v1, v2, size)
float *v1, *v2;
int size;
{
	int j;
	for (j=0; j<size; j++)
	 { if (!FEQ(v1[j], v2[j]))
	    { return (1);
	    }
	 }
	return (0);
}

main()
{
	MANIP *obj;
	MANIP *mnp;
	int mid1, mid2, mid3;
	TRSF *objTarget;
	TRSF *mnpTarget;
	POS *objPos;
	POS *mnpPos;
	int tau, sigma;
	int tauSet, sigmaSet;
	float tvel, rvel;
	float tvelSet, rvelSet;
	float tvelReal, rvelReal;
	float tacc, racc;
	float taccSet, raccSet;
	float taccReal, raccReal;
	unsigned long mask, mask1, mask2;
	JNTS jvel, jacc;
	JNTS jvelReal, jaccReal;
	JNTS jnts, jntsSet;
	FORCE force, forceSet;
	DIFF diff, diffSet;
	float floatval;
	unsigned long maskSet, maskSetMnp, maskSetObj;
	float interval;
	int k;
	int defaultModesMnp;
	int defaultModesObj;
	int defaultOptions;
	TRSF *trsfPtr[2];
	POS *posPtr[2];
	void *memPtr[2];

	/* simple test of the allocators */

	rcclSetOptions (RCCL_ERROR_EXIT);

	objTarget = allocTrans (NULL, UNDEF);
	mnpTarget = allocTrans (NULL, UNDEF);

	rcclClearOptions (RCCL_ERROR_EXIT);	

	if ((trsfPtr[0] = allocTrans(NULL, UNDEF)) == NULL)
	 { errorExit (300, NULL);
	 }
	if (freeTrans(trsfPtr[0]) != 1)
	 { errorExit (301, NULL);
	 }
	if ((posPtr[0] = makePosition(NULL, T6, EQ, mnpTarget, TL, T6))==NULL)
	 { errorExit (302, NULL);
	 }
	if (freePosition(posPtr[0]) != 1)
	 { errorExit (303, NULL);
	 }
	if ((trsfPtr[1] = allocTrans(NULL, UNDEF)) == NULL)
	 { errorExit (304, NULL);
	 }
	if (trsfPtr[0] != trsfPtr[1])
	 { errorExit (305, "pointers are different\n");
	 }
	freeTrans (trsfPtr[1]);
	if ((posPtr[1] = makePosition(NULL, T6, EQ, mnpTarget, TL, T6))==NULL)
	 { errorExit (306, NULL);
	 }
	if (posPtr[0] != posPtr[1])
	 { errorExit (307, "pointers are different\n");
	 }
	freePosition (posPtr[1]);
	
	if ((trsfPtr[0] = allocTrans("foo", UNDEF)) == NULL)
	 { errorExit (308, NULL);
	 }
	if ((posPtr[0] = makePosition("foo", T6, EQ, mnpTarget, TL, T6))==NULL)
	 { errorExit (309, NULL);
	 }
	if ((memPtr[0] = allocMem ("foo", 35, UNDEF)) == NULL)
	 { errorExit (310, NULL);
	 }
	if ((trsfPtr[1] = allocTrans("foo", UNDEF)) != NULL ||
	     getErrorCode(0) != EDuplicate)
	 { errorExit (311, NULL);
	 }
	clearErrors();
	if ((posPtr[1] = makePosition("foo",T6,EQ,mnpTarget,TL,T6))!=NULL ||
	     getErrorCode(0) != EDuplicate)
	 { errorExit (312, NULL);
	 }
	clearErrors();
	if ((memPtr[1] = allocMem ("foo", 35, UNDEF)) != NULL ||
	     getErrorCode(0) != EDuplicate)
	 { errorExit (313, NULL);
	 }
	clearErrors();

	rcclSetOptions (RCCL_ERROR_EXIT);

	freePosition (posPtr[0]);
	freeTrans (trsfPtr[0]);
	freeMem (memPtr[0]);

	obj = rcclCreate ("obj", 0);
	mnp = rcclCreate (getDefaultRobot(), 0);
	cbk = ALLOC_MEM (NULL, PT_BLK, UNDEF);
	objPos = makePosition (NULL, T6, EQ, objTarget, TL, T6);
	mnpPos = makePosition (NULL, T6, EQ, mnpTarget, TL, T6);

	rcclStart();
	runMonitorFxn (obj, ptMonitor, (int)cbk);
	*mnpTarget = *mnp->here;

	/* test the numMotionsQueued() routine */

	CHECKnumMotionsQueued (200, obj, 0, PLANNING);	
	CHECKnumMotionsQueued (201, obj, 0, CTRL);	
	CHECKnumMotionsQueued (202, mnp, 0, PLANNING);	
	CHECKnumMotionsQueued (203, mnp, 0, CTRL);	

	setTime (obj, F_DEFAULT, F_UNDEF);
	mid1 = move (obj, objPos);
	setTime (obj, F_DEFAULT, F_UNDEF);
	mid2 = move (obj, objPos);
	setTime (obj, F_DEFAULT, F_UNDEF);
	mid3 = move (obj, objPos);
	waitForStart (mid1);

	CHECKnumMotionsQueued (204, obj, 2, PLANNING);	
	CHECKnumMotionsQueued (205, obj, 2, CTRL);	
	CHECKnumMotionsQueued (206, mnp, 0, PLANNING);	
	CHECKnumMotionsQueued (207, mnp, 0, CTRL);	

	stopMotion (mid1, 0);
	waitForStart (mid2);
	CHECKnumMotionsQueued (208, obj, 1, PLANNING);	
	CHECKnumMotionsQueued (209, obj, 1, CTRL);	

	stopCurrentMotion (obj, 0);
	waitForStop (mid2);
	CHECKnumMotionsQueued (210, obj, 0, PLANNING);	
	CHECKnumMotionsQueued (211, obj, 0, CTRL);	

	stopMotion (mid3, 0);

	/* test rcclSetOptions */

	maskSet = rcclGetOptions(~0);
	defaultOptions = maskSet;
	CHECKrcclGetOptions (220, obj, maskSet, PLANNING);
	CHECKrcclGetOptions (221, obj, maskSet, CTRL);

	maskSet |= 0x12340000;
	rcclPutOptions (maskSet);
	CHECKrcclGetOptions (222, obj, maskSet, PLANNING);
	CHECKrcclGetOptions (223, obj, maskSet, CTRL);

	maskSet |= 0xffff0000;	
	rcclSetOptions (0xffff0000);
	CHECKrcclGetOptions (224, obj, maskSet, PLANNING);

	CHECKrcclGetOptions (225, obj, maskSet, CTRL);
	maskSet &= ~0x12340000;	
	rcclClearOptions (0x12340000);
	CHECKrcclGetOptions (226, obj, maskSet, PLANNING);
	CHECKrcclGetOptions (227, obj, maskSet, CTRL);

	maskSet = rcclGetOptions(~0);
	maskSet |= 0x12340000;
	cbk->intval[0] = maskSet;
	askMonitor (obj, RCCL_PUT_OPTIONS);
	CHECKrcclGetOptions (228, obj, maskSet, PLANNING);
	CHECKrcclGetOptions (229, obj, maskSet, CTRL);

	maskSet |= 0xffff0000;	
	cbk->intval[0] = 0xffff0000;
	askMonitor (obj, RCCL_SET_OPTIONS);
	CHECKrcclGetOptions (230, obj, maskSet, PLANNING);
	CHECKrcclGetOptions (231, obj, maskSet, CTRL);

	maskSet &= ~0x12340000;	
	cbk->intval[0] = 0x12340000;
	askMonitor (obj, RCCL_CLEAR_OPTIONS);
	CHECKrcclGetOptions (232, obj, maskSet, PLANNING);
	CHECKrcclGetOptions (233, obj, maskSet, CTRL);

	rcclSetOptions (defaultOptions);

	/* test rcclSetModes */

	defaultModesMnp = maskSetMnp = rcclGetModes(mnp, ~0);
	defaultModesObj = maskSetObj = rcclGetModes(obj, ~0);
	CHECKrcclGetModes (234, obj, maskSetObj, PLANNING);
	CHECKrcclGetModes (235, obj, maskSetObj, CTRL);
	CHECKrcclGetActiveModes (234, obj, maskSetObj, PLANNING);
	CHECKrcclGetActiveModes (235, obj, maskSetObj, CTRL);
	CHECKrcclGetActiveModes (236, mnp, maskSetObj, PLANNING);
	CHECKrcclGetActiveModes (237, mnp, maskSetObj, CTRL);

	maskSetObj |= 0x12340000;
	rcclPutModes (obj, maskSetObj);
	CHECKrcclGetModes (238, obj, maskSetObj, PLANNING);
	CHECKrcclGetModes (239, obj, maskSetObj, CTRL);
	CHECKrcclGetModes (240, mnp, maskSetMnp, PLANNING);
	CHECKrcclGetModes (241, mnp, maskSetMnp, CTRL);

	/* assume no delayed modes in the 0x12340000 mask */

	CHECKrcclGetActiveModes (242, obj, maskSetObj, PLANNING);
	CHECKrcclGetActiveModes (243, obj, maskSetObj, CTRL);
	CHECKrcclGetActiveModes (244, mnp, maskSetMnp, PLANNING);
	CHECKrcclGetActiveModes (245, mnp, maskSetMnp, CTRL);

	maskSetObj |= 0xffff0000;	
	rcclSetModes (obj, 0xffff0000);
	CHECKrcclGetModes (246, obj, maskSetObj, PLANNING);
	CHECKrcclGetModes (247, obj, maskSetObj, CTRL);

	maskSetObj &= ~0x12340000;	
	rcclClearModes (obj, 0x12340000);
	CHECKrcclGetModes (248, obj, maskSetObj, PLANNING);
	CHECKrcclGetModes (249, obj, maskSetObj, CTRL);

	/* now check delayed modes */

	maskSetObj &= ~COMPLETE_AT_TB;
	rcclClearModes (obj, COMPLETE_AT_TB);
	CHECKrcclGetModes (250, obj, maskSetObj, PLANNING);
	CHECKrcclGetModes (251, obj, maskSetObj, CTRL);
	CHECKrcclGetActiveModes (252, obj,maskSetObj|COMPLETE_AT_TB,PLANNING);
	CHECKrcclGetActiveModes (253, obj,maskSetObj|COMPLETE_AT_TB,CTRL);

	setTime (obj, F_DEFAULT, F_UNDEF);
	mid1 = move (obj, objPos);
	waitForStart (mid1);

	CHECKrcclGetActiveModes (254, obj,maskSetObj,PLANNING);
	CHECKrcclGetActiveModes (255, obj,maskSetObj,CTRL);

	rcclSetModes (obj, COMPLETE_AT_TB);

	CHECKrcclGetActiveModes (256, obj,maskSetObj,PLANNING);
	CHECKrcclGetActiveModes (257, obj,maskSetObj,CTRL);

	stopCurrentMotion(obj, 0);
	setTime (obj, F_DEFAULT, F_UNDEF);
	mid1 = move (obj, objPos);
	waitForStart (mid1);

	maskSetObj |= COMPLETE_AT_TB;
	CHECKrcclGetActiveModes (258, obj,maskSetObj,PLANNING);
	CHECKrcclGetActiveModes (259, obj,maskSetObj,CTRL);

	rcclSetModes (obj, defaultModesObj);
	rcclSetModes (mnp, defaultModesMnp);
	stopCurrentMotion (obj, 0);

	/* do some basic boiler plate testing */

	tvelSet = PI*7.0;
	rvelSet = PI/7.0;
	setCartVel (mnp, PI*7.0, PI/7.0);
	CHECKgetCartVel (1, mnp, tvelSet, rvelSet, PLANNING);
	CHECKgetCartVel (2, mnp, tvelSet, rvelSet, CTRL);
	taccSet = PI*7.0;
	raccSet = PI/7.0;
	setCartAccel (mnp, PI*7.0, PI/7.0);
	CHECKgetCartAccel (3, mnp, taccSet, raccSet, PLANNING);
	CHECKgetCartAccel (4, mnp, taccSet, raccSet, CTRL);
	jntsSet = testJnts1;
	setJointVel (mnp, &jntsSet);
	CHECKgetJointVel (5, mnp, &jntsSet, PLANNING);
	CHECKgetJointVel (6, mnp, &jntsSet, CTRL);
	jntsSet = testJnts1;
	setJointAccel (mnp, &jntsSet);
	CHECKgetJointAccel (7, mnp, &jntsSet, PLANNING);
	CHECKgetJointAccel (8, mnp, &jntsSet, CTRL);
	setSpeed (mnp, PI);
	CHECKgetSpeed (9, mnp, PI, PLANNING);
	CHECKgetSpeed (10, mnp, PI, CTRL);
	setJvelScale (mnp, PI/12);
	CHECKgetJvelScale (11, mnp, PI/12, PLANNING);
	CHECKgetJvelScale (12, mnp, PI/12, CTRL);
	setAccelScale (mnp, PI*12);
	CHECKgetAccelScale (13, mnp, PI*12, PLANNING);
	CHECKgetAccelScale (14, mnp, PI*12, CTRL);

	getDefaultCartVel (mnp, &tvelSet, &rvelSet);
	CHECKgetDefaultCartVel (15, mnp, tvelSet, rvelSet, CTRL);
	getDefaultJointVel (mnp, &jntsSet);
	CHECKgetDefaultJointVel (16, mnp, &jntsSet, CTRL);

#define TESTSIZE 17
	k = setMotionQueueSize (mnp, TESTSIZE);
	CHECKgetMotionQueueSize (17, mnp, TESTSIZE, PLANNING);
	CHECKgetMotionQueueSize (18, mnp, TESTSIZE, CTRL);

	setMotionQueueSize (mnp, k);
	setCartVel (mnp, F_DEFAULT, F_DEFAULT);
	setCartAccel (mnp, F_DEFAULT, F_DEFAULT);
	setJointVel (mnp, &defaultJnts);
	setJointAccel (mnp, &defaultJnts);
	setSpeed (mnp, 1.0);
	setJvelScale (mnp, 1.0);
	setAccelScale (mnp, 1.0);

	getCartVel (mnp, &tvelSet, &rvelSet);
	CHECKgetActiveCartVel (19, mnp, tvelSet, rvelSet, PLANNING);
	CHECKgetActiveCartVel (20, mnp, tvelSet, rvelSet, CTRL);
	getCartAccel (mnp, &taccSet, &raccSet);
	CHECKgetActiveCartAccel (21, mnp, taccSet, raccSet, PLANNING);
	CHECKgetActiveCartAccel (22, mnp, taccSet, raccSet, CTRL);
	getJointVel (mnp, &jntsSet);
	CHECKgetActiveJointVel (23, mnp, &jntsSet, PLANNING);
	CHECKgetActiveJointVel (24, mnp, &jntsSet, CTRL);
	getJointAccel (mnp, &jntsSet);
	CHECKgetActiveJointAccel (25, mnp, &jntsSet, PLANNING);
	CHECKgetActiveJointAccel (26, mnp, &jntsSet, CTRL);

	/* test things with the object frame first */

	interval = rcclGetInterval()/1000.0;

	setCartVel (obj, 100.0, 90.0);
	CHECKgetCartVel (400, obj, 100.0, 90.0, PLANNING);
	CHECKgetActiveMotionCounts (401, obj, DONTCARE, UNDEF, PLANNING);
	CHECKgetActiveMotionCounts (402, obj, DONTCARE, UNDEF, CTRL);
	objTarget->p.x = 100.0;
	setTime (obj, 0.0, F_DEFAULT);
	mid1 = move (obj, objPos);
	waitForStart (mid1);
	sigmaSet = NINT(1/interval);
	CHECKgetActiveMotionCounts (403, obj, 0, sigmaSet, PLANNING);
	CHECKgetActiveMotionCounts (404, obj, 0, sigmaSet, CTRL);
	waitForStop (mid1);
	CHECKgetActiveMotionCounts (405, obj, DONTCARE, UNDEF, PLANNING);
	CHECKgetActiveMotionCounts (406, obj, DONTCARE, UNDEF, CTRL);
	objTarget->p.x = 0.0;
	setTime (obj, 100.0, 300.0);
	tauSet = MAX(NINT(100.0/(1000.0*interval)), 2);
	sigmaSet = MAX(NINT(300.0/(1000.0*interval)), 2*tauSet);
	mid1 = move (obj, objPos);
	waitForStart (mid1);
	CHECKgetActiveMotionCounts (407, obj, tauSet, sigmaSet, PLANNING);
	CHECKgetActiveMotionCounts (408, obj, tauSet, sigmaSet, CTRL);
	CHECKgetActiveMod (409, obj, 'c', PLANNING);
	waitForCompleted (mnp);

	/* check the comply and limit primitives on the object frame */

#define CP_MASK	0x5a5a
#define FL_MASK	0x1111
#define DL_MASK	0x2222

	CHECKgetComply (31, obj, 0, NULL, PLANNING);
	CHECKgetComply (32, obj, 0, NULL, CTRL);
	CHECKgetActiveComply (33, obj, 0, NULL, PLANNING);
	CHECKgetActiveComply (34, obj, 0, NULL, CTRL);
	CHECKgetActiveForceLimit (35, obj, 0, NULL, PLANNING);
	CHECKgetActiveForceLimit (36, obj, 0, NULL, CTRL);
	CHECKgetActiveDispLimit (37, obj, 0, NULL, PLANNING);
	CHECKgetActiveDispLimit (38, obj, 0, NULL, CTRL);

	setComply (obj, CP_MASK, &testcp);
	CHECKgetComply (39, obj, CP_MASK, &testcp, PLANNING);
	CHECKgetComply (40, obj, CP_MASK, &testcp, CTRL);
	CHECKgetActiveComply (41, obj, 0, NULL, PLANNING);
	CHECKgetActiveComply (42, obj, 0, NULL, CTRL);

	/* now setup force and diff motion limits */

	setDispLimit (obj, DL_MASK, &testdl);
	setForceLimit (obj, FL_MASK, &testfl);
	CHECKgetActiveForceLimit (43, obj, 0, NULL, PLANNING);
	CHECKgetActiveForceLimit (44, obj, 0, NULL, CTRL);
	CHECKgetActiveDispLimit (45, obj, 0, NULL, PLANNING);
	CHECKgetActiveDispLimit (46, obj, 0, NULL, CTRL);

	/* now queue a motion */

	setTime (obj, F_DEFAULT, 2000.0);
	mid1 = move (obj, objPos);
	waitForStart (mid1);	

	CHECKgetActiveComply (47, obj, CP_MASK, &testcp, PLANNING);
	CHECKgetActiveComply (48, obj, CP_MASK, &testcp, CTRL);
	CHECKgetActiveForceLimit (49, obj, FL_MASK, &testfl, PLANNING);
	CHECKgetActiveForceLimit (50, obj, FL_MASK, &testfl, CTRL);
	CHECKgetActiveDispLimit (51, obj, DL_MASK, &testdl, PLANNING);
	CHECKgetActiveDispLimit (52, obj, DL_MASK, &testdl, CTRL);

	if (motionStatus (mid1) == M_FINISHED)
	 { printf ("Warning: (%d) motion finished early\n", 38);
	 }
	waitForStop (mid1);

	CHECKgetActiveComply (53, obj, 0, NULL, PLANNING);
	CHECKgetActiveComply (54, obj, 0, NULL, CTRL);
	CHECKgetActiveForceLimit (55, obj, 0, NULL, PLANNING);
	CHECKgetActiveForceLimit (56, obj, 0, NULL, CTRL);
	CHECKgetActiveDispLimit (57, obj, 0, NULL, PLANNING);
	CHECKgetActiveDispLimit (58, obj, 0, NULL, CTRL);

	/* now do another motion and check stickiness */

	setTime (obj, F_DEFAULT, 2000.0);

	mid1 = move (obj, objPos);
	waitForStart (mid1);	

	CHECKgetActiveComply (59, obj, CP_MASK, &testcp, PLANNING);
	CHECKgetActiveComply (60, obj, CP_MASK, &testcp, CTRL);
	CHECKgetActiveForceLimit (61, obj, 0, NULL, PLANNING);
	CHECKgetActiveForceLimit (62, obj, 0, NULL, CTRL);
	CHECKgetActiveDispLimit (63, obj, 0, NULL, PLANNING);
	CHECKgetActiveDispLimit (64, obj, 0, NULL, CTRL);

	/* now do a tracking motion */

	setDispLimit (obj, DL_MASK, &testdl);
	setForceLimit (obj, FL_MASK, &testfl);
	rcclSetModes (obj, TRACKING_MODE);
	setTime (obj, F_DEFAULT, 200.0);
	mqcheck = mnp->mq;	
	mid1 = move (obj, objPos);
	waitForCompleted (obj);

	CHECKgetActiveComply (47, obj, CP_MASK, &testcp, PLANNING);
	CHECKgetActiveComply (48, obj, CP_MASK, &testcp, CTRL);
	CHECKgetActiveForceLimit (49, obj, FL_MASK, &testfl, PLANNING);
	CHECKgetActiveForceLimit (50, obj, FL_MASK, &testfl, CTRL);
	CHECKgetActiveDispLimit (51, obj, DL_MASK, &testdl, PLANNING);
	CHECKgetActiveDispLimit (52, obj, DL_MASK, &testdl, CTRL);

	/* now kill the track */

	rcclClearModes (obj, TRACKING_MODE);
	rcclBlock();
	waitForStop (mid1);

	CHECKgetActiveComply (71, obj, 0, NULL, PLANNING);
	CHECKgetActiveComply (72, obj, 0, NULL, CTRL);
	CHECKgetActiveForceLimit (73, obj, 0, NULL, PLANNING);
	CHECKgetActiveForceLimit (74, obj, 0, NULL, CTRL);
	CHECKgetActiveDispLimit (75, obj, 0, NULL, PLANNING);
	CHECKgetActiveDispLimit (76, obj, 0, NULL, CTRL);

	CHECKgetMod (80, mnp, 'j', PLANNING);
	CHECKgetMod (81, mnp, 'j', CTRL);
	CHECKgetActiveMod (82, mnp, 'j', PLANNING);
	CHECKgetActiveMod (83, mnp, 'j', CTRL);

	setMod (mnp, 'c');
	CHECKgetMod (84, mnp, 'c', PLANNING);
	CHECKgetMod (85, mnp, 'c', CTRL);
	CHECKgetActiveMod (86, mnp, 'j', PLANNING);
	CHECKgetActiveMod (87, mnp, 'j', CTRL);

	setTime (mnp, F_DEFAULT, 1000.0);
	mid1 = move (mnp, mnpPos);
	waitForStart (mid1);
	CHECKgetActiveMod (88, mnp, 'c', PLANNING);
	CHECKgetActiveMod (89, mnp, 'c', CTRL);

	waitForStop (mid1);
	CHECKgetActiveMod (90, mnp, 'c', PLANNING);
	CHECKgetActiveMod (91, mnp, 'c', CTRL);

	/* check out the acceleration and velocity settings 
	   using zero motions */

	getActiveCartVel (mnp, &tvelReal, &rvelReal);
	getActiveCartAccel (mnp, &taccReal, &raccReal);
	getActiveJointVel (mnp, &jvelReal);
	getActiveJointAccel (mnp, &jaccReal);

	tvel = 100.0;
	rvel = 123.0;
	setCartVel (mnp, tvel, rvel);
	tacc = 1.2;
	racc = 2.0;
	setCartAccel (mnp, tacc, racc);
	jvel = testJnts1;
	setJointVel (mnp, &jvel);
	jacc = testJnts2;
	setJointAccel (mnp, &jacc);

	CHECKgetCartVel (100, mnp, tvel, rvel, PLANNING);
	CHECKgetCartVel (101, mnp, tvel, rvel, CTRL);
	CHECKgetCartAccel (102, mnp, tacc, racc, PLANNING);
	CHECKgetCartAccel (103, mnp, tacc, racc, CTRL);
	CHECKgetJointVel (104, mnp, &jvel, PLANNING);
	CHECKgetJointVel (105, mnp, &jvel, CTRL);
	CHECKgetJointAccel (106, mnp, &jacc, PLANNING);
	CHECKgetJointAccel (107, mnp, &jacc, CTRL);

	CHECKgetActiveCartVel (108, mnp, tvelReal, rvelReal, PLANNING);
	CHECKgetActiveCartVel (109, mnp, tvelReal, rvelReal, CTRL);
	CHECKgetActiveCartAccel (110, mnp, taccReal, raccReal, PLANNING);
	CHECKgetActiveCartAccel (111, mnp, taccReal, raccReal, CTRL);
	CHECKgetActiveJointVel (112, mnp, &jvelReal, PLANNING);
	CHECKgetActiveJointVel (113, mnp, &jvelReal, CTRL);
	CHECKgetActiveJointAccel (114, mnp, &jaccReal, PLANNING);
	CHECKgetActiveJointAccel (115, mnp, &jaccReal, CTRL);

	/* now post a motion */

	setTime (mnp, 0.0, 2000.0);
	mid1 = move (mnp, mnpPos);
	waitForStart (mid1);

	CHECKgetActiveCartVel (116, mnp, tvel, rvel, PLANNING);
	CHECKgetActiveCartVel (117, mnp, tvel, rvel, CTRL);
	CHECKgetActiveCartAccel (118, mnp, tacc, racc, PLANNING);
	CHECKgetActiveCartAccel (119, mnp, tacc, racc, CTRL);
	CHECKgetActiveJointVel (120, mnp, &jvel, PLANNING);
	CHECKgetActiveJointVel (121, mnp, &jvel, CTRL);
	CHECKgetActiveJointAccel (122, mnp, &jacc, PLANNING);
	CHECKgetActiveJointAccel (123, mnp, &jacc, CTRL);

	waitForStop (mid1);

	CHECKgetActiveCartVel (124, mnp, tvel, rvel, PLANNING);
	CHECKgetActiveCartVel (125, mnp, tvel, rvel, CTRL);
	CHECKgetActiveCartAccel (126, mnp, tacc, racc, PLANNING);
	CHECKgetActiveCartAccel (127, mnp, tacc, racc, CTRL);
	CHECKgetActiveJointVel (128, mnp, &jvel, PLANNING);
	CHECKgetActiveJointVel (129, mnp, &jvel, CTRL);
	CHECKgetActiveJointAccel (130, mnp, &jacc, PLANNING);
	CHECKgetActiveJointAccel (131, mnp, &jacc, CTRL);

	/* now test the scaling */

	CHECKgetSpeed (132, mnp, 1.0, PLANNING);
	CHECKgetSpeed (133, mnp, 1.0, CTRL);
	CHECKgetAccelScale (134, mnp, 1.0, PLANNING);
	CHECKgetAccelScale (135, mnp, 1.0, CTRL);

	setSpeed (mnp, 0.5);
	setAccelScale (mnp, 2.0);
	
	CHECKgetSpeed (136, mnp, 0.5, PLANNING);
	CHECKgetSpeed (137, mnp, 0.5, CTRL);
	CHECKgetAccelScale (138, mnp, 2.0, PLANNING);
	CHECKgetAccelScale (139, mnp, 2.0, CTRL);

	tvelReal = 0.5 * tvel;
	rvelReal = 0.5 * rvel;
	taccReal = 2.0 * tacc;
	raccReal = 2.0 * racc;
	scaleJnts (&jvelReal, 0.5, &jvel);
	scaleJnts (&jaccReal, 2.0, &jacc);

	CHECKgetCartVel (140, mnp, tvel, rvel, PLANNING);
	CHECKgetCartVel (141, mnp, tvel, rvel, CTRL);
	CHECKgetCartAccel (142, mnp, tacc, racc, PLANNING);
	CHECKgetCartAccel (143, mnp, tacc, racc, CTRL);
	CHECKgetJointVel (144, mnp, &jvel, PLANNING);
	CHECKgetJointVel (145, mnp, &jvel, CTRL);
	CHECKgetJointAccel (146, mnp, &jacc, PLANNING);
	CHECKgetJointAccel (147, mnp, &jacc, CTRL);

	CHECKgetActiveCartVel (148, mnp, tvel, rvel, PLANNING);
	CHECKgetActiveCartVel (149, mnp, tvel, rvel, CTRL);
	CHECKgetActiveCartAccel (150, mnp, tacc, racc, PLANNING);
	CHECKgetActiveCartAccel (151, mnp, tacc, racc, CTRL);
	CHECKgetActiveJointVel (152, mnp, &jvel, PLANNING);
	CHECKgetActiveJointVel (153, mnp, &jvel, CTRL);
	CHECKgetActiveJointAccel (154, mnp, &jacc, PLANNING);
	CHECKgetActiveJointAccel (155, mnp, &jacc, CTRL);

	/* now post a motion */

	setTime (mnp, 0.0, 2000.0);
	mid1 = move (mnp, mnpPos);
	waitForStart (mid1);

	CHECKgetActiveCartVel (156, mnp, tvelReal, rvelReal, PLANNING);
	CHECKgetActiveCartVel (157, mnp, tvelReal, rvelReal, CTRL);
	CHECKgetActiveCartAccel (158, mnp, taccReal, raccReal, PLANNING);
	CHECKgetActiveCartAccel (159, mnp, taccReal, raccReal, CTRL);
	CHECKgetActiveJointVel (160, mnp, &jvelReal, PLANNING);
	CHECKgetActiveJointVel (161, mnp, &jvelReal, CTRL);
	CHECKgetActiveJointAccel (162, mnp, &jaccReal, PLANNING);
	CHECKgetActiveJointAccel (163, mnp, &jaccReal, CTRL);

	waitForStop (mid1);

	CHECKgetJvelScale (164, mnp, 1.0, PLANNING);
	CHECKgetJvelScale (165, mnp, 1.0, CTRL);

	setJvelScale (mnp, 5.0);

	CHECKgetJvelScale (166, mnp, 5.0, PLANNING);
	CHECKgetJvelScale (167, mnp, 5.0, CTRL);

	CHECKgetJointVel (168, mnp, &jvel, PLANNING);
	CHECKgetJointVel (169, mnp, &jvel, CTRL);
	CHECKgetActiveJointVel (170, mnp, &jvelReal, PLANNING);
	CHECKgetActiveJointVel (171, mnp, &jvelReal, CTRL);

	scaleJnts (&jvelReal, 5.0, &jvelReal);

	/* now post a motion */

	setTime (mnp, 0.0, 1000.0);
	mid1 = move (mnp, mnpPos);
	waitForStart (mid1);

	CHECKgetActiveJointVel (172, mnp, &jvelReal, PLANNING);
	CHECKgetActiveJointVel (173, mnp, &jvelReal, CTRL);

	waitForStop (mid1);

	setMotionQueueSize (mnp, k);
	setCartVel (mnp, F_DEFAULT, F_DEFAULT);
	setCartAccel (mnp, F_DEFAULT, F_DEFAULT);
	setJointVel (mnp, &defaultJnts);
	setJointAccel (mnp, &defaultJnts);
	setSpeed (mnp, 1.0);
	setJvelScale (mnp, 1.0);
	setAccelScale (mnp, 1.0);

	printf ("\nROUTINES OK\n\n");

	rcclRelease(1);

}

askMonitor (mnp, com)
MANIP *mnp;
int com;
{
	cbk->mnp = mnp;
	cbk->com = com;
	rcclBlock();
	if (cbk->com != 0)
	 { fatalMsg (-1, "test monitor does not respond\n");
	 }
}

CHECKgetComply (mark, mnp, mask, force, plan)
int mark;
MANIP *mnp;
unsigned long mask;
FORCE *force;
int plan;
{
	FORCE checkforce;
	unsigned long checkmask;

	if (plan)
	 { getComply (mnp, &checkmask, &checkforce);
	 }
	else
	 { askMonitor (mnp, GET_COMPLY);
	   checkmask = cbk->ulongval[0];
	   checkforce = cbk->forceval;
	 }
	if (mask == 0)
	 { if (checkmask != 0)
	    { fatalMsg (-1, "(%d) mask = 0x%x\n", mark, checkmask);
	    }
	 }
	else
	 { if (checkmask != mask || cmpVf (&checkforce, force, 6) != 0)
	    { printf ("fatal (%d) -- mask = 0x%x\n", mark, checkmask);
	      printVf ("%g\n", (float*)&checkforce, 6);
	      exit (-1);
	    }
	 }
}

CHECKgetActiveComply (mark, mnp, mask, force, plan)
int mark;
MANIP *mnp;
unsigned long mask;
FORCE *force;
int plan;
{
	FORCE checkforce;
	unsigned long checkmask;

	if (plan)
	 { getActiveComply (mnp, &checkmask, &checkforce);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_COMPLY);
	   checkmask = cbk->ulongval[0];
	   checkforce = cbk->forceval;
	 }
	if (mask == 0)
	 { if (checkmask != 0)
	    { fatalMsg (-1, "(%d) mask = 0x%x\n", mark, checkmask);
	    }
	 }
	else
	 { if (checkmask != mask || cmpVf (&checkforce, force, 6) != 0)
	    { printf ("fatal (%d) -- mask = 0x%x\n", mark, checkmask);
	      printVf ("%g\n", (float*)&checkforce, 6);
	      exit (-1);
	    }
	 }
}

CHECKgetActiveForceLimit (mark, mnp, mask, force, plan)
int mark;
MANIP *mnp;
unsigned long mask;
FORCE *force;
int plan;
{
	FORCE checkforce;
	unsigned long checkmask;

	if (plan)
	 { getActiveForceLimit (mnp, &checkmask, &checkforce);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_FORCE_LIMIT);
	   checkmask = cbk->ulongval[0];
	   checkforce = cbk->forceval;
	 }
	if (mask == 0)
	 { if (checkmask != 0)
	    { fatalMsg (-1, "(%d) mask = 0x%x\n", mark, checkmask);
	    }
	 }
	else
	 { if (checkmask != mask || cmpVf (&checkforce, force, 6) != 0)
	    { printf ("fatal (%d) -- mask = 0x%x\n", mark, checkmask);
	      printVf ("%g\n", (float*)&checkforce, 6);
	      exit (-1);
	    }
	 }
}

CHECKgetActiveDispLimit (mark, mnp, mask, diff, plan)
int mark;
MANIP *mnp;
unsigned long mask;
DIFF *diff;
int plan;
{
	DIFF checkdiff;
	unsigned long checkmask;

	if (plan)
	 { getActiveDispLimit (mnp, &checkmask, &checkdiff);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_DIFF_LIMIT);
	   checkmask = cbk->ulongval[0];
	   checkdiff = cbk->diffval;
	 }
	if (mask == 0)
	 { if (checkmask != 0)
	    { fatalMsg (-1, "(%d) mask = 0x%x\n", mark, checkmask);
	    }
	 }
	else
	 { if (checkmask != mask || cmpVf (&checkdiff, diff, 6) != 0)
	    { printf ("fatal (%d) -- mask = 0x%x\n", mark, checkmask);
	      printVf ("%g\n", (float*)&checkdiff, 6);
	      exit (-1);
	    }
	 }
}

CHECKgetActiveCartVel (mark, mnp, tvel, rvel, plan)
int mark;
MANIP *mnp;
float tvel, rvel;
int plan;
{
	float checkTvel, checkRvel;

	if (plan)
	 { getActiveCartVel (mnp, &checkTvel, &checkRvel);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_CART_VEL);
	   checkTvel = cbk->floatval[0];
	   checkRvel = cbk->floatval[1];
	 }
	if (!FEQ(checkTvel, tvel) || !FEQ(checkRvel, rvel))
	 { fatalMsg (-1, "(%d) tvel = %g vs. %g, rvel = %g vs. %g\n",
			mark, checkTvel, tvel, checkRvel, rvel);
	 }
}

CHECKgetActiveCartAccel (mark, mnp, tacc, racc, plan)
int mark;
MANIP *mnp;
float tacc, racc;
int plan;
{
	float checkTacc, checkRacc;

	if (plan)
	 { getActiveCartAccel (mnp, &checkTacc, &checkRacc);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_CART_ACCEL);
	   checkTacc = cbk->floatval[0];
	   checkRacc = cbk->floatval[1];
	 }
	if (!FEQ(checkTacc, tacc) || !FEQ(checkRacc, racc))
	 { fatalMsg (-1, "(%d) tacc = %g vs. %g, racc = %g vs. %g\n",
			mark, checkTacc, tacc, checkRacc, racc);
	 }
}

CHECKgetActiveJointVel (mark, mnp, jvel, plan)
int mark;
MANIP *mnp;
JNTS *jvel;
int plan;
{
	JNTS checkJvel;

	if (plan)
	 { getActiveJointVel (mnp, &checkJvel);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_JOINT_VEL);
	   checkJvel = cbk->jntsval;
	 }
	if (cmpVf (&checkJvel, jvel, 6) != 0)
	 { printf ("fatal (%d) --\n", mark);
	   printVf ("%g vs.\n", checkJvel.v, 6);
	   printVf ("%g\n", jvel->v, 6);
	   exit (-1);
	 }
}

CHECKgetActiveJointAccel (mark, mnp, jacc, plan)
int mark;
MANIP *mnp;
JNTS *jacc;
int plan;
{
	JNTS checkJacc;

	if (plan)
	 { getActiveJointAccel (mnp, &checkJacc);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_JOINT_ACCEL);
	   checkJacc = cbk->jntsval;
	 }
	if (cmpVf (&checkJacc, jacc, 6) != 0)
	 { printf ("fatal (%d) --\n", mark);
	   printVf ("%g\n", checkJacc.v, 6);
	   exit (-1);
	 }
}

CHECKgetCartVel (mark, mnp, tvel, rvel, plan)
int mark;
MANIP *mnp;
float tvel, rvel;
int plan;
{
	float checkTvel, checkRvel;

	if (plan)
	 { getCartVel (mnp, &checkTvel, &checkRvel);
	 }
	else
	 { askMonitor (mnp, GET_CART_VEL);
	   checkTvel = cbk->floatval[0];
	   checkRvel = cbk->floatval[1];
	 }
	if (!FEQ(checkTvel, tvel) || !FEQ(checkRvel, rvel))
	 { fatalMsg (-1, "(%d) tvel = %g vs. %g, rvel = %g vs. %g\n",
			mark, checkTvel, tvel, checkRvel, rvel);
	 }
}

CHECKgetDefaultCartVel (mark, mnp, tvel, rvel, plan)
int mark;
MANIP *mnp;
float tvel, rvel;
int plan;
{
	float checkTvel, checkRvel;

	if (plan)
	 { getDefaultCartVel (mnp, &checkTvel, &checkRvel);
	 }
	else
	 { askMonitor (mnp, GET_DEFAULT_CART_VEL);
	   checkTvel = cbk->floatval[0];
	   checkRvel = cbk->floatval[1];
	 }
	if (!FEQ(checkTvel, tvel) || !FEQ(checkRvel, rvel))
	 { fatalMsg (-1, "(%d) tvel = %g vs. %g, rvel = %g vs. %g\n",
			mark, checkTvel, tvel, checkRvel, rvel);
	 }
}

CHECKgetCartAccel (mark, mnp, tacc, racc, plan)
int mark;
MANIP *mnp;
float tacc, racc;
int plan;
{
	float checkTacc, checkRacc;

	if (plan)
	 { getCartAccel (mnp, &checkTacc, &checkRacc);
	 }
	else
	 { askMonitor (mnp, GET_CART_ACCEL);
	   checkTacc = cbk->floatval[0];
	   checkRacc = cbk->floatval[1];
	 }
	if (!FEQ(checkTacc, tacc) || !FEQ(checkRacc, racc))
	 { fatalMsg (-1, "(%d) tacc = %g vs. %g, racc = %g vs. %g\n",
			mark, checkTacc, tacc, checkRacc, racc);
	 }
}

CHECKgetJointVel (mark, mnp, jvel, plan)
int mark;
MANIP *mnp;
JNTS *jvel;
int plan;
{
	JNTS checkJvel;

	if (plan)
	 { getJointVel (mnp, &checkJvel);
	 }
	else
	 { askMonitor (mnp, GET_JOINT_VEL);
	   checkJvel = cbk->jntsval;
	 }
	if (cmpVf (&checkJvel, jvel, 6) != 0)
	 { printf ("fatal (%d) --\n", mark);
	   printVf ("%g\n", checkJvel.v, 6);
	   exit (-1);
	 }
}

CHECKgetDefaultJointVel (mark, mnp, jvel, plan)
int mark;
MANIP *mnp;
JNTS *jvel;
int plan;
{
	JNTS checkJvel;

	if (plan)
	 { getDefaultJointVel (mnp, &checkJvel);
	 }
	else
	 { askMonitor (mnp, GET_DEFAULT_JOINT_VEL);
	   checkJvel = cbk->jntsval;
	 }
	if (cmpVf (&checkJvel, jvel, 6) != 0)
	 { printf ("fatal (%d) --\n", mark);
	   printVf ("%g\n", checkJvel.v, 6);
	   exit (-1);
	 }
}

CHECKgetJointAccel (mark, mnp, jacc, plan)
int mark;
MANIP *mnp;
JNTS *jacc;
int plan;
{
	JNTS checkJacc;

	if (plan)
	 { getJointAccel (mnp, &checkJacc);
	 }
	else
	 { askMonitor (mnp, GET_JOINT_ACCEL);
	   checkJacc = cbk->jntsval;
	 }
	if (cmpVf (&checkJacc, jacc, 6) != 0)
	 { printf ("fatal (%d) --\n", mark);
	   printVf ("%g\n", checkJacc.v, 6);
	   exit (-1);
	 }
}

CHECKgetMod (mark, mnp, mod, plan)
int mark;
MANIP *mnp;
int mod;
int plan;
{
	int check;

	if (plan)
	 { check = getMod(mnp);
	 }
	else
	 { askMonitor (mnp, GET_MOD);
	   check = cbk->retval;
	 }
	if (check != mod)
	 { fatalMsg (-1, "(%d) mod = %d vs. %d\n", mark, check, mod);
	 }
}

CHECKgetActiveMod (mark, mnp, mod, plan)
int mark;
MANIP *mnp;
int mod;
int plan;
{
	int check;

	if (plan)
	 { check = getActiveMod(mnp);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_MOD);
	   check = cbk->retval;
	 }
	if (check != mod)
	 { fatalMsg (-1, "(%d) mod = %d vs. %d\n", mark, check, mod);
	 }
}

CHECKgetActiveMotionCounts (mark, mnp, tau, sigma, plan)
int mark;
MANIP *mnp;
int tau, sigma;
int plan;
{
	int checkSigma, checkTau;

	if (plan)
	 { getActiveMotionCounts (mnp, &checkTau, &checkSigma);
	 }
	else
	 { askMonitor (mnp, GET_ACTIVE_MOTION_COUNTS);
	   checkTau = cbk->intval[0];
	   checkSigma = cbk->intval[1];
	 }
	if (sigma != DONTCARE && sigma != checkSigma)
	 { fatalMsg (-1, "(%d) sigma = %d vs. %d\n", mark, checkSigma, sigma);
	 }
	if (tau != DONTCARE && tau != checkTau)
	 { fatalMsg (-1, "(%d) tau = %d vs. %d\n", mark, checkTau, tau);
	 }
}
   
CHECKgetSpeed (mark, mnp, speed, plan)
int mark;
MANIP *mnp;
float speed;
int plan;
{
	float check;

	if (plan)
	 { check = getSpeed(mnp);
	 }
	else
	 { askMonitor (mnp, GET_SPEED);
	   check = cbk->floatval[0];
	 }
	if (!FEQ(check, speed))
	 { fatalMsg (-1, "(%d) speed = %g vs. %g\n", mark, check, speed);
	 }
}

CHECKgetJvelScale (mark, mnp, scale, plan)
int mark;
MANIP *mnp;
float scale;
int plan;
{
	float check;

	if (plan)
	 { check = getJvelScale(mnp);
	 }
	else
	 { askMonitor (mnp, GET_JVEL_SCALE);
	   check = cbk->floatval[0];
	 }
	if (!FEQ(check, scale))
	 { fatalMsg (-1, "(%d) scale = %g vs. %g\n", mark, check, scale);
	 }
}

CHECKgetAccelScale (mark, mnp, scale, plan)
int mark;
MANIP *mnp;
float scale;
int plan;
{
	float check;

	if (plan)
	 { check = getAccelScale(mnp);
	 }
	else
	 { askMonitor (mnp, GET_ACCEL_SCALE);
	   check = cbk->floatval[0];
	 }
	if (!FEQ(check, scale))
	 { fatalMsg (-1, "(%d) scale = %g vs. %g\n", mark, check, scale);
	 }
}

CHECKnumMotionsQueued (mark, mnp, num, plan)
int mark;
MANIP *mnp;
int num;
int plan;
{
	int check;

	if (plan)
	 { check = numMotionsQueued (mnp);
	 }
	else
	 { askMonitor (mnp, NUM_MOTIONS_QUEUED);
	   check = cbk->retval;
	 }
	if (check != num)
	 { fatalMsg (-1, "(%d) num = %d vs. %d\n", check, num);
	 }
}

CHECKgetMotionQueueSize (mark, mnp, num, plan)
int mark;
MANIP *mnp;
int num;
int plan;
{
	int check;

	if (plan)
	 { check = getMotionQueueSize (mnp);
	 }
	else
	 { askMonitor (mnp, GET_MOTION_QUEUE_SIZE);
	   check = cbk->retval;
	 }
	if (check != num)
	 { fatalMsg (-1, "(%d) num = %d vs. %d\n", check, num);
	 }
}

CHECKrcclGetOptions (mark, mnp, mask, plan)
int mark;
MANIP *mnp;
unsigned long mask;
int plan;
{
	unsigned long check;

	if (plan)
	 { check = rcclGetOptions(~0);
	 }
	else
	 { askMonitor (mnp, RCCL_GET_OPTIONS);
	   check = cbk->retval;
	 }
	if (check != mask)
	 { fatalMsg (-1, "(%d) mask = 0x%x vs. 0x%x\n", mark, check, mask);
	 }
}

CHECKrcclGetModes (mark, mnp, mask, plan)
int mark;
MANIP *mnp;
unsigned long mask;
int plan;
{
	unsigned long check;

	if (plan)
	 { check = rcclGetModes (mnp, ~0);
	 }
	else
	 { askMonitor (mnp, RCCL_GET_MODES);
	   check = cbk->retval;
	 }
	if (check != mask)
	 { fatalMsg (-1, "(%d) mask = 0x%x vs. 0x%x\n", mark, check, mask);
	 }
}

CHECKrcclGetActiveModes (mark, mnp, mask, plan)
int mark;
MANIP *mnp;
unsigned long mask;
int plan;
{
	unsigned long check;

	if (plan)
	 { check = rcclGetActiveModes (mnp, ~0);
	 }
	else
	 { askMonitor (mnp, RCCL_GET_ACTIVE_MODES);
	   check = cbk->retval;
	 }
	if (check != mask)
	 { fatalMsg (-1, "(%d) mask = 0x%x vs. 0x%x\n", mark, check, mask);
	 }
}

errorExit (mark, str)
int mark;
char *str;
{
	printf ("(%d) %s\n", mark, str ? str : "");
	printErrors();
	exit (-1);
}
