// SendOne.cpp : implementation file
//

#include "stdafx.h"
#include "math.h"
#include "AVDSNetworkTest.h"
#include "SendOne.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


/////////////////////////////////////////////////////////////////////////////
// CSendOne property page

IMPLEMENT_DYNCREATE(CSendOne, CPropertyPage)

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

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

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

}

CSendOne::~CSendOne()
{
}

void CSendOne::DoDataExchange(CDataExchange* pDX)
{
	CPropertyPage::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CSendOne)
	DDX_Control(pDX, IDC_SLIDER_TURN_RATE_ONE, m_ctrlSlideTurnRate);
	DDX_Control(pDX, IDC_SLIDER_ALTITUDE_ONE, m_ctrlSlideAltitude);
	DDX_Control(pDX, IDC_SLIDER_AIRSPEED_ONE, m_ctrlSlideAirspeed);
	DDX_Control(pDX, IDC_CHECK_SEND_ONE_STOP, m_ctrlButtonStop);
	DDX_Control(pDX, IDC_CHECK_SEND_ONE_START, m_ctrlButtonStart);
	//}}AFX_DATA_MAP
}


BEGIN_MESSAGE_MAP(CSendOne, CPropertyPage)
	//{{AFX_MSG_MAP(CSendOne)
	ON_BN_CLICKED(IDC_CHECK_SEND_ONE_START, OnCheckSendOneStart)
	ON_BN_CLICKED(IDC_CHECK_SEND_ONE_STOP, OnCheckSendOneStop)
	ON_WM_TIMER()
	ON_WM_HSCROLL()
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CSendOne message handlers

void CSendOne::OnCheckSendOneStart() 
{
	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 CSendOne::OnCheckSendOneStop() 
{
	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 CSendOne::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 CSendOne::WorkFunction(UINT uiTimerID)
{
	if(m_uiTimerIDElapsedDynamics<0)
	{
		return;
	}
	if(uiTimerID == m_uiTimerIDSend)
	{
		PromoSend();
	}
	else if(uiTimerID == m_uiTimerIDDynamics)
	{
		UpdateDynamics(dGetElapsedTimeSec(m_uiTimerIDElapsedDynamics));
	}	
}


void CSendOne::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar* pScrollBar) 
{
	CSliderCtrl* pSlide = (CSliderCtrl*) pScrollBar;
	CString strTextTmp;

	switch(pScrollBar->GetDlgCtrlID())
	{
	case IDC_SLIDER_AIRSPEED_ONE:
		fAirspeed = (float)m_ctrlSlideAirspeed.GetPos();
		strTextTmp.Format("%d",(int)m_ctrlSlideAirspeed.GetPos());
		SetDlgItemText(IDC_STATIC_AIRSPEED_ONE,strTextTmp);
		break;	
	case IDC_SLIDER_ALTITUDE_ONE:
		fAltitude = (float)m_ctrlSlideAltitude.GetPos();
		strTextTmp.Format("%d",(int)m_ctrlSlideAltitude.GetPos());
		SetDlgItemText(IDC_STATIC_ALTITUDE_ONE,strTextTmp);
		break;	
	case IDC_SLIDER_TURN_RATE_ONE:
		fTurnRate = (float)m_ctrlSlideTurnRate.GetPos();
		ChangeTurnRate(fTurnRate);
		strTextTmp.Format("%d",(int)m_ctrlSlideTurnRate.GetPos());
		SetDlgItemText(IDC_STATIC_TURN_RATE_ONE,strTextTmp);
		break;	
	}
	
	
	CPropertyPage::OnHScroll(nSBCode, nPos, pScrollBar);
}

BOOL CSendOne::OnInitDialog() 
{
	CPropertyPage::OnInitDialog();
	
	CString strTextTmp;

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

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


	m_ctrlSlideAltitude.SetRange(100,30000,TRUE);
	m_ctrlSlideAltitude.SetPos((int)fAltitude);
	strTextTmp.Format("%d", (int)fAltitude);
	SetDlgItemText(IDC_STATIC_ALTITUDE_ONE,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 CSendOne::InitAircraft()
{
	float fLon,fLat,fAlt;

	fLon=-122.366;fLat=37.613;fAlt=15000.0;
	Init_Aircraft(&f18,"F18_Drone",1,AIRCRAFT_TYPE_01,&fLon,&fLat,&fAlt);
	f18.rot_X = 0.0;
	f18.rot_Y = 8.0;
	f18.rot_Z = 0.0;

	sprintf(f18.packet.home_port,"RST");

	f18.packet.hostid = 6758;
	f18.packet.craftid = 6758;

	f18.engine = 100;

	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;
//#define FILTER_PARAMETER (0.95)
#define FILTER_PARAMETER (0.0)
	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;
	f18.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = 0.0;


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

}

void CSendOne::ChangeTurnRate(float fValue)
{
	fTurnRate = (float)fValue/60.0;
	f18.rot_Y = (float)asin(.26*(double)fValue/90.0)/DEG_TO_RAD;

}

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

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

	sphi = sin(f18.rot_Y*DEG_TO_RAD);
	cphi = cos(f18.rot_Y*DEG_TO_RAD);
	spsi = sin(f18.rot_Z*DEG_TO_RAD);
	cpsi = cos(f18.rot_Z*DEG_TO_RAD);

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

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


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

	f18.dt += dt;
	f18.pos_X += dE * dt * xscale ;
	f18.pos_Y += dN * dt;
	f18.pos_Z = fAltitude;
	f18.packet.dUserData[USER_DATA_ElapsedSimulationTimeSec] = dGetTimeSinceStartSec(m_uiTimerIDElapsedDynamics);
}

void CSendOne::PromoSend()
{
	Put_Aircraft(&f18);
	f18.dt = 0.0;
	bNewFrame = 1;
}
