/*-=-=-=-=-=-=-=-=-=-=-=- 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 <stdio.h>
#include <math.h>

#include "../h/cdefs.h"
#include "../h/robotClass.h"
#include "../h/puma_kynvar.h"
#include "../h/errorCodes.h"
#include "../h/robotMath.h"

#define EPSILONF 	1.0e-7

puma_kyn (kyn, fp)
KYN *kyn;
FILE *fp;
{
	PUMA_KYN *pkyn = (PUMA_KYN*)kyn->ext;
	ConfCodeDesc *desc;

	int oldCoordDefs;
	float a2, d3, a3, d4;
	float cp20, cp21, cp22, cp30, cp50;

	char *fxnName = "puma_kyn()";

	readInit(fp,"=");
	if (readExactString("OLD") && readExactString("COORDINATES"))
	 { if (!readExactString("=") || !readInteger(&oldCoordDefs))
	    { errorMsg (EBadFile,
"%s -- can't read OLD COORDINATES setting\n", fxnName);
	      return (-1);
	    }
	 }
	else
	 { oldCoordDefs = 0;
	 }
	if (!readExactString("A2") || !readExactString("=") ||
	    !readFloat(&a2) ||
	    !readExactString("D3") || !readExactString("=") ||
	    !readFloat(&d3) ||
	    !readExactString("A3") || !readExactString("=") || 
	    !readFloat(&a3) ||
	    !readExactString("D4") || !readExactString("=") ||
	    !readFloat(&d4))
	 { errorMsg (EBadFile, 
"%s -- can't read `A' matrix parameters\n", fxnName);
	   return (-1);
	 }
	if (!readExactString("CP20") || !readExactString("=") ||
	    !readFloat(&cp20) ||
	    !readExactString("CP21") || !readExactString("=") ||
	    !readFloat(&cp21) ||
	    !readExactString("CP22") || !readExactString("=") || 
	    !readFloat(&cp22) ||
	    !readExactString("CP30") || !readExactString("=") || 
	    !readFloat(&cp30) ||
	    !readExactString("CP50") || !readExactString("=") || 
	    !readFloat(&cp50))
	 { errorMsg (EBadFile, 
"%s -- can't read gravity coefficients\n", fxnName);
	   return (-1);
	 }

	pkyn->oldCoordDefs = oldCoordDefs;

	kyn->gen->dh[1].a = a2;
	kyn->gen->dh[2].d = d3;
	kyn->gen->dh[2].a = a3;
	kyn->gen->dh[3].d = d4;
     
	if (pkyn->oldCoordDefs)
	 { kyn->gen->dh[0].alpha = -PI/2;
	   kyn->gen->dh[2].alpha =  PI/2;
	   kyn->gen->dh[3].alpha = -PI/2;
	   kyn->gen->dh[4].alpha =  PI/2;
	 }
	else
	 { kyn->gen->dh[0].alpha =  PI/2;
	   kyn->gen->dh[2].alpha = -PI/2;
	   kyn->gen->dh[3].alpha =  PI/2;
	   kyn->gen->dh[4].alpha = -PI/2;
	 }
	pkyn->d32 = d3*d3;
	pkyn->a22_a32d42 = a2*a2-a3*a3-d4*d4;
	pkyn->atana3d4 = atan2(a3,d4);
	pkyn->sqrt_a32d42 = sqrt(a3*a3+d4*d4);
	pkyn->d42a22a32 = d4*d4 + a2*a2 + a3*a3;
	pkyn->d32a222d422a32 = d3*d3 + a2*a2 + 2*d4*d4 + 2*a3*a3;
	pkyn->epsilon_a2d4 = (a2+d4)/2.0*EPSILONF;
	pkyn->epsilon_d32 = d3*d3*EPSILONF;

	pkyn->cp20 = cp20;
	pkyn->cp21 = cp21;
	pkyn->cp30 = cp30;
	pkyn->cp50 = cp50;	

	/* now read the friction values */

	{ static char *labels[3] = { "STATF", "COULF", "VISCF" };
	  float *vector[3];
	  char fmt[64];
	  int k, j;

	  vector[0] = kyn->gen->statFriction;
	  vector[1] = kyn->gen->coulFriction;
	  vector[2] = kyn->gen->viscFriction;
	  for (k=0; k<3; k++)
	   { for (j=0; j<PUMAJ; j++)
	      { sprintf (fmt, "%s%d", labels[k], j+1);
		if (!readExactString(fmt) || !readExactString("=") ||
		    !readFloat (&((vector[k])[j])) )
		 { errorMsg (EBadFile,
"%s -- can't read %s for joint %d\n", fxnName, labels[k], j+1);
		   return (-1);
		 }
	      }
	   }
	}
	
	/* ---- Configuration codes ---- */

	kyn->gen->numConf = 3;
	desc = kyn->gen->confCodes;
	desc[0].code  = PUMA_RIGHT_CONF;
	desc[0].set   = 'r';
	desc[0].clear = 'l';
	desc[1].code  = PUMA_DOWN_CONF;
	desc[1].set   = 'd';
	desc[1].clear = 'u';
	desc[2].code  = PUMA_FLIP_CONF;
	desc[2].set   = 'f';
	desc[2].clear = 'n';
	
	/* ---- Instantiate function fields ---- */

	kyn->gen->fwdKinematics = pumaFwdKinematics;
	kyn->gen->invKinematics = pumaInvKinematics;
	kyn->gen->rangeAngles = pumaRangeAngles;
	kyn->gen->solveConf = pumaSolveConf;
	kyn->gen->fwdJacob = pumaFwdJacob;
	kyn->gen->invJacob = pumaInvJacob;
	kyn->gen->fwdJacobT = pumaFwdJacobT;
	kyn->gen->invJacobT = pumaInvJacobT;
	kyn->gen->gravload = pumaGravload;
	kyn->gen->fwdJacobVar = pumaFwdJacobVar;
	kyn->gen->invJacobVar = pumaInvJacobVar;
	kyn->gen->fwdJacobTVar = pumaFwdJacobTVar;
	kyn->gen->invJacobTVar = pumaInvJacobTVar;
	kyn->gen->gravloadVar = pumaGravloadVar;
	kyn->gen->updateVar = pumaUpdateVar;
	kyn->gen->updateVarXsincos = pumaUpdateVarXsincos;

	kyn->gen->class = PUMA;

	return (1);
}
