/*-=-=-=-=-=-=-=-=-=-=-=- 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:	stanJacob.c
 *
 * DESCRIPTION:	Jacobian computations for the Stanford arm.
 *		For description, see 'man pumaJacob'
 * 
 * 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/stan_jls.h"
#include "../h/stan_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 D2	gen->dh[1].d		/* basic A matrix parameters */

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

#define S1	svar->s[J1]
#define C1	svar->c[J1]
#define S2	svar->s[J2]
#define C2	svar->c[J2]
#define S4	svar->s[J4]
#define C4	svar->c[J4]
#define S5	svar->s[J5]
#define C5	svar->c[J5]
#define S6	svar->s[J6]
#define C6	svar->c[J6]
#define D3	svar->d3

#define U4	svar->u4

stanFwdJacobT(t, f, j6, kyn)
float *t;
FORCE *f;
float *j6;
KYN *kyn;
{
	STAN_VAR var;

	stanUpdateVar (&var, j6, kyn);
	return (stanFwdJacobTVar (t, f, &var, kyn));
}

stanFwdJacob(dc, dj, j6, kyn)
DIFF *dc;
float *dj;
float *j6;
KYN *kyn;
{
	STAN_VAR var;

	stanUpdateVar (&var, j6, kyn);
	return (stanFwdJacobVar (dc, dj, &var, kyn));
}

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

	stanUpdateVar (&var, j6, kyn);
	return (stanInvJacobTVar (f, t, &var, kyn, epsilon));
}

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

	stanUpdateVar (&var, j6, kyn);
	return (stanInvJacobVar (dj, dc, &var, kyn, epsilon));
}

stanFwdJacobTVar(j, f, var, kyn) /*::*/
float *j;
FORCE *f;
void *var;
KYN *kyn;
{
	register STAN_VAR *svar = (STAN_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	FORCE fl3;

	fl3.f.x = U4.n.x * f->f.x + U4.o.x * f->f.y + U4.a.x * f->f.z;
	fl3.f.y = U4.n.y * f->f.x + U4.o.y * f->f.y + U4.a.y * f->f.z;
	fl3.f.z = U4.n.z * f->f.x + U4.o.z * f->f.y + U4.a.z * f->f.z;
	fl3.m.x = U4.n.x * f->m.x + U4.o.x * f->m.y + U4.a.x * f->m.z;
	fl3.m.y = U4.n.y * f->m.x + U4.o.y * f->m.y + U4.a.y * f->m.z;
	fl3.m.z = U4.n.z * f->m.x + U4.o.z * f->m.y + U4.a.z * f->m.z;

	j[J1] = -D2*(C2*fl3.f.x + S2*fl3.f.z) + D3*S2*fl3.f.y 
		-S2*fl3.m.x + C2*fl3.m.z;
	j[J2] = D3*fl3.f.x + fl3.m.y;
	j[J3] = fl3.f.z;

	j[J4] = fl3.m.z;
	j[J5] = -S4*fl3.m.x + C4*fl3.m.y;
	j[J6] = C4*S5*fl3.m.x + S4*S5*fl3.m.y + C5*fl3.m.z;
}

stanFwdJacobVar(d, q, var, kyn) /*::*/
register DIFF_PTR d;
register float *q;
void *var;
KYN *kyn;
{
	register STAN_VAR *svar = (STAN_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	DIFF dl3;

	dl3.t.x = -C2*D2*q[J1] + D3*q[J2];
	dl3.t.y =  D3*S2*q[J1];
	dl3.t.z = -D2*S2*q[J1] + q[J3];
	dl3.r.x = -S2*q[J1] - S4*q[J5] + C4*S5*q[J6];
	dl3.r.y =  q[J2] + C4*q[J5] + S4*S5*q[J6];
	dl3.r.z =  C2*q[J1] + q[J4] + C5*q[J6];

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

stanInvJacobVar(q, d, var, kyn, epsilon)
register float *q;
register DIFF_PTR d;
void *var;
KYN *kyn;
double epsilon;
{
	STAN_KYN *skyn = (STAN_KYN*)kyn->ext;
	register STAN_VAR *svar = (STAN_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	DIFF dl3;
	int code = 0;
	register float threshold;

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

	/* Massage epsilon into something reasonable */

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

	/* theta1 */
   
	threshold = epsilon*skyn->d3Rng;
	if (FABS(D3*S2) < threshold) 
	 { code |= STAN_ALIGN_DEG;
	   q[J1] = dl3.t.y * (D3*S2) / (threshold*threshold);
	 }
	else
	 { q[J1] = dl3.t.y / (D3*S2);
	 }
   
	/* theta2 */
   
	if (FABS(D3) < threshold) 
	 { code |= STAN_ARM_DEG;
	   q[J2] = (dl3.t.x + q[J1]*C2*D2) * D3 / (threshold*threshold);
	 }
	else
	 { q[J2] = (dl3.t.x + q[J1]*C2*D2) / D3;
	 }
   
	/* theta3 */

	q[J3] = q[J1]*D2*S2 + dl3.t.z;

	/* theta6 */
   
	threshold = epsilon;
	if (FABS(S5) < threshold)
	 { code |= STAN_WRIST_DEG;
	   q[J6] = ((dl3.r.y - q[J2])*S4 + (q[J1]*S2 + dl3.r.x)*C4) * S5 /
		   (threshold*threshold);
	 }
	else
	 { q[J6] = ((dl3.r.y - q[J2])*S4 + (q[J1]*S2 + dl3.r.x)*C4) / S5;
	 }
   
	/* theta5 */

	q[J5] = (-q[J1]*S2 - dl3.r.x)*S4 + (dl3.r.y - q[J2])*C4;

	/* theta4 */
   
	q[J4] = dl3.r.z - C5*q[J6] - C2*q[J1];

	return (code);
}

stanInvJacobTVar(f, t, var, kyn, epsilon)
register FORCE *f;
register float *t;
void *var;
KYN *kyn;
double epsilon;
{
	STAN_KYN *skyn = (STAN_KYN*)kyn->ext;
	register STAN_VAR *svar = (STAN_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	FORCE fl3;
	int code = 0;
	float threshold, k3;

	/* Set threshold to something reasonable */

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

	fl3.m.z = t[J4];
  
	threshold = epsilon;
	if (FABS(S5) < threshold)
	 { code |= STAN_WRIST_DEG;
	   k3 = (C5*fl3.m.z - t[J6]) * S5 / (threshold * threshold);
	 }
	else
	 { k3 = (C5*fl3.m.z - t[J6]) / S5;
	 }

	fl3.m.x = -S4*t[J5] - C4*k3;

	fl3.m.y =  C4*t[J5] - S4*k3;
   
	fl3.f.z = t[J3];

	threshold = skyn->d3Rng*epsilon;
	if (FABS(D3) < threshold)
	 { code |= STAN_ARM_DEG;
	   fl3.f.x = -(fl3.m.y - t[J2]) * D3 / (threshold*threshold);
	 }
	else
	 { fl3.f.x = -(fl3.m.y - t[J2]) / D3;
	 }

	if (FABS(D3*S2) < threshold)
	 { code |= STAN_ALIGN_DEG;
	   fl3.f.y = ((D2*fl3.f.x - fl3.m.z)*C2 + (fl3.m.x + D2*fl3.f.z)*S2 +
		      t[J1]) * (D3*S2) / (threshold*threshold);
	 }
	else
	 { fl3.f.y = ((D2*fl3.f.x - fl3.m.z)*C2 + (fl3.m.x + D2*fl3.f.z)*S2 +
		      t[J1]) / (D3*S2);
	 }

	f->m.x = U4.n.x * fl3.m.x + U4.n.y * fl3.m.y + U4.n.z * fl3.m.z;
	f->m.y = U4.o.x * fl3.m.x + U4.o.y * fl3.m.y + U4.o.z * fl3.m.z;
	f->m.z = U4.a.x * fl3.m.x + U4.a.y * fl3.m.y + U4.a.z * fl3.m.z;
	f->f.x = U4.n.x * fl3.f.x + U4.n.y * fl3.f.y + U4.n.z * fl3.f.z;
	f->f.y = U4.o.x * fl3.f.x + U4.o.y * fl3.f.y + U4.o.z * fl3.f.z;
	f->f.z = U4.a.x * fl3.f.x + U4.a.y * fl3.f.y + U4.a.z * fl3.f.z;

	return (code);
}

/* Jacobian updating functions */

stanUpdateVar (var, ang, kyn)
void *var;
register float *ang;
KYN *kyn;
{
	register STAN_VAR *svar = (STAN_VAR*)var;

	SINCOS (ang[J1], &S1, &C1);
	SINCOS (ang[J2], &S2, &C2);
	SINCOS (ang[J4], &S4, &C4);
	SINCOS (ang[J5], &S5, &C5);
	SINCOS (ang[J6], &S6, &C6);

	stanUpdateVarXsincos (var, ang, kyn);
}

stanUpdateVarXsincos (var, ang, kyn)
void *var;
float *ang;
KYN *kyn;
{
   
	register STAN_KYN *skyn = (STAN_KYN*)kyn->ext;
	register STAN_VAR *svar = (STAN_VAR*)var;

	svar->d3 = ang[J3];

	/* update the U4 coefficients */
   
	U4.n.x =  C4*C5*C6 - S4*S6;
	U4.n.y =  S4*C5*C6 + C4*S6;
	U4.n.z = -S5*C6;
	U4.o.x = -C4*C5*S6 - S4*C6;
	U4.o.y = -S4*C5*S6 + C4*C6;
	U4.o.z =  S5*S6;
	U4.a.x =  C4*S5;
	U4.a.y =  S4*S5;
	U4.a.z =  C5;
}
