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

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

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

#include <Vlibmath.h>
#include <dis/dis.h>

void MatrixToEuler(VMatrix * mt, double *heading, double *pitch, double *roll);
void BuildEulerMatrix(double roll, double pitch, double heading, VMatrix * m);
void GenerateWorldToLocalMatrix(WorldCoordinates * w, VMatrix * m);
dis_timestamp TimeDoubleToDIS(double time, int reference);
void transpose(VMatrix * m, VMatrix * r);

static struct itimerval it, ot;

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

void
timer_alarm (int val)
{
}

#define OPT_SITE      1
#define OPT_APPL      2
#define OPT_ENTITY    3
#define OPT_LATITUDE  4
#define OPT_LONGITUDE 5
#define OPT_ALTITUDE  6
#define OPT_ETYPE     7

#ifdef __GNU_LIBRARY__

/*
 *  long command line options
 */

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

#endif

int
main(int argc, char **argv)
{

	struct sigaction sa, old_sa;
	struct timeval curtime;
	int status;
	pid_t pid;
	int c;
	int exercise_id = 1;
	double unix_time_sec;
	int option_index = 0;


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

	dis_pdu estate, em;
	dis_entity_state_pdu *esPDU;
	DISxApplicationInfo * app;
	WorldCoordinates pos;

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

	/*
	 * 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;
	esPDU->hdr.exercise_id = 1;
	esPDU->hdr.pdu_type = PDUTypeEntityState;
	esPDU->hdr.protocol_family;
	esPDU->hdr.time_stamp;
	esPDU->hdr.length;
	esPDU->hdr.padding;

	/*
	 *  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, "f:a:s:bc:d:012",
						  long_options, &option_index );
#else
		c = getopt ( argc, argv, "f:a:s:bc:d:" );
#endif

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

		switch (c) {
		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':
			exercise_id = atoi(optarg);
			break;
		case 'l':
			pos.latitude = DEGtoRAD(atof(optarg));
			break;
		case 'L':
			pos.longitude = DEGtoRAD(atof(optarg));
			break;
		case 'A':
			pos.z = FEETtoMETERS(atof(optarg));
			break;
		case 't':
			break;
		}
	}

	memset ( &sa, 0, sizeof(sa) );
	sa.sa_handler = timer_alarm;
#ifdef linux
	sa.sa_flags = SA_NOMASK;
#endif

	it.it_interval.tv_sec = 3;
	it.it_interval.tv_usec = 0;
	it.it_value.tv_sec = 3;
	it.it_value.tv_usec = 0;

	if (sigaction( SIGALRM, &sa, &old_sa )) {
		perror( "sigaction failed" );
		exit ( 1 );
	}

    if (setitimer( ITIMER_REAL, &it, &ot )) {
		perror ( "setitimer failed" );
		exit ( 1 );
	}

	app = DISxInitializeApplication( 1,
						  estate.entity_state.id.sim_id.site_id,
						  estate.entity_state.id.sim_id.application_id );

	/*
	 * loop until interrupted ...
	 */

	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;

		DISWorldCoordinatesToGeocentric( &pos, &esPDU->pos );
		BuildEulerMatrix (0.0, 0.0, 0.0, &trihedral);
		GenerateWorldToLocalMatrix ( &pos, &XYZtoNED );

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

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

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

		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));
		strcpy(esPDU->marking.marking, "HI THERE");
		esPDU->capabilities = 0;
		esPDU->art_parm = NULL;

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

		pause();
	}
}

/*
 *  Generate a transform matrix to get from geocentric to local NED coordinates
 */

void
GenerateWorldToLocalMatrix(WorldCoordinates * w, VMatrix * m)
{
	dis_world_coordinates gc;
	VPoint    p;

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

void
BuildEulerMatrix(double roll, double pitch, double heading, VMatrix * m)
{

	register double sinPhi, cosPhi, sinTheta, cosTheta, sinPsi, cosPsi;

	sinPhi = sin(roll);
	cosPhi = cos(roll);
	sinTheta = sin(pitch);
	cosTheta = cos(pitch);
	sinPsi = sin(heading);
	cosPsi = cos(heading);

	m->m[0][0] = cosTheta * cosPsi;
	m->m[0][1] = sinPhi * sinTheta * cosPsi - cosPhi * sinPsi;
	m->m[0][2] = cosPhi * sinTheta * cosPsi + sinPhi * sinPsi;
	m->m[1][0] = cosTheta * sinPsi;
	m->m[1][1] = sinPhi * sinTheta * sinPsi + cosPhi * cosPsi;
	m->m[1][2] = cosPhi * sinTheta * sinPsi - sinPhi * cosPsi;
	m->m[2][0] = -sinTheta;
	m->m[2][1] = sinPhi * cosTheta;
	m->m[2][2] = cosPhi * cosTheta;
	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;

}

/*
 *  Convert a transformation matrix into the equivalent
 *  heading, pitch and roll angles.
 */

#define EPSILON 1.0e-6

void
MatrixToEuler(VMatrix * mt, double *heading, double *pitch, double *roll)
{
	double    sin_theta;

	sin_theta = -mt->m[2][0];

	if (fabs(sin_theta) > 1.0 - EPSILON) {
		/* we have the nose pointing very close to straight up or straight down,
		   set roll to zero and compute the resulting heading */

		*heading = atan2(-mt->m[0][1], mt->m[1][1]);
		if (*heading < 0.0)
			*heading += 2.0 * M_PI;

		if (sin_theta > 0.0)
			*pitch = M_PI / 2.0;
		else
			*pitch = -M_PI / 2.0;

		*roll = 0.0;
	}
	else {
		*heading = atan2(mt->m[1][0], mt->m[0][0]);
		if (*heading < 0.0)
			*heading += 2.0 * M_PI;

		*pitch = asin(sin_theta);

		*roll = atan2(mt->m[2][1], mt->m[2][2]);
	}
}

/*
 *  T i m e D o u b l e T o D I S
 *
 *  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.
 */

dis_timestamp
TimeDoubleToDIS(double time, int reference)
{
	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;
}

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];
}