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

/*---------McGill-Computer-Vision-and-Robotics-Laboratory--C-Module---------
 *		
 * NAME:	jns_to_tr(), tr_to_jns(), solveconf() -- kinematic functions
 *		for the PUMA manipulator.
 *
 * USAGE:	#include <robot/rccl.h>
 *		#include <robot/kine.h>
 *		
 *		jns_to_tr (t, j, sncs)
 *		  TRSF *t;
 *		  JNTS *j;
 *		  SNCS *sncs;
 *
 *		status = tr_to_jns (j, t, refj, sncs)
 *		  int status;
 *		  JNTS *j, *refj;
 *		  TRSF *t;
 *		  SNCS *sncs;
 *
 *		solveconf (j)
 *		  JNTS *j;
 *
 * DESCRIPTION: jns_to_tr() computes the T6 transform for a set of joint
 *		  angles. It also solves for the configuration of the robot,
 *		  placing this in the .conf field of 'j'.
 *
 *		tr_to_jns() performs the kinematic solution; it takes the
 *		  T6 transform, along with a configuration specified in
 *		  the .conf field of 'j', and sets the angles in 'j' to
 *		  the appropriate range coordinates. Some joints may have
 *		  more than 360 degrees of travel, which makes the choice
 *		  of joint angles ambiguous. In these cases, the argument
 *		  'refj' is use to describe a set of reference angles.
 *		  The angle values are computed to be as close to 'refj'
 *		  as possible. 'refj' is also used to supply the value for
 *		  angles when required by singularities.
 *
 *		solveconf() computes the configuration corresponding to a
 *		  set of joint values in 'j' (range coordinates), and sets
 *		  the .conf field of 'j' to the proper configuration.
 *
 *		  'sncs' is an optional pointer to a SNCS data structure
 *		  (defined in ../h/kine.h) which describes the sine and
 *		  cosine values for each joint, as well as other parameters
 *		  related to the Jacobian. The data structure is used by
 *		  the Jacobian manipulation routines. If 'sncs' is supplied,
 *		  then the kinematic functions load the structure with the 
 *		  sine and cosine values (which are calculated as a by 
 *		  product of the kinematics). If this is not desired, then
 *		  'sncs' should be supplied as NULL.
 *		
 * ARGUMENTS:	  TRSF *t -- 	the T6 transformation.
 *
 *		  JNTS *j --	manipulator joint values (range coordinates).
 *				Also, the .conf field is loaded by the forward
 *				kinematics and solveconf(), and used by the 
 *				inverse kinematics.
 *
 *		  JNTS *refj --	reference joint values used when the a joint
 *				can travel more than 360 degrees.
 *
 *		  SNCS *sncs -- Data structure that contains terms for
 *		  		computing Jacobians, which is loaded with the
 *				sines and cosines of the joint angles if
 *				the argument is specified as non-NULL.
 *
 * RETURN VALS: jns_to_tr() returns nothing.
 *
 *		tr_to_jns() returns a bit code describing the if the desired
 *		  position is out of range, or if the solution is close to
 *		  a singularity and hence possibly inaccurate.
 *
 *		  The present return values are:
 *		
 *			BITS 0-5:	Joints 1-6 out of range
 *			BIT 6:		Angle 5 is zero (degeneracy)
 *			BIT 7:		Position too close to robot
 *			BIT 8:		Position too far from robot
 *			BIT 9:		Position too close to robot
 *		
 *		  THESE VALUES WILL PROBABLY BE REDEFINED SHORTLY.
 *
 * AUTHOR:	Vincent Hayward, Purdue University, 1983.
 *		Rewritten by John Lloyd, McGill University, fall 1986.
 *
 * NOTES:	The 'configuartion' of the manipulator describes the which
 *		of the multiple solutions to the kinematics is in effect.
 *		Except for degenerate cases, there are 8 solutions
 *		corresponding to a choice of right-handed/left-handed
 *		(denoted by characters 'r' and 'l', respectively), 
 *		shoulder up/down ('u', 'd'), and wrist flip/noflip
 *		('f', 'n'). A configuartion is described by a string of
 *		the appropriate characters.
 *
 *-------------McGill-Computer-Vision-and-Robotics-Laboratory--------------*/

#include "rccl.h"
#include "kine.h"
#include "pumadata.h"

#define EPSILONF 2.0e-6
#define FABS(x)	((x) < 0.0 ? -(x) : (x))

/* #define FASTMATH */

#ifdef FASTMATH
#include "fastmath.h"
#else
#define ATAN2(y,x)	atan2(y,x)
#define SINCOS(a, s, c) { *(s) = sin(a); *(c) = cos(a); }
#define COS(a)		cos(a)
#endif

/* performs mod 2 pi */

static float range(a) /*##*/	/* Sets a to  0 <= a < pi2  */
double a;
{
#ifdef vax
	asm ("emodd	$0d0.159154943091895336,$0,4(ap),r4,r0");
	asm ("bgeq	1f");
	asm ("addf2	$0f1.0,r0");
	asm ("1:");
	asm ("mulf2	$0f6.283185307179586454,r0");
#else
	if (a > 15. || a < -15.)
		return(10.);

	while(a >= PIT2)
		a -= PIT2;
	while(a < 0.)
		a += PIT2;
	if (a > PIT2)
		return(0.);
	return(a);
#endif
}

jns_to_tr(t, j, sc) /*::*/
register TRSF_PTR t;	  /* result transform 			  	 */
register JNTS_PTR j;	  /* joints in range coordinates + configuration */
register SNCS *sc;	  /* are sncs_d entries updated ?	  	 */
{
	/* sines and cosines */

	real c1, s1, c23, s23, c2, s2, c4, s4, c5, s5, c6, s6;

 	/* constants */

	register float k2, k3, k4, x;

	/* Get the sines and cosines of joints 1-6, with absolute */
	/* radian coordinates.					  */

	SINCOS (j->th1 + jmin_c.th1, &s1, &c1);
	SINCOS (j->th2 + jmin_c.th2, &s2, &c2);
	SINCOS (j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3, &s23, &c23);
	SINCOS (j->th4 + jmin_c.th4, &s4, &c4);
	SINCOS (j->th5 + jmin_c.th5, &s5, &c5);
	SINCOS (j->th6 + jmin_c.th6, &s6, &c6);

	/* Compute the constants  ... */

	/* and plug them into the equation for u1: */

	k2  = c4 * c5 * s6 + s4 * c6;
	k3  = - c23 * k2 + s23 * s5 * s6;
	k4  = - s4 * c5 * s6 + c4 * c6;

	t->o.x = c1 * k3 - s1 * k4;
	t->o.y = s1 * k3 + c1 * k4;
	t->o.z = - s23 * k2 - c23 * s5 * s6;

	k2  = s4 * s5;
	k3 = c23 * c4 * s5 + s23 * c5;

	t->a.x = - c1 * k3 + s1 * k2;
	t->a.y = - s1 * k3 - c1 * k2;
	t->a.z = - s23 * c4 * s5 + c23 * c5;

	k2 = - D4 * s23 + A3 * c23 + A2 * c2;

	t->p.x = c1 * k2 + s1 * D3;
	t->p.y = s1 * k2 - c1 * D3;
	t->p.z = D4 * c23 + A2 * s2 + A3 * s23;

	/* Get n from n = o X a, to make matrix more normal and save time */

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

	/* Determine the configurations: */
	/*  Righty - Lefty		 */

	x = range( j->th1 + jmin_c.th1 - ATAN2(t->p.y, t->p.x));
	(j->conf)[0] = (x < PIB2 || x > PI + PIB2) ? 'r' : 'l';

	/*  Up - Down  */

	x = range(-ATANA3D4 + j->th3 + jmin_c.th3);
	(j->conf)[1] = (x < PIB2 || x > PI + PIB2) ? 'd' : 'u';

	/*  Flip - Noflip  */

	(j->conf)[2] = (range(j->th5 + jmin_c.th5) > PI) ? 'f' : 'n';
	(j->conf)[3] = '\0';

	if (sc != NULL) {			/* Update the sncs_d if needed  */
		sc->s1 = s1;
		sc->c1 = c1;
		sc->s2 = s2;
		sc->c2 = c2;
		sc->s23 = s23;
		sc->c23 = c23;
		SINCOS (j->th3 + jmin_c.th3, &(sc->s3), &(sc->c3));
		sc->s4 = s4;
		sc->c4 = c4;
		sc->s5 = s5;
		sc->c5 = c5;
		sc->s6 = s6;
		sc->c6 = c6;
	}
}

tr_to_jns(j, t, refj, sc) /*::*/
JNTS *j;	  	  /* joints in range coordinates + configuration */
register TRSF_PTR t;	  /* input transform 			  	 */
JNTS *refj;		  /* reference joints				 */
register SNCS *sc;	  /* are sncs_d entries updated ?	  	 */
{
	int code = 0;

	register float r1, r2, r3;
	float f11a, f11o, f11p, d; 	/* fast constants        */

	real k1, d1, d2, p1, p2;
	real epsilon2;
	real a22a32d42;

	real t1d, t3d, f13a, f31a, f13o, w1, w2, wd;
	real s1, c1, s2, c2, s23, c23, s3, c3, s4, c4, s5, c5, s6, c6;

	JNTS ang;				/* non-range angle values */

	double sqrt_arg;			/* argument to square root */

	static real t4o;			/* old value of t4	 */

	/* Robot has a default configuration 'run' if non is specified	 */

	static lefty = NO, up = YES, flip = NO; /* configuration flags	 */
	char *c = j->conf;

	/* Parse the configuration string to find configuration		 */

	while (*c) {
		switch (*c++) {
		case 'l' : 
			lefty = YES;
			break;
		case 'r' : 
			lefty = NO;
			break;

		case 'u' : 
			up = YES;
			break;

		case 'd' : 
			up = NO;
			break;

		case 'f' : 
			flip  = YES;
			break;

		case 'n' : 
			flip  = NO;
			break;

		default  : 
			break;
		}
	}

	/* Now go ahead and compute the angles */

	/* theta1 */

#define EPSILON1 (EPSILONF*D32)

	sqrt_arg = t->p.x * t->p.x + t->p.y * t->p.y - D32;
 	if (sqrt_arg < EPSILON1)
	 { if (sqrt_arg < EPSILON1)
	    { return (0x80);
	    }
	   else
	    { sqrt_arg = 0.0;
	    }
	 }
	ang.th1 = ATAN2(t->p.y, t->p.x) + 
		  ATAN2(D3, (lefty) ? -sqrt(sqrt_arg) : sqrt(sqrt_arg));
	SINCOS (ang.th1, &s1, &c1);

	/* theta2 */

#define EPSILON2	(EPSILONF*(A2+D4)/2)
	
	r1 = c1 * t->p.x + s1 * t->p.y;
	r2 = t->p.z * t->p.z + r1 * r1;

	r3 = (A22A32D42 + r2) / ( 2 * A2 );
	sqrt_arg = r2 - r3 * r3;

	if (sqrt_arg < EPSILON2)
	 { if (sqrt_arg < -EPSILON2)
	    { return (0x100);
	    }
	   else
	    { sqrt_arg = 0.0;
	    }
	 }
	ang.th2 = ATAN2 (r3, (up) ? -sqrt(sqrt_arg) : sqrt(sqrt_arg)) -
		  ATAN2 (r1, t->p.z);
	SINCOS (ang.th2, &s2, &c2);

	/* theta3 */

	r2 = t->p.z * s2 + c2 * r1 - A2;
	r3 = c2 * t->p.z - r1 * s2;
	ang.th3 = ATAN2 (-D4*r2 + A3*r3, A3*r2 + D4*r3);
	SINCOS (ang.th3, &s3, &c3);

	s23 = s2 * c3 + s3 * c2;
	c23 = c2 * c3 - s2 * s3;
	
	/* theta4 */

	r1 = - s1 * t->a.x + c1 * t->a.y;
	r2 =   c1 * t->a.x + s1 * t->a.y;
	r3 = c23 * r2 + s23 * t->a.z;

	if (FABS (r1) < EPSILONF && FABS (r3) < EPSILONF)
	 { ang.th4 = JMIN4 + refj->th4;
	   code |= 0x40;
	 }
	else
	 { ang.th4 = (flip) ? ATAN2(r1,  r3) : ATAN2( -r1, -r3);
	 }

	SINCOS (ang.th4, &s4, &c4);

	/* theta5 */

	s5 = - c4 * r3 - s4 * r1 ;
	c5 = - s23 * r2 + c23 * t->a.z;
	ang.th5 = ATAN2(s5, c5);

	/* theta6 */

 	r1 =   c1 * t->o.x + s1 * t->o.y;
	r3 = - s1 * t->o.x + c1 * t->o.y;
	s6 = - c5 * (c4 * (c23 * r1 + s23 * t->o.z) + s4 * r3)
	     + s5 * (s23 * r1 - c23 * t->o.z);
	c6 = - s4 * (c23 * r1 + s23 * t->o.z) + c4 * r3;
	
	ang.th6 = ATAN2(s6, c6);

	code |= range_angles (&(j->th1), &(ang.th1), &(jmin_c.th1), 
			      &(refj->th1), &(jrng_c.th1));

	if (sc != NULL) {		/* Update sncs_d if needed */
		sc->s1 = s1;
		sc->c1 = c1;
		sc->s2 = s2;
		sc->c2 = c2;
		sc->s23 = s23;
		sc->c23 = c23;
		sc->s3 = s3;
		sc->c3 = c3;
		sc->s4 = s4;
		sc->c4 = c4;
		sc->s5 = s5;
		sc->c5 = c5;
		sc->s6 = s6;
		sc->c6 = c6;
	}
	return(code);
}

range_angles (r, a, jmin, aref, jrng)
register float r[], a[], aref[];
float jmin[], jrng[];
{
	int code = 0;
	register int j;
	register float angle;

	for (j=0; j<6; j++)
 	 { angle = a[j] - jmin[j];

	   if (angle <= aref[j])
	    { while (angle <= aref[j])
	       { angle += PIT2;
	       }
	      if (angle - aref[j] <= PI && angle < jrng[j])
	       { r[j] = angle;
	       }
	      else
	       { angle -= PIT2;
	         if ((r[j] = angle) <= 0.0)
	          { code |= (1 << j);
	          }
	       }
	    }
	   else
	    { while (angle > aref[j])
	       { angle -= PIT2;
	       }
	      if (aref[j] - angle <= PI && angle > 0.0)
	       { r[j] = angle;
	       }
	      else
	       { angle += PIT2;
	         if ((r[j] = angle) >= jrng[j])
	          { code |= (j << 1);
		  }
	       }
	    }
	 }
	return (code);
}

solveconf(j) /*::*/
register JNTS_PTR j;	  /* joints in range coordinates + configuration */
{
	real s23, c23, c2;	/* sines, cosines, and constants */
	real h1, phi;

	SINCOS (j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3, &s23, &c23);
	c2 = COS (j->th2 + jmin_c.th2);

	/* Righty - Lefty */

	h1 = - D4 * s23 + A2 * c2 + c23 * A3;
	(j->conf)[0] = (h1 >= 0) ? 'r' : 'l';

	/* Up - Down */

	phi = range( j->th3 + jmin_c.th3 - ATANA3D4);
	(j->conf)[1] = (phi < PIB2 || phi > PI + PIB2) ? 'd' : 'u';

	/* Flip - Noflip */

	(j->conf)[2] = (range(j->th5 + jmin_c.th5) > PI) ? 'f' : 'n';
	(j->conf)[3] = '\0';
}
