/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#ifndef _PUMA_KYN_H_
#define _PUMA_KYN_H_

#include "../h/robotTypes.h"
#include "../h/robot_kynvar.h"
#include "../h/puma_jls.h"

#define PUMA_ALIGN_DEG  0x0100   /* normal to axis of jnt 1 is minimum	*/
#define PUMA_ELBOW_DEG  0x0200   /* elbow is flat out			*/
#define PUMA_WRIST_DEG  KYN_WRIST_SING

#define PUMA_RIGHT_CONF	0x0001	 /* right-handed configuration 		*/
#define PUMA_DOWN_CONF	0x0002	 /* elbow-down configuration		*/
#define PUMA_FLIP_CONF	0x0004	 /* wrist-flip configuration		*/

typedef struct {

	int 	oldCoordDefs;	/* older (Non-McGill) coordinates 	*/

	float	d32;		/*   D3*D3				*/
	float	a22_a32d42;	/*   A2*A2-A3*A3-D4*D4			*/
	float	atana3d4;	/*   atan2(A3,D4)			*/
	float	sqrt_a32d42;	/*   sqrt(A3*A3 + D4*D4)		*/
	float	d42a22a32;	/*   D4*D4+A2*A2+A3*A3			*/
	float	d32a222d422a32;	/*   D3*D3+A2*A2+2*D4*D4+2*A3*A3	*/
	float	epsilon_a2d4;	/* Precision of (A2+D4)/2		*/
	float   epsilon_d32;	/* Precision of D32			*/

	float	cp20;		/* joint 2 gravity load coefficients	*/
	float	cp21;		/*   "   "    "				*/
	float	cp22;		/*   "   "    "				*/
	float	cp30;		/* joint 3 gravity load coefficient	*/
	float	cp50;		/* joint 5 gravity load coefficient	*/

} PUMA_KYN;

typedef struct {
	float	c[PUMAJ];	/* sines of the joint angles 		*/
	float	s[PUMAJ];	/* coines of the joint angles 		*/
	float	c23, s23;	/* sine/cosine of ang2 + ang3		*/

	float	j11, j12, j13;	/* Some elements of the Jacobian, where	*/
	float	j21, j22, j23;	/*   j<m><n> = J (m, n)			*/
	float	j31, j32, j33;
	float	j41;
	float	j51;
	float	j61;

	float	h1, h2, h3;	/* denominators used for inverse Jacobian */
	float	h1_norm;	/* used to find dist. from ALIGN singularity*/

	TRSF	u5;		/* transform from link 4 to link 6	*/

	/* -- special stuff (added at GE/ATL) for working with wrist 
	      singularities -- */

	int     singenable;	
	float	theta4;		/* For resolving wrist singularities */
	float	singstate;	/* state with respect to wrist singularity*/
	float   region1;
	float   region2;
	float   slopRange;
} PUMA_VAR;

extern pumaFwdKinematics PROTO((FWDKINEMATICS_PROTO));
extern pumaInvKinematics PROTO((INVKINEMATICS_PROTO));
extern pumaRangeAngles PROTO((RANGEANGLES_PROTO));
extern pumaSolveConf PROTO((SOLVECONF_PROTO));
extern pumaStrToConfig PROTO((STRTOCONFIG_PROTO));
extern pumaConfigToStr PROTO((CONFIGTOSTR_PROTO));
extern pumaFwdJacob PROTO((FWDJACOB_PROTO));
extern pumaInvJacob PROTO((INVJACOB_PROTO));
extern pumaFwdJacobT PROTO((FWDJACOBT_PROTO));
extern pumaInvJacobT PROTO((INVJACOBT_PROTO));
extern pumaGravload PROTO((GRAVLOAD_PROTO));
extern pumaUpdateVar PROTO((UPDATEVAR_PROTO));
extern pumaUpdateVarXsincos PROTO((UPDATEVARXSINCOS_PROTO));
extern pumaFwdJacobVar PROTO((FWDJACOBVAR_PROTO));
extern pumaInvJacobVar PROTO((INVJACOBVAR_PROTO));
extern pumaFwdJacobTVar PROTO((FWDJACOBTVAR_PROTO));
extern pumaInvJacobTVar PROTO((INVJACOBTVAR_PROTO));
extern pumaGravloadVar PROTO((GRAVLOADVAR_PROTO));

#endif
