#include "../h/cdefs.h"
#include "../h/robotTypes.h"
#include "../h/robotMath.h"
#include "../h/robotClass.h"
#include "../h/robot_jls.h"
#include "../h/robot_kynvar.h"

fwdKinematics (t6, conf, j6, kyn, var)
TRSF *t6;
unsigned long *conf;
float *j6;
KYN *kyn;
void *var;
{
	return (kyn->gen->fwdKinematics (t6, conf, j6, kyn, var));
}

invKinematics (j6, t6, conf, refj6, kyn, jls, var)
float *j6;
TRSF *t6;
unsigned long conf;
float *refj6;
KYN *kyn;
void *var;
JLS *jls;
{
	return (kyn->gen->invKinematics(j6,t6,conf,refj6,kyn,jls,var));
}

rangeAngles (jr, j1, jref, kyn, jls)
float *jr;
float *j1;
float *jref;
KYN *kyn;
JLS *jls;
{
	return (kyn->gen->rangeAngles(jr, j1, jref, kyn, jls));
}

solveConf (confp, j6, kyn)
unsigned long *confp;
float *j6;
KYN *kyn;
{
	return (kyn->gen->solveConf (confp, j6, kyn));
}
	
fwdJacob (dc, dj, j6, kyn)
DIFF *dc;
float *dj;
float *j6;
KYN *kyn;
{
	return (kyn->gen->fwdJacob (dc, dj, j6, kyn));
}

invJacob (dj, dc, j6, kyn, epsilon)
float *dj;
DIFF *dc;
float *j6;
KYN *kyn;
double epsilon;
{
	return (kyn->gen->invJacob (dj, dc, j6, kyn, epsilon));
}

fwdJacobT (t, f, j6, kyn)
float *t;
FORCE *f;
float *j6;
KYN *kyn;
{
	return (kyn->gen->fwdJacobT (t, f, j6, kyn));
}

invJacobT (f, t, j6, kyn, epsilon)
FORCE *f;
float *t;
float *j6;
KYN *kyn;
double epsilon;
{
	return (kyn->gen->invJacobT (f, t, j6, kyn, epsilon));
}

gravload (load, j6, kyn)
float *load;
KYN *kyn;
float *j6;
{
	return (kyn->gen->gravload (load, j6, kyn));
}

updateVar (var, angles, kyn)
void *var;
float *angles;
KYN *kyn;
{
	return (kyn->gen->updateVar (var, angles, kyn));
}

updateVarXsincos (var, angles, kyn)
void *var;
float *angles;
KYN *kyn;
{
	return (kyn->gen->updateVarXsincos (var, angles, kyn));
}

fwdJacobVar (dc, dj, var, kyn)
DIFF *dc;
float *dj;
void *var;
KYN *kyn;
{
	return (kyn->gen->fwdJacobVar (dc, dj, var, kyn));
}

invJacobVar (dj, dc, var, kyn, epsilon)
float *dj;
DIFF *dc;
void *var;
KYN *kyn;
double epsilon;
{
	return (kyn->gen->invJacobVar (dj,dc,var,kyn,epsilon));
}

fwdJacobTVar (t, f, var, kyn)
float *t;
FORCE *f;
void *var;
KYN *kyn;
{
	return (kyn->gen->fwdJacobTVar (t, f, var, kyn));
}

invJacobTVar (f, t, var, kyn, epsilon)
FORCE *f;
float *t;
void *var;
KYN *kyn;
double epsilon;
{
	return (kyn->gen->invJacobTVar (f, t, var, kyn, epsilon));
}

gravloadVar (load, var, kyn)
float *load;
KYN *kyn;
void *var;
{
	return (kyn->gen->gravloadVar (load, var, kyn));
}

unsigned long strToConfig (confp, str, kyn)
unsigned long *confp;
const char *str;
KYN *kyn;
{
	register ConfCodeDesc *desc;
	unsigned long set, clear;
	int numConf;
	int c, j;

	numConf = kyn->gen->numConf;
	if (strlen(str) > numConf)
	 { return (-1);
	 }
	
	set = clear = 0;
	while (c = *str++)
	 { desc = kyn->gen->confCodes;
	   for (j=0; j<numConf; j++)
	    { if (c == desc->set)
	       { if (desc->code & (set | clear))
		  { return (-1);	/* already specified */
		  }
		 else
		  { set |= desc->code;
		    break;
		  }
	       }
	      else if (c == desc->clear)
	       { if (desc->code & (set | clear))
		  { return (-1);	/* already specified */
		  }
		 else
		  { clear |= desc->code;
		    break;
		  }
	       }
	      desc++;
	    }
	   if (j == numConf)		/* invalid character */
	    { return (-1);
	    }
	 }

	*confp = set;
	return (set | clear);
}
	
char *configToStr (str, conf, kyn)
char *str;
unsigned long conf;
KYN *kyn;
{
	register ConfCodeDesc *desc;
	int numConf;
	int i;

	numConf = kyn->gen->numConf;
	desc = kyn->gen->confCodes;

	for (i=0; i<numConf; i++)
	 { if (conf & desc->code)
	    { str[i] = desc->set;
	    }
	   else 
	    { str[i] = desc->clear;
	    }
	   desc++;
	 }
	str[numConf] = '\0';
	return (str);
}

