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

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

#define STAN_ALIGN_DEG  0x0100   /* normal to axis of jnt 1 is minimum	*/
#define STAN_ARM_DEG	0x0200	 /* end of joint 3 is at origin 	*/
#define STAN_WRIST_DEG  KYN_WRIST_SING

#define STAN_RIGHT_CONF	0x0001	 /* right-handed configuration 		*/
#define STAN_DOWN_CONF  0x0002	 /* joint 3 is negative			*/
#define STAN_FLIP_CONF	0x0004	 /* wrist-flip configuration		*/

typedef struct {

	float   d3Rng;		/* Range of joint 3; should get from jls*/

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

} STAN_KYN;

typedef struct {
	float	c[STANJ];	/* sines of the joint angles 		*/
	float	s[STANJ];	/* coines of the joint angles 		*/
	float   d3;

	TRSF	u4;		/* transform from link 4 to link 6	*/
} STAN_VAR;

extern stanFwdKinematics PROTO((FWDKINEMATICS_PROTO));
extern stanInvKinematics PROTO((INVKINEMATICS_PROTO));
extern stanRangeAngles PROTO((RANGEANGLES_PROTO));
extern stanSolveConf PROTO((SOLVECONF_PROTO));
extern stanStrToConfig PROTO((STRTOCONFIG_PROTO));
extern stanConfigToStr PROTO((CONFIGTOSTR_PROTO));
extern stanFwdJacob PROTO((FWDJACOB_PROTO));
extern stanInvJacob PROTO((INVJACOB_PROTO));
extern stanFwdJacobT PROTO((FWDJACOBT_PROTO));
extern stanInvJacobT PROTO((INVJACOBT_PROTO));
extern stanGravload PROTO((GRAVLOAD_PROTO));
extern stanUpdateVar PROTO((UPDATEVAR_PROTO));
extern stanUpdateVarXsincos PROTO((UPDATEVARXSINCOS_PROTO));
extern stanFwdJacobVar PROTO((FWDJACOBVAR_PROTO));
extern stanInvJacobVar PROTO((INVJACOBVAR_PROTO));
extern stanFwdJacobTVar PROTO((FWDJACOBTVAR_PROTO));
extern stanInvJacobTVar PROTO((INVJACOBTVAR_PROTO));
extern stanGravloadVar PROTO((GRAVLOADVAR_PROTO));

#endif
