#include <stdio.h>
#include <strings.h>
#include <signal.h>
#include <setjmp.h>
#include <sys/types.h>

#include "../h/cdefs.h"
#include "../h/ctree.h"
#include "../h/timeval.h" /* includes <sys/time.h> as well */
#include "../h/robotMath.h"
#include "../h/robotTypes.h"
#include "../h/moper_com.h"
#include "../h/pendant.h"

#include "../pdb/pdb.h"
#include "robotsim.h"
#include "grf4.h"
#include "display.h"
#include "linkedList.h"

static char *getErrMsg();

harderror(rd, code, loc)
RobotDesc *rd;
int code, loc;
{
	/* Record the error and location codes for a harderror. 
	   A 'harderror' means an error condition for which we
	   shut down the robot immediately, and do not wait until
	   the next cycle to tell the host system of the error.
	 */
	rd->ss.hardError = code;
	rd->ss.hardErrLoc = loc;
}

#define MAXROBOTS	32
#define MAXINT	((unsigned)(~0) >> 1)

char **argvec;

int silentMode = 0;		/* silent mode; don't print anything */
int interactive = 0;		/* interactive mode; accept Ctree input */
int graphicsMode = 1;		/* do graphics display, if possible */
int autoCycling = 1;
int doGraphicPendant = 0;
char *prompt = "ROBOTSIM> ";

int graphicsFlags = GRF_VIEW_CONTROL;

int bgColor = GRF_BLACK;	/* back ground color value */
int fgColor = GRF_WHITE;	/* foreground color value */
int sampleInterval = 100;	/* sample rate of the simulator, in msec. */
int dontReadConfigFile = 0;	/* don't read in config file */
char *configFileName = 0;	/* alternate name for config file */

PdbSet *configPar;		/* parameter set read in from config file */
RobotDesc *robots;		/* list of robots run by simulator */
int numRobots;			/* number of robots run by simulator */
ListDesc groupList;		/* list of robot groups */
RobotDesc *interactingRobot;

StaticRobotDesc *staticRobots;	/* fixed robots which don't move */
ObjectDesc *objects;		/* list of work space objects */
MaterialDesc *materials;
LightDesc *lights;

MaterialDesc *defaultMatDesc;
void *defaultMat;

PdbPar *viewWindowConfig;
PdbPar *viewTransConfig;
PdbPar *windowPosConfig;
int orthoProjection = 0;

float viewWinPar[6];

RobotDesc *getRobotByName();

static char* copystr();
static readConfigFile();
static inputFile();

static char* copystr(s)
char *s;
{
	return (strcpy((char*)malloc(strlen(s)+1),s));
}

parseArgs (argc, argv)
int argc;
char **argv;
{
	/* Called once at program startup time to parse command line args. */
	      
	RobotDesc *rd = 0;

	argc--; argv++;
	while (argc > 0)
	 { if (ARGUMENT ("-s"))
	    { silentMode = 1;
	    }
	   else if (ARGUMENT ("-i"))
	    { interactive = 1;
#if 0
	      fprintf (stderr,
"%s: option -i: interactive mode not yet supported in this version\n",
		       argvec[0]);
	      exit (-1);
#endif
	    }
	   else if (ARGUMENT ("-noauto"))
	    { autoCycling = 0;
	    }
	   else if (ARGUMENT ("-ng"))
	    { graphicsMode = 0;
	    }
	   else if (ARGUMENT ("-X11"))
	    { graphicsFlags |= GRF_X11;
	    }
	   else if (ARGUMENT ("-wf"))
	    { graphicsFlags |= GRF_WIRE_FRAME;
	    }
	   else if (ARGUMENT("-bg"))
	    { char *ptr;
	      *argv++; argc--;
	      if (argc <= 0)
	       { goto usage;
	       }
	      bgColor = strtol (*argv, &ptr, 16);
	      if (ptr == *argv)
	       { goto usage;
	       }
	    }
	   else if (ARGUMENT("-fg"))
	    { char *ptr;
	      *argv++; argc--;
	      if (argc <= 0)
	       { goto usage;
	       }
	      fgColor = strtol (*argv, &ptr, 16);
	      if (ptr == *argv)
	       { goto usage;
	       }
	    }
	   else if (ARGUMENT("-r"))
	    { char *ptr;
	      *argv++; argc--;
	      if (argc <= 0)
	       { goto usage;
	       }
	      sampleInterval = strtol (*argv, &ptr, 10);
	      if (ptr == *argv)
	       { goto usage;
	       }
	      if (sampleInterval < 1)
	       { fprintf(stderr, 
"%s, option -r: sample interval must be a positive integer\n", argvec[0]);
		 exit(-1);
	       }			 
	    }
	   else if (ARGUMENT("-f"))
	    { char *ptr;
	      *argv++; argc--;
	      if (argc <= 0)
	       { goto usage;
	       }
	      configFileName = *argv;
	    }
	   else if (ARGUMENT("-pendant"))
	    { doGraphicPendant = 1;
	    }
	   else if (ARGUMENT("-nc"))
	    { if (!rd)
	       { goto usage;
	       }
	      rd->ss.nocal = 1;
	    }
	   else if (ARGUMENT("-nocfg"))
	    { if (!rd)
	       { goto usage;
	       }
	      dontReadConfigFile = 1;
	    }
	   else if (ARGUMENT("-M"))
	    { if (!rd)
	       { goto usage;
	       }
	      rd->ss.matlabFormat = 1;
	    }
	   else if (ARGUMENT("-jlf"))
	    { argc--; argv++;
	      if (!rd || argc == 0)
	       { goto usage;
	       }
	      if ((rd->ss.jlf = fopen(*argv, "r")) == NULL)
	       { fprintf (stderr, "%s: can't open joint log file '%s'\n",
			  argvec[0], *argv);
		 exit (-1);
	       }
	      rd->ss.jlfName = copystr(*argv);
	    }
	   else if (ARGUMENT("-clf"))
	    { argc--; argv++;
	      if (!rd || argc == 0)
	       { goto usage;
	       }
	      if ((rd->ss.clf = fopen(*argv, "r")) == NULL)
	       { fprintf (stderr, "%s: can't open Cartesian log file '%s'\n",
			  argvec[0], *argv);
		 exit (-1);
	       }
	      rd->ss.clfName = copystr(*argv);
	    }
	   else if (ARGUMENT("-p"))
	    { argc--; argv++;
	      if (!rd || argc == 0)
	       { goto usage;
	       }
	      rd->ss.initAngles = (float*)malloc(6*sizeof(float));
	      if (getRobotPosition (rd->ss.initAngles, *argv, rd->name) <= 0)
	       { fprintf (stderr, 
"%s: can't get position '%s' for robot '%s'\n",
			  argvec[0], *argv, rd->name);
		 exit (-1);
	       }
	    }
	   else
	    { /* the argument specifies a robot name; set up the 
		 corresponding robot. */

	      RobotDesc *r;

	      if (numRobots >= MAXROBOTS)
	       { fprintf (stderr, "%s: can't allocate more the %d robots\n",
			  argvec[0], MAXROBOTS);
		 exit(-1);
	       }
	      for (r=robots; r; r=r->next)
	       { if (strcmp(r->name,*argv) == 0)
		  { fprintf (stderr, "%s: robot '%s' specified twice\n",
			     argvec[0], *argv);
	   	    exit (-1);
		  }
	       }
	      if (!(rd = (RobotDesc*)calloc(1,sizeof(RobotDesc))))
	       { fprintf (stderr, "%s: out of memory\n", argvec[0]);
		 exit (-1);
	       }
	      if ((rd->rbtID = find_robot(*argv)) == 0)
	       { fprintf (stderr, 
"%s: can't find robot '%s' in RCI tables\n", argvec[0], *argv);
		 exit (-1);
	       }
	      if ((rd->class = robot_class(*argv)) == NULL)
	       { fprintf (stderr, 
"%s: can't get class type for robot '%s'\n", argvec[0], *argv);
		 exit (-1);
	       }
	      if ((rd->jls = getJls(*argv)) == NULL)
	       { fprintf (stderr, 
"%s: can't get JLS structure for robot '%s'\n", argvec[0], *argv);
		 exit (-1);
	       }
	      if ((rd->kyn = getKyn(*argv, 0)) == NULL)
	       { fprintf (stderr, 
"%s: can't get kyn struct for robot '%s'\n", argvec[0], *argv);
		 exit (-1);
	       }
	      rd->var = (void*)malloc(getVarSize(*argv));

	      rd->name = copystr(*argv);
	      rd->next = robots;
	      robots = rd;
	      rd->ss.num = numRobots;
	      rd->ss.code = (1<<numRobots);
	      numRobots++;
	    }
	   argc--; argv++;
	 }
	return;
usage:	fprintf (stderr, 
"Usage: %s	[-s] [-i] [-ng] [-r <interval>] [-noauto]\n\
		[-ng] [-X11] [-bg <background>] [-fg <foreground>] [-wf]\n\
		[-nocfg] [-f <configFile>] [-pendant]\n\
		[ <robotName> [-nc] [-p <position>]\n\
		  [-clf <cartesianLogFile>] [-jlf <jointLogFile>]\n\
		] ... \n", argvec[0]);
	exit (-1);
}

static readConfigFile()
{
	/* Read in the parameter set from the .robotsim file, if
	   one exists. Look in cwd first, then $HOME, and finally 
	   $RCCL/conf. */

	FILE *fp;
	char fileName[256];
	char *dir;

	if (configFileName)
	 { if ((fp = fopen(configFileName, "r")) == 0)
	    { fprintf (stderr, "%s: can't open config file %s\n",
				argvec[0], configFileName);
	      exit (-1);
	    }
	   inputFile (fp, configFileName);
	   return;
	 }
	if (fp = fopen(".robotsim", "r"))
	 { inputFile (fp, ".robotsim");
	   return;
	 }
	if (dir = (char*)getenv("HOME"))
	 { sprintf (fileName, "%s/.robotsim", dir);
	   if (fp = fopen (fileName, "r"))
	    { inputFile (fp, fileName);
	      return;
	    }	   
	 }
	if (dir = (char*)getenv("RCCL"))
	 { sprintf (fileName, "%s/conf/.robotsim", dir);
	   if (fp = fopen (fileName, "r"))
	    { inputFile (fp, fileName);
	      return;
	    }	   
	 }
}

static inputFile(fp, name)
FILE *fp;
char *name;
{	
	/* Parse a specific .robotsim file which has been openned. */

	if (!(configPar = pdbParse(fp)))
	 { fprintf (stderr, "%s: error parsing parameter file '%s'\n", 
		    argvec[0], name);
	   exit (-1);
	 }
	if (interpretParamSet(configPar) < 0)
	 { fprintf (stderr, "%s: error interpreting parameter file '%s'\n",
		    argvec[0], name);
	   printErrors();
	   exit (-1);
	 }
}

struct timeval tvInterval;	/* timeval equivalent of sampleInterval */
struct timeval tvZero;		/* timeval with 0 value */
struct timeval tvLastCycle;	/* time when last cycle tick occured */

char *keytree;			/* parse spec. for Ctree matcher  LATER */
char key_buf[256];		/* buffer for Ctree matcher input */

ListDesc fxnList;		/* simulation functions. */
ListDesc drawList;		/* list of drawable objects, in the order
				   they are to be drawn */

/* LATER - do display stuff */
#define DEFAULT_DISPLAY (DISPLAY_JOINTS | DISPLAY_TIME | DISPLAY_TORQUE)
#define DEFAULT_DISPLAY_FREQUENCY	10
int displayFrequency = DEFAULT_DISPLAY_FREQUENCY;
int displayMode = DEFAULT_DISPLAY;
int savedDisplayMode = DEFAULT_DISPLAY;
int lastDisplayCycle = 0;

int cycleCnt = 0;		/* simulator cycle counter */
int lastCycleCnt = 0;		/* last simulator cycle count */
long robotInputReady;		/* bit mask of robots whose input is ready */

ListDesc eventList;		/* 'run' queue of robot groups */

int CtreeInputFd;
int stateChanged = 0;
int freezeOn;
int cycleCntLimit = 0;		/* limit the number of cycles we can
				   execute */
int refreshPrompt = 0;

void
onIntr()
{
	programExit(0);
}

programExit(code)
int code;
{
	if (interactive)
	 { tree_match_reset();
	 }
	closeRobotLogFiles();
	exit(code);
}

main(argc, argv)
int argc;
char **argv;
{
	RobotDesc *rd;

	argvec = argv;

	parseArgs (argc, argv);
	if (!robots)
	 { doGraphicPendant = 0;
	 }

	if (graphicsMode)
	 { int flags;

	   if ((flags = grf4SetDisplay (graphicsFlags)) < 0)
	    { if (graphicsFlags & GRF_X11)
	       { fprintf (stderr, "Can't initialize X11 graphics\n");
	       }
	      else
	       { fprintf (stderr, "Can't initialize graphics\n");
	       }
	      exit (-1);
	    }
	   graphicsFlags = flags | (graphicsFlags & ~(GRF_WIRE_FRAME|GRF_X11));
	 }

	if (!dontReadConfigFile)
	 { readConfigFile();
	 }

	/* initialize robots, and set them listening for connections */

	for (rd=robots; rd; rd=rd->next)
	 { robotInit(rd);
	   robotListen(rd);
	 }

	/* initialize simulation functions */

	fxnListInit();

	/* initialize graphics */

	if (graphicsMode)
	 { if (!graphicsInit())
	    { graphicsMode = 0;
	    }
	   else
	    { graphicsDraw();
	    }
	 }

	tvInterval.tv_sec  = sampleInterval/1000;
	tvInterval.tv_usec = (sampleInterval%1000)*1000;
	signal (SIGPIPE, SIG_IGN);
	signal (SIGINT, onIntr);

	if (interactive)
	 { tree_match_setup();
	   buildCtree();
	   checkCtreeInput();
	 }
	interactingRobot = robots;

	pollForEvents();
	programExit(0);
}

pollForEvents()
{
	RobotDesc *rd;
	GroupDesc *gd;
	struct timeval tout;
	struct timeval *tvp;
	int gEvnt = 0;
	int status;
	fd_set rfds;
	
	tout = tvInterval;
	tvp = &tout;
	
	while(1)
	 {
	   /* set up up file descriptor set for select() */
	   FD_ZERO(&rfds);
	   if (interactive)
	    { FD_SET(CtreeInputFd, &rfds);
	    }
	   for (rd=robots; rd; rd=rd->next)
	    { if (rd->ss.active)
	       { if (!(robotInputReady & rd->ss.code))
		  { FD_SET(rd->hio.rcisock, &rfds);
		  }
		 FD_SET(rd->hio.oobsock, &rfds);
	       }
	      else
	       { FD_SET(rd->hio.accrcisock, &rfds);
	       }
	    }

	   if (refreshPrompt)
	    { printf ("%s", prompt);
	      fflush (stdout);
	      refreshPrompt = 0;
	    }
	   if (graphicsMode)
	    { status = grf4Select(&gEvnt, &rfds, (fd_set*)0, (fd_set*)0, tvp);
	    }
	   else
	    { status = select (FD_SETSIZE, &rfds, (fd_set*)0, (fd_set*)0,tvp);
	      /* XXX should make this immune to EINTR */
	    }
	   if (status < 0)
	    { perror ("Error in select");
	      exit (-1);
	    }
	   else if (status == 0 && gEvnt == 0) /* a timeout occured */
	    { gettimeofday (&tvLastCycle, (struct timezone*)0);
	      if (!freezeOn && (autoCycling || cycleCntLimit > 0))
	       { cycleCnt++;
		 if (!autoCycling)
		  { cycleCntLimit--;
		  }
		 if (cycleRunnable())
		  { doCycle(&tout);
		    tvp = &tout;
		  }
		 else
		  { tvp = 0;		/* no more timeouts for the moment */
		  }
	       }
	      else
	       { tout = tvInterval;	/* try again */
		 tvp = &tout;
	       }
	    }
	   else
	    { if (status > 0)
	       { if (FD_ISSET (CtreeInputFd, &rfds) && interactive)
		  { checkCtreeInput();
		  }
		 for (rd=robots; rd; rd=rd->next)
		  { if (!rd->ss.active)
		     { if (FD_ISSET (rd->hio.accrcisock, &rfds))
			{ 
			  if (robotStart(rd) < 0)
			   { robotLocalRelease(rd);
			     robotListen(rd);
			   }
			  gd = rd->ss.gd;
			  if (gd && (gd->code == gd->active))
			   { /* all robots in group have are active, so
				start them all off */
			     ListElem *re;
			     for (re=gd->robots.head; re; re=re->next)
			      { RobotDesc *r = (RobotDesc*)(re->ptr);
				if (robotCycleFront (r) <= 0)
				 { robotLocalRelease (r);
				   robotListen (r);
				 }
			      }
			     if (gd->active == 0)
			      { robotGroupDelete(gd);
			      }
			     else if (gd->active == gd->code)
			      { scheduleRobotGroup (gd);
			      }
			   }
			}
		     }
		    else
		     { if (FD_ISSET (rd->hio.oobsock, &rfds))
			{ if (robotHostRelease(rd) < 0)
			   { robotLocalRelease(rd);
			   }
			  robotListen(rd);
			  gd = rd->ss.gd;
			  if (gd->active == 0)
			   { robotGroupDelete(gd);
			   }
			}
		       else if (FD_ISSET (rd->hio.rcisock, &rfds))
			{ robotInputReady |= rd->ss.code;
			}
		     }
		  }
	       }
	      if (cycleRunnable())
	       { doCycle(&tout);
		 tvp = &tout;
	       }
	    }
	 }
}

cycleRunnable()
{
	/* Called to see if we can run the next simulator cycle.
	   We can do this if 

		1. the next cycle tick has come in, and 
		2. all robot groups scheduled to be read from
		   that cycle have their input ready.
	 */

	ListElem *ge;
	GroupDesc *gd;
	
	/* Return if frozen or next cycle tick didn't arrive */

	if (cycleCnt == lastCycleCnt)	
	 { return (0);	
	 }

	/* Now make sure input is ready from every robot in every
	   group scheduled for this cycle; i.e., each group whose 
	   cycle count is <= cycleCnt. */

	for (ge=eventList.head; ge; ge=ge->next)
	 { gd = (GroupDesc*)(ge->ptr);
	   if (gd->cycleCnt > cycleCnt)
	    { break;
	    }
	   else if (gd->code & ~(robotInputReady))
	    { return (0);
	    }
	 }
	return (1);
}

doCycle(tvp)
struct timeval *tvp;
{
	/* Primary execution routine for each simulator cycle. 
	   Basically, this routine

	   	1. read input in from the necessary robots
		2. calls simulation and update functions 
		3. writes output to each necessary robot
	 */	  
	struct timeval tnow;
	struct timeval tnext;
	ListElem *ge, *ge_next;
	ListElem *re, *fe;
	GroupDesc *gd;
	RobotDesc *rd;
	SimFxnDesc *fxd;
	ListDesc runList;

	/* First, remove all the robot group events (for this cycle)
	   from the event queue and place them on a separate list. */

	runList.head = runList.tail = 0;
	for (ge=eventList.head; ge; ge=ge_next)
	 { ge_next = ge->next;
	   gd = (GroupDesc*)(ge->ptr);
	   if (gd->cycleCnt > cycleCnt)
	    { break;
	    }
	   listLinkBefore (&runList, 0, (void*)gd);
	   listUnlink (&eventList, ge);
	   gd->event = 0;
	 }

	/* Read input in from each robot */

	for (ge=runList.head; ge; ge=ge->next)
	 { gd = (GroupDesc*)(ge->ptr);
	   for (re=gd->robots.head; re; re=re->next)
	    { rd = (RobotDesc*)(re->ptr);
	      if (robotCycleBack(rd) <= 0)
	       { robotLocalRelease(rd);
		 robotListen(rd);
	       }
	      stateChanged = 1;
	    }
	   robotInputReady &= ~gd->code;
	 }
	
	/* Execute simulator functions and update displays */

	for (fe=fxnList.head; fe; fe=fe->next)
	 { SimFxnDesc *fxd = (SimFxnDesc*)(fe->ptr);
	   (*fxd->fxn)(fxd->arg, fxd->ptr);
	 }
	updateDisplays();

	/* Write output to each robot */

	for (ge=runList.head; ge; ge=ge_next)
	 { ge_next = ge->next;
	   gd = (GroupDesc*)(ge->ptr);
	   for (re=gd->robots.head; re; re=re->next)
	    { rd = (RobotDesc*)(re->ptr);
	      if (rd->ss.active)    /* in case released during cycle back */
	       { if (robotCycleFront(rd) <= 0)
		  { robotLocalRelease(rd);
		    robotListen(rd);
		  }
	       }
	    }
	   if (gd->code == gd->active)
	    { scheduleRobotGroup (gd);
	    }
	   else if (gd->active == 0)
	    { robotGroupDelete(gd);
	    }
	   listUnlink (&runList, ge);
	 }

	/* Compute the timeout time for the next call to select:
	   timeout time = (last cycle time + interval) - now */

	gettimeofday (&tnow, (struct timezone*)0);
	tnext = addTimeval(tvLastCycle, tvInterval);
	*tvp = diffTimeval (tnext, tnow);
	if (timercmp(tvp,&tvZero,<))
	 { *tvp = tvZero;	/* clip negative time to zero */
	 }
	lastCycleCnt = cycleCnt;
}

scheduleRobotGroup (gd)
GroupDesc *gd;
{
	ListElem *ge;
	int cnt;

	cnt = cycleCnt + gd->interval;
	for (ge=eventList.tail; ge; ge=ge->prev)
	 { if (((GroupDesc*)ge->ptr)->cycleCnt <= cnt)
	    { break;
	    }
	 }
	gd->cycleCnt = cnt;
	gd->event = listLinkAfter (&eventList, ge, (void*)gd);
}

printRobotMsg (rd, str)
RobotDesc *rd;
{
	if (!silentMode)
	 { if (interactive)
	    { putchar ('\n');
	    }
	   printf ("%s %s\n", rd->name, str);
	   if (interactive && keytree)
	    { refreshPrompt = 1;
	    }
	 }
}

robotInit(rd)
RobotDesc *rd;
{
	char simname[256];
	char *user;

	if (rd->ss.jlf && rd->ss.matlabFormat)
	 { fprintf (rd->ss.jlf, "%s_j = [\n", rd->ss.jlfName);
	   fflush (rd->ss.jlf);
	 }
	if (rd->ss.clf && rd->ss.matlabFormat)
	 { fprintf (rd->ss.clf, "%s_c = [\n", rd->ss.clfName);
	   fflush (rd->ss.clf);
	 }
	initHostIO (&rd->hio);
	pendantReset(rd);
	initGenericIO(rd);
	initSpecialIO(rd);
	initRobotIO (rd);
	if (initManip (rd, rd->name, rd->ss.initAngles ? 
				     rd->ss.initAngles : NULL) < 0)
	 { printErrors ();
	   exit (-1);
	 }

	user = (char*)getenv("USER");
	sprintf (simname, "%s:%s", rd->name, user ? user : "0");
	hostDevice(&rd->hio, simname, 0/*don't use SIGIO for out band data*/);
	if (rbtInit(rd, !rd->ss.nocal) < 0)
	 { fprintf (stderr, "rbtInit() failed for %s, code 0x%x\n", 
			     rd->name, rd->cs.rbtError);
	   exit (-1);
	 }
	printRobotMsg (rd, "Moper started\n");
}

robotListen(rd)
RobotDesc *rd;
{
#define CAL_LED		0x0001
#define M_NULL		0x8000

	rd->ss.softError = 0;
	rd->ss.hardError = 0;

	if (rbtSetup(rd) < 0)
	 { fprintf (stderr, "rbtSetup() failed for %s, code 0x%x\n", 
			     rd->name, rd->cs.rbtError);
	   exit (-1);
	 }

	setPendant(rd, CAL_LED | M_NULL);
	startPendant(rd);

	if (hostListen(&rd->hio, (int(*)())0, 0) < 0)
	 { fprintf (stderr, "Robot %s: hostListen() failed, code 0x%x\n",
			     rd->name, rd->hio.error);
	   exit (-1);
	 }
	printRobotMsg (rd, "listening");
}

GroupDesc *freeGroupDescs;

#define BUFSIZE	(MAX_DATABUF/sizeof(short))
unsigned short buffer[BUFSIZE];

GroupDesc *robotGroupCreate(rd, rcnt, rids)
RobotDesc *rd;
int rcnt;
short *rids;
{
	GroupDesc *gd;

	if (!(gd = freeGroupDescs))
	 { gd = (GroupDesc*)calloc(1,sizeof(GroupDesc));
	 }
	else
	 { freeGroupDescs = gd->next;
	 }
	gd->interval = rd->cs.interval/(sampleInterval*10);
	
	if (rcnt == 0)
	 { /* only one robot in the group. */
	   listLinkBefore (&gd->robots, (ListElem*)0, (void*)rd);
           gd->code |= rd->ss.code;
	   rd->ss.gd = gd;
	 }
	else
	 { RobotDesc *r;
	   int k, foundRd;

	   foundRd = 0;
	   for (k=0; k<rcnt; k++)
	    { for (r=robots; r; r=r->next)
	       { if (r->rbtID == rids[k])
		  { listLinkBefore (&gd->robots, (ListElem*)0, (void*)r);
		    gd->code |= r->ss.code;
		    if (r == rd)
		     { foundRd = 1;
		     }
		    r->ss.gd = gd;
		  }
	       }
	    }
	   if (!foundRd)
	    { fprintf (stderr, "Robot %s: not in group ID list\n", rd->name);
	      exit (-1);
	    }
	 }
	listLinkAfter (&groupList, (ListElem*)0, gd);
	return (gd);
}

robotGroupDelete(gd)
GroupDesc *gd;
{
	ListElem *re, *re_next;

	for (re=gd->robots.head; re; re=re_next)
	 { re_next = re->next;
	   ((RobotDesc*)re->ptr)->ss.gd = 0;
	   listUnlink (&gd->robots, re);
	 }
	if (re=listGetFirst(&groupList, (void*)gd))
	 { listUnlink (&groupList, re);
	 }
	else
	 { fprintf (stderr, "Robot group is not in group list\n");
	   exit (-1);
	 }
	gd->next = freeGroupDescs;
	freeGroupDescs = gd;
}

robotStart (rd)
RobotDesc *rd;
{
	GroupDesc *gd, *g;
	ListElem *ge;
	RobotDesc *r;
	int rcnt;
	short oneRid;
	short *rids;
	int k, size;
	int foundRd;

	if (hostAttach(&rd->hio) < 0)
	 { harderror (rd, rd->hio.error, 5);
	   return (-1);
	 }
	if ((size = rbtStart (rd, buffer)) < 0)
	 { harderror (rd, rd->cs.rbtError, rd->cs.errorLoc);
	   return (-1);
	 }
	if (hostWrite (&rd->hio, (char*)buffer, size) < 0)
	 { harderror (rd, rd->hio.error, 6);
	   return (-1);
	 }
	if ((size = hostRead (&rd->hio, (char*)buffer, BUFSIZE)) < 0)
	 { harderror (rd, rd->hio.error, 7);
	   return (-1);
	 }
	if (rbtIcton (rd, &rcnt, &rids, buffer, size) < 0)
	 { rd->ss.softError = rd->cs.rbtError;
	 }
	if (rd->cs.opFlags & MASTER_F)
	 { /* SIMULATE: don't support master drive */
	   fprintf (stderr, "Robot %s: master mode not supported\n", rd->name);
	   exit (-1);
	 }
	if (interactive)
	 { rd->ss.numCycles = MAXINT;
	 }
	else
	 { rd->ss.numCycles = 0;
	 }

	gd = 0;
	for (ge=groupList.head; ge; ge=ge->next)
	 { 
	   if (rd->ss.code & ((GroupDesc*)ge->ptr)->code)
	    { gd = (GroupDesc*)(ge->ptr);
	      break;
	    }
	 }
	if (gd)
	 { if (rd->cs.interval/(sampleInterval*10) != gd->interval)
	    { fprintf (stderr, "Robot %s: rate != group rate\n", rd->name);
	      exit (-1);
	    }
	 }
	else
	 { /* build the group associated with the robot */
	   gd = robotGroupCreate (rd, rcnt, rids);
	 }
	gd->active |= rd->ss.code;

	rd->ss.exchangeNumber = 0;
	rd->ss.realTime = 0.0;
	rd->ss.active = 1;

	printRobotMsg (rd, "active (slave)");
	return (1);
	/* update the robot expected data structures */
}

robotCycleFront(rd)
RobotDesc *rd;
{
	int size;

	if (!rd->ss.softError)
	 { if ((size = rbtGather (rd, buffer)) < 0)
	    { rd->ss.softError = rd->cs.rbtError;
	    }
	 }
	if (rd->ss.softError)
	 { if (hostWrite (&rd->hio, (char*)&rd->ss.softError, 1) < 0)
	    { harderror (rd, rd->hio.error, 8);
	      return (-1);
	    }
	   return (0);
	 }
	else
	 { if (hostWrite (&rd->hio, (char*)buffer, size) < 0)
	    { harderror (rd, rd->hio.error, 9);
	      return (-1);
	    }
	 }
	return (1);
}

robotCycleBack(rd)
RobotDesc *rd;
{
	int size;

	if ((size = hostRead (&rd->hio, (char*)buffer, BUFSIZE)) < 0)
	 { harderror (rd, rd->hio.error, 10);
	   return (-1);
	 }
	if (rbtCommands (rd, buffer, size) < 0)
	 { rd->ss.softError = rd->cs.rbtError;
	 }

	rd->ss.exchangeNumber++;
	rd->ss.realTime = rd->ss.exchangeNumber * rd->rs.commandRate;

	if (rd->ss.jlf)
	 { float degrees[MAXJNTS];
	   radianToDegree (degrees, rd->rs.angles, rd->jls);
	   fprintf (rd->ss.jlf, "%-5d ", rd->ss.exchangeNumber);
	   fprintVf (rd->ss.jlf, "%10.5f\n", degrees, rd->jls->numJ);
	   fflush (rd->ss.jlf);
	 }
	if (rd->ss.clf)
	 { TRSF t6;
	   QTRN q;
	   char conf[4];

	   manipFwdKinematics (rd, &t6, conf, rd->rs.angles);
	   trsfToQtrn (&q, &t6);

	   if (rd->ss.matlabFormat)
	    { fprintf (rd->ss.clf, "%-5d  ", rd->ss.exchangeNumber);
	    }
	   else
	    { fprintf (rd->ss.clf, "%-5d  %s  ", rd->ss.exchangeNumber, conf);
	    }
	   fprintVf (rd->ss.clf, "%8.2f  ", &t6.p, 3);
	   fprintVf (rd->ss.clf, "%8.5f\n", &q, 4);
	   fflush (rd->ss.clf);
	 }
	return (1);
}

robotHostRelease(rd)
RobotDesc *rd;
{
	char buf[256];
	int size;

#ifdef LINUX_CFG
	/* MS We can call this sighandler function, but can no longer expect
	   it to return a value.
	*/
	sigHandler(&rd->hio);
       
#else
	if (sigHandler(&rd->hio) < 0)
	 { harderror (rd, rd->hio.error, 1);
	   return (-1);
	 }
#endif
	if ((size = hostRead (&rd->hio, (char*)buffer, BUFSIZE)) < 0)
	 { harderror (rd, rd->hio.error, 2);
	   return (-1);
	 }
	if (!rd->ss.softError)
	 { if (rbtCommands (rd, buffer, size) < 0)
	    { rd->ss.softError = rd->cs.rbtError;
	    }
	 }
	if (rd->ss.softError)
	 { if (hostWrite (&rd->hio, (char*)&rd->ss.softError, 1) < 0)
	    { harderror (rd, rd->hio.error, 3);
	      return (-1);
	    }
	   rbtExit(rd);
	   sprintf (buf, "softerror 0x%x: %s", rd->ss.softError, 
			  getErrMsg(rd->ss.softError));
	 }
	else
	 { if (hostWrite (&rd->hio, (char*)buffer, 0) < 0)
	    { harderror (rd, rd->hio.error, 4);
	      return (-1);
	    }
	   sprintf (buf, "released");
	 }
	printRobotMsg (rd, buf);
	robotDetach (rd);
	hostDetach (&rd->hio);
	rd->ss.active = 0;
}

robotLocalRelease(rd)
RobotDesc *rd;
{
	int code;
	char buf[256];

	if (code = rd->ss.hardError)
	 { sprintf (buf, "harderror 0x%x: %s at %d",
		    code, getErrMsg(code), rd->ss.hardErrLoc);
	   rbtExit(rd);
	 }
	else if (code = rd->ss.softError)
	 { sprintf (buf, "softerror 0x%x: %s", code, getErrMsg(code));
	   rbtExit(rd);
	 }
	else
	 { sprintf (buf, "released");
	 }
	printRobotMsg (rd, buf);
	robotDetach (rd);
	hostDetach (&rd->hio);
	rd->ss.active = 0;
}

robotDetach(rd)
RobotDesc *rd;
{
	/* Remove robot rd from its group. */

	GroupDesc *gd;
	ListElem *re, *re_next;

	gd = rd->ss.gd;
	if (gd)
	 { gd->active &= ~rd->ss.code;
	   if (gd->event)
	    { listUnlink (&eventList, gd->event);
	      gd->event = 0;
	    }
	 }
	robotInputReady &= ~rd->ss.code;
}

closeJointLogFile(rd)
RobotDesc *rd;
{
	if (rd->ss.jlf)
	 { if (rd->ss.matlabFormat)
	    { fprintf (rd->ss.jlf, "];\n");
	    }
	   fclose (rd->ss.jlf);
	   rd->ss.jlf = NULL;
	 }
}

closeCartesianLogFile(rd)
RobotDesc *rd;
{ 
	if (rd->ss.clf)
	 { if (rd->ss.matlabFormat)
	    { fprintf (rd->ss.clf, "];\n");
	    }
	   fclose (rd->ss.clf);
	   rd->ss.clf = NULL;
	 }
}

closeRobotLogFiles()
{
	RobotDesc *rd;
	for (rd=robots; rd; rd=rd->next)
	 { closeJointLogFile (rd);
	   closeCartesianLogFile (rd);
	 }
}

int ival1, ival2;
float fval1, fval2;
char sval1[256];
char sval2[256];

buildCtree()
{
	if (keytree == NULL)
	 { keytree = tree_match_parse("					\
	    (quit							\
	     robot							\
	      (%s", sval1, ",		\"robot name\")			\
	     autocycling (on off)					\
	     step							\
	      (%d", &ival1,",0,,	\"number of cycles\")		\
	     set							\
	      (tool 							\
	       force							\
		(all 							\
		 [fx fy fz tx ty tz]					\
		  (%f", &fval1, ",,,	\"force value\"))		\
	       displacement						\
		(all 							\
		 [dx dy dz rx ry rz]					\
		  (%f", &fval1, ",,,	\"displacement value\"))	\
	       pioIn							\
		(%d", &ival2, ",,,	\"channel number\"		\
		  (%x", &ival1, ",,,	\"value (hex)\"))		\
	       ADC							\
		(%d", &ival2, ",,,	\"channel number\"		\
		  (%d", &ival1, ",,,	\"value (dec)\"))		\
	       joystick							\
		(all							\
		 [1 2 3 4 5 6]						\
		  (%f", &fval1, ",,,	\"joystick value\"))		\
	      )								\
	     clear							\
	      (tool force displacement					\
	      )								\
	     postscript							\
	      ([eps idraw]						\
		(%f", &fval1, ",,,	\"width (inches)\"		\
		(%f", &fval2, ",,,	\"height (inches)\"		\
		(%s", sval1,  ",	\"file name\" ))))		\
	     pendant							\
	      (reset							\
		release							\
	       button							\
	        (REC hold-run clamp1 clamp2 tool world joint free comp  \
	         next-step move-tp pound panic tp+ tp- tick fast slow medium\
		 x- x+ y- y+ z- z+ rx- rx+ ry- ry+ rz- rz+)		\
	      )								\
	     show 							\
	      (tool force displacement pendant				\
	       pio ADC/DAC options sim controller robot graphics	\
	      )								\
	     open							\
	      ([cartesian joint]					\
		(log							\
		  (%s", sval1, ",	\"log file name\")))		\
	     close							\
	      ([cartesian joint]					\
		(log))							\
	    )", 0);

	   if (keytree == 0)
	    { printf ("CtreeMatcher: parse error at %.30s\n", 
		       tree_match_err_at());
	      exit (-1);
	    }
	 }
}

checkCtreeInput()
{
	static int inputFd;
	static int first = 1;
	int newFd;

	while (tree_match_nb (keytree, prompt, key_buf, &newFd))
	 { tree_match_reset();
	   doCtreeCommand();
	   tree_match_setup();
	 }
	if (first)
	 { CtreeInputFd = newFd;
	   first = 0;
	 }
	else
	 { if (newFd != CtreeInputFd)
	    { CtreeInputFd = newFd;
	    }
	 }
}

doCtreeCommand()
{ 
	RobotDesc *rd = interactingRobot;

	if (COMMAND ("quit"))
	 { programExit(0);
	 }
	else if (COMMAND ("robot"))
	 { RobotDesc *rd;
	   if ((rd = getRobotByName(sval1)) == NULL)
	    { printf ("Can't find robot %s\n", sval1);
	    }
	   changeInteractingRobot(rd);
	   return;
	 }
	else if (COMMAND ("show graphics"))
	 { if (!graphicsMode)
	    { fprintf (stderr, "Graphics are not active\n");
	    }
	   else
	    { float roll, pitch, yaw;
	      TRSF view;

	      putchar ('\n');

	      grf4GetView (&view);
	      trsfToRpy (&roll, &pitch, &yaw, &view);
	      printf ("view transform:  x=%g y=%g z=%g\n", 
		       view.p.x, view.p.y, view.p.z);
	      printf ("                 roll=%g pitch=%g yaw=%g\n", 
		       roll, pitch, yaw);
	      printf ("viewing pyramid: left=%g right=%g bottom=%g top=%g\n",
		       viewWinPar[0],viewWinPar[1],viewWinPar[2],viewWinPar[3]);
	      printf ("                 near=%g far=%g\n",
		       viewWinPar[4],viewWinPar[5]);

	      putchar ('\n');
	    }
	   return;
	 }
	else if (COMMANDS ("postscript"))
	 { if (!graphicsMode || !(graphicsFlags & GRF_WIRE_FRAME))
	    { fprintf (stderr,"Wire frame graphics needed for this option\n");
	    }
	   else
	    { FILE *fp;

	      if ((fp = fopen (sval1, "w")) == NULL)
	       { printf ("Can't open file %s\n", sval1);
		 return;
	       }
	      if (COMMAND ("postscript idraw"))
	       { grf4DumpPostScript(fp, sval1, /*flags=*/GRF_IDRAW_PS, 
					/*width=*/72*fval1,
					/*height=*/72*fval2);
	       }
	      else
	       { grf4DumpPostScript(fp, sval1, /*flags=*/0,
					/*width=*/72*fval1, 
					/*height=*/72*fval2);
	       }
	      grf4HideLines(1);
	      graphicsDraw();
	      grf4HideLines(0);
	      fclose(fp);
	    }
	   return;
	 }
	else if (COMMAND("step"))
	 { if (ival1 < 0)
	    { fprintf (stderr, "Nice try\n");
	    }
	   else
	    { cycleCntLimit += ival1;
	    }
	   return;
	 }
	else if (COMMAND("autocycling on"))
	 { autoCycling = 1;
	 }
	else if (COMMAND("autocycling off"))
	 { autoCycling = 0;
	   cycleCntLimit = 0;
	 }
	else if (COMMAND ("show sim"))
	 { if (autoCycling)
	    { printf ("autocycling ON\n");
	    }
	   else
	    { printf ("autocycling OFF\n");
	    }
	   putchar('\n');
	   printf ("cycle count:    %d\n", cycleCnt);
	   if (!autoCycling)
	    { printf ("pending cycles: %d\n", cycleCntLimit);
	    }
	   putchar('\n');
	   return;
	 }

	if (!rd)
	 { fprintf(stderr, "No robot defined\n");
	   return;
	 }
	if (COMMAND("set tool"))
	 { double x, y, z, roll, pitch, yaw;
	   TRSF tool;
	   
	   printf ("Enter x, y, z, roll, pitch, and yaw > ");
	   if (scanf ("%lf%lf%lf%lf%lf%lf", 
		       &x, &y, &z, &roll, &pitch, &yaw) != 6)
	    { printf ("Cannot scan values\n");
	    }
	   else 
	    { xyzToTrsf (&tool, x, y, z);
	      rpyToTrsf  (&tool, roll, pitch, yaw);
	      rd->rs.tool = tool;
	    }
	 }
	else if (COMMANDS("set force"))
	 { if (COMMAND ("set force all"))
	    { float f[6];
	      printf ("fx,fy,fz,tx,ty,tz > ");
	      if (scanVf (f, 6) != 1)
	       { printf ("Error reading values\n");
		 return;
	       }
	      bcopy (f, &rd->rs.extForce, sizeof(FORCE));
	    }
	   else
	    { int k = key_buf[strlen("set force x")] - 'x';
	      if (COMMANDS ("set force t"))
	       { k += 3;
	       }
	      switch (k)
	       { case 0: rd->rs.extForce.f.x = fval1; 
			 break;
		 case 1: rd->rs.extForce.f.y = fval1; 
			 break;
		 case 2: rd->rs.extForce.f.z = fval1; 
			 break;
		 case 3: rd->rs.extForce.m.x = fval1; 
			 break;
		 case 4: rd->rs.extForce.m.y = fval1; 
			 break;
		 case 5: rd->rs.extForce.m.z = fval1; 
			 break;
	       }
	    }
	 }
	else if (COMMANDS("set displacement"))
	 { int k;
       
	   if (COMMAND ("set displacement all"))
	    { float d[6];
	      
	      printf ("fx,fy,fz,tx,ty,tz > ");
	      if (scanVf (d, 6) != 1)
	       { printf ("Error reading values\n");
		 return;
	       }
	      for (k=3; k<6; k++)
	       { d[k] *= DEGTORAD;
	       }
	      bcopy (d, &rd->rs.extPosErr, sizeof(DIFF));
	    }
	   else
	    { int k = key_buf[strlen("set displacement x")] - 'x';
	      if (COMMANDS ("set displacement r"))
	       { k += 3;
	       }
	      switch (k)
	       { case 0: rd->rs.extPosErr.t.x = fval1; 
			 break;
		 case 1: rd->rs.extPosErr.t.y = fval1; 
			 break;
		 case 2: rd->rs.extPosErr.t.z = fval1; 
			 break;
		 case 3: rd->rs.extPosErr.r.x = fval1*DEGTORAD;
			 break;
		 case 4: rd->rs.extPosErr.r.y = fval1*DEGTORAD;
			 break;
		 case 5: rd->rs.extPosErr.r.z = fval1*DEGTORAD;
			 break;
	       }
	    }
	 }
	else if (COMMAND("clear tool"))
	 { identTrsf (&rd->rs.tool);
	 }
	else if (COMMAND("clear force"))
	 { bzero (&rd->rs.extForce, sizeof(FORCE));
	 }
	else if (COMMAND("clear displacement"))
	 { bzero (&rd->rs.extPosErr, sizeof(DIFF));
	 }
	else if (COMMAND ("clear joystick"))
	 { int k;
	   for (k=0; k<6; k++)
	    { setJstick (rd, k, 0);
	    }
	 }
	else if (COMMAND ("set pioIn"))
	 { if (ival2 < 0 || ival2 >= rd->sio.numPioIn)
	    { printf (
"Channel out of range (%d available)\n", rd->sio.numPioIn);
	      return;
	    }
	   rd->sio.pioInBuf[ival2] = ival1;
	 }
	else if (COMMAND ("set ADC"))
	 { if (ival2 < 0 || ival2 >= rd->sio.numADC)
	    { printf (
"Channel out of range (%d available)\n", rd->sio.numADC);
	      return;
	    }
	   rd->sio.ADCBuf[ival2] = ival1;
	 }
	else if (COMMANDS ("set joystick"))
	 { int k;
	   if (COMMAND ("set joystick all"))
	    { float jvalues[6];
	      if (scanVf (jvalues, 6) != 1)
	       { printf ("Error reading values\n");
		 return;
	       }
	      for (k=0; k<6; k++)
	       { setJstick (rd, k, (int)jvalues[k]);
	       }
	    }
	   else
	    { k = key_buf[strlen("set joystick ")] - '0';
	      setJstick (rd, k, (int)fval1);
	    }
	 }
	else if (COMMAND ("open cartesian log"))
	 { if (rd->ss.clf)
	    { closeCartesianLogFile(rd);
	    }
	   if (rd->ss.clfName)
	    { free (rd->ss.clfName);
	    }
	   if ((rd->ss.clf = fopen(sval1,"w")) == NULL)
	    { fprintf (stderr, "Cannot open file %s\n", sval1);
	      return;
	    }
	   rd->ss.clfName = copystr(sval1);
	 }
	else if (COMMAND ("close cartesian log"))
	 { if (rd->ss.clf)
	    { closeCartesianLogFile(rd);
	    }
	 }
	else if (COMMAND ("open joint log"))
	 { if (rd->ss.jlf)
	    { closeJointLogFile(rd);
	    }
	   if (rd->ss.jlfName)
	    { free (rd->ss.jlfName);
	    }
	   if ((rd->ss.jlf = fopen(sval1,"w")) == NULL)
	    { fprintf (stderr, "Cannot open file %s\n", sval1);
	      return;
	    }
	   rd->ss.jlfName = copystr(sval1);
	 }
	else if (COMMAND ("close joint log"))
	 { if (rd->ss.jlf)
	    { closeJointLogFile(rd);
	    }
	 }
	else if (COMMAND ("show options"))
	 { putchar ('\n');
	   printc (-30, "numSlu = %d", rd->sio.numSlu);
	   printc (-30, "moperID = %d", rd->cs.moperID);
	   putchar ('\n');

	   printc (-30, "numPioIn = %d", rd->sio.numPioIn);
	   printc (-30, "robotID = %d", rd->cs.robotID);
	   putchar ('\n');

	   printc (-30, "numPioOut = %d", rd->sio.numPioOut);
	   putchar ('\n');

	   printc (-30, "numADC = %d", rd->sio.numADC);
	   putchar ('\n');

	   printc (-30, "numDAC = %d", rd->sio.numDAC);
	   putchar ('\n');

	   printf ("Available features:\n");
	   if (!(rd->sio.torqueAvail  || rd->sio.jstickAvail || 
		 rd->sio.pendantAvail || rd->sio.forceAvail  || 
		 rd->sio.potsAvail))
	    { printf ("NONE\n");
	    }
	   else
	    { printf ("%s%s%s%s%s\n", 
		     rd->sio.torqueAvail ? "TORQUE " : "",
		     rd->sio.forceAvail ? "FORCE " : "",
		     rd->sio.jstickAvail ? "JOYSTICK " : "",
		     rd->sio.pendantAvail ? "PENDANT " : "",
		     rd->sio.potsAvail ? "POTS " : "");
	    }		 	
	 }
	else if (COMMAND ("show controller"))
	 { int j;

	   putchar('\n');

	   printc (-30, "arm power %s", armPowerOn(rd) ? "ON" : "OFF");
	   printc (-30, "calib flag = %hd", rd->cs.calibrated);
	   putchar ('\n');
	   putchar ('\n');
	   printf ("Joint Status Register:\n");
	   for (j=0; j<rd->jls->numJ; j++)
	    { printf (" %2d  0x%04hx\n", j+1, rd->cs.jstat[j]);
	    }
	   putchar ('\n');
	 }
	else if (COMMAND ("show robot"))
	 { printf ("Robot %s:\n", rd->name);
	   putchar ('\n');
	   display (rd, 0xffffffff);
	   putchar ('\n');
	 }
	else if (COMMAND ("show tool"))
	 { printTrsf ("%m\n", &rd->rs.tool);
	 }
	else if (COMMAND ("show force"))
	 { FORCE *f = &rd->rs.extForce;
	   printf ("%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",
		    f->f.x, f->f.y, f->f.z, f->m.x, f->m.y, f->m.z);
	 }
	else if (COMMAND ("show displacement"))
	 { DIFF *d = &rd->rs.extPosErr;
	   printf ("%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",
		    d->t.x, d->t.y, d->t.z, 
		    RADTODEG*d->r.x, RADTODEG*d->r.y, RADTODEG*d->r.z);
	 }
	else if (COMMAND ("show pendant"))
	 { printf ("\n");
	   printf ("%04x\n", pendantWord(rd, 0));
	   printf ("%04x\n", pendantWord(rd, 1));
	   printf ("%04x\n", pendantWord(rd, 2));
	   printf ("\n");
	 }
	else if (COMMAND ("show pio"))
	 { int k;

	   putchar ('\n');		
	   printf ("CHAN   INPUT  OUTPUT\n");
	   /*	      DD   0xXXXX  0xXXXX	*/
	   for (k=0; k<rd->sio.numPioOut || k<rd->sio.numPioIn; k++)
	    { printf (" %2d  ", k);
	      if (k<rd->sio.numPioIn)
	       { printf ("0x%04hx  ", rd->sio.pioInBuf[k]);
	       }
	      else
	       { printf ("        ");
	       }
	      if (k<rd->sio.numPioOut)
	       { printf ("0x%04hx  ", rd->sio.pioOutBuf[k]);
	       }
	      putchar ('\n');
	    }
	   putchar ('\n');		
	 }
	else if (COMMAND ("show ADC/DAC"))
	 { int k;

	   putchar ('\n');		
	   printf ("CHAN    ADC     DAC\n");
	   /* 	      DD  -DDDDD  -DDDDD	*/
	   for (k=0; k<rd->sio.numADC || k<rd->sio.numDAC; k++)
	    { printf (" %2d  ", k);
	      if (k<rd->sio.numADC)
	       { short value;
		 getADC (rd, k, &value);
		 printf ("%6d  ", (int)value);
	       }
	      else
	       { printf ("        ");
	       }
	      if (k<rd->sio.numDAC)
	       { printf ("%6hd", rd->sio.DACBuf[k]);
	       }
	      putchar ('\n');
	    }
	   putchar ('\n');		
	 }
	else if (COMMAND ("pendant reset"))
	 { pendantReset(rd);
	 }
	else if (COMMAND ("pendant release"))
	 { pendantButtonRelease (rd);
	 }
	else if (COMMANDS ("pendant button"))
	 { pendantButtonHit (rd, &key_buf[15]);
	 }
}

updateDisplays()
{
	if (graphicsMode && stateChanged)
	 { graphicsDraw();
	   grf4Cycle();
	   stateChanged = 0;
	 }
}

graphicsDraw()
{
	LocFxnDesc *fxd;
	ListElem *e;
	DrawDesc *dd;

	TRSF baseFrame;
	TRSF frame;

	grf4PushTrans();
	grf4Clear();

	grf4GetTrans (&baseFrame);

	for (e=drawList.head; e; e=e->next)
	 { dd = (DrawDesc*)e->ptr;
	   switch (dd->code)
	    { case DRAW_ROBOT:
	       { drawRobot (dd->u.rd, &baseFrame);
		 break;
	       }
	      case DRAW_STATIC_ROBOT:
	       { drawStaticRobot (dd->u.sd, &baseFrame);
		 break;
	       }
	      case DRAW_OBJECT:
	       { drawObject (dd->u.od, &baseFrame);
		 break;
	       }
	    }
	 }

	grf4Flush();
	grf4PopTrans();
}

static int msgButton;
static int rbtSelButton;

static int toolButton;
static int worldButton;
static int jointButton;
static int freeButton;

static int fastButton;
static int mediumButton;
static int slowButton;

static int speedButton;

changeInteractingRobot(new)
RobotDesc *new;
{
	unsigned short oldmsg, newmsg;

	RobotDesc *old = interactingRobot;
	int bnum;

	if (new == old)
	 { return;
	 }
	grf4SetButtonName (rbtSelButton, new->name);
	oldmsg = old->sio.pendantWBuf;
	newmsg = new->sio.pendantWBuf;
	interactingRobot = new;
	updateGraphicPendant (new, newmsg, oldmsg);
	switch (new->sio.pendantSpeed)
	 { case 0:
	    { bnum = slowButton;
	      break;
	    }
	   case 1:
	    { bnum = mediumButton;
	      break;
	    }
	   case 2:
	    { bnum = fastButton;
	      break;
	    }
	 }
	if (bnum != speedButton)
	 { clearButton(speedButton);
	   lightButton(bnum);
	   speedButton = bnum;
	 }
}

onRbtSelButton(n, code, arg)
int n, code;
int arg;
{
	RobotDesc *rd = interactingRobot;

	if (code == BF_PRESSED)
	 { if (rd)
	    { rd = (rd->next ? rd->next : robots);
	      changeInteractingRobot(rd);
	    }
	 }
}

onFreezeButton(n, code, arg)
int n, code;
int arg;
{
	int flags;

	if (code == BF_RELEASED)
	 { flags = grf4GetButtonFlags (n);
	   if (freezeOn)
	    { grf4SetButtonFlags (n, flags & ~(BF_ILLUMINATED));
	      freezeOn = 0;
	    }
	   else
	    { grf4SetButtonFlags (n, flags | BF_ILLUMINATED);
	      freezeOn = 1;
	    }
	 }
}

onQuitButton(n, code)
int n, code;
{
	if (code == BF_RELEASED)
	 { programExit(0);
	 }
}

onHideButton(n, code)
int n, code;
{
	if (code == BF_RELEASED)
	 { grf4HideLines(1);
	   graphicsDraw();
	   grf4HideLines(0);
	 }
}

onPdntButton(n, code, arg)
int n, code;
int arg;
{
	char *str = (char*)arg;
	RobotDesc *rd = interactingRobot;

	if (code == BF_PRESSED)
	 { pendantButtonHit (rd, str);
	 }
	else if (code == BF_RELEASED)
	 { pendantButtonRelease (rd);
	 }

	if (n == jointButton || n == worldButton ||
	    n == toolButton || n == freeButton)
	 { return;
	 }
	else if (n == fastButton || n == mediumButton || n == slowButton)
	 { if (code == BF_PRESSED)
	    { if (speedButton && speedButton != n)
	       { clearButton (speedButton);
	       }
	      lightButton (n);
	      speedButton = n;
	    }
	 }
	else if (code == BF_PRESSED)
	 { pendantButtonHit (rd, str);
	   lightButton (n);
	 }
	else if (code == BF_RELEASED)
	 { pendantButtonRelease (rd);
	   clearButton (n);
	 }
}

updateGraphicPendant(r, new, old)
RobotDesc *r;
unsigned short new, old;
{
	int newMode, oldMode;
	int newMsg;
	
	newMode = (new & 0xe);
	oldMode = (old & 0xe);

	newMsg = (new & 0xff00);

	if (newMode != oldMode)
	 { switch (oldMode)
	    { case TOOL_LED:
	       { clearButton (toolButton);
		 break;
	       }
	      case WORLD_LED:
	       { clearButton (worldButton);
		 break;
	       }
	      case JOINT_LED:
	       { clearButton (jointButton);
		 break;
	       }
	      case FREE_LED:
	       { clearButton (freeButton);
		 break;
	       }
	      default:
	       { break;
	       }
	    }
	   switch (newMode)
	    { case TOOL_LED:
	       { lightButton (toolButton);
		 break;
	       }
	      case WORLD_LED:
	       { lightButton (worldButton);
		 break;
	       }
	      case JOINT_LED:
	       { lightButton (jointButton);
		 break;
	       }
	      case FREE_LED:
	       { lightButton (freeButton);
		 break;
	       }
	      default:
	       { break;
	       }
	    }
	 }
	if (newMsg != (old & 0xff00))
	 { char *str;
	   switch (newMsg)
	    { default:
	      case M_NULL:
	       { str = " ";
		 break;
	       }
	      case M_LIMIT1:
	       { str = "limit stop 1";
		 break;
	       }
	      case M_LIMIT2:
	       { str = "limit stop 2";
		 break;
	       }
	      case M_LIMIT3:
	       { str = "limit stop 3";
		 break;
	       }
	      case M_LIMIT4:
	       { str = "limit stop 4";
		 break;
	       }
	      case M_LIMIT5:
	       { str = "limit stop 5";
		 break;
	       }
	      case M_LIMIT6:
	       { str = "limit stop 6";
		 break;
	       }
	      case M_LIMIT7:
	       { str = "limit stop 7";
		 break;
	       }
	      case M_TOO_CLOSE:
	       { str = "too close";
		 break;
	       }
	      case M_TOO_FAR:
	       { str = "too far";
		 break;
	       }
	      case M_TEACH_MODE_J:
	       { str = "teach mode j";
		 break;
	       }
	      case M_MANUAL_MODE:
	       { str = "manual mode";
		 break;
	       }
	      case M_ARM_PWR_OFF:
	       { str = "arm pwr off";
		 break;
	       }
	      case M_PROG_RUNNING:
	       { str = "prog running";
		 break;
	       }
	      case M_HOLD:
	       { str = "hold";
		 break;
	       }
	      case M_FATAL_ERROR:
	       { str = "fatal error";
		 break;
	       }
	      case M_UNIMATION1:
	       { str = "unimation1";
		 break;
	       }
	      case M_NO_POINTS:
	       { str = "no points";
		 break;
	       }
	      case M_TEACH_MODE_S:
	       { str = "teach mode s";
		 break;
	       }
	      case M_COMP_MODE:
	       { str = "comp mode";
		 break;
	       }
	    }
	   grf4SetButtonName (msgButton,str);
	 }
}

graphicsInit()
{
	RobotDesc *rd;
	StaticRobotDesc *sd;
	DrawDesc *dd;
	MaterialDesc *md;
	LightDesc *ld;
	ListElem *e;

	int winw, winh;
	void *light1, *light2;
	extern void *rbtGraphicsInit();
	float radius, lastRadius, below, above;
	float autoPlaceWidth;
	float maxBase;
	float near, far;
	float vw, vh;
	int flags;

	/* variables for _rough_ estimate of robot cell bounding solid: */

	float xmax, xmin;
	float ymax, ymin;
	float zmax, zmin;

	xmax = ymax = zmax = -1e20;
	xmin = ymin = zmin =  1e20;

	autoPlaceWidth = maxBase = 0;

	/* if the config file specifies the window placement, handle
	   that here: */

	if (windowPosConfig)
	 { int place[4];

	   place[0] = -1;
	   place[1] = -1;
	   place[2] = -1;
	   place[3] = -1;

	   pdbSetVecI (windowPosConfig->val.set, place);

	   grf4PlaceWindow (place[0], place[1], place[2], place[3]);
	 }

	grf4AddButton ("quit", 0, 1, onQuitButton, 0);
	grf4AddButton ("freeze", BF_SAMEROW, 1, onFreezeButton, 0);

	if (graphicsFlags & GRF_WIRE_FRAME)
	 { grf4AddButton ("hide lines", 0, 1, onHideButton, 0);
	 }

	 { char *name = NULL;
	   int maxLen = 0;
	   for (rd=robots; rd; rd=rd->next)
	    { if (strlen(rd->name) > maxLen)
	       { maxLen = strlen(rd->name);
		 name = rd->name;
	       }
	    }
	   if (!name)
	    { name = "no robot";
	    }
	   rbtSelButton = grf4AddButton (name, BF_ILLUMINATED, 1, 
					       onRbtSelButton, 0);
	 }

	if (doGraphicPendant)
	 { grf4AddButton ("Teach Pendant", BF_NOTBOXED|BF_INACTIVE, 1, 0, 0);


	   grf4AddButton("record", 0, 1, onPdntButton, "REC");
	   msgButton = grf4AddButton(" ", 
	      BF_ILLUMINATED|BF_SAMEROW|BF_NOTBOXED|BF_INACTIVE, 2, 0, 0);

	   grf4AddButton("hand1", 0, 2, onPdntButton, "clamp1");
	   grf4AddButton("  x/1", BF_SAMEROW|BF_NOTBOXED|BF_INACTIVE, 2,0,0);
	   grf4AddButton("+", BF_SAMEROW, 1, onPdntButton, "x+");
	   grf4AddButton("-", BF_SAMEROW, 1, onPdntButton, "x-");

	   grf4AddButton("hand2", 0, 2, onPdntButton, "clamp2");
	   grf4AddButton("  y/2", BF_SAMEROW|BF_NOTBOXED|BF_INACTIVE, 2,0,0);
	   grf4AddButton("+", BF_SAMEROW, 1, onPdntButton, "y+");
	   grf4AddButton("-", BF_SAMEROW, 1, onPdntButton, "y-");

	   freeButton = grf4AddButton("free", 0, 2, onPdntButton, "free");
	   grf4AddButton("  z/3", BF_SAMEROW|BF_NOTBOXED|BF_INACTIVE, 2,0,0);
	   grf4AddButton("+", BF_SAMEROW, 1, onPdntButton, "z+");
	   grf4AddButton("-", BF_SAMEROW, 1, onPdntButton, "z-");

	   toolButton = grf4AddButton("tool", 0, 2, onPdntButton, "tool");
	   grf4AddButton(" rx/4", BF_SAMEROW|BF_NOTBOXED|BF_INACTIVE, 2,0,0);
	   grf4AddButton("+", BF_SAMEROW, 1, onPdntButton, "rx+");
	   grf4AddButton("-", BF_SAMEROW, 1, onPdntButton, "rx-");

	   worldButton = grf4AddButton("world", 0, 2, onPdntButton, "world");
	   grf4AddButton(" ry/5", BF_SAMEROW|BF_NOTBOXED|BF_INACTIVE, 2,0,0);
	   grf4AddButton("+", BF_SAMEROW, 1, onPdntButton, "ry+");
	   grf4AddButton("-", BF_SAMEROW, 1, onPdntButton, "ry-");

	   jointButton = grf4AddButton("joint", 0, 2, onPdntButton, "joint");
	   grf4AddButton(" rz/6", BF_SAMEROW|BF_NOTBOXED|BF_INACTIVE, 2,0,0);
	   grf4AddButton("+", BF_SAMEROW, 1, onPdntButton, "rz+");
	   grf4AddButton("-", BF_SAMEROW, 1, onPdntButton, "rz-");

	   fastButton = grf4AddButton("fast", 0, 1, onPdntButton, "fast");
	   mediumButton = grf4AddButton("med", BF_SAMEROW|BF_ILLUMINATED, 1, 
					 onPdntButton, "medium");
	   speedButton = mediumButton;
	   slowButton = grf4AddButton("slow", BF_SAMEROW, 1, 
					 onPdntButton, "slow");
	 }

	if (grf4Init ("robotsim", fgColor, bgColor, graphicsFlags) < 0)
	 { return (0);
	 }

	if (robots)
	 { grf4SetButtonName (rbtSelButton, robots->name);
	 }

	lastRadius = 0;
	for (rd=robots; rd; rd=rd->next)
	 { if (!(rd->rgp = rbtGraphicsInit(rd->name)))
	    { fprintf (stderr, "Robot %s: couldn't init graphics\n",
				rd->name);
	      exit (1);
	    }
	   rbtBounds (rd->rgp, &radius, &below, &above);
	   autoPlaceWidth += 1.5*lastRadius;
	   if (below > maxBase)
	    { maxBase = below;
	    }
	   lastRadius = radius;
	 }
	for (sd=staticRobots; sd; sd=sd->next)
	 { if (!(sd->rgp = rbtGraphicsInit(sd->name)))
	    { fprintf (stderr, "Robot %s: couldn't init graphics\n",
				sd->name);
	      exit (1);
	    }
	   rbtBounds (sd->rgp, &radius, &below, &above);
	   autoPlaceWidth += 1.5*lastRadius;
	   if (below > maxBase)
	    { maxBase = below;
	    }
	   lastRadius = radius;
	 }
	lastRadius = 0;
	for (rd=robots; rd; rd=rd->next)
	 { rbtBounds (rd->rgp, &radius, &below, &above);
	   /* if there is no location, define one */
	   if (rd->loc == 0)
	    { rd->loc = ALLOC(TRSF);
	      identTrsf (rd->loc);
	      rd->loc->p.x = 1.5*lastRadius - autoPlaceWidth/2;
/*	      rd->loc->p.z = -maxBase; */
	    }
	   if (!rd->configured)
	    { addNewDrawable (DRAW_ROBOT, (void*)rd);
	      rd->hasEndEffector = 1;
	    }
	   lastRadius = radius;
	   xmax = MAX(xmax, rd->loc->p.x+radius);
	   ymax = MAX(ymax, rd->loc->p.y+radius);
	   zmax = MAX(zmax, rd->loc->p.z+above+below);
	   xmin = MIN(xmin, rd->loc->p.x-radius);
	   ymin = MIN(ymin, rd->loc->p.y-radius);
	   zmin = MIN(zmin, rd->loc->p.z);

	 }
	for (sd=staticRobots; sd; sd=sd->next)
	 { rbtBounds (sd->rgp, &radius, &below, &above);
	   /* if there is no location, define one */
	   if (sd->loc == 0)
	    { sd->loc = ALLOC(TRSF);
	      identTrsf (sd->loc);
	      sd->loc->p.x = 1.5*lastRadius - autoPlaceWidth/2;
/*	      sd->loc->p.z = -maxBase; */
	    }
	   lastRadius = radius;
	   xmax = MAX(xmax, sd->loc->p.x+radius);
	   ymax = MAX(ymax, sd->loc->p.y+radius);
	   zmax = MAX(zmax, sd->loc->p.z+above+below);
	   xmin = MIN(xmin, sd->loc->p.x-radius);
	   ymin = MIN(ymin, sd->loc->p.y-radius);
	   zmin = MIN(zmin, sd->loc->p.z);
	 }

	if (robots || staticRobots)
	 { vw = (xmax-xmin)/2;
	   vh = (zmax-zmin)/2;
	   near = 5*MAX(vw,vh);
	   far =  7*MAX(vw,vh);
	 }

	grf4GetWindowSize (&winw, &winh);

	/* Set up the view window */

	 { float w[6];

	   if (!robots && !staticRobots)
	    { w[0] = -1;
	      w[1] = 1;
	      w[2] = -1;
	      w[3] = 1;
	      w[4] = 5;
	      w[5] = 2000;
	    }
	   else if (orthoProjection)
	    { w[0] = -vw;	/* left */
	      w[1] =  vw;	/* right */
	      w[2] = -vh;	/* bottom */
	      w[3] =  vh;	/* top */
	      w[4] = 0.0;	/* near */
	      w[5] =  2*far;	/* far */
	    }
	   else
	    { w[0] = -vw/100;	/* left */
	      w[1] =  vw/100;	/* right */
	      w[2] = -vh/100;	/* bottom */
	      w[3] =  vh/100;	/* top */
	      w[4] = near/100;	/* near */
	      w[5] =  2*far;	/* far */
	    }

	   if (viewWindowConfig)
	    { pdbSetVecF (viewWindowConfig->val.set, w);
	    }
	   if (orthoProjection)
	    { grf4Ortho (w[0],w[1],w[2],w[3],w[4],w[5], /*aspect=*/1.0);
	    }
	   else
	    { grf4Window (w[0],w[1],w[2],w[3],w[4],w[5], /*aspect=*/1.0);
	    }
	   bcopy ((char*)w, (char*)viewWinPar, sizeof(w));
	 }

	/* Set up the view transform */

	if (viewTransConfig)
	 { TRSF loc;
	   if (setLocation (viewTransConfig, &loc) < 0)
	    { errorMsg (0, "Config file: bad spec. for viewTrans\n");
	      printErrors();
	      exit (-1);
	    }
	   grf4SetViewTrsf (&loc);
	 }
	else
	 { float v[6];

	   if (!robots && !staticRobots)
	    { v[0] = 0.0;	/* x */
	      v[1] = 0.0;	/* y */
	      v[2] = -600;	/* z */
	    }
	   else
	    { v[0] = 0.0;	/* x */
	      v[1] = -maxBase;	/* y */
	      v[2] = -(near+far)/2; /* z */
	    }
	   v[3] = 0.0;		/* rx */
	   v[4] = 0.0;		/* ry */
	   v[5] = 20.0;		/* rz */

	   grf4SetView (v[0],v[1],v[2], v[3],v[4],v[5]);
	 }

	grf4Rotate (0.0, 0.0, -90.0);
	grf4SetDrawFxn (graphicsDraw, 0);

	/* set the defined lights, if there are any, or set
	   up some default lights */

	if (lights)
	 { for (ld=lights; ld; ld=ld->next)
	    { if ((ld->ptr = grf4DefLight (ld->color, ld->ambient,
					   ld->x, ld->y, ld->z, ld->w))==0)
	       { fprintf(stderr, "Error setting light from definition\n");
		 exit (-1);
	       }
	      grf4SetLight(ld->num,ld->ptr);
	    }
	 }
	else
	 { light1 = grf4DefLight (0xffffff, 0x404040, 1.0, 1.0, 1.0, 0.0);
	   light2 = grf4DefLight (0x808080, 0, -1.0,-1.0,-1.0, 0.0);

	   grf4SetLight(0,light1);	
	   grf4SetLight(1,light2);
	 }

	/* define a default material, if this has not yet been done */

	if (!defaultMatDesc)
	 { defaultMat = grf4DefMaterial (0xffffff, 0, 0, 0, 0.0, 0.0);
	 }
	else
	 { if ((defaultMat = grf4DefMaterialD (defaultMatDesc)) == 0)
	    { fprintf (stderr, "Error setting default material\n");
	      exit (-1);
	    }
	 }

	/* Go through and init all defined materials ... */
	for (md=materials; md; md=md->next)
	 { if ((md->ptr = grf4DefMaterialD(&md->def)) == 0)
	    { fprintf(stderr, "Error setting material from definition\n");
	      exit (-1);
	    }
	 }
	/* and then bind them to their associated objects */
	for (e=drawList.head; e; e=e->next)
	 { dd = (DrawDesc*)e->ptr;
	   if (dd->code == DRAW_OBJECT)
	    { setObjectMaterial (dd->u.od, defaultMat);
	    }
	 }
	return (1);
}

setObjectMaterial (obj, mat)
ObjectDesc *obj;
void *mat;
{
	ObjectDesc *son;
	void *newmat;

	if (obj->matdef)
	 { newmat = obj->matdef->ptr;
	 }
	else
	 { newmat = mat;
	 }
	if (obj->sons)
	 { for (son=obj->sons; son; son=son->sibs)
	    { setObjectMaterial (son, newmat);
	    }
	 }
	else
	 { grf4SetObjMaterial (obj->go, newmat);
	 }
}

computeAllManipStates()
{
	RobotDesc *rd;
	for (rd=robots; rd; rd=rd->next)
	 { manipStateCompute(rd);
	 }
}

fxnListInit()
{
	extern SimFxnDesc baseFxnTable[];
	SimFxnDesc *fxd;

	for (fxd = &baseFxnTable[0]; fxd->name; fxd++)
	 { listLinkBefore (&fxnList, 0, (void*)fxd);
	 }
}

TRSF *genLocFxn(T, com, arg, ptr)
TRSF *T;
int com;
int arg;
void *ptr;
{
	*T = *(TRSF*)ptr;
	return (T);
}

TRSF *robotHandLocFxn(T, com, arg, ptr)
TRSF *T;
int com;
int arg;
void *ptr;
{
	double val;

	identTrsf (T);
	val = *((int*)ptr) * arg;
	switch (com)
	 { case 1:
	    { T->p.x = val;
	      break;
	    }
	   case 2:
	    { T->p.y = val;
	      break;
	    }
	   case 3:
	    { T->p.z = val;
	      break;
	    }
	   case 4:
	    { rpyToTrsf (T, 0.0, 0.0, val);
	      break;
	    }
	   case 5:
	    { rpyToTrsf (T, 0.0, val, 0.0);
	      break;
	    }
	   case 6:
	    { rpyToTrsf (T, val, 0.0, 0.0);
	      break;
	    }
	 }
	return (T);
}

drawRobot (rd, base)
RobotDesc *rd;
TRSF *base;
{
	TRSF newbase;
	LocFxnDesc *fxd;

	if (fxd = rd->locfxn)
	 { (*fxd->fxn)(&newbase, fxd->com, fxd->arg, fxd->ptr);
	 }
	else
	 { newbase = *base;
	 }
	if (rd->loc)
	 { multTrsf (&newbase, &newbase, rd->loc);
	 }
	grf4SetTrans (&newbase);
	rbtDrawRobot (rd->rgp, rd->var, rd->L, rd->hasEndEffector, 
					       rd->rs.handPos);
}

drawStaticRobot (sd, base)
StaticRobotDesc *sd;
TRSF *base;
{
	TRSF newbase;
	LocFxnDesc *fxd;

	if (fxd = sd->locfxn)
	 { (*fxd->fxn)(&newbase, fxd->com, fxd->arg, fxd->ptr);
	 }
	else
	 { newbase = *base;
	 }
	if (sd->loc)
	 { multTrsf (&newbase, &newbase, sd->loc);
	 }
	grf4SetTrans (&newbase);
	rbtDrawRobot (sd->rgp, sd->var, sd->L, sd->hasEndEffector, 1);
}

drawObject(obj, base)
ObjectDesc *obj;
TRSF *base;
{
	TRSF newbase;
	LocFxnDesc *fxd;
	ObjectDesc *son;

	if (fxd = obj->locfxn)
	 { (*fxd->fxn)(&newbase, fxd->com, fxd->arg, fxd->ptr);
	 }
	else
	 { newbase = *base;
	 }
	if (obj->loc)
	 { multTrsf (&newbase, &newbase, obj->loc);
	 }
	if (obj->sons)
	 { for (son=obj->sons; son; son=son->sibs)
	    { drawSubObject (son, &newbase);
	    }
	 }
	else
	 { grf4SetTrans (&newbase);
	   grf4DrawObject (obj->go);
	 }
}

drawSubObject(obj, base)
ObjectDesc *obj;
TRSF *base;
{
	TRSF frame;
	TRSF newbase;
	LocFxnDesc *fxd;
	ObjectDesc *son;

	newbase = *base;
	if (fxd = obj->locfxn)
	 { (*fxd->fxn)(&frame, fxd->com, fxd->arg, fxd->ptr);
	   multTrsf (&newbase, &newbase, &frame);
	 }
	if (obj->loc)
	 { multTrsf (&newbase, &newbase, obj->loc);
	 }
	if (obj->sons)
	 { for (son=obj->sons; son; son=son->sibs)
	    { drawSubObject (son, &newbase);
	    }
	 }
	else
	 { grf4SetTrans (&newbase);
	   grf4DrawObject (obj->go);
	 }
}

addNewDrawable (code, ptr)
int code;
void *ptr;
{
	DrawDesc *dd;

	dd = ALLOC(DrawDesc);
	dd->code = code;
	switch (code)
	 { case DRAW_ROBOT:
	    { dd->u.rd = (RobotDesc*)ptr;
	      break;
	    }
	   case DRAW_STATIC_ROBOT:
	    { dd->u.sd = (StaticRobotDesc*)ptr;
	      break;
	    }
	   case DRAW_OBJECT:
	    { dd->u.od = (ObjectDesc*)ptr;
	      break;
	    }
	 }
	listLinkBefore (&drawList, 0, (void*)dd);
}

static char *getErrMsg(code)
int code;
{
	switch (code)
	 { case MPR_BADRATE_E:
	    { return ("illegal rate request");
	      break;
	    }
	   case MPR_TIMEOUT_E:
	    { return ("timeout waiting for host");
	      break;
	    }
	   case MPR_MEMINIT_E:
	    { return ("error intializing memory");
	      break;
	    }
	   case MPR_ESTOP_E:
	    { return ("emergency stop set");
	      break;
	    }
	   case HST_TIMEOUT_E:
	    { return ("timeout talking to host");
	      break;
	    }
	   case HST_HEADER_E:
	    { return ("bad header from host");
	      break;
	    }
	   case HST_CHKSUM_E:
	    { return ("checksum error on host data");
	      break;
	    }
	   case HST_COUNT_E:
	    { return ("bad host packet count");
	      break;
	    }
	   case HST_SYNC_E:
	    { return ("lost sync with host");
	      break;
	    }
	   case HST_BADCMD_E:
	    { return ("bad command from host");
	      break;
	    }
	   case SRV_TIMEOUT_E:
	    { return ("joint servo timeout");
	      break;
	    }
	  case SLE_LIMIT_E:
	    { return ("hard joint limit");
	      break;
	    }
	  case SLE_POWERAMP_E:
	    { return ("power amp failure");
	      break;
	    }
	  case SLE_ENCODERS_E:
	    { return ("encoder failure");
	      break;
	    }
	  case SLE_BRAKES_E:
	    { return ("brakes release is enabled");
	      break;
	    }
	  default:
	    { if (code > 100)
	       { return ("error diagnostic");
	       }
	      else
	       { return ("???");
	       }
	    }
	 }
}
