/*-=-=-=-=-=-=-=-=-=-=-=- 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 transform primitives. To operate,
   just run the program; if all is well, it should print 

	ROUTINES OK

   after a while and exit.
*/

#ifdef VXWORKS_CFG
#define main testTrans_
#endif

#if defined(VXWORKS_CFG) || defined(sgi)
#define NO_BAD_FXNS
#endif

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

#include "../rccl/transform.h"

extern varbTrsf();
extern constTrsf();
extern changeX();
extern changeY();

#define PX	1
#define PY	2

/* testing variables built into transSetup.c */
extern int trtestLinkBp;
extern TRSF *trtestLinkBuf[];
extern int trtestUnlinkBp;
extern TRSF *trtestUnlinkBuf[];

main()
{
	TRSF_PTR trsf[6];
	TRSF_PTR check1;
	TRSF_PTR check2;
	TRSF testTrsf1;
	TRSF testTrsf2;
	int cpu0;
	int cpu1;
	int mask;
	MANIP *obj0, *obj1;
	int mid0, mid1;
	POS *p0, *p1, *p01;

	rcclSetOptions (RCCL_ERROR_EXIT);
	rcclScheduling (ON_CLOCK);

	/* try to use two CPUs if we can */

	mask = rciAllocCpus (0x3);
	if (!mask & 0x1)
	 { printf ("Can't allocate arbiter CPU!\n");
	   exit (-1);
	 }
	cpu0 = 0;
	cpu1 = (mask & 0x2) ? 1 : 0;

	identTrsf (&testTrsf1);
	identTrsf (&testTrsf2);
	xyzToTrsf (&testTrsf1, 10.0, 20.0, 30.0);
	xyzToTrsf (&testTrsf2, 40.0, 50.0, 60.0);
	rpyToTrsf (&testTrsf2, 15.0, 25.0, 35.0);				

	trsf[0] = allocTrans ("trsf0", cpu0);
	trsf[1] = allocTrans ("trsf1", cpu1);
	writeTrans (trsf[0], &testTrsf1);
	writeTrans (trsf[1], &testTrsf2);
	CHECKreadTrans (1, trsf[0], &testTrsf1);
	CHECKreadTrans (2, trsf[1], &testTrsf2);
	CHECKgetTransByAddr (3, trsf[0], trsf[0]);
	CHECKgetTransByAddr (4, trsf[1], trsf[1]);
	CHECKgetTransByName (5, "trsf0", trsf[0]);
	CHECKgetTransByName (6, "trsf1", trsf[1]);

	freeTrans (trsf[0]);
	freeTrans (trsf[1]);
	CHECKgetTransByName (5, "trsf0", NULL);
	CHECKgetTransByName (6, "trsf1", NULL);

	trsf[2] = allocTrans (NULL, cpu0);
	freeTrans (trsf[2]);
	check1 = trsf[2];
	trsf[2] = allocTrans (NULL, cpu0);
	trsf[3] = allocTrans (NULL, cpu1);	
	freeTrans (trsf[3]);
	check2 = trsf[3];
	trsf[3] = allocTrans (NULL, cpu1);	

	CHECKaddrs (7, trsf[2], check1);
	CHECKaddrs (8, trsf[3], check2);

	trsf[0] = trsf[2];
	trsf[1] = trsf[3];

	setTransVarb (trsf[0]);
	setTransVarb (trsf[1]);
	CHECKbinding (9, trsf[0], TRANS_VARB, varbTrsf, UNDEF);
	CHECKbinding (10, trsf[1], TRANS_VARB, varbTrsf, UNDEF);
	setTransConst (trsf[0]);
	setTransConst (trsf[1]);
	CHECKbinding (11, trsf[0], TRANS_CONST, constTrsf, UNDEF);
	CHECKbinding (12, trsf[1], TRANS_CONST, constTrsf, UNDEF);

	rcclClearOptions (RCCL_ERROR_EXIT);

#ifndef VXWORKS_CFG
	CHECKerror (13, EBadArg, transEval (trsf[0], NULLF, 0));
#endif
	CHECKerror (14, EDescriptor, transEval (&testTrsf1, NULLF, 0));
#ifndef NO_BAD_FXNS
	CHECKerror (15, EBadArg, transEval (trsf[1], main, 0));
#endif

	rcclSetOptions (RCCL_ERROR_EXIT);

	obj1 = rcclCreate ("obj1", (1<<cpu1));
	
	transEval (trsf[0], changeX, 1);
	transEval (trsf[1], changeY, 2);
	CHECKbinding (16, trsf[0], TRANS_EVAL, changeX, 1);
	CHECKbinding (17, trsf[1], TRANS_EVAL, changeY, 2);
	setTransConst (trsf[1]);
	CHECKbinding (18, trsf[1], TRANS_CONST, constTrsf, UNDEF);
	
	rcclStart();

	CHECKchange (19, trsf[0], PX, 1.0);
	CHECKchange (21, trsf[1], 0, 0.0);

	transEval (trsf[0], changeX, -1);

	CHECKchange (23, trsf[0], PX, -1.0);
	setTransConst (trsf[0]);
	CHECKbinding (24, trsf[0], TRANS_CONST, constTrsf, UNDEF);
	setTransVarb (trsf[0]);

	CHECKchange (25, trsf[0], PX, 0.0);

	rcclRelease(0);
	obj0 = rcclCreate ("obj0", (1<<cpu0));
	rcclStart();

	transEval (trsf[0], changeY, 1);
	transEval (trsf[1], changeY, -1);

	CHECKchange (26, trsf[0], PY, 1.0);
	CHECKchange (27, trsf[1], PY, -1.0);

	setTransConst (trsf[0]);

	CHECKchange (30, trsf[0], PY, 0.0);
	CHECKchange (31, trsf[1], PY, -1.0);

	rcclRelease(0);

	transEval (trsf[0], changeX, -1);
	setTransVarb (trsf[1]);

	rcclStart();

	CHECKchange (32, trsf[0], PX, -1.0);
	CHECKchange (34, trsf[1], 0, 0.0);

	rcclRelease(0);

	CHECKchange (36, trsf[0], PX, 0.0);

	/* now check out transMotionEval() */

	rcclClearOptions (RCCL_ERROR_EXIT);

	CHECKerror (37, EBadArg, transMotionEval (trsf[0], NULLF, 0, obj0));
	CHECKerror (38, EDescriptor, transMotionEval(&testTrsf1,NULLF,0,obj0));
#ifndef NO_BAD_FXNS
	CHECKerror (39, EBadArg, transMotionEval (trsf[1], main, 0, obj0));
#endif
	rcclSetOptions (RCCL_ERROR_EXIT);

	setTransConst (trsf[0]);
	transMotionEval (trsf[1], changeX, 1, obj1);

	rcclStart();

	CHECKchange (40, trsf[0], 0, 0.0);
	CHECKchange (41, trsf[1], 0, 0.0);

	p0 = makePosition (NULL, T6, EQ, trsf[0], TL, T6);
	p1 = makePosition (NULL, T6, EQ, trsf[1], TL, T6);
	p01 = makePosition (NULL, T6, EQ, trsf[0], trsf[1], TL, T6);

	doMove (obj1, p1);

	CHECKchange (42, trsf[0], 0, 0.0);
	CHECKchange (43, trsf[1], PX, 1.0);

	cancelMove (obj1);

	CHECKchange (44, trsf[1], PX, 0.0);

	doMove (obj0, p1);
	doMove (obj1, p0);

	CHECKchange (45, trsf[0], 0, 0.0);
	CHECKchange (46, trsf[1], 0, 0.0);

	cancelMove (obj0);
	cancelMove (obj1);
	doMove (obj1, p1);

	CHECKchange (47, trsf[0], 0, 0.0);
	CHECKchange (48, trsf[1], PX, 1.0);

	transMotionEval (trsf[1], changeX, -1, obj1);

	CHECKchange (49, trsf[0], 0, 0.0);
	CHECKchange (50, trsf[1], PX, -1.0);

	transEval (trsf[1], changeX, 1);

	CHECKchange (51, trsf[0], 0, 0.0);
	CHECKchange (52, trsf[1], PX, 1.0);

	transMotionEval (trsf[1], changeX, -1, obj1);

	CHECKchange (53, trsf[0], 0, 0.0);
	CHECKchange (54, trsf[1], PX, -1.0);

	setTransConst (trsf[1]);
	setTransVarb (trsf[1]);

	CHECKchange (55, trsf[0], 0, 0.0);
	CHECKchange (56, trsf[1], PX, 0.0);

	transMotionEval (trsf[1], changeY, 1, obj1);

	CHECKchange (57, trsf[1], PY, 1.0);

	transMotionEval (trsf[1], changeX, 1, obj1);

	CHECKchange (58, trsf[1], PX, 1.0);

	cancelMove (obj1);

	CHECKchange (59, trsf[1], 0, 0.0);

	doMove (obj1, p1);

	CHECKchange (60, trsf[1], PX, 1.0);

	transMotionEval (trsf[1], changeX, 1, obj0);

	CHECKchange (61, trsf[1], 0, 0.0);

	doMove (obj0, p1);

	CHECKchange (62, trsf[1], PX, 1.0);

	cancelMove (obj1);

	CHECKchange (63, trsf[1], PX, 1.0);

	cancelMove (obj0);

	CHECKchange (64, trsf[1], 0, 0.0);

	transMotionEval (trsf[0], changeY, 1, obj1);
	transMotionEval (trsf[1], changeX, 1, obj1);

	doMove (obj1, p0);

	CHECKchange (65, trsf[0], PY, 1.0);
	CHECKchange (66, trsf[1], 0, 0.0);

	cancelMove (obj1);

	doMove (obj1, p01);

	CHECKchange (67, trsf[0], PY, 1.0);
	CHECKchange (68, trsf[1], PX, 1.0);

	rcclRelease (0);

	CHECKchange (69, trsf[0], 0, 0.0);
	CHECKchange (70, trsf[1], 0, 0.0);

	rcclStart();

	CHECKchange (71, trsf[0], 0, 0.0);
	CHECKchange (72, trsf[1], 0, 0.0);

	setTransVarb (trsf[0]);
	setTransVarb (trsf[1]);

	doMove (obj1, p01);

	CHECKchange (73, trsf[0], 0, 0.0);
	CHECKchange (74, trsf[1], 0, 0.0);

	transMotionEval (trsf[0], changeY, -1, obj1);	

	CHECKchange (75, trsf[0], PY, -1.0);
	CHECKchange (76, trsf[1], 0, 0.0);

	transMotionEval (trsf[1], changeX, 1, obj1);	

	CHECKchange (77, trsf[0], PY, -1.0);
	CHECKchange (78, trsf[1], PX, 1.0);

	transMotionEval (trsf[1], changeX, -1, obj0);

	CHECKchange (79, trsf[1], 0, 0.0);

	doMove (obj0, p0);

	CHECKchange (80, trsf[1], 0, 0.0);

	cancelMove (obj0);

	doMove (obj0, p01);

	CHECKchange (81, trsf[0], PY, -1.0);
	CHECKchange (82, trsf[1], PX, -1.0);

	setTransVarb (trsf[1]);
	setTransConst (trsf[0]);

	CHECKchange (83, trsf[0], 0, 0.0);
	CHECKchange (84, trsf[1], 0, 0.0);

	transMotionEval (trsf[0], changeY, -1, obj0);
	transMotionEval (trsf[1], changeY, -1, obj0);

	CHECKchange (85, trsf[0], PY, -1.0);
	CHECKchange (86, trsf[1], PY, -1.0);

	rcclRelease(0);
	
	setTransConst (trsf[0]);
	setTransConst (trsf[1]);

	rcclStart();

	transMotionEval (trsf[0], changeX, 1, obj0);
	transMotionEval (trsf[1], changeX, 1, obj0);

	doMove (obj0, p01);

	cancelMove (obj0);

	trtestLinkBp = 0;
	trtestUnlinkBp = 0;

	hook();
	freeTrans (trsf[0]);
	freeTrans (trsf[1]);

	CHECKlinkBuf (89, NULL, NULL);

	trsf[0] = allocTrans (NULL, cpu0);
	trsf[1] = allocTrans (NULL, cpu1);

	transMotionEval (trsf[0], changeX, 1, obj0);
	transMotionEval (trsf[1], changeX, 1, obj0);

	p01 = makePosition (NULL, T6, EQ, trsf[0], trsf[1], TL, T6);
	doMove (obj0, p01);

	freeTrans (trsf[0]);
	freeTrans (trsf[1]);

	CHECKunlinkBuf (90, NULL, NULL);
	CHECKlinkBuf (91, trsf[0], trsf[1]);
	trtestLinkBp = 0;

	cancelMove (obj0);

	trsf[2] = allocTrans (NULL, UNDEF);
	CHECKunlinkBuf (92, trsf[0], trsf[1]);
	trtestUnlinkBp = 0;
	freeTrans (trsf[2]);

	trsf[0] = allocTrans (NULL, cpu0);
	trsf[1] = allocTrans (NULL, cpu1);

	transMotionEval (trsf[0], changeX, 1, obj0);
	transMotionEval (trsf[1], changeX, 1, obj0);

	p01 = makePosition (NULL, T6, EQ, trsf[0], trsf[1], TL, T6);
	setTime (obj0, F_DEFAULT, F_UNDEF);
	move (obj0, p01);
	setTime (obj0, F_DEFAULT, F_UNDEF);
	move (obj0, p01);
	setTime (obj0, F_DEFAULT, F_UNDEF);
	move (obj0, p1);

	freeTrans (trsf[0]);
	freeTrans (trsf[1]);

	CHECKunlinkBuf (93, NULL, NULL);
	CHECKlinkBuf (94, trsf[0], trsf[1]);
	trtestLinkBp = 0;

	stopCurrentMotion (obj0, 0);
	rcclBlock();
	trsf[2] = allocTrans(NULL, UNDEF);
	CHECKunlinkBuf (95, NULL, NULL);

	stopCurrentMotion (obj0, 0);
	rcclBlock();
	trsf[2] = allocTrans(NULL, UNDEF);
	CHECKunlinkBuf (96, trsf[0], NULL);
	trtestUnlinkBp = 0;

	stopCurrentMotion (obj0, 0);
	rcclBlock();
	trsf[2] = allocTrans(NULL, UNDEF);
	CHECKunlinkBuf (97, trsf[1], NULL);
	trtestUnlinkBp = 0;

	rcclRelease(1);

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

doMove (mnp, pos)
MANIP *mnp;
POS *pos;
{
	int mid;
	setTime (mnp, F_DEFAULT, F_UNDEF);
	mid = move (mnp, pos);
	waitForStart (mid);
}

cancelMove (mnp)
MANIP *mnp;
{
	stopCurrentMotion (mnp, 0);
	waitForCompleted (mnp);
}

#define EPSILON 1.0e-6

CHECKreadTrans (mark, trsf, set)
int mark;
TRSF *trsf, *set;
{
	TRSF check;

	readTrans (trsf, &check);
	if (cmpTrsf (&check, set) > EPSILON)
	 { printf ("(%d) transforms differ\n", mark);
	   exit (-1);
	 }
}

CHECKgetTransByAddr (mark, addr, test)
int mark;
TRSF *addr, *test;
{
	TRSF *check;

	check = getTransByAddr (addr);
	if (check != test)
	 { printf ("(%d) getTransByAddr returns 0x%x vs. 0x%x\n",
			 mark, check, test);
	   exit (-1);
	 }
}

CHECKgetTransByName (mark, name, test)
int mark;
char *name;
TRSF *test;
{
	TRSF *check;

	check = getTransByName (name);
	if (check != test)
	 { printf ("(%d) getTransByName returns 0x%x vs. 0x%x\n",
			 mark, check, test);
	   exit (-1);
	 }
}

CHECKaddrs(mark, trsf1, trsf2)
int mark;
TRSF *trsf1, *trsf2;
{
	if (trsf1 != trsf2)
	 { printf ("(%d) address = 0x%x vs. 0x%x\n", mark, trsf1, trsf2);
	   exit (-1);
	 }
}

CHECKbinding (mark, trsf, code, fxn, arg)
int mark;
TRSF *trsf;
int (*fxn)();
int arg;
{
	TRANS_BINDING binding;

	getTransBinding (trsf, &binding);

	if (binding.code != code)
	 { printf ("(%d) code = %d vs. %d\n", mark, binding.code, code);
	   exit (-1);
	 }
	if ((code == TRANS_EVAL || code == TRANS_MOTION_EVAL) &&
	    (binding.fxn != fxn))
	 { printf ("(%d) fxn = 0x%x vs. 0x%x\n", mark, binding.fxn, fxn);
	   exit (-1);
	 }
	if (arg != UNDEF && binding.arg != arg)
	 { printf ("(%d) arg = %d vs. %d\n", mark, binding.arg, arg);
	   exit (-1);
	 }
}

CHECKerror (mark, code, retval)
int mark;
int code;
int retval;
{
	if (retval != -1 || code != getErrorCode(0))
	 { printf ("(%d) error %d not reported\n", mark, code);
	   exit (-1);
	 }
}

CHECKchange (mark, tr, dof, inc)
int mark;
TRSF *tr;
int dof;
float inc;
{
	TRSF tr0, tr1;
	float change;

	tr0 = *tr;
	if (rcclActive())
	 { rcclBlock();
	   rcclBlock();
	   rcclBlock();
	 }
	else
	 { delay (500.0);
	 }
	tr1 = *tr;
	switch (dof)
	 { case PX:
	    { change = tr1.p.x - tr0.p.x;
	      tr1.p.x = tr0.p.x;
	      break;
	    }
	   case PY:
	    { change = tr1.p.y - tr0.p.y;
	      tr1.p.y = tr0.p.y;
	      break;
	    }
	   default:
	    { change = 0.0;
	      break;
	    }
	 }
	if (cmpTrsf (&tr0, &tr1) > EPSILON)
	 { printf ("(%d) illegal transform change, magnitude = %g\n",
		    mark, cmpTrsf (&tr0, &tr1));
	 }
	if (inc == 0.0)
	 { if (change != 0.0)
	    { printf ("(%d) value changed by %g\n", mark, change);
	      rciClose();
	      exit (-1);
	    }
	 }
	else if (inc < 0.0)
	 { if (change > 3*inc)
	    { printf ("(%d) value changed by %g vs. at least %g\n",
			mark, change, 3*inc);
	      rciClose();
	      exit (-1);
	    }
	 }
	else
	 { if (change < 3*inc)
	    { printf ("(%d) value changed by %g vs. at least %g\n",
			mark, change, 3*inc);
	      rciClose();
	      exit (-1);
	    }
	 }
}
	
searchBuf (buf, size, tr)
TRSF **buf;
int size;
TRSF *tr;
{
	int k;
	for (k=0; k<size; k++)
	 { if (buf[k] == tr)
	    { return (1);
	    }
	 }
	return (0);
}

CHECKlinkBuf (mark, t1, t2)
int mark;
TRSF *t1, *t2;
{
	int num = 0;

	if (t1)
	 { if (!searchBuf(t1, trtestLinkBuf, trtestLinkBp))
	    { printf ("(%d) trsf 0x%x not linked\n", mark, t1);
	      exit (-1);
	    }
	   num++;
	 }
	if (t2)
	 { if (!searchBuf(t2, trtestLinkBuf, trtestLinkBp))
	    { printf ("(%d) trsf 0x%x not linked\n", mark, t2);
	      exit (-1);
	    }
	   num++;
	 }
	if (num != trtestLinkBp)
	 { printf ("(%d) num xforms linked = %d vs. %d\n", mark,
		trtestLinkBp, num);
	   exit (-1);
	 }
}

CHECKunlinkBuf (mark, t1, t2)
int mark;
TRSF *t1, *t2;
{
	int num = 0;

	if (t1)
	 { if (!searchBuf(t1, trtestUnlinkBuf, trtestUnlinkBp))
	    { printf ("(%d) trsf 0x%x not unlinked\n", mark, t1);
	      exit (-1);
	    }
	   num++;
	 }
	if (t2)
	 { if (!searchBuf(t2, trtestUnlinkBuf, trtestUnlinkBp))
	    { printf ("(%d) trsf 0x%x not unlinked\n", mark, t2);
	      exit (-1);
	    }
	   num++;
	 }
	if (num != trtestUnlinkBp)
	 { printf ("(%d) num xforms unlinked = %d vs. %d\n", 
		   mark, trtestUnlinkBp, num);
	   exit (-1);
	 }
}

hook()
{
}
