/*
 *   DIS/x : An implementation of the IEEE 1278.1 protocol
 *
 *   Copyright (C) 1996, Riley Rainey (rainey@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.
 */

/**
 * DIS client test program with command line interface only, that creates a DIS
 * entity (currently a SA-13 SAM launcher) at the given position and periodically
 * sends the corresponding DIS state packets until interrupted (CTRL-C).
 * 
 * For more about the command line options available use the -h option.
 * 
 * @file
 */

#include <sys/time.h>
#include <unistd.h>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include "../../util/error.h"

/*
 *  GNU C library includes getopt_long()
 */

#ifdef __GNU_LIBRARY__
#include <getopt.h>
#endif

#include "../../V/Vlibmath.h"
#include "../dis/dis.h"
#include "../dis/disx.h"
#include "../../util/units.h"
#include "../../util/timer.h"

#define FEETtoMETERS(x) ((x) * 0.3048)

#ifdef __GNU_LIBRARY__

/*
 *  long command line options
 */

static struct option long_options[] =
{
	{ "help",        0, 0, 'h' },
	{ "relay",       1, 0, 'R' },
	{ "port",        1, 0, 'P' },
	{ "force-id",    1, 0, 'f' },
	{ "appl-id",     1, 0, 'a' },
	{ "site-id",     1, 0, 's' },
	{ "entity-id",   1, 0, 'e' },
	{ "exercise-id", 1, 0, 'E' },
	{ "latitude",    1, 0, 'l' },
	{ "longitude",   1, 0, 'L' },
	{ "altitude",    1, 0, 'A' },
	{ "type",        1, 0, 't' },
};

#endif


static void help()
{
	printf(
"Test DIS client application that generates a SA-13 SAM launcher entity and\n"
"keeps sending DIS state packets until interrupted.\n"
"\n"
"Options:\n"
"  -R host     Name of the relay host. Empty for broadcasting (default).\n"
"  -P port     UDP port for broadcasting or UDP port of the relay (0-65535,\n"
"              default 3000).\n"
"  -f force    Force: 0=other, 1=friendly (defaut), 2=opposing, 3=neutral.\n"
"  -a appl     Application ID (0-65535, default 1).\n"
"  -s site     Site ID (0-65535, default 1).\n"
"  -e entity   Entity ID (0-65535, default 1).\n"
"  -E exercise Exercise ID (0-65535, default 1).\n"
"  -l lat      Latitude ([-90.0,+90.0] DEG, default 0.0 DEG).\n"
"  -L lon      Longitude ([-180.0,+180.0] DEG, default 0.0 DEG).\n"
"  -A alt      Altitude (feet, default 0.0).\n"
"  -t type     entity type of the form \"9.9.9.9.9.9.9\" (default\n"
"              \"1.1.222.4.21.1\" the SA-13 SAM.\n"
"  -h          This help.\n"
"\n"
"Example:\n"
"\n"
"   $ ./decoy -R 192.168.1.33 -a 123 -l 32.87 -L -96.85 -A 2000\n"
"\n"
"generates a SA-13 SAM launcher floating at 2000 ft over the Love Field airport,\n"
"using the specified host as a relay.\n"
);
}

/**
 *  Generate a transform matrix to get from geocentric to local NED coordinates
 */
static void GenerateWorldToLocalMatrix(earth_LatLonAlt * w, VMatrix * m)
{
	VPoint gc;
	VPoint    p;

	VIdentMatrix(m);
	VRotate(m, ZRotation, -w->longitude);
	VRotate(m, YRotation, w->latitude);
	VRotate(m, YRotation, units_DEGtoRAD(90.0));
	earth_LatLonAltToXYZ(w, &gc);
	VTransform(&gc, m, &p);
	m->m[0][3] = -p.x;
	m->m[1][3] = -p.y;
	m->m[2][3] = -p.z;
}

/**
 *  Convert a double in UNIX format (seconds since 1970) to a DIS timestamp.
 *  If reference is 0, the time will be marked relative.
 *  If reference is 1, the time will be marked absolute, i.e. true UTC time.
 */
static dis_timestamp
TimeDoubleToDIS(double time, int reference)
{
	return (dis_timestamp) {0};  // NOT IMPLEMENTED - dis_if ignores DIS timestamps anyway
#ifdef NOT_IMPLEMENTED
	unsigned long tmp;
	dis_timestamp res;

	tmp = (unsigned long) (fmod(time, 3600.0));
	if (tmp > 2147483647L)		/* 2^31 - 1 */
		res.time = 2147483647L;
	else
		res.time = tmp;
	res.type = reference;

	return res;
#endif
}

static void
transpose(VMatrix * m, VMatrix * r)
{

	int       i, j;

	for (i = 0; i < 4; ++i)
		for (j = 0; j < 4; ++j)
			r->m[i][j] = m->m[j][i];
}

int main(int argc, char **argv)
{
	char *relay = NULL;
	int port = 3000;
	dis_Transceiver *xcvr;
	struct timeval curtime;
	int c;
	double unix_time_sec;
#ifdef __GNU_LIBRARY__
	int option_index = 0;
#endif

	VMatrix XYZtoNED, NEDtoXYZ, trihedral, ABCtoXYZ;
	double orientation[3];
	VPoint velocity = { 0, 0, 0 };

	dis_pdu estate, em;
	dis_entity_state_pdu *esPDU;
	disx_ApplicationInfo * app;
	earth_LatLonAlt pos;
	
	error_prog_name = argv[0];

	memset (&orientation, 0, sizeof(orientation));
	memset (&estate, 0, sizeof(dis_pdu));
	memset (&em,     0, sizeof(dis_pdu));
	memset (&pos,    0, sizeof(pos));

	/*
	 * Fill out PDUs
	 */

	esPDU = (dis_entity_state_pdu *) &estate;
	esPDU->id.sim_id.site_id = 1;
	esPDU->id.sim_id.application_id = 1;
	esPDU->id.entity_id = 1;
	esPDU->force_id = 1;
	esPDU->hdr.protocol_version = DISProtocolVersionIEEE1278_95;
	esPDU->hdr.exercise_id = 1;
	
	esPDU->hdr.pdu_type = PDUTypeEntityState;
	esPDU->hdr.protocol_family = 1; // must match the PDU type above
	
	esPDU->hdr.time_stamp = (dis_timestamp) {0}; // will set later
	esPDU->hdr.length = 0;
	esPDU->hdr.padding = 0;

	/*
	 *  SA-13 SAM launcher
	 */

	esPDU->type.kind = 1;
	esPDU->type.domain = 1;
	esPDU->type.country = 222;
	esPDU->type.category = 4;
	esPDU->type.subcategory = 21;
	esPDU->type.specific = 1;

	/*
	 * process command line arguments
	 */

	while (1) {

#ifdef __GNU_LIBRARY__
		c = getopt_long ( argc, argv, "hR:P:f:a:s:e:E:l:L:A:t:",
						  long_options, &option_index );
#else
		c = getopt ( argc, argv, "hR:P:f:a:s:e:E:l:L:A:t:" );
#endif

		if (c == -1) {
			break;
		}

		switch (c) {
		case 'h':
			help();
			return 0;
		case 'R':
			relay = strdup(optarg);
			break;
		case 'P':
			port = atoi(optarg);
			break;
		case 'f':
			estate.entity_state.force_id = atoi(optarg);
			break;
		case 'a':
			estate.entity_state.id.sim_id.application_id = atoi(optarg);
			break;
		case 's':
			estate.entity_state.id.sim_id.site_id = atoi(optarg);
			break;
		case 'e':
			estate.entity_state.id.entity_id = atoi(optarg);
			break;
		case 'E':
			esPDU->hdr.exercise_id = atoi(optarg);
			break;
		case 'l':
			pos.latitude = units_DEGtoRAD(atof(optarg));
			break;
		case 'L':
			pos.longitude = units_DEGtoRAD(atof(optarg));
			break;
		case 'A':
			pos.z = FEETtoMETERS(atof(optarg));
			break;
		case 't':
			if( ! dis_parseEntityType(optarg, &esPDU->type) ){
				fprintf(stderr, "invalid entity type specification: %s\n", optarg);
				return 1;
			}
			break;
		default:
			fprintf(stderr, "unknown option %c -- try -h for help\n", c);
			return 1;
		}
	}
	
	xcvr = dis_openTransceiver(0, relay, port);
	if( xcvr == NULL )
		error_external("failed opening socket");

	app = disx_initializeApplication(xcvr,
		1,
		estate.entity_state.id.sim_id.site_id,
		estate.entity_state.id.sim_id.application_id );
	if( app == NULL )
		error_internal("failed configuring disx", 0);

	/*
	 * loop until interrupted ...
	 */

	double psi = 0.0;
	while (1) {

		gettimeofday( &curtime, NULL );

		unix_time_sec = (double) curtime.tv_sec + 
			(double) curtime.tv_usec / 1000000.0;

		esPDU->hdr.time_stamp = TimeDoubleToDIS( unix_time_sec, 1 );

		/* esPDU->id already set */

		//esPDU->force_id = DISForceOpposing;

		esPDU->art_parm_count = 0;

		earth_LatLonAltToXYZ( &pos, &esPDU->pos );
		psi += units_DEGtoRAD(5);
		VEulerToMatrix(0.0, 0.0, psi, &trihedral);
		GenerateWorldToLocalMatrix ( &pos, &XYZtoNED );

		/*
		 *  Derive ECI [Geocentric] heading, pitch, roll
		 */
		transpose(&XYZtoNED, &NEDtoXYZ);
		/* the trihedral is an "ABCtoNED" transformation */
		VMatrixMultByRank(&trihedral, &NEDtoXYZ, &ABCtoXYZ, 3);
		VMatrixToEuler(&ABCtoXYZ,
					  &orientation[0], 
					  &orientation[1], 
					  &orientation[2]);

		esPDU->orientation.phi   = orientation[0];
		esPDU->orientation.theta = orientation[1];
		esPDU->orientation.psi   = orientation[2];

		esPDU->vel.x = velocity.x;
		esPDU->vel.y = velocity.y;
		esPDU->vel.z = velocity.z;

		esPDU->appearance = ( DISAppearancePaintUniform |
							  DISAppearanceFirepowerNormal |
							  DISAppearanceHatchClosed |
							  DISAppearanceLandLauncherRaised );

		esPDU->dr_parm.algorithm = DISDRMethodRVW;

		esPDU->dr_parm.linear_acc.x = 0.0f;
		esPDU->dr_parm.linear_acc.y = 0.0f;
		esPDU->dr_parm.linear_acc.z = 0.0f;

		esPDU->dr_parm.angular_vel.x = 0.0f;
		esPDU->dr_parm.angular_vel.y = 0.0f;
		esPDU->dr_parm.angular_vel.z = 0.0f;

		esPDU->marking.charset = DISCharSetASCII;
		memset(esPDU->marking.marking, 0, sizeof(esPDU->marking.marking));
		strncpy((char *) esPDU->marking.marking, "HI THERE", sizeof(esPDU->marking.marking));
		esPDU->capabilities = 0;
		esPDU->art_parm = NULL;

		if ( ! disx_writePDU(app, (dis_pdu *) esPDU) ) {
			printf ( "error writing PDU\n" );
		}
		else {
			printf ( "." );
			fflush ( stdout );
		}

		timer_sleepMilliseconds(2000);
	}
}
