/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/* program to automactically test a few miscellaneous things.
   Just run this program, and it all is well, it should print

	ROUTINES OK

   and exit.
*/

#ifdef VXWORKS_CFG
#define main testMisc_
#endif

#include "../h/rccl.h"
#include "../rccl/testMisc.h"

extern function1();
extern function2();

CTRL_BLK *cbk;

main()
{
	MANIP *obj;
	POS *p0;
	TRSF *start;
	float time1, time2, time3;
	float delta1, delta2, delta3;
	float margin;
	int mid1, mid2;
	int fxnId1, fxnId2;
	int k;

	rcclSetOptions (RCCL_ERROR_EXIT);
	rcclScheduling (ON_CLOCK);

	start = allocTrans(NULL, UNDEF);
	p0 = makePosition ("P0", T6, EQ, start, TL, T6);

	cbk = ALLOC_MEM (NULL, CTRL_BLK, UNDEF);
	obj = rcclCreate ("foo", 0);
	rcclStart();
	*start = *obj->here;

	/* test motion delay using a virtual manipulator */

	printf ("Testing motion delay stuff ...\n");

	margin = 3*rcclGetInterval();

	setTime (obj, 0.0, 1000.0);
	mid1 = move (obj, p0);
	setTime (obj, 0.0, 1000.0);
	mid2 = move (obj, p0);
	time1 = rcclSysTime();
	waitForStart (mid1);
	time2 = rcclSysTime();
	waitForStart (mid2);
	time3 = rcclSysTime();
	waitForCompleted (obj);

	CHECKdelta (1, time2 - time1, 0.0, margin);
	CHECKdelta (2, time3 - time1, 1000.0-margin, 1000.0+margin);
	
	rcclSetMotionDelay (1000.0);
	setTime (obj, 0.0, 2000.0);
	mid1 = move (obj, p0);
	setTime (obj, 0.0, 2000.0);
	mid2 = move (obj, p0);
	time1 = rcclSysTime();
	waitForStart (mid1);
	time2 = rcclSysTime();
	waitForStart (mid2);
	time3 = rcclSysTime();
	waitForCompleted (obj);

	CHECKdelta (3, time2 - time1, 1000.0-margin, 1000.0+margin);
	CHECKdelta (4, time3 - time1, 3000.0-margin, 3000.0+margin);	

	rcclSetMotionDelay (0.0);
	setTime (obj, 0.0, 1000.0);
	mid1 = move (obj, p0);
	rcclBlock();
	rcclBlock();
	rcclSetMotionDelay (2000.0);
	setTime (obj, 0.0, 1000.0);
	mid2 = move (obj, p0);
	time1 = rcclSysTime();
	waitForStart (mid1);
	time2 = rcclSysTime();
	waitForStart (mid2);
	time3 = rcclSysTime();
	waitForCompleted (obj);

	CHECKdelta (5, time2 - time1, 0.0, margin);
	CHECKdelta (6, time3 - time1, 2000.0-margin, 2000.0+margin);	

	rcclSetMotionDelay (0.0);	
	setTime (obj, 0.0, 1000.0);
	mid1 = move (obj, p0);
	setTime (obj, 0.0, 1000.0);
	mid2 = move (obj, p0);
	time1 = rcclSysTime();
	waitForStart (mid1);
	time2 = rcclSysTime();
	waitForStart (mid2);
	time3 = rcclSysTime();
	waitForCompleted (obj);

	CHECKdelta (7, time2 - time1, 0.0, margin);
	CHECKdelta (8, time3 - time1, 1000.0-margin, 1000.0+margin);

	printf ("Testing monitor function stuff ...\n");

	fxnId1 = runMonitorFxn (obj, function1, (int)cbk);
	CHECKchange (9, 1, 0);
	fxnId2 = runMonitorFxn (obj, function2, (int)cbk);
	CHECKchange (10, 1, 1);
	deleteMonitorFxn (obj, fxnId1);
	CHECKchange (11, 0, 1);
	deleteMonitorFxn (obj, fxnId2);
	CHECKchange (12, 0, 0);

	printf ("Testing rcclPause() stuff ...\n");

	fxnId1 = runMonitorFxn (obj, function1, (int)cbk);
	runMotionFxn (obj, function2, (int)cbk);
	CHECKchange (14, 1, 0);
	setTime (obj, 0.0, F_UNDEF);
	mid1 = move (obj, p0);
	rcclBlock();
	CHECKchange (15, 1, 1);		
	rcclPause (F_DEFAULT, F_UNDEF);
	CHECKchange (16, 1, 0);		
	rcclResume();
	CHECKchange (17, 1, 1);
	stopCurrentMotion(obj,0);
	rcclBlock();
	CHECKchange (18, 1, 0);

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

CHECKdelta (mark, delta, lower, upper)
int mark;
float delta, upper, lower;
{
	if (lower != F_UNDEF && delta < lower)
	 { printf ("(%d) delta = %g, lower = %g, upper = %g\n",
		    mark, delta, lower, upper);
	   exit (-1);
	 }
	if (upper != F_UNDEF && delta > upper)
	 { printf ("(%d) delta = %g, lower = %g, upper = %g\n",
		    mark, delta, lower, upper);
	   exit (-1);
	 }
}

CHECKchange (mark, first, second)
int mark, first, second;
{
	int val1, val2;
	int delta1;
	int delta2;
	float t1, t2;
	t1 = rcclSysTime();
	rcclBlock();
	val1 = cbk->retval1;
	val2 = cbk->retval2;
	rcclBlock();
	rcclBlock();
	t2 = rcclSysTime();
	delta1 = cbk->retval1 - val1;
	delta2 = cbk->retval2 - val2;
	if (first)
	 { if (delta1 < 2)
	    { printf ("(%d) delta1 < 2 -> %d\n", mark,  delta1);
	      exit (-1);
	    }
	 }
	else
	 { if (delta1 != 0)
	    { printf ("(%d) delta1 != 0 -> %d\n", mark,  delta1);
	      exit (-1);
	    }
	 }
	if (second)
	 { if (delta2 < 2)
	    { printf ("(%d) delta2 < 2 -> %d\n", mark,  delta2);
	      exit (-1);
	    }
	 }
	else
	 { if (delta2 != 0)
	    { printf ("(%d) delta2 != 0 -> %d\n", mark,  delta2);
	      exit (-1);
	    }
	 }
	
}
