/*-=-=-=-=-=-=-=-=-=-=-=- 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 _ELBOW_KYN_H_
#define _ELBOW_KYN_H_

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

#define ELBOW_ALIGN_DEG  0x0100   /* normal to axis of jnt 1 is minimum	*/
#define ELBOW_ELBOW_DEG  0x0200   /* elbow is flat out			*/
#define ELBOW_WRIST_DEG  KYN_WRIST_SING

#define ELBOW_RIGHT_CONF 0x0001	 /* right-handed configuration 		*/
#define ELBOW_DOWN_CONF	 0x0002	 /* elbow-down configuration		*/
#define ELBOW_FLIP_CONF	 0x0004	 /* wrist-flip configuration		*/

#define ELBOW_CANT_REACH	0x0800	 /* kinematic solution out of reach	*/

typedef struct {

	float	cp20;		/* joint 2 gravity load coefficient	*/
	float	cp30;		/* joint 3    "      "       "		*/
	float   cp40;		/* joint 4    "	     "       "		*/
	float	cp50;		/* joint 5    "	     "       "		*/

} ELBOW_KYN;

typedef struct {
	float	c[ELBOWJ];	/* sines of the joint angles 		*/
	float	s[ELBOWJ];	/* coines of the joint angles 		*/
	float	c23, s23;	/* sine/cosine of ang2 + ang3		*/
	float	c34, s34;	/* sine/cosine of ang3 + ang4		*/
	float	c234, s234;	/* sine/cosine of ang2 + ang3 + ang4	*/

	float	h1;		/* denominator used for inverse Jacobian */
	float	h1_norm;	/* used to find dist. from ALIGN singularity*/

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

extern elbowFwdKinematics PROTO((FWDKINEMATICS_PROTO));
extern elbowInvKinematics PROTO((INVKINEMATICS_PROTO));
extern elbowRangeAngles PROTO((RANGEANGLES_PROTO));
extern elbowSolveConf PROTO((SOLVECONF_PROTO));
extern elbowStrToConfig PROTO((STRTOCONFIG_PROTO));
extern elbowConfigToStr PROTO((CONFIGTOSTR_PROTO));
extern elbowFwdJacob PROTO((FWDJACOB_PROTO));
extern elbowInvJacob PROTO((INVJACOB_PROTO));
extern elbowFwdJacobT PROTO((FWDJACOBT_PROTO));
extern elbowInvJacobT PROTO((INVJACOBT_PROTO));
extern elbowGravload PROTO((GRAVLOAD_PROTO));
extern elbowUpdateVar PROTO((UPDATEVAR_PROTO));
extern elbowUpdateVarXsincos PROTO((UPDATEVARXSINCOS_PROTO));
extern elbowFwdJacobVar PROTO((FWDJACOBVAR_PROTO));
extern elbowInvJacobVar PROTO((INVJACOBVAR_PROTO));
extern elbowFwdJacobTVar PROTO((FWDJACOBTVAR_PROTO));
extern elbowInvJacobTVar PROTO((INVJACOBTVAR_PROTO));
extern elbowGravloadVar PROTO((GRAVLOADVAR_PROTO));

#endif
