/*-=-=-=-=-=-=-=-=-=-=-=- 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.
 *
 *		scaraSingularityDetect (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.
 *
 *		scaraSingularitySet (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.
 *
 *		scaraSingularityRemove (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/scara_kynvar.h"

/* ---- defines for 'kyn' constants, to make code easier to read ---- */

#define A1		skyn->a1		/* basic A matrix parameters */
#define A2		skyn->a2

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

scaraSingularityDetect (angles, distance, kyn)
float *angles;
float distance;
KYN *kyn;
{
	SCARA_KYN *skyn = (SCARA_KYN*)kyn->ext;

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

	distance = FABS(distance);

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

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

	return (code);
}

scaraSingularitySet (outangles, inangles, distance, mask, kyn, jls)
float outangles[];
float inangles[];
float distance;
unsigned long mask;
KYN *kyn;
JLS *jls;
{
	SCARA_KYN *skyn = (SCARA_KYN*)kyn->ext;

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

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

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

	if (mask & SCARA_ELBOW_DEG)
	 { outangles[1] = distance;
	 }
}

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[SCARAJ];
	float result[SCARAJ];
	float output[SCARAJ];
	int mask, j;

	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);
	   for (j=0; j<SCARAJ; j++)
	    { if (j!=2)
	       { angles[j] *= DEGTORAD;
	       }
	    }
	   scanf ("%x", &mask);
	   while (getchar() != '\n')
	      ;

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

	   for (j=0; j<SCARAJ; j++)
	    { if (j==2)
	       { output[j] = result[j];
	       }
	      else
	       { output[j] = result[j]*RADTODEG;
	       }
	    }
	   printVf ("%10.4f\n", output, 6);

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

#endif
