/*-=-=-=-=-=-=-=-=-=-=-=- 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/stan_kynvar.h"
#include "../h/errorCodes.h"
#include "../h/robotMath.h"

#define EPSILONF 	1.0e-7

stan_kyn (kyn, fp)
KYN *kyn;
FILE *fp;
{
	STAN_KYN *skyn = (STAN_KYN*)kyn->ext;
	ConfCodeDesc *desc;

	float d2, d3Rng;
	float cp50, cp40, cp31, cp30, cp20;

	char *fxnName = "stan_kyn()";

	readInit(fp,"=");

	if (!readExactString("D2") || !readExactString("=") ||
	    !readFloat(&d2) ||
	    !readExactString("D3_RNG") || !readExactString("=") ||
	    !readFloat(&d3Rng))
	 { errorMsg (EBadFile, 
"%s -- can't read `A' matrix parameters\n", fxnName);
	   return (-1);
	 }
	if (!readExactString("CP20") || !readExactString("=") || 
	    !readFloat(&cp20) ||
	    !readExactString("CP30") || !readExactString("=") || 
	    !readFloat(&cp30) ||
	    !readExactString("CP31") || !readExactString("=") || 
	    !readFloat(&cp31) ||
	    !readExactString("CP40") || !readExactString("=") || 
	    !readFloat(&cp40) ||
	    !readExactString("CP50") || !readExactString("=") || 
	    !readFloat(&cp50))
	 { errorMsg (EBadFile, 
"%s -- can't read gravity coefficients\n", fxnName);
	   return (-1);
	 }
	
	kyn->gen->dh[1].d = d2;

	kyn->gen->dh[0].alpha = -PI/2;
	kyn->gen->dh[1].alpha =  PI/2;
	kyn->gen->dh[3].alpha = -PI/2;
	kyn->gen->dh[4].alpha =  PI/2;

	skyn->d3Rng = d3Rng;

       	skyn->cp20 = cp20;
	skyn->cp30 = cp30;
	skyn->cp31 = cp31;
	skyn->cp40 = cp40;
	skyn->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<STANJ; 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  = STAN_RIGHT_CONF;
	desc[0].set   = 'r';
	desc[0].clear = 'l';
	desc[1].code  = STAN_DOWN_CONF;
	desc[1].set   = 'd';
	desc[1].clear = 'u';
	desc[2].code  = STAN_FLIP_CONF;
	desc[2].set   = 'f';
	desc[2].clear = 'n';
	
	/* ---- Instantiate function fields ---- */

	kyn->gen->fwdKinematics = stanFwdKinematics;
	kyn->gen->invKinematics = stanInvKinematics;
	kyn->gen->rangeAngles = stanRangeAngles;
	kyn->gen->solveConf = stanSolveConf;
	kyn->gen->fwdJacob = stanFwdJacob;
	kyn->gen->invJacob = stanInvJacob;
	kyn->gen->fwdJacobT = stanFwdJacobT;
	kyn->gen->invJacobT = stanInvJacobT;
	kyn->gen->gravload = stanGravload;
	kyn->gen->fwdJacobVar = stanFwdJacobVar;
	kyn->gen->invJacobVar = stanInvJacobVar;
	kyn->gen->fwdJacobTVar = stanFwdJacobTVar;
	kyn->gen->invJacobTVar = stanInvJacobTVar;
	kyn->gen->gravloadVar = stanGravloadVar;
	kyn->gen->updateVar = stanUpdateVar;
	kyn->gen->updateVarXsincos = stanUpdateVarXsincos;

	kyn->gen->class = STAN;

	return (1);
}
