/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : solut.c
 *      Remarks : kinematic functions (solve handpos).
 *      Usage   : part of the library
 */

/*
 * jns_to_tr is handpos, it also computes the configuration (in a string )
 * solveconf just compute the configuration
 * tr_to_jns is solve, it has  static conf flags and get the changes
 */


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


/* performs mod 2 pi */

static double range(a) /*##*/
real a;
{
	if (a > 15. || a < -15.)
		return(10.);

	while(a >= pit2_m)
		a -= pit2_m;
	while(a < 0.)
		a += pit2_m;
	if (a > pit2_m)
		return(0.);
	return(a);
}


#ifdef PUMA

jns_to_tr_n(t, j, u) /*::*/
register TRSF_PTR t;
register JNTS_PTR j;
bool u;
{
	real c1, s1, c2, s2, c23, s23, c4, s4, c5, s5, c6, s6,
	k1, k2, k3, k4, k5, k6, k7, x;

	SINCOS(s1, c1, j->th1 + jmin_c.th1);
	SINCOS(s2, c2, j->th2 + jmin_c.th2);
	SINCOS(s23, c23, j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3);
	SINCOS(s4, c4, j->th4 + jmin_c.th4);
	SINCOS(s5, c5, j->th5 + jmin_c.th5);
	SINCOS(s6, c6, j->th6 + jmin_c.th6);

	k1  = c23 * s5;
	k2  = c4 * c5 * s6 + s4 * c6;
	k3  = - c23 * k2 + s23 * s5 * s6;
	k4  = - s4 * c5 * s6 + c4 * c6;
	k5  = s4 * s5;
	k6 = c23 * c4 * s5 + s23 * c5;
	k7 = armk_c.d4 * s23 + armk_c.a3 * c23 + armk_c.a2 * c2;

	t->o.x = c1 * k3 - s1 * k4;
	t->o.y = s1 * k3 + c1 * k4;
	t->o.z = s23 * k2 + k1 * s6;

	t->a.x = c1 * k6 - s1 * k5;
	t->a.y = s1 * k6 + c1 * k5;
	t->a.z = - s23 * c4 * s5 + c23 * c5;

	t->p.x = c1 * k7 - s1 * armk_c.d3;
	t->p.y = s1 * k7 + c1 * armk_c.d3;
	t->p.z = armk_c.d4 * c23 - armk_c.a3 * s23 - armk_c.a2 * s2;

	/* n = o X a */

	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;

	x = range(atan2(t->p.y, t->p.x) - j->th1 - jmin_c.th1);
	(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';

	x = range(armk_c.aa3d4 - j->th3 - jmin_c.th3);
	(j->conf)[1] = (x < pib2_m || x > pi_m + pib2_m) ? 'u' : 'd';

	(j->conf)[2] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
	(j->conf)[3] = '\0';

	if (u) {
		sncs_d.s1 = s1;
		sncs_d.c1 = c1;
		sncs_d.s2 = s2;
		sncs_d.c2 = c2;
		sncs_d.s23 = s23;
		sncs_d.c23 = c23;
		SINCOS(sncs_d.s3, sncs_d.c3, j->th3 + jmin_c.th3);
		sncs_d.s4 = s4;
		sncs_d.c4 = c4;
		sncs_d.s5 = s5;
		sncs_d.c5 = c5;
		sncs_d.s6 = s6;
		sncs_d.c6 = c6;
	}
}



tr_to_jns_n(j, t, u) /*::*/
register JNTS_PTR j;
register TRSF_PTR t;
{
	int code = 0;
	real t1d, f11p, d, t3d, f13a, f11a, f31a, f13o, f11o, w1, w2, wd,
	s1, c1, s23, c23, s3, c3, s4, c4, s5, c5, s6, c6,
	px, py, pz, ax, ay, az, ox, oy, oz,
	t1, t2, t3, t4, t5, t6;
	static real t4o;
	static lefty = NO, up = YES, flip = NO;
	char *c = j->conf;

	while (*c) {
		switch (*c++) {
		case 'l' :
			lefty = YES;
			break;
		case 'r' :
			lefty = NO;
			break;

		case 'u' :
			up = YES;
			break;

		case 'd' :
			up = NO;
			break;

		case 'f' :
			flip  = YES;
			break;

		case 'n' :
			flip  = NO;
			break;

		default  :
			break;
		}
	}
	px = t->p.x;
	py = t->p.y;
	pz = t->p.z;
	ax = t->a.x;
	ay = t->a.y;
	az = t->a.z;
	ox = t->o.x;
	oy = t->o.y;
	oz = t->o.z;

	/* theta1 */

	t1d = sqrt(px * px + py * py - armk_c.d32);
	t1 = atan2(py, px) - atan2(armk_c.d3, (lefty) ? t1d : - t1d);

	if ((j->th1 = range (t1 - jmin_c.th1)) > jrng_c.th1)
		code |= 01;

	SINCOS(s1, c1, t1);

	/* theta3 */

	f11p = c1 * px + s1 * py;
	d = f11p * f11p + pz * pz - armk_c.e432;
	t3d = sqrt(armk_c.e4aa4ad - d * d);
	t3 = armk_c.aa3d4 - atan2(d, (up) ? t3d : - t3d);

	if ((j->th3 = range (t3 - jmin_c.th3)) > jrng_c.th3)
		code |= 04;

	SINCOS(s3, c3, t3);

	/* theta2 */

	w1 = armk_c.a2 * c3 + armk_c.a3;
	w2 = armk_c.d4 + armk_c.a2 * s3;
	wd = f11p * f11p + pz * pz;
	if (FABS(wd) < SMALL) {
		return(01000);
	}
	s23 = (w2 * f11p - w1 * pz) / wd;
	c23 = (w1 * f11p + w2 * pz) / wd;
	t2 = atan2(s23, c23) - t3;

	if ((j->th2 = range (t2 - jmin_c.th2)) > jrng_c.th2)
		code |= 02;

	/* theta4 */

	f13a = - s1 * ax + c1 * ay;
	f11a =   c1 * ax + s1 * ay;
	f31a = c23 * f11a - s23 * az;
	t4 = (flip) ? atan2(- f13a, - f31a) : atan2(  f13a,  f31a);

	if ((j->th4 = range (t4 - jmin_c.th4)) > jrng_c.th4)
		code |= 010;

	SINCOS(s4, c4, t4);

	/* theta5 */

	s5 = c4 * f31a + s4 * f13a ;
	c5 = s23 * f11a + c23 * az;

	t5 = atan2(s5, c5);

	if (FABS(t5) < SMALL) {
		j->th4 = range(t4o - jmin_c.th4);
		SINCOS(s4, c4, t4o);
		s5 = 0.;
		c5 = 1.;
		code |= 0400;
	}
	else {
		t4o = t4;
	}

	if ((j->th5 = range (t5 - jmin_c.th5)) > jrng_c.th5)
		code |= 020;

	/* theta6 */

	f13o = - s1 * ox + c1 * oy;
	f11o =   c1 * ox + s1 * oy;
	s6 = - c5 * (c4 * (c23 * f11o - s23 * oz) + s4 * f13o)
	     + s5 * (s23 * f11o + c23 * oz);
	c6 = - s4 * (c23 * f11o - s23 * oz) + c4 * f13o;

	t6 = atan2(s6, c6);

	if ((j->th6 = range (t6 - jmin_c.th6)) > jrng_c.th6)
		code |= 040;

	if (u) {
		sncs_d.s1 = s1;
		sncs_d.c1 = c1;
		SINCOS(sncs_d.s2, sncs_d.c2, t2);
		sncs_d.s23 = s23;
		sncs_d.c23 = c23;
		sncs_d.s3 = s3;
		sncs_d.c3 = c3;
		sncs_d.s4 = s4;
		sncs_d.c4 = c4;
		sncs_d.s5 = s5;
		sncs_d.c5 = c5;
		sncs_d.s6 = s6;
		sncs_d.c6 = c6;
	}
	return(code);
}



solveconf_n(j) /*::*/
register JNTS_PTR j;
{
	real s1, c1, s23, c23, c2, px, py, k, x;

	SINCOS(s1, c1, j->th1 + jmin_c.th1);
	SINCOS(s23, c23, j->th2 + jmin_c.th2 + j->th3 + jmin_c.th3);
	c2 = cos(j->th2 + jmin_c.th2);

	k = armk_c.d4 * s23 + armk_c.a3 * c23 + armk_c.a2 * c2;
	px = c1 * k - s1 * armk_c.d3;
	py = s1 * k + c1 * armk_c.d3;

	x = range(atan2(py, px) - j->th1 - jmin_c.th1);
	(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';

	x = range(armk_c.aa3d4 - j->th3 - jmin_c.th3);
	(j->conf)[1] = (x < pib2_m || x > pi_m + pib2_m) ? 'u' : 'd';

	(j->conf)[2] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
	(j->conf)[3] = '\0';
}
#endif


#ifdef STAN

jns_to_tr_n(t, j, u) /*::*/
register TRSF_PTR t;
register JNTS_PTR j;
bool u;
{
	real c1, s1, c2, s2, d3, c4, s4, c5, s5, c6, s6,
	k1, k2, k3, k4, k5, k6, k7, x;

	SINCOS(s1, c1, j->th1 + jmin_c.th1);
	SINCOS(s2, c2, j->th2 + jmin_c.th2);
	SINCOS(s4, c4, j->th4 + jmin_c.th4);
	SINCOS(s5, c5, j->th5 + jmin_c.th5);
	SINCOS(s6, c6, j->th6 + jmin_c.th6);

	d3 = j->th3 + jmin_c.th3;
	k1 = c4 * s5;
	k2 = s4 * s5;
	k3 = c4 * c5 * s6 + s4 * c6;
	k4 = - c2 * k3 + s2 * s5 * s6;
	k5 = - s4 * c5 * s6 + c4 * c6;
	k6 = c2 * k1 + s2 * c5;
	k7 = s2 * d3;

	t->o.x = c1 * k4 - s1 * k5;
	t->o.y = s1 * k4 + c1 * k5;
	t->o.z = s2 * k3 + c2 * s5 * s6;

	t->a.x = c1 * k6 - s1 * k2;
	t->a.y = s1 * k6 + c1 * k2;
	t->a.z = - s2 * k1 + c2 * c5;

	t->p.x = c1 * k7 - s1 * armk_c.d2;
	t->p.y = s1 * k7 + c1 * armk_c.d2;
	t->p.z = c2 * d3;

	/* n = o X a */

	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;

	x = range(atan2(t->p.y, t->p.x) - j->th1 - jmin_c.th1);
	(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';

	(j->conf)[1] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
	(j->conf)[2] = '\0';

	if (u) {
		sncs_d.s1 = s1;
		sncs_d.c1 = c1;
		sncs_d.s2 = s2;
		sncs_d.c2 = c2;
		sncs_d.d3 = d3;
		sncs_d.s4 = s4;
		sncs_d.c4 = c4;
		sncs_d.s5 = s5;
		sncs_d.c5 = c5;
		sncs_d.s6 = s6;
		sncs_d.c6 = c6;
	}
}



tr_to_jns_n(j, t, u) /*::*/
register JNTS_PTR j;
register TRSF_PTR t;
bool u;
{
	int code = 0;
	real s1, c1, s2, c2, s4, c4, s5, c5, s6, c6,
	f11p, f13a, f11a, f21a, f11o, f21o, f22o,
	px, py, pz, ax, ay, az, ox, oy, oz,
	t1d, t1, t2, d3, t4, t5, t6;
	static real t4o;
	static int lefty = YES, flip = NO;
	char *c = j->conf;

	while (*c) {
		switch (*c++) {
		case 'l' :
			lefty = YES;
			break;
		case 'r' :
			lefty = NO;
			break;

		case 'f' :
			flip  = YES;
			break;

		case 'n' :
			flip  = NO;
			break;

		default  :
			break;
		}
	}
	px = t->p.x;
	py = t->p.y;
	pz = t->p.z;
	ax = t->a.x;
	ay = t->a.y;
	az = t->a.z;
	ox = t->o.x;
	oy = t->o.y;
	oz = t->o.z;

	/* theta1 */

	t1d = sqrt(px * px + py * py - armk_c.d22);
	t1 = atan2(py, px) - atan2(armk_c.d2, (lefty) ? t1d : - t1d);

	if ((j->th1 = range (t1 - jmin_c.th1)) > jrng_c.th1)
		code |= 01;

	SINCOS(s1, c1, t1);

	/* theta2 */

	t2 = atan2(f11p = c1 * px + s1 * py, pz);

	if ((j->th2 = range (t2 - jmin_c.th2)) > jrng_c.th2)
		code |= 02;

	SINCOS(s2, c2, t2);

	/* dist3 */

	d3 = s2 * f11p + c2 * pz;

	if ((j->th3 = d3 - jmin_c.th3) > jrng_c.th3 || j->th3 < 0.)
		code |= 04;

	/* theta4 */

	f13a = - s1 * ax + c1 * ay;
	f11a =   c1 * ax + s1 * ay;
	f21a = c2 * f11a - s2 * az;
	t4 = (flip) ? atan2(- f13a, - f21a) : atan2(f13a,  f21a);

	if ((j->th4 = range (t4 - jmin_c.th4)) > jrng_c.th4)
		code |= 010;

	SINCOS(s4, c4, t4);

	/* theta5 */

	s5 = c4 * f21a + s4 * f13a ;
	c5 = s2 * f11a + c2 * az;

	t5 = atan2(s5, c5);

	if (FABS(t5) < SMALL) {
		j->th4 = range(t4o - jmin_c.th4);
		SINCOS(s4, c4, t4o);
		s5 = 0.;
		c5 = 1.;
		code |= 0400;
	}
	else {
		t4o = t4;
	}
	if ((j->th5 = range (t5 - jmin_c.th5)) > jrng_c.th5)
		code |= 020;

	/* theta6 */

	f11o =   c1 * ox + s1 * oy;
	f21o = c2 * f11o - s2 * oz;
	f22o = - s1 * ox + c1 * oy;

	s6 = - c5 * (c4 * f21o + s4 * f22o)
	     + s5 * (s2 * f11o + c2 * oz);
	c6 = - s4 * f21o + c4 * f22o;
	t6 = atan2(s6, c6);

	if ((j->th6 = range (t6 - jmin_c.th6)) > jrng_c.th6)
		code |= 040;

	if (u) {
		sncs_d.s1 = s1;
		sncs_d.c1 = c1;
		sncs_d.s2 = s2;
		sncs_d.c2 = c2;
		sncs_d.d3 = d3;
		sncs_d.s4 = s4;
		sncs_d.c4 = c4;
		sncs_d.s5 = s5;
		sncs_d.c5 = c5;
		sncs_d.s6 = s6;
		sncs_d.c6 = c6;
	}

	return(code);
}


solveconf_n(j) /*::*/
register JNTS_PTR j;
{
	real s1, c1, s2, px, py, x;

	SINCOS(s1, c1, j->th1 + jmin_c.th1);
	s2 = sin(j->th2 + jmin_c.th2);
	x  = s2 * (j->th3 + jmin_c.th3);

	px = c1 * x - s1 * armk_c.d2;
	py = s1 * x + c1 * armk_c.d2;

	x = range(atan2(py, px) - j->th1 - jmin_c.th1);
	(j->conf)[0] = (x < pib2_m || x > pi_m + pib2_m) ? 'l' : 'r';

	(j->conf)[1] = (range(j->th5 + jmin_c.th5) > pi_m) ? 'f' : 'n';
	(j->conf)[2] = '\0';
}
#endif
