/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/* A program to test the 'updateTrans()' primitives */

#ifdef VXWORKS_CFG
#define main testUt_
#endif

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

extern int setpDebug1;

main()
{
	MANIP *mnp;
	TRSF_PTR a, tx, b, c, r;
	TRSF check;
	POS *p;

	rcclSetOptions (RCCL_ERROR_EXIT);
	rcclScheduling (ON_CLOCK);
	
	a  = allocTransRpy (NULL, UNDEF, 1.0, 2.0, 3.0, 10.0, 20.0, 30.0);
	b  = allocTransRpy (NULL, UNDEF, 4.0, 5.0, 6.0, 40.0, 50.0, 40.0);
	c  = allocTransRpy (NULL, UNDEF, 7.0, 8.0, 9.0, 80.0, 70.0, 60.0);
	tx = allocTransRpy (NULL, UNDEF, 1.1, 1.2, 1.3, 15.0, 25.0, 35.0);
	r  = allocTrans (NULL, UNDEF);

	p  = makePosition (NULL, a, T6, b, EQ, c, TL, b);

	mnp = rcclCreate ("foo", 0x0);
	rcclStart();

	updateTrans (mnp, r, p, a, tx);
	distance (mnp, "dz", 100.0);
	move (mnp, mnp->last);
	waitForCompleted (mnp);
	solveTrans (&check, p, a, tx);
	CHECKtrsf (1, &check, r);

	updateTrans (mnp, r, p, T6, NULL);
	move (mnp, mnp->last);
	waitForCompleted (mnp);
	solveTrans (&check, p, T6, NULL);
	CHECKtrsf (2, &check, r);

	updateTrans (mnp, r, p, b, tx);
	move (mnp, mnp->last);
	waitForCompleted (mnp);
	solveTrans (&check, p, b, tx);
	CHECKtrsf (3, &check, r);

	updateTrans (mnp, r, p, c, tx);
	move (mnp, mnp->last);
	waitForCompleted (mnp);
	solveTrans (&check, p, c, tx);
	CHECKtrsf (4, &check, r);

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

	rcclRelease(1);	
}

#define EPSILON		1.0e-6

CHECKtrsf (mark, check, test)
int mark;
TRSF *check, *test;
{
	if (cmpTrsf (check, test) > EPSILON)
	 { printf ("(%d) actual and desired trsf values differ by %g\n",
		    mark, cmpTrsf (check, test));
	   exit (-1);
	 }
}
