// SendMany.cpp : implementation file
//

#include "stdafx.h"
#include "math.h"
#include "AVDSNetworkTest.h"
#include "SendMany.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#define TIME_PROMO_SEND_HZ 20	// send packets at the rate of 20 times a second
#define TIME_UPDATE_DYNAMICS_HZ 50	// update dynamics at the rate of 50 times a second

#define TIMER_PROMO_SEND 1
#define TIMER_UPDATE_DYNAMICS 2

#define DEG_TO_RAD 1.745329e-2

/////////////////////////////////////////////////////////////////////////////
// CSendMany property page

IMPLEMENT_DYNCREATE(CSendMany, CPropertyPage)

CSendMany::CSendMany() : CPropertyPage(CSendMany::IDD)
{
	//{{AFX_DATA_INIT(CSendMany)
	//}}AFX_DATA_INIT

	fAirspeed = 500.0;
	fTurnRate = 90.0/60.0;
	fAltitude = 15000;
	fSpread = 100.0;

	m_uiTimerIDDynamics = -1;
	m_uiTimerIDSend = -1;
	m_uiTimerIDElapsedDynamics = -1;

}

CSendMany::~CSendMany()
{
}

void CSendMany::DoDataExchange(CDataExchange* pDX)
{
	CPropertyPage::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CSendMany)
	DDX_Control(pDX, IDC_CHECK_SEND_MANY_STOP, m_ctrlButtonStop);
	DDX_Control(pDX, IDC_CHECK_SEND_MANY_START, m_ctrlButtonStart);
	DDX_Control(pDX, IDC_SLIDER_TURN_RATE, m_SlideTurnRate);
	DDX_Control(pDX, IDC_SLIDER_SPREAD, m_SlideSpread);
	DDX_Control(pDX, IDC_SLIDER_AIRSPEED, m_SlideAirspeed);
	DDX_Control(pDX, IDC_SLIDER_ALTITUDE, m_SlideAltitude);
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(CSendMany, CPropertyPage)
	//{{AFX_MSG_MAP(CSendMany)
	ON_WM_TIMER()
	ON_WM_HSCROLL()
	ON_BN_CLICKED(IDC_CHECK_SEND_MANY_START, OnCheckSendManyStart)
	ON_BN_CLICKED(IDC_CHECK_SEND_MANY_STOP, OnCheckSendManyStop)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CSendMany message handlers
void CSendMany::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pScrollBar) 
{
	CSliderCtrl* pSlide = (CSliderCtrl*) pScrollBar;
	CString strTextTmp;

	switch(pScrollBar->GetDlgCtrlID())
	{
	case IDC_SLIDER_AIRSPEED:
		fAirspeed = (float)m_SlideAirspeed.GetPos();
		strTextTmp.Format("%d",(int)m_SlideAirspeed.GetPos());
		SetDlgItemText(IDC_STATIC_AIRSPEED,strTextTmp);
		break;	
	case IDC_SLIDER_ALTITUDE:
		fAltitude = (float)m_SlideAltitude.GetPos();
		strTextTmp.Format("%d",(int)m_SlideAltitude.GetPos());
		SetDlgItemText(IDC_STATIC_ALTITUDE,strTextTmp);
		break;	
	case IDC_SLIDER_TURN_RATE:
		fTurnRate = (float)m_SlideTurnRate.GetPos();
		ChangeTurnRate(fTurnRate);
		strTextTmp.Format("%d",(int)m_SlideTurnRate.GetPos());
		SetDlgItemText(IDC_STATIC_TURN_RATE,strTextTmp);
		break;	
	case IDC_SLIDER_SPREAD:
		fSpread = (float)m_SlideSpread.GetPos();
		strTextTmp.Format("%d",(int)m_SlideSpread.GetPos());
		SetDlgItemText(IDC_STATIC_SPREAD,strTextTmp);
		break;	
	}
	
	CPropertyPage::OnHScroll(nSBCode, nPos, pScrollBar);
}

BOOL CSendMany::OnInitDialog() 
{
	CPropertyPage::OnInitDialog();
	

	CString strTextTmp;

	m_SlideAirspeed.SetRange(100,1000,TRUE);
	m_SlideAirspeed.SetPos((int)fAirspeed);
	strTextTmp.Format("%d", (int)fAirspeed);
	SetDlgItemText(IDC_STATIC_AIRSPEED,strTextTmp);

	m_SlideTurnRate.SetRange(1,100,TRUE);
	m_SlideTurnRate.SetPos((int)fTurnRate);
	strTextTmp.Format("%d", (int)fTurnRate);
	SetDlgItemText(IDC_STATIC_TURN_RATE,strTextTmp);


	m_SlideAltitude.SetRange(100,30000,TRUE);
	m_SlideAltitude.SetPos((int)fAltitude);
	strTextTmp.Format("%d", (int)fAltitude);
	SetDlgItemText(IDC_STATIC_ALTITUDE,strTextTmp);

	m_SlideSpread.SetRange(50,1000,TRUE);
	m_SlideSpread.SetPos((int)fSpread);
	strTextTmp.Format("%d", (int)fSpread);
	SetDlgItemText(IDC_STATIC_SPREAD,strTextTmp);
	
	m_ctrlButtonStart.SetCheck(FALSE);
	m_ctrlButtonStop.SetCheck(TRUE);

	return TRUE;  // return TRUE unless you set the focus to a control
	              // EXCEPTION: OCX Property Pages should return FALSE
}


void CSendMany::OnTimer(UINT nIDEvent) 
{
#ifdef STEVETEST
	switch (nIDEvent)
	{
		case TIMER_PROMO_SEND:
			PromoSend();
			break;
		case TIMER_UPDATE_DYNAMICS:
			dwCurrentTimeMs = GetTickCount();
			dwElapsedTimeMs = dwCurrentTimeMs-dwLastTimeMs;
			dwLastTimeMs = dwCurrentTimeMs;
			UpdateDynamics((float)(dwElapsedTimeMs/1000.0));
			break;
	}	
#endif	//STEVETEST
	
	CPropertyPage::OnTimer(nIDEvent);
}

void CSendMany::WorkFunction(UINT uiTimerID)
{
	if(m_uiTimerIDElapsedDynamics<0)
	{
		return;
	}
	if(uiTimerID == m_uiTimerIDSend)
	{
		PromoSend();
	}
	else if(uiTimerID == m_uiTimerIDDynamics)
	{
		UpdateDynamics(dGetElapsedTimeSec(m_uiTimerIDElapsedDynamics));
	}	
}


void CSendMany::PromoSend()
{
	Put_Aircraft(&b707);
	Put_Aircraft(&f18);
	Put_Aircraft(&f16);
	Put_Aircraft(&f15);
	Put_Aircraft(&f22);
	Put_Aircraft(&f23);
	Put_Aircraft( &f117);

	b707.dt = 0.0;
	f18.dt = 0.0;
	f16.dt = 0.0;
	f15.dt = 0.0;
	f22.dt = 0.0;
	f23.dt = 0.0;
	f117.dt = 0.0;
}

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

	/* control stick */
	b707.rot_Z += fTurnRate*dt;
	u = fAirspeed;

	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 = fAltitude;

/* right */
	dN = 	.5*fSpread*cpsi - fSpread*cphi*spsi;
	dE =	.5*fSpread*spsi + fSpread*cphi*cpsi;
	dh	=	-fSpread*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 += fTurnRate*dt;

/* left */
	dN = 	.5*fSpread*cpsi + fSpread*cphi*spsi;
	dE =	.5*fSpread*spsi - fSpread*cphi*cpsi;
	dh	=	fSpread*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 += fTurnRate*dt;
/* left slot */
	dN = 	-fSpread*cpsi + fSpread*cphi*spsi;
	dE =	-fSpread*spsi - fSpread*cphi*cpsi;
	dh	=	fSpread*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 += fTurnRate*dt;

/* right rear slot */
	dN = 	-1.5*fSpread*cpsi - .3*fSpread*cphi*spsi;
	dE =	-1.5*fSpread*spsi + .3*fSpread*cphi*cpsi;
	dh	=	-.5*fSpread*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 += fTurnRate*dt;

/* left rear slot */
	dN = 	-1.5*fSpread*cpsi + .3*fSpread*cphi*spsi;
	dE =	-1.5*fSpread*spsi - .3*fSpread*cphi*cpsi;
	dh	=	.5*fSpread*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 += fTurnRate*dt;

/* right slot */
	dN = 	-fSpread*cpsi - fSpread*cphi*spsi;
	dE =	-fSpread*spsi + fSpread*cphi*cpsi;
	dh	=	-fSpread*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 += fTurnRate*dt;

	double dTimeElapsed = dGetTimeSinceStartSec(m_uiTimerIDElapsedDynamics);
	b707.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dTimeElapsed;
	f18.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dTimeElapsed;
	f16.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dTimeElapsed;
	f15.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dTimeElapsed;
	f22.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dTimeElapsed;
	f23.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dTimeElapsed;
	f117.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dTimeElapsed;


}


void CSendMany::ChangeTurnRate(float fValue)
{
	fTurnRate = (float)fValue/60.0;
	b707.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;
	f18.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;
	f16.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;
	f15.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;
	f22.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;
	f23.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;
	f117.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;
}


void CSendMany::OnCheckSendManyStart() 
{
	if (m_ctrlButtonStart.GetCheck()){
		int iCount = 1;
		char* strAddress[1];
		strAddress[0] = strdup("224.0.5.20:2267");/* this is the default AVDS multicast address*/
		OpenAVDSNetwork(iCount,strAddress);
		InitAircraft();
//		SetTimer(TIMER_PROMO_SEND, 1000.0/(float)TIME_PROMO_SEND_HZ, NULL); 
//		SetTimer(TIMER_UPDATE_DYNAMICS, 1000.0/(float)TIME_UPDATE_DYNAMICS_HZ, NULL); 
//		dwLastTimeMs = GetTickCount();
		m_uiTimerIDElapsedDynamics = iStartElapsedTimer();
		MMSetCallbackTimer(&m_uiTimerIDDynamics,1000.0/(float)TIME_UPDATE_DYNAMICS_HZ);
		MMSetCallbackTimer(&m_uiTimerIDSend,1000.0/(float)TIME_PROMO_SEND_HZ);
	}
	m_ctrlButtonStart.SetCheck(TRUE);
	m_ctrlButtonStop.SetCheck(FALSE);
	
}

void CSendMany::OnCheckSendManyStop() 
{
	if (m_ctrlButtonStop.GetCheck()){
//		KillTimer(TIMER_PROMO_SEND);
//		KillTimer(TIMER_UPDATE_DYNAMICS);
		MMDestroyTimer(m_uiTimerIDDynamics);
		MMDestroyTimer(m_uiTimerIDSend);
	}
	m_ctrlButtonStart.SetCheck(FALSE);
	m_ctrlButtonStop.SetCheck(TRUE);
	
}

void CSendMany::InitAircraft()
{
	float fLon,fLat,fAlt;

	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;

	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;	

	sprintf(b707.packet.home_port,"RST");
	sprintf(f18.packet.home_port,"RST");
	sprintf(f16.packet.home_port,"RST");
	sprintf(f15.packet.home_port,"RST");	
	sprintf(f22.packet.home_port,"RST");	
	sprintf(f23.packet.home_port,"RST");	
	sprintf(f117.packet.home_port,"RST");

	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;

	fAirspeed = 500.0;
	fTurnRate = 90.0/60.0;
	fAltitude = 15000;
	fSpread = 100.0;


//#define FILTER_PARAMETER (0.99)
//#define FILTER_PARAMETER (0.80)
//#define FILTER_PARAMETER (0.90)
#define FILTER_PARAMETER (0.0)

	b707.packet.status |= STATUS_NO_EXTRAPOLATE;
	b707.packet.dUserData[USER_DATA_V_FeetSec] = 300;
	b707.packet.dUserData[USER_DATA_Gs] = 1.2;
	b707.packet.dUserData[USER_DATA_Alpha_Rad] = 0.0872664625997164788461845384244306;
	b707.packet.dUserData[USER_DATA_Beta_Rad] = 0.0872664625997164788461845384244306;
	b707.packet.dUserData[USER_DATA_Filter_X] = FILTER_PARAMETER;
	b707.packet.dUserData[USER_DATA_Filter_Y] = FILTER_PARAMETER;
	b707.packet.dUserData[USER_DATA_Filter_Z] = FILTER_PARAMETER;

	f18.packet.status |= STATUS_NO_EXTRAPOLATE;
	f18.packet.dUserData[USER_DATA_V_FeetSec] = 300;
	f18.packet.dUserData[USER_DATA_Gs] = 1.2;
	f18.packet.dUserData[USER_DATA_Alpha_Rad] = 0.0872664625997164788461845384244306;
	f18.packet.dUserData[USER_DATA_Beta_Rad] = 0.0872664625997164788461845384244306;
	f18.packet.dUserData[USER_DATA_Filter_X] = FILTER_PARAMETER;
	f18.packet.dUserData[USER_DATA_Filter_Y] = FILTER_PARAMETER;
	f18.packet.dUserData[USER_DATA_Filter_Z] = FILTER_PARAMETER;

	f16.packet.status |= STATUS_NO_EXTRAPOLATE;
	f16.packet.dUserData[USER_DATA_V_FeetSec] = 300;
	f16.packet.dUserData[USER_DATA_Gs] = 1.2;
	f16.packet.dUserData[USER_DATA_Alpha_Rad] = 0.0872664625997164788461845384244306;
	f16.packet.dUserData[USER_DATA_Beta_Rad] = 0.0872664625997164788461845384244306;
	f16.packet.dUserData[USER_DATA_Filter_X] = FILTER_PARAMETER;
	f16.packet.dUserData[USER_DATA_Filter_Y] = FILTER_PARAMETER;
	f16.packet.dUserData[USER_DATA_Filter_Z] = FILTER_PARAMETER;

	f15.packet.status |= STATUS_NO_EXTRAPOLATE;
	f15.packet.dUserData[USER_DATA_V_FeetSec] = 300;
	f15.packet.dUserData[USER_DATA_Gs] = 1.2;
	f15.packet.dUserData[USER_DATA_Alpha_Rad] = 0.0872664625997164788461845384244306;
	f15.packet.dUserData[USER_DATA_Beta_Rad] = 0.0872664625997164788461845384244306;
	f15.packet.dUserData[USER_DATA_Filter_X] = FILTER_PARAMETER;
	f15.packet.dUserData[USER_DATA_Filter_Y] = FILTER_PARAMETER;
	f15.packet.dUserData[USER_DATA_Filter_Z] = FILTER_PARAMETER;

	f22.packet.status |= STATUS_NO_EXTRAPOLATE;
	f22.packet.dUserData[USER_DATA_V_FeetSec] = 300;
	f22.packet.dUserData[USER_DATA_Gs] = 1.2;
	f22.packet.dUserData[USER_DATA_Alpha_Rad] = 0.0872664625997164788461845384244306;
	f22.packet.dUserData[USER_DATA_Beta_Rad] = 0.0872664625997164788461845384244306;
	f22.packet.dUserData[USER_DATA_Filter_X] = FILTER_PARAMETER;
	f22.packet.dUserData[USER_DATA_Filter_Y] = FILTER_PARAMETER;
	f22.packet.dUserData[USER_DATA_Filter_Z] = FILTER_PARAMETER;

	f23.packet.status |= STATUS_NO_EXTRAPOLATE;
	f23.packet.dUserData[USER_DATA_V_FeetSec] = 300;
	f23.packet.dUserData[USER_DATA_Gs] = 1.2;
	f23.packet.dUserData[USER_DATA_Alpha_Rad] = 0.0872664625997164788461845384244306;
	f23.packet.dUserData[USER_DATA_Beta_Rad] = 0.0872664625997164788461845384244306;
	f23.packet.dUserData[USER_DATA_Filter_X] = FILTER_PARAMETER;
	f23.packet.dUserData[USER_DATA_Filter_Y] = FILTER_PARAMETER;
	f23.packet.dUserData[USER_DATA_Filter_Z] = FILTER_PARAMETER;

	f117.packet.status |= STATUS_NO_EXTRAPOLATE;
	f117.packet.dUserData[USER_DATA_V_FeetSec] = 300;
	f117.packet.dUserData[USER_DATA_Gs] = 1.2;
	f117.packet.dUserData[USER_DATA_Alpha_Rad] = 0.0872664625997164788461845384244306;
	f117.packet.dUserData[USER_DATA_Beta_Rad] = 0.0872664625997164788461845384244306;
	f117.packet.dUserData[USER_DATA_Filter_X] = FILTER_PARAMETER;
	f117.packet.dUserData[USER_DATA_Filter_Y] = FILTER_PARAMETER;
	f117.packet.dUserData[USER_DATA_Filter_Z] = FILTER_PARAMETER;

	b707.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;
	f18.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;
	f16.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;
	f15.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;
	f22.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;
	f23.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;
	f117.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;
}


