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

#include <sys/time.h>		/* for select() arguments */

#ifndef VXWORKS_CFG
#include <stdio.h>		/* for debugging */
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <signal.h>
#include <fcntl.h>
#include <errno.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#else
#include <vxWorks.h>
#include <stdioLib.h>
#include <types.h>
#include <socket.h>
#include <in.h>
#endif

#include "../h/cdefs.h"
#include "../h/IOdevice.h"
#include "../h/errorCodes.h"
#include "../h/simDevice.h"

#include "../rci/link.h"

#include "../drivers/driErrorMsgs.h"

extern int errno;

#define NOBLOCK	1
#define BLOCK	0

#define DEBUG	0

typedef struct _OpenDevsDesc {
	/* Fop VxWorks, to keep track of open devices */
	struct _OpenDevsDesc *next;
	struct _OpenDevsDesc *prev;
	int sd;
	int obd;
} OpenDevsDesc;

static OpenDevsDesc *openDevs;

static int writeAll();
static int writeAllCnt();
static int readAll();
static int readAllCnt();
static int waitState();
static int checkState();
static closeAllDevs();

static int initDone = 0;
#define INIT_CHECK()	if (!initDone) doInit();
static doInit()
{
	initDone = 1;
	openDevs = NULL;	/* paranoid */
	requestFlagInit (&initDone);
	requestExitFxn (closeAllDevs);
}

/* Begin VxWorks code for keeping track of open files */

static closeAllDevs()
{
	OpenDevsDesc *od, *od_next;

	for (od=openDevs; od; od=od_next)
	 { od_next = od->next;
	   close(od->sd);
	   close(od->obd);
	   FREE(od);
	 }
	openDevs = NULL;
}

static registerDevs(sd, obd)
{
	OpenDevsDesc *od;

	if ((od=ALLOC(OpenDevsDesc)) == NULL)
	 { return (-1);
	 }
	od->sd = sd;
	od->obd = obd;
	LINK_H (openDevs,od);
	return (1);
}

static closeDevs(sd, obd)
{
	OpenDevsDesc *od;
	for (od=openDevs; od; od=od->next)
	 { if (od->sd == sd && od->obd == obd)
	    { close(od->sd);
	      close(od->obd);
	      UNLINK_H(openDevs,od);
	      FREE(od);
	      return (1);
	    }
	 }
	return (-1);
}

/* End VxWorks code for keeping track of open files */

simRead (dev, buf, size)
IO_DEVICE *dev;
void *buf;
int size;
{
	/* Read up to 'size' bytes from the other end. */

	char *fxnName = "simRead()";
	struct simreg *simdev = (struct simreg *)dev->location;
	unsigned short readcount;
	unsigned short wbuf;

#if DEBUG
	printf ("simRead\n");
#endif
	if (readAllCnt (simdev, &wbuf, sizeof(wbuf), BLOCK) < 0)
	 { errorMsg (ETimeout, driTimeoutMsg, fxnName, 2);
	   return (-1);
	 }
	readcount = ntohs(wbuf);
	if (readcount > size || readcount > SIM_BUFSIZE)
	 { errorMsg (ECount, driCountMsg, fxnName, readcount);
	   return (-1);
	 }
	if (readcount)
	 { unsigned char rbuf[SIM_BUFSIZE];
	   unsigned short *b1, *b2;
	   int i;
	   if (readAllCnt (simdev, rbuf, readcount, BLOCK) < 0)
	    { errorMsg (ETimeout, driTimeoutMsg, fxnName, 3);
	      return (-1);
	    }
	   b1 = (unsigned short *)rbuf;
	   b2 = (unsigned short *)buf;
	   for (i=0; i<readcount; i+=sizeof(short))
	    { *b2++ = ntohs(*b1++);
	    }
	 }
	return (readcount);
}

simWrite (dev, buf, size)
IO_DEVICE *dev;
void *buf;
int size;
{
	/* Write 'size' bytes to the other end. */

	char *fxnName = "simWrite()";
	struct simreg *simdev = (struct simreg *)dev->location;
	unsigned short wbuf[SIM_BUFSIZE];
	register unsigned short *p1, *p2;
	int i;
#if DEBUG
	printf ("simWrite\n");
#endif
	/* round size up to the nearest short boundary */
	size = (size+sizeof(short)-1)/sizeof(short)*sizeof(short);
	if (size > SIM_BUFSIZE || size < 0)
	 { errorMsg (ECount, driCountMsg, fxnName, size);
	   return(-1);
	 }
	p1 = (unsigned short *)buf;
	p2 = wbuf;
	*p2++ = htons((unsigned short)size);
	if (size)
	 { for (i=0; i<size; i+=sizeof(short))
	    { *p2++ = htons(*p1++);
	    }
	 }
	if (writeAllCnt (simdev, (char*)wbuf, size+sizeof(short)) < 0)
	 { errorMsg (ETimeout, driTimeoutMsg, fxnName, 3);
	   return (-1);
	 }
	return (size);
}

simProbe (dev)
IO_DEVICE *dev;
{
	/* This is mostly a stub routine -- just sets a couple of fields.
	   Since the "device" is setup with sockets, we can assume that
	   it always exists. 
	 */
	INIT_CHECK();
	dev->location = 0;
	dev->timeout = 0;
	dev->size = sizeof(struct simreg);
	return (sizeof(struct simreg));
}

simAttach (dev)
IO_DEVICE *dev;
{
	/* Allocate data structure for the sim device and create socket
	   connections to the other end.
	 */
	char *fxnName = "simAttach()";
	struct simreg *simdev;
	char hostname[256];
	char simname[256];
	int port;

#if DEBUG
	printf ("simAttach(): addrStr = %s\n", dev->addrStr);
#endif
	INIT_CHECK();
	simdev = ALLOC (struct simreg);
	simdev->writeCount = 0;
	simdev->readCount = 0;
	parseSimAddr (simname, hostname, &port, dev->addrStr);
	if (connectToSim (simdev, simname, hostname, port) < 0)
	 { errorMsg (EConnect, driListeningMsg, fxnName);
	   free (simdev);
	   return (-1);
	 }
	simdev->state = SIM_NO_WAIT;
	dev->location = (unsigned char*)simdev;
	return (1);
}

simDetach (dev)
IO_DEVICE *dev;
{
	/* If a sim device is open, close the sockets and deallocate
	   the data structure.
	 */
	struct simreg *simdev;

	if (dev->location)
	 { simdev = (struct simreg *)dev->location;
	   if (closeDevs (simdev->sd, simdev->obd) < 0)
	    { printf ("Internal error in simDetach(): can't find devs\n");
	      close (simdev->sd);
	      close (simdev->obd);
	    }
	   free (dev->location);
	   return (1);
	 }
	else
	 { return (0);
	 }
}

simSignal (dev, num)
IO_DEVICE *dev;
int num;
{
	/* Send a signal to the other end using the out of band channel
	   (obd). 
	 */
	char *fxnName = "simSignal()";
	struct simreg *simdev = (struct simreg *)dev->location;
	char junk[256];
	unsigned count;
	char n;

#if DEBUG
if (num==2) printf ("rci: simSignal 2 start\n");
#endif
	count = htonl(simdev->writeCount);
	if (writeAll (simdev->obd, &count, sizeof(count)) < 0)
	 { errorMsg (ETimeout, driTimeoutMsg, fxnName, 1);
	   return (-1);
	 }
	if (readAll (simdev->obd, &count, sizeof(count), BLOCK) < 0)
	 { errorMsg (ETimeout, driTimeoutMsg, fxnName, 2);
	   return (-1);
	 }
	count = ntohl(count);
	readAll (simdev->sd, junk, count-simdev->readCount, BLOCK);
#if DEBUG
printf ("flush: count-readCount = %d\n", count-simdev->readCount);
#endif
	simdev->writeCount = simdev->readCount = 0;
	n = num;
	if (writeAll (simdev->obd, &n, sizeof(n)) < 0)
	 { errorMsg (ETimeout, driTimeoutMsg, fxnName, 3);
	   return (-1);
	 }
#if DEBUG
if (num==2) printf ("rci: simSignal 2 done\n");
#endif
}

simIoctl (dev, com, data)
IO_DEVICE *dev;
int com;
void *data;
{
	char *fxnName = "simIoctl()";

	INIT_CHECK();
	switch (com)
	 { /* Note: these timeout controls are not really used
	      with the simulator since it runs without timeouts.
	    */
	   case IOC_SIGINTER_ENABLE:
	   case IOC_SIGINTER_DISABLE:
	    { break;
	    }	     
	   case IOC_TIMEOUT:
	    { dev->timeout = *(int *)data;
	      break;
	    }
	   case IOC_GET_BLOCKTIME:
	    { *(int *)data = dev->blocktime;
	      break;
	    }
	   case IOC_SET_BLOCKTIME:
	    { dev->blocktime = *(int *)data;
	      break;
	    }
	   default:
	    { errorMsg (EBadIoctl, driIoctlMsg, fxnName, com);
	      return (-1);
	    }
	 }
	return (1);
}

simTestRead (dev)
IO_DEVICE *dev;
{
	/* Check to see if the other end is waiting to write. */

	struct simreg *simdev = (struct simreg *)dev->location;
	struct timeval tvZero;
	fd_set fds;

	FD_ZERO(&fds);
	FD_SET(simdev->sd,&fds);
	tvZero.tv_sec = tvZero.tv_usec = 0;
	select(FD_SETSIZE, &fds, (fd_set*)0, (fd_set*)0, &tvZero);
	return (FD_ISSET(simdev->sd,&fds));
}

simTestWrite (dev)
IO_DEVICE *dev;
{
	/* Check to see if the other end is waiting to read. */

	return (1);
}
		
static readAllCnt(simdev, buffer, size, noblock)
struct simreg *simdev;
char *buffer;
int size;
int noblock;
{
	int n;

	if(!simdev) return(0);
	if ((n = readAll(simdev->sd, buffer, size, noblock)) > 0)
	 { simdev->readCount += n;
	 }
	return (n);
}

static readAll(fd, buffer, size, noblock)
int fd;
char *buffer;
int size;
int noblock;
{
	/* If 'noblock' is true, return 0 if there is nothing to be read.
	   Otherwise, read until 'size' bytes have been read.
	 */
	int n, numread;

#if DEBUG
	printf ("R%d-", fd); fflush(stdout);
#endif
	if (noblock)
	 { fd_set rset;
	   int status;
	   struct timeval tvZero;

	   while (1)
	    {
	      FD_ZERO(&rset);
	      FD_SET(fd,&rset);
	      tvZero.tv_sec = tvZero.tv_usec = 0;
	      status = select(FD_SETSIZE,&rset,(fd_set*)0,(fd_set*)0,&tvZero);
#ifndef VXWORKS_CFG
	      if (status < 0 && errno != EINTR)
	       { perror ("simDriver: select");
		 exit (-1);
	       }
#else
	      if (status < 0)
	       { printf ("select returns %d\n", status);
		 exit (-1);
	       }
#endif
	      if (!FD_ISSET(fd,&rset))
	       { return(0);
	       }
	    }
	 }
	numread = 0;

	while (numread < size)
	 { n = read_restart (fd, &buffer[numread], size-numread);
	   if (n <= 0)
	    { printf ("read on %d returns %d\n", fd, n);
	      return (-1);
	    }
	   numread += n;
	 }
#if DEBUG
	printf ("%d\n",numread); fflush(stdout);
#endif
	return (numread);
}

static writeAllCnt (simdev, buffer, size)
struct simreg *simdev;
char *buffer;
int size;
{
	int n;
	if ((n = writeAll (simdev->sd, buffer, size)) > 0)
	 { simdev->writeCount += n;
	 }
	return (n);
}

static writeAll (fd, buffer, size)
int fd;
char *buffer;
int size;
{
	/* Just a basic wrapper for the 'write()' call. */
	int n;
#if DEBUG
	printf ("W%d-", fd); fflush(stdout);
#endif
	n = write (fd, buffer, size);
#if DEBUG
	printf ("%d\n", n); fflush(stdout);
#endif
	return (n);
}

#include "../rci/link.h"

#include "../robotsim/RCISimMuxd.h"

#ifdef VXWORKS_CFG 
int RCISimMuxService = 5347;	/* XXX !!! */
#endif
#ifdef LINUX_CFG
int RCISimMuxService = 5347;	/* XXX !!! */
#endif

connectToSim (simdev, simname, hostname, port)
struct simreg *simdev;
char *simname;
char *hostname;
int port;
{
	/* return a valid socket connection to a simulator on the specified
	   host that will service the specified 'simname'. */

	char *fxnName = "connectToSim()";

	int regsock;			/* multiplexor process connection */
	int rcisock;			/* simulator connection */
	int oobsock;			/* out of band simulator connection */
	short int rciportno;		/* port number for rcisock */
	short int oobportno;		/* port number for oobsock */
	struct servent *sp;		/* to reference multiplexor port */
	struct sockaddr_in sin;		/* for specifying socket */
	struct sockaddr_in save_sin;	/*   addressess */
	short int nlen, nlenNet;	/* 'simname' length variables */
	char ack;			/* for read/write of acknowledge */
	int k;

#if DEBUG
printf ("rci: trying to connect to %s\n", simname);
#endif
	nlen = strlen(simname);

	/* set up 'sim' address structure to reference target machine */

	bzero((char *)&sin,sizeof(sin)); /* needed on Suns */
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = inet_addr(hostname);
#ifdef VXWORKS_CFG
printf ("hostname=%s\n", hostname);
#endif
	if (sin.sin_addr.s_addr == -1)
	 { 
#ifndef VXWORKS_CFG
	   struct hostent *hp;
	   hp = gethostbyname(hostname);
	   if (hp == 0)
	    { errorMsg (EConnect, "%s -- host %s unknown\n", 
				  fxnName, hostname);
	      return (-1);
	    }
	   if ((hp->h_addrtype != AF_INET) || (hp->h_length != sizeof(struct in_addr)))
	    { errorMsg (EConnect, "%s -- host %s on strange network\n",
				  fxnName, hostname);
	      return (-1);
	    }
	   bcopy(hp->h_addr,(char *)&sin.sin_addr,hp->h_length);
#else
	   int hp;
	   hp = hostGetByName (hostname);
	   if (hp == ERROR)
	    { errorMsg (EConnect, "%s -- host %s unknown\n",fxnName,hostname);
	      return (-1);
	    }
	   sin.sin_addr.s_addr = hp;
#endif
	 }
	save_sin = sin;

	/* Look up port number for multiplexor and try and connect a
	   socket to it. */
	 
	if (port == -1)
	 { 
#ifdef  LINUX_CFG
	   sin.sin_port = htons((short)RCISimMuxService);
#else
#ifdef VXWORKS_CFG 
	   sin.sin_port = htons((short)RCISimMuxService);
#else
	   sp = getservbyname (SIMSERVICE, "tcp");
	   if (sp == 0)
	    { errorMsg (EInternal, "%s -- multiplexor service %s unknown\n",
				   fxnName, SIMSERVICE);
	      return (-1);
	    }
	   sin.sin_port = sp->s_port;
#endif
#endif
	 }
	else
	 { sin.sin_port = htons(port);
	 }
	regsock = socket(AF_INET,SOCK_STREAM,0);
	if (connect(regsock,(struct sockaddr *)&sin,sizeof(sin)) < 0)
	 { sysErrorMsg (EConnect, fxnName, "reg connect");
	   return (-1);
	 }
#if DEBUG
printf ("rci: connection made\n");
#endif

	/* Write to the multiplexor: a 'P' command, the length of 'simname',
	   and 'simname' itself. Then read back the available port number,
	   if any. If there is no prot, then wait a half second or so and
	   try again, in case we just released to the robot.
	 */
#define WAIT_TIME	1.0 /* seconds */
	rciportno = 0;
	for (k=0; k<2 && rciportno==0; k++)
	 { if (k>0)
	    { nap(WAIT_TIME);
	    }
	   write(regsock,"P",1);
	   nlenNet = htons(nlen);
	   write(regsock,(char*)&nlenNet,sizeof(nlenNet));
	   write(regsock,simname,nlen);
	   if (readAll(regsock,(char *)&rciportno,
		      		sizeof(rciportno),BLOCK) != sizeof(rciportno))
	    { errorMsg (EInternal, "%s -- error reading rci port\n", fxnName);
	      return (-1);
	    }
	   rciportno = ntohs(rciportno);
#if DEBUG
printf ("k=%d rciportno=%d\n", k, rciportno);
#endif
	 }
	close(regsock);
	if (rciportno == 0 /* NOT_REGISTERED */)
	 { errorMsg (EConnect, "%s -- simulator not registered\n", fxnName);
	   return (-1);
	 }
	rcisock = socket(AF_INET,SOCK_STREAM,0);
	/* Connect to the simulator port, and read back the port number
	   for the out-of-band channel. */

	sin = save_sin;
	sin.sin_port = htons(rciportno);
	if (connect(rcisock,(struct sockaddr *)&sin,sizeof(sin)) < 0)
	 { sysErrorMsg (EConnect, fxnName, "rci connect");
	   return (-1);
	 }
#if DEBUG
printf ("rci: got socket %d\n", rcisock);
#endif
	if (readAll(rcisock,(char *)&oobportno,
			    sizeof(oobportno),BLOCK) != sizeof(oobportno))
	 { errorMsg (EInternal, "%s -- error reading oob port\n", fxnName);
	   return (-1);
	 }
	/* Connect to the out-of-band channel, and then finish by
	   reading back an acknowledge from the other side. */
	oobsock = socket(AF_INET,SOCK_STREAM,0);
#if DEBUG
printf ("rci: got oob socket and portnumber %d\n", ntohs(oobportno));
#endif
	sin = save_sin;
	sin.sin_port = oobportno;
	if (connect(oobsock,(struct sockaddr *)&sin,sizeof(sin)) < 0)
	 { sysErrorMsg (EInternal, fxnName, "oob connect");
	   return (-1);
	 }
	if (read_restart(rcisock,&ack,sizeof(ack)) != 1 || ack != 'A')
	 { errorMsg (EInternal, "%s -- failed acknowledge\n", fxnName);
	   return (-1);
	 }
#if DEBUG
printf ("rci: connected to oob socket\n");
#endif
	if (registerDevs (rcisock,oobsock) < 0)
	 { errorMsg (EMemory, "%s -- no memory to register devs\n",fxnName);
	   close(rcisock);
	   close(oobsock);
	   return (-1);
	 }

	simdev->sd = rcisock;
	simdev->obd = oobsock;

	return (1);
}
