/*-=-=-=-=-=-=-=-=-=-=-=- 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:	elbowKinematic.c
 *
 * DESCRIPTION:	Forward and inverse kinematic routines for the ELBOW robots.
 *		For description, see 'man elbowKinematics'
 * 
 * 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/elbow_jls.h"
#include "../h/elbow_kynvar.h"
#include "../h/robotTypes.h"

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

#define EPSILONF 2.0e-6

/* 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 A3	gen->dh[2].a
#define A4	gen->dh[3].a

#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

elbowFwdKinematics (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 ELBOW_VAR *evar = (ELBOW_VAR*)var;
	register GEN_KYN *gen = kyn->gen;

	/* sines and cosines */

	float c1, s1, c2, s2, c5, s5, c6, s6;
	float c23, s23, c234, s234;

 	/* temps */

	float h1;

	/* 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] + j[J3] + j[J2], &s234, &c234);
	SINCOS (j[J5], &s5, &c5);

	SINCOS (j[J6], &s6, &c6);

	/* and plug them into the equation for u1: */
   
	t->o.x = -(c1*c234*c5-s1*s5)*s6 - c1*c6*s234;
	t->o.y = -(c1*s5+c234*c5*s1)*s6 - c6*s1*s234;
	t->o.z = c234*c6-c5*s234*s6;
   
	t->a.x = c1*c234*s5 + c5*s1;
	t->a.y = c234*s1*s5 - c1*c5;
	t->a.z = s234*s5;
   
	h1 = A4*c234 + A3*c23 + A2*c2;

	t->p.x = h1*c1;
	t->p.y = h1*s1;
	t->p.z = A4*s234 + A3*s23 + A2*s2;

	/* 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 (h1 >= 0.0)
	    { *conf |= ELBOW_RIGHT_CONF;
	    }
   
	   /*  Up - Down  */
	   if (range(j[J3]) <= PI)
	    { *conf |= ELBOW_DOWN_CONF;
	    }
   
	   /*  Flip - Noflip  */
	   if (range(j[J5]) <= PI)
	    { *conf |= ELBOW_FLIP_CONF;
	    }
	 }

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

static float epsilon5 = 5.0*DEGTORAD;

elbowInvKinematics(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 ELBOW_VAR *evar = (ELBOW_VAR*)var;
	register GEN_KYN *gen = kyn->gen;

	int code = 0;

	register float r1, r2;
	double dx, dy, ka;
	float epsilon;

	float s1, c1, s2, c2, s3, c3, s234, c234, s4, c4, s5, c5, s6, c6;
	float theta234;

	float ang[ELBOWJ];			/* non-ranged angle values */
	double sqrt_arg;			/* argument to square root */

	/* theta 1 */

	ang[J1] = (conf & ELBOW_RIGHT_CONF) ? ATAN2( t->p.y,  t->p.x) :
					      ATAN2(-t->p.y, -t->p.x);
	SINCOS (ang[J1], &s1, &c1);
   
	/* theta 6 */
   
	r1 = s1*t->o.x - c1*t->o.y;
	r2 = c1*t->n.y - s1*t->n.x;

	if (FABS (r1) < epsilon5 && FABS (r2) < epsilon5)
	 { ang[J6] = (refj ? refj[J6] : (jls->jmax[J6] + jls->jmin[J6])/2);
	   code |= ELBOW_WRIST_DEG;
	 }
	else
	 { ang[J6] = (conf & ELBOW_FLIP_CONF) ? ATAN2( r1,  r2) : 
						ATAN2(-r1, -r2);
	 }
	SINCOS (ang[J6], &s6, &c6);
	
	/* theta 5 */

	ang[J5] = ATAN2(r1*s6+r2*c6, t->a.x*s1-t->a.y*c1);
	SINCOS (ang[J5], &s5, &c5);	

	theta234 = ATAN2(-c5*s6*t->o.z + s5*t->a.z + c5*c6*t->n.z,
			  s6*t->n.z + c6*t->o.z);
	SINCOS (theta234, &s234, &c234);

	dx = t->p.y*s1 + c1*t->p.x - A4*c234;
	dy = t->p.z - A4*s234;
	c3 = -(A2*A2 + A3*A3 - dx*dx - dy*dy)/(2*A2*A3);

	sqrt_arg = 1-c3*c3;
	if (sqrt_arg < EPSILONF)
	 { if (sqrt_arg < -EPSILONF)
	    { return (code | KYN_CANT_REACH);
	    }
	   else
	    { sqrt_arg = 0.0;
	    }
	 }
	r1 = (conf & ELBOW_DOWN_CONF) ? SQRT(sqrt_arg) : -SQRT(sqrt_arg);
	r2 = 1 + c3;
	if (FABS(r1) < EPSILONF && FABS(r2) < EPSILONF)
	 { ang[J3] = (refj ? refj[J3] : (jls->jmax[J3] + jls->jmin[J3])/2);
	 }
	else
	 { ang[J3] = 2*ATAN2(r1,r2);
	 }

	SINCOS(ang[J3], &s3, &c3);

	/* theta 2 */

	ka = A3*c3 + A2;
	ang[J2] = ATAN2(-A3*s3*dx + ka*dy, ka*dx + A3*s3*dy);
	SINCOS(ang[J2], &s2, &c2);

	/* theta 4 */

	ang[J4] = theta234 - ang[J3] - ang[J2];
	SINCOS(ang[J4], &s4, &c4);

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

	if (var != NULL)	/* Update sines and cosines if needed */
	 { evar->s[J1] = s1;
	   evar->c[J1] = c1;
	   evar->s[J2] = s2;
	   evar->c[J2] = c2;
	   evar->s[J3] = s3;
	   evar->c[J3] = c3;
	   evar->s[J4] = s4;
	   evar->c[J4] = c4;
	   evar->s234 = s234;
	   evar->c234 = c234;
	   SINCOS (j[J2]+j[J3], &(evar->s23), &(evar->c23));
	   SINCOS (j[J3]+j[J4], &(evar->s34), &(evar->c34));
	   evar->s[J5] = s5;
	   evar->c[J5] = c5;
	   evar->s[J6] = s6;
	   evar->c[J6] = c6;
	 }
	return(code);
}

elbowRangeAngles (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 < 360 degrees */

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

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

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

	float c234, c23, c2;	/* sines, cosines, and constants */

	c2 = COS (j[J2]);
	c23 = COS (j[J2] + j[J3]);
	c234 = COS (j[J2] + j[J3] + j[J4]);

	*conf = 0;

	 { /* Righty - Lefty */
	   if (A4*c234 + A3*c23 + A2*c2 >= 0.0)
	    { *conf |= ELBOW_RIGHT_CONF;
	    }
   
	   /*  Up - Down  */
	   if (range(j[J3]) <= PI)
	    { *conf |= ELBOW_DOWN_CONF;
	    }
   
	   /*  Flip - Noflip  */
	   if (range(j[J5]) <= PI)
	    { *conf |= ELBOW_FLIP_CONF;
	    }
	 }
}
