/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	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:	singularity.c
 *
 * DESCRIPTION:	Manipulator dependant functions that are used in testing
 *		kinematic code.
 *
 *		A NOTE, BEFORE THE DESCRIPTION, ABOUT UNITS:
 *
 *		   All rotational units are radians; all translational units
 *		   are millimters.
 *
 *		pumaSingularityDetect (angles, distance, kyn) is a function 
 *	 	   that 
 *		   returns a bit mask describing any singularities that the 
 *		   manipulator is at or near. The arm is defined to be near
 *		   a singularity when associated angle is at or within 
 *		   'distance' units of the singular position. 'Angles' is 
 *		   an array of 6 floats.
 *
 *		pumaSingularitySet (outangles, inangles, distance, mask, 
 *    			            kyn, jls) 
 *		   copies 'inangles' to 'outangles', except for making
 *		   the minimum changes necessary to ensure that the 
 *		   manipulator is 'distance' units away from the singularities
 *		   specified by the mask. If there is more than one choice
 *		   of angle to use to achieve the singularity, the one
 *		   closest to the original angle is used.
 *
 *		   This routine can be used to put the manipulator at a
 *		   singularity ('distance = 0'), or some arbitrary distance
 *		   away from it. 
 *
 *		   WARNING: The routine is not too smart: If 'distance' is
 *		   large enough to cause the manipulator to be at ANOTHER
 *		   sinularity, this will not be detected.
 *
 *		pumaSingularityRemove (outangles, inangles, distance, mask) 
 *		   takes a set of 'inangles' and copies them to 'outangles',
 *		   removing singularities the manipultor is within 'epsilon'
 *		   of by adding the angular amount 'delta' to the necessary 
 *		   angles. The angles are passed as arrays of 6 floats.
 *		   THIS ROUTINE IS NOT CURRENTLY WORKING CORRECTLY.
 * 
 * AUTHOR/DATE:	John Lloyd, Spring 87
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <math.h>

#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"
#include "../h/puma_kynvar.h"

/* ---- defines for 'kyn' constants, to make code easier to read ---- */
							
#define A2		kyn->gen->dh[1].a  /* basic A matrix parameters */
#define D3		kyn->gen->dh[2].d
#define D4		kyn->gen->dh[3].d
#define A3		kyn->gen->dh[2].a
#define ATANA3D4	pkyn->atana3d4	/* atan2(A3, D4) */

static float range180(a)
float a;
{
	while (a <= -PI)
	 { a += PIT2;
	 }
	while (a > PI)
	 { a -= PIT2;
	 }
	return ( (a > -PI) ? a : PI );
}

static float range360(a)
float a;
{
	while (a < 0.0)
	 { a += PIT2;
	 }
	while (a >= PIT2)
	 { a -= PIT2;
	 }
	return ( (a >= 0.0) ? a : 0.0 );
}

pumaSingularityDetect (angles, distance, kyn)
float *angles;
float distance;
KYN *kyn;
{
	register PUMA_KYN *pkyn = (PUMA_KYN*)kyn->ext;

	int code = 0;
	float phi, si3, co3;

	distance = FABS(distance);

	if (closeTo (angles[4], 0.0, distance))
	 { code |= PUMA_WRIST_DEG;
	 }

	if (pkyn->oldCoordDefs)
	 { if (closeTo (angles[2], -PIB2-ATANA3D4, distance) ||
	       closeTo (angles[2],  PIB2-ATANA3D4, distance))
	    { code |= PUMA_ELBOW_DEG;
	    }
	 }
	else
	 { if (closeTo (angles[2],  PIB2+ATANA3D4, distance) ||
	       closeTo (angles[2], -PIB2+ATANA3D4, distance))
	    { code |= PUMA_ELBOW_DEG;
	    }
	 }

	si3 = sin (angles[2]);
	co3 = cos (angles[2]);
	if (pkyn->oldCoordDefs)
	 { phi = atan2 (D4*si3 + A3*co3 + A2, A3*si3 - co3*D4);
	 }
	else
	 { phi = atan2 (-D4*si3 + A3*co3 + A2, A3*si3 + co3*D4);
	 }
	if (closeTo (angles[1], phi, distance) || 
	    closeTo (angles[1], phi+PI, distance))
	 { code |= PUMA_ALIGN_DEG;
	 }

	return (code);
}

pumaSingularitySet (outangles, inangles, distance, mask, kyn, jls)
float outangles[];
float inangles[];
float distance;
unsigned long mask;
KYN *kyn;
JLS *jls;
{
	register PUMA_KYN *pkyn = (PUMA_KYN*)kyn->ext;

	int j;
	float si3, co3, phi;
	float r2, rs1, rs2;

	for (j=0; j<PUMAJ; j++)
	 { outangles[j] = inangles[j];
	 }

	if (mask & PUMA_WRIST_DEG)
	 { outangles[4] = distance;
	 }

	if (mask & PUMA_ELBOW_DEG)
	 { if (pkyn->oldCoordDefs)
	    { outangles[2] = PIB2 - ATANA3D4 + distance;
	    }
	   else
	    { outangles[2] = -PIB2 + ATANA3D4 + distance;
	    }
	 }

	if (mask & PUMA_ALIGN_DEG)
	 { si3 = sin (outangles[2]);
	   co3 = cos (outangles[2]);
	   if (pkyn->oldCoordDefs)
	    { phi = atan2 (D4*si3 + A3*co3 + A2, A3*si3 - co3*D4);
	    }
	   else
	    { phi = atan2 (-D4*si3 + A3*co3 + A2, A3*si3 + co3*D4);
	    }

	   /* Map the angles into range coordinates so we can compare them */
	
#define    JMIN2  jls->jmin[1]
#define    JRNG2  jls->jrng[1]

	   r2 = range360 (outangles[1] - JMIN2);
	   rs1 = range360 (phi + distance - JMIN2);
	   rs2 = range360 (phi + distance + PI - JMIN2);
	   if ( (rs1 < JRNG2 && FABS(r2-rs1) <= FABS (r2-rs2)) ||
		(rs2 >= JRNG2) )
	    { outangles[1] = rs1 + JMIN2;
	    }
	   else
	    { outangles[1] = rs2 + JMIN2;
	    }
	 }
}

closeTo (angle, ref, delta)
float angle, ref, delta;
{
	/* Returns true if 'angle' is within 'ref' of 'delta' */

	float theta;

	theta = range180 (angle - ref);
	if (FABS(theta) <= FABS(delta))
	 { return (1);
	 }
	else
	 { return (0);
	 }
}

#ifdef TEST

main()
{
	float angles[PUMAJ];
	float result[PUMAJ];
	float output[PUMAJ];
	int mask, i;

	extern char* getDefaultRobot();
	KYN *kyn;
	JLS *jls;

	if ((jls = getJls(getDefaultRobot())) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	if ((kyn = getKyn(getDefaultRobot(), NULL)) == NULL)
	 { printErrors();
	   exit (-1);
	 }

	while (1)
	 { 
	   printf ("Enter angles and mask> ");
	   scanVf (angles, 6);
	   scaleVf (angles, DEGTORAD, angles, 6);
	   scanf ("%x", &mask);
	   while (getchar() != '\n')
	      ;

	   pumaSingularitySet (result, angles, DEGTORAD*.2, mask, kyn, jls);

	   scaleVf (output, RADTODEG, result, 6);
	   printVf ("%10.4f\n", output, 6);

	   printf ("Detection with espsilon .2: 0x%x\n",
		pumaSingularityDetect (result, DEGTORAD*(.2+.0001), kyn));
	 }
}

#endif
