/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#include <signal.h>
#include <strings.h>

#include "../h/robotMath.h"
#include "../h/robotControl.h"
#include "../h/rciMacros.h"
#include "../h/chase.h"
#include "../h/rciTask.h"
#include "../h/cdefs.h"

#define NUMJ	6

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

RCI_RBT *rbt;
HOW *how;
JLS *jls;
RCI_DESC *td;
CHASE_CTRL *cb;

int controlOn = NO;

main(argc, argv)
int argc;
char **argv;
{
	char **argvec = argv;
	char *robotName = NULL;

	char *keytree, *tree_match_parse(), *tree_match_err_at();
	float ang, fval;
	int enc, ival, j;
	int jntMask;
	int rbtnum;

	int timeout = UNDEF;
	int rate = UNDEF;
	int cpuNum = UNDEF;
	int scheduling = UNDEF;
	int simflag = 0;
	char *simAddr = NULL;

	argc--; argv++;
	while (argc)
	 { if (ARGUMENTS ("-sim="))
	    { char *sname;
	      if (strlen(sname=index(*argv,'=')+1) == 0)
	       { argc = -1;
		 break;
	       }
	      simAddr = sname;
	      simflag = 1;
	    }
	   else if (ARGUMENT ("-sim"))
	    { simflag = 1;
	    }
	   else if (ARGUMENT ("-c"))
	    { argc--; argv++;
	      if (argc < 1)
	       { argc = -1;
		 break;
	       }
	      if (!scanInteger (&cpuNum, *argv))
	       { fatalMsg (-1, "integer expected after '-c' option\n");
	       }
	      if (cpuNum < 0)
	       { fatalMsg (-1, "cpu number must be non-negative\n");
	       }
	    }
	   else if (ARGUMENT ("-r"))
	    { argc--; argv++;
	      if (argc < 1)
	       { argc = -1;
		 break;
	       }
	      if (!scanInteger (&rate, *argv))
	       { fatalMsg (-1, "integer expected after '-r' option\n");
	       }
	    }
	   else if (ARGUMENT ("-t"))
	    { argc--; argv++;
	      if (argc < 1)
	       { argc = -1;
		 break;
	       }
	      if (!scanInteger (&timeout, *argv))
	       { fatalMsg (-1, "integer expected after '-t' option\n");
	       }
	    }
	   else if (ARGUMENT ("-s"))
	    { argc--; argv++;
	      if (argc < 1)
	       { argc = -1;
		 break;
	       }
	      if (strcmp (*argv, "CLOCK") == 0)
	       { scheduling = ON_CLOCK;
	       }
	      else if (strcmp (*argv, "FUNCTION") == 0)
	       { scheduling = ON_FUNCTION;
	       }
	      else if (strcmp (*argv, "TRIGGER") == 0)
	       { scheduling = ON_TRIGGER;
	       }
	      else
	       { fatalMsg (-1, "illegal scheduling discipline %s\n", *argv);
	       }
	    }
	   else if (robotName == NULL)
	    { robotName = *argv;
	    }
	   else	      
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc < 0)
	 { usageMsg (-1, "\
% [<robotName>] [-c <cpu>] [-sim[=<simulatorAddr>]]\n\
[-s (CLOCK|TRIGGER|FUNCTION)] [-r <rate>] [-t <timeout>]\n", argvec[0]);
	 }
	if (cpuNum == UNDEF)
	 { cpuNum = 0;
	 }
	if (rate == UNDEF)
	 { rate = simflag ? 1000 : 50;
	 }
	if (scheduling == UNDEF)
	 { scheduling = ON_CLOCK;
	 }
	if (simflag && cpuNum != 0)
	 { printf ("WARNING: '-sim' forces cpu to be arbiter\n");
	   cpuNum = 0;
	 }
	if (robotName == NULL)
	 { robotName = (char*)getDefaultRobot();
	 }

	keytree = tree_match_parse("					\
(	quit								\
	show 								\
	stop								\
	halt								\
	cycle								\
	 (%x", &jntMask, ",,,	\"mask describing joints to cycle (hex)\"\
	   (by								\
	     (%f", &ang, ",,,	\"sweep angle (degrees)\")))		\
	move								\
	 ([1 2 3 4 5 6]							\
	   ([to by]							\
	     (encoder							\
	       (%d", &enc, ",,,			\"encoder count\")	\
	      %f", &ang, ",,,			\"angle (degrees)\"))	\
	  [jnts]							\
	   (%x", &jntMask, ",,,	\"mask describing joints to move (hex)\"))\
	[encoder angle bogus]						\
	 (mode)								\
	free								\
	 ([1 2 3 4 5 6 all])						\
	lock								\
	 ([1 2 3 4 5 6 all])						\
	set								\
	 (angle								\
	   (speed (%f", &fval, ",,,	\"speed (deg./sec)\")		\
	    accel (%f", &fval, ",,,	\"acceleration (deg./sec^2)\")	\
	    target							\
	     ([1 2 3 4 5 6]						\
	       (%f", &fval, ",,,	\"angle (deg.)\")		\
	      all))							\
	  encoder 							\
	   (speed (%d", &ival, ",,,	\"speed (ticks/sec)\")		\
	    accel (%d", &ival, ",,,	\"acceleration (ticks/sec^2)\")	\
	    target							\
	     ([1 2 3 4 5 6]						\
	       (%d", &ival, ",,,	\"encoder count\")		\
	      all))							\
	  synchro (on off)						\
	  power (on off)						\
	 )								\
	clear (compcode)						\
	start								\
	release								\
)",0);

/*
	if (simflag != 1)
	 { fatalMsg (-1, "must use simflag for now\n");
	 }
*/

	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }
	tree_match_catch_sig (0, sigmask(SIGALRM));
	if ((td = rciCreate ("chase", (1<<cpuNum))) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	if (simflag)
	 { if ((rbtnum = simAttachRobot (td, robotName, simAddr)) < 0)
	    { printErrors();
	      exit (-1);
	    }
	 }
	else
	 { if ((rbtnum = rciAttachRobot (td, robotName, NULL)) < 0)
	    { printErrors();
	      exit (-1);
	    }
	 }
	if ((cb = (CHASE_CTRL*)rciSharedMemory(
		  "CHASE_CTRL", sizeof(CHASE_CTRL), td)) == NULL)
	 { printErrors();
	   exit (-1);
	 }

	if (rciControlFxns (td, chaseStart, chaseDrive, chaseRelease, 0)<0)
	 { printErrors();
	   exit (-1);
	 }
	if (simflag)
	 { simScheduling (td);
	   rciRate (td, rate, 0);
	 }
	else
	 { rciScheduling (td, scheduling, TASKROBOT(td, 0)->idev);
	   rciRate (td, rate, 0);
	   if (timeout != UNDEF)
	    { rciTimeout (td, timeout, UNDEF, 0);
	    }
	 }

	rbt = TASKROBOT(td, rbtnum);
	how = rbt->how;
	jls = rbt->jls;

	chaseCtrlInit(cb);

	copyVf  (cb->angSpeed, jls->nominalVel, jls->numJ);
	scaleVf (cb->encSpeed, jls->averageAngtoenc, cb->angSpeed, jls->numJ);
	copyVf  (cb->angAccel, jls->nominalAcc, jls->numJ);
	scaleVf (cb->encAccel, jls->averageAngtoenc, cb->angAccel, jls->numJ);

	rciPrintToTerminal(YES);

	do
 	 { tree_match (keytree, "CHASETEST> ", key_buf);

	   if (COMMAND("start"))
	    { if (rciStart(td->taskCode) < 0)
	       { printErrors();
		 clearErrors();
	       }
	      controlOn = YES;
	    }
	   else if (COMMAND("release"))
	    { if (rciRelease(td->taskCode) < 0)
	       { printErrors();
		 clearErrors();
	       }
	      controlOn = NO;
	    }
	   else if (COMMAND("show"))
	    { display();
	    }
	   else if (COMMAND ("stop"))
	    { cb->stop = 1;
	    }
	   else if (COMMAND ("halt"))
	    { cb->halt = 1;
	    }
	   else if (COMMAND ("encoder mode"))
	    { if (controlOn)
	       { cb->stop = 1;
		 while (cb->stop || cb->compcode != 0x3f)
		  { nap(0.2);
		  }
	       }
	      cb->controlLevelChange = MOTOR_LEVEL;
	    }
	   else if (COMMAND ("angle mode"))
	    { if (controlOn)
	       { cb->stop = 1;
		 while (cb->stop || cb->compcode != 0x3f)
		  { nap(0.2);
		  }
	       }
	      cb->controlLevelChange = JOINT_LEVEL;
	    }
	   else if (COMMAND ("bogus mode"))
	    { if (controlOn)
	       { cb->stop = 1;
		 while (cb->stop || cb->compcode != 0x3f)
		  { nap(0.2);
		  }
	       }
	      cb->controlLevelChange = 0xf342e78; /* ... something inane! */
	    }
	   else if (COMMAND ("free all"))
	    { for (j=0; j<NUMJ; j++)
	       { cb->torqueControl[j] = TORQUE_ZERO;
	       }
	    }
	   else if (COMMANDS ("free"))
	    { j = key_buf[5] - '1';
	      cb->torqueControl[j] = TORQUE_ZERO;
	    }
	   else if (COMMAND ("lock all"))
	    { for (j=0; j<NUMJ; j++)
	       { cb->torqueControl[j] = 0;
	       }
	    }
	   else if (COMMANDS ("lock"))
	    { j = key_buf[5] - '1';
	      cb->torqueControl[j] = 0;
	    }
	   else if (COMMANDS ("move"))
	    { if (COMMAND ("move jnts"))
	       { if (jntMask & ~0x3f)
		  { printf ("mask describes joints that don't exist\n");
		    continue;
		  }
		 else
		  { cb->movecode = jntMask;
		    while (controlOn && cb->movecode)
		     { nap (0.2);
		     }
		  }
	       }
	      else
	       { j = key_buf[5] - '1';
		 key_buf[5] = 'X';
	      
		 if (COMMAND ("move X by encoder"))
		  { if (cb->controlLevel != MOTOR_LEVEL)
		     { printf ("Please enter encoder mode first\n");
		       continue;
		     }
		    cb->encTarget[j] += enc;
		  }
		 else if (COMMAND ("move X to encoder"))
		  { if (cb->controlLevel != MOTOR_LEVEL)
		     { printf ("Please enter encoder mode first\n");
		       continue;
		     }
		    if (enc < 0 || enc > 0x10000)
		     { printf ("encoder value out of range\n");
		       continue;
		     }
		    cb->encTarget[j] = enc;
		  }
		 if (COMMAND ("move X by"))
		  { 
		    if (cb->controlLevel != JOINT_LEVEL)
		     { printf ("Please enter angle mode first\n");
		       continue;
		     }
		    cb->angTarget[j] += ang * DEGTORAD;
		  }
		 else if (COMMAND ("move X to"))
		  { 
		    if (cb->controlLevel != JOINT_LEVEL)
		     { printf ("Please enter angle mode first\n");
		       continue;
		     }
		    if (ang < -1000.0 || ang > 1000.0)
		     { printf ("angle value out of range\n");
		       continue;
		     }
		    cb->angTarget[j] = ang * DEGTORAD;
		  }
		 cb->movecode = (1<<j);
		 while (controlOn && cb->movecode)
		  { nap (0.2);
		  }
	       }
	    }
	   else if (COMMANDS ("set angle target"))
	    { if (COMMAND ("set angle target all"))
	       { float ang;
		 int error = 0;
		 printf ("Enter %d angle values> ", NUMJ);
		 for (j=0; j<NUMJ; j++)
		  { if (scanf ("%f", &ang) != 1)
		     { printf ("error reading values\n");
		       error = 1;
		       break;
		     }
		    else
		     { cb->angTarget[j] = DEGTORAD*ang;
		     }
		  }
		 if (error)
		  { continue;
		  }
	       }
	      else
	       { j = key_buf[strlen("set angle target ")]-'1';
		 cb->angTarget[j] = DEGTORAD*fval;
	       }
	    }
	   else if (COMMANDS ("set encoder target"))
	    { if (COMMAND ("set encoder target all"))
	       { int enc;
		 int error = 0;
		 printf ("Enter %d encoder values> ", NUMJ);
		 for (j=0; j<NUMJ; j++)
		  { if (scanf ("%d", &enc) != 1)
		     { printf ("error reading values\n");
		       error = 1;
		       break;
		     }
		    else
		     { cb->encTarget[j] = enc;
		     }
		  }
		 if (error)
		  { continue;
		  }
	       }
	      else
	       { j = key_buf[strlen("set encoder target ")]-'1';
		 cb->encTarget[j] = ival;
	       }
	    }
	   else if (COMMAND ("cycle by"))
	    { unsigned long mask;
	      int j, error;

	      if (cb->controlLevel != JOINT_LEVEL)
	       { printf ("Please enter angle mode first\n");
		 continue;
	       }
	      cb->stop = 1;
	      rciBlock (td);

	      ang *= DEGTORAD;

	      j = 0;
	      error = 0;
	      for (mask = jntMask; mask; mask >>= 1)
	       { if (mask & 1)
		  { if (cb->angTarget[j] + ang > jls->jmax[j] ||
			cb->angTarget[j] - ang < jls->jmin[j])
		     { printf("sweep would take joint %d out of bounds\n",j+1);
		       error = 1;
		       break;
		     }
		  }
		 j++;
	       }
	      if (error)
	       { continue;
	       }
	      doCycles (jntMask, ang);
	    }
	   else if (COMMAND ("set angle speed"))
	    { for (j=0; j<jls->numJ; j++)
	       { cb->angSpeed[j] = fval * DEGTORAD;
	       }
	    }
	   else if (COMMAND ("set angle accel"))
	    { for (j=0; j<jls->numJ; j++)
	       { cb->angAccel[j] = fval * DEGTORAD;
	       }
	    }
	   else if (COMMAND ("set encoder speed"))
	    { for (j=0; j<jls->numJ; j++)
	       { cb->encSpeed[j] = ival;
	       }
	    }
	   else if (COMMAND ("set encoder accel"))
	    { for (j=0; j<jls->numJ; j++)
	       { cb->encAccel[j] = ival;
	       }
	    }
	   else if (COMMAND ("set synchro on"))
	    { cb->synchronize = 1;
	    }
	   else if (COMMAND ("set synchro off"))
	    { cb->synchronize = 0;
	    }
	   else if (COMMAND ("set power on"))
	    { SET_POWER_ON (rbt);
	    }
	   else if (COMMAND ("set power off"))
	    { SET_POWER_OFF (rbt);
	    }
	   else if (COMMAND ("clear compcode"))
	    { cb->compcode = 0;
	    }
	 }
	while (!(COMMAND("quit")));

	rciClose();
}

display()
{
	int j;

/*---------------------------------------------------------------------------
012345678901234567890123456789012345678901234567890123456789012345678901234567
	t	t	t	t	t	t	t	t	t
Jnt      angle angTarget angSetpnt      encoder encTarget encSetpnt
 X    -XXX.XXX  -XXX.XXX  -XXX.XXX       XXXXXX    XXXXXX    XXXXXX

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

	printf (
"Jnt      angle angTarget angSetpnt       encoder encTarget\n");
	for (j=0; j<NUMJ; j++)
	 { printf (
"%2d  %c %8.3f  %8.3f  %8.3f       %6d    %6d    %6d\n",
		   j+1,
		   cb->torqueControl[j] ? 'T' : ' ',
		   how->ang[j] * RADTODEG,
		   cb->angTarget[j] * RADTODEG,
		   cb->angSetpnt[j] * RADTODEG,
		   how->enc[j],
		   cb->encTarget[j],
		   cb->encSetpnt[j]);
	 }
	putchar ('\n');
	printf ("compcode = 0x%04x   movecode = 0x%04x\n",
		 cb->compcode, cb->movecode);
	printf ("angSpeed = %g\n", cb->angSpeed[0]*RADTODEG);
	printf ("angAccel = %g\n", cb->angAccel[0]*RADTODEG);
	printf ("encSpeed = %g\n", cb->encSpeed[0]);
	printf ("encAccel = %g\n", cb->encAccel[0]);
	printf ("   block = %d\n", cb->block);
	printf ("    halt = %d\n", cb->halt);
	printf ("synchro: %s  controlLevel: %s  control is %s\n",
		cb->synchronize ? "ON" : "OFF" ,
		(cb->controlLevel == MOTOR_LEVEL) ? "ENCODER" :
		(cb->controlLevel == JOINT_LEVEL) ? "ANGLE" : "UNDEF",
		controlOn ? "ON" : "OFF");
}


#define MOVE(x)		cb->movecode = (x); rciBlock(td); cb->compcode = 0;\
			while (!(cb->compcode & (x))) nap(0.10);

doCycles (jntMask, ang)
unsigned long jntMask;
float ang;
{
	unsigned long mask;
	float target1[MAXJNTS];
	float target2[MAXJNTS];
	int quit, j;
	
	printf ("\nHit <CR> to abort\n");

	/* --- set up the target angles --- */

	j = 0;
	for (mask = jntMask; mask; mask >>= 1)
	 { if (mask & 1)
	    { target1[j] = cb->angTarget[j] - ang;
	      target2[j] = cb->angTarget[j] + ang;
	    }
	   else
	    { target1[j] = target2[j] = cb->angTarget[j]; /* just for safety */
	    }
	   j++;
	 }

	/* --- now do the cycling --- */
	
	quit = 0;
	while (!quit)
	 { j = 0;
	   for (mask = jntMask; mask; mask >>= 1)
	    { if (mask & 1)
	       { cb->angTarget[j] = target1[j];
	       }
	      j++;
	    }
	   cb->movecode = jntMask;
	   rciBlock (td);
	   cb->compcode = 0;

	   while (!(cb->compcode & jntMask) && !quit)
	    { if (testchar(0.0))
	       { while (getchar() != '\n')
		    ;
		 quit = 1;
	       }
	    }
	   if (quit)
	    { break;
	    }
	   j = 0;
	   for (mask = jntMask; mask; mask >>= 1)
	    { if (mask & 1)
	       { cb->angTarget[j] = target2[j];
	       }
	      j++;
	    }
	   cb->movecode = jntMask;
	   rciBlock (td);
	   cb->compcode = 0;

	   while (!(cb->compcode & jntMask) && !quit)
	    { if (testchar(0.0))
	       { while (getchar() != '\n')
		    ;
		 quit = 1;
	       }
	    }
	 }

	cb->stop = 1;
}
