/*-=-=-=-=-=-=-=-=-=-=-=- 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:	scaraKinematic.c
 *
 * DESCRIPTION:	Forward and inverse kinematic routines for the SCARA 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/scara_jls.h"
#include "../h/scara_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 A1	gen->dh[0].a		/* basic A matrix parameters */
#define A2	gen->dh[1].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

scaraFwdKinematics (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 */
{
	SCARA_VAR *svar = (SCARA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	/* sines and cosines */

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

 	/* temporaries */

	float k1, k2;

	/* 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[J1] + j[J2], &s12, &c12);
	SINCOS (j[J4], &s4, &c4);
	SINCOS (j[J5], &s5, &c5);
	SINCOS (j[J6], &s6, &c6);

	k1 = c4*c5*s6 + c6*s4;
	k2 = c4*c6 - c5*s4*s6;

	t->o.x = -c12*k1 - s12*k2;
	t->o.y =  c12*k2 - s12*k1;
	t->o.z = -s5*s6;

	t->a.x =  (s12*s4 - c12*c4)*s5;
	t->a.y = -(c12*s4 + s12*c4)*s5;
	t->a.z = c5;

	t->p.x = A2*c12 + A1*c1;
	t->p.y = A2*s12 + A1*s1;
	t->p.z = 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 (range(j[J2]) > PI)
	    { *conf |= SCARA_RIGHT_CONF;
	    }
      
	   /*  Flip - Noflip  */
   
	   if (range(j[J5]) > PI)
	    { *conf |= SCARA_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->s12 = s12;
	   svar->c12 = c12;
	   svar->s[J4] = s4;
	   svar->c[J4] = c4;
	   svar->s[J5] = s5;
	   svar->c[J5] = c5;
	   svar->s[J6] = s6;
	   svar->c[J6] = c6;
	 }
}

static float epsilon5 = 5.0*DEGTORAD;

scaraInvKinematics(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	 */
{
	SCARA_VAR *svar = (SCARA_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	int code = 0;

	register float acy, acx, ocy, ocx;

	double px2py2, k11, sqrt_arg;
	float epsilon2;
	float a22a32d42;

	float s1, c1, s2, c2, s12, c12, s3, c3, s4, c4, s5, c5, s6, c6;
	float ang[SCARAJ];			/* non-ranged angle values */

	/* theta 1 */

	px2py2 = t->p.x*t->p.x + t->p.y+t->p.y;
	k11 = (px2py2 - A2*A2 + A1*A1)/A1;
	epsilon2 = (A2+A1)*EPSILONF;

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

	/* theta2 */

	ang[J2] = ATAN2(c1*t->p.y - s1*t->p.x,
			s1*t->p.y + c1*t->p.x - A1);
	SINCOS (ang[J2], &s2, &c2);

	/* theta3 */

	ang[J3] = t->p.z;
	
	/* theta4 */

	acx = t->a.y*s12 + t->a.x*c12;
	acy = t->a.y*c12 - t->a.x*s12;

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

	SINCOS (ang[J4], &s4, &c4);

	/* theta5 */

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

	/* theta6 */

	ocx = t->o.y*s12 + t->o.x*c12;
	ocy = t->o.y*c12 - t->o.x*s12;

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

	code |= scaraRangeAngles (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->s12 = s12;
	   svar->c12 = c12;
	   svar->s[J3] = s3;
	   svar->c[J3] = c3;
	   svar->s[J4] = s4;
	   svar->c[J4] = c4;
	   svar->s[J5] = s5;
	   svar->c[J5] = c5;
	   svar->s[J6] = s6;
	   svar->c[J6] = c6;
	 }
	return(code);
}

scaraRangeAngles (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 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);
}

scaraSolveConf(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 (range(j[J2]) > PI)
	 { *conf |= SCARA_RIGHT_CONF;
	 }
   
	/*  Flip - Noflip  */

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