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

/*=~=~=~=~ McGill-Computer-Vision-and-Robotics-Laboratory--C-Module ~=~=~=~=
 *		
 * MODULE NAME:	RCIchannel
 *
 * DESCRIPTION:	Contains the interface routines between the simulator program
 *		and the RCI program.
 * 
 * AUTHOR/DATE:	John Lloyd, Fall 1986
 *
 *~=~=~=~=~=~= McGill-Computer-Vision-and-Robotics-Laboratory ~=~=~=~=~=~=*/

#include <stdio.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <sys/errno.h>
#include <signal.h>
#include <fcntl.h>
#include <netdb.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include "../h/moper_com.h"
#include "../h/simDevice.h"
#include "../h/cdefs.h"

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

#define DEBUG	0

extern int errno;

initHostIO(hio)
HostIO *hio;
{
	bzero ((char*)hio, sizeof(HostIO));
	hio->oobsock = -1;
	hio->rcisock = -1;
	hio->regsock = -1;
}
#ifdef LINUX_CFG
void
#endif
sigHandler(hio)
HostIO *hio;
{
	char signum;
	int (*fxn)();
	long count;
	char junk[256];

	if (readAll (hio->oobsock, (char*)&count, sizeof(count)) <= 0)
	 { hio->error = HST_TIMEOUT_E;
#ifdef LINUX_CFG
	   return;
#else
	   return (-1);
#endif
	 }
	count = ntohl(count);
#if DEBUG
if (signum == 2) printf ("sim: sigHandler 2 begin\n");
	printf ("flush count = %d\n", (int)(count - hio->readCount));
#endif
	if (count - hio->readCount)
	 { if (readAll (hio->rcisock, junk, (int)(count-hio->readCount)) <= 0)
	    { hio->error = HST_TIMEOUT_E;
#ifdef LINUX_CFG
	    return;
#else
	    return (-1);
#endif
	    }
	 }
	count = htonl(hio->writeCount);
	if (writeAll (hio->oobsock, (char*)&count, sizeof(count)) < 0)
	 { hio->error = HST_TIMEOUT_E;
#ifdef LINUX_CFG
	    return;
#else
	    return (-1);
#endif

	 }
	if (readAll (hio->oobsock, &signum, sizeof(signum)) <= 0)
	 { hio->error = HST_TIMEOUT_E;
#ifdef LINUX_CFG
	    return;
#else
	    return (-1);
#endif
	 }
	hio->readCount = hio->writeCount = 0;

#if DEBUG
if (signum == 2) printf ("sim: sigHandler 2 end\n");
#endif
#ifdef LINUX_CFG
	    return;
#else
	    return (-1);
#endif

}

hostDevice(hio, address, useSigIO)
HostIO *hio;
char *address;
int useSigIO;
{
	hio->simname = (char*)copyofstr(address);
	signal(SIGIO, sigHandler);
	hio->useSigIO = useSigIO;
}

hostAttach(hio)
HostIO *hio;
{
	char ack;
	int slen;
	int accoobsock;
	short int oobportno;
	struct sockaddr_in sin;

	hio->readCount = hio->writeCount = 0;

	bzero((char*)&sin, sizeof(sin));
	slen = sizeof(sin);
	hio->rcisock = accept(hio->accrcisock,(struct sockaddr *)&sin,&slen);
	if (hio->rcisock < 0)
	 { perror ("sim: error in accept");
	   close (hio->accrcisock);
	   hio->accrcisock = -1;
	   return (-1);
	 }
	/* create oob data socket and remember port number */
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = INADDR_ANY;
	sin.sin_port = 0;
	accoobsock = socket(AF_INET,SOCK_STREAM,0);
	if (bind(accoobsock,(struct sockaddr *)&sin,sizeof(sin)) < 0)
	 { printf ("sim: error, can't bind oob socket\n");
	   return (-1);
	 }
	slen = sizeof(sin);
	if (getsockname(accoobsock,(struct sockaddr *)&sin,&slen) < 0)
	 { printf ("sim: error, can't get oob port number\n");
	   return (-1);
	 }
	oobportno = ntohs(sin.sin_port);
	listen(accoobsock,1);
#if DEBUG
printf ("sim: sending oobportno %d\n", oobportno);
#endif
	oobportno = htons(oobportno);
	if (write(hio->rcisock,(char *)&oobportno,sizeof(oobportno)) < 0)
	 { hio->error = HST_TIMEOUT_E;
	   return (-1);
	 }
	hio->oobsock = accept(accoobsock,(struct sockaddr *)&sin,&slen);
	close(hio->accrcisock);
	hio->accrcisock = -1;
	close(accoobsock);
#if DEBUG
printf ("sim: got rci socket %d\n", hio->rcisock);
#endif
	if (hio->useSigIO)
	 { if (fcntl (hio->oobsock, F_SETFL, 
		      fcntl(hio->oobsock,F_GETFL,0)|FASYNC) == -1)
	    { printf ("sim: FASYNC failed\n");
	      perror("");
	      return (-1);
	    }
	   if (fcntl (hio->oobsock, F_SETOWN, getpid()) == -1)
	    { printf ("sim: F_SETOWN failed\n");
	      perror("");
	      return (-1);
	    }
	 }
	ack = 'A';
	if (write(hio->rcisock,&ack,sizeof(ack)) < 0)
	 { hio->error = HST_TIMEOUT_E;
	   return (-1);
	 }
	return (1);
}

hostDetach(hio)
HostIO *hio;
{
	if (hio->regsock >= 0)
	 { close (hio->regsock);
	   hio->regsock = -1;
	 }
	if (hio->rcisock >= 0)
	 { close (hio->rcisock);
	   hio->rcisock = -1;
	 }
	if (hio->oobsock >= 0)
	 { fcntl(hio->oobsock,F_SETFL,fcntl(hio->oobsock,F_GETFL,0)&~FASYNC);
	   close (hio->oobsock);
	   hio->oobsock = -1;
	 }
}

hostRead (hio, buffer, size)
HostIO *hio;
char *buffer;
int size;
{
	short readcount;
	unsigned short wbuf;
#if DEBUG
printf ("hostRead\n"); fflush(stdout);
#endif
	if (readAllCnt (hio, hio->rcisock, (char*)&wbuf, sizeof(wbuf)) <= 0)
	 { return (-1);
	 }
	readcount = ntohs(wbuf);
	if (readcount < 0 || readcount > size*sizeof(short) 
			  || readcount > SIM_BUFSIZE*sizeof(short))
	 { hio->error = HST_COUNT_E;
	   fprintf (stderr, "bad read count (%d) from host\n", readcount);
	   return(-1);
	 }
	if (readcount)
	 { unsigned char rbuf[SIM_BUFSIZE];
	   register unsigned short *p1, *p2;
	   register int i;
	   if (readAllCnt (hio, hio->rcisock, (char*)rbuf, readcount) <= 0)
	    { return (-1);
	    }
	   p1 = (unsigned short*)rbuf;
	   p2 = (unsigned short*)buffer;
	   for (i=0; i<readcount; i+=sizeof(short))
	    { *p2++ = ntohs(*p1++);
	    }
	 }
	return (readcount/sizeof(short));
}

hostWrite (hio, buffer, size)
HostIO *hio;
char *buffer;
int size;
{
	unsigned short count = size*sizeof(short);
	unsigned char wbuf[SIM_BUFSIZE];
	register unsigned short *p1, *p2;
	register int i;
#if DEBUG
printf ("hostWrite\n"); fflush(stdout);
#endif
	p1 = (unsigned short *)buffer;
	p2 = (unsigned short *)wbuf;
	*p2++ = htons(count);
	if (count)
	 { for (i=0; i<count; i+=sizeof(short))
	    { *p2++ = htons(*p1++);
	    }
	 }
	if (writeAllCnt (hio, hio->rcisock, (char*)wbuf, 
					    (int)(count+sizeof(short))) < 0)
	 { return (-1);
	 }
	return (size);
}

hostSignal(hio)
HostIO *hio;
{
	/* This routine is not currently used or implemented */
	return (1);
}

hostOnSig(hio, signum, fxn, arg)
HostIO *hio;
int signum;
int (*fxn)();
int arg;
{
	if (signum == 1)
	 { hio->sig1Fxn = fxn;
	   hio->sig1Arg = arg;
	 }
	else if (signum == 2)
	 { hio->sig2Fxn = fxn;
	   hio->sig2Arg = arg;
	 }
}

readAllCnt (hio, fd, buffer, size)
HostIO *hio;
int fd;
char *buffer;
int size;
{
	int n = 0;
	int mask;
	fd_set fds;

	if (hio->useSigIO)
	 { mask = sigblock(sigmask(SIGIO));

	   FD_ZERO(&fds);
	   FD_SET(hio->oobsock,&fds);
	   FD_SET(fd,&fds);
	   if (select(FD_SETSIZE, &fds, (fd_set*)0, (fd_set*)0, 
					(struct timeval*)0) < 0)
	    { perror ("hostIO: select():");
	      exit (-1);
	    }

	   /* I *think* this is broken: what happens if oobsock *is*
	      set? The routine will return 0, which could cause
	      hostRead() to fail. I'll ignore this for now because
	      we don't use SIGIO anymore anyway. */

	   if (!FD_ISSET(hio->oobsock,&fds))
	    { if ((n = readAll(fd,buffer,size)) > 0)
	       { hio->readCount += n;
	       }
	    }
	   if (n <= 0)
	    { hio->error = HST_TIMEOUT_E;
	    }
	   sigsetmask(mask);
	   return(n);
	 }
	else
	 { if ((n = readAll(fd,buffer,size)) > 0)
	    { hio->readCount += n;
	    }
	   if (n <= 0)
	    { hio->error = HST_TIMEOUT_E;
	    }
	   return (n);
	 }
}

readAll(fd, buffer, size)
int fd;
char *buffer;
int size;
{
	int n, numread;
#if DEBUG
printf ("R%d-", fd); fflush(stdout);
#endif
	numread = 0;
	while (numread < size)
	 { n = read_restart (fd, &buffer[numread], size-numread);
	   if (n <= 0)
	    { return (n);
	    }
	   numread += n;
	 }
#if DEBUG
printf ("%d\n",size); fflush(stdout);
#endif
	return (numread);
}

writeAllCnt (hio, fd, buffer, size)
HostIO *hio;
int fd;
char *buffer;
int size;
{
	int n;
	int mask;
	mask = sigblock(sigmask(SIGIO));
	if ((n = writeAll (fd, buffer, size)) > 0);
	 { hio->writeCount += n;
	 }
	if (n < 0)
	 { hio->error = HST_TIMEOUT_E;
	 }
	sigsetmask(mask);
	return (n);
}

writeAll(fd, buffer, size)
int fd;
char *buffer;
int size;
{
	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);
}

hostListen (hio, fxn, arg)
HostIO *hio;
int (*fxn)();
int arg;
{
	int slen;
	struct servent *sp;
	short int rciportno;
	struct sockaddr_in sin;
	short int nlen, nlenNet;

	hostDetach(hio);
#if DEBUG
printf ("sim: trying to register %s\n", hio->simname);
#endif
	nlen = strlen(hio->simname);

	bzero((char *)&sin,sizeof(sin));
	/* create accept socket and remember port number */
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = INADDR_ANY;
	sin.sin_port = 0;
	hio->accrcisock = socket(AF_INET,SOCK_STREAM,0);
	if (bind(hio->accrcisock,(struct sockaddr *)&sin,sizeof(sin)) < 0)
	 { printf ("sim: error, can't bind acc socket\n");
	   return (-1);
	 }
	slen = sizeof(sin);
	if (getsockname(hio->accrcisock,(struct sockaddr *)&sin,&slen) < 0)
	 { printf ("sim: error, can't get acc port number\n");
	   return (-1);
	 }
	rciportno = ntohs(sin.sin_port);

	sp = getservbyname(SIMSERVICE,"tcp");
	if (sp == 0)
	 { printf ("sim: service %s not known\n", SIMSERVICE);
	   return (-1);
	 }
	hio->regsock = socket(AF_INET,SOCK_STREAM,0);
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = INADDR_ANY;
	sin.sin_port = sp->s_port;
	if (bind(hio->regsock,(struct sockaddr *)&sin,sizeof(sin)) == 0)
	 { spawnMultiplexor(hio->regsock);
	 }
	else if (errno != EADDRINUSE)
	 { printf ("sim: strange error binding socket\n");
	   return (-1);
	 }
	close(hio->regsock);
	hio->regsock = socket(AF_INET,SOCK_STREAM,0);
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = INADDR_ANY;
	sin.sin_port = sp->s_port;
	if (connect(hio->regsock,(struct sockaddr *)&sin,sizeof(sin)) < 0)
	 { printf ("can't connect to multiplexor to register ourselves\n");
	   return (-1);
	 }
#if DEBUG
printf ("sim: connected to mux\n");
#endif
	nlenNet = htons(nlen);
	write(hio->regsock,"R",1);
	rciportno = htons(rciportno);
	write(hio->regsock,(char *)&rciportno,sizeof(rciportno));
	write(hio->regsock,(char*)&nlenNet,sizeof(nlenNet));
	listen(hio->accrcisock,1);
	if (write(hio->regsock,hio->simname,nlen) < 0)
	 { hio->error = HST_TIMEOUT_E;
	   return (-1);
	 }
#if DEBUG
printf ("sim: listening for rci\n");
#endif
	return (1);
}

spawnMultiplexor(regsock)
int regsock;
{
	static char buf[16];
	static char *argv[3] = { "RCISimMuxd", buf, 0 };

	if (fork() == 0)
	 { sprintf (argv[1], "%d", regsock);
	   if (execvp ("RCISimMuxd", argv) < 0)
	    { printf ("sim: error, cannot spawn RCISimMuxd\n");
	      return (-1);
	    }
	 }
	else
	 { wait((union wait *)0);
	 }
	return (1);
}
