/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/* Control level for the "rccltest" program. Contains mostly simple "demo"
   type transform functions.
*/

#include "../h/rcclDefs.h"
#include "../h/robotMath.h"
#include "rccltest.h"
#include <math.h>

#define PERIOD	10000

sawFxn (tr, arg, mnp)
TRSF *tr;
int arg;
MANIP *mnp;
{
	struct fxnParams *fxnp;
	int tau;

	fxnp = (struct fxnParams*)getMemByAddr((void*)arg);
	tau = ((int)(rcclSysTime()) % PERIOD);

	if (tau < (PERIOD/2))
	 { tr->p.x = 8 * fxnp->amplitude * (tau/(float)PERIOD);
	 }
	else
	 { tr->p.x = 8 * fxnp->amplitude * (1.0 - tau/(float)PERIOD);
	 }
}

rampFxn (tr, arg, mnp)
TRSF *tr;
int arg;
MANIP *mnp;
{
	tr->p.x += 1.0;
}

sineFxn (tr, arg, mnp)
TRSF *tr;
int arg;
MANIP *mnp;
{
	struct fxnParams *fxnp;

	fxnp = (struct fxnParams*)getMemByAddr((void*)arg);
	tr->p.x = fxnp->amplitude * sin(fxnp->speed*rcclSysTime()/500.0);
}

cosineFxn (tr, arg, mnp)
TRSF *tr;
int arg;
MANIP *mnp;
{
	struct fxnParams *fxnp;

	fxnp = (struct fxnParams*)getMemByAddr((void*)arg);
	tr->p.z = fxnp->amplitude * cos(fxnp->speed*rcclSysTime()/500.0);
}

turntableFxn (tr, arg, mnp)
TRSF *tr;
int arg;
MANIP *mnp;
{
	struct fxnParams *fxnp;

	fxnp = (struct fxnParams*)getMemByAddr((void*)arg);
	tr->p.x = fxnp->tableRadius * cos(0.2*rcclSysTime()/500.0);
	tr->p.y = fxnp->tableRadius * sin(0.2*rcclSysTime()/500.0);
}

timeRipple (arg)
int arg;
{
	float ang = PI*rciTime()/1000;
	int delay, k;

	delay = sin(ang)*arg;
	for (k=0; k<delay; k++)
	   ;		
}
