/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/* A routine to test transform multiplication. We have this around
   because we are always trying to make transform multiplication faster,
   and when we do so its nice to know if it still works.
 */

#include <stdio.h>
#include "../h/cdefs.h"
#include "../h/vector.h"
#include "../h/robotTypes.h"

#include "../h/cputime.h"

extern float frandom();

#define SCANVF(v,s) 	  scanVf((float*)(v),(s))
#define PRINTVF(f,v,s) 	  printVf((f),(float*)(v),(s))
#define NORMVF(v,s)	  normVf((float*)(v),(s))
#define ADDVF(r,a,b,s)	  addVf((float*)(r),(float*)(a),(float*)(b),(s))
#define SUBVF(r,a,b,s) subtractVf((float*)(r),(float*)(a),(float*)(b),(s))

TRSF *trmult_old(tr, t1, t2) /*::*/
register TRSF *tr, *t1, *t2;
{
	/* Reference routine */

 	register float x, y, z;
	register float tx, ty, tz;
 
 	x = t2->n.x;
	y = t2->n.y;
	z = t2->n.z;
	tx =  t1->n.x * x; tx += t1->o.x * y; tx += t1->a.x * z;
	ty =  t1->n.y * x; ty += t1->o.y * y; ty += t1->a.y * z;
	tz =  t1->n.z * x; tz += t1->o.z * y; tz += t1->a.z * z;
	tr->n.x = tx;
	tr->n.y = ty;
	tr->n.z = tz;

 	x = t2->o.x;
	y = t2->o.y;
	z = t2->o.z;
	tx =  t1->n.x * x; tx += t1->o.x * y; tx += t1->a.x * z;
	ty =  t1->n.y * x; ty += t1->o.y * y; ty += t1->a.y * z;
	tz =  t1->n.z * x; tz += t1->o.z * y; tz += t1->a.z * z;
	tr->o.x = tx;
	tr->o.y = ty;
	tr->o.z = tz;

 	x = t2->a.x;
	y = t2->a.y;
	z = t2->a.z;
	tx =  t1->n.x * x; tx += t1->o.x * y; tx += t1->a.x * z;
	ty =  t1->n.y * x; ty += t1->o.y * y; ty += t1->a.y * z;
	tz =  t1->n.z * x; tz += t1->o.z * y; tz += t1->a.z * z;
	tr->a.x = tx;
	tr->a.y = ty;
	tr->a.z = tz;

 	x = t2->p.x;
	y = t2->p.y;
	z = t2->p.z;	
	tx =  t1->n.x * x; tx += t1->o.x * y; tx += t1->a.x * z;
	ty =  t1->n.y * x; ty += t1->o.y * y; ty += t1->a.y * z;
	tz =  t1->n.z * x; tz += t1->o.z * y; tz += t1->a.z * z;
	tx += t1->p.x;
	ty += t1->p.y;
	tz += t1->p.z;
	tr->p.x = tx;
	tr->p.y = ty;
	tr->p.z = tz;

	return(tr);
}

TRSF *invert_old(tr, t1)  /*::*/
register TRSF *tr, *t1;
{
	/* Reference routine for inversion */

	tr->n.x = t1->n.x;
	tr->n.y = t1->o.x;
	tr->n.z = t1->a.x;
	tr->o.x = t1->n.y;
	tr->o.y = t1->o.y;
	tr->o.z = t1->a.y;
	tr->a.x = t1->n.z;
	tr->a.y = t1->o.z;
	tr->a.z = t1->a.z;

	tr->p.x = - (t1->p.x * t1->n.x + t1->p.y * t1->n.y
			+ t1->p.z * t1->n.z);
	tr->p.y = - (t1->p.x * t1->o.x + t1->p.y * t1->o.y
			+ t1->p.z * t1->o.z);
	tr->p.z = - (t1->p.x * t1->a.x + t1->p.y * t1->a.y
			+ t1->p.z * t1->a.z);
	return(tr);
}

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf [256];
int numtimes;

#define NUMELEMS	12

float cmpTrans (t1, t2)
TRSF *t1;
TRSF *t2;
{
#define EPSILON 1.0e-5

	TRSF diff;
	VECT pav;

	float rnorm;
	float pnorm;
	float psize;
	
	SUBVF (&diff, t1, t2, NUMELEMS);
	rnorm = NORMVF (&diff, 9);	/* get only rotational part */
	
	pnorm = NORMVF (&diff.p, 3);
	ADDVF (&pav, &t1->p, &t2->p, 3);
	if ((psize = NORMVF (&pav, 3)) > EPSILON)
	 { pnorm /= psize;
	 }
	return (MAX (pnorm, rnorm));
}

main()
{
	char *keytree, *tree_match_parse(), *tree_match_err_at();

	keytree = tree_match_parse("					\
(	exit								\
	test 								\
	 ([multTrsf invertTrsf multLiTrsf multRiTrsf]			\
	   (%d", &numtimes, ",0,,	\"number of trials\"))		\
	do 								\
	 ([multTrsf invertTrsf multLiTrsf multRiTrsf])			\
	time 								\
	   ([multTrsf trmultinp invertTrsf multLiTrsf multRiTrsf]		\
	     (%d", &numtimes, ",0))					\
)",0);
#ifdef notdef
	[free lock]							\
	 (all-joints							\
	  one-joint							\
	   (%d",&joint,",1,6,		\"joint number (1 to 6)\")	\
	 )								\
	show (encoder-values)
#endif

	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	do
 	 { tree_match (keytree, "TMTEST> ", key_buf);

	   if (COMMANDS ("do"))
	    { TRSF tr, t1, t2, checkr;
	      DIFF t1Spec, t2Spec;

	      printf ("Enter T1 x, y, z, roll, pitch, yaw > ");
	      if (SCANVF (&t1Spec, 6) != 1)
	       { printf ("ERROR/ cannot scan values\n");
		 continue;
	       }
	      rpyToTrsf  (&t1, t1Spec.r.x, t1Spec.r.y, t1Spec.r.z);
	      xyzToTrsf (&t1, t1Spec.t.x, t1Spec.t.y, t1Spec.t.z);

	      if (!COMMAND("do invertTrsf"))
	       { 
		 printf ("Enter T2 x, y, z, roll, pitch, yaw > ");
		 if (SCANVF (&t2Spec, 6) != 1)
		  { printf ("ERROR/ cannot scan values\n");
		    continue;
		  }
		 rpyToTrsf  (&t2, t2Spec.r.x, t2Spec.r.y, t2Spec.r.z);
		 xyzToTrsf (&t2, t2Spec.t.x, t2Spec.t.y, t2Spec.t.z);
	       }

	      if (COMMAND ("do multTrsf"))
	       { trmult_old (&checkr, &t1, &t2);
	         multTrsf (&tr, &t1, &t2);
	       }
	      else if (COMMAND ("do invertTrsf"))
	       { invert_old (&checkr, &t1);
	         invertTrsf (&tr, &t1);
	       }
	      else if (COMMAND ("do multLiTrsf"))
	       { TRSF left;
		 invert_old (&left, &t1);
		 trmult_old (&checkr, &left, &t2);
	         multLiTrsf (&tr, &t1, &t2);
	       }	
	      else if (COMMAND ("do multRiTrsf"))
	       { TRSF right;
		 invert_old (&right, &t2);
		 trmult_old (&checkr, &t1, &right);
	         multRiTrsf (&tr, &t1, &t2);
	       }	
	      printf ("CHECK:\n");
	      printTrsf ("%m\n", &checkr);
	      printf ("RESULT:\n");
	      printTrsf ("%m\n", &tr);
	    }

	   if (COMMANDS("test"))
	    { TRSF t1, t2, tr, checkr;
	      TRSF left, right;
	      DIFF t1Spec, t2Spec;
	      DIFF maxT1, maxT2;
	      float maxError = -1.0;
	      int maxI;
	      char maxPoint;
	      int i, j;
	      static TRSF ident = { 1.0, 0.0, 0.0, 0.0,
				    1.0, 0.0, 0.0, 0.0, 
				    1.0, 0.0, 0.0, 0.0 };

	      for (i=0; i<numtimes; i++)
	       {

#define COMPARE_ERROR(t1,t2,c)	if (cmpTrans(t1,t2) > maxError) \
				 { maxError = cmpTrans(t1,t2); 	\
				   maxI = i;			\
				   maxPoint = (c);		\
				   maxT1 = t1Spec;		\
				   maxT2 = t2Spec;		\
				 }

		 /* generate first test transform */

		 t1Spec.t.x = frandom (-1000., 1000.);
	         t1Spec.t.y = frandom (-1000., 1000.);
	         t1Spec.t.z = frandom (-1000., 1000.);
	 	 t1Spec.r.x = frandom (-180., 180.);
	 	 t1Spec.r.y = frandom (-180., 180.);
	 	 t1Spec.r.z = frandom (-180., 180.);

	      	 rpyToTrsf  (&t1, t1Spec.r.x, t1Spec.r.y, t1Spec.r.z);
	      	 xyzToTrsf (&t1, t1Spec.t.x, t1Spec.t.y, t1Spec.t.z);

		 /* generate second test transform */

		 t2Spec.t.x = frandom (-1000., 1000.);
	         t2Spec.t.y = frandom (-1000., 1000.);
	         t2Spec.t.z = frandom (-1000., 1000.);
	 	 t2Spec.r.x = frandom (-180., 180.);
	 	 t2Spec.r.y = frandom (-180., 180.);
	 	 t2Spec.r.z = frandom (-180., 180.);

	      	 rpyToTrsf  (&t2, t2Spec.r.x, t2Spec.r.y, t2Spec.r.z);
	      	 xyzToTrsf (&t2, t2Spec.t.x, t2Spec.t.y, t2Spec.t.z);

	         if (COMMAND ("test multTrsf"))
	          { 
		    trmult_old (&checkr, &t1, &t2);

		    left = t1;
		    right = t2;
		    multTrsf (&tr, &left, &right);
		    COMPARE_ERROR (&checkr, &tr, 'A');

		    left = t1;
		    right = t2;
		    multTrsf (&left, &left, &right);
		    COMPARE_ERROR (&checkr, &left, 'B');

		    left = t1;
		    right = t2;
		    multTrsf (&right, &left, &right);
		    COMPARE_ERROR (&checkr, &right, 'C');

		    trmult_old (&checkr, &t1, &t1);
		    left = t1;
		    multTrsf (&left, &left, &left);
		    COMPARE_ERROR (&checkr, &left, 'D');
	          }
	         else if (COMMAND ("test invertTrsf"))
	          { invert_old (&checkr, &t1);

		    left = t1;
	            invertTrsf (&tr, &left);
		    COMPARE_ERROR (&checkr, &tr, 'A');
		    
		    left = t1;
	            invertTrsf (&left, &left);
		    COMPARE_ERROR (&checkr, &left, 'B');
		    
		    multTrsf (&checkr, &left, &t1);
		    COMPARE_ERROR (&checkr, &ident, 'C');

	          }
	         else if (COMMAND ("test multLiTrsf"))
	          { 
		    invert_old (&left, &t1);
		    trmult_old (&checkr, &left, &t2);

		    left = t1;
		    right = t2;
		    multLiTrsf (&tr, &left, &right);
		    COMPARE_ERROR (&checkr, &tr, 'A');

		    left = t1;
		    right = t2;
		    multLiTrsf (&left, &left, &right);
		    COMPARE_ERROR (&checkr, &left, 'B');

		    left = t1;
		    right = t2;
		    multLiTrsf (&right, &left, &right);
		    COMPARE_ERROR (&checkr, &right, 'C');

		    left = t1;
		    multLiTrsf (&left, &left, &left);
		    COMPARE_ERROR (&ident, &left, 'D');
		  }
	         else if (COMMAND ("test multRiTrsf"))
		  { 
		    invert_old (&right, &t2);
		    trmult_old (&checkr, &t1, &right);

		    left = t1;
		    right = t2;
		    multRiTrsf (&tr, &left, &right);
		    COMPARE_ERROR (&checkr, &tr, 'A');

		    left = t1;
		    right = t2;
		    multRiTrsf (&left, &left, &right);
		    COMPARE_ERROR (&checkr, &left, 'B');

		    left = t1;
		    right = t2;
		    multRiTrsf (&right, &left, &right);
		    COMPARE_ERROR (&checkr, &right, 'C');

		    left = t1;
		    multRiTrsf (&left, &left, &left);
	          }	
	       }
	      printf ("Maximum difference = %g at point %c, iteration %d\n",
		      maxError, maxPoint, maxI);
	      printf ("T1:\n");
	      PRINTVF ("%7.1f\n", &maxT1, 6);
	      printf ("T2:\n");
	      PRINTVF ("%7.1f\n", &maxT2, 6);
	    }
	   else if (COMMANDS ("time"))
	    { float start, stop;
	      TRSF t2, tr;
	      register int i;

	      rpyToTrsf (&t2, 30., 30., 30.);
	      xyzToTrsf (&t2, 1., 2., 3.);
	      tr = t2;
	
	      start = cputime (User);
	   	
#define SPIN(x)	for (i=numtimes;i>0;i--) { x; }

	      if (COMMAND ("time multTrsf"))
	       { SPIN (multTrsf (&tr, &t2, &t2));
	       }
	      else if (COMMAND ("time trmultinp"))
	       { SPIN (multTrsf (&tr, &tr, &t2));
	       }
	      else if (COMMAND ("time invertTrsf"))
	       { SPIN (invertTrsf (&tr, &t2));
	       }
	      else if (COMMAND ("time multLiTrsf"))
	       { SPIN (multLiTrsf (&tr, &t2, &t2));
	       }
	      else if (COMMAND ("time multRiTrsf"))
	       { SPIN (multRiTrsf (&tr, &t2, &t2));
	       }
	      stop = cputime (User);

	      printf ("Execution time = %lg seconds\n", stop-start);
	    }
	 }
	while (!(COMMAND("exit")));
}


read_transform (t)
TRSF *t;
{
	float x, y, z, roll, pitch, yaw;

	printf ("x, y, z, roll, pitch, yaw > \n");
	if (scanf ("%lf%lf%lf%lf%lf%lf", 
		   &x, &y, &z, &roll, &pitch, &yaw) != 6)
	 { return (0);
	 }

	xyzToTrsf (t, x, y, z);
	rpyToTrsf (t, roll, pitch, yaw);
	return (1);
}
