/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : select.c
 *      Remarks : Perform a simple minded joint selection algorithm for comply
 *                The method is to compare the elements of the jacobian
 *                3 X 3 upper left matrix for translations
 *                3 X 3 lower right ........  rotations
 *                The tool transform is passed as argument, NULL is passed if
 *                none, changes transformed into L4 coordinate.
 *                Need some more sophisticated method.
 *      Usage   : part of library
 */

#include "../h/rccl.h"
#include "../h/manip.h"
#include "../h/which.h"
#include "../h/kine.h"
#include "../h/umac.h"


/*
 * Selects the joint 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.
 * assumes coeff U5, jacob updated
 */

#ifdef PUMA

select_n(dirs, tool) /*::*/
register int dirs;
register TRSF_PTR tool;
{
	register int best = 0;

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

		if (tool != NULL) {
			Trmult(&tot4, &sncs_d.u5, tool);
		}
		else {
			Assigntr(&tot4, &sncs_d.u5);
		}

		if (dirs & SELFX) {
			ct1x = tot4.n.x * sncs_d.d1x +
			       tot4.n.y * sncs_d.d1y +
			       tot4.n.z * sncs_d.d1z;
			ct1x = FABS(ct1x);

			ct2x = tot4.n.x * sncs_d.d2x +
			       tot4.n.y * sncs_d.d2y +
			       tot4.n.z * sncs_d.d2z;

			ct3x = tot4.n.x * sncs_d.d3x +
			       tot4.n.y * sncs_d.d3y +
			       tot4.n.z * sncs_d.d3z;

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

			ct3x = FABS(ct3x);
		}
		if (dirs & SELFY) {
			ct1y = tot4.o.x * sncs_d.d1x +
			       tot4.o.y * sncs_d.d1y +
			       tot4.o.z * sncs_d.d1z;
			ct1y = FABS(ct1y);

			ct2y = tot4.o.x * sncs_d.d2x +
			       tot4.o.y * sncs_d.d2y +
			       tot4.o.z * sncs_d.d2z;

			ct3y = tot4.o.x * sncs_d.d3x +
			       tot4.o.y * sncs_d.d3y +
			       tot4.o.z * sncs_d.d3z;

			ct2y = ct2y + ct3y;
			ct2y = FABS(ct2y);

			ct3y = FABS(ct3y);
		}
		if (dirs & SELFZ) {
			ct1z = tot4.a.x * sncs_d.d1x +
			       tot4.a.y * sncs_d.d1y;
			ct1z = FABS(ct1z);

			ct2z = tot4.a.x * sncs_d.d2x +
			       tot4.a.y * sncs_d.d2y;

			ct3z = tot4.a.x * sncs_d.d3x +
			       tot4.a.y * sncs_d.d3y;

			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 |= SELJ4;
		}
		if (dirs & SELMY) {
			best |= SELJ5;
		}
		if (dirs & SELMZ) {
			best |= SELJ6;
		}
	}
	return(best);
}

#endif


#ifdef STAN

select_n(dirs, tool) /*::*/
register int dirs;
register TRSF_PTR tool;
{
	register int best = 0;

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

		if (tool != NULL) {
			Assigntr(&tot, tool);
		}
		else {
			Assigntr(&tot, unitr);
		}
		if (dirs & SELFX) {
			ct1x = tot.n.x * sncs_d.d1x +
			       tot.n.y * sncs_d.d1y +
			       tot.n.z * sncs_d.d1z;
			ct1x = FABS(ct1x);

			ct2x = tot.n.x * sncs_d.d2x +
			       tot.n.y * sncs_d.d2y +
			       tot.n.z * sncs_d.d2z;

			ct3x = tot.n.x * sncs_d.d3x +
			       tot.n.y * sncs_d.d3y +
			       tot.n.z * sncs_d.d3z;

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

			ct3x = FABS(ct3x);
		}
		if (dirs & SELFY) {
			ct1y = tot.o.x * sncs_d.d1x +
			       tot.o.y * sncs_d.d1y +
			       tot.o.z * sncs_d.d1z;
			ct1y = FABS(ct1y);

			ct2y = tot.o.x * sncs_d.d2x +
			       tot.o.y * sncs_d.d2y +
			       tot.o.z * sncs_d.d2z;

			ct3y = tot.o.x * sncs_d.d3x +
			       tot.o.y * sncs_d.d3y +
			       tot.o.z * sncs_d.d3z;

			ct2y = ct2y + ct3y;
			ct2y = FABS(ct2y);

			ct3y = FABS(ct3y);
		}
		if (dirs & SELFZ) {
			ct1z = tot.a.x * sncs_d.d1x +
			       tot.a.y * sncs_d.d1y;
			ct1z = FABS(ct1z);

			ct2z = tot.a.x * sncs_d.d2x +
			       tot.a.y * sncs_d.d2y;

			ct3z = tot.a.x * sncs_d.d3x +
			       tot.a.y * sncs_d.d3y;

			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 |= SELJ4;
		}
		if (dirs & SELMY) {
			best |= SELJ5;
		}
		if (dirs & SELMZ) {
			best |= SELJ6;
		}
	}
	return(best);
}

#endif
