/*-=-=-=-=-=-=-=-=-=-=-=- 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:	stanKinematic.c
 *
 * DESCRIPTION:	Forward and inverse kinematic routines for the STAN robots.
 * 
 * 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/stan_jls.h"
#include "../h/stan_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 D2	gen->dh[1].d		/* basic A matrix parameters */

#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

stanFwdKinematics (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 STAN_VAR *svar = (STAN_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	/* sines and cosines */

	float c1, s1, c2, s2, c4, s4, c5, s5, c6, s6;

 	/* temporaries */

	float k3, k4, k5, k6;

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

	/* Use reg sin/cos for J1 since tests indicate we need the
	   extra precision. */
	s1 = sin(j[J1]);
	c1 = cos(j[J1]);
	SINCOS (j[J2], &s2, &c2);
	SINCOS (j[J4], &s4, &c4);
	SINCOS (j[J5], &s5, &c5);
	SINCOS (j[J6], &s6, &c6);

	k3 = c4*c5*s6 + s4*c6;
	k4 = -c2*k3 + s2*s5*s6;
	k5 = -s4*c5*s6 + c4*c6;
	k6 = c2*c4*s5 + s2*c5;

	t->o.x = c1*k4 - s1*k5;
	t->o.y = s1*k4 + c1*k5;
	t->o.z = s2*k3 + c2*s5*s6;

	t->a.x = c1*k6 - s1*s4*s5;
	t->a.y = s1*k6 + c1*s4*s5;
	t->a.z = -s2*c4*s5 + c2*c5;

	t->p.x = c1*s2*j[J3] - s1*D2;
	t->p.y = s1*s2*j[J3] + c1*D2;
	t->p.z = c2*j[J3];

	/* 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 (j[J3]*SIN(j[J2]) < 0)   
	    { *conf |= STAN_RIGHT_CONF;
	    }
      
	   /* down - up */
	   if (j[J3] < 0.0)
	    { *conf |= STAN_DOWN_CONF;
	    }
     
	   /*  Flip - Noflip  */
	   if (range(j[J5]) > PI)
	    { *conf |= STAN_FLIP_CONF;
	    }
	 }

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

static float epsilon5 = 5.0*DEGTORAD;

stanInvKinematics(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	 */
{
	STAN_KYN *skyn = (STAN_KYN*)kyn->ext;
	register STAN_VAR *svar = (STAN_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	int code = 0;

	register float acx, acy, ocy, ocx, ka1, ko1;

	double px2py2, k11, sqrt_arg;
	float epsilon;

	float s1, c1, s2, c2, s4, c4, s5, c5, s6, c6;
	float ang[STANJ];			/* non-ranged angle values */

	/* theta 1 */

	px2py2 = t->p.x*t->p.x + t->p.y*t->p.y;
	epsilon = skyn->d3Rng*EPSILONF;
	sqrt_arg = px2py2 - D2*D2;
	if (sqrt_arg < epsilon)
	 { if (sqrt_arg < -epsilon)
	    { return (KYN_CANT_REACH);		/* too far away */
	    }
	   else
	    { sqrt_arg = 0.0;
	    }
	 }
	ang[J1] = 2*ATAN2(-t->p.x + ((conf&STAN_RIGHT_CONF)? -SQRT(sqrt_arg):
							      SQRT(sqrt_arg)),
			  D2+t->p.y);
	SINCOS (ang[J1], &s1, &c1);

	/* joints 2 and 3 */

	k11 = t->p.y*s1 + c1*t->p.x;
	sqrt_arg = k11*k11 + t->p.z*t->p.z;
	if (sqrt_arg < epsilon)
	 { /* this argument can't really be less than 0, so we just
	      set it to 0.0 */
	   ang[J2] = (refj ? refj[J2] : (jls->jmin[J2]+jls->jmax[J2])/2);
	   ang[J3] = 0.0;
	 }
	else
	 { if (conf & STAN_DOWN_CONF)
	    { ang[J2] = ATAN2 (-k11, -t->p.z);
	      ang[J3] = -SQRT(sqrt_arg);
	    }
	   else
	    { ang[J2] = ATAN2 (k11, t->p.z);
	      ang[J3] = SQRT(sqrt_arg);
	    }
	 }
	SINCOS (ang[J2], &s2, &c2);

	/* theta4 */

	ka1 = t->a.y*s1 + t->a.x*c1;
	acx = c2*ka1 - t->a.z*s2;
	acy = t->a.y*c1 - t->a.x*s1;

	if (FABS (acx) < EPSILONF && FABS (acy) < EPSILONF)
	 { ang[J4] = (refj ? refj[J4] : (jls->jmin[J4]+jls->jmax[J4])/2);
	   code |= STAN_WRIST_DEG;
	 }
	else
	 { ang[J4] = (conf & STAN_FLIP_CONF) ? ATAN2(-acy, -acx) : 
					       ATAN2(acy, acx);
	 }
	SINCOS (ang[J4], &s4, &c4);

	/* theta5 */

	s5 = s4*acy + c4*acx;
	c5 = ka1*s2 + t->a.z*c2;
	ang[J5] = ATAN2(s5, c5);

	/* theta6 */

	ko1 = t->o.y*s1 + t->o.x*c1;
	ocx = c2*ko1 - s2*t->o.z;
	ocy = t->o.y*c1 - t->o.x*s1;

	s6 = s5*(s2*ko1 + c2*t->o.z) - c5*(ocy*s4 + ocx*c4);
	c6 = c4*ocy - ocx*s4;
	ang[J6] = ATAN2(s6, c6);

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

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

stanRangeAngles (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;

	/* Joint 3 is prismatic */
	r[2] = a[2];

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

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

	*conf = 0;

	/* Righty - Lefty */

	if (j[J3]*SIN(j[J2]) < 0)
	 { *conf |= STAN_RIGHT_CONF;
	 }
 
	/* down - up */
	if (j[J3] < 0.0)
	 { *conf |= STAN_DOWN_CONF;
	 }
  
	/*  Flip - Noflip  */

	if (range(j[J5]) > PI)
	 { *conf |= STAN_FLIP_CONF;
	 }
}
