/*
 *  Copyright (C) 1995  Mats Lofkvist  CelsiusTech Electronics AB
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 dated June, 1991.
 *
 *  This program 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program;  if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave., Cambridge, MA 02139, USA.
 */

/* FIXME: what this test program should do? */

#include <stdio.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <math.h>
#include <sys/time.h>
#include "../../src/acm/dis_if.h"
#include "../../src/acm/pm.h"
#include "../../src/util/units.h"

#define REALTIME
#define ABSOLUTETIME

#define DT 0.05
#define RSKIP 10

#define MAX_ENTITY 256

#ifndef DEVICE
#define DEVICE "le0"
#endif

static double
sysTime(void)
{
	struct timeval tv;

	gettimeofday(&tv, NULL);

	return tv.tv_sec + tv.tv_usec / 1000000.0;
}

#ifdef SOL2

#include <sys/types.h>
#include <unistd.h>
#include <string.h>

void
usleep(unsigned useconds)
{
	struct timeval tv;
	fd_set    myset;
	int       seconds;

	seconds = 0;
	while (useconds >= 1000000) {
		seconds++;
		useconds -= 1000000;
	}

	tv.tv_sec = seconds;
	tv.tv_usec = useconds;

	FD_ZERO(&myset);

	select(1, &myset, &myset, &myset, &tv);
}

#endif

static double simtime = 0.0;

typedef struct {
	dis_entity_type etype;
	int       force;
	int       remote;
} Entity;

static Entity inUse[MAX_ENTITY];

static void
entityEnterCb(int eid, dis_entity_type * etype, int force, craft ** cptr)
{
	printf("  %5.2f: enter %d <>\n", fmod(simtime, 60.0), eid);

	if (inUse[eid].force != -1)
		fprintf(stderr, "duplicate eid\n");

	if (eid >= MAX_ENTITY)
		fprintf(stderr, "too large eid\n");

	inUse[eid].etype = *etype;
	inUse[eid].force = force;
	inUse[eid].remote = 1;
}

/*
static void
entityExitCb(int eid)
{
	printf("  %4.1f: exit %d <>\n", fmod(simtime, 60.0), eid);

	inUse[eid].force = -1;
}
*/

/*
static void
fireCb(int ftype, int firingEid, int targetEid,
	   double time, int rounds,
	   double location[3], double velocity[3],
	   double range)
{
}
*/

static void
detonationCb(int ftype, craft *firing, craft *target,
			 double time, double worldLocation[3],
			 double entityLocation[3], 
		craft * munition,
		dis_detonation_pdu *dpdu)
{
	printf("  %4.1f: detonation %d @ %.0f %.0f %.0f (%d from %d)\n",
		   fmod(simtime, 60.0),
		 target->disId, worldLocation[0], worldLocation[1], worldLocation[2],
		   ftype, firing->disId);
}

static void
cleanup(int sign)
{
	dis_if_close();

	exit(0);
}

int
main(int argc, char *argv[])
{
	int       sendresult, eid, loop, teid, err;
	double    position[3] =
	{0, 0, 0};
	double    velocity[3] =
	{100, 0, 0};
	double    acceleration[3] =
	{0, 1, 0};
	double    attitude[3] =
	{0, 0, units_DEGtoRAD(90.0)};

	/* rotation = angular velocity */
	double    rotation[3] =
	{units_DEGtoRAD(60.0), units_DEGtoRAD(10.0), units_DEGtoRAD(-13.0)};
	double    roteul[3];
	VMatrix   curmat, rotmat;
	double    rpos[3], rvel[3], ratt[3];
	double    lasttime, dt, halfdtsq;
	double    wloc[3], eloc[3];
	dis_entity_type f16   = {1, 2, 225, 1, 3, 3, 0};
	dis_entity_type MiG29 = {1, 2, 222, 1, 2, 5, 0};
	int       team_one = 1;

	VEulerToMatrix(attitude[0], attitude[1], attitude[2], &curmat);

	for (eid = 0; eid < MAX_ENTITY; eid++)
		inUse[eid].force = -1;

	//if (dis_if_init(DEVICE, 3000, 1, 1, argc, entityEnterCb, entityExitCb,
	//			 fireCb, detonationCb) != 0) {
	if (dis_if_init(NULL, 0, 1, 1, 1, entityEnterCb, detonationCb, NULL) != 0) {
		fprintf(stderr, "dis_init failed\n");
		exit(17);
	}

	signal(SIGINT, cleanup);

	dis_if_setDRThresholds(5.0, units_DEGtoRAD(0.5));

#ifdef REALTIME
	simtime = sysTime();
#else
	simtime = 0.0;
#endif
#ifdef ABSOLUTETIME
	dis_if_setTimeAbsolute();
#else
	dis_setTime(simtime);
#endif

	if (argc > 1) {

		dis_if_entityEnter(team_one, NULL, &f16, &MiG29,
							position, velocity, acceleration,
							attitude, rotation, &eid);
		if (eid >= MAX_ENTITY) {
			fprintf(stderr, "to big eid\n");
			exit(17);
		}
		inUse[eid].etype = f16;
		inUse[eid].force = team_one;
		inUse[eid].remote = 0;
	}

	loop = 0;
	while (1) {
		loop++;
#ifdef WINNT
		// usleep() is deprecated by POSIX, tells MinGW. Workaround:
		Sleep(1000000 * DT);
#else
		// Under Linux, it's simpler to continue using usleep :-)
		usleep(1000000 * DT);
#endif
		
		lasttime = simtime;
#ifdef REALTIME
		simtime = sysTime();
#else
		simtime += DT;
#endif
		dt = simtime - lasttime;
#ifdef ABSOLUTETIME
		dis_if_setTimeAbsolute();
#else
		dis_setTime(simtime);
#endif
		err = dis_if_receive();
		if (err != 0)
			printf("receive err = %d\n", err);
		for (eid = 0; eid < MAX_ENTITY; eid++)
			if (inUse[eid].force != -1) {
				if (inUse[eid].remote == 1) {
					/* remote */
					if (loop % RSKIP == 0) {
						if (dis_if_getEntityState(eid, rpos, rvel, ratt) != 0) {
							printf("bar! %d\n", eid);
							exit(17);
						}
						printf("R %5.2f: %d @ %.0f %.0f %.0f [ %.2f %.2f %.2f ]\n",
							   fmod(simtime, 60.0), eid, rpos[0], rpos[1], rpos[2],
							   units_RADtoDEG(ratt[0]), units_RADtoDEG(ratt[1]), units_RADtoDEG(ratt[2]));
					}
				}				/* if remote == 1 */
				else {
					/* local */
					halfdtsq = dt * dt / 2;
					position[0] += dt * velocity[0] + halfdtsq * acceleration[0];
					position[1] += dt * velocity[1] + halfdtsq * acceleration[1];
					position[2] += dt * velocity[2] + halfdtsq * acceleration[2];
					velocity[0] += dt * acceleration[0];
					velocity[1] += dt * acceleration[1];
					velocity[2] += dt * acceleration[2];

					roteul[0] = rotation[2];
					roteul[1] = rotation[1];
					roteul[2] = rotation[0];
					VEulerToMatrix(dt*roteul[0], dt*roteul[1], dt*roteul[2], &rotmat);
					VMatrixMult(&curmat, &rotmat, &curmat);
					VMatrixToEuler(&curmat, &attitude[0], &attitude[1], &attitude[2]);

					sendresult = dis_if_setEntityState(eid, position, velocity,
									   acceleration, attitude, rotation);
					if (sendresult == 1)
						printf("T %5.2f: %d @ %5.0f %5.0f %5.0f"
							   "     [ %8.2f %8.2f %8.2f ]\n",
							   fmod(simtime, 60.0),
							   eid, position[0], position[1], position[2],
							   units_RADtoDEG(attitude[2]), units_RADtoDEG(attitude[1]), units_RADtoDEG(attitude[0]));
					else if (sendresult < 0) {
						fprintf(stderr, "  %5.2f foo? %d\n",
								fmod(simtime, 60.0), eid);
						exit(17);
					}

					if ((loop % 100) == 0) {
						for (teid = 0; teid < MAX_ENTITY; teid++)
							if (inUse[teid].force != -1 && inUse[teid].remote == 1)
								break;
						if (teid < MAX_ENTITY) {
							/* found a target */
							wloc[0] = 1000;
							wloc[1] = 2000;
							wloc[2] = 3000;
							eloc[0] = 4;
							eloc[1] = 5;
							eloc[2] = 6;
							if ((loop % 200) == 0) {
								printf("  %5.2f %d detonating M61A1 at %d ",
									   fmod(simtime, 60.0), eid, teid);
/*
								err = dis_if_detonation(dis_if_FIRE_M61A1, eid, teid,
													 0, 0, wloc, eloc);
*/
								err = dis_if_detonation(
									&f16,
									eid, teid,
									dis_if_FIRE_M61A1,
									wloc, eloc, NULL);
								printf("%d\n", err);
							}
							else {
								printf("  %5.2f %d detonating AIM9M at %d ",
									   fmod(simtime, 60.0), eid, teid);
								err = dis_if_detonation(&f16, dis_if_FIRE_AIM9M, eid, teid,
													wloc, eloc, NULL);
								printf("%d\n", err);
							}
						}		/* if found target */
					}			/* if loop % 100 == 0 */

					/* 1000 = 50s with DT = 0.05 */
					if (loop == 1000) {
						printf("  %5.2f exiting %d\n", fmod(simtime, 60.0), eid);
						dis_if_entityExit(eid);
						inUse[eid].force = -1;
					}
				}				/* if remote != 1 */
			}					/* if etype != -1 */
	}							/* while 1 */

	return 0;
}
