/*-=-=-=-=-=-=-=-=-=-=-=- 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 ~=~=~=~=
 *		
 * MODULE NAME:	elbowJacob.c
 *
 * DESCRIPTION:	Jacobian computations for the ELBOW robots.
 *		For description, see 'man elbowJacob'
 * 
 * AUTHOR/DATE:	John Lloyd, Spring 1987, after Vincent Hayward, Purdue, 1983
 *		Rewrote routines to interface with new RCI, winter 1988.
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <math.h>

#include "../h/fastmath.h"
#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"
#include "../h/elbow_jls.h"
#include "../h/elbow_kynvar.h"
#include "../h/robotTypes.h"

#define EPSILONF 2.0e-6

#define J1	0
#define J2	1
#define J3	2
#define J4	3
#define J5	4
#define J6	5

/* ---- Definition of link parameters to make code more readable ---- */

#define A2	gen->dh[1].a		/* basic A matrix parameters */
#define A3	gen->dh[2].a
#define A4	gen->dh[3].a

/* ---- Definition of var structure parameters ---- */

#define S1	evar->s[J1]
#define C1	evar->c[J1]
#define S2	evar->s[J2]
#define C2	evar->c[J2]
#define S23	evar->s23
#define C23	evar->c23
#define S34	evar->s34
#define C34	evar->c34
#define S234	evar->s234
#define C234	evar->c234
#define S3	evar->s[J3]
#define C3	evar->c[J3]
#define S4	evar->s[J4]
#define C4	evar->c[J4]
#define S5	evar->s[J5]
#define C5	evar->c[J5]
#define S6	evar->s[J6]
#define C6	evar->c[J6]

#define U5	evar->u5

elbowFwdJacobT(t, f, j6, kyn)
float *t;
FORCE *f;
float *j6;
KYN *kyn;
{
	ELBOW_VAR var;

	elbowUpdateVar (&var, j6, kyn);
	return (elbowFwdJacobTVar (t, f, &var, kyn));
}

elbowFwdJacob(dc, dj, j6, kyn)
DIFF *dc;
float *dj;
float *j6;
KYN *kyn;
{
	ELBOW_VAR var;

	elbowUpdateVar (&var, j6, kyn);
	return (elbowFwdJacobVar (dc, dj, &var, kyn));
}

elbowInvJacobT(f, t, j6, kyn, epsilon)
FORCE *f;
float *t;
float *j6;
KYN *kyn;
double epsilon;
{
	ELBOW_VAR var;

	elbowUpdateVar (&var, j6, kyn);
	return (elbowInvJacobTVar (f, t, &var, kyn, epsilon));
}

elbowInvJacob(dj, dc, j6, kyn, epsilon)
float *dj;
DIFF *dc;
float *j6;
KYN *kyn;
double epsilon;
{
	ELBOW_VAR var;

	elbowUpdateVar (&var, j6, kyn);
	return (elbowInvJacobVar (dj, dc, &var, kyn, epsilon));
}

elbowFwdJacobTVar(j, f, var, kyn) /*::*/
float *j;
FORCE *f;
void *var;
KYN *kyn;
{
	register ELBOW_VAR *evar = (ELBOW_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	FORCE fl4;
	float tor3, tor2;

	fl4.f.x = U5.n.x * f->f.x + U5.o.x * f->f.y + U5.a.x * f->f.z;
	fl4.f.y = U5.n.y * f->f.x + U5.o.y * f->f.y + U5.a.y * f->f.z;
	fl4.f.z = U5.n.z * f->f.x + U5.o.z * f->f.y + U5.a.z * f->f.z;
	fl4.m.x = U5.n.x * f->m.x + U5.o.x * f->m.y + U5.a.x * f->m.z;
	fl4.m.y = U5.n.y * f->m.x + U5.o.y * f->m.y + U5.a.y * f->m.z;
	fl4.m.z = U5.n.z * f->m.x + U5.o.z * f->m.y + U5.a.z * f->m.z;

	j[J1] = -evar->h1*fl4.f.z + S234*fl4.m.x + C234*fl4.m.y;
	 tor2 = A2*(S34*fl4.f.x + C34*fl4.f.y);
	 tor3 = A3*(S4*fl4.f.x + C4*fl4.f.y);
	j[J4] = A4*fl4.f.y + fl4.m.z;
	j[J5] = fl4.m.y;
	j[J6] = S5*fl4.m.x + C5*fl4.m.z;

	j[J2] = tor2 + tor3;
	j[J3] = tor3 + j[J4];

}

elbowFwdJacobVar(d, q, var, kyn) /*::*/
register DIFF_PTR d;
register float *q;
void *var;
KYN *kyn;
{
	register ELBOW_VAR *evar = (ELBOW_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	DIFF dl4;

	float q23 = q[J2] + q[J3];
	float q34 = q[J3] + q[J4];

	dl4.t.x = A2*S34*q[J2] + A3*S4*q23;
	dl4.t.y = A2*C34*q[J2] + A3*C4*q23 + A4*q34;
	dl4.t.z = -evar->h1*q[J1];
	dl4.r.x = S234*q[J1] + S5*q[J6];
	dl4.r.y = C234*q[J1] + q[J5];
	dl4.r.z = q34 + C5*q[J6];

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

elbowInvJacobVar(q, d, var, kyn, epsilon) /*::*/
register float *q;
register DIFF_PTR d;
void *var;
KYN *kyn;
double epsilon;
{
	register ELBOW_VAR *evar = (ELBOW_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	DIFF dl4;
	int code = 0;
	register float threshold;
	float q23, q34;

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

	/* Massage epsilon into something reasonable */

	if (epsilon < EPSILONF)
	 { epsilon = EPSILONF;
	 }

	/* theta1 */

	threshold = epsilon*evar->h1_norm;
	if (FABS(evar->h1) < threshold) 
	 { code |= ELBOW_ALIGN_DEG;
	   q[J1] = -dl4.t.z*evar->h1 /(threshold*threshold);
	 }
	else
	 { q[J1] = -dl4.t.z / evar->h1;
	 }
	
	/* theta 6 */

	threshold = epsilon;
	if (FABS(S5) < threshold)
	 { code |= ELBOW_WRIST_DEG;
	   q[J6] = (dl4.r.x - S234*q[J1])*S5/(threshold*threshold);
	 }
	else
	 { q[J6] = (dl4.r.x - S234*q[J1])/S5;
	 }

	/* theta 5 */

	q[J5] = dl4.r.y - C234*q[J1];

	/* theta 3 + 4 */

	q34 = dl4.r.z - C5*q[J6];

	/* theta 2 */

	threshold = epsilon;
	if (FABS(S3) < threshold)
	 { code |= ELBOW_ELBOW_DEG;
	   q[J2] = (S4*(A4*dl4.r.z - dl4.t.y - A4*q[J6]*C5) + C4*dl4.t.x) *
		   (A2*S3) / (threshold*threshold);
	 }
	else
	 { q[J2] = (S4*(A4*dl4.r.z - dl4.t.y - A4*q[J6]*C5) + C4*dl4.t.x) /
		   (A2*S3);

	 }

	/* theta 2 + 3 */

	q23 = (S4*dl4.t.x + C4*(dl4.t.y - A4*dl4.r.z + A4*q[J6]*C5)
			  - C3*q[J2]*A2) / A3;

	q[J3] = q23 - q[J2];
	q[J4] = q34 - q[J3];

	return (code);
}

elbowInvJacobTVar(f, t, var, kyn, epsilon)
register FORCE *f;
register float *t;
void *var;
KYN *kyn;
double epsilon;
{
	register ELBOW_VAR *evar = (ELBOW_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	FORCE fl4;
	int code = 0;
	float tor2, tor3, threshold, k3;

	/* Set threshold to something reasonable */

	if (epsilon < EPSILONF)
	 { epsilon = EPSILONF;
	 }

	tor3 = t[J3] - t[J4];
	tor2 = t[J2] - tor3;

	threshold = epsilon;
	if (FABS(S3) < threshold)
	 { code |= ELBOW_ELBOW_DEG;
	   k3 = (C3*tor3/A3 - tor2/A2)*S3/(threshold*threshold);
	 }
	else
	 { k3 = (C3*tor3/A3 - tor2/A2)/S3;
	 }

	fl4.f.x = S4*tor3/A3 - C4*k3;
	fl4.f.y = C4*tor3/A3 + S4*k3;
	fl4.m.z = t[J4] - A4*fl4.f.y;

	fl4.m.y = t[J5];

	threshold = epsilon;
	if (FABS(S5) < threshold)
	 { code |= ELBOW_WRIST_DEG;
	   fl4.m.x = (t[J6] - C5*fl4.m.z) * S5 / (threshold*threshold);
	 }
	else
	 { fl4.m.x = (t[J6] - C5*fl4.m.z) / S5;
	 }

	threshold = epsilon*evar->h1_norm;
	if (FABS(evar->h1) < threshold)
	 { code |= ELBOW_ALIGN_DEG;
	   fl4.f.z = (C234*fl4.m.y + S234*fl4.m.x - t[J1])*(evar->h1) /
		     (threshold * threshold);
	 }
	else
	 { fl4.f.z = (C234*fl4.m.y + S234*fl4.m.x - t[J1])/(evar->h1);
	 }

	f->m.x = U5.n.x * fl4.m.x + U5.n.y * fl4.m.y + U5.n.z * fl4.m.z;
	f->m.y = U5.o.x * fl4.m.x + U5.o.y * fl4.m.y + U5.o.z * fl4.m.z;
	f->m.z = U5.a.x * fl4.m.x + U5.a.y * fl4.m.y + U5.a.z * fl4.m.z;
	f->f.x = U5.n.x * fl4.f.x + U5.n.y * fl4.f.y + U5.n.z * fl4.f.z;
	f->f.y = U5.o.x * fl4.f.x + U5.o.y * fl4.f.y + U5.o.z * fl4.f.z;
	f->f.z = U5.a.x * fl4.f.x + U5.a.y * fl4.f.y + U5.a.z * fl4.f.z;

	return (code);
}

/* Jacobian updating functions */

elbowUpdateVar (var, ang, kyn)
void *var;
register float *ang;
KYN *kyn;
{
	register ELBOW_VAR *evar = (ELBOW_VAR*)var;

	SINCOS (ang[J1], &S1, &C1);
	SINCOS (ang[J2], &S2, &C2);
	SINCOS (ang[J3], &S3, &C3);
	SINCOS (ang[J4], &S4, &C4);
	SINCOS (ang[J5], &S5, &C5);
	SINCOS (ang[J6], &S6, &C6);
	SINCOS (ang[J2]+ang[J3], &S23, &C23);
	SINCOS (ang[J3]+ang[J4], &S34, &C34);
	SINCOS (ang[J2]+ang[J3]+ang[J4], &S234, &C234);

	elbowUpdateVarXsincos (var, ang, kyn);
}

elbowUpdateVarXsincos (var, ang, kyn)
void *var;
float *ang;
KYN *kyn;
{
	register ELBOW_VAR *evar = (ELBOW_VAR*)var;
	register GEN_KYN *gen = kyn->gen;

	/* update h values */
   
	evar->h1 =   A4*C234 + A3*C23 + A2*C2;
	evar->h1_norm = A2+A3+A4;
      
	/* update the U5 coefficients */
   
	U5.n.x =  C5*C6;
	U5.n.y =  S5*C6;
	U5.n.z =  S6;
	U5.o.x = -C5*S6;
	U5.o.y = -S5*S6;
	U5.o.z =  C6;
	U5.a.x =  S5;
	U5.a.y = -C5;
	U5.a.z =  0.;
}
