/************************************************************************\
|*	Copyright © 1997 Artificial Horizons, Inc..                    	*|
|*	Printed in the United States of America.                       	*|
|*	44 Montgomery. Suite 1480, San Francisco, CA, 94104 U.S.A.     	*|
|*	All rights reserved.                                           	*|
|*	                                                               	*|
|*	This product or document is protected by copyright and         	*|
|*	distributed under licenses restricting its use,                	*|
|*	copyright and distribution under licenses restricting          	*|
|*	its use, copying, distribution and decompilation.              	*|
|*	                                                               	*|
|*	No part of this product or document may be reproduced in       	*|
|*	any form by any means without prior written authorization of   	*|
|*	Artificial Horizons, Inc. and its licensors.                   	*|
|*	                                                               	*|
|*	Artificial Horizons, Aviator, AVDS, FlightCam, ChaseCam,       	*|
|*	and TowerCam are trademarks or registered trademarks of        	*|
|*	of Artificial Horizons, Inc.                                   	*|
\************************************************************************/
/***************************************************************************\
|*                                                                           *|
|*                            Produced under:                                *|
|*        Cooperative Reasearch and Development Agreement 94-173-EL-02       *|
|*                               between                                     *|
|*                        Artificial Horizons®, Inc.                         *|
|*                                 and                                       *|
|*                    Wright Laboratory, WL/FIGS, USAF                       *|
|*                                                                           *|
|*                                  by                                       *|
|*           Robert R. Smith Jr. and Capt Steven J. Rasmussen                *|
|*                                1994-95                                    *|
|*                                                                           *|
\***************************************************************************/
/********************************************************************\
|*	Portons Copyright © 1998 RasSimTech Ltd.                       	*|
|*	Printed in the United States of America.                       	*|
|*	All rights reserved.                                           	*|
|*	                                                               	*|
|*	This product or document is protected by copyright and         	*|
|*	distributed under licenses restricting its use,                	*|
|*	copyrightand distribution under licenses restricting           	*|
|*	its use, copying, distribution and decompilation.              	*|
|*	                                                               	*|
|*	No part of this product or document may be reproduced in       	*|
|*	any form by any means without prior written authorization of   	*|
|*	RasSimTech Ltd.							                     	*|
|*	                                                               	*|
\********************************************************************/

#include "windows.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <UserNetwork.h>

#define PRINT_TIME_MS 1000
#define RUN_TIME_MS 20
#define SEND_TIME_MS 80
#define TOTAL_TIME_SEC 100.0  /*total run time inseconds*/

Aircraft b707,f18,f16,f15,f22,f23,f117;
float deg_to_rad = 3.14159/180.0;
int	new_frame = 0;
void promo_send();
void promo_run();
float turn_rate,airspeed,altitude;
float spread;

void
main(argc, argv)
int argc;
char *argv[];
{

	DWORD dwCurrentTimeMs,dwLastTimeMs;/*MILISECONDS*/
	float dwTotalTimeSec;
	DWORD dwElapsedTimeMs,dwRunTimeMs,dwSendTimeMs,dwPrintTimeMs;

	float fLon,fLat,fAlt;
	int iCount = 1;
	char* strAddress[1];
	strAddress[0] = strdup("224.0.5.20:2267");/* this is the default AVDS multicast address*/
	OpenAVDSNetwork(iCount,strAddress);

	fLon=-122.366;fLat=37.613;fAlt=15000.0;
	Init_Aircraft(&b707,"707_Drone",1,AIRCRAFT_TYPE_00,&fLon,&fLat,&fAlt);

	b707.rot_X = 0.0;
	b707.rot_Y = 8.0;
	b707.rot_Z = 0.0;
	Init_Aircraft(&f18,"F18_Drone",2,AIRCRAFT_TYPE_01,&fLon,&fLat,&fAlt);
	f18.rot_X = 0.0;
	f18.rot_Y = 8.0;
	f18.rot_Z = 0.0;
	Init_Aircraft(&f16,"F16_Drone",3,AIRCRAFT_TYPE_02,&fLon,&fLat,&fAlt);
	f16.rot_X = 0.0;
	f16.rot_Y = 8.0;
	f16.rot_Z = 0.0;
	Init_Aircraft(&f15,"F15_Drone",4,AIRCRAFT_TYPE_03,&fLon,&fLat,&fAlt);
	f15.rot_X = 0.0;
	f15.rot_Y = 8.0;
	f15.rot_Z = 0.0;
	Init_Aircraft(&f22,"F22_Drone",5,AIRCRAFT_TYPE_04,&fLon,&fLat,&fAlt);
	f22.rot_X = 0.0;
	f22.rot_Y = 8.0;
	f22.rot_Z = 0.0;
	Init_Aircraft(&f23,"F23_Drone",6,AIRCRAFT_TYPE_05,&fLon,&fLat,&fAlt);
	f23.rot_X = 0.0;
	f23.rot_Y = 8.0;
	f23.rot_Z = 0.0;
	Init_Aircraft(&f117,"F117_Drone",7,AIRCRAFT_TYPE_06,&fLon,&fLat,&fAlt);

	f117.rot_X = 0.0;
	f117.rot_Y = 8.0;
	f117.rot_Z = 0.0;
	airspeed = 500.0;
	turn_rate = 90.0/60.0;
	altitude = 15000;
	spread = 100.0;

	b707.packet.hostid = 1234;
	f18.packet.hostid = 7125;
	f16.packet.hostid = 5423;
	f15.packet.hostid = 9821;	
	f22.packet.hostid = 6243;	
	f23.packet.hostid = 5678;	
	f117.packet.hostid = 3498;	

	b707.packet.craftid = 1234;
	f18.packet.craftid = 7125;
	f16.packet.craftid = 5423;
	f15.packet.craftid = 9821;	
	f22.packet.craftid = 6243;	
	f23.packet.craftid = 5678;	
	f117.packet.craftid = 3498;	

	b707.engine = 100;
	f18.engine = 100;
	f16.engine = 100;
	f15.engine = 100;
	f22.engine = 100;
	f23.engine = 100;
	f117.engine = 100;

	dwLastTimeMs = GetTickCount();
	dwRunTimeMs = 0;
	dwSendTimeMs=0;

	dwTotalTimeSec = 0;

	while (TRUE){
		dwCurrentTimeMs = GetTickCount();
		dwElapsedTimeMs = dwCurrentTimeMs-dwLastTimeMs;
		dwTotalTimeSec+=(float)dwElapsedTimeMs/1000.0;
		dwRunTimeMs+=dwElapsedTimeMs;
		dwSendTimeMs+=dwElapsedTimeMs;
		if (dwRunTimeMs>RUN_TIME_MS){
				promo_run( (float)dwElapsedTimeMs/1000.0 );
				dwRunTimeMs = 0;
		}
		if (dwSendTimeMs>SEND_TIME_MS){
			promo_send();
			dwSendTimeMs=0;
		}
		if (dwTotalTimeSec>TOTAL_TIME_SEC){
			break;
		}
		dwLastTimeMs = dwCurrentTimeMs;
		dwPrintTimeMs+=dwElapsedTimeMs;
		if (dwPrintTimeMs>PRINT_TIME_MS){
			fprintf(stderr,"Total Time = %f \n",dwTotalTimeSec);
			dwPrintTimeMs=0;
		}
	}
	CloseAVDSNetwork();
	fprintf(stderr,"Finished:Total Time = %f \n",dwTotalTimeSec);
}

void
promo_run( dt )
float dt;
{
	float u,ydeg,xscale;
	double sphi,cphi,spsi,cpsi;
	float dN,dE,dh;

	/* control stick */
	b707.rot_Z += turn_rate*dt;
	u = airspeed;

	sphi = sin(b707.rot_Y*deg_to_rad);
	cphi = cos(b707.rot_Y*deg_to_rad);
	spsi = sin(b707.rot_Z*deg_to_rad);
	cpsi = cos(b707.rot_Z*deg_to_rad);

/* theta = 0, ctheta = 1.0, stheta = 0.0 */

	ydeg = b707.pos_Y/364605.0;
	xscale = (1.0-fabs((b707.ref_Y + ydeg)/90.0-1.0));
	xscale = 1.0 ; /* wasn't working CWC */


/* main */
	dN = 	u*cpsi;
	dE =	u*spsi;

	b707.dt += dt;
	b707.pos_X += dE * dt * xscale ;
	b707.pos_Y += dN * dt;
	b707.pos_Z = altitude;
	if ( new_frame == 1 ){
		Put_Aircraft(&b707);
		b707.dt = 0.0;
	}

/* right */
	dN = 	.5*spread*cpsi - spread*cphi*spsi;
	dE =	.5*spread*spsi + spread*cphi*cpsi;
	dh	=	-spread*sphi;
	f18.dt += dt;
	f18.pos_X = b707.pos_X + dE;
	f18.pos_Y = b707.pos_Y + dN;
	f18.pos_Z = b707.pos_Z + dh;
	f18.rot_Z += turn_rate*dt;
	if ( new_frame == 1 ){
		Put_Aircraft(&f18);
		f18.dt = 0.0;
		f18.dt = 0.0;
	}

/* left */
	dN = 	.5*spread*cpsi + spread*cphi*spsi;
	dE =	.5*spread*spsi - spread*cphi*cpsi;
	dh	=	spread*sphi;
	f16.dt += dt;
	f16.pos_X = b707.pos_X + dE;
	f16.pos_Y = b707.pos_Y + dN;
	f16.pos_Z = b707.pos_Z + dh;
	f16.rot_Z += turn_rate*dt;
	if ( new_frame == 1 ){
		Put_Aircraft(&f16);
		f16.dt = 0.0;
	}

/* left slot */
	dN = 	-spread*cpsi + spread*cphi*spsi;
	dE =	-spread*spsi - spread*cphi*cpsi;
	dh	=	spread*sphi;
	f15.dt += dt;
	f15.pos_X = b707.pos_X + dE;
	f15.pos_Y = b707.pos_Y + dN;
	f15.pos_Z = b707.pos_Z + dh;
	f15.rot_Z += turn_rate*dt;
	if ( new_frame == 1 ){
		Put_Aircraft(&f15);
		f15.dt = 0.0;
	}

/* right rear slot */
	dN = 	-1.5*spread*cpsi - .3*spread*cphi*spsi;
	dE =	-1.5*spread*spsi + .3*spread*cphi*cpsi;
	dh	=	-.5*spread*sphi;
	f22.dt += dt;
	f22.pos_X = b707.pos_X + dE;
	f22.pos_Y = b707.pos_Y + dN;
	f22.pos_Z = b707.pos_Z + dh;
	f22.rot_Z += turn_rate*dt;
	if ( new_frame == 1 ){
		Put_Aircraft(&f22);
		f22.dt = 0.0;
	}

/* left rear slot */
	dN = 	-1.5*spread*cpsi + .3*spread*cphi*spsi;
	dE =	-1.5*spread*spsi - .3*spread*cphi*cpsi;
	dh	=	.5*spread*sphi;
	f23.dt += dt;
	f23.pos_X = b707.pos_X + dE;
	f23.pos_Y = b707.pos_Y + dN;
	f23.pos_Z = b707.pos_Z + dh;
	f23.rot_Z += turn_rate*dt;
	if ( new_frame == 1 ){
		Put_Aircraft(&f23);
		f23.dt = 0.0;
	}

/* right slot */
	dN = 	-spread*cpsi - spread*cphi*spsi;
	dE =	-spread*spsi + spread*cphi*cpsi;
	dh	=	-spread*sphi;
	f117.dt += dt;
	f117.pos_X = b707.pos_X + dE;
	f117.pos_Y = b707.pos_Y + dN;
	f117.pos_Z = b707.pos_Z + dh;
	f117.rot_Z += turn_rate*dt;
	if ( new_frame == 1 ){
		Put_Aircraft( &f117);
		f117.dt = 0.0;
	}

	if ( new_frame == 1 ) {
		new_frame = 0;
	}
}

void
promo_send(dt)
float dt;
{
	new_frame = 1;
}



