/* Function to implement recursive Newton-Euler dynamics */

#include "../h/robot_jls.h"
#include "../h/robot_kynvar.h"
#include "../h/robotTypes.h"

VECT *multInertiaVect (vr, I, v1)
VECT *vr, *v1;
INERTIA *I;
{
	register float x, y, z;

	x = v1->x;
	y = v1->y;
	z = v1->z;

	vr->x = I->xx*x + I->xy*y + I->zx*z;
	vr->y = I->xy*x + I->yy*y + I->yz*z;
	vr->z = I->zx*x + I->yz*y + I->zz*z;

	return (vr);
}

foo(jtor, load, jvel, jacc, kyn, jls)
float *jtor;
FORCE *load;
float *jvel;
float *jacc;
KYN *kyn;
JLS *jls;
{
	register int j;
	register DIFF *v;
	register DIFF *a;
	register FORCE *F;
	register FORCE *f;
	TRSF A;

	DIFF vel[MAXJNTS+1];
	DIFF acc[MAXJNTS+1];
	FORCE force[MAXJNTS];
	FORCE floc[MAXJNTS];
	VECT pvec[MAXJNTS];

	/* initialize terms for link 0: */

	bzero (&vel[0], sizeof(DIFF));
	bzero (&acc[0], sizeof(DIFF));
	v = &vel;
	a = &acc;
	
	for (j=0; j<jls->numJ; j++)
	 { VECT atmp, vtmp, cross;

	   v++; a++;
	   linkTrsf (&A, j, kyn);
	   pvec[j] = A.p;

	   if (jls->jtype[j] == 'R')	/* rotational joint */
	    { 
	      /* compute local angular velocity and acceleration */

	      vtmp.x = v[-1].r.x;
	      vtmp.y = v[-1].r.y;
	      vtmp.z = v[-1].r.z + jvel[j];

	      atmp.x = a[-1].r.x + a[-1].r.y*jacc[j];
	      atmp.y = a[-1].r.y - a[-1].r.x*jacc[j];
	      atmp.z = a[-1].r.z + jvel[j];

	      multLiRotVect (&v[0].r, &A, &vtmp);
	      multLiRotVect (&a[0].r, &A, &atmp);

	      /* compute local translational velocity and acceleration */

	      multLiRotVect (&v[0].t, &A, &v[-1].t);	      
	      addVect (&v[0].t, &v[0].t, crossVect (&cross,&v[0].r,&pvec[j]));

	      multLiRotVect (&a[0].t, &A, &a[-1].t);
	      addVect (&a[0].t, &a[0].t, crossVect (&cross,&v[0].r,&cross));
	      addVect (&a[0].t, &a[0].t, crossVect (&cross,&a[0].r,&pvec[j]));
	    }
	   else				/* prismatic joint */
	    { 
	      /* compute local angular velocity and acceleration */

	      multLiRotVect (&v[0].r, &A, &v[-1].r);
	      multLiRotVect (&a[0].r, &A, &a[-1].r);

	      /* compute local translational velocity and acceleration */

	      vtmp.x = v[-1].t.x;
	      vtmp.y = v[-1].t.y;
	      vtmp.z = v[-1].t.z + jvel[j];

	      multLiRotVect (&v[0].t, &A, &vtmp);
	      addVect (&v[0].t, &v[0].t, crossVect (&cross,&v[0].r,&pvec[j]));

	      atmp.x = a[-1].r.x;
	      atmp.y = a[-1].r.y;
	      atmp.z = a[-1].r.z + jvel[j];

	      multLiRotVect (&a[0].t, &A, &atmp);
	      addVect (&a[0].t, &a[0].t, crossVect (&cross,&v[0].r,&cross));
	      addVect (&a[0].t, &a[0].t, crossVect (&cross,&a[0].r,&pvec[j]));
	      crossVect (&cross, &v[0].r, &A.a);

	      a[0].t.x += 2 * jacc[j] * cross.x;
	      a[0].t.y += 2 * jacc[j] * cross.y;
	      a[0].t.z += 2 * jacc[j] * cross.z;
	    }

	  /* adjust translational accel. so that is WRT the center of mass */

	  crossVect (&cross, &a[0].r, &kyn->gen->cmv[j])
	  addVect (&a[0].t, &a[0].t, &cross);
	  crossVect (&cross, &v[0].r, &kyn->gen->cmv[j]);
	  crossVect (&cross, &v[0].r, &cross);
	  addVect (&a[0].t, &a[0].t, &cross);

	  /* compute translational force */

	  scaleVect (&force[j].f, kyn->gen->mass[j], &a[0].t);

	  /* compute torque */

	  multInertiaVect (&force[j].m, &kyn->gen->inertia[j], &a[0].r);
	  multInertiaVect (&cross, &kyn->gen->inertia[j], &v[0].r);
	  addVect (&force[j].m, &force[j].m, 
				crossVect (&cross, &v[0].r, &cross);
	 }

	/* now initialize the load vectors */

	floc[jls->numJ] = *load;
	
	F = &force[jls->numJ];
	f = &floc[jls->numJ];

	identTrsf (&A);

	for (j=jls->numJ-1; j>=0; j--)
	 { VECT cross;
	   VECT mtmp;

	   f--; F--;

	   multRotVect (&f[0].f, &A, &f[1].f);
	   addVect (&f[0].f, &f[0].f, &F[0].f);

	   multLiRotVect (&cross, &A, &pvec[j]);
	   crossVect (&cross, &cross, &f[1].f);
	   addVect (&cross, &cross, &f[1].m);
	   multRotVect (&f[0].m, &A, &cross);
	   addVect (&cross, &pvec[j], &kyn->gen->cmv[j]);
	   crossVect (&cross, &cross, &F[0].f);
	   addVect (&f[0].m, &cross);
	   addVect (&f[0].m, &F[0].m);

	   if (jls->jtype[j] == 'R')
	    { jtor[j] = dotVect (&f[0].m, &A.a);
	    }
	   else
	    { jtor[j] = dotVect (&f[0].f, &A.a);
	    }
	   linkTrsf (&A, j, kyn);
	 }
}
