/*-=-=-=-=-=-=-=-=-=-=-=- 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/robot_jls.h"

puma_encoderToAngle (a, e, jls)
register float a[];
register int e[];
register JLS *jls;
{
	register int e3, e4;

	a[0] = jls->enctoang[0] * (e[0] - jls->encOffset[0]);
	a[1] = jls->enctoang[1] * (e[1] - jls->encOffset[1]);
	a[2] = jls->enctoang[2] * (e[2] - jls->encOffset[2]);

	e3 = e[3] - jls->encOffset[3];
 	e4 = e[4] - jls->encOffset[4];

	a[3] = jls->enctoang[3] * e3;
	a[4] = jls->enctoang[6] * e3 +
	       jls->enctoang[4] * e4;
	a[5] = jls->enctoang[7] * e3 +
	       jls->enctoang[8] * e4 +
	       jls->enctoang[5] * (e[5] - jls->encOffset[5]);
}

puma_angleToEncoder (e, a, jls)
register int e[];
register float a[];
register JLS *jls;
{
	/* we add in the 0.5 so that we will round to nearest int;
	   doing this assumes that encoder values are always positive */

	e[0] = jls->angtoenc[0] * a[0] + 0.5 + jls->encOffset[0];
	e[1] = jls->angtoenc[1] * a[1] + 0.5 + jls->encOffset[1];
	e[2] = jls->angtoenc[2] * a[2] + 0.5 + jls->encOffset[2];
	e[3] = jls->angtoenc[3] * a[3] + 0.5 + jls->encOffset[3];
	e[4] = jls->angtoenc[6] * a[3] + 
	       jls->angtoenc[4] * a[4] + 0.5 + jls->encOffset[4];
	e[5] = jls->angtoenc[7] * a[3] +
	       jls->angtoenc[8] * a[4] +
	       jls->angtoenc[5] * a[5] + 0.5 + jls->encOffset[5];
}

puma_encoderToJoint (a, e, jls)
register float a[];
register int e[];
register JLS *jls;
{
	a[0] = jls->enctoang[0] * e[0];
	a[1] = jls->enctoang[1] * e[1];
	a[2] = jls->enctoang[2] * e[2];
	a[3] = jls->enctoang[3] * e[3];
	a[4] = jls->enctoang[6] * e[3] +
	       jls->enctoang[4] * e[4];
	a[5] = jls->enctoang[7] * e[3] +
	       jls->enctoang[8] * e[4] +
	       jls->enctoang[5] * e[5];
}

puma_jointToEncoder (e, a, jls)
register int e[];
register float a[];
register JLS *jls;
{
	/* we add in the 0.5 so that we will round to nearest int;
	   doing this assumes that encoder values are always positive */

	e[0] = jls->angtoenc[0] * a[0] + 0.5;
	e[1] = jls->angtoenc[1] * a[1] + 0.5;
	e[2] = jls->angtoenc[2] * a[2] + 0.5;
	e[3] = jls->angtoenc[3] * a[3] + 0.5;
	e[4] = jls->angtoenc[6] * a[3] +
	       jls->angtoenc[4] * a[4] + 0.5;
	e[5] = jls->angtoenc[7] * a[3] +
	       jls->angtoenc[8] * a[4] +
	       jls->angtoenc[5] * a[5] + 0.5;
}

puma_mtorqueToJtorque (jt, mt, jls)
register float jt[];
register float mt[];
register JLS *jls;
{
	jt[0] = mt[0] * jls->forGearRatios[0];
	jt[1] = mt[1] * jls->forGearRatios[1];
	jt[2] = mt[2] * jls->forGearRatios[2];
	jt[3] = mt[3] * jls->forGearRatios[3] +
		mt[4] * jls->forGearRatios[6] +
		mt[5] * jls->forGearRatios[7];
	jt[4] = mt[4] * jls->forGearRatios[4] +
	 	mt[5] * jls->forGearRatios[8];
	jt[5] = mt[5] * jls->forGearRatios[5];
}

puma_jtorqueToMtorque (mt, jt, jls)
register float mt[];
register float jt[];
register JLS *jls;
{
	mt[0] = jt[0] / jls->forGearRatios[0];
	mt[1] = jt[1] / jls->forGearRatios[1];
	mt[2] = jt[2] / jls->forGearRatios[2];

	mt[5] = jt[5] / jls->forGearRatios[5];
	mt[4] = (jt[4] - mt[5] * jls->forGearRatios[8]) / jls->forGearRatios[4];
	mt[3] = (jt[3] - mt[5] * jls->forGearRatios[7]
		       - mt[4] * jls->forGearRatios[6]) / jls->forGearRatios[3];
}
