/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1991 by John E. Lloyd.				    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
#include <stdio.h>
#include <math.h>

#include "../h/cdefs.h"
#include "../h/rccl.h"	/* XXX MUST include rccl.h before grf4.h */
#include "../h/elbow_kynvar.h"

#include "grf4.h"

#define NUM_CIRC_EDGES		12
#define NUM_LINKS 		7

#define J1	0
#define J2	1
#define J3	2
#define J4	3
#define J5	4
#define J6	5

#define A2	(gen->dh[1].a/10)
#define A3	(gen->dh[2].a/10)
#define A4	(gen->dh[3].a/10)

static int capacity;

#define BEIGE		RGBPACK(225,160, 90)
#define DARK_BROWN	RGBPACK( 85, 55, 35)
#define DARK_GREY	RGBPACK( 50, 50, 50)
#define BLACK		RGBPACK(  0,  0,  0)
#define WHITE		RGBPACK(255,255,255)
#define SILVER		RGBPACK(200,200,220)
#define AMBER		RGBPACK(255,125,  0)

/* Elbow sim stuff */

#define ESIM_BASE_PLATE_HEIGHT		20.0
#define ESIM_BASE_PLATE_BOTRADIUS	17.0
#define ESIM_BASE_PLATE_TOPRADIUS	15.0
#define ESIM_PEDESTAL_HEIGHT		40.0
#define ESIM_PEDESTAL_TOPRADIUS		 7.5

#define ESIM_LINK1_RADIUS		10.0
#define ESIM_LINK1_DEPTH		40.0

#define ESIM_LINK2_DEPTH		10.0
#define ESIM_JOINT3_RADIUS		 5.0
#define ESIM_JOINT3_DEPTH		14.0
#define ESIM_LINK3_DEPTH		10.0
#define ESIM_JOINT4_RADIUS		 5.0
#define ESIM_JOINT4_DEPTH		14.0
#define ESIM_LINK4_DEPTH		 3.0
#define ESIM_JOINT5_RADIUS		 3.5
#define ESIM_JOINT5_DEPTH		10.0

#define ESIM_HAND_SIZE			10.0

static float elbowSim_l2bot[] = {
	-5.0,    3.0,
       -45.0,    3.0,
       -45.0,   -3.0,
	-5.0,   -3.0,
};

static float elbowSim_l3bot[] = {
	-5.0,    3.0,
       -45.0,    3.0,
       -45.0,   -3.0,
	-5.0,   -3.0,
};

static float elbowSim_l4bot[] = {
	-3.5,    3.0,
       -10.0,    3.0,
       -10.0,   -3.0,
	-3.5,   -3.0,
};


static multA1();
static multA2();
static multA3();
static multA4();
static multA5();
static multA6();

drawElbowHand (size, open)
float size;
int open;
{
	float m, h;

	m = 0.5*size;
	h = (open ? m : m/4);

	grf4Color (SILVER);

	/* draw the stem and the slide */

	grf4Move (0.0, 0.0, 0.0);
	grf4Draw (  m, 0.0, 0.0);
	grf4Move (  m,  -m, 0.0);
	grf4Draw (  m,   m, 0.0);

	/* draw the two fingers */

	grf4Move (size, -h, 0.0);
	grf4Draw (   m, -h, 0.0);
	grf4Move (size,  h, 0.0);
	grf4Draw (   m,  h, 0.0);
}

Grf4Object *elbowSimbasePlate;
Grf4Object *elbowSimJoint3;
Grf4Object *elbowSimJoint4;
Grf4Object *elbowSimJoint5;
Grf4Object *elbowSimlink[NUM_LINKS];

defineElbowSimLinks()
{
	void *baseMat;
	void *bodyMat;
	void *wristMat;

	baseMat = grf4DefMaterial(DARK_BROWN, DARK_BROWN, 0, 0, 0.0, 1.0);
	bodyMat = grf4DefMaterial(BEIGE, BEIGE, 0, 0, 0.0, 1.0);
	wristMat = grf4DefMaterial(DARK_GREY, DARK_GREY, 0,0x808080,50.0,1.0);

	elbowSimbasePlate = grf4GenCone(NUM_CIRC_EDGES, 
				ESIM_BASE_PLATE_BOTRADIUS,
				ESIM_BASE_PLATE_TOPRADIUS,
				ESIM_BASE_PLATE_HEIGHT, baseMat);

	elbowSimlink[0] = grf4GenCone(NUM_CIRC_EDGES, 
				ESIM_BASE_PLATE_TOPRADIUS,
				ESIM_PEDESTAL_TOPRADIUS, 
				ESIM_PEDESTAL_HEIGHT, baseMat);

	elbowSimlink[1] = grf4GenDisc(NUM_CIRC_EDGES, ESIM_LINK1_RADIUS, 
				ESIM_LINK1_DEPTH, bodyMat);

	elbowSimlink[2] = grf4GenPrism(ARRAY_SIZE(elbowSim_l2bot)/2, 
				elbowSim_l2bot, (float*)0, (char*)0,
				ESIM_LINK2_DEPTH, bodyMat);

	elbowSimJoint3 = grf4GenDisc(NUM_CIRC_EDGES, ESIM_JOINT3_RADIUS, 
				ESIM_JOINT3_DEPTH, bodyMat);

	elbowSimlink[3] = grf4GenPrism(ARRAY_SIZE(elbowSim_l3bot)/2, 
				elbowSim_l3bot, (float*)0, (char*)0,
				ESIM_LINK3_DEPTH, bodyMat);

	elbowSimJoint4 = grf4GenDisc(NUM_CIRC_EDGES, ESIM_JOINT4_RADIUS, 
				ESIM_JOINT4_DEPTH, bodyMat);

	elbowSimlink[4] = grf4GenPrism(ARRAY_SIZE(elbowSim_l4bot)/2, 
				elbowSim_l4bot, (float*)0, (char*)0,
				ESIM_LINK4_DEPTH, bodyMat);

	elbowSimJoint5 = grf4GenDisc(NUM_CIRC_EDGES, ESIM_JOINT5_RADIUS, 
				ESIM_JOINT5_DEPTH, bodyMat);

	capacity = grf4Capacity();
}

drawElbowSim (T, kyn, var, drawHand, handpos)
register TRSF *T;
KYN *kyn;
void *var;
int drawHand;
int handpos;
{
	/* clear the display and draw a robot, given a set of joint angles */

	ELBOW_KYN *evar = (ELBOW_KYN*)var;
	GEN_KYN *gen = kyn->gen;

	TRSF trans;

	grf4DrawObject (elbowSimbasePlate);

	grf4GetTrans (&trans);
	multTrsfXyz (&trans, 0.0, 0.0,ESIM_BASE_PLATE_HEIGHT);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimlink[0]);

	multTrsfXyz (&trans, 0.0, 0.0,ESIM_PEDESTAL_HEIGHT+ESIM_LINK1_RADIUS);
	T[0] = trans;
	multA1 (&trans, &T[0], kyn, evar);
	T[1] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -ESIM_LINK1_DEPTH/2);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimlink[1]);

	multA2 (&trans, &T[1], kyn, evar);
	T[2] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -ESIM_LINK2_DEPTH/2);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimlink[2]);
	multTrsfXyz (&trans, 0.0,0.0,-(ESIM_JOINT3_DEPTH-ESIM_LINK2_DEPTH)/2);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimJoint3);

	multA3 (&trans, &T[2], kyn, evar);
	T[3] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -ESIM_LINK3_DEPTH/2);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimlink[3]);
	multTrsfXyz (&trans, 0.0,0.0,-(ESIM_JOINT4_DEPTH-ESIM_LINK3_DEPTH)/2);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimJoint4);

	multA4 (&trans, &T[3], kyn, evar);
	T[4] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -ESIM_LINK4_DEPTH/2);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimlink[4]);
       	multTrsfXyz (&trans, 0.0,0.0,-(ESIM_JOINT5_DEPTH-ESIM_LINK4_DEPTH)/2);
	grf4SetTrans (&trans);
	grf4DrawObject (elbowSimJoint5);

	multA5 (&trans, &T[4], kyn, evar);
	T[5] = trans;
	multA6 (&trans, &T[5], kyn, evar);
	T[6] = trans;

	if (drawHand)
	 { multTrsfRpy (&trans, 0.0, -90.0, 90.0);
	   multTrsfXyz (&trans, ESIM_JOINT5_RADIUS, 0.0, 0.0);
	   grf4SetTrans (&trans);
	   drawPumaHand (ESIM_HAND_SIZE, handpos);
	 }
}

elbowSimViewSize (r, b, h, kyn)
float *r, *b, *h;
KYN *kyn;
{
	GEN_KYN *gen = kyn->gen;

	*r = A2 + A3 + A4 + ESIM_JOINT5_RADIUS + ESIM_HAND_SIZE;
	*b = ESIM_BASE_PLATE_HEIGHT + ESIM_PEDESTAL_HEIGHT + 
	     ESIM_LINK1_RADIUS;
	*h = *r;
}

elbowSimLinkFrames (T, B, kyn, var)
TRSF *T, *B;
KYN *kyn;
void *var;
{
	ELBOW_KYN *evar = (ELBOW_KYN*)var;

	T[0] = *B;
	multTrsfXyz (&T[0], 0.0, 0.0, ESIM_BASE_PLATE_HEIGHT + 
				      ESIM_PEDESTAL_HEIGHT+ESIM_LINK1_RADIUS);
	multA1 (&T[1], &T[0], kyn, evar);
	multA2 (&T[2], &T[1], kyn, evar);
	multA3 (&T[3], &T[2], kyn, evar);
	multA4 (&T[4], &T[3], kyn, evar);
	multA5 (&T[5], &T[4], kyn, evar);
	multA6 (&T[6], &T[5], kyn, evar);
}


/* Routines for calculating the frame matrices for ELBOW robots */

static multA1(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
ELBOW_VAR *var;
{
	register GEN_KYN *gen = kyn->gen;
	register float C;
	register float S;

	C = var->c[J1];
	S = var->s[J1];

	tr->n.x = C*t1->n.x + S*t1->o.x;
	tr->o.x = t1->a.x;
	tr->a.x = S*t1->n.x - C*t1->o.x;
	tr->p.x = t1->p.x;

	tr->n.y = C*t1->n.y + S*t1->o.y;
	tr->o.y = t1->a.y;
	tr->a.y = S*t1->n.y - C*t1->o.y;
	tr->p.y = t1->p.y;

	tr->n.z = C*t1->n.z + S*t1->o.z;
	tr->o.z = t1->a.z;
	tr->a.z = S*t1->n.z - C*t1->o.z;
	tr->p.z = t1->p.z;
}

static multA2(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
ELBOW_VAR *var;
{
	register GEN_KYN *gen = kyn->gen;
	register float C;
	register float S;

	C = var->c[J2];
	S = var->s[J2];

	tr->n.x = C*t1->n.x + S*t1->o.x;
	tr->o.x = -S*t1->n.x + C*t1->o.x;
	tr->a.x = t1->a.x;
	tr->p.x = A2*C*t1->n.x + A2*S*t1->o.x + t1->p.x;

	tr->n.y = C*t1->n.y + S*t1->o.y;
	tr->o.y = -S*t1->n.y + C*t1->o.y;
	tr->a.y = t1->a.y;
	tr->p.y = A2*C*t1->n.y + A2*S*t1->o.y + t1->p.y;

	tr->n.z = C*t1->n.z + S*t1->o.z;
	tr->o.z = -S*t1->n.z + C*t1->o.z;
	tr->a.z = t1->a.z;
	tr->p.z = A2*C*t1->n.z + A2*S*t1->o.z + t1->p.z;
}

static multA3(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
ELBOW_VAR *var;
{
	register GEN_KYN *gen = kyn->gen;
	register float C;
	register float S;

	C = var->c[J3];
	S = var->s[J3];

	tr->n.x = C*t1->n.x + S*t1->o.x;
	tr->o.x = -S*t1->n.x + C*t1->o.x;
	tr->a.x = t1->a.x;
	tr->p.x = A3*C*t1->n.x + A3*S*t1->o.x + t1->p.x;

	tr->n.y = C*t1->n.y + S*t1->o.y;
	tr->o.y = -S*t1->n.y + C*t1->o.y;
	tr->a.y = t1->a.y;
	tr->p.y = A3*C*t1->n.y + A3*S*t1->o.y + t1->p.y;

	tr->n.z = C*t1->n.z + S*t1->o.z;
	tr->o.z = -S*t1->n.z + C*t1->o.z;
	tr->a.z = t1->a.z;
	tr->p.z = A3*C*t1->n.z + A3*S*t1->o.z + t1->p.z;
}

static multA4(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
ELBOW_VAR *var;
{
	register GEN_KYN *gen = kyn->gen;
	register float C;
	register float S;

	C = var->c[J4];
	S = var->s[J4];

	tr->n.x = C*t1->n.x + S*t1->o.x;
	tr->o.x = -t1->a.x;
	tr->a.x = -S*t1->n.x + C*t1->o.x;
	tr->p.x = t1->p.x + A4*(S*t1->o.x + C*t1->n.x);

	tr->n.y = C*t1->n.y + S*t1->o.y;
	tr->o.y = -t1->a.y;
	tr->a.y = -S*t1->n.y + C*t1->o.y;
	tr->p.y = t1->p.y + A4*(S*t1->o.y + C*t1->n.y);

	tr->n.z = C*t1->n.z + S*t1->o.z;
	tr->o.z = -t1->a.z;
	tr->a.z = -S*t1->n.z + C*t1->o.z;
	tr->p.z = t1->p.z + A4*(S*t1->o.z + C*t1->n.z);
}

static multA5(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
ELBOW_VAR *var;
{
	register GEN_KYN *gen = kyn->gen;
	register float C, S;

	C = var->c[J5];
	S = var->s[J5];

	tr->n.x =   C*t1->n.x + S*t1->o.x;
	tr->o.x =   t1->a.x;
	tr->a.x =   S*t1->n.x - C*t1->o.x;
	tr->p.x =   t1->p.x;

	tr->n.y =   C*t1->n.y + S*t1->o.y;
	tr->o.y =   t1->a.y;
	tr->a.y =   S*t1->n.y - C*t1->o.y;
	tr->p.y =   t1->p.y;

	tr->n.z =   C*t1->n.z + S*t1->o.z;
	tr->o.z =   t1->a.z;
	tr->a.z =   S*t1->n.z - C*t1->o.z;
	tr->p.z =   t1->p.z;
}

static multA6(tr, t1, kyn, var)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
ELBOW_VAR *var;
{
	register GEN_KYN *gen = kyn->gen;
	register float C, S;

	C = var->c[J6];
	S = var->s[J6];

	tr->n.x =  C*t1->n.x + S*t1->o.x;
	tr->o.x = -S*t1->n.x + C*t1->o.x;
	tr->a.x =  t1->a.x;
	tr->p.x =  t1->p.x;

	tr->n.y =  C*t1->n.y + S*t1->o.y;
	tr->o.y = -S*t1->n.y + C*t1->o.y;
	tr->a.y =  t1->a.y;
	tr->p.y =  t1->p.y;

	tr->n.z =  C*t1->n.z + S*t1->o.z;
	tr->o.z = -S*t1->n.z + C*t1->o.z;
	tr->a.z =  t1->a.z;
	tr->p.z =  t1->p.z;
}
