#ifndef _ROBOT_KYNVAR_H_
#define _ROBOT_KYNVAR_H_

#include "../h/robotTypes.h"
#include "../h/robot_jls.h"

/* These codes are returned by the invKinematics() routine */

#define KYN_JOINT_LIMIT	0x0000ff	/* bits to indicate joint limits */
#define KYN_SINGULAR	0x01ff00	/* bits to indicate singularities */
#define KYN_WRIST_SING	0x010000	/* the 'wrist' singularity */
#define KYN_CANT_REACH	0x020000	/* solution is out of reach */

#define KYN_MAXCONF	8		/* Max. no. of kinematic configs */

#ifndef PROTO
#ifndef NO_PROTOTYPES
#define PROTO(x) x
#else
#define PROTO(x) ()
#endif
#endif

/* Generic function prototypes for the kinematic/dynamic routines */

#define FWDKINEMATICS_PROTO \
	TRSF*,\
	unsigned long*,\
	float*,\
	KYN*,\
	void*

#define INVKINEMATICS_PROTO \
	float*,\
	TRSF*,\
	unsigned long,\
	float*,\
	KYN*,\
	JLS*,\
	void*

#define RANGEANGLES_PROTO \
	float*,\
	float*,\
	float*,\
	KYN*,\
	JLS*

#define SOLVECONF_PROTO \
	unsigned long*,\
	float*,\
	KYN*

#define STRTOCONFIG_PROTO \
	unsigned long*,\
	const char*,\
	KYN*

#define CONFIGTOSTR_PROTO \
	char*,\
	unsigned long,\
	KYN*

#define FWDJACOB_PROTO \
	DIFF*,\
	float*,\
	float*,\
	KYN*

#define INVJACOB_PROTO \
	float*,\
	DIFF*,\
	float*,\
	KYN*,\
	double

#define FWDJACOBT_PROTO \
	float*,\
	FORCE*,\
	float*,\
	KYN*

#define INVJACOBT_PROTO \
	FORCE*,\
	float*,\
	float*,\
	KYN*,\
	double

#define GRAVLOAD_PROTO \
	float*,\
	float*,\
	KYN*

#define UPDATEVAR_PROTO \
	void*,\
	float*,\
	KYN*

#define UPDATEVARXSINCOS_PROTO \
	void*,\
	float*,\
	KYN*

#define FWDJACOBVAR_PROTO \
	DIFF*,\
	float*,\
	void*,\
	KYN*

#define INVJACOBVAR_PROTO \
	float*,\
	DIFF*,\
	void*,\
	KYN*,\
	double

#define FWDJACOBTVAR_PROTO \
	float*,\
	FORCE*,\
	void*,\
	KYN*

#define INVJACOBTVAR_PROTO \
	FORCE*,\
	float*,\
	void*,\
	KYN*,\
	double

#define GRAVLOADVAR_PROTO \
	float*,\
	void*,\
	KYN*

typedef struct {
	unsigned long code;
	char set, clear;
} ConfCodeDesc;

typedef struct _INERTIA {
	/* Describes the inertia matrix for a rigid body */

	float xx, yy, zz;	/* diaonal interia terms */
	float xy, xz, yz;	/* off diagonal inertia terms */
} INERTIA;

typedef struct _DH_PARAMS {
	/* Describes the Denavit-Hartenburg parameters for a robot link */

	float theta;		/* rotation about previous z axis */
	float d;		/* translation along previous z axis */
	float a;		/* translation along old/new x axis */
	float alpha;		/* rotation about old/new x axis */

  } DH_PARAMS;

typedef struct _GEN_KYN {

	/* This structure contains function pointers for robot
	   specific kinematic and dynamic computation routines.
	   It must be the first element in every robot's VAR structure.
	*/
	
	int (*fwdKinematics) PROTO((FWDKINEMATICS_PROTO));
	int (*invKinematics) PROTO((INVKINEMATICS_PROTO));
	int (*rangeAngles) PROTO((RANGEANGLES_PROTO));
	int (*solveConf) PROTO((SOLVECONF_PROTO));

	int (*fwdJacob) PROTO((FWDJACOB_PROTO));
	int (*invJacob) PROTO((INVJACOB_PROTO));
	int (*fwdJacobT) PROTO((FWDJACOBT_PROTO));
	int (*invJacobT) PROTO((INVJACOBT_PROTO));
	int (*gravload) PROTO((GRAVLOAD_PROTO));

	int (*updateVar) PROTO((UPDATEVAR_PROTO));
	int (*updateVarXsincos) PROTO((UPDATEVARXSINCOS_PROTO));

	int (*fwdJacobVar) PROTO((FWDJACOBVAR_PROTO));
	int (*invJacobVar) PROTO((INVJACOBVAR_PROTO));
	int (*fwdJacobTVar) PROTO((FWDJACOBTVAR_PROTO));
	int (*invJacobTVar) PROTO((INVJACOBTVAR_PROTO));
	int (*gravloadVar) PROTO((GRAVLOADVAR_PROTO));

	int class;

	/* kinematic information */

	DH_PARAMS dh[MAXJNTS];

	/* configuration info */

	int numConf;			/* no. of kinematic configs */
	ConfCodeDesc confCodes[KYN_MAXCONF]; /* codes for configs */

	/* dynamic information */

	INERTIA inertia[MAXJNTS];
	VECT massCenter[MAXJNTS];
	float mass[MAXJNTS];

	/* friction terms */

	float statFriction[MAXJNTS];	/* static friction terms */
	float coulFriction[MAXJNTS];	/* Coulomb (sliding) friction */
	float viscFriction[MAXJNTS];	/* viscous (moving) friction */

} GEN_KYN;

extern fwdKinematics PROTO((FWDKINEMATICS_PROTO));
extern invKinematics PROTO((INVKINEMATICS_PROTO));
extern rangeAngles PROTO((RANGEANGLES_PROTO));
extern solveConf PROTO((SOLVECONF_PROTO));
extern fwdJacob PROTO((FWDJACOB_PROTO));
extern invJacob PROTO((INVJACOB_PROTO));
extern fwdJacobT PROTO((FWDJACOBT_PROTO));
extern invJacobT PROTO((INVJACOBT_PROTO));
extern gravload PROTO((GRAVLOAD_PROTO));
extern updateVar PROTO((UPDATEVAR_PROTO));
extern updateVarXsincos PROTO((UPDATEVARXSINCOS_PROTO));
extern fwdJacobVar PROTO((FWDJACOBVAR_PROTO));
extern invJacobVar PROTO((INVJACOBVAR_PROTO));
extern fwdJacobTVar PROTO((FWDJACOBTVAR_PROTO));
extern invJacobTVar PROTO((INVJACOBTVAR_PROTO));
extern gravloadVar PROTO((GRAVLOADVAR_PROTO));

extern unsigned long strToConfig PROTO((STRTOCONFIG_PROTO));
extern char *configToStr PROTO((CONFIGTOSTR_PROTO));

#endif
