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

	An interactive test program that checks the basic communication with
	a particular robot (with the system default robot being assumed if
	none is specified).

	If called with no options, this program will try to connect to
	the system default robot and communicate with it at a rate
	of 30 msec., running its control task ON_TRIGGER. The program
	does not attempt to control the robot; it's only purpose
	is to check the communication.

	Other options allow ON_FUNCTION and ON_CLOCK scheduling to
	be tested, along with different sample rates and timeout
	tolerances. The program can also be made to communicate with
	two robots at the same time.

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


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

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

#include <setjmp.h>

#include "../h/rci.h"
#include "../h/ctree.h"
#include "../h/timeval.h"
#include "rbtcom.h"

int rate = 30;
int timeout = 2;
int testTime = 1800000;	/* 1.8e6 msec. (about 1/2 hour) */

extern do_start();
extern do_run();
extern do_release();

char key_buf[256];
char *keytree;

CTRL_BLK *ctrlBlk;
	
int expectedAbortCode;
int failureExpected = 0;
jmp_buf env;
char abortMsg[256];
int automatic = 0;
int startReleaseCount;

onAbort(code, msg, td)
int code;
char *msg;
RCI_DESC *td;
{
	extern struct timeval lastTrigger;

	if (code == expectedAbortCode)
	 { strcpy (abortMsg, msg);
	   if (abortMsg[strlen(abortMsg)-1] == '\n')
	    { abortMsg[strlen(abortMsg)-1] = '\0';
	    }
	   if (failureExpected)
	    { failureExpected = 0;
	    }
	   else
	    { longjmp (env, 1);
	    }
	 }
	else
	 { FILE *af;

	   rciPrintAbort (code, msg, td);
#ifndef VXWORKS_CFG
	   if (af = fopen ("abortlog", "w"))
	    { fprintf (af, "\nRCI ABORT: task '%s' on cpu %d, cycle %d\n%s\n",
		   	   rciTaskName(td), td->cpu, td->tcb->cycleCount, msg);
	      fprintf (af, "startReleaseCount = %d\n", startReleaseCount);
	      fprintf (af, "last trigger was at %d.%06d\n",
		   	   lastTrigger.tv_sec, lastTrigger.tv_usec);
	      fclose (af);
	    }
#endif
	   printf ("startReleaseCount = %d\n", startReleaseCount);
	   fflush(stdout);
	   exit (-1);
	 }
}

void startrci()
{
	rciStart (~0);
}

main(argc, argv)
int argc;
char **argv;
{
	RCI_DESC *td;
	char *robotName1 = NULL;
	char *robotName2 = NULL;
	float seconds;
	int scheduling = ON_TRIGGER;
	int abortCode;
	int com;
	int cpu = UNDEF;
	int delay1;
	int delay2;
	int burstDelay;
	int triggerDelay;
	int burstInterval;
	int i;

	rate = 30;
	timeout = 2;
	testTime = 1800000;
	expectedAbortCode = 0;
	automatic = 0;
	startReleaseCount = 0;

	argc--; argv++;
	while (argc > 0)
	 { 
	   if (ARGUMENT ("-r"))
	    { argc--; argv++;
	      rate = atoi(*argv);
	    }
	   else if (ARGUMENT ("-c"))
	    { argc--; argv++;
	      cpu = atoi(*argv);
	    }
	   else if (ARGUMENT ("-A"))
	    { automatic = 1;
	    }
	   else if (ARGUMENT ("-t"))
	    { argc--; argv++;
	      timeout = atoi(*argv);
	    }
	   else if (ARGUMENT ("-s"))
	    { argc--; argv++;
	      if (ARGUMENT ("CLOCK"))
	       { scheduling = ON_CLOCK;
	       }
	      else if (ARGUMENT ("TRIGGER"))
	       { scheduling = ON_TRIGGER;
	       }
	      else if (ARGUMENT ("FUNCTION"))
	       { scheduling = ON_FUNCTION;
	       }
	      else
	       { fatalMsg (-1, "illegal scheduling option %s\n", *argv);
	       }
	    }
	   else if (ARGUMENT ("-a"))
	    { argc--; argv++;
	      if (sscanf (*argv, "%x", &abortCode) != 1)
	       { fatalMsg (-1, "illegal value for -a\n");
	       }
	    }
	   else if (robotName1 == NULL && *argv[0] != '-')
	    { robotName1 = *argv;
	    }
	   else if (robotName2 == NULL && *argv[0] != '-')
	    { robotName2 = *argv;
	    }
	   else
	    { argc = -1;
	      break;
	    }
	   argc--; argv++;
	 }
	if (argc == -1)
	 { printf (
"USAGE: rbtcom [-A] [-t <timeout>] [-r <rate>] [-a <abortCode>]\n");
	   printf (
"              [-s (CLOCK|TRIGGER|FUNCTION)] [-c <cpu>]\n");
	   exit (-1);
	 }
	if (!robotName1)
	 { robotName1 = (char*)getDefaultRobot();
	 }
	if (cpu == UNDEF)
	 { cpu = 0;
	 }

	keytree = tree_match_parse("					\
(	quit								\
	start								\
	release								\
	rate								\
	 (%d", &rate, ",,,		\"interval (msec)\")		\
	show 								\
	nap								\
	 (%f", &seconds, ",,,		\"seconds (float)\")		\
	set								\
	 (hostFailure							\
	   (%x", &com, ",,,\"code for test failure on host (hex)\"	\
	     (%x", &abortCode, ",,,\"expected RCI abort code (hex)\"))	\
	  robotFailure							\
	   (%x", &com, ",,,\"code for test failure on robot (hex)\"	\
	     (%x", &abortCode, ",,,\"expected RCI abort code (hex)\"))	\
	  delay1							\
	   (%d", &delay1, ",,,	\"delay 1 (spin count)\")		\
	  delay2							\
	   (%d", &delay2, ",,,	\"delay 2 (spin count)\")		\
	  burstDelay							\
	   (%d", &burstDelay, ",,,\"burst delay (spin count)\"		\
	     (%d", &burstInterval, ",,,\"burst interval (cycles)\"))	\
	  triggerDelay							\
	   (%d", &triggerDelay, ",,,\"burst delay (spin count)\")	\
	 )								\
	scheduling 							\
	 (CLOCK FUNCTION TRIGGER)					\
	waitForFail							\
	startRelease							\
)",0);
	if (keytree == 0)
	 { printf ("ERROR/Parse error at %.30s\n", tree_match_err_at());
	   exit (-1);
	 }

	rciSetOptions (EXIT_ON_ERROR);
	td = rciCreate ("test", (1<<cpu));
	ctrlBlk = (CTRL_BLK*)allocMem (NULL, sizeof(CTRL_BLK), 0);
	ctrlBlk->triggerDelay = 50;
	rciAttachRobot (td, robotName1, NULL);
	if (robotName2)
	 { rciAttachRobot (td, robotName2, NULL);
	 }
	rciRate (td, rate, 0);
	rciTimeout (td, timeout, UNDEF, UNDEF);
	rciControlFxns (td, do_start, do_run, do_release, (int)ctrlBlk);
	rciScheduling (td, scheduling, td->rbts[0]->idev);

	signal(SIGALRM,startrci);
	rciStart(~0);
/*DEBUG*/
	if (automatic)
	 { startReleaseDrill(td);
	 }
/*END*/
/* 	tree_match_catch_sig (0, sigmask(SIGHUP)); */

	rciAbortHandler (onAbort);
	if (setjmp(env) == 1)
	 { printf ("Abort \"%s\" OK\n", abortMsg);
	   expectedAbortCode = 0;
	   failureExpected = 0;
	   ctrlBlk->abortCode = 0;
	 }
	do
 	 { failureExpected = 1;
	   tree_match (keytree, "RBTCOM> ", key_buf);
	   if (failureExpected == 0)
	    { printf ("Abort \"%s\" OK\n", abortMsg);
	      ctrlBlk->abortCode = 0;
	    }
	   failureExpected = 0;
	   if (COMMAND("show"))
	    { printf ("cycle %d\n", td->tcb->cycleCount);
	    }
	   else if (COMMAND ("release"))
	    { rciRelease (~0);
	    }
	   else if (COMMAND ("start"))
	    { rciStart (~0);
	    }
	   else if (COMMAND ("rate"))
	    { rciRate (td, rate, 0);
	    }
	   else if (COMMAND ("set com"))
	    { rciRate (td, rate, 0);
	      SET_MAINT_COM (td->rbts[0], com);
	    }
	   else if (COMMAND ("nap"))
	    { nap(seconds);
	    }
	   else if (COMMAND ("show"))
	    { printf ("\nabortCode=0x%x  com=0x%x  delay1=%d delay2=%d\n",
		      abortCode, com, delay1, delay2);
	    }
	   else if (COMMAND ("set hostFailure"))
	    { ctrlBlk->abortCode = com;
	      expectedAbortCode = abortCode;
	    }
	   else if (COMMAND ("set robotFailure"))
	    { ctrlBlk->com = com;
	      expectedAbortCode = abortCode;
	      failureExpected = 1;
	    }
	   else if (COMMAND ("set delay1"))
	    { ctrlBlk->delay1 = delay1;
	    }
	   else if (COMMAND ("set delay2"))
	    { ctrlBlk->delay2 = delay2;
	    }
	   else if (COMMAND ("set burstDelay"))
	    { ctrlBlk->burstInterval = burstInterval;
	      ctrlBlk->burstDelay = burstDelay;
	    }
	   else if (COMMAND ("set triggerDelay"))
	    { ctrlBlk->triggerDelay = triggerDelay;
	    }
	   else if (COMMAND ("waitForFail"))
	    { if (td->tcb->controlMode == ACTIVE)
	       { int k = 0;
		 while (td->tcb->controlMode == ACTIVE && k++ < 200)
		  { nap (0.1);
		  }
		 if (td->tcb->controlMode == ACTIVE)
		  { printf ("\n *** ERROR ***: failure did not occur\n\n");
		    rciRelease (~0);
		  }
		 else
		  { printf ("Abort \"%s\" OK\n", abortMsg);
		  }
	       }
	      ctrlBlk->abortCode = 0;
	    }
	   else if (COMMAND ("scheduling CLOCK"))
	    { scheduling = ON_CLOCK;
	      rciScheduling (td, scheduling, td->rbts[0]->idev);
	    }
	   else if (COMMAND ("scheduling TRIGGER"))
	    { scheduling = ON_TRIGGER;
	      rciScheduling (td, scheduling, td->rbts[0]->idev);
	    }
	   else if (COMMAND ("scheduling FUNCTION"))
	    { scheduling = ON_FUNCTION;
	      rciScheduling (td, scheduling, td->rbts[0]->idev);
	    }
	   else if (COMMAND ("startRelease"))
	    { startReleaseDrill(td);
	    }
	 }
	while (!(COMMAND("quit")));
	rciRelease (~0);
}

#if 0
autoTest()
{
	/* Not implemented yet. The idea WILL BE to 
	   Conduct an automatic test of the communication.

	   Over the course of the total test time, check each of 
	   the three scheduling disciplines.

	   Run at the specified rate with both a burst delay and
	   a ripple delay. 

	   Spend 1/2 of the time doing straight communication.
	   Spend 1/4 of the time doing back/back starts and releases.
	   Spend 1/4 of the time doing starts and release at some
	   random interval.
	 */
}
#endif

startReleaseDrill(td)
RCI_DESC *td;
{
	int k = 0;

	startReleaseCount = 0;

	rciRelease(~0);
	while (1)
	 { nap (0.1);
#if 0
	   rciStart(~0);
	   rciRelease(~0);
	   nap (0.1);
	   rciStart(~0);
	   rciRelease(~0);
	   nap (0.1);
	   rciStart(~0);
	   rciRelease(~0);
	   nap (0.1);
	   rciStart(~0);
	   rciBlock (td);
	   rciRelease(~0);
	   nap (0.1);
	   rciStart(~0);
	   rciBlock (td);
	   rciRelease(~0);
	   nap (0.5);
#endif
	   rciStart(~0);
	   rciBlock (td);
	   rciBlock (td);
	   rciRelease(~0);
	   startReleaseCount++;
	   k++;
	   if (k >= 100)
	    { printf ("%d\n", startReleaseCount);
	      fflush (stdout);
	      k = 0;
	    }
	 }
}
