/*
 *   DIS/x : An implementation of the IEEE 1278.1 protocol
 *
 *   Copyright (C) 1997, Riley Rainey (rrainey@ix.netcom.com)
 *
 *   This library is free software; you can redistribute it and/or
 *   modify it under the terms of either:
 *
 *   a) the GNU Library General Public License as published by the Free
 *   Software Foundation; either version 2 of the License, or (at your
 *   option) any later version.  A description of the terms and conditions
 *   of the GLPL may be found in the "COPYING.LIB" file.
 *
 *   b) the "Artistic License" which comes with this Kit.  Information
 *   about this license may be found in the "Artistic" file.
 *
 *   This library is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *   Library General Public License or the Artistic License for more details.
 *
 *   You should have received a copy of the GNU Library General Public
 *   License along with this library; if not, write to the Free
 *   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *   Information describing how to contact the author can be found in the
 *   README file.
 */
#ifdef WINNT
#define WINVER WindowsXP
#endif
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <sys/types.h>
#ifdef WINNT
#include <winsock2.h>
#include <ws2tcpip.h>
#else
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netdb.h>
#include <net/if.h>
#endif
#include <unistd.h>
#include <sys/time.h>
#include <math.h>
#include <ctype.h>

#include "../../util/error.h"
#include "../../util/memory.h"
#include "xdr.h"
#include "xdr_dis.h"
#include "datum.h"

#define dis_IMPORT
#include "dis.h"

#ifdef WINNT
	// recvmsg() not available under WINNT.
	#undef HAVE_RECVMSG
#else
	#define HAVE_RECVMSG
	// Cope with slightly different field names of msghdr: Solaris and BSD call
	// them "msg_accessright...", any other system calls them "msg_control...".
	#undef  HAVE_MSG_ACCRIGHTS
	#define HAVE_MSG_CONTROL
#endif

#define MILLION			1000000
#define dis_timestamp_const	2147483646L		/* 2 ^ 31 - 1 */

#ifdef WINNT
typedef __int64 my_quad_t;
#else
typedef long long my_quad_t;
#endif

#ifdef WINNT
#else
#define INVALID_SOCKET	-1
#define SOCKET_ERROR	-1
#endif


#ifdef WINNT

static char *errorCodeToString(int code)
{
	static char s[999];
	char win_err_descr[900];
	DWORD err = FormatMessageA(
		FORMAT_MESSAGE_FROM_SYSTEM,
		NULL,
		code,
		LANG_SYSTEM_DEFAULT,
		win_err_descr,
		sizeof(win_err_descr),
		NULL
	);
	if( err > 0 ){
		snprintf(s, sizeof(s), "%s (Windows error code %d)", win_err_descr, code);
	} else {
		snprintf(s, sizeof(s), "error code %d (description not available: FormatMessageA() failed with code %lu)", code, err);
	}
	return s;
}

#endif


DISForce dis_parseForce(char *s)
{
	if( s == NULL )
		return -1;
	else if( strcmp(s, "Other") == 0 )
		return DISForceOther;
	else if( strcmp(s, "Friendly") == 0 )
		return DISForceFriendly;
	else if( strcmp(s, "Opposing") == 0 )
		return DISForceOpposing;
	else if( strcmp(s, "Neutral") == 0 )
		return DISForceNeutral;
	else
		return -1;
}


char * dis_forceToString(DISForce force)
{
	switch(force){
	case DISForceOther: return "Other";
	case DISForceFriendly: return "Friendly";
	case DISForceOpposing: return "Opposing";
	case DISForceNeutral: return "Neutral";
	default: error_internal("invalid force: %d", force);
	}
}


int dis_entityWildcardMatch (const dis_entity_type *in,
					 const dis_entity_type *pattern,
					 const dis_entity_type *pattern_mask)
{
	if (pattern_mask->kind == 0 || 
		pattern->kind == in->kind) {

		if (pattern_mask->domain == 0 || 
			pattern->domain == in->domain) {

			if (pattern_mask->country == 0 || 
				pattern->country == in->country) {

				if (pattern_mask->category == 0 || 
					pattern->category == in->category) {

					if (pattern_mask->subcategory == 0 || 
						pattern->subcategory == in->subcategory) {

						if (pattern_mask->specific == 0 || 
							pattern->specific == in->specific) {

							if (pattern_mask->extra == 0 || 
								pattern->extra == in->extra) {
								return 1;
							}

						}

					}

				}

			}

		}

	}

	return 0;
}

dis_Result
dis_addArticulationParm(dis_entity_state_pdu * esp, dis_articulation_parm * parm, int *parmID)
{
	int       n = esp->art_parm_count + 1;

	if (esp->art_parm_count == 0) {
		esp->art_parm = (dis_articulation_parm *)
			memory_allocate(sizeof(dis_articulation_parm), NULL);
	}
	else {
		esp->art_parm = (dis_articulation_parm *)
			memory_realloc(esp->art_parm, sizeof(dis_articulation_parm) * n);
	}

/*
 *  Return an error if the memory could not be allocated
 */

	if (esp->art_parm == NULL) {
		esp->art_parm_count = 0;
		return dis_RESULT_NO_MEMORY;
	}

	esp->art_parm[esp->art_parm_count] = *parm;
	esp->art_parm_count = n;
	*parmID = n;
	return dis_RESULT_OK;
}

int
dis_setNBIOState(dis_Transceiver * xcvr, int state)
{
#ifdef WINNT
	unsigned long int i;
#else
	int i;
#endif

	i = (state) ? 1 : 0;

#ifdef WINNT
	if (ioctlsocket(xcvr->s, FIONBIO, &i) != 0) {
#else
	if (ioctl(xcvr->s, FIONBIO, &i) != 0) {
#endif
		return -1;
	}
	return 0;
}

dis_Transceiver *
dis_openTransceiver(int isServer, char *host_name, int host_port)
{
#ifdef WINNT
	//char      Hostname[100];
	//HOSTENT  *pHostEnt;
	//char     *ad;

#else
	char      buf[BUFSIZ];
	int       n;
	struct ifconf ifc;
	struct ifreq *ifr;

#endif
	int       s, i = 0;
	int       on = 1;
	dis_Transceiver *xcvr;
	int isClient, useBroadcast;

	/* Empty host name is the same as NULL: */
	if( host_name != NULL && host_name[0] == 0 )
		host_name = NULL;
	
	/* Check port range: */
	if( !(0 <= host_port && host_port <= 65535) )
		error_internal("expected host port in [0,65535], %d given", host_port);
	
	isClient = ! isServer;
	useBroadcast = isClient && host_name == NULL;

#ifdef WINNT
	/*
	 * Initialize Windows sockets DLL.
	 */
	WORD wVersionRequested;
	WSADATA wsaData;
	int err;

	wVersionRequested = MAKEWORD(2, 2);

	err = WSAStartup(wVersionRequested, &wsaData);
	if (err != 0) {
		fprintf(stderr, "failed initializing Winsock.dll -- error code from WSAStartup() is %d\n", err);
		return NULL;
	}

	if (LOBYTE(wsaData.wVersion) != 2 || HIBYTE(wsaData.wVersion) != 2) {
		fprintf(stderr, "could not find a usable version of Winsock.dll\n");
		WSACleanup();
		return NULL;
	}
#endif

	/* Create socket: */
	if ((s = socket(AF_INET, SOCK_DGRAM, 0)) == INVALID_SOCKET) {
		perror("failed creating Internet socket");
		return NULL;
	}

	/*
	 * If client without relay server, enable broadcasting on any interface
	 * on the specified port:
	 */
	if( isClient && host_name == NULL ){
		
		struct sockaddr_in sin;
		
		if( useBroadcast ){
			if (setsockopt(s, SOL_SOCKET, SO_BROADCAST, (char *) &on,
						   sizeof(on)) == SOCKET_ERROR) {
#ifdef WINNT
				errno = WSAGetLastError();
#endif
				perror("can't set broadcast flag");
				return NULL;
			}

			if (setsockopt(s, SOL_SOCKET, SO_REUSEADDR, (char *) &on,
						   sizeof(on)) == SOCKET_ERROR) {
				perror("can't reuse broadcast port");
				return NULL;
			}
		}

		sin.sin_family = AF_INET;
		sin.sin_addr.s_addr = htonl(INADDR_ANY);
		sin.sin_port = htons(host_port);
		if( bind(s, (struct sockaddr *) &sin, sizeof(sin)) != 0 ){
			perror("bind()");
			return NULL;
		}
	}
	
	if( isServer ){
		
		struct sockaddr_in sin;
		
		if (setsockopt(s, SOL_SOCKET, SO_REUSEADDR, (char *) &on,
					   sizeof(on)) == SOCKET_ERROR) {
			perror("can't reuse port");
			return NULL;
		}

		sin.sin_family = AF_INET;
		sin.sin_addr.s_addr = htonl(INADDR_ANY);
		sin.sin_port = htons(host_port);
		if( bind(s, (struct sockaddr *) &sin, sizeof(sin)) != 0 ){
			perror("bind()");
			return NULL;
		}
	}

	/*
	 *  Allocate and initialize the transceiver structure.
	 */

	xcvr = (dis_Transceiver *) memory_allocate(sizeof(dis_Transceiver), NULL);
	xcvr->s = 0;
	xcvr->num_dest = 0;
	xcvr->s = s;

	/*
	 * If client with relay server, configure a single destination:
	 */
	if ( isClient && host_name != NULL ) {
		char service[6];
		struct addrinfo hints;
		struct addrinfo *results;
		
		/* Retrieves list of available matching relay host services: */
		memory_zero(&hints);
		hints.ai_family = AF_INET;
		hints.ai_socktype = SOCK_DGRAM;
		hints.ai_protocol = 0;
		hints.ai_flags = 0;
		sprintf(service, "%d", host_port);
		int err = getaddrinfo(host_name, service, &hints, &results);
		if( err != 0 ){
			fprintf(stderr, "Failed retrieving info for %s:%d: %s\n",
				host_name, host_port, gai_strerror(err));
			close(s);
			memory_dispose(xcvr);
			return NULL;
		}
		if( results == NULL ){
			fprintf(stderr, "The host %s:%d does not support IPv4 datagrams\n",
				host_name, host_port);
			freeaddrinfo(results);
			close(s);
			memory_dispose(xcvr);
			return NULL;
		}
		/* ...and use the first matching result: */
		xcvr->dest[0].addr = *((struct sockaddr_in *)(results->ai_addr));
		xcvr->dest[0].type = 1;
		xcvr->num_dest = 1;
		freeaddrinfo(results);
		return xcvr;
	}
	
	/*
	 * Currently the relay server uses its own sending function, so no need to
	 * configure the destinations for it.
	 */
	if( isServer ){
		xcvr->num_dest = 0;
		return xcvr;
	}

/*
 *  Client using broadcast: configure all the available interfaces.
 * 
 *  Determine how many interfaces are configured on the local system.
 */

#ifdef WINNT
#ifdef notdef
	gethostname(Hostname, sizeof(Hostname));
	pHostEnt = gethostbyname(Hostname);

	i = 0;
	while (pHostEnt->h_addr_list[i]) {
		// pHostEnt->h_addr_list[i] - the current address in host order
		ad = pHostEnt->h_addr_list[i];
		bcopy((char *) ad,
			  (char *) &xcvr->dest[i].addr.sin_addr,
			  sizeof(xcvr->dest[i].addr));
		bcopy((char *) ad,
			  (char *) x,
			  4);
		xcvr->dest[i].addr.sin_family = AF_INET;
		xcvr->dest[i].addr.sin_port = htons(host_port);
		xcvr->dest[i].type = 0;
		i++;
	}
#endif
	xcvr->dest[0].addr.sin_family = AF_INET;
	xcvr->dest[0].addr.sin_addr.S_un.S_addr = INADDR_BROADCAST;
	xcvr->dest[0].addr.sin_port = htons(host_port);
	xcvr->dest[0].type = 0;
	i = 1;

#else

	ifc.ifc_len = BUFSIZ;
	ifc.ifc_ifcu.ifcu_buf = buf;
	if (ioctl(s, SIOCGIFCONF, (char *) &ifc) < 0) {
		perror("error getting interface configuration");
		close(s);
		memory_dispose(xcvr);
		return NULL;
	}

	n = ifc.ifc_len / sizeof(struct ifreq);

/*
 *  Insure that there are enough elements in blist to accomodate all interfaces.
 */

	if (n > 32) {
		fprintf(stderr, "Too many host interfaces: %d\n", n);
		memory_dispose(xcvr);
		return NULL;
	}

	for (ifr = ifc.ifc_req; --n >= 0; ifr++) {

/*
 *  We're only interested in Internet domain interfaces
 */

		if (ifr->ifr_addr.sa_family != AF_INET)
			continue;

		if (ioctl(s, SIOCGIFFLAGS, (char *) ifr) < 0) {
			perror("error getting interface flags");
			close(s);
			memory_dispose(xcvr);
			return NULL;
		}

/*
 *  Skip boring cases ...
 */

		if( (ifr->ifr_flags & IFF_UP) == 0)
			/* Ignore interface down. */
			continue;
		
		if( (ifr->ifr_flags & IFF_LOOPBACK) != 0 )
			/* Ignore loopback interface. */
			continue;

/*
 *  Get the appropriate broadcast address based on the interface type.
 */

		if (! useBroadcast && ifr->ifr_flags & IFF_POINTOPOINT) {
			if (ioctl(s, SIOCGIFDSTADDR, (char *) ifr) < 0) {
				close(s);
				perror("error getting address");
				memory_dispose(xcvr);
				return NULL;
			}

			bcopy((char *) &ifr->ifr_dstaddr,
				  (char *) &xcvr->dest[i].addr,
				  sizeof(ifr->ifr_dstaddr));

		}
		else if (useBroadcast && ifr->ifr_flags & IFF_BROADCAST) {
			if (ioctl(s, SIOCGIFBRDADDR, (char *) ifr) < 0) {
				close(s);
				perror("error getting broadcast address");
				memory_dispose(xcvr);
				return NULL;
			}

			bcopy((char *) &ifr->ifr_broadaddr,
				  (char *) &xcvr->dest[i].addr,
				  sizeof(ifr->ifr_broadaddr));

		}
		else {
			continue;
		}

		xcvr->dest[i].addr.sin_port = htons(host_port);
		xcvr->dest[i].type = 0;
		i++;

	}

#endif							/* not WINNT */

	xcvr->num_dest = i;

	return xcvr;
}

void
dis_closeTransceiver(dis_Transceiver * xcvr)
{
	if( xcvr == NULL )
		return;
#ifdef WINNT
	shutdown(xcvr->s, SD_BOTH);
	closesocket(xcvr->s);
	WSACleanup();
#else
	close(xcvr->s);
#endif
	memory_dispose(xcvr);
}

int
dis_readPDU(dis_Transceiver * xcvr, dis_pdu * pdu)
{
	char      buffer[2048];
	int       size;
	xdr_Type *xdr;
	
#ifdef HAVE_RECVMSG
	struct sockaddr from;
	struct msghdr msg;
	struct iovec vec;
#endif

read_next_pdu:
#ifdef HAVE_RECVMSG
	msg.msg_name = (caddr_t) & from;
	msg.msg_namelen = sizeof(from);
	msg.msg_iov = &vec;
	msg.msg_iovlen = 1;
#ifdef HAVE_MSG_ACCRIGHTS 
	msg.msg_accrights = (caddr_t) NULL;
	msg.msg_accrightslen = 0;
#endif    
#ifdef HAVE_MSG_CONTROL
	msg.msg_control = (caddr_t) NULL;
	msg.msg_controllen = 0;
#endif    
	vec.iov_base = (caddr_t) & buffer;
	vec.iov_len = sizeof(buffer);

	size = recvmsg(xcvr->s, &msg, 0);
#else
	size = recv(xcvr->s, buffer, sizeof(buffer), 0);
#endif

	if (size == -1) {
		return 0;
	}

	xdr = xdr_new(buffer, size, xdr_DECODE);

	if( ! xdr_dis_pdu(xdr, pdu) ){
		fprintf(stderr, "ERROR: failed decoding received PDU: %s\n",
			xdr_getErrorDescription(xdr));
		xdr_free(xdr);
		goto read_next_pdu;
	}

	if( xdr_getpos(xdr) != pdu->hdr.length ){
		fprintf(stderr, "ERROR: failed decoding received PDU: stated length %d does not match actual packet length %d\n",
			pdu->hdr.length, xdr_getpos(xdr));
		xdr_free(xdr);
		goto read_next_pdu;
	}
	
	xdr_free(xdr);
	
	return 1;
}

int
dis_writePDU(dis_Transceiver * xcvr, dis_pdu * pdu)
{
	char      buffer[2048], *p;

#ifdef HAVE_RECVMSG
	struct msghdr msg;
	struct iovec vec;
#endif
	xdr_Type *xdr;
	int       i, len;
	int       err = 0;

/*
 *  Fill-out any length fields internal to the PDU (other than the length
 *  field in the header.
 */

	dis_addPDUSizes(pdu);

	xdr = xdr_new(buffer, sizeof(buffer), xdr_ENCODE);

	if( ! xdr_dis_pdu(xdr, pdu) )
		error_internal("dis_writePDU(): failed encoding packet: %s",
			xdr_getErrorDescription(xdr));

	len = xdr_getpos(xdr);
	
	xdr_free(xdr);

/*
 *  Now for a hack. We need to insert the correct packet length into
 *  the PDU header.  The header is somewhat stable from one protocol release
 *  to the next, so I've just hard-coded it here.
 */

	p = buffer + 8;
	*((u_short *) p) = htons(len);

#ifdef HAVE_RECVMSG

	msg.msg_namelen = sizeof(struct sockaddr);

	msg.msg_iov = &vec;
	msg.msg_iovlen = 1;
#ifdef HAVE_MSG_ACCRIGHTS
	msg.msg_accrights = (caddr_t) NULL;
	msg.msg_accrightslen = 0;
#endif
#ifdef HAVE_MSG_CONTROL
	msg.msg_control = (caddr_t) NULL;
	msg.msg_controllen = 0;
#endif    
	vec.iov_base = (caddr_t) & buffer;
	vec.iov_len = len;

	for (i = 0; i < xcvr->num_dest; ++i) {
		msg.msg_name = (caddr_t) & xcvr->dest[i].addr;
		if (sendmsg(xcvr->s, &msg, 0) == -1) {
			perror("on sendmsg");
			err = 1;
		}
	};

#else

	for (i = 0; i < xcvr->num_dest; ++i) {
		if (sendto(xcvr->s, buffer, len, 0,
				(struct sockaddr *) &xcvr->dest[i].addr,
				sizeof(struct sockaddr)) == -1) {
#ifdef WINNT
			fprintf(stderr, "failed sending packet: %s\n", errorCodeToString(WSAGetLastError()));
#else
			perror("sendto()");
#endif
		}
	};

#endif
	
	if( err )
		fprintf(stderr, "ERROR: failed sending DIS PDU to at least one destination address.\n");

	return ! err;
}


static void dis_disposeVariableDatum(dis_variable_datum *v)
{
	/*
	 * BEWARE.
	 * ======
	 * Here anything other than the types listed below is assumed
	 * to be a dynamically allocated string. The same assumption
	 * holds for xdr_dis_variable_datum() so both functions MUST be
	 * kept in sync.
	 */
	if( !( v->datum_id == datum_GeocentricCoordinatesX
		|| v->datum_id == datum_GeocentricCoordinatesY
		|| v->datum_id == datum_GeocentricCoordinatesZ
		)
	){
		memory_dispose(v->value.ptr_value);
	}
}


void
dis_freePDUComponents(dis_pdu * p)
{
	int       i, j;
	
	if( p == NULL )
		return;

	switch (p->hdr.pdu_type) {
		
	case PDUTypeDetonation:
		{
			dis_detonation_pdu *det = (dis_detonation_pdu *) p;
			if (det->num_art_parms > 0) {
				memory_dispose(det->art_parm);
			}
		}
		break;
		
	case PDUTypeEmission:
		{
			dis_em_emission_pdu *pdu = (dis_em_emission_pdu *) p;
			dis_em_system_info *s = pdu->system;
			dis_beam_info *b;

			for (i = 0; i < pdu->num_systems; ++i, ++s) {
				b = s->beam;
				for (j = 0; j < s->num_beams; ++j, ++b) {
					if (b->num_targets > 0) {
						memory_dispose((char *) b->tracked_target);
					}
				}
				if (s->num_beams > 0) {
					memory_dispose(s->beam);
				}
			}
			if (pdu->num_systems > 0) {
				memory_dispose(pdu->system);
			}
		}
		break;

	case PDUTypeEntityState:
		{
			dis_entity_state_pdu *pdu = (dis_entity_state_pdu *) p;

			if (pdu->art_parm_count > 0) {
				memory_dispose(pdu->art_parm);
			}
		}
		break;
	
	case PDUTypeComment:
		{
			dis_comment_pdu *m = (dis_comment_pdu *) p;
			if( m->num_fixed_data > 0 )
				memory_dispose(m->fixed_datum);
			for(i = 0; i < m->num_variable_data; i++){
				dis_disposeVariableDatum( &m->variable_datum[i] );
			}
			memory_dispose(m->variable_datum);
		}
		break;
	
	case PDUTypeSetData:
		{
			dis_set_data_pdu *sd = (dis_set_data_pdu *) p;
			dis_datum_spec_record *dsr = &sd->datum_info;
			if( dsr->num_fixed_data > 0 )
				memory_dispose(dsr->fixed_datum);
			for(i = 0; i < dsr->num_variable_data; i++){
				dis_disposeVariableDatum( &dsr->variable_datum[i] );
			}
			memory_dispose(dsr->variable_datum);
		}
		break;

	default:
		break;
	}
}


void
dis_addPDUSizes(dis_pdu * p)
{
	int       i, j;

	switch (p->hdr.pdu_type) {
	case PDUTypeEmission:
		{
			dis_em_emission_pdu *pdu = (dis_em_emission_pdu *) p;
			dis_em_system_info *s = pdu->system;
			dis_beam_info *b;
			unsigned long len;

			for (i = 0; i < pdu->num_systems; ++i, ++s) {
				b = s->beam;
				len = 0;
				for (j = 0; j < pdu->system[i].num_beams; ++j, ++b) {
					b->beam_data_length = 13 + b->num_targets * 2;
					len += b->beam_data_length;
				}
				s->sys_data_length = (unsigned char) (5 + len);
			}
		}
		break;

	default:
		break;
	}
}



int
dis_getRealTime(dis_time * result)
{
	struct timeval t;
	struct timezone tz;
	my_quad_t i;

	if (gettimeofday(&t, &tz) != 0) {
		return -1;
	}
	result->hour = t.tv_sec / 3600;
	i = (t.tv_sec % 3600) * MILLION + t.tv_usec;
	i = (i * dis_timestamp_const / 3600) / MILLION;
	// set relative time (bit 32 reset)
	result->rel.timexxx = i & 0x7fffffff;
	return 0;
}

int
dis_getTimestamp(dis_timestamp * result)
{

#if defined(NPSNET_COMPAT)
	time_t    i = time(0);

	result->time = i >> 1;
	result->type = i & 1;
#else
	struct timeval t;
	struct timezone tz;
	my_quad_t i;

	if (gettimeofday(&t, &tz) != 0) {
		return -1;
	}
	i = (t.tv_sec % 3600) * MILLION + t.tv_usec;
	i = (i * dis_timestamp_const / 3600) / MILLION;
	// set relative time (bit 32 reset))
	result->timexxx = i & 0x7ffffff;
	//result->type = 0;
#endif
	return 0;
}

void
dis_timestampToTimeval(dis_timestamp * in, struct timeval *out)
{
#if defined(NPSNET_COMPAT)
	out->tv_sec = (in->time << 1) + in->type;
	out->tv_usec = 0;
#else
	error_internal("not implemented", 0);
/*
	my_quad_t i;

	i = (my_quad_t) in->time * MILLION * 3600 / dis_timestamp_const;
	out->tv_sec = (long) ( i / MILLION);
	out->tv_usec = (long) ( i % MILLION );
*/
#endif
}

void
dis_timeToTimeval(dis_time * in, struct timeval *out)
{
	error_internal("not implemented", 0);
/*
	my_quad_t i;

	i = (my_quad_t) in->rel.time * MILLION / dis_timestamp_const;
	out->tv_sec = (long) ( in->hour * 3600 + i / MILLION );
	out->tv_usec = (long) ( i % MILLION );
*/
}


void
dis_processNewDRParameters(dis_entity_state_pdu * pdu, dis_dr_parameters * dr)
{
	switch (pdu->dr_parm.algorithm) {
	case DISDRMethodRPB:
	case DISDRMethodRVB:
	case DISDRMethodRPW:
	case DISDRMethodRVW:
		dis_generateDRParameters(pdu, dr);
		break;

	case DISDRMethodStatic:
	case DISDRMethodFPW:
	case DISDRMethodFVW:
	case DISDRMethodFPB:
	case DISDRMethodFVB:
		break;

	case DISDRMethodOther:
	default:
		break;
	}

	dr->pdu = *pdu;
	VEulerToMatrix(
		pdu->orientation.phi,
		pdu->orientation.theta,
		pdu->orientation.psi,
		&dr->R0);
}

void
dis_generateDRParameters(dis_entity_state_pdu * pdu, dis_dr_parameters * dr)
{
	double    ox, oy, oz;
	double    ax = 0.0, ay = 0.0, az = 0.0;
	double    omega;

	ox = pdu->dr_parm.angular_vel.x;
	oy = pdu->dr_parm.angular_vel.y;
	oz = pdu->dr_parm.angular_vel.z;
	omega = sqrt(ox * ox + oy * oy + oz * oz);

	if (omega > 0.0) {
		ax = ox / omega;
		ay = oy / omega;
		az = oz / omega;
	}

	dr->omega = omega;

	dr->skew.m[0][0] = dr->skew.m[1][1] = dr->skew.m[2][2] = 0.0;
	dr->skew.m[1][0] = -az;
	dr->skew.m[0][1] = az;
	dr->skew.m[2][0] = ay;
	dr->skew.m[0][2] = -ay;
	dr->skew.m[2][1] = -ax;
	dr->skew.m[1][2] = ax;

	dr->aat.m[0][0] = ax * ax;
	dr->aat.m[1][0] = ax * ay;
	dr->aat.m[2][0] = ax * az;
	dr->aat.m[0][1] = ay * ax;
	dr->aat.m[1][1] = ay * ay;
	dr->aat.m[2][1] = ay * az;
	dr->aat.m[0][2] = az * ax;
	dr->aat.m[1][2] = az * ay;
	dr->aat.m[2][2] = az * az;
}

static void DISComputeDRMatrix(dis_dr_parameters * dr, double dT, VMatrix * m)
{
	double    theta = dr->omega * dT;
	double    cosTheta = cos(theta);
	double    sinTheta = sin(theta);
	double    Icos, x = (1.0 - cosTheta);
	int       i, j;

	for (i = 0; i < 3; ++i) {
		for (j = 0; j < 3; ++j) {
			if (i == j) {
				Icos = cosTheta;
			}
			else {
				Icos = 0.0;
			}
			m->m[i][j] = Icos -
				dr->skew.m[i][j] * sinTheta +
				dr->aat.m[i][j] * x;
		}
	}

	m->m[0][3] = m->m[1][3] = m->m[2][3] = 0.0;
	m->m[3][0] = m->m[3][1] = m->m[3][2] = 0.0;
	m->m[3][3] = 1.0;
}

void
dis_computeDRPosition(dis_dr_parameters * dr,
					 double dT,
					 VPoint * pos,
					 dis_linear_vel_vector * vel,
					 VMatrix * orientation)
{
	VMatrix   DR;
	double    hDTSqr;
	dis_entity_state_pdu * pdu = &dr->pdu;

	/* Position */

	switch (dr->pdu.dr_parm.algorithm) {

	case DISDRMethodRPW:
		pos->x = pdu->pos.x + pdu->vel.x * dT;
		pos->y = pdu->pos.y + pdu->vel.y * dT;
		pos->z = pdu->pos.z + pdu->vel.z * dT;
		*vel = pdu->vel;
		DISComputeDRMatrix(dr, dT, &DR);
		VMatrixMultByRank(&DR, &dr->R0, orientation, 3);
		break;

	case DISDRMethodRVW:
		hDTSqr = 0.5 * dT * dT;
		pos->x = pdu->pos.x + pdu->vel.x * dT +
			pdu->dr_parm.linear_acc.x * hDTSqr;
		pos->y = pdu->pos.y + pdu->vel.y * dT +
			pdu->dr_parm.linear_acc.y * hDTSqr;
		pos->z = pdu->pos.z + pdu->vel.z * dT +
			pdu->dr_parm.linear_acc.z * hDTSqr;
		vel->x = (float) (pdu->vel.x + pdu->dr_parm.linear_acc.x * dT);
		vel->y = (float) (pdu->vel.y + pdu->dr_parm.linear_acc.y * dT);
		vel->z = (float) (pdu->vel.z + pdu->dr_parm.linear_acc.z * dT);

		DISComputeDRMatrix(dr, dT, &DR);
		VMatrixMultByRank(&DR, &dr->R0, orientation, 3);

		break;

	case DISDRMethodStatic:
		*pos = pdu->pos;
		*vel = pdu->vel;
		*orientation = dr->R0;
		break;

	case DISDRMethodFPW:
		pos->x = pdu->pos.x + pdu->vel.x * dT;
		pos->y = pdu->pos.y + pdu->vel.y * dT;
		pos->z = pdu->pos.z + pdu->vel.z * dT;
		*vel = pdu->vel;
		*orientation = dr->R0;
		break;

	case DISDRMethodFVW:
		hDTSqr = 0.5 * dT * dT;
		pos->x = pdu->pos.x + pdu->vel.x * dT +
			pdu->dr_parm.linear_acc.x * hDTSqr;
		pos->y = pdu->pos.y + pdu->vel.y * dT +
			pdu->dr_parm.linear_acc.y * hDTSqr;
		pos->z = pdu->pos.z + pdu->vel.z * dT +
			pdu->dr_parm.linear_acc.z * hDTSqr;
		vel->x = (float) (pdu->vel.x + pdu->dr_parm.linear_acc.x * dT);
		vel->y = (float) (pdu->vel.y + pdu->dr_parm.linear_acc.y * dT);
		vel->z = (float) (pdu->vel.z + pdu->dr_parm.linear_acc.z * dT);
		*orientation = dr->R0;
		break;

	/*
	 * For all the remaining unimplemented methods it seems safe to simply
	 * keep the current position, velocity and orientation:
	 */

	case DISDRMethodRPB:
	case DISDRMethodRVB:
		/* todo: position */
		/* todo: orientation */

	case DISDRMethodFPB:
	case DISDRMethodFVB:
		/* todo: position */

	case DISDRMethodOther:
		/* how on earth would we handle this? callbacks, perhaps? */
		
	default:
		/* Unknown method -- should we display a warning? */
		
		*pos = pdu->pos;
		*vel = pdu->vel;
		*orientation = dr->R0;
		
		break;
	}

}

void
dis_getDRThresholds(dis_dr_parameters * dr, double *time, double *location, double *orientation)
{
	*time = dr->timeThreshold;
	*location = dr->locationThreshold;
	*orientation = dr->orientationThreshold;
}

void
dis_setDRThresholds(dis_dr_parameters * dr, double time, double location, double orientation)
{
	dr->timeThreshold = time;
	dr->locationThreshold = location;
	dr->orientationThreshold = orientation;
}

dis_DR_FLAGS
dis_testDRThresholds(dis_dr_parameters *dr, double deltaT,
			VPoint *current_location,
			dis_euler_angles *current_orientation)
{
	int result = 0;
	VPoint dr_loc, d_loc;
	dis_linear_vel_vector dr_vel;
	double d_squared, d1, d2, d3, orientation_error_squared;
	VMatrix dr_orientation, cur_orientation;

	if (deltaT > dr->timeThreshold) {
		result |= dis_DR_TIME;
	}
	else {

		dis_computeDRPosition(dr,
					 deltaT,
					 &dr_loc,
					 &dr_vel,
					 &dr_orientation);

		d_loc.x = current_location->x - dr_loc.x;
		d_loc.y = current_location->y - dr_loc.y;
		d_loc.z = current_location->z - dr_loc.z;
		d_squared = d_loc.x * d_loc.x + d_loc.y * d_loc.y + d_loc.z * d_loc.z;

		if (d_squared > dr->locationThreshold * dr->locationThreshold) {
			result |= dis_DR_LOCATION;
		}

		VEulerToMatrix(
			current_orientation->phi,
			current_orientation->theta,
			current_orientation->psi,
			&cur_orientation);
		
		d1= dr_orientation.m[0][0] * cur_orientation.m[0][0] +
			dr_orientation.m[0][1] * cur_orientation.m[0][1] +
			dr_orientation.m[0][2] * cur_orientation.m[0][2]   ;
		d2= dr_orientation.m[1][0] * cur_orientation.m[1][0] +
			dr_orientation.m[1][1] * cur_orientation.m[1][1] +
			dr_orientation.m[1][2] * cur_orientation.m[1][2]   ;
		d3= dr_orientation.m[2][0] * cur_orientation.m[2][0] +
			dr_orientation.m[2][1] * cur_orientation.m[2][1] +
			dr_orientation.m[2][2] * cur_orientation.m[2][2]   ;

		d1 = 1.0 - d1 * d1;
		d2 = 1.0 - d2 * d2;
		d3 = 1.0 - d3 * d3;

		orientation_error_squared = d1 * d1 + d2 * d2 + d3 * d3;

		if (orientation_error_squared > 
			dr-> orientationThreshold * dr-> orientationThreshold) {
			result |= dis_DR_ORIENTATION;
		}

	}

	return result;
}

#define DMAX 16  /* maximum number of delimiter chars */
#define BMAX 64  /* maximum incoming string length */

int
dis_parseEntityID (dis_entity_id *p, 
				  char * buf, 
				  int bufsize,
				  char *delim)
{
	char pdelim[DMAX+1];
	char tbuf[BMAX+1];
	char *cur, *next, *endptr;
	long rval;
	int result = 1;

	memset ( p, 0, sizeof(dis_entity_id));

	/*
	 *  Buffer too large?
	 */

	if (bufsize > BMAX ) {
		return 2;
	}

	memory_strcpy(tbuf, sizeof(tbuf), buf);

	if (delim)
		memory_strcpy(pdelim, sizeof(pdelim), delim);
	else
		memory_strcpy(pdelim, sizeof(pdelim), ":./");

	cur = tbuf;

	next = strpbrk ( cur, pdelim );

	if ( next != NULL ) {

		/*
		 * Once we get a delimiter, all other delimeters must match
		 */

		pdelim[0] = *next;
		pdelim[1] = '\0';

		/*
		 *  Get Site ID
		 */

		endptr = next;
		rval = strtol ( cur, &endptr, 0 );

		if (rval < 0 || rval > 0xffff) {
			return 3;
		}
		else {
			p->sim_id.site_id = (unsigned short) rval;
		}

		/*
		 *  Ensure strtol stopped parsing at the correct spot
		 */

		if ( endptr != next ) {
			return 4;
		}

		cur = next+1;

		next = strpbrk ( cur, pdelim );

		if ( next != NULL ) {

			/*
			 *  Get application ID
			 */

			endptr = next;
			rval = strtol ( cur, &endptr, 0 );

			if (rval < 0 || rval > 0xffff) {
				return 3;
			}
			else {
				p->sim_id.application_id = (unsigned short) rval;
			}

			/*
			 *  Ensure strtol stopped parsing at the correct spot
			 */

			if ( endptr != next ) {
				return 4;
			}

			/*
			 *  Get Entity ID
			 */

			cur = next+1;

			rval = strtol ( cur, NULL, 0 );

			if (rval < 0 || rval > 0xffff) {
				return 3;
			}
			else {
				p->entity_id = (unsigned short) rval;
			}

			result = 0;

		}
	}

	return result;
}


int dis_parseEntityType(char *s, dis_entity_type *et)
{
	int f[7];
	char buf[6];
	
	if( s == NULL )
		return 0;
	// skip leading white spaces:
	while(isspace(*s))
		s++;
	int i = 0;
	do {
		// scan number:
		if( ! isdigit(*s) )
			return 0;
		char *start = s;
		do { s++; } while(isdigit(*s));
		if( s - start > 5 )
			return 0;
		memcpy(buf, start, s - start);
		buf[s - start] = 0;
		f[i] = atoi(buf);
		if( !((i == 2 && f[i] <= 65535) || (i != 2 && f[i] <= 255)) )
			return 0;
		// skip '.':
		i++;
		if( i == 7 )
			break;
		if( *s != '.' )
			return 0;
		s++;
	} while(1);
	// skip trailing white spaces:
	while(isspace(*s))
		s++;
	// check for trailing chars:
	if( *s != 0 )
		return 0;
	
	et->kind = f[0];
	et->domain = f[1];
	et->country = f[2];
	et->category = f[3];
	et->subcategory = f[4];
	et->specific = f[5];
	et->extra = f[6];
	return 1;
}


char * dis_entityTypeToString(dis_entity_type *et)
{
	static char s[99];
	
	sprintf(s, "%u.%u.%u.%u.%u.%u.%u",
		et->kind, et->domain, et->country, et->category,
		et->subcategory, et->specific, et->extra);
	return s;
}



#ifdef NOT_USED_________________________________
int
DISSetMulticastMode(dis_Transceiver * xcvr, unsigned long maddress, unsigned long ointerface)
{
	int       ttl = 8;
	unsigned long addr = ointerface;
	struct ip_mreq mreq;

	mreq.imr_multiaddr.s_addr = maddress;
	mreq.imr_interface.s_addr = ointerface;

	setsockopt(xcvr->s, IPPROTO_IP, IP_MULTICAST_TTL, (char *) &ttl, sizeof(ttl));
	setsockopt(xcvr->s, IPPROTO_IP, IP_MULTICAST_IF, (char *) &addr, sizeof(addr));
	setsockopt(xcvr->s, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &mreq, sizeof(mreq));

	return 0;
}
#endif
