/*-=-=-=-=-=-=-=-=-=-=-=- 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 test the syncronization routines */

#include <stdio.h>

#ifdef VXWORKS_CFG
#define main testSync_
#endif

#include "../h/rccl.h"
#include "../rccl/testSync.h"

#define INTERVAL	100	/* milliseconds */

MANIP *vmp;
ADJUST_CTRL *cbk;

extern adjustFxn();

main()
{
	POS *p0;
	POS *p1;
	TRSF *t0;
	TRSF *t1;
	float time0;
	float time1;
	float time2;
	float startTime;
	float stopTime;
	float startTimeReq;
	float stopTimeReq;
	int mid;

	rcclSetOptions (RCCL_ERROR_EXIT);
	rcclSetInterval (INTERVAL);
	rcclScheduling (ON_CLOCK);

	t0 = allocTrans (NULL, UNDEF);
	t1 = allocTransXyz (NULL, UNDEF, 100.0, 0.0, 0.0);
	p0 = makePosition (NULL, T6, EQ, t0, NULL);
	p1 = makePosition (NULL, T6, EQ, t1, NULL);

	vmp = rcclCreate ("virtual", 0);

	cbk = ALLOC_MEM(NULL,ADJUST_CTRL,UNDEF);
	cbk->vmpAddr = vmp;

	rcclStart();

	runMonitorFxn (vmp, adjustFxn, (int)cbk);

#define SEGTIME1	1000.0
#define SIGMA1		(SEGTIME1/INTERVAL)
#define QUARTER_SECOND	 300.0
#define HALF_SECOND	 500.0
#define ONE_SECOND	 1000.0
#define MSEC		 1.0
#define HALF_SPEED	 0.5
#define DOUBLE_SPEED	 2.0
#define SAME_SPEED	 1.0

	setTime (vmp, 0.0, SEGTIME1);

	time0 = rcclSysTime();
	mid = move (vmp, p1);
	waitForStart (mid);
	time1 = rcclSysTime();
	waitForStop (mid);
	time2 = rcclSysTime();

	startTime = motionStartTime(mid);
	stopTime = motionStopTime(mid);
	checkTime (1, startTime, time0, time1);
	checkTime (2, stopTime, startTime+SEGTIME1, startTime+SEGTIME1);

	/* -- now check out setStartTime() and setStopTime() -- */

	/* does setStartTime work? */

	setTime (vmp, 0.0, SEGTIME1);
	time0 = rcclSysTime();
	startTimeReq = time0 + QUARTER_SECOND;
	setStartTime (vmp, startTimeReq);
	mid = move(vmp, p0);
	waitForStop (mid);
	
	startTime = motionStartTime(mid);
	stopTime = motionStopTime(mid);
	checkTime (3, startTime, startTimeReq, startTimeReq);
	checkTime (4, stopTime, startTime+SEGTIME1, startTime+SEGTIME1);

	/* does setStartTime work if called too late ? */

	setTime (vmp, 0.0, SEGTIME1);
	time0 = rcclSysTime();
	startTimeReq = time0 - QUARTER_SECOND;
	setStartTime (vmp, startTimeReq);
	mid = move(vmp, p0);
	waitForStart (mid);
	time1 = rcclSysTime();
	waitForStop (mid);
	
	startTime = motionStartTime(mid);
	checkTime (5, startTime, time0, time1);

	/* does set stop time work ? */

	setTime (vmp, 0.0, SEGTIME1);
	time0 = rcclSysTime();
	stopTimeReq = time0 + ONE_SECOND;
	setStopTime (vmp, stopTimeReq);
	mid = move(vmp, p0);
	waitForStop (mid);
	
	stopTime = motionStopTime(mid);
	checkTime (6, stopTime, stopTimeReq, stopTimeReq);

	/* does set stop time work if called early ? */

	setTime (vmp, 0.0, SEGTIME1);
	time0 = rcclSysTime();
	stopTimeReq = time0 - ONE_SECOND;
	setStopTime (vmp, stopTimeReq);
	mid = move(vmp, p0);
	waitForStop (mid);
	time1 = rcclSysTime();
	
	stopTime = motionStopTime(mid);
	checkTime (7, stopTime, time0, time1);

	mid = doAjustCmd (p1, DO_SPEED, HALF_SPEED,0.0,ONE_SECOND, 200*MSEC);
	waitForStop (mid);
	checkSpeedChange (8, mid, HALF_SPEED);	

	mid = doAjustCmd (p0, DO_SPEED,DOUBLE_SPEED,0.0,ONE_SECOND,200*MSEC);
	waitForStop (mid);
	checkSpeedChange (9, mid, DOUBLE_SPEED);	

	mid = doAjustCmd (p1, DO_SPEED, SAME_SPEED,0.0,ONE_SECOND, 200*MSEC);
	waitForStop (mid);
	checkSpeedChange (10, mid, SAME_SPEED);	

#if 0
	time0 = rcclSysTime();
	mid = doAjustCmd (p0, DO_STOPTIME, time0+2*ONE_SECOND, 0.0, 
					   ONE_SECOND, 200*MSEC);
	waitForStop (mid);
	checkStopTimeChange(11, mid, time0+2*ONE_SECOND);

	time0 = rcclSysTime();
	mid = doAjustCmd (p0, DO_STOPTIME, time0-2*ONE_SECOND, 0.0, 
					   ONE_SECOND, 200*MSEC);
	waitForStop (mid);
	stopTime = motionStopTime(mid);
	checkTime (12, stopTime, cbk->time, cbk->time);
	checkStopTimeChange(13, mid, stopTime);
#endif

	rcclRelease(YES);

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

checkTime (mark, value, low, high)
int mark;
float value, low, high;
{
	if (value > high || value < low)
	 { printf ("(%d) time is %g; should be within [%g,%g]\n",
		     mark, value, low, high);
	   exit (1);
	 }
}

doAjustCmd (p, cmd, value, trans, segTime, delayTime)
POS *p;
int cmd;
float value, trans, segTime, delayTime;
{
	int mid;

	setTime (vmp, 0.0, segTime);
	mid = move (vmp, p);
	setTime (vmp, 0.0, F_DEFAULT);
	stop (vmp, 0.0);
	delay (delayTime);
	cbk->value = value;
	cbk->trans = trans;
	cbk->cmd = cmd;
	cbk->mid = mid;
	rcclBlock();
	rcclBlock();
	if (cbk->cmd)
	 { printf ("ERROR: adjustFxn() did not execute\n");
	   exit (-1);
	 }
	return (mid);
}

checkSpeedChange (mark, mid, scale)
int mark, mid;
float scale;
{
	float startTime, stopTime;
	float c, c_new;

	startTime = motionStartTime(mid);
	stopTime = motionStopTime(mid);

	c_new = (stopTime-cbk->time)/INTERVAL;
	c = c_new - (cbk->newSigma-cbk->oldSigma);
	if (c/c_new > scale)
	 { printf ("(%d) c_new/c > scale\n", mark);
	   printf ("     c_new=%g c=%g scale=%g oldSigma=%d newSigma=%d\n",
			 c_new, c, scale, cbk->oldSigma, cbk->newSigma);
	   printf ("     startTime=%g time=%g stopTime=%g\n",
			 startTime, cbk->time, stopTime);
	   exit(1);
	 }
	if (c/(c_new-1) <= scale)
	 { printf ("(%d) (c_new-1)/c <= scale\n", mark);
	   printf ("     c_new=%g c=%g scale=%g oldSigma=%d newSigma=%d\n",
			 c_new, c, scale, cbk->oldSigma, cbk->newSigma);
	   printf ("     startTime=%g time=%g stopTime=%g\n",
			 startTime, cbk->time, stopTime);
	   exit(1);
	 }
}

#if 0
checkStopTimeChange (mark, mid, time)
int mark, mid;
float time;
{
	float startTime, stopTime;
	float c, c_new;

	startTime = motionStartTime(mid);
	stopTime = motionStopTime(mid);

	c_new = (stopTime-cbk->time)/INTERVAL;
	c = c_new - (cbk->newSigma-cbk->oldSigma);
	if (stopTime != time)
	 { printf ("(%d) stopTime(%g) != time(%g)\n", stopTime, time);
	   exit(1);
	 }
	if (startTime+cbk->oldSigma+(c_new-c) != stopTime)
	 { printf("(%d) startTime+oldSigma+(c_new-c) != stopTime\n");
	   printf("     startTime=%g oldSigma=%g c_new=%g c=%g stopTime=%g\n",
			startTime, cbk->oldSigma, c_new, c, stopTime);
	   exit(1);
	 }
}
#endif
