#include <stdio.h>

#include "../h/cdefs.h"
#include "../h/robotClass.h"
#include "../h/robot_jls.h"
#include "../h/robot_kynvar.h"

#include "grf4.h"
#include "rbtGraphics.h"

extern void *malloc();

void *rbtGraphicsInit(name)
char *name;
{
	RBT_GRF_DESC *rgd;

	rgd = (RBT_GRF_DESC*)malloc(sizeof(RBT_GRF_DESC));
	
	if ((rgd->class = robot_class(name)) == 0)
	 { return (0);
	 }
	if ((rgd->jls = getJls (name)) == NULL)
	 { return (0);
	 }
	if ((rgd->kyn = getKyn (name,(int*)0)) == NULL)
	 { return (0);
	 }

	/* XXX XXX XXX */
	switch (rgd->class)
	 { default:
	    { return (0);
	    }
	   case PUMA:
	    { /* XXX XXX XXX */
	      if (rgd->kyn->gen->dh[1].a < 300.0)
	       { rgd->type = Puma_260;
		 defineP260Links();
	       }
	      else if (rgd->kyn->gen->dh[1].a < 400.0)
	       { rgd->type = Manutec;
		 defineManutecLinks();
	       }
	      else if (rgd->kyn->gen->dh[1].a < 500.0)
	       { rgd->type = Puma_560;
		 defineP560Links();
	       }
	      else
	       { rgd->type = Puma_760;
		 defineP760Links();
	       }
	      break;
	    }
	   case ELBOW:
	    { rgd->type = Elbow_sim;
	      defineElbowSimLinks();
	      break;
	    }
	   case STAN:
	    { rgd->type = Stan_sim;
	      defineStanSimLinks();
	      break;
	    }
	 }
	rgd->code = rgd->class + rgd->type;
	return ((void*)rgd);
}

rbtBounds(ptr, radiusp, belowp, abovep)
void *ptr;
float *radiusp, *belowp, *abovep;
{
	RBT_GRF_DESC *rgd = (RBT_GRF_DESC*)ptr;

	switch (rgd->code)
	 { /* The viewSize functions return the dimensions of the
	      "bounding cylinder" of the robot: r is the radius,
	      b is the base height to the "shoulder", and h is
	      the height above the shoulder. */ 

	   default:
	    { fprintf (stderr, "Unknown robot type\n");
	      exit (-1);
	    }
	   case (PUMA + Puma_260):
	    { p260ViewSize (radiusp, belowp, abovep, rgd->kyn);
	      break;
	    }
	   case (PUMA + Manutec):
	    { manutecViewSize (radiusp, belowp, abovep, rgd->kyn);
	      break;
	    }
	   case (PUMA + Puma_560):
	    { p560ViewSize (radiusp, belowp, abovep, rgd->kyn);
	      break;
	    }
	   case (PUMA + Puma_760):
	    { p760ViewSize (radiusp, belowp, abovep, rgd->kyn);
	      break;
	    }
	   case (ELBOW + Elbow_sim):
	    { elbowSimViewSize (radiusp, belowp, abovep, rgd->kyn);
	      break;
	    }
	   case (STAN + Stan_sim):
	    { stanSimViewSize (radiusp, belowp, abovep, rgd->kyn);
	      break;
	    }
	 }
}

rbtViewSelect(ptr, winw, winh)
void *ptr;
int winw, winh;
{
	float radius, below, above;
	float vh, vw, near, far;

	rbtBounds (ptr, &radius, &below, &above);

	vw = radius;
	vh = MAX(below,above);
	near = 5*MAX(vw,vh);
	far =  7*MAX(vw,vh);
	grf4Window (-vw/100,vw/100,-vh/100,vh/100, near/100, 2*far, 
		     /*aspect=*/1.0);
	grf4SetView (0.0, 0.0, -(near+far)/2, 0.0, 0.0, 20.0);
	grf4Rotate (0.0, 0.0, -90.0);
	grf4Translate (0.0, 0.0, -below);
}

rbtDrawRobot (ptr, var, T, drawHand, handpos)
void *ptr;
void *var;
TRSF *T;
int drawHand;
int handpos;
{
	RBT_GRF_DESC *rgd = (RBT_GRF_DESC*)ptr;

	grf4PushTrans();
	switch (rgd->code)
	 { default:
	    { fprintf (stderr, "Unknown robot type\n");
	      exit (-1);
	    }
	   case PUMA + Puma_260:
	    { drawP260 (T, rgd->kyn, var, drawHand, handpos);
	      break;
	    }
	   case PUMA + Manutec:
	    { drawManutec (T, rgd->kyn, var, drawHand, handpos);
	      break;
	    }
	   case PUMA + Puma_560:
	    { drawP560 (T, rgd->kyn, var, drawHand, handpos);
	      break;
	    }
	   case PUMA + Puma_760:
	    { drawP760 (T, rgd->kyn, var, drawHand, handpos);
	      break;
	    }
	   case (ELBOW + Elbow_sim):
	    { drawElbowSim (T, rgd->kyn, var, drawHand, handpos);
	      break;
	    }
	   case (STAN + Stan_sim):
	    { drawStanSim (T, rgd->kyn, var, drawHand, handpos);
	      break;
	    }
	 }
	grf4PopTrans();
}

#if 0
/* not used any more */
displayRobot (ptr, T, handpos)
void *ptr;
float *T;
int handpos;
{
	/* clear the display and draw a robot, given a set of joint angles */

	grf4Clear();
	drawRobot (ptr, T, handpos);
	grf4Flush();
}
#endif
