/*-=-=-=-=-=-=-=-=-=-=-=- 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 routines for the test program 'rcitest'.

  -------------------------------------------------------------------------*/

#include <stdio.h>

#include "../h/rciTask.h"

#include "../rci/rcitest.h"

int wakeup = 0;
int count = 0;

wakeupFxn()
{
	return (wakeup);
}

triggerFxn(k)
int k;
{
	rciPrintf ("trigger %d\n", k);
	wakeup = 1;
}

timerAst(k)
int k;
{
	int time = rciTime();
	
	rciPrintf (" !! %d %d\n", time, k);
	rciTimerAst (time+1500, timerAst, k+1);
}

cfstart(td, arg)
RCI_DESC *td;
int arg;
{
	RCI_TCB *tcb = td->tcb;
	TASKCOM *com;

	if ((com = (TASKCOM*)getSharedMemory ("TASKCOM"))==NULL)
	 { rciAbort (0, "cfstart() -- can't get shared control memory\n");
	 }
	tcb->privateMem = (void*)com;

	if (com->printing)
	 { rciPrintf ("startup called, arg %d, disp = %d\n",
		      arg, tcb->scheduleDisp);
	 }

	if (com->astRate)
	 { rciTimerAst (rciTime()+com->astRate, timerAst, 1);
	 }
	if (com->abortCount)
	 { rciAbort (1, "abort count exceeded\n");
	 }
	if (com->testCrash)
	 { testCrash (com->testCrash);
	   com->testCrash = 0;
	 }
	if (com->testRciAbortErrors)
	 { testRciAbortErrors();
	   com->testRciAbortErrors = 0;
	 }
	switch (tcb->scheduleDisp)
	 { case ON_TRIGGER:
	    { rciWakeupOn (td, wakeupFxn, 0);
	      wakeup = 0;
	      rciSetTrigger (triggerFxn, td->tcb->cycleCount);
	      break;
	    }
	   case ON_FUNCTION:
	    { if (com->wakeupFxn)
	       { rciWakeupOn (td, com->wakeupFxn, (int)td);
	       }
	      break;
	    }
	   case ON_SIGNAL:
	    { rciWakeupOn (td, rciOnInterrupt, (int)td);
	      break;
	    }
	 }
	return (1);
}

cfrelse (td, arg, code)
RCI_DESC *td;
int arg, code;
{
	TASKCOM *com = (TASKCOM*)td->tcb->privateMem;

	if (com->printing)
	 { rciPrintf ("cfrelse called, code %d\n", code);
	 }
	return (1);
}

cfxn1(td, arg)
RCI_DESC *td;
int arg;
{	
	TASKCOM *com = (TASKCOM*)td->tcb->privateMem;

	if (com->printing)
	 { rciPrintf ("111 task %d, time %d, arg %d\n", 
		      td->taskID, rciTime(), arg);
	 }
	generic(td, arg);
	return (1);
}

cfxn2(td, arg)
RCI_DESC *td;
int arg;
{
	TASKCOM *com = (TASKCOM*)td->tcb->privateMem;

	if (com->printing)
	 { 
	   if (count++ > 1000) {
	     count = 0;
	     rciPrintf ("222 task %d, time %d, arg %d\n",
		      td->taskID, rciTime(), arg);
	   }
	 }

	generic(td, arg);
	return (1);
}

cfxn3(td, arg)
RCI_DESC *td;
int arg;
{
	TASKCOM *com = (TASKCOM*)td->tcb->privateMem;

	if (com->printing)
	 { rciPrintf ("333 task %d, time %d, arg %d\n",
		      td->taskID, rciTime(), arg);
	 }

	generic(td, arg);
	return (1);
}

generic(td, arg)
RCI_DESC *td;
int arg;
{
	TASKCOM *com = (TASKCOM*)td->tcb->privateMem;
	int k;

	floatComputations();

	if (com->testCrash)
	 { testCrash (com->testCrash);
	   com->testCrash = 0;
	 }
	if (com->testRciAbortErrors)
	 { testRciAbortErrors();
	   com->testRciAbortErrors = 0;
	 }
	if (com->abortCount &&
	    td->tcb->cycleCount >= com->abortCount)
	 { rciAbort (1, "abort count exceeded\n");
	 }
	if (td->tcb->scheduleDisp == ON_TRIGGER)
	 { wakeup = 0;
	   rciSetTrigger (triggerFxn, td->tcb->cycleCount);
	 }
	for (k=0; k<com->delayCount; k++)
	   ;
	return (1);
}

testCrash(com)
{
#ifdef VXWORKS_CFG
#define BOGUS_ADDR	 0x80000000
#else
#ifdef vax
#define BOGUS_ADDR	-1
#endif
#if defined(sun) || defined(sgi) || defined(linux)
#define BOGUS_ADDR	 0
#endif
#endif

	rciPrintf ("testCrash %d\n", com);
	switch (com)
	 { case DIVIDE0:
	    { divide (4.5, 0.0);
	      break;
	    }
	   case SEGMENTATION_D:
	    { access (BOGUS_ADDR);
	      break;
	    }
	   case SEGMENTATION_T:
	    { callthrough (BOGUS_ADDR);
	      break;
	    }
	   case SYSCALL:
	    { 
#ifndef VXWORKS_CFG
	      getpid();
#endif
	      break;
	    }
	 }
}

testRciAbortErrors()
{
	char *fxnName = "testRciAbortErrors()";
	extern char *errorStack;

	errorMsg (11, "%s -- error message (%d)\n", fxnName, 1);
	errorMsg (22, "%s -- error message (%d)\n", fxnName, 2);
	errorMsg (33, "%s -- error message (%d)\n", fxnName, 3);
	rciAbortErrors ();
	rciPrintf ("stack = 0x%x\n", errorStack);
}

divide (a, b)
float a, b;
{
	a /= b;
}

access (p)
char *p;
{
	char dummy = *p;
}

callthrough (p)
int (*p)();
{
	(*p)();
}

executeFxn (td, arg)
RCI_DESC *td;
int arg;
{
	rciPrintf ("executeFxn, task %s, arg %d\n", rciTaskName(td), arg);
	return (123);
}

typedef struct {		/* Basic spatial 3-vector		*/
	float x, y, z;
} VECT, *VECT_PTR;

typedef struct {		/* 4X4 homogenous transform:		*/
	VECT n, o, a, p;	/*   the 4 column vectors of the matrix	*/
} TRSF, *TRSF_PTR;

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

floatComputations()
{
	static DIFF d1 = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 };
	static TRSF t1 = { 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 
			   1.0, 2.0, 3.0, 1.0, 2.0, 3.0 };
	static TRSF t2 = { 1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 
			   1.0, 2.0, 3.0, 1.0, 2.0, 3.0 };

	trsfToVel (&d1, &t1, &t2);
}

trsfToVel (vr, t1, t2)
DIFF *vr;
TRSF *t1, *t2;
{
	float dnx, dox, dax;
	float dny, doy, day;

	dnx = t2->n.x - t1->n.x;
	dox = t2->o.x - t1->o.x;
	dax = t2->a.x - t1->a.x;
	dny = t2->n.y - t1->n.y;
	doy = t2->o.y - t1->o.y;
	day = t2->a.y - t1->a.y;

	vr->r.x = -( dny * t2->n.z + doy * t2->o.z + day * t2->a.z);
	vr->r.y =  ( dnx * t2->n.z + dox * t2->o.z + dax * t2->a.z);
	vr->r.z = -( dnx * t2->n.y + dox * t2->o.y + dax * t2->a.y);

	vr->t.x = t2->p.x - t1->p.x;
	vr->t.y = t2->p.y - t1->p.y;
	vr->t.z = t2->p.z - t1->p.z;
}
