/*-=-=-=-=-=-=-=-=-=-=-=- 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---------
 *		
 * NAME:	pumaSelect()
 *
 * USAGE:	RCCL Library routine
 *		
 *		#include <puma_kynvar.h>
 *
 *		joint_code = pumaSelect (dirs, tool, kyn, var)
 *		  int joint_code, dirs;
 *		  TRSF *tool;
 *		  PUMA_KYN *kyn;
 *		  PUMA_VAR *var;
 *
 * DESCRIPTION: Selects the joint(s) most likely to provide for motion along or
 * 		around principal directions of t6; assumes that 1 2 3 will do
 *		for positions and 4 5 6 for rotations. Returns joints 
 *		selection, one per direction.
 *
 *		If there is a tool transform involved, it should be passed to
 *		the routine via "tool". The direction requirements are passed
 *		to the routine by setting bits in the variable "dirs", 
 *		according to the following code:
 *		
 *			along X		0x01
 *			along Y		0x02
 *			along Z		0x04
 *			around X	0x08
 *			around Y	0x10
 *			around Z	0x20
 *		
 *		The return code indicates joints 1-6 as bits 0-5.
 *		
 *		The variable var is a data structure containing the jacobian
 *		coefficients for the present joint configuration, the sines and
 *		cosines of the joint angles, and other such information. These
 *		values should be set prior to enteriong the routine, using
 *		the functions pumaUdateVar(), or equivalent.
 *
 *		The variable 'kyn' is a structure that contains the kinematic 
 *		constants for the robot.
 *
 * GLOBAL VARS:	None.
 *
 * FILES:
 *
 * AUTHOR:	Vincent Hayward, Purdue University, 1983.
 *
 * UPDATES:	CVaRL installation and modification by John Lloyd.
 *
 * NOTES:	Perform a simple minded joint selection algorithm:
 *              The method is to examine the elements of the jacobian 3 X 3 
 *		upper left matrix for translations and the 3 X 3 lower right
 *		matrix for rotations. The jacobian is examined in link4, with
 *		U5 performing the mapping between link4 and t6.
 *
 *-------------McGill-Computer-Vision-and-Robotics-Laboratory--------------*/

#include "../h/cdefs.h"
#include "../h/puma_kynvar.h"
#include "../h/robotTypes.h"

#define	SELFX	0x01
#define	SELFY	0x02
#define	SELFZ	0x04
#define	SELMX	0x08
#define	SELMY	0x10
#define	SELMZ	0x20

#define	SELJ1	0x01
#define	SELJ2	0x02
#define	SELJ3	0x04
#define	SELJ4	0x08
#define	SELJ5	0x10
#define	SELJ6	0x20

pumaSelect (dirs, tool, kyn, var) /*::*/
register int dirs;
register TRSF *tool;
register PUMA_KYN *kyn;
register PUMA_VAR *var;
{
	register int best = 0;

	if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFY | SELFZ)) {
		best |= SELJ1 | SELJ2 | SELJ3;
	}
	else {
		float ct1x, ct1y, ct1z, ct2x, ct2y, ct2z, ct3x, ct3y, ct3z;
		TRSF tot4;

		if (tool != NULL) {
			multTrsf(&tot4, &var->u5, tool);
		}
		else {
			copyTrsf(&tot4, &var->u5);
		}

		if (dirs & SELFX) {
			ct1x = tot4.n.x * var->j11 +
			       tot4.n.y * var->j21 +
			       tot4.n.z * var->j31;
			ct1x = FABS(ct1x);

			ct2x = tot4.n.x * var->j12 +
			       tot4.n.y * var->j22 +
			       tot4.n.z * var->j32;

			ct3x = tot4.n.x * var->j13 +
			       tot4.n.z * var->j33;

			ct2x = ct3x + ct2x;
			ct2x = FABS(ct2x);

			ct3x = FABS(ct3x);
		}
		if (dirs & SELFY) {
			ct1y = tot4.o.x * var->j11 +
			       tot4.o.y * var->j21 +
			       tot4.o.z * var->j31;
			ct1y = FABS(ct1y);

			ct2y = tot4.o.x * var->j12 +
			       tot4.o.y * var->j22 +
			       tot4.o.z * var->j32;

			ct3y = tot4.o.x * var->j13 +
			       tot4.o.z * var->j33;

			ct2y = ct2y + ct3y;
			ct2y = FABS(ct2y);
			ct3y = FABS(ct3y);
		}
		if (dirs & SELFZ) {
			ct1z = tot4.a.x * var->j11 +
			       tot4.a.y * var->j21;
			ct1z = FABS(ct1z);

			ct2z = tot4.a.x * var->j12 +
			       tot4.a.y * var->j22;

			ct3z = tot4.a.x * var->j13;

			ct2z = ct2z + ct3z;
			ct2z = FABS(ct2z);

			ct3z = FABS(ct3z);
		}

		if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFY)) {
			if ((ct1x > ct2x) && (ct1x > ct3x)) {
				best |= SELJ1;
				if (ct2y > ct3y) {
					best |= SELJ2;
				}
				else {
					best |= SELJ3;
				}
			}
			else {
				if (ct2x > ct3x) {
					best |= SELJ2;
					if (ct1y > ct3y) {
						best |= SELJ1;
					}
					else {
						best |= SELJ3;
					}
				}
				else {
					best |= SELJ3;
					if (ct1y > ct2y) {
						best |= SELJ1;
					}
					else {
						best |= SELJ2;
					}
				}
			}
		}
		if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFX | SELFZ)) {
			if ((ct1x > ct2x) && (ct1x > ct3x)) {
				best |= SELJ1;
				if (ct2z > ct3z) {
					best |= SELJ2;
				}
				else {
					best |= SELJ3;
				}
			}
			else {
				if (ct2x > ct3x) {
					best |= SELJ2;
					if (ct1z > ct3z) {
						best |= SELJ1;
					}
					else {
						best |= SELJ3;
					}
				}
				else {
					best |= SELJ3;
					if (ct1z > ct2z) {
						best |= SELJ1;
					}
					else {
						best |= SELJ2;
					}
				}
			}
		}
		if ((dirs & (SELFX | SELFY | SELFZ)) == (SELFY | SELFZ)) {
			if ((ct1y > ct2y) && (ct1y > ct3y)) {
				best |= SELJ1;
				if (ct2z > ct3z) {
					best |= SELJ2;
				}
				else {
					best |= SELJ3;
				}
			}
			else {
				if (ct2y > ct3y) {
					best |= SELJ2;
					if (ct1z > ct3z) {
						best |= SELJ1;
					}
					else {
						best |= SELJ3;
					}
				}
				else {
					best |= SELJ3;
					if (ct1z > ct2z) {
						best |= SELJ1;
					}
					else {
						best |= SELJ2;
					}
				}
			}
		}
		if ((dirs & (SELFX | SELFY | SELFZ)) == SELFX) {
			if ((ct1x > ct2x) && (ct1x > ct3x)) {
				best |= SELJ1;
			}
			else {
				if (ct2x > ct3x) {
					best |= SELJ2;
				}
				else {
					best |= SELJ3;
				}
			}
		}
		if ((dirs & (SELFX | SELFY | SELFZ)) == SELFY) {
			if ((ct1y > ct2y) && (ct1y > ct3y)) {
				best |= SELJ1;
			}
			else {
				if (ct2y > ct3y) {
					best |= SELJ2;
				}
				else {
					best |= SELJ3;
				}
			}
		}
		if ((dirs & (SELFX | SELFY | SELFZ)) == SELFZ) {
			if ((ct1z > ct2z) && (ct1z > ct3z)) {
				best |= SELJ1;
			}
			else {
				if (ct2z > ct3z) {
					best |= SELJ2;
				}
				else {
					best |= SELJ3;
				}
			}
		}
	}
	if ((dirs & (SELMX | SELMY | SELMZ)) == (SELMX | SELMY | SELMZ)) {
		best |= SELJ4 | SELJ5 | SELJ6;
	}
	else {
		if (dirs & SELMX) {
			best |= SELJ5;
		}
		if (dirs & SELMY) {
			best |= SELJ4;
		}
		if (dirs & SELMZ) {
			best |= SELJ6;
		}
	}
	return(best);
}
