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

/*---------------------------------------------------------------------------

	Interactive program to test the RCI robot features. More
	specificly, it tests the functioning of the HOW structure
	and the various robot SET primitives. It starts up and tries
	to connect to the system default robot. After that, the user
	can interactively examine the HOW structure and set commands.

	Note two things about this program: First, when you get the
	prompt, the program has not yet started communicating
	with the robot. To get that going, you have to enter the
	'start' command. Second, the default rate for communication
	is quite slow (about 1 second). This is to allow lots of time
	on the robot controller side for things like debugging messages.

 --------------------------------------------------------------------------*/

#include <stdio.h>
#include <signal.h>

#ifdef VXWORKS_CFG
#define main(ac,av)	rbttest_(ac,av)
#endif

#include <setjmp.h>
#include <fcntl.h>
#include <errno.h>
#include <strings.h>

#include "../h/rciTask.h"
#include "../h/rciMacros.h"
#include "../h/robotControl.h"
#include "../h/robotMath.h"
#include "../h/IOdevice.h"

#include "rbttest.h"

#define	COMMAND(s)	(strcmp (key_buf, s) == 0)
#define	COMMANDS(s)	(strncmp (key_buf, s, strlen(s)) == 0)
char key_buf [256];

extern ctrlStartup();
extern ctrlFxn();
extern ctrlRelease();

extern int auxDebug;

#define PUMAJ	6

int displayCommands;
jmp_buf env;

abortHandler(code, msg, td)
int code;
char *msg;
RCI_DESC *td;
{
	fprintf (stderr, "RCI abort, code %d, task %s, cycle %d\n", code, 
		 rciTaskName(td), td->tcb->cycleCount);
	fprintf (stderr, "%s\n", msg);
	longjmp (env, 1);
}

main(argc, argv)
int argc;
char **argv;
{
	char *robotName = NULL;
	int simflag = 0;
	char *simname = NULL;

	RCI_DESC *td;
	RCI_RBT *rbt;
	HOW *how;
	CHG *chg;
	RCB *rcb;

	int robot;

	char *keytree, *tree_match_parse(), *tree_match_err_at();
	char strbuf[256];
	int uval, ival, param, mask, chan, j;
	float fval;
	int key_len;
	int interval, offset;
	int blkTime, runReqs;
	int cpuNo = UNDEF;
	int rate = UNDEF;
	int abortCode;

	displayCommands = 0;

	argc--; argv++;
	while (argc)
	 { if (ARGUMENT ("-cpu"))
	    { argc--; argv++;
	      if (argc == 0)
	       { argc = -1;
		 break;
	       }
	      if (!(scanInteger (&cpuNo, *argv)))
	       { fatalMsg (-1, "integer expected after -cpu option\n");
	       }
	    }
	   else if (ARGUMENT ("-rate"))
	    { argc--; argv++;
	      if (argc < 1)
	       { argc = -1;
		 break;
	       }
	      if (!scanInteger (&rate, *argv))
	       { fatalMsg (-1, "integer expected after '-rate' argument\n");
	       }
	      if (rate < 1)
	       { fatalMsg (-1, "rate must be greater than 0\n");
	       }
	    }
	   else if (ARGUMENTS ("-sim="))
	    { char *sname;
	      if (strlen(sname=index(*argv,'=')+1) == 0)
	       { argc = -1;
		 break;
	       }
	      simname = sname;
	      simflag = YES;
	    }
	   else if (ARGUMENT ("-sim"))
	    { simflag = 1;
	    }
	   else if (robotName == NULL)
	    { robotName = *argv;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { usageMsg (-1, 
"rbttest [<robotName>] [-cpu <cpuNumber>] [-sim] [-rate <rate>]\n");
	 }

	if (simflag)
	 { if (cpuNo != UNDEF)
	    { printf ("warning: '-sim' option sets cpu to 0 (arbiter)\n");
	    }
	   cpuNo = 0;
	 }
	else
	 { if (cpuNo == UNDEF)
	    { cpuNo = 0;
	    }
	 }
	if (rate == UNDEF)
	 { rate = 100;
	 }
	if (robotName == NULL)
	 { if ((robotName = (char*)getDefaultRobot()) == NULL)
	    { printErrors();
	      exit (-1);
	    }
	 }

	keytree = tree_match_parse("					\
(	quit								\
	start								\
	abort								\
	 (%d", &abortCode, ",,,		\"abort code\")			\
	cycle								\
	 (%d", &ival, ",0,,		\"cycle count\")		\
	delay								\
	 (%d", &ival, ",,,		\"abort delay count\")		\
	release								\
	rcb								\
	rate								\
	 (%d", &interval,",,,		\"interval\"			\
	   (%d", &offset, ",,,		\"offset\"))			\
	show 								\
	 (rcb								\
	  auxDebug							\
	  how (rbt io)							\
	  numFds)							\
	display (commands (on off))					\
	motor (level (control))						\
	joint (level (control))						\
	set 								\
	 ( step-size							\
	     (%d", &uval,",0,,		\"unsigned integer value\")	\
	   timeout							\
	    (%d", &blkTime, ",,,	\"block timeout time\"		\
	      (%d", &runReqs, ",,,	\"max number of run reqs\"))	\
	   [encoder]							\
	   (%d", &uval, ",0,,		\"unsigned integer value\"	\
	     ([all 1 2 3 4 5 6])					\
	    value							\
	     (%d", &uval, ",0,,		\"unsigned integer value\"	\
	       ([all 1 2 3 4 5 6])))					\
	  [angle]							\
	   (%f", &fval,",,,		\"floating value\"		\
	     ([all 1 2 3 4 5 6]))					\
	  [jtorque]							\
	   (%f", &fval,",,,		\"floating value\"		\
	     ([all 1 2 3 4 5 6]))					\
	  mtorque							\
	   (%f", &fval,",,,		\"floating value\"		\
	     ([all 1 2 3 4 5 6]))					\
	  stop								\
	   ([all 1 2 3 4 5 6])						\
	  stop_all							\
	  [handpos handforce]						\
	   (%d", &ival,",,,		\"integer value\")		\
	  [integration calibmode checking]				\
	   (%x", &mask,",,,		\"bit mask value (hex)\")	\
	  calibflag							\
	   (%d", &ival,")						\
	  power								\
	   ([on off])							\
	  [pot torque pendant jstick force]				\
	   (read							\
	     (%d", &ival,"))						\
	  [adc slu]							\
	   (chan 							\
	     (%x", &mask,",,,		\"channel mask (hex)\")))	\
	post								\
	 (ctrlparam							\
	   (%d", &param,",,,		\"parameter number\"		\
	     (%d", &ival, ",						\
	       (%x", &mask, ",,,	\"joint mask\"))))		\
	load								\
	 (ctrlparams)							\
	put								\
	 (dac								\
	   (%d", &chan, ",,,		\"channel number\"		\
	     (%d", &ival, "))						\
	  pio								\
	   (%d", &chan, ",,,		\"channel number\"		\
	     (%x", &ival, "))						\
	 slu								\
	   (%d", &chan, ",,,		\"channel number\"		\
	     (%s", strbuf, "))						\
	 pendant							\
	   (%x", &uval, ",,,		\"pendant word (hex)\"))	\
	 block								\
)",0);
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	tree_match_catch_sig (0, sigmask(SIGALRM));

	/* set up the rci task */

	if ((td = rciCreate ("rbttest", (1<<cpuNo))) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	if ((rcb = (RCB*)rciSharedMemory ("RCB", sizeof(RCB), td)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	if (rciControlFxns (td, ctrlStartup, ctrlFxn, ctrlRelease, 0) < 0)
	 { printErrors();
	   exit (-1);
	 }

	if (!simflag)
	 { if ((robot = rciAttachRobot (td, robotName, NULL)) < 0)
	    { printErrors();
	      exit (-1);
	    }
	   if (rciScheduling (td, ON_CLOCK, 0) < 0)
	    { printErrors ();
	      exit (-1);
	    }
	 }
	else
	 { if ((robot = simAttachRobot (td, robotName, simname)) < 0)
	    { printErrors();
	      exit (-1);
	    }
	   if (rciScheduling (td, ON_SIGNAL, SIGALRM) < 0)
	    { printErrors ();
	      exit (-1);
	    }
	 }

	rciRate (td, rate, 0);
	rbt = TASKROBOT(td, robot);
	how = rbt->how;
	chg = rbt->chg;
/* MS */
	SET_CHECKING(rbt,0);

	rciAbortHandler (abortHandler);
	setjmp(env);
	rcb->abortRequest = 0;
	rcb->cycleRequest = 0;
	rcb->cycleCount = 0;
	rcb->cycleOff = 1;
	
	do
 	 { tree_match (keytree, "RBTTEST> ", key_buf);
	   key_len = strlen (key_buf);

	   if (COMMAND("start"))
	    { if (rciStart(td->taskCode) < 0)
	       { printErrors();
		 exit (-1);
	       }
	    }
	   else if (COMMAND("release"))
	    { if (rciRelease(td->taskCode) < 0)
	       { printErrors();
		 exit (-1);
	       }
	    }
	   else if (COMMAND("abort"))
	    { rcb->abortRequest = abortCode;
	    }
	   else if (COMMAND("delay"))
	    { rcb->abortDelay = ival;
	    }
	   else if (COMMAND("cycle"))
	    { if (!simflag)
	       { printf ("demand cycling only supported in simulator mode\n");
	       }
	      if (ival > 0)
	       { rcb->cycleRequest += ival;
		 if (rcb->cycleOff)
		  { rcb->cycleCount = 1;
		    rcb->cycleOff = 0;
		    rcb->cycleRequest = ival;
		    simSetTimer (td->tcb->interval, (struct timeval*)0);
		  }
	       }
	    }
	   else if (COMMAND("rate"))
	    { if (rciRate (td, interval, offset) < 0)
	       { printErrors();
		 clearErrors();
	       }
	    }
	   else if (COMMAND("display commands on"))
	    { displayCommands = 1;
	    }
	   else if (COMMAND("display commands off"))
	    { displayCommands = 0;
	    }
	   else if (COMMAND("show auxDebug"))
	    { printf ("%d\n", auxDebug);
	    }
 	   else if (COMMAND ("show rcb"))
 	    { printf ("cycleCount=%d\n", rcb->cycleCount);
 	    }
	   else if (COMMAND("show how rbt"))
	    { printHowRbt (how, PUMAJ);
	    }
	   else if (COMMAND("show how io"))
	    { printHowIo (how);
	    }
 	   else if (COMMAND("show numFds"))
 	    { 
#ifdef VXWORKS_CFG
	      printf ("don't know how to do this on vxWorks yet\n");
#else
	      int i, total;
 	      total = 0;
 	      for (i=getdtablesize()-1; i>=0; i--)
 	       { if (fcntl(i,F_GETFL,0) >= 0)
 		  { printf ("%d open\n", i);
 		    total++;
 		  }
 	       }
 	      printf ("%d files open\n", total);
#endif
	    }
	   else if (COMMAND("motor level control"))
	    { MOTOR_LEVEL_CONTROL (rbt);
	    }
	   else if (COMMAND("joint level control"))
	    { JOINT_LEVEL_CONTROL (rbt);
	    }
	   else if (COMMAND("set timeout"))
	    { if (rciTimeout (td, blkTime, runReqs, DEFAULTVALUE) < 0)
	       { printErrors();
		 clearErrors();
	       }
	    }
	   else if (COMMAND("set encoder value all"))
	    { for (j=0; j<PUMAJ; j++)
	       { SET_ENCODER (rbt, uval, j);
	       }
	    }
	   else if (COMMANDS("set encoder value"))
	    { SET_ENCODER (rbt, uval, key_buf[key_len-1]-'1');
	    }
	   else if (COMMAND("set encoder all"))
	    { for (j=0; j<PUMAJ; j++)
	       { MOVE_ENCODER (rbt, uval, j);
	       }
	    }
	   else if (COMMANDS("set encoder"))
	    { int j = key_buf[key_len-1]-'1';
	      MOVE_ENCODER (rbt, uval, j);
	    }
	   else if (COMMAND("set angle all"))
	    { for (j=0; j<PUMAJ; j++)
	       { MOVE_ANGLE (rbt, fval*DEGTORAD, j);
	       }
	    }
	   else if (COMMANDS("set angle"))
	    { MOVE_ANGLE (rbt, fval*DEGTORAD, key_buf[key_len-1]-'1');
	    }
	   else if (COMMAND("set mtorque all"))
	    { for (j=0; j<PUMAJ; j++)
	       { SET_MTORQUE (rbt, fval, j);
	       }
	    }
	   else if (COMMANDS("set mtorque"))
	    { j = key_buf[key_len-1]-'1';
	      SET_MTORQUE (rbt, fval, j);
	    }
	   else if (COMMAND("set jtorque all"))
	    { for (j=0; j<PUMAJ; j++)
	       { SET_JTORQUE (rbt, fval, j);
	       }
	    }
	   else if (COMMAND("set torque read"))
	    { SET_TORQUE_READ (rbt, ival);
	    }
	   else if (COMMANDS("set jtorque"))
	    { SET_JTORQUE (rbt, fval, key_buf[key_len-1]-'1');
	    }
	   else if (COMMAND("set stop all"))
	    { for (j=0; j<PUMAJ; j++)
	       { SET_STOP (rbt, j);
	       }
	    }
	   else if (COMMAND("set stop_all"))
	    { SET_STOP_ALL (rbt);
	    }
	   else if (COMMANDS("set stop"))
	    { SET_STOP (rbt, key_buf[key_len-1]-'1');
	    }
	   else if (COMMAND("set handpos"))
	    { SET_HANDPOS (rbt, ival);
	    }
	   else if (COMMAND("set handforce"))
	    { SET_HANDFORCE (rbt, ival);
	    }
	   else if (COMMAND("post ctrlparam"))
	    { POST_CTRLPARAM (rbt, param, ival, mask);
	    }
	   else if (COMMAND("load ctrlparams"))
	    { LOAD_CTRLPARAMS (rbt);
	    }
	   else if (COMMAND("set integration"))
	    { SET_INTEGRATION (rbt, mask);
	    }
	   else if (COMMAND("set calibmode"))
	    { SET_CALIBMODE (rbt, mask);
	    }
	   else if (COMMAND("set calibflag"))
	    { SET_CALIBFLAG (rbt, ival);
	    }
	   else if (COMMAND("set power on"))
	    { SET_POWER_ON (rbt);
	    }
	   else if (COMMAND("set power off"))
	    { SET_POWER_OFF (rbt);
	    }
	   else if (COMMAND("set checking"))
	    { SET_CHECKING (rbt, mask);
	    }
	   else if (COMMAND("set torque read"))
	    { SET_TORQUE_READ (rbt, ival);
	    }
	   else if (COMMAND("set pot read"))
	    { SET_POT_READ (rbt, ival);
	    }
	   else if (COMMAND("set pendant read"))
	    { SET_PENDANT_READ (rbt, ival);
	    }
	   else if (COMMAND("set force read"))
	    { SET_FORCE_READ (rbt, ival);
	    }
	   else if (COMMAND("set jstick read"))
	    { SET_JSTICK_READ (rbt, ival);
	    }
	   else if (COMMAND("set adc chan"))
	    { SET_ADC_CHAN (rbt, mask);
	    }
	   else if (COMMAND("set slu chan"))
	    { SET_SLU_CHAN (rbt, mask);
	    }
	   else if (COMMAND("put pio"))
	    { PUT_PIO (rbt, chan, ival);
	    }
	   else if (COMMAND("put dac"))
	    { PUT_DAC (rbt, chan, ival);
	    }
	   else if (COMMAND("put slu"))
	    { PUT_SLU (rbt, chan, strbuf, strlen(strbuf));
	    }
	   else if (COMMAND("put pendant"))
	    { unsigned short word;
	      word = uval;
	      PUT_PENDANT_MSG (rbt, (char*)&word, 2);
	    }
	   else if (COMMAND("block"))
	    { rciBlock (td);
	    }
	 }
	while (!(COMMAND("quit")));

	rciRelease (td->taskCode);	
}

printHowRbt (how, numj)
HOW *how;
int numj;
{
	int j;

	putchar ('\n');

/* -------- Basic format for the angle information is given here ---------

("JNT    enc      ang      vel   mtorque   jtorque   pots
   X  XXXXXX -XXX.XXX -XXX.XXX -XX.XXXXX -XXXX.XXX  XXXXX   

   ------------------------------------------------------------------------ */
	printf(
"JNT    enc      ang      vel   mtorque   jtorque   pots\n");
	for (j=0; j<numj; j++)
	 { printf ("%2d  %6d %8.3f %8.3f %9.5f %9.3f  %5d\n",
		   j+1, how->enc[j], 
		   how->ang[j] * RADTODEG,
		   how->angVel[j] * RADTODEG,
		   how->mtorque[j], how->jtorque[j],
		   how->potInput[j]);
	 }

	putchar('\n');

/* -------- Basic format for set angle information is given here ---------

("JNT mode setEnc   setAng   setMtor   setJtor
   X       XXXXXX -XXX.XXX -XX.XXXXX -XXXX.XXX

   ------------------------------------------------------------------------ */
	printf(
"JNT mode setEnc   setAng   setMtor   setJtor\n");
	for (j=0; j<numj; j++)
	 { printf ("%2d    %c  %6d %8.3f %9.5f %9.3f\n",
		   j+1,
		   how->torqueMode & (1<<j) ? 'T' : ' ',
		   how->encSet[j],
		   how->angSet[j] * RADTODEG,
		   how->mtorqueSet[j],
		   how->jtorqueSet[j]);
	 }

	putchar ('\n');

/* -------- Basic format for other robot information given here ---------
 
012345678901234567890123456789012345678901234567890123456789012345678901234567
	t	t	t	t	t	t	t	t	t
    robotID = XXXX        power = X     integration = 0xXXXX time = XXXXXXXXXX
controllerID = XXXX         hand = -XXXX   calibMode = 0xXXXX
    options = 0xXXXX  handForce = -XXXX   markFound = 0xXXXX
   checking = 0xXXXX calibrated = XXXX        state = 0xXXXX

 "controlLevel: <string> reading: <strings>
   ------------------------------------------------------------------------ */

	printf (
"    robotID = %-4d\t  power = %1d\tintegration = 0x%04x time = %-10u\n",
		how->robotID, how->power, how->integration, how->time);
	printf (
"controllerID = %-4d\t   hand = %-5d   calibMode = 0x%04x\n",
		how->controllerID, how->hand, how->calibMode);
	printf (
"    options = 0x%04x  handForce = %-5d   markFound = 0x%04x\n",
		how->options, how->handForce, how->markFound);
	printf (
"   checking = 0x%04x calibrated = %-4d        state = 0x%04x\n",
		how->checking, how->calibrated, how->state);
	putchar ('\n');
	printf ("controlLevel: %s reading: %s%s\n",
		(how->controlLevel == JOINT_LEVEL) ? "JOINT" :
		(how->controlLevel == MOTOR_LEVEL) ? "MOTOR" : "UNDEF",
		(how->torqueOK ? "TORQUE " : ""),
		(how->potOK ? "POTS " : ""));
}

printHowIo (how)
HOW *how;
{
	int i, k, rows, adcRows;

	putchar ('\n');
	printf ("numAdc = %d\t\tnumPioOut = %d\n",
		 how->numAdc, how->numPioOut);
	printf ("numDac = %d\t\tnumSlu = %d\n",
		 how->numDac, how->numSlu);
	printf ("numPioIn = %d\n",
		 how->numPioIn);

	putchar ('\n');
	printf ("adc = 0x%x\t\tslu = 0x%x\n", how->adc, how->slu);
	printf ("reading: %s%s%s\n",
		(how->pendantOK ? "PENDANT " : ""),
		(how->forceOK ? "FORCE " : ""),
		(how->jstickOK ? "JSTICK " : ""));

	putchar ('\n');
/* --------- Basic format for the sensor and IO information ------------- 

(" N        force   jstick adcInput adcInput pioInput pioOutput dacOutput
   X -XXXXXX.XXXX  -XXXXXX  -XXXXXX  -XXXXXX   0xXXXX    0xXXXX   -XXXXXX

   ---------------------------------------------------------------------- */

	adcRows = (HOW_MAXADC+1)/2;
	rows = MAX (MAX(HOW_MAXDAC,adcRows), MAX(HOW_MAXPIO,6) );
	
	printf(
" N        force   jstick  adcInput adcInput pioInput pioOutput dacOutput\n");
	for (i=0; i<rows; i++)
	 { printf ("%2d ", i);
	   if (i<6)
	    { printf ("%12.4f  %7d ", how->forceInput[i], 
				        how->jstickInput[i]);
	    }
	   else
	    { spaces (22);
	    }
	   if (i < adcRows)
	    { if (i+adcRows < HOW_MAXADC)
	       { printf ("  %7d  %7d ", 
			 how->adcInput[i], 
		 	 how->adcInput[i+(HOW_MAXADC+1)/2]);
	       }
	      else
	       { printf ("  %7d", how->adcInput[i]);
		 spaces (10);
	       }
	    }
	   else
	    { spaces (19);
	    }
	   if (i < HOW_MAXPIO)
	    { printf ("  0x%04hx    0x%04hx ", 
		       how->pioInput[i],
		       how->pioOutput[i]);
	    }
	   else
	    { spaces (19);
	    }
	   if (i < HOW_MAXDAC)
	    { printf ("  %7d\n", how->dacOutput[i]);
	    }
	   else
	    { putchar ('\n');
	    }
	 }
	putchar ('\n');
	printf ("pendantInput = ");
 	for (i = 0; i < HOW_PENDANT; i++)
	 { printf ("0x%08x ", how->pendantInput[i]);
	 }
 	putchar ('\n');
	for (i = 0; i < HOW_MAXSLU; i++)
	 { printf ("slu%d = (%d) ", i, how->sluInput[i].num);
	   for (k=0; k<how->sluInput[i].num; k++)
	    { printf ("%02x ", how->sluInput[i].buf[k]);
	    }
	   putchar ('\n');
	 }
	printf ("pendantMsg = (%d) ", how->pendantMsg.num);
	for (k=0; k<how->pendantMsg.num; k++)
	 { printf ("%02x ", how->pendantMsg.buf[k]);
	 }
	putchar ('\n');
	putchar ('\n');
}

spaces (x)
{
	int i;
 	for (i=0; i<x; i++)
  	 { putchar(' ');
	 }
}
