/*-=-=-=-=-=-=-=-=-=-=-=- 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/stan_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 D2	(gen->dh[1].d/10)
#define D3	(svar->d3/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)

/* Stan sim stuff */

#define STAN_BASE_PLATE_HEIGHT		5.0
#define STAN_BASE_PLATE_RADIUS	       12.5
#define STAN_PEDESTAL_HEIGHT	       36.5
#define STAN_PEDESTAL_RADIUS		6.0

#define STAN_LINK1_RADIUS		6.0
#define STAN_LINK1_DEPTH	       25.4
#define STAN_LINK1_OFFSET	       15.0

#define STAN_LINK2_XWIDTH	       14.0
#define STAN_LINK2_YWIDTH	       14.0
#define STAN_LINK2_LENGTH	       30.0

#define STAN_LINK3_XWIDTH	        6.0
#define STAN_LINK3_YWIDTH	        6.0
#define STAN_LINK3_LENGTH	       85.0

#define STAN_LINK4_RADIUS		4.0
#define STAN_LINK4_DEPTH	       10.0
#define STAN_LINK5_RADIUS	        1.75
#define STAN_LINK5_DEPTH	       12.0

#define STAN_HAND_SIZE		       10.0

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

drawStanHand (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 *stanSimbasePlate;
Grf4Object *stanLink3Back;
Grf4Object *stanSimlink[NUM_LINKS];

defineStanSimLinks()
{
	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);

	stanSimbasePlate = grf4GenDisc(NUM_CIRC_EDGES, 
				STAN_BASE_PLATE_RADIUS,
				STAN_BASE_PLATE_HEIGHT, baseMat);

	stanSimlink[0] = grf4GenDisc(NUM_CIRC_EDGES, 
				STAN_PEDESTAL_RADIUS, 
				STAN_PEDESTAL_HEIGHT, baseMat);

	stanSimlink[1] = grf4GenDisc(NUM_CIRC_EDGES, STAN_LINK1_RADIUS, 
				STAN_LINK1_DEPTH, bodyMat);

	stanSimlink[2] = grf4GenBox(STAN_LINK2_XWIDTH, STAN_LINK2_YWIDTH, 
				    STAN_LINK2_LENGTH, bodyMat);

	/* The z dimension for link[3] and link3Back
	   will be reset for each rendering. */

	stanSimlink[3] = grf4GenBox(STAN_LINK3_XWIDTH, STAN_LINK3_YWIDTH, 
				    1.0, bodyMat);
	stanLink3Back  = grf4GenBox(STAN_LINK3_XWIDTH, STAN_LINK3_YWIDTH, 
				    1.0, bodyMat);

	stanSimlink[4] = grf4GenDisc(NUM_CIRC_EDGES, STAN_LINK4_RADIUS, 
				STAN_LINK4_DEPTH, bodyMat);

	stanSimlink[5] = grf4GenDisc(NUM_CIRC_EDGES, STAN_LINK5_RADIUS, 
				STAN_LINK5_DEPTH, bodyMat);

	capacity = grf4Capacity();
}

drawStanSim (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 */

	STAN_VAR *svar = (STAN_VAR*)var;
	GEN_KYN *gen = kyn->gen;

	TRSF trans;
	float z;

	grf4DrawObject (stanSimbasePlate);

	grf4GetTrans (&trans);
	multTrsfXyz (&trans, 0.0, 0.0,STAN_BASE_PLATE_HEIGHT);
	grf4SetTrans (&trans);
	grf4DrawObject (stanSimlink[0]);

	multTrsfXyz (&trans, 0.0, 0.0,STAN_PEDESTAL_HEIGHT+STAN_LINK1_RADIUS);
	T[0] = trans;
	multA1 (&trans, &T[0], kyn, svar);
	T[1] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -STAN_LINK1_OFFSET);
	grf4SetTrans (&trans);
	grf4DrawObject (stanSimlink[1]);

	multA2 (&trans, &T[1], kyn, svar);
	T[2] = trans;
	multTrsfXyz (&trans, -STAN_LINK2_XWIDTH/2.0, -STAN_LINK2_YWIDTH/2.0,
			     -STAN_LINK2_LENGTH/2.0);
	grf4SetTrans (&trans);
	grf4DrawObject (stanSimlink[2]);


	/* The calculation for link3 is a bit tricky, since we
	   have to draw both the front and the back of the prismatic
	   joint. We can't draw just one joint because grf4 doesn't (yet)
	   support hidden line removal for intersecting polygons. So 
	   the link is drawn as two objects whose depth is constantly
	   changing. */

	multA3 (&trans, &T[2], kyn, svar);
	T[3] = trans;
	z = D3 - STAN_LINK2_LENGTH/2;
	grf4SetPrismDepth (stanSimlink[3], z-STAN_LINK4_RADIUS);
	multTrsfXyz (&trans,-STAN_LINK3_XWIDTH/2.0,-STAN_LINK3_YWIDTH/2.0,-z);
	grf4SetTrans (&trans);
	grf4DrawObject (stanSimlink[3]);

	z = (STAN_LINK3_LENGTH+STAN_LINK4_RADIUS) - z;
	grf4SetPrismDepth (stanLink3Back,(STAN_LINK3_LENGTH+STAN_LINK4_RADIUS)-
				     (D3+STAN_LINK2_LENGTH/2) );
	multTrsfXyz (&trans, 0.0, 0.0, -z);
	grf4SetTrans (&trans);
	grf4DrawObject (stanLink3Back);

	multA4 (&trans, &T[3], kyn, svar);
	T[4] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, -STAN_LINK4_DEPTH/2);
	grf4SetTrans (&trans);
	grf4DrawObject (stanSimlink[4]);

	multA5 (&trans, &T[4], kyn, svar);
	T[5] = trans;
	multTrsfXyz (&trans, 0.0, 0.0, STAN_LINK4_RADIUS);
	grf4SetTrans (&trans);
	grf4DrawObject (stanSimlink[5]);

	multA6 (&trans, &T[5], kyn, svar);
	T[6] = trans;

	if (drawHand)
	 { multTrsfRpy (&trans, 0.0, -90.0, 90.0);
	   multTrsfXyz (&trans, STAN_LINK4_RADIUS+STAN_LINK5_DEPTH, 0.0, 0.0);
	   grf4SetTrans (&trans);
	   drawPumaHand (STAN_HAND_SIZE, handpos);
	 }
}

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

	*r = 70.0 + STAN_LINK4_RADIUS + STAN_LINK5_DEPTH + STAN_HAND_SIZE;
	*b = STAN_BASE_PLATE_HEIGHT + STAN_PEDESTAL_HEIGHT + STAN_LINK1_RADIUS;
	*h = *r;
}

stanSimLinkFrames (T, B, kyn, var)
TRSF *T, *B;
KYN *kyn;
void *var;
{
	STAN_VAR *svar = (STAN_VAR*)var;

	T[0] = *B;
	multTrsfXyz (&T[0], 0.0, 0.0, STAN_BASE_PLATE_HEIGHT + 
				      STAN_PEDESTAL_HEIGHT+STAN_LINK1_RADIUS);
	multA1 (&T[1], &T[0], kyn, svar);
	multA2 (&T[2], &T[1], kyn, svar);
	multA3 (&T[3], &T[2], kyn, svar);
	multA4 (&T[4], &T[3], kyn, svar);
	multA5 (&T[5], &T[4], kyn, svar);
	multA6 (&T[6], &T[5], kyn, svar);
}


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

static multA1(tr, t1, kyn, svar)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
STAN_VAR *svar;
{
	register float C;
	register float S;

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

	tr->n.x = C*t1->n.x + S*t1->o.x;
	tr->o.x = -t1->a.x;
	tr->a.x = C*t1->o.x - S*t1->n.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 = C*t1->o.y - S*t1->n.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 = C*t1->o.z - S*t1->n.z;
	tr->p.z = t1->p.z;
}

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

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

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

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

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

static multA3(tr, t1, kyn, svar)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
STAN_VAR *svar;
{
	register float C;
	register float S;

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

	*tr = *t1;

	tr->p.x += t1->a.x*D3;
	tr->p.y += t1->a.y*D3;
	tr->p.z += t1->a.z*D3;
}

static multA4(tr, t1, kyn, svar)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
STAN_VAR *svar;
{
	register float C;
	register float S;

	C = svar->c[J4];
	S = svar->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;

	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 multA5(tr, t1, kyn, svar)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
STAN_VAR *svar;
{
	register float C, S;

	C = svar->c[J5];
	S = svar->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, svar)
register TRSF *tr;
register TRSF *t1;
KYN *kyn;
STAN_VAR *svar;
{
	register float C, S;

	C = svar->c[J6];
	S = svar->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;
}
