/*-=-=-=-=-=-=-=-=-=-=-=- 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.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	simSupport
 *
 * DESCRIPTION:	Some subroutines to make interfacing an RCI task to a robot
 *		simulator program easier.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1987
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include <signal.h>
#include <strings.h>
#include <sys/time.h>

#include "../h/cdefs.h"
#include "../h/rciTask.h"
#include "../h/rciMacros.h"
#include "../h/IOdevice.h"

simAttachRobot (td, robotName, simAddr)
RCI_DESC *td;
const char *robotName;
const char *simAddr;
{
	/* used in place of 'rciAttachRobot()': attaches a robot
	   to an RCI task, but specifies a simulator connection as 
	   the I/O device for that robot.
	 */

	char *fxnName = "simAttachRobot()";
	struct devAddr devAddr;
	char simname[256];
	char hostname[256];
	char tagname[256];
	char *user;
	int port;

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	devAddr.name = "sim";
	devAddr.address = find_robot(robotName);

	if (!simAddr)
	 { 
#if defined(VXWORKS_CFG)
	   char ubuf[256];
	   remCurIdGet (ubuf, 0);
	   user = ubuf;
#else
	   user = (char*)getenv("USER");
#endif
	   sprintf (simname, "%s:%s", robotName, user ? user : "0");
	 }
	else
	 { parseSimAddr (tagname, hostname, &port, simAddr);
	   if (port != -1)
	    { sprintf (simname, "%s:%s@%s<%d>", 
		       robotName, tagname, hostname, port);
	    }
	   else
	    { sprintf (simname, "%s:%s@%s", robotName, tagname, hostname);
	    }
	 }
	devAddr.addrStr = simname;
	return (rciAttachRobot (td, robotName, &devAddr));
}

simScheduling (td)
RCI_DESC *td;
{
	/* Used in place of rciScheduling(): selects the appropriate
	   scheduling to run an RCI task off of a simulator.  
	 */

	char *fxnName = "simScheduling()";

	if (!planningLevelCheck(fxnName))
	 { return (-1);
	 }
	if (rciScheduling (td, ON_SIGNAL, SIGALRM) < 0)
	 { return (-1);
	 }
	return (1);
}

simRobotsReady(td)
RCI_DESC *td;
{
	/* A wakeup function that makes sure an RCI control task
	   running in simulated mode will execute only when (1)
	   the simulator signal arrives, and (2) when all the
	   robots are ready to run. If the robots are not ready
	   to run, the simulator signal is reset 
	*/

	RCI_RBT *rbt;

	if (td->tcb->interruptSet)
	 { int k;
	   td->tcb->interruptSet = 0;
	   for (k=0; k<TASKNUMROBOTS(td); k++)
	    { if (rbt = TASKROBOT(td, k))
	       { if (!(*rbt->ready)(rbt->readyArg))
		  { /* reschedule */
		    simSetTimer (td->tcb->interval/2, (struct timeval*)0);
		    return (0);
		  }
	       }
	    }
	   return (1);
	 }
	else
	 { return (0);
	 }	
}

simSetTimer (interval, tv0)
int interval;
struct timeval *tv0;
{
	struct itimerval value;
	int delay;

	if (tv0)
	 { int lag;
	   struct timeval tv1;
	   getSystemTime (&tv1);
	   lag = (tv1.tv_sec - tv0->tv_sec)*1000 +
		 (tv1.tv_usec - tv0->tv_usec)/1000;
	   if (lag <= interval/2)
	    { delay = interval - lag;
	    }
	   else
	    { delay = interval/2;
	    }
	 }
	else
	 { delay = interval;
	 }
	timerclear (&value.it_interval);
	value.it_value.tv_sec = delay/1000;
	value.it_value.tv_usec = (delay%1000)*1000;

	setitimer (ITIMER_REAL, &value, NULL);
}

#include <ctype.h>

parseSimAddr (simname, hostname, port, str)
char *hostname, *simname, *str;
int *port;
{
	/* The address is parsed as follows:

	   <simulatorname>[@<hostname>]['<'port'>'] */

	char *cp, *lp, *np;

	*port = -1;
	lp = &str[strlen(str)];
	while (isspace(*lp))
	 { lp--;
	 }
	if (*lp == '>')
	 { for (cp=lp-1; isdigit(*cp); cp--)
	      ;
	   if (*cp == '<' && cp != lp-1)
	    { *port = atoi(cp+1);
	      lp = cp-1;
	    }
	 }
	if ((cp = index(str,'@')) && cp+1 != lp)
	 { for (np=hostname,cp=cp+1; cp!=lp; )
	    { *np++ = *cp++;
	    }
	   *np = '\0';
	   lp = index(str,'@');
	 }
	else
	 { 
#ifndef VXWORKS_CFG
	   strcpy (hostname, "127.0.0.1");
#else
	   /* use the vxWorks host */
	   extern char sysBootHost[];
	   strcpy (hostname, sysBootHost);
#endif
	 }
	if (str==lp)
	 { char *user;

#if defined(VXWORKS_CFG)
	   char ubuf[256];
	   remCurIdGet (ubuf, 0);
	   user = ubuf;
#else
	   user = (char*)getenv("USER");
#endif
	   if (user)
	    { strcpy (simname, user);
	    }
	   else
	    { strcpy (simname, "0");
	    }
	 }
	else
	 { for (np=simname,cp=str; cp!=lp; )
	    { *np++ = *cp++;
	    }
	   *np = '\0';
	 }
}

#if 0

checkSimAddr (simname)
char *simname;
{
	/* Check to see if a simulator address string is legal.
	   Returns 1 if it is, and 0 if it is not.

	   The address is parsed as follows:

	   <simulatorname>[@<hostname>]['<'port'>'] */

	char *cp, *lp, *np;

	lp = &str[strlen(str)];
	while (isspace(*lp))
	 { lp--;
	 }
	if (*lp == '>')
	 { for (cp=lp-1; isdigit(*cp); cp--)
	      ;
	   if (*cp == '<' && cp != lp-1)
	    { lp = cp-1;
	    }
	   else
	    { return (0);
	    }
	 }
	if (cp = index(str,'@'))
	 { while (isalnum(*np) || (*np=='.'))
	    { cp++;
	    }
	   if (cp != lp)
	    { return (0);
	    }
	   lp = index(str,'@');
	 }
	return (1);
}

#endif

