/*-=-=-=-=-=-=-=-=-=-=-=- 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 ~=~=~=~=
 *		
 * MODULE NAME:	pumaKinematic.c
 *
 * DESCRIPTION:	Forward and inverse kinematic routines for the PUMA robots.
 *		For description, see 'man pumaKinematics'
 * 
 * AUTHOR/DATE:	John Lloyd, Spring 1987, after Vincent Hayward, Purdue, 1983
 *		Rewrote routines to interface with new RCI, winter 1988.
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <math.h>

#include "../h/fastmath.h"
#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"
#include "../h/puma_jls.h"
#include "../h/puma_kynvar.h"
#include "../h/robotTypes.h"

#include "../h/rcclDefs.h"	/* for F_UNDEF */

#define EPSILONF 2.0e-6

#define SINGTEST	0

/* 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
}

/* ---- Definition of link parameters to make code more readable ---- */

#define A2		gen->dh[1].a	/* basic A matrix parameters */
#define D3		gen->dh[2].d
#define D4		gen->dh[3].d
#define A3		gen->dh[2].a
#define D32		pkyn->d32
#define A22_A32D42	pkyn->a22_a32d42 	/* A2*A2 - A3*A3 - D4*D4 */
#define ATANA3D4	pkyn->atana3d4		/* atan2(A3, D4) */
#define	EPSILON_A2D4	pkyn->epsilon_a2d4	/* Precision of (A2+D4)/2 */
#define EPSILON_D32	pkyn->epsilon_d32	/* Precision of D32 */

#define J1	0
#define J2	1
#define J3	2
#define J4	3
#define J5	4
#define J6	5

#ifdef sgi
#define REGISTER		/* !! patch for SGI optimizer bug */
#else
#define REGISTER register
#endif

pumaFwdKinematics (t, conf, j, kyn, var)
REGISTER TRSF *t;	  /* resulting transform */
unsigned long *conf;	  /* returns configuration information */
REGISTER float *j;	  /* joints in range coordinates + configuration */
KYN *kyn;	  	  /* kinematic parameters */
void *var;		  /* palce for storing computing sines/cosines */
{
	register PUMA_KYN *pkyn = (PUMA_KYN*)kyn->ext;
	register PUMA_VAR *pvar = (PUMA_VAR*)var;
	register GEN_KYN *gen = kyn->gen;

	/* sines and cosines */

	float 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[J1], &s1, &c1);
	SINCOS (j[J2], &s2, &c2);
	SINCOS (j[J3] + j[J2], &s23, &c23);
	SINCOS (j[J4], &s4, &c4);
	SINCOS (j[J5], &s5, &c5);
	SINCOS (j[J6], &s6, &c6);

	/* Compute the constants  ... */

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

	if (pkyn->oldCoordDefs)
	 {
	   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;
	 }
	else
	 { 
	   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;

	if (conf != NULL)
	 { *conf = 0;

	   /* Determine the configurations: */
	   /*  Righty - Lefty */
   
	   if (pkyn->oldCoordDefs)
	    { x = range(j[J1] + ATAN2(t->p.y, -t->p.x));
	    }
	   else
	    { x = range(j[J1] - ATAN2(t->p.y, t->p.x));
	    }

	   if (x < PIB2 || x > PI + PIB2)
	    { *conf |= PUMA_RIGHT_CONF;
	    }
   
	   /*  Up - Down  */
   
	   if (pkyn->oldCoordDefs)
	    { x = range(ATANA3D4 + j[J3]);
	    }
	   else
	    { x = range(-ATANA3D4 + j[J3]);
	    }
	   if (x < PIB2 || x > PI + PIB2)
	    { *conf |= PUMA_DOWN_CONF;
	    }
   
	   /*  Flip - Noflip  */
   
	   if (pkyn->oldCoordDefs)
	    { if (range(j[J5]) <= PI)
	       { *conf |= PUMA_FLIP_CONF;
	       }
	    }
	   else
	    { if (range(j[J5]) > PI)
	       { *conf |= PUMA_FLIP_CONF;
	       }
	    }
	 }

	if (pvar != NULL)	/* Update sines and cosinesd if needed */
	 { pvar->s[J1] = s1;
	   pvar->c[J1] = c1;
	   pvar->s[J2] = s2;
	   pvar->c[J2] = c2;
	   pvar->s23 = s23;
	   pvar->c23 = c23;
	   SINCOS (j[J3], &(pvar->s[J3]), &(pvar->c[J3]));
	   pvar->s[J4] = s4;
	   pvar->c[J4] = c4;
	   pvar->s[J5] = s5;
	   pvar->c[J5] = c5;
	   pvar->s[J6] = s6;
	   pvar->c[J6] = c6;
	 }
}

float epsilon5 = 5.0*DEGTORAD;

#if SINGTEST

#define SAFE	   1
#define ENTERING   2
#define EXITING    3
#define SINGULAR   4

pumaSingTolSetup(enable, r1, r2, sr, pvar)
int enable;
float r1, r2, sr;
PUMA_VAR *pvar;
{
	pvar->singenable = enable;
	if (enable)
	 { if (r1 != F_UNDEF)
	    { pvar->region1 = r1;
	    }
	   if (r2 != F_UNDEF)
	    { pvar->region2 = r2;
	    }
	   if (sr != F_UNDEF)
	    { pvar->slopRange = sr;
	    }
	 }
}

#endif

pumaSingTolEnable(j, pvar)
float *j;
PUMA_VAR *pvar;
{
#if SINGTEST
	/* Called to enable the wrist singularity tolerance algorithm
	   built into pumaInvKinematics */

	float s5;
	float mags5;

	s5 = SIN (j[J5]);
	mags5 = FABS(s5);

	if (mags5 < pvar->region2)
	 { pvar->singstate = SINGULAR;
	   pvar->theta4 = j[J4];
	 }
	else if (mags5 < pvar->region1)
	 { pvar->singstate = EXITING;
	   pvar->theta4 = j[J4];
	 }
	else
	 { pvar->singstate = SAFE;
	 }
#endif
}

pumaInvKinematics(j, t, conf, refj, kyn, jls, var) /*::*/
float *j;	  	  /* joints in range coordinates + configuration */
REGISTER TRSF *t;	  /* input transform 			  	 */
unsigned long conf;	  /* configuration information			 */
float *refj;		  /* reference joints				 */
KYN *kyn;		  /* kinematic parameters			 */
void *var;		  /* run-time variables 			 */
JLS *jls;		  /* joint level data -- usd to get min. joints	 */
{
	register PUMA_KYN *pkyn = (PUMA_KYN*)kyn->ext;
	register PUMA_VAR *pvar = (PUMA_VAR*)var;
	register GEN_KYN *gen = kyn->gen;

	int code = 0;

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

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

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

	float ang[PUMAJ];			/* non-ranged angle values */

	double sqrt_arg;			/* argument to square root */

	refj4 =	(refj ? refj[J4] : (jls->jmax[J4] + jls->jmin[J4])/2);

	/* theta1 */

	if (pkyn->oldCoordDefs)
	 { 
	   sqrt_arg = t->p.x * t->p.x + t->p.y * t->p.y - D32;
	   if (sqrt_arg < EPSILON_D32)
	    { if (sqrt_arg < -EPSILON_D32)
	       { return (KYN_CANT_REACH);		/* too far away */
	       }
	      else
	       { sqrt_arg = 0.0;
	       }
	    }
	   ang[J1] = - ATAN2(t->p.y, -t->p.x) + 
		     ATAN2(D3, (conf & PUMA_RIGHT_CONF) ? SQRT(sqrt_arg) :
							  -SQRT(sqrt_arg));
	   SINCOS (ang[J1], &s1, &c1);
   
	   /* theta2 */
   
	   r1 = c1 * t->p.x + s1 * t->p.y;
	   r2 = t->p.z * t->p.z + r1 * r1;
   
	   r3 = (A22_A32D42 + r2) / ( 2 * A2 );
	   sqrt_arg = r2 - r3 * r3;
   
	   if (sqrt_arg < EPSILON_A2D4)
	    { if (sqrt_arg < -EPSILON_A2D4)
	       { return (KYN_CANT_REACH);		/* too close in */
	       }
	      else
	       { sqrt_arg = 0.0;
	       }
	    }
	   ang[J2] = ATAN2 (-r3, (conf & PUMA_DOWN_CONF) ? SQRT(sqrt_arg) : 
							 -SQRT(sqrt_arg)) -
		     ATAN2 (-r1, t->p.z);
	   SINCOS (ang[J2], &s2, &c2);
   
	   /* theta3 */
   
	   r2 = - t->p.z * s2 + c2 * r1 - A2;
	   r3 = - c2 * t->p.z - r1 * s2;
	   ang[J3] = ATAN2 (D4*r2 + A3*r3, A3*r2 - D4*r3);
	   SINCOS (ang[J3], &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 SINGTEST
	   if (pvar && pvar->singenable)
	    { 
	      float mags5, s;
      
	      mags5 = SQRT (r1*r1 + r3*r3);
      
	      if (mags5 < pvar->region2)
	       {
		  pvar->singstate = SINGULAR;
		  ang[J4] = refj4;
		  code |= PUMA_WRIST_DEG;
	       }
	      else if (mags5 < pvar->region1)
	       {
		  if( pvar->singstate == SAFE)
		  {
		     pvar->singstate = ENTERING;
		     pvar->theta4 = refj4;
		  } else if( pvar->singstate == SINGULAR)
		  {
		     pvar->singstate = EXITING;
		     pvar->theta4 = refj4;
		  }
      
		  /* s = 0 at singularity region, 1 at free region */
		  s = (mags5 - pvar->region2)/(pvar->region1 - pvar->region2);
		  ang[J4] = (conf & PUMA_FLIP_CONF) ? ATAN2(r1,  r3) : 
						      ATAN2(-r1, -r3);
		  if( pvar->singstate == ENTERING)
		  {
		     ang[J4] = s * ang[J4] +
			(1.0-s)*( (pvar->slopRange/PIB2)
				      * atan( (PIB2/pvar->slopRange) *(ang[J4]-pvar->theta4))
				      + pvar->theta4);
		  } else /* pvar->singstate == EXITING */
		  {
		     ang[J4] = s * ang[J4] + (1.0-s) * pvar->theta4;
		  }
	       }
	      else
	       {  ang[J4] = (conf & PUMA_FLIP_CONF) ? ATAN2(r1,  r3) : 
						     ATAN2(-r1, -r3);
		  pvar->singstate = SAFE;
	       }
	    }
	   else
#endif
	    { 
	      if (FABS (r1) < epsilon5 && FABS (r3) < epsilon5)
	       { ang[J4] = refj4;
		 code |= PUMA_WRIST_DEG;
	       }
	      else
	       { ang[J4] = (conf & PUMA_FLIP_CONF) ? ATAN2(r1,  r3) : 
						     ATAN2(-r1, -r3);
	       }
	    }

	   SINCOS (ang[J4], &s4, &c4);
   
	   /* theta5 */
   
	   s5 = c4 * r3 + s4 * r1 ;
	   c5 = s23 * r2 + c23 * t->a.z;
	   ang[J5] = 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[J6] = ATAN2(s6, c6);
	 }
	else
	 { 
	   sqrt_arg = t->p.x * t->p.x + t->p.y * t->p.y - D32;
	   if (sqrt_arg < EPSILON_D32)
	    { if (sqrt_arg < -EPSILON_D32)
	       { return (KYN_CANT_REACH);		/* too far away */
	       }
	      else
	       { sqrt_arg = 0.0;
	       }
	    }
	   ang[J1] = ATAN2(t->p.y, t->p.x) + 
		     ATAN2(D3, (conf & PUMA_RIGHT_CONF) ? SQRT(sqrt_arg) :
							 -SQRT(sqrt_arg));
	   SINCOS (ang[J1], &s1, &c1);
   
	   /* theta2 */
   
	   r1 = c1 * t->p.x + s1 * t->p.y;
	   r2 = t->p.z * t->p.z + r1 * r1;
   
	   r3 = (A22_A32D42 + r2) / ( 2 * A2 );
	   sqrt_arg = r2 - r3 * r3;
   
	   if (sqrt_arg < EPSILON_A2D4)
	    { if (sqrt_arg < -EPSILON_A2D4)
	       { return (KYN_CANT_REACH);		/* too close in */
	       }
	      else
	       { sqrt_arg = 0.0;
	       }
	    }
	   ang[J2] = ATAN2 (r3, (conf & PUMA_DOWN_CONF) ? SQRT(sqrt_arg) : 
							 -SQRT(sqrt_arg)) -
		     ATAN2 (r1, t->p.z);
	   SINCOS (ang[J2], &s2, &c2);
   
	   /* theta3 */
   
	   r2 = t->p.z * s2 + c2 * r1 - A2;
	   r3 = c2 * t->p.z - r1 * s2;
	   ang[J3] = ATAN2 (-D4*r2 + A3*r3, A3*r2 + D4*r3);
	   SINCOS (ang[J3], &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 SINGTEST
	   if (pvar && pvar->singenable)
	    { 
	      float mags5, s;
      
	      mags5 = SQRT (r1*r1 + r3*r3);
      
	      if (mags5 < pvar->region2)
	       {
		  pvar->singstate = SINGULAR;
		  ang[J4] =  refj4;
		  code |= PUMA_WRIST_DEG;
	       }
	      else if (mags5 < pvar->region1)
	       {
		  if( pvar->singstate == SAFE)
		  {
		     pvar->singstate = ENTERING;
		     pvar->theta4 = refj4;
		  } else if( pvar->singstate == SINGULAR)
		  {
		     pvar->singstate = EXITING;
		     pvar->theta4 = refj4;
		  }
      
		  /* s = 0 at singularity region, 1 at free region */
		  s = (mags5 - pvar->region2)/(pvar->region1 - pvar->region2);
		  ang[J4] = (conf & PUMA_FLIP_CONF) ? ATAN2(r1,  r3) : 
						      ATAN2(-r1, -r3);
		  if( pvar->singstate == ENTERING)
		  {
		     ang[J4] = s * ang[J4] +
			(1.0-s)*( (pvar->slopRange/PIB2)
				      * atan( (PIB2/pvar->slopRange) *(ang[J4]-pvar->theta4))
				      + pvar->theta4);
		  } else /* pvar->singstate == EXITING */
		  {
		     ang[J4] = s * ang[J4] + (1.0-s) * pvar->theta4;
		  }
	       }
	      else
	       {  ang[J4] = (conf & PUMA_FLIP_CONF) ? ATAN2(r1,  r3) : 
						      ATAN2(-r1, -r3);
		  pvar->singstate = SAFE;
	       }
	    }
	   else
#endif
	    { 
	      if (FABS (r1) < EPSILONF && FABS (r3) < EPSILONF)
	       { ang[J4] = refj4;
		 code |= PUMA_WRIST_DEG;
	       }
	      else
	       { ang[J4] = (conf & PUMA_FLIP_CONF) ? ATAN2(r1,  r3) : 
						     ATAN2(-r1, -r3);
	       }
	    }

	   SINCOS (ang[J4], &s4, &c4);
   
	   /* theta5 */
   
	   s5 = - c4 * r3 - s4 * r1 ;
	   c5 = - s23 * r2 + c23 * t->a.z;
	   ang[J5] = 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[J6] = ATAN2(s6, c6);
	 }

	code |= pumaRangeAngles (j, ang, refj, kyn, jls);

	if (pvar != NULL)	/* Update sines and cosines if needed */
	 { pvar->s[J1] = s1;
	   pvar->c[J1] = c1;
	   pvar->s[J2] = s2;
	   pvar->c[J2] = c2;
	   pvar->s23 = s23;
	   pvar->c23 = c23;
	   pvar->s[J3] = s3;
	   pvar->c[J3] = c3;
	   pvar->s[J4] = s4;
	   pvar->c[J4] = c4;
	   pvar->s[J5] = s5;
	   pvar->c[J5] = c5;
	   pvar->s[J6] = s6;
	   pvar->c[J6] = c6;
	 }
	return(code);
}

pumaRangeAngles (r, a, refj, kyn, jls)
register float *r, *a;
float *refj;
KYN *kyn;
JLS *jls;
{
	register float *jmin, *jmax;
	register float angle;
	register float refAngle;
	int code = 0;

	jmin = jls->jmin;
	jmax = jls->jmax;

	/* Range of joint 1 < 360 degrees */

	if ((angle = a[0]) <= jmin[0])
	 { do
	    { angle += 2*PI;
	    }
	   while (angle <= jmin[0]);
	   if (angle >= jmax[0])
	    { code |= (1 << 0);
	    }
	 }
	else if (angle >= jmax[0])
	 { do
	    { angle -= 2*PI;
	    }
	   while (angle >= jmax[0]);
	   if (angle <= jmin[0])
	    { code |= (1 << 0);
	    }
	 }
	r[0] = angle;

	/* Range of joint 2 < 360 degrees */

	if ((angle = a[1]) <= jmin[1])
	 { do
	    { angle += 2*PI;
	    }
	   while (angle <= jmin[1]);
	   if (angle >= jmax[1])
	    { code |= (1 << 1);
	    }
	 }
	else if (angle >= jmax[1])
	 { do
	    { angle -= 2*PI;
	    }
	   while (angle >= jmax[1]);
	   if (angle <= jmin[1])
	    { code |= (1 << 1);
	    }
	 }
	r[1] = angle;

	/* Range of joint 3 < 360 degrees */

	if ((angle = a[2]) <= jmin[2])
	 { do
	    { angle += 2*PI;
	    }
	   while (angle <= jmin[2]);
	   if (angle >= jmax[2])
	    { code |= (1 << 2);
	    }
	 }
	else if (angle >= jmax[2])
	 { do
	    { angle -= 2*PI;
	    }
	   while (angle >= jmax[2]);
	   if (angle <= jmin[2])
	    { code |= (1 << 2);
	    }
	 }
	r[2] = angle;

	/* Range of joint 4 is potentially unlimited, so we need to
	   use reference angles */

	angle = a[3];
	refAngle = (refj ? refj[3] : (jmax[3] + jmin[3])/2);
	if (angle <= refAngle)
	 { while (angle <= refAngle)	
	    { angle += PIT2;
	    }
	   if (angle - refAngle <= PI && angle < jmax[3])
	    { r[3] = angle;		
	    }
	   else
	    { angle -= PIT2;
	      if ((r[3] = angle) <= jmin[3])
	       { code |= (1 << 3);
	       }
	    }
	 }
	else
	 { while (angle > refAngle)
	    { angle -= PIT2;
	    }
	   if (refAngle - angle <= PI && angle > jmin[3])
	    { r[3] = angle;
	    }
	   else
	    { angle += PIT2;
	      if ((r[3] = angle) >= jmax[3])
	       { code |= (1 << 3);
	       }
	    }
	 }

	/* Range of joint 5 < 360 degrees */

	if ((angle = a[4]) <= jmin[4])
	 { do
	    { angle += 2*PI;
	    }
	   while (angle <= jmin[4]);
	   if (angle >= jmax[4])
	    { code |= (1 << 4);
	    }
	 }
	else if (angle >= jmax[4])
	 { do
	    { angle -= 2*PI;
	    }
	   while (angle >= jmax[4]);
	   if (angle <= jmin[4])
	    { code |= (1 << 4);
	    }
	 }
	r[4] = angle;

	/* Range of joint 6 is potentially unlimited, so we need to
	   use reference angles */

	angle = a[5];
	refAngle = (refj ? refj[5] : (jmax[5] + jmin[5])/2);
	if (angle <= refAngle)
	 { while (angle <= refAngle)	
	    { angle += PIT2;
	    }
	   if (angle - refAngle <= PI && angle < jmax[5])
	    { r[5] = angle;		
	    }
	   else
	    { angle -= PIT2;
	      if ((r[5] = angle) <= jmin[5])
	       { code |= (1 << 5);
	       }
	    }
	 }
	else
	 { while (angle > refAngle)
	    { angle -= PIT2;
	    }
	   if (refAngle - angle <= PI && angle > jmin[5])
	    { r[5] = angle;
	    }
	   else
	    { angle += PIT2;
	      if ((r[5] = angle) >= jmax[5])
	       { code |= (1 << 5);
	       }
	    }
	 }
	return (code);
}

pumaSolveConf(conf, j, kyn)
unsigned long *conf;	/* configuration description */
register float *j;	/* joints in radians */
KYN *kyn;
{
	register PUMA_KYN *pkyn = (PUMA_KYN*)kyn->ext;
	register GEN_KYN *gen = kyn->gen;

	float s23, c23, c2;	/* sines, cosines, and constants */
	float h1, phi;

	SINCOS (j[J2] + j[J3], &s23, &c23);
	c2 = COS (j[J2]);

	*conf = 0;

	if (pkyn->oldCoordDefs)
	 { /* Righty - Lefty */

	   h1 = D4 * s23 + A2 * c2 + c23 * A3;
	   if (h1 < 0)
	    { *conf |= PUMA_RIGHT_CONF;
	    }
	   /* Up - Down */
   
	   phi = range(j[J3] + ATANA3D4);
	   if (phi < PIB2 || phi > PI + PIB2)
	    { *conf |= PUMA_DOWN_CONF;
	    }
   
	   /* Flip - Noflip */
   
	   if (range(j[J5]) <= PI)
	    { *conf |= PUMA_FLIP_CONF;
	    }
	 }
	else
	 { /* Righty - Lefty */

	   h1 = - D4 * s23 + A2 * c2 + c23 * A3;
	   if (h1 >= 0)
	    { *conf |= PUMA_RIGHT_CONF;
	    }
	   /* Up - Down */
   
	   phi = range(j[J3] - ATANA3D4);
	   if (phi < PIB2 || phi > PI + PIB2)
	    { *conf |= PUMA_DOWN_CONF;
	    }
   
	   /* Flip - Noflip */
   
	   if (range(j[J5]) > PI)
	    { *conf |= PUMA_FLIP_CONF;
	    }
	 }
}
