/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : src
 *      File    : jacob.c
 *      Remarks : Jacobian related functions.
 *                The idea is not to compute twice the same thing
 *                regardless of the order the functions are called.
 *      Usage   : part of the library
 */

/*LINTLIBRARY*/

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


#ifdef PUMA

static int lasth = 0;                   /* last time h calculated       */
static int lastjac = 0;                 /* last time jacob coeff .....  */
static int lastu5 = 0;                  /* last time U5 ....            */


/*
 *                T  A4
 * compute : T = J     F,
 *                        assumes sin cos available;
 */


static jaco4T(j, f) /*##*/
register JNTS_PTR j;
register FORCE_PTR f;
{
	if (rtime != lasth) {
		lasth = rtime;
		GETH;
	}
	if (rtime != lastjac) {
		lastjac = rtime;
		UPDJ;
	}
	j->th1 = sncs_d.d1x * f->f.x +
		 sncs_d.d1y * f->f.y +
		 sncs_d.d1z * f->f.z +
		 sncs_d.r1x * f->m.x -
		 sncs_d.c23 * f->m.y +
		 sncs_d.r1z * f->m.z;

	j->th2 = sncs_d.d2x * f->f.x +
		 sncs_d.d2z * f->f.z +
		 sncs_d.d2y * f->f.y;

	j->th3 = sncs_d.d3x * f->f.x +
		 sncs_d.d3y * f->f.y +
		 sncs_d.d3z * f->f.z +
		 sncs_d.s4  * f->m.x +
		 sncs_d.c4  * f->m.z;

	j->th2 += j->th3;

	j->th4 =           -f->m.y;

	j->th5 =            f->m.z;

	j->th6 = sncs_d.s5  * f->m.x -
		 sncs_d.c5  * f->m.y;
}


/*
 *          A4
 * compute D   = J Q,
 *                        assumes sin cos avialable ;
 */


static jaco4D(d, q) /*##*/
register DIFF_PTR d;
register JNTS_PTR q;
{
	real q23 = q->th2 + q->th3;

	if (rtime != lasth) {
		lasth = rtime;
		GETH;
	}
	if (rtime != lastjac) {
		lastjac = rtime;
		UPDJ;
	}
	d->t.x = sncs_d.d1x * q->th1 +
		 sncs_d.d2x * q->th2 +
		 sncs_d.d3x * q23;

	d->t.y = sncs_d.d1y * q->th1 +
		 sncs_d.d2y * q->th2 +
		 sncs_d.d3y * q23;

	d->t.z = sncs_d.d1z * q->th1 +
		 sncs_d.d2z * q->th2 +
		 sncs_d.d3z * q23;

	d->r.x = sncs_d.r1x * q->th1 +
		 sncs_d.s4  * q23    +
		 sncs_d.s5  * q->th6;

	d->r.y= -sncs_d.c23 * q->th1 -
			    q->th4 -
		 sncs_d.c5  * q->th6;

	d->r.z = sncs_d.r1z * q->th1 +
		 sncs_d.c4  * q23    +
			    q->th5;
}


/*
 *                T T6
 * compute : T = J    F,
 *                        assumes sin cos jacob coeff available;
 */

jacobT_n(j, f) /*::*/
JNTS_PTR j;
FORCE_PTR f;
{
	FORCE fl4;

	if (lastu5 != rtime) {
		GETU5;
		lastu5 = rtime;
	}
	fl4.f.x =
	sncs_d.u5.n.x * f->f.x + sncs_d.u5.o.x * f->f.y + sncs_d.u5.a.x * f->f.z;
	fl4.f.y =
	sncs_d.u5.n.y * f->f.x + sncs_d.u5.o.y * f->f.y + sncs_d.u5.a.y * f->f.z;
	fl4.f.z =
	sncs_d.u5.n.z * f->f.x + sncs_d.u5.o.z * f->f.y;
	fl4.m.x =
	sncs_d.u5.n.x * f->m.x + sncs_d.u5.o.x * f->m.y + sncs_d.u5.a.x * f->m.z;
	fl4.m.y =
	sncs_d.u5.n.y * f->m.x + sncs_d.u5.o.y * f->m.y + sncs_d.u5.a.y * f->m.z;
	fl4.m.z =
	sncs_d.u5.n.z * f->m.x + sncs_d.u5.o.z * f->m.y;

	jaco4T(j, &fl4);
}


/*
 *          T6
 * compute D   = J Q,
 *                        assumes sin cos jacob coeff available;
 */

jacobD_n(d, q) /*::*/
register DIFF_PTR d;
register JNTS_PTR q;
{
	DIFF dl4;

	jaco4D(&dl4, q);
	if (lastu5 != rtime) {
		GETU5;
		lastu5 = rtime;
	}
	d->t.x =
	sncs_d.u5.n.x * dl4.t.x + sncs_d.u5.n.y * dl4.t.y + sncs_d.u5.n.z * dl4.t.z;
	d->t.y =
	sncs_d.u5.o.x * dl4.t.x + sncs_d.u5.o.y * dl4.t.y + sncs_d.u5.o.z * dl4.t.z;
	d->t.z =
	sncs_d.u5.a.x * dl4.t.x + sncs_d.u5.a.y * dl4.t.y;
	d->r.x =
	sncs_d.u5.n.x * dl4.r.x + sncs_d.u5.n.y * dl4.r.y + sncs_d.u5.n.z * dl4.r.z;
	d->r.y =
	sncs_d.u5.o.x * dl4.r.x + sncs_d.u5.o.y * dl4.r.y + sncs_d.u5.o.z * dl4.r.z;
	d->r.z =
	sncs_d.u5.a.x * dl4.r.x + sncs_d.u5.a.y * dl4.r.y;
}



/*
 * Compute an approximation of differential joint changes given
 * Cartesian changes, ignores shoulder and elbow offsets.
 *                              assumes sin cos availables
 */

static jacobI4(q, d) /*::*/
register JNTS_PTR q;
register DIFF_PTR d;
{
	register int code = 0;
	real q23;

	if (rtime != lasth) {
		lasth = rtime;
		GETH;
	}

	/* theta2 */

	if (FABS(sncs_d.c3) < SMALL) {
		code |= ELBOW_DEG;
	}
	else {
		q->th2 = d->t.y / (armk_c.a2 * sncs_d.c3);
	}

	/* theta1 */

	if (FABS(sncs_d.h) < SMALL) {
		code |= ALIGN_DEG;
	}
	else {
		q->th1 = (sncs_d.s4 * d->t.x + sncs_d.c4 * d->t.z);
	}

	/* theta3 */

	if (!(code & ELBOW_DEG)) {
		q23 = (sncs_d.c4 * d->t.x - sncs_d.s4 * d->t.z -
		       armk_c.a2 * sncs_d.s3 * q->th2) / armk_c.d4;
		q->th3 = q23 - q->th2;
	}

	/* theta6 */

	if (FABS(sncs_d.s5) < SMALL) {
		code |= WRIST_DEG;
	}
	else {
		if (!((code & ELBOW_DEG) || (code & ALIGN_DEG))) {
			q->th6 = (d->r.x + sncs_d.s23 * sncs_d.c4 * q->th1 -
				  sncs_d.s23 * sncs_d.c4 * q23) / sncs_d.s5;
		}
	}

	/* theta4 */

	if (!code) {
		q->th4 = -(d->r.y + sncs_d.c23 * q->th1 + sncs_d.c5 * q->th6);

		/* theta5 */

		q->th5 = d->r.z - sncs_d.s23 * sncs_d.s4 * q->th1 - sncs_d.c4 * q23;
	}
	return(code);
}


jacobI_n(q, d) /*::*/
register JNTS_PTR q;
register DIFF_PTR d;
{
	DIFF dl4;

	if (lastu5 != rtime) {
		GETU5;
		lastu5 = rtime;
	}

	dl4.t.x =
	sncs_d.u5.n.x * d->t.x + sncs_d.u5.o.x * d->t.y + sncs_d.u5.a.x * d->t.z;
	dl4.t.y =
	sncs_d.u5.n.y * d->t.x + sncs_d.u5.o.y * d->t.y + sncs_d.u5.a.y * d->t.z;
	dl4.t.z =
	sncs_d.u5.n.z * d->t.x + sncs_d.u5.o.z * d->t.y;
	dl4.r.x =
	sncs_d.u5.n.x * d->r.x + sncs_d.u5.o.x * d->r.y + sncs_d.u5.a.x * d->r.z;
	dl4.r.y =
	sncs_d.u5.n.y * d->r.x + sncs_d.u5.o.y * d->r.y + sncs_d.u5.a.y * d->r.z;
	dl4.r.z =
	sncs_d.u5.n.z * d->r.x + sncs_d.u5.o.z * d->r.y;

	return(jacobI4(q, &dl4));
}




/*
 * computes gravity loadings
 */

gravload_n(l, c2, c23, s23, c4, s4, c5, s5) /*::*/
register JNTS_PTR l;
real c2, c23, s23, c4, s4, c5, s5;
{
	l->th1 = 0.;
	l->th3 = armk_c.cp32 * s23 + armk_c.cp31 * c23;
	l->th2 = l->th3 + armk_c.cp21 * c2;
	l->th4 = -(armk_c.cp50 * s23 * s4 * s5);
	l->th5 = armk_c.cp50 * (s23 * c4 * c5 + c23 * s5);
	l->th6 = 0.;
}
#endif


#ifdef STAN

static int lastjac = 0;

jacobT_n(j, f) /*::*/
register JNTS_PTR j;
register FORCE_PTR f;
{
	if (rtime != lastjac) {
		lastjac = rtime;
		UPDJ;
	}

	j->th1 = sncs_d.d1x * f->f.x +
		 sncs_d.d1y * f->f.y +
		 sncs_d.d1z * f->f.z +
		 sncs_d.r1x * f->m.x +
		 sncs_d.r1y * f->m.y +
		 sncs_d.r1z * f->m.z;

	j->th2 = sncs_d.d2x * f->f.x +
		 sncs_d.d2y * f->f.y +
		 sncs_d.d2z * f->f.z +
		 sncs_d.r2x * f->m.x +
		 sncs_d.r2y * f->m.y +
		 sncs_d.r2z * f->m.z;

	j->th3 = sncs_d.d3x * f->f.x +
		 sncs_d.d3y * f->f.y +
		 sncs_d.d3z * f->f.z;

	j->th4 = sncs_d.r4x * f->m.x +
		 sncs_d.r4y * f->m.y +
		 sncs_d.c5  * f->m.z;

	j->th5 = sncs_d.s6  * f->m.x +
		 sncs_d.c6  * f->m.y;

	j->th6 =            f->m.z;
}


/*
 * compute jacobian
 */

jacobD_n(d, q) /*::*/
register DIFF_PTR d;
register JNTS_PTR q;
{
	if (rtime != lastjac) {
		lastjac = rtime;
		UPDJ;
	}
	d->t.x = sncs_d.d1x * q->th1 +
		 sncs_d.d2x * q->th2 +
		 sncs_d.d3x * q->th3;

	d->t.y = sncs_d.d1y * q->th1 +
		 sncs_d.d2y * q->th2 +
		 sncs_d.d3y * q->th3;

	d->t.z = sncs_d.d1z * q->th1 +
		 sncs_d.d2z * q->th2 +
		 sncs_d.d3z * q->th3;

	d->r.x = sncs_d.r1x * q->th1 +
		 sncs_d.r2x * q->th2 +
		 sncs_d.r4x * q->th4 +
		 sncs_d.s6  * q->th5;

	d->r.y = sncs_d.r1y * q->th1 +
		 sncs_d.r2y * q->th2 +
		 sncs_d.r4y * q->th4 +
		 sncs_d.c6  * q->th5;

	d->r.z = sncs_d.r1z * q->th1 +
		 sncs_d.r2z * q->th2 +
		 sncs_d.c5  * q->th4 +
			    q->th6;
}


jacobI_n(q, d) /*::*/
JNTS_PTR q;
DIFF_PTR d;
{
	/* not implemented */
}


gravload_n(l, s2, c2, d3, c4, s4, c5, s5) /*::*/
register JNTS_PTR l;
real s2, c2, d3, c4, s4, c5, s5;
{
	/* not implemented */
}
#endif
