/*-=-=-=-=-=-=-=-=-=-=-=- 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/robotMath.h"
#include "../h/robot_jls.h"
#include "../h/robot_kynvar.h"
#include "../h/robotTypes.h"

#include "manipIO.h"
#include "robotsim.h"

#define NmToNmm 1000.0
#define NmmToNm 0.001

#define EPSILON 1.0e-7

/* ---- routines for setting up and defining the manipulator ---- */

initManip (r, name, joints)
RobotDesc *r;
char *name;
float *joints;
{
	int j;

	r->rs.commandRate = 28;

	/* ---- assume we start of with the park angles ---- */

	if (!joints)
	 { joints = r->jls->parkAngles;
	 }

	r->jls->angleToEncoder (r->rs.enc, joints, r->jls);

	for (j=0; j<r->jls->numJ; j++)
	 { r->rs.angles[j] = joints[j];
	   r->rs.jDACvalues [j] = 0;
	   r->rs.mtorque[j] = 0.0;
	   r->rs.mode [j] = (MESERVO | MEPOSSV);
	   r->rs.stat [j] = (JRSERVO | JRPOSSV);
	   r->rs.ctrl [j] = HOLD;
	   r->rs.com [j] = 0;
	   setOneEncoder (r, j, r->rs.enc[j]);
	 }
	identTrsf (&r->rs.tool);
	bzero (&r->rs.extForce, sizeof(FORCE));
	bzero (&r->rs.extPosErr, sizeof(DIFF));
	r->rs.power = 0;
	
	updateVar (r->var, r->rs.angles, r->kyn);
	gravloadVar (r->rs.gravload, r->var, r->kyn);

	return (1);
}

manipStateCompute(r)
RobotDesc *r;
{
	register int j;
	int current[6];

	/* for now, just copy command 'com' into 'ctrl' */

	for (j=0; j<6; j++)
	 { if (r->rs.com[j])
	    { r->rs.ctrl[j] = r->rs.com[j];
	      r->rs.com[j] = 0;
	    }
	   if (r->rs.ctrl[j] == POSITION)
	    { r->rs.enc[j] = r->rs.setp[j];
	    }
	   else if (r->rs.ctrl[j] == CURRENT)
	    { 
	      /* --- don't change encoder values --- */
	    }
	 }

	/* PUT FORWARD MODEL HERE .... 
	   r->rs.com is the command
	   r->rs.setp is the associated value
	   r->rs.ctrl is our private copy of the command 
	   r->rs.enc is the result of the command in encoder values 
	 */

	r->jls->encoderToAngle (r->rs.angles, r->rs.enc, r->jls);

	/* ----- determine effects from applied force and differences ---- */

	 { TRSF toolI;
	   FORCE ft6;
	   DIFF dt6;
	   float appliedTorque[MAXJNTS];
	   float deltaJoints[MAXJNTS];
	   float jtorque[MAXJNTS];

	   invertTrsf (&toolI, &r->rs.tool);
	   transForce (&ft6, &r->rs.extForce, &toolI);
	   transDiff (&dt6, &r->rs.extPosErr, &toolI);

	   r->jls->encoderToAngle (r->rs.angles, r->rs.enc, r->jls);
	   updateVar (r->var, r->rs.angles, r->kyn);

	   invJacobVar (deltaJoints, &dt6, r->var, r->kyn, 0.1);
	   addVf (r->rs.angles, r->rs.angles, deltaJoints, 6);

	   r->jls->angleToEncoder (r->rs.enc, r->rs.angles, r->jls);

	   fwdJacobTVar (appliedTorque, &ft6, r->var, r->kyn);

	   gravloadVar (r->rs.gravload, r->var, r->kyn);

	   combineVf (jtorque,-NmmToNm, appliedTorque,1.0, r->rs.gravload, 6);

	   r->jls->jtorqueToMtorque (r->rs.mtorque, jtorque, r->jls);
	   for (j=0; j<r->jls->numJ; j++)
	    { current[j] = r->jls->mtorqueToCurrent[j]*r->rs.mtorque[j];
	      if (r->rs.ctrl[j] == CURRENT)
	       { r->rs.mtorque[j] = 
			(r->rs.setp[j]-r->jls->currentBias[j])/
			 r->jls->mtorqueToCurrent[j];
	       }
	    }
	 }	
	   
	for (j=0; j<r->jls->numJ; j++)
	 { 
	   if (!r->rs.power)
	    { r->rs.jDACvalues[j] = 0;
	    }
	   else if (r->rs.ctrl[j] == POSITION)
	    { r->rs.jDACvalues[j] = current[j]+r->jls->currentBias[j];
	    }
	   else if (r->rs.ctrl[j] == CURRENT)
	    { r->rs.jDACvalues[j] = r->rs.setp[j];
	    }
	 }
}

manipFwdKinematics (r, t6, conf, ang)
RobotDesc *r;
TRSF *t6;
char *conf;
float *ang;
{
	unsigned long confcode;

	fwdKinematics (t6, &confcode, ang, r->kyn, (void*)0);
	configToStr (conf, confcode, r->kyn);	
}
