/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	drive
 *
 * DESCRIPTION:	This module contains routines for creating and manipulating
 *		robot drive parameters. The drive parameters are a set of 6
 *		scalars from which a straight line motion may be computed
 *		in Cartesian coordinates. For details, see Richard Paul's
 *		book, Chapter 5.
 *
 *		The drive parameters are stored in a structure called DRIVEP.
 * 
 * NOTES:	All of these functions work on a pass by reference basis to 
 *		save execution time. All the routines return the address of
 *		the result.
 *
 *		The functions 'scale_driveparams' does not scale the 'dpsi'
 *		field since most applications do not desire this.
 *
 * AUTHOR/DATE:	John Lloyd, May, 1986, and Vincent Hayward, Purdue University,
 *		1983.
 *
 *-------------------------------------------------------------------------*/

#include <math.h>

#include "../h/fastmath.h"
#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/robotMath.h"

#define SMALL 1.0e-5

DRIVEP *scaleDrive (dr, a1, d1)
register DRIVEP *dr, *d1;
float a1;
{
	dr->dx = a1 * d1->dx;
	dr->dy = a1 * d1->dy;
	dr->dz = a1 * d1->dz;
	dr->dphi = a1 * d1->dphi;
	dr->dthe = a1 * d1->dthe;
	dr->dpsi = d1->dpsi;
	return (dr);
}

#define DOT(v1,v2)	((v1).x * (v2).x + (v1).y * (v2).y + (v1).z * (v2).z)

DRIVEP *trsfToDrive (dr, t1)
register DRIVEP *dr;
register TRSF *t1;
{
	/* Compute the drive parameters from a transform */
	
	register float n1, n2;	/* local variables for common subexpr */
	register float x, y;
	float spsi, cpsi, vthe;

	dr->dx = t1->p.x;
	dr->dy = t1->p.y;
	dr->dz = t1->p.z;

	if (FABS(t1->a.x) < SMALL && FABS(t1->a.y) < SMALL)
	 { dr->dpsi = 0.0;
	 }
	else 
	 { dr->dpsi = ATAN2 (t1->a.y, t1->a.x);
	 }

	/* compute theta */

	y = SQRT(t1->a.x * t1->a.x + t1->a.y * t1->a.y);

	if (y < SMALL && FABS(t1->a.z) < SMALL)
	 { dr->dthe = 0.;
	 }
	else
	 { dr->dthe = ATAN2 (y, t1->a.z);
	 }

	/* compute phi */

	SINCOS(dr->dpsi, &spsi, &cpsi);
	vthe = 1. - t1->a.z;

	n1 = - spsi * cpsi * vthe;
	n2 =   cpsi * cpsi * vthe + t1->a.z;

	y = n1 * t1->n.x + n2 * t1->n.y - t1->a.y * t1->n.z;
	x = n1 * t1->o.x + n2 * t1->o.y - t1->a.y * t1->o.z;

	if (FABS(y) < SMALL && FABS(x) < SMALL)
	 { dr->dphi = 0.;
	 }
	else
	 { dr->dphi = ATAN2 (y, x);
	 }

	return (dr);
}

DRIVEP *diffTrsfToDrive (dr, t1, t2)
register DRIVEP *dr;
register TRSF *t1, *t2;
{
	/*	
		Computes the drive parameters between t1 and t2
		using differential realtionships; i.e., this function
		assumes that t1 and t2 are close together 

		T + dT = T (DEL)
			    		    T      T
		(DEL - I) = inv(T) dT =  ( R dR   R dp + p' ) 

			   T
		but p' = -R  p, so

			      T	    T
		(DEL - I) = (R dR  R (dp - p))


	The formula for the drive parameters is


		    0         -dp     Cy dtheta		0	rz	ry

		   dp           0     Sy dtheta	   =    -rz	0	rx

	       -Cy dtheta  -Sy dtheta     0		-ry	-rx	0
	*/

	/* start by taking the difference between t1 and t2 */

	float dpx, dpy, dpz, dox, doy, doz, dax, day, daz;
	float rx, ry, rz;

	dox = t2->o.x - t1->o.x;
	doy = t2->o.y - t1->o.y;
	doz = t2->o.z - t1->o.z;
	dax = t2->a.x - t1->a.x;
	day = t2->a.y - t1->a.y;
	daz = t2->a.z - t1->a.z;
	dpx = t2->p.x - t1->p.x;
	dpy = t2->p.y - t1->p.y;
	dpz = t2->p.z - t1->p.z;

	/* Now compute the velocities: */

	dr->dx = t1->n.x * dpx + t1->n.y * dpy + t1->n.z * dpz;
	dr->dy = t1->o.x * dpx + t1->o.y * dpy + t1->o.z * dpz;
	dr->dz = t1->a.x * dpx + t1->a.y * dpy + t1->a.z * dpz;

	rz = t1->n.x * dox + t1->n.y * doy + t1->n.z * doz;
	ry = t1->n.x * dax + t1->n.y * day + t1->n.z * daz;
	rx = t1->o.x * dax + t1->o.y * day + t1->o.z * daz;

	/* And now compute the rotational drive parameters */

	dr->dphi = -rz;
	if (FABS(rx) < SMALL && FABS(ry) < SMALL)
	 { dr->dpsi = 0.0;
	 }
	else
	 { dr->dpsi = ATAN2 (rx, ry);
	 }
	dr->dthe = SQRT (ry*ry + rx*rx);

	return (dr);
}

TRSF *driveToTrsf (tr, dp)
register TRSF *tr;
register DRIVEP *dp;
{
	float sphi, cphi, spsi, cpsi, sthe, cthe, vthe, loc;

	tr->p.x = dp->dx;
	tr->p.y = dp->dy;
	tr->p.z = dp->dz;

	SINCOS(dp->dphi, &sphi, &cphi);
	SINCOS(dp->dpsi, &spsi, &cpsi);
	SINCOS(dp->dthe, &sthe, &cthe);
	vthe = 1. - cthe;

	tr->a.x = cpsi * sthe;
	tr->a.y = spsi * sthe;
	tr->a.z = cthe;

	tr->o.x = - sphi * (spsi * spsi * vthe + cthe)
		  + cphi * (loc = - spsi * cpsi * vthe);
	tr->o.y = - sphi * (loc) + cphi * (cpsi * cpsi * vthe + cthe);
	tr->o.z = sphi * tr->a.x - cphi * tr->a.y;

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

	return (tr);
}

TRSF *extrapTrsfByDrive (tr, s, dp)
register TRSF *tr;
float s;
register DRIVEP *dp;
{
	/* Compute the transform corresponding to scaled
	   drive parameters.

	   Essentially, this is 

		tr = driveToTrsf (dp * s)

	   We unroll the driveToTrsf call here because we
	   are executing on a microVAX, and are rather fussy
	   about routine call overhead.
	*/

	float sphi, cphi, spsi, cpsi, sthe, cthe, vthe, loc;

	tr->p.x = s * dp->dx;
	tr->p.y = s * dp->dy;
	tr->p.z = s * dp->dz;

	SINCOS(s * dp->dphi, &sphi, &cphi);
	SINCOS(s * dp->dthe, &sthe, &cthe);
	SINCOS(dp->dpsi, &spsi, &cpsi);
	vthe = 1. - cthe;

	tr->a.x = cpsi * sthe;
	tr->a.y = spsi * sthe;
	tr->a.z = cthe;

	tr->o.x = - sphi * (spsi * spsi * vthe + cthe)
		  + cphi * (loc = - spsi * cpsi * vthe);
	tr->o.y = - sphi * (loc) + cphi * (cpsi * cpsi * vthe + cthe);
	tr->o.z = sphi * tr->a.x - cphi * tr->a.y;

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

	return (tr);
}

#if TEST
#include <stdio.h>
#include "../h/ctree.h"
#include "../h/vector.h"
#include "../h/cputime.h"

#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))

char key_buf [256];

TRSF tr, t1;
DRIVEP dr, d1;

VECT zerovect = { 0.0, 0.0, 0.0 };

main(argc, argv)
int argc;
char **argv;
{
	char **argvec;
	char *keytree;
	char tName[256];
	char rName[256];
	char oName[256];

	float tx, ty, tz;
	float dthe, dphi, dpsi;
	float roll, pitch, yaw;
	float scale;

	int interactive = NO;

	argvec = argv;
	argc--; argv++;
	while (argc)
	 { if (ARGUMENT ("-i"))
	    { interactive = YES;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { fprintf (stderr, "Usage: %s [-i]\n", argvec[0]);
	   exit (-1);
	 }

	if (interactive == NO)
	 { doAutoTest();
	   return;
	 }

	keytree = tree_match_parse("					\
(	quit								\
	set 								\
	 (trsf								\
	   (%f", &tx, 	",,, \"x translation value (mm.)\"		\
	   (%f", &ty, 	",,, \"y translation value (mm.)\"		\
	   (%f", &tz, 	",,, \"z translation value (mm.)\"		\
	   (%f", &yaw, 	",,, \"yaw angle (degrees)\"			\
	   (%f", &pitch, ",,, \"pitch angle (degrees)\"			\
	   (%f", &roll,	",,, \"roll angle (degrees)\"))))))		\
	  drive								\
	   (%f", &tx, 	",,, \"x translation value (mm.)\"		\
	   (%f", &ty, 	",,, \"y translation value (mm.)\"		\
	   (%f", &tz, 	",,, \"z translation value (mm.)\"		\
	   (%f", &dthe,	",,, \"dthe (degrees)\"				\
	   (%f", &dphi, ",,, \"dphi (degrees)\"				\
	   (%f", &dpsi, ",,, \"dpsi (degrees)\"))))))			\
	 )								\
	trsfToDrive							\
	driveToTrsf							\
	extrapTrsfByDrive							\
	 (%f", &scale, ",,, \"scale factor\")				\
	show								\
)");
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	do
 	 { tree_match (keytree, "DRIVEP> ", key_buf);

	   if (COMMAND("set trsf"))
	    { xyzToTrsf (&t1, tx, ty, tz);
	      rpyToTrsf (&t1, roll, pitch, yaw);
	    }
	   else if (COMMAND("set drive"))
	    { d1.dx = tx;
	      d1.dy = ty;
	      d1.dz = tz;
	      d1.dthe = dthe * DEGTORAD;
	      d1.dphi = dphi * DEGTORAD;
	      d1.dpsi = dpsi * DEGTORAD;
	    }
	   else if (COMMAND("show"))
	    { printDrive ("d1", &d1);
	      printDrive ("dr", &dr);
	      printf ("%s:\n", "t1");
	      printTrsf ("%11.8.5m\n", &t1);
	      printf ("%s:\n", "tr");
	      printTrsf ("%11.8.5m\n", &tr);
	    }
	   else if (COMMAND("trsfToDrive"))
	    { trsfToDrive (&dr, &t1);
	      printDrive ("dr", &dr);
	    }
	   else if (COMMAND("driveToTrsf"))
	    { driveToTrsf (&tr, &d1);
	      printf ("tr: \n");
	      printTrsf ("%11.8.5m\n", &tr);
	    }
	   else if (COMMAND("extrapTrsfByDrive"))
	    { extrapTrsfByDrive (&tr, scale, &d1);
	      printf ("tr:\n");
	      printTrsf ("%11.8.5m\n\n", &tr);
	    }
	 }
	while (!(COMMAND("quit")));
}

printDrive (name, d1)
char *name;
DRIVEP *d1;
{
	DRIVEP dtmp;

	dtmp = *d1;
	dtmp.dthe *= RADTODEG;
	dtmp.dphi *= RADTODEG;
	dtmp.dpsi *= RADTODEG;
	printf ("%s:\n", name);
	PRINTVF ("%11.5f\n", &dtmp, 6);
}

double rangeAngle (a)
double a;
{
	/* return a in the range -PI < a <= PI */

	double n, r;

	n = a/(2*PI);
	r = n - (int)n;
	if (r <= 0.5)
	 { return (2*PI*r);
	 }
	else
	 { return (2*PI*(r-1.0));
	 }
}

double cmpAngle (a1, a2)
double a1, a2;
{
	double distance;

	a1 = rangeAngle (a1);
	a2 = rangeAngle (a2);
	if ((distance = FABS(a1 - a2)) > PI)
	 { distance -= 2*PI;
	 }
	return (distance/(2*PI));
}

double cmpDrive (drv1, drv2)
DRIVEP *drv1, *drv2;
{
	/* Returns an estimate of the % error between drv1 and drv2. */

	VECT delta;
	double size;
	double error;

	error = 0;

	error += cmpAngle (drv1->dthe, drv2->dthe);
	error += cmpAngle (drv1->dpsi, drv2->dpsi);
	error += cmpAngle (drv1->dphi, drv2->dphi);

	size = (NORMVF(drv1, 3) + NORMVF(drv2, 3)) / 2.0; /* XXX */
	if (size > SMALL)
	 { error += 3*NORMVF(SUBVF(&delta, drv1, drv2, 3),3)/size;
	 }

	return (error/6);
}

doAutoTest()
{
	/* Automatic test procedure.
	*/

	static struct {
	    VECT p;
	    float a;
	} testcases[] =
	{   0.0,   0.0,   0.0,    0.0,     
	    1.0,   2.0,   3.0,    0.1,
	    1.0,   2.0,   3.0,    0.01,
	    1.0,   2.0,   3.0,    0.001,
	    1.0,   2.0,   3.0,    0.0001,
	    1.0,   2.0,   3.0,    0.00001,
	   -1.0,  -2.0,  -3.0,   -0.001,
	    1.0,   2.0,   3.0,    1.0,
	    4.0,   5.0,   6.0,   -1.0,
	    4.0,   5.0,   6.0,   90.0,
	   -4.0,  -5.0,  -6.0,  -90.0,
	    4.0,   5.0,   6.0,  179.99999,
	    4.0,   5.0,   6.0,  179.9999,
	    4.0,   5.0,   6.0,  179.999,
	    4.0,   5.0,   6.0,  179.99,
	    4.0,   5.0,   6.0,  179.9,
	    4.0,   5.0,   6.0, -179.9999,
	    4.0,   5.0,   6.0,  180.0,
	    4.0,   5.0,   6.0, -180.0,
	    4.0,   5.0,   6.0,  180.0001,
	   -4.0,  -5.0,  -6.0, -180.0001,
	   -1.0,  -2.0,  -3.0,  270.0,
	    1.0,   2.0,   3.0, -270.0,
	};

	static VECT testvects[] =
	{ 0.0,  0.0,  1.0,
	  0.0,  0.0, -1.0,
	  0.0,  1.0,  0.0,
	  0.0, -1.0,  0.0,
	  0.0,  1.0,  1.0,
	  0.0,  1.0, -1.0,
	  0.0, -1.0,  1.0,
	  0.0, -1.0, -1.0,
	  1.0,  0.0,  1.0,
	  1.0,  0.0, -1.0,
	  1.0,  1.0,  0.0,
	  1.0, -1.0,  0.0,
	  1.0,  1.0,  1.0,
	  1.0,  1.0, -1.0,
	  1.0, -1.0,  1.0,
	  1.0, -1.0, -1.0,
	 -1.0,  0.0,  1.0,
	 -1.0,  0.0, -1.0,
	 -1.0,  1.0,  0.0,
	 -1.0, -1.0,  0.0,
	 -1.0,  1.0,  1.0,
	 -1.0,  1.0, -1.0,
	 -1.0, -1.0,  1.0,
	 -1.0, -1.0, -1.0,
	};

	static float testscales[] =
	{ -3.0, -0.3, 0.0, 0.3, 1.0, 3.0,
	};

	float error, maxerror;

	int ic, maxIc;
	int iv, maxIv;
	int is, maxIs;

	TRSF trsfIn, trsfOut;
	DRIVEP driveIn, driveOut;

	float time1, time2, looptime;
	int k;
	
	/* Try each of the above test cases with each of the test vectors.

	   In each case, create the appropriate transform using
	   quaternions. Then convert to DRIVEP and check the result.
	   Convert DRIVEP back to a TRSF and check the result again.
	   Then test the extrap function with each of the test scales.
	*/

	maxerror = 0.0;
	for (ic=0; ic<ARRAY_SIZE(testcases); ic++)
	 { for (iv=0; iv<ARRAY_SIZE(testvects); iv++)
	    { DSPL dspl;

	      dspl.p = testcases[ic].p;
	      dspl.a = testcases[ic].a * DEGTORAD;
	      dspl.u = testvects[iv];
	      unitDspl (&dspl, &dspl);

	      /* built the input TRSF and DRIVE transforms */
	      dsplToTrsf (&trsfIn, &dspl);
	      checkTrsfToDrive (&driveIn, &trsfIn);

	      /* transform is now built. Roll it through trsfToDrive()
		  and see what we get.
	       */

	      is = -1;

	      trsfToDrive (&driveOut, &trsfIn);
	      if ((error = cmpDrive (&driveOut, &driveIn)) > maxerror)
	       { maxerror = error;
		 maxIs = is; maxIc = ic; maxIv = iv;
	       }
	      driveToTrsf (&trsfOut, &driveIn);
	      if ((error = cmpTrsf (&trsfOut, &trsfIn)) > maxerror)
	       { maxerror = error;
		 maxIs = is; maxIc = ic; maxIv = iv;
	       }

	      for (is=0; is<ARRAY_SIZE(testscales); is++)
	       { float angScale;
		 DRIVEP driveInScale;

		 scaleDrive (&driveInScale, testscales[is], &driveIn);
		 extrapTrsfByDrive (&trsfOut, testscales[is], &driveIn);
		 driveToTrsf (&trsfIn, &driveInScale);

	      if (ic==9 && iv==2 && is==5) /* DEBUGGING */
	       { breakpoint();
	       }

		 if ((error = cmpTrsf (&trsfOut, &trsfIn)) > maxerror)
		  { maxerror = error;
		    maxIs = is; maxIc = ic; maxIv = iv;
		  }
	       }
	    }
	 }

#define TOLERANCE	1.0e-5
	printf ("\n%s\n\n", maxerror <= TOLERANCE ?
			   "ROUTINES OK" : "POSSIBLE ERROR CONDITION" );
	printf ("max error = %g at test indicies ic=%d iv=%d is=%d\n",
		 maxerror, maxIc, maxIv, maxIs);

	driveIn.dthe = .25*PI;
	driveIn.dpsi = .25*PI;
	driveIn.dphi = .25*PI;
	
#define TESTCOUNT 20000

	time1 = cputime (User);
	for (k=0; k<TESTCOUNT; k++)
	   ;
	time2 = cputime (User);
	looptime = time2 - time1;

	time1 = cputime (User);
	for (k=0; k<TESTCOUNT; k++)
	 { driveToTrsf (&trsfOut, &driveIn);
	 }
	time2 = cputime (User);
	printf ("driveToTrsf() takes %g usec.\n", 
		 1000000*(time2-time1)/TESTCOUNT);

	time1 = cputime (User);
	for (k=0; k<TESTCOUNT; k++)
	 { trsfToDrive (&driveOut, &trsfOut);
	 }
	time2 = cputime (User);
	printf ("trsfToDrive() takes %g usec.\n", 
		 1000000*(time2-time1)/TESTCOUNT);
}

breakpoint()
{
}

checkTrsfToDrive (dr, t1)
DRIVEP *dr;
TRSF *t1;
{
	/* At the moment, this is just a clone of trsfToDrive(). To do
	   this right, I should compute the drive parameters by a different
	   method. The test routine will still work... except it will just
	   be testing the forward calculation against the inverse.

	   I also use doubles in this routine for greater precision.
	 */

	double n1, n2;		/* local variables for common subexpr */
	double x, y;
	double lthe, lpsi;
	double spsi, cpsi, vthe;

	dr->dx = t1->p.x;
	dr->dy = t1->p.y;
	dr->dz = t1->p.z;

	if (FABS(t1->a.x) < SMALL && FABS(t1->a.y) < SMALL)
	 { lpsi = dr->dpsi = 0.;
	 }
	else 
	 { lpsi = atan2 (t1->a.y, t1->a.x);
	   dr->dpsi = lpsi;
	 }

	/* compute theta */

	y = sqrt(t1->a.x * t1->a.x + t1->a.y * t1->a.y);

	if (y < SMALL && FABS(t1->a.z) < SMALL)
	 { lthe = dr->dthe = 0.;
	 }
	else
	 { lthe = atan2 (y, t1->a.z);
	   dr->dthe = lthe;
	 }

	/* compute phi */

	SINCOS(lpsi, &spsi, &cpsi);
	vthe = 1. - t1->a.z;

	n1 = - spsi * cpsi * vthe;
	n2 =   cpsi * cpsi * vthe + t1->a.z;

	y = n1 * t1->n.x + n2 * t1->n.y - t1->a.y * t1->n.z;
	x = n1 * t1->o.x + n2 * t1->o.y - t1->a.y * t1->o.z;

	if (FABS(y) < SMALL && FABS(x) < SMALL)
	 { dr->dphi = 0.;
	 }
	else
	 { dr->dphi = atan2 (y, x);
	 }
}

#endif
