/*
 * RCCL Version 1.0           Author :  Vincent Hayward
 *                                      School of Electrical Engineering
 *                                      Purdue University
 *      Dir     : h
 *      File    : kine.h
 *      Remarks : Very dependent on arm type
 *      Usage   : Included in concerned files, made available to the user.
 */

#ifdef PUMA

#define ELBOW_DEG       01              /* elbow degeneracy             */
#define ALIGN_DEG       02              /* T6 in X Z Jt 1 plan          */
#define WRIST_DEG       03              /* wrist degeneracy             */

typedef struct kindyn {
	       real a2, a3, d3, d4, d32, e432, aa3d4, e4aa4ad;
	       real cp21, cp31, cp32, cp50;
} KINDYN, *KINDYN_PTR;

typedef struct sincos {
	real c1, s1, c2, s2, c23, s23, c3, s3, c4, s4, c5, s5, c6, s6;
	real d1x, d1y, d1z, r1x, r1z, d2x, d2y, d2z, d3x, d3y, d3z;
	real h;
	TRSF u5;
} SNCS, *SNCS_PTR;

/*
 * Macro updates coef of Jacob from the sin cos
 */

#define GETH\
{\
	sncs_d.h = sncs_d.c2 * armk_c.a2 +\
		   sncs_d.s23 * armk_c.d4 +\
		   sncs_d.c23 * armk_c.a3;\
}
#define UPDJ\
{\
	sncs_d.d1x = sncs_d.h * sncs_d.s4 -\
		     armk_c.d3 * sncs_d.c23 * sncs_d.c4;\
	sncs_d.d1y = sncs_d.s23 * armk_c.d3;\
	sncs_d.d1z = sncs_d.h * sncs_d.c4 + armk_c.d3 * sncs_d.c23 * sncs_d.s4;\
	sncs_d.r1x = -sncs_d.s23 * sncs_d.c4;\
	sncs_d.r1z = sncs_d.s23 * sncs_d.s4;\
	sncs_d.d2x = armk_c.a2 * sncs_d.s3 * sncs_d.c4;\
	sncs_d.d2y = armk_c.a2 * sncs_d.c3;\
	sncs_d.d2z = -armk_c.a2 * sncs_d.s3 * sncs_d.s4;\
	sncs_d.d3x = sncs_d.c4 * armk_c.d4;\
	sncs_d.d3y = armk_c.a3;\
	sncs_d.d3z = -sncs_d.s4 * armk_c.d4;\
}

#define GETU5\
{\
	sncs_d.u5.n.x = sncs_d.c5 * sncs_d.c6;\
	sncs_d.u5.n.y = sncs_d.s5 * sncs_d.c6;\
	sncs_d.u5.n.z = sncs_d.s6;\
	sncs_d.u5.o.x= -sncs_d.c5 * sncs_d.s6;\
	sncs_d.u5.o.y= -sncs_d.s5 * sncs_d.s6;\
	sncs_d.u5.o.z = sncs_d.c6;\
	sncs_d.u5.a.x = sncs_d.s5;\
	sncs_d.u5.a.y= -sncs_d.c5;\
	sncs_d.u5.a.z = 0.;\
}
#endif

#ifdef STAN
typedef struct kindyn {
	real d2, d22;
} KINDYN, *KINDYN_PTR;

typedef struct sincos {
	real c1, s1, c2, s2, d3, c4, s4, c5, s5, c6, s6;
	real d1x, d1y, d1z, r1x, r1y, r1z, d2x, d2y, d2z, r2x, r2y, r2z,
		d3x, d3y, d3z, r4x, r4y;
} SNCS, *SNCS_PTR;

#define UPDJ\
{\
	real\
	k1 = sncs_d.c4 * sncs_d.c5,\
	k2 = sncs_d.s4 * sncs_d.c5,\
	k3 = sncs_d.c4 * sncs_d.s5,\
	k4 = sncs_d.s2 * sncs_d.d3,\
	k5 = k1      * sncs_d.c6,\
	k6 = sncs_d.s4 * sncs_d.c6,\
	k7 = k5 - sncs_d.s4 * sncs_d.s6,\
	k8 = k2 * sncs_d.c6 + sncs_d.c4 * sncs_d.s6,\
	k9 = k1 * sncs_d.s6 + k6,\
	k10= - k2 * sncs_d.s6 + sncs_d.c4 * sncs_d.c6,\
	k11= k5 + k6,\
	k12= sncs_d.s4 * sncs_d.s5,\
	k13= sncs_d.s5 * sncs_d.c6,\
	k14= sncs_d.s5 * sncs_d.s6;\
	sncs_d.d1x = (-armk_c.d2 * (sncs_d.c2 * k7 - sncs_d.s2 * k13) +\
		      k4 * k8);\
	sncs_d.d2x = sncs_d.d3 * k7;\
	sncs_d.d3x = -k13;\
	sncs_d.d1y = (-armk_c.d2 * (- sncs_d.c2 * k9 + sncs_d.s2 * k14) +\
		      k4 * k10);\
	sncs_d.d2y = -sncs_d.d3 * k11;\
	sncs_d.d3y = k14;\
	sncs_d.d1z = (-armk_c.d2 * (sncs_d.c2 * k3 + sncs_d.s2 * sncs_d.c5) +\
		      k4 * k12);\
	sncs_d.d2z = sncs_d.d3 * k3;\
	sncs_d.d3z = sncs_d.c5;\
	sncs_d.r1x = (-sncs_d.s2 * k7 - sncs_d.c2 * k13);\
	sncs_d.r2x = k8;\
	sncs_d.r4x = -k13;\
	sncs_d.r1y = (sncs_d.s2 * k9 + sncs_d.c2 * k14);\
	sncs_d.r2y = k10;\
	sncs_d.r4y = k14;\
	sncs_d.r1z = (-sncs_d.s2 * k3 + sncs_d.c2 * sncs_d.c5);\
	sncs_d.r2z = k12;\
}
#endif


extern  KINDYN  armk_c;                 /* arm kinematic and dynamic    */
					/* constants                    */

extern  SNCS    sncs_d;                 /* current sin cos, jacob coeff */
					/* and U5 matrix                */

extern  JNTS    jcal_c;                 /* rest position joint range    */

extern  JNTS    jmin_c;                 /* angles range offset values   */

extern  JNTS    jrng_c;                 /* maximum joint range values   */

extern  JNTS    jmxv_c;                 /* max joint velocities */
