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

#define EPSILONF 	1.0e-7

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

	float a1, a2;
	float cp30, cp50;

	char *fxnName = "scara_kyn()";

	readInit(fp,"=");

	if (!readExactString("A1") || !readExactString("=") ||
	    !readFloat(&a1) ||
	    !readExactString("A2") || !readExactString("=") ||
	    !readFloat(&a2))
	 { errorMsg (EBadFile, 
"%s -- can't read `A' matrix parameters\n", fxnName);
	   return (-1);
	 }
	if (!readExactString("CP30") || !readExactString("=") || 
	    !readFloat(&cp30) ||
	    !readExactString("CP50") || !readExactString("=") || 
	    !readFloat(&cp50))
	 { errorMsg (EBadFile, 
"%s -- can't read gravity coefficients\n", fxnName);
	   return (-1);
	 }
	
	kyn->gen->dh[0].a = a1;
	kyn->gen->dh[1].a = a2;

	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->cp30 = cp30;
	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<SCARAJ; 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 = 2;
	desc = kyn->gen->confCodes;
	desc[0].code  = SCARA_RIGHT_CONF;
	desc[0].set   = 'r';
	desc[0].clear = 'l';
	desc[2].code  = SCARA_FLIP_CONF;
	desc[2].set   = 'f';
	desc[2].clear = 'n';
	
	/* ---- Instantiate function fields ---- */

	kyn->gen->fwdKinematics = scaraFwdKinematics;
	kyn->gen->invKinematics = scaraInvKinematics;
	kyn->gen->rangeAngles = scaraRangeAngles;
	kyn->gen->solveConf = scaraSolveConf;
	kyn->gen->fwdJacob = scaraFwdJacob;
	kyn->gen->invJacob = scaraInvJacob;
	kyn->gen->fwdJacobT = scaraFwdJacobT;
	kyn->gen->invJacobT = scaraInvJacobT;
	kyn->gen->gravload = scaraGravload;
	kyn->gen->fwdJacobVar = scaraFwdJacobVar;
	kyn->gen->invJacobVar = scaraInvJacobVar;
	kyn->gen->fwdJacobTVar = scaraFwdJacobTVar;
	kyn->gen->invJacobTVar = scaraInvJacobTVar;
	kyn->gen->gravloadVar = scaraGravloadVar;
	kyn->gen->updateVar = scaraUpdateVar;
	kyn->gen->updateVarXsincos = scaraUpdateVarXsincos;

	kyn->gen->class = SCARA;

	return (1);
}
