/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"

encoderToAngle (ang, enc, jls)
register float ang[];
register int enc[];
register JLS *jls;
{
	int e[MAXJNTS];
	int nj = jls->numJ;
	int nc = jls->numIC;

	register int j, k;
	register int *c = (int *)jls->invCouplings;

	for (j=0; j<nj; j++)
	 { ang[j] = jls->enctoang[j] * (e[j] = enc[j] - jls->encOffset[j]);
	 }
	for (k=0; k<nc; k++)
	 { ang[c[0]] += jls->enctoang[j++] * e[c[1]];
	   c += 2;
	 }
}

angleToEncoder (enc, ang, jls)
register int enc[];
register float ang[];
register JLS *jls;
{
	float val;
	int nj = jls->numJ;
	int nc = jls->numFC;

	register int j, k;
	register int *c = (int *)jls->forCouplings;

	for (j=0; j<nj; j++)
	 { val = jls->angtoenc[j] * ang[j] + jls->encOffset[j];
	   enc[j] = NINT(val);
	 }
	for (k=0; k<nc; k++)
	 { enc[c[0]] += jls->angtoenc[j++] * ang[c[1]];
	   c += 2;
	 }
}

encoderToJoint (ang, enc, jls)
register float ang[];
register int enc[];
register JLS *jls;
{
	int nj = jls->numJ;
	int nc = jls->numIC;

	register int j, k;
	register int *c = (int *)jls->invCouplings;

	for (j=0; j<nj; j++)
	 { ang[j] = jls->enctoang[j] * enc[j];
	 }
	for (k=0; k<nc; k++)
	 { ang[c[0]] += jls->enctoang[j++] * enc[c[1]];
	   c += 2;
	 }
}

jointToEncoder (enc, ang, jls)
register int enc[];
register float ang[];
register JLS *jls;
{
	float val;
	int nj = jls->numJ;
	int nc = jls->numFC;

	register int j, k;
	register int *c = (int *)jls->forCouplings;

	for (j=0; j<nj; j++)
	 { val = jls->angtoenc[j] * ang[j];
	   enc[j] = NINT(val);
	 }
	for (k=0; k<nc; k++)
	 { enc[c[0]] += jls->angtoenc[j++] * ang[c[1]];
	   c += 2;
	 }
}

mtorqueToJtorque (jt, mt, jls)
register float jt[];
register float mt[];
JLS *jls;
{
	int nj = jls->numJ;
	int nc = jls->numFC;

	register int j, k;
	register int *c = (int *)jls->forCouplings;

	for (j=0; j<nj; j++)
	 { jt[j] = jls->forGearRatios[j] * mt[j];
	 }
	for (k=0; k<nc; k++)
	 { jt[c[1]] += jls->forGearRatios[j++] * mt[c[0]];
	   c += 2;
	 }
}

jtorqueToMtorque (mt, jt, jls)
register float mt[];
register float jt[];
JLS *jls;
{
	int nj = jls->numJ;
	int nc = jls->numIC;

	register int j, k;
	register int *c = (int *)jls->invCouplings;

	for (j=0; j<nj; j++)
	 { jt[j] = jls->invGearRatios[j] * mt[j];
	 }
	for (k=0; k<nc; k++)
	 { jt[c[1]] += jls->invGearRatios[j++] * mt[c[0]];
	   c += 2;
	 }
}

radianToDegree (deg, rad, jls)
float *deg, *rad;
JLS *jls;
{
	register int j;

	for (j=0; j<jls->numJ; j++)
	 { if (jls->jtype[j] == 'R')
	    { deg[j] = RADTODEG*rad[j];
	    }
	   else
	    { deg[j] = rad[j];
	    }
	 }
}

degreeToRadian (rad, deg, jls)
float *rad, *deg;
JLS *jls;
{
	register int j;

	for (j=0; j<jls->numJ; j++)
	 { if (jls->jtype[j] == 'R')
	    { rad[j] = DEGTORAD*deg[j];
	    }
	   else
	    { rad[j] = deg[j];
	    }
	 }
}
