/*!
	\file Wheelchair.cpp
	\brief main application implementation.
*/

// Wheelchair.cpp : Defines the class behaviors for the application.
//

/* July 2011
Modifications made by Radhika for implementation of Shared Control 
are explicity mentioned with comments labelled "start and end radhika"
*/

#include "stdafx.h"
#include "Wheelchair.h"
#include "detours.h"
#include <iostream>

using namespace std;

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

//TODO consider adding separate logs for shared control

//! \name Macros
//@{
#define arrayof(x)	(sizeof(x)/sizeof(x[0])) //!< Calculate number of elements in array
#define UTWND CWnd::FindWindow(NULL,CString("Unreal Tournament 2004")) //! Find the UT window
//@}

//! \name Unreal string constants
//@{
#define UTCNAME "Unreal Tournament 2004"
#define UTCAPP  "ut2004\\System\\ut2004"
#define DLLFILE "ut2004\\System\\Hook"
//@}

//global variables
CWheelchairApp theApp;			//!< The one and only CWheelchairApp object
void * CWheelchairApp::pObject;	//!< Needed in TimerProc_Wrapper


// CWheelchairApp

BEGIN_MESSAGE_MAP(CWheelchairApp, CWinApp)
	ON_COMMAND(ID_HELP, &CWinApp::OnHelp)
	ON_THREAD_MESSAGE(UWM_SERVER_READY, OnServerReady)
//	ON_THREAD_MESSAGE(UWM_RECVD_STARTPOSE, OnRecvdStartPose)
END_MESSAGE_MAP()

// CWheelchairApp construction

CWheelchairApp::CWheelchairApp()
{
	EnableHtmlHelp();
	// TODO: add construction code here,
	// Place all significant initialization in InitInstance
}




const GUID CDECL BASED_CODE _tlid =
		{ 0x13D65F40, 0xC8CC, 0x42B5, { 0xA4, 0xD0, 0x16, 0x13, 0x83, 0x3A, 0x3, 0xB7 } };
const WORD _wVerMajor = 1;
const WORD _wVerMinor = 0;


// CWheelchairApp initialization
//! Initialises the application object.
/** Following initialisation (including reading in of the ocnfiguration parameters),
	The main modal dialog (\ref CWheelchairApp) is open. The function waits until that dialog 
	is closed, then calls ::CleanUp and returns FALSE causing the application to exit.
**/
BOOL CWheelchairApp::InitInstance()
{
//TODO: call AfxInitRichEdit2() to initialize richedit2 library.
	// InitCommonControlsEx() is required on Windows XP if an application
	// manifest specifies use of ComCtl32.dll version 6 or later to enable
	// visual styles.  Otherwise, any window creation will fail.
	INITCOMMONCONTROLSEX InitCtrls;
	InitCtrls.dwSize = sizeof(InitCtrls);
	// Set this to include all the common control classes you want to use
	// in your application.
	InitCtrls.dwICC = ICC_WIN95_CLASSES;
	InitCommonControlsEx(&InitCtrls);

	CWinApp::InitInstance();

	if (!AfxSocketInit())
	{
		AfxMessageBox(IDP_SOCKETS_INIT_FAILED);
		return FALSE;
	}

	// Standard initialization
	// If you are not using these features and wish to reduce the size
	// of your final executable, you should remove from the following
	// the specific initialization routines you do not need
	// Change the registry key under which our settings are stored
	// TODO: You should modify this string to be something appropriate
	// such as the name of your company or organization
	SetRegistryKey(_T("University of Strathclyde"));


UINT nDevices;
PRAWINPUTDEVICELIST pRawInputDeviceList;
if (GetRawInputDeviceList(NULL, &nDevices, sizeof(RAWINPUTDEVICELIST)) != 0) {
	MessageBox(NULL, "1","Error",MB_OK|MB_ICONSTOP);}
if ((pRawInputDeviceList = (PRAWINPUTDEVICELIST) malloc(sizeof(RAWINPUTDEVICELIST) * nDevices)) == NULL) {	
	MessageBox(NULL, "2","Error",MB_OK|MB_ICONSTOP);}
if (GetRawInputDeviceList(pRawInputDeviceList, &nDevices, sizeof(RAWINPUTDEVICELIST)) == -1) {	
	MessageBox(NULL, "3","Error",MB_OK|MB_ICONSTOP);}


free(pRawInputDeviceList);


	CWheelchairDlg dlg = CWheelchairDlg(this);
	m_pMainWnd = &dlg;	//required by MFC
	m_pMainGUI = &dlg;
	m_utclient = NULL;
	m_pTimer = NULL;
	pObject = this;		// Needed for timer wrapper
	m_pConfigurationDlg = NULL;


	m_arOut = NULL;
	m_arIn = NULL;
	m_File = NULL;
	m_pUSARSocket = new USARSocket(this); 
	m_bConnected = false;

	m_logfile = nullptr;

	/***********************************
	 * Read-in configurable properties *
	 ***********************************/
	CAppConfig::getProperty("utHeight", m_cUTHEIGHT);
	CAppConfig::getProperty("utWidth", m_cUTWIDTH);
	CAppConfig::getProperty("FPS", m_cFPS);
	CAppConfig::getProperty("PortNumber", m_cPORTNUM);
	CAppConfig::getProperty("SPI_LENS_POS_Y", m_LensPosY);
	CAppConfig::getProperty("SPI_LENS_POS_Z", m_LensPosZ);
	CAppConfig::getProperty("SPI_EYE_POS_Y", m_EyePosY);
	CAppConfig::getProperty("SPI_EYE_POS_Z", m_EyePosZ);
	CAppConfig::getProperty("utCommand", m_cUTCCMD);
	CAppConfig::getProperty("FirstRun", m_bFirstRun);
	CAppConfig::getProperty("UseFrameBufferObjects", m_bUseFBO);
	CAppConfig::getProperty("ShowConfig", m_bShowConfig);
	CAppConfig::getProperty("ChannelSize", m_ChanSize);
	CAppConfig::getProperty("SimWidth", m_SimWidth);
	CAppConfig::getProperty("SimHeight", m_SimHeight);
	CAppConfig::getProperty("UTPath", m_ConfUTPath);
	CAppConfig::getProperty("AppPath", m_AppPath);
	CAppConfig::getProperty("DeviceName", m_Device);
	CAppConfig::getProperty("ControlDevice", m_CtrlDev);
	CAppConfig::getProperty("ControlMode", m_CtrlMode);
	CAppConfig::getProperty("RawInputDataSize", m_RawInputDataSize);

	CString path = m_ConfUTPath;
	path.Append("\\System\\ut2004\0");
	strcpy_s(m_utPath,path);
	
	path = m_ConfUTPath;
	path.Append("\\System\\Hook\0");
	strcpy_s(m_dllPath,path);
	
	/******************************
	 * Cached Move initialisation *
	 ******************************/
	m_CachedMove.vx=0.001f;
	m_CachedMove.vy=0.001f;
	m_CachedMove.rx=-1;
	m_CachedMove.cx=0.001f;
	m_CachedMove.cy=0.001f;
	m_CachedMove.cz=0.001f;

	memset(&m_Joyinfo,0,sizeof(JOYINFOEX));
	m_Joyinfo.dwSize = sizeof(JOYINFOEX);
	m_Joyinfo.dwFlags = JOY_RETURNPOV | JOY_RETURNX | JOY_RETURNY | JOY_RETURNZ | JOY_RETURNR;
//	m_iJMoveCount = 10;	// will cause a log message on first execution.
	m_turnAmount = 0.0;
	m_moveAmount = 0.0;
	m_lastClock = 0;
	m_frameCount = 0;
	m_hookDLL = NULL;
	m_pFrameData = NULL;
	m_pfFrameData = NULL;
	m_pSim = NULL;
	
	/****************
	 * SPI Settings *
	 ****************/
	m_bSPISettingsChange = true;
	m_pSettingsDlg = new CSettingsDlg(this, m_pMainGUI); 
	m_pSettingsDlg->Create(IDD_SETTINGS, m_pMainGUI);
	m_bSPICaptureImage = false;
	m_pEvent = new CEvent(FALSE, TRUE); //manual event
	m_pEvent->SetEvent();
	m_bUTFreeze = false;

	m_pStateControl = new StateControl(this);

	//radhika start
	m_useSharedControl = false; //shared control is disabled by default
	//end radhika

	INT_PTR nResponse = dlg.DoModal();
	if (nResponse == IDOK)
	{
		// TODO: Place code here to handle when the dialog is
		//  dismissed with OK
	}
	else if (nResponse == IDCANCEL)
	{
		// TODO: Place code here to handle when the dialog is
		//  dismissed with Cancel
	}
	
	CleanUp();

	// Since the dialog has been closed, return FALSE so that we exit the
	// application, rather than start the application's message pump.
	return FALSE;
}


//! Starts the UT2004 server using the specified map
/*! \param map The map to be loaded.
	If UT2004 is already open it will be closed and reloaded. 
	This function is adopted from the SimpleUI sample in USARSim*/
bool CWheelchairApp::LoadUT(CString map)
{
	TRACE0("LoadUT\r\n");

	// First of all check if UT2004 is already running. 
	// If so, we need to close it.
	CWnd* ut = UTWND;
	if (ut!=NULL) {
		ut->SendMessage(WM_CLOSE);
		AfxMessageBox("A Runing UT Client Is Killed!", MB_OK|MB_ICONASTERISK);
	}

	// Now proceed with loading new UT client

	STARTUPINFO si;
	PROCESS_INFORMATION pi;
	CHAR szFullExe[1024] = "\0";
	CHAR szMesg[1024]="\0";
	CString utPath, dllPath; // store the paths.
	PCHAR pszFileExe = NULL;
	
	// This is the command that starts UT2004 for USARSim simulation.
	CString Command =  " " + map + m_cUTCCMD;
    //This conversion is required for DetourCreateProcessWithDLL below
	char* cmd = (char *)(LPCSTR)Command; 

	map.Replace(".ut2","");  //strips the extension

	ZeroMemory(&si, sizeof(si));
	ZeroMemory(&pi, sizeof(pi));
	si.cb = sizeof(si);


	//Find the path to the executable ut2004.exe
	SearchPath("C:\\", m_utPath, ".exe", arrayof(szFullExe), szFullExe, &pszFileExe);
	utPath = szFullExe[0] ? szFullExe : NULL;
	utPath.Replace("\\","\\\\");
	dllPath = utPath;
	dllPath.Replace("ut2004.exe", "Hook.dll");

	//! The Detorus technology is used here to hook UT2004. This allows capturing backbuffer images.
    if (!DetourCreateProcessWithDll(szFullExe[0] ? szFullExe : NULL,
		                            cmd, NULL, NULL, TRUE,
                                    CREATE_DEFAULT_ERROR_MODE, NULL, NULL,
                                    &si, &pi, dllPath, NULL)) {
		//If we get here, Detours failed. If the file was not found, we let the user point us.
		int errno;
		errno = GetLastError();
		if (errno==2) { //File not found
			AfxMessageBox( IDS_OPEN_UT2004, MB_OK|MB_ICONSTOP );
			errno = 0;
			//Let the user indicate the executable
			CFileDialog * pcf = new CFileDialog(true,".exe","ut2004",
				                                OFN_HIDEREADONLY | OFN_OVERWRITEPROMPT,
												"Executable Files (*.exe)|*.exe");
			pcf->DoModal();
			utPath = pcf->GetPathName();
			CString file = pcf->GetFileName();
			delete pcf;
			utPath.Replace("\\","\\\\");
			strncpy_s(m_utPath,(LPCSTR)utPath,512);
			utPath.Replace((LPCSTR)file,"Hook.dll");
			strncpy_s(m_dllPath,(LPCSTR)utPath,512);
			
			//Try again
			if (!DetourCreateProcessWithDll(m_utPath,
		                            cmd, NULL, NULL, TRUE,
                                    CREATE_DEFAULT_ERROR_MODE, NULL, NULL,
                                    &si, &pi, m_dllPath, NULL))
				errno = GetLastError();
		}
		// If there's still an error, give up, display warning and return
		if (errno) {
			CString strMsg;
			strMsg.Format( IDS_HOOK_ERROR, errno );
			AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
			return false;
		}
    }

	
	// Waiting for UT2004 window. Checking every 100 ms for a minute
	int time=0;
	while (time<60000) {
		Sleep(100);
		if ((m_utclient=CWnd::FindWindow(NULL,UTCNAME))!=NULL)
			break;
		time+=100;
	}
	// If the search times out display a warning and return
	if (m_utclient==NULL) {
        CString strMsg;
		strMsg.Format( IDS_CANT_CATCH_UTC, UTCNAME );
		AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
		return false;
	}


	//Removes frame from the window (so that image is the correct size)
	m_utclient->ModifyStyle(WS_CAPTION,0);

	//Adjusts resolution:
	m_utclient->SetForegroundWindow();
	Sleep(500);
	VirtualKeyPress(VK_TAB);
	char lstr[100];
	sprintf_s(lstr,100,"setres %dx%d\0",m_cUTWIDTH-6,m_cUTHEIGHT-6);
	InputString((LPTSTR)lstr);
	VirtualKeyPress(VK_RETURN);
	Sleep(500);
	//return focus to the GUI
	SetForegroundWindow(m_pMainGUI->m_hWnd);

	return true;
}


//! Creates USARSocket and connects to the UT2004 Server.
/*! \param host Address of the UT server
	This function is adopted from the SimpleUI sample in USARSim*/
bool CWheelchairApp::Connect(CString host)
{
	TRACE0("Connect\r\n");

	if (!m_bConnected) {
		if (!m_pUSARSocket->Create()) {
			MessageBox(m_pMainGUI->m_hWnd,"Can't create socket","Socket",MB_OK);
			return false;
		}
		if (!m_pUSARSocket->Connect(host,m_cPORTNUM)) {
			MessageBox(m_pMainGUI->m_hWnd,"Can't connect to server","Connect",MB_OK);
			m_pUSARSocket->Close();
			return false;
		}
		if (m_File==NULL)  m_File  = new CSocketFile(m_pUSARSocket);
		if (m_arIn==NULL)  m_arIn  = new CArchive(m_File, CArchive::load,8192);
		if (m_arOut==NULL) m_arOut = new CArchive(m_File, CArchive::store);
	
		//CString str;
		//ReadUTMessage(str);
	} 	
	
	m_bConnected =  true;
	return true; 
}

//! Resets buffers and closes the USARSocket.
void CWheelchairApp::CloseConnection()
{
	TRACE0("CloseConnection\r\n");
	if (m_arOut!=NULL) {
		delete m_arOut;
		m_arOut=NULL;
	}
	if (m_arIn!=NULL) {
		delete m_arIn;
		m_arIn=NULL;
	}
	if (m_File!=NULL) {
		delete m_File;
		m_File=NULL;
	}
	m_pUSARSocket->Close();
	m_bConnected = false;
};

//! Frees memory for all objects created on the heap.
void CWheelchairApp::CleanUp()
{	
	TRACE0("CleanUp\r\n");
	if(m_pTimer!=NULL) {
		StopTimer();
	}
	if (m_pSim!=NULL){
		delete m_pSim;
		m_pSim = NULL;
	}
	if (UTWND==NULL && m_utclient!=NULL) { // happens if UT was closed or crashed
		m_utclient = NULL;
	}
	if (m_utclient!=NULL) {
		m_utclient->SendMessage(WM_CLOSE);
		m_utclient = NULL;
	}
	if (m_arIn!=NULL) {
		delete m_arIn;
		m_arIn=NULL;
	}
	if (m_arOut != NULL) {
		delete m_arOut;
		m_arOut=NULL;
	}
	if (m_File != NULL) {
		delete m_File;
		m_File=NULL;
	}
	if (m_pUSARSocket != NULL){
		m_pUSARSocket->Close();
		delete m_pUSARSocket;
		m_pUSARSocket = NULL;
	}
	if (m_pConfigurationDlg != NULL) {
		delete m_pConfigurationDlg;
		m_pConfigurationDlg = NULL;
	}
	if (m_pSettingsDlg!=NULL) {
		delete m_pSettingsDlg;
		m_pSettingsDlg = NULL;
	}
	if (m_pStateControl!=NULL) {
		delete m_pStateControl;
		m_pStateControl= NULL;
	}

	//radhika start
	if (m_pSharedControl!=NULL) {
		delete m_pSharedControl;
		m_pSharedControl= NULL;
	}
	//end radhika

	if (m_logfile){
		m_logfile->Close();
		delete m_logfile;
		m_logfile = nullptr;
	}
}
//! Reads in the robot class and location from the controls and spawns the robot.
/*! \param model The model of the robot
	\param position The position where robot is to be spawned
	Evoked on pressing the "Spawn" button.*/
bool CWheelchairApp::Spawn(CString model, CString position, CString rotation) 
{	
	TRACE0("Spawn\r\n");
	if (!m_bConnected) {
		MessageBox(m_pMainGUI->m_hWnd, "Not connected. Cannot spawn.","Error",MB_OK|MB_ICONSTOP);
		return false;
	}
	
	// apply the necessary z offset:
	CString corrected_position = CorrectElevation(model, position);

	CString str;
	str = "INIT {ClassName USARBot." + model + "} {Location " + corrected_position + "}{Rotation " + rotation + "}";

	try {
		m_arOut->WriteString(str);
		m_arOut->WriteString("\r\n");
		m_arOut->Flush();
	}catch (CException* e) {
		e->Delete();
		ConnectionLost();
		m_pMainGUI->AddToLog(ERRORMSG, "The following message was not delivered:");
	}

	m_pMainGUI->AddToLog(OUTGOING, str); // add server msg to the log
	
	/* This block sets focus to the UT2004 and simualates a left click there
	 * causing a switch from side view to 1st person view through robot cameras
	 */
	m_utclient->SetForegroundWindow();
	Sleep(500);
	LeftClick();
	//VirtualKeyPress(VK_TAB);
	//LPTSTR lstr = "setres 522x522\0";
	//InputString(lstr);
	Sleep(2000);
	//set focus to SimWnd
	SetForegroundWindow(m_pSim->m_hWnd);
	Sleep(500);
	//return focus to the GUI
	SetForegroundWindow(m_pMainGUI->m_hWnd);

	m_pStateControl->Reset();

	return true;
}

//! Altitude correction for spawning robots.
/*! \param model	The robot model
	\param location	The original location
	The hight of playerstart is constant in UT2004. Each robot (wheelchair) 
	therefore requires correction so that it spawns smoothely.
	*/
CString CWheelchairApp::CorrectElevation(CString model, CString location)
{
	TRACE0("CorrectElevation\r\n");
	//  REMEMBER - add to z according to robot!!
	int pos = location.ReverseFind(',');
	CString temp= location.Right(location.GetLength() - pos -1);
	location.Delete(pos+1, temp.GetLength());
	float z = 0.0; // spawn elevation

	if (model == "P2DX") {
		z = (float)atof((LPCSTR) temp);
		z -= (float)0.01;
	}
	if (model == "P2AT") {
		z = (float)atof((LPCSTR) temp);
		z -= (float)0.07;
	}
	if (model == "Wheelchair") {
		z = (float)atof((LPCSTR) temp);
		z -= (float)0.112;
	}
	temp.Format("%3.2f",z);
	location.Append(temp);
	return location;
}


//! Implements keyboard control. 
/*!	Interpets arrow-keys messages and sends appropriate commands to the UT2004 Server. \n	
	This function is adopted from the Usar_UI sample in USARSim.
	\param wParam member of MSG struct. Stores the key pressed.
	\param lParam member of MSG struct. Not used in this function.
	\param type 1 for KEYDOWN, 0 for KEYUP*/
//int CWheelchairApp::KMove(WPARAM wParam, LPARAM lParam, int type)
//{
//	TRACE0("KMove\r\n");
//
//	// don't bother if there's no connection
//	if (!CheckConnection()) 
//		return 0;
//
//	CString str;
//	int processed=1;
//	switch(wParam) {
//		/*! The speed of turning or moving forward/backward is increased by 0.2 until the maximum 
//			of 20.0 while the key is held down. When the key is released movement stops.
//		*/
//		case VK_LEFT:
//			m_turnAmount=type?m_turnAmount+0.2f:0.0f;
//			m_turnAmount=(m_turnAmount>20.0f)?20.0f:m_turnAmount;
//			str.Format("DRIVE {Left %f} {Right %f} {Normalized true}",-m_turnAmount,m_turnAmount);
//			break;
//		case VK_RIGHT:
//			m_turnAmount=type?m_turnAmount+0.2f:0.0f;
//			m_turnAmount=(m_turnAmount>20.0f)?20.0f:m_turnAmount;
//			str.Format("DRIVE {Left %f} {Right %f} {Normalized true}",m_turnAmount,-m_turnAmount);
//			break;
//		case VK_UP:
//			m_moveAmount=type?m_moveAmount+0.2f:0.0f;
//			m_moveAmount=(m_moveAmount>20.0f)?20.0f:m_moveAmount;
//			str.Format("DRIVE {Left %f} {Right %f} {Normalized true}",m_moveAmount,m_moveAmount);
//			break;
//		case VK_DOWN:
//			m_moveAmount=type?m_moveAmount+0.2f:0.0f;
//			m_moveAmount=(m_moveAmount>20.0f)?20.0f:m_moveAmount;
//			str.Format("DRIVE {Left %f} {Right %f} {Normalized true}",-m_moveAmount,-m_moveAmount);
//			break;
//		case VK_RETURN:
//		case VK_ESCAPE:
//			break;
//		default:
//			processed=0;
//	}
//
//	if (str.GetLength()>1) {
//		try {
//			m_arOut->WriteString(str);
//			m_arOut->WriteString("\r\n");
//			m_arOut->Flush();
//		}catch (CException* e) {
//			e->Delete();
//			ConnectionLost();
//			m_pMainGUI->AddToLog(ERRORMSG, "The following message was not delivered:");
//		}
//		m_pMainGUI->AddToLog(OUTGOING, str);
//	}
//	return processed;
//}
int CWheelchairApp::KMove(WPARAM wParam, LPARAM lParam, int type)
{
	TRACE0("KMove\r\n");
	if (type == 0)
		return 1;
	
	// don't bother if there's no connection
	if (!CheckConnection()) 
		return 0;

	CString str;
	int processed=1;
	switch(wParam) {
		case VK_LEFT:
		case VK_NUMPAD4:
			m_pStateControl->TransFun(LEFT);
			break;
		case VK_RIGHT:
		case VK_NUMPAD6:
			m_pStateControl->TransFun(RIGHT);
			break;
		case VK_UP:
		case VK_NUMPAD8:
			m_pStateControl->TransFun(FORWARD);
			break;
		case VK_DOWN:
		case VK_NUMPAD2:
			m_pStateControl->TransFun(BACKWARD);
			break;
		case VK_SPACE:
		case VK_NUMPAD5:
			m_pStateControl->TransFun(STOP);
		case VK_RETURN:
		case VK_ESCAPE:
			break;
		default:
			processed=0;
	}

	if (str.GetLength()>1) {
		try {
			m_arOut->WriteString(str);
			m_arOut->WriteString("\r\n");
			m_arOut->Flush();
		}catch (CException* e) {
			e->Delete();
			ConnectionLost();
			m_pMainGUI->AddToLog(ERRORMSG, "The following message was not delivered:");
		}
		m_pMainGUI->AddToLog(OUTGOING, str);
	}
	return processed;
}

//! Implements Joystick control
/*! This function is adopted from the Usar_UI sample in USARSim*/
void CWheelchairApp::JMove()
{
	TRACE0("JMove\r\n");
	//increase the execution counter.
	//m_iJMoveCount++;

	CString str,tmp;
	WORD x,y,z;
	//WORD pov;
	//float rx,ry,rz,d;
	float vx,vy,vz, scale;
	
	//rx = 0; ry = 0; rz = 0;
	vx =0; vy = 0; vz = 0;
	//d = 0;	
	
	joyGetPosEx(JOYSTICKID1, &m_Joyinfo);

	// Turn left/right

	// maximum dwXpos is 65535 so it fits in LOWORD
	// we shift it right by 11 bits to bring the number to between 0 and 31
	x = LOWORD(m_Joyinfo.dwXpos) >> 11; 
    if (x <= 11) vx = (x - 12)/0.6f;
    else if (x >= 20) vx = (x - 19)/0.6f;
	
	// Move forward/backward
	y = LOWORD(m_Joyinfo.dwYpos) >> 11;
	if (y <= 11) vy = (y - 12)/0.6f;
	else if (y >= 20) {
		vy = (y - 19)/0.6f;
	}
	
	z = LOWORD(m_Joyinfo.dwZpos) >> 11;
	scale = 0.1f + 0.9f * (float)z / 31.0f;


	if (vx!=m_CachedMove.vx||vy!=m_CachedMove.vy) {
		m_CachedMove.vx = vx;
		m_CachedMove.vy = vy;
		str.Format("DRIVE {Left %f} {Right %f} {Normalized true}",5*scale*(vx-vy),5*scale*(-vy-vx));
	}

	//This section is not useful currently -- no point-of-view hat 
	//// View pitch & yaw
	//pov = LOWORD(m_Joyinfo.dwPOV);
	//rx=0;ry=0;rz=0;
	//if (pov<36000) {
	//	d = pov*3.1415926f/18000;
	//	ry=(float)(0.006*cos(d));
	//	rz=(float)(0.006*sin(d));
	//}
	//rx=0;

	//if (rx!=m_CachedMove.cx || m_CachedMove.cx!=0 ||
	//	ry!=m_CachedMove.cy || m_CachedMove.cy!=0 ||
	//	rz!=m_CachedMove.cz || m_CachedMove.cy!=0) {
	//	m_CachedMove.cx = rx;
	//	m_CachedMove.cy = ry;
	//	m_CachedMove.cz = rz;
	//	tmp.Format("CAMERA {Rotation %f,%f,%f}\r\n",rx,ry,rz);
	//	//m_Msg.InsertString(0,tmp);
	//	//str+=tmp;
	//}


	if (m_arOut!=NULL&&str.GetLength()>1) {
		try {
			m_arOut->WriteString(str);
			m_arOut->WriteString("\r\n");
			m_arOut->Flush();
		}catch (CException* e) {
			e->Delete();
			ConnectionLost();
			m_pMainGUI->AddToLog(ERRORMSG, "The following message was not delivered:");
		}
		m_pMainGUI->AddToLog(OUTGOING, str);
	}

	//if (m_iJMoveCount > 10){
	//	m_iJMoveCount = 0;
	//	m_pMainGUI->AddToLog(OUTGOING, str);
	//	str.Format("X=%d %d Y=%d %d Z=%d %d R=%d POV=%d %d",
	//			m_Joyinfo.dwXpos,x,
	//			m_Joyinfo.dwYpos,y,
	//			m_Joyinfo.dwZpos,0,
	//			m_Joyinfo.dwRpos,
	//			m_Joyinfo.dwPOV,pov);
	//	m_pMainGUI->AddToLog(OUTGOING, str);
	//}
}

//! Implements Mouse control
/*! \param xMove	horizontal position of cursor or mouse-replacement joystick
	\param yMove	vertical position of the cursor or mouse-replacement joystick
	\param [scale = 1.0] optional scaling parameter
*/
void CWheelchairApp::MMove(float xMove, float yMove, float scale){

	//TRACE0("MMove\r\n");
	
	
	

	TRACE2("New move %f %f\r\n",xMove,yMove);
	TRACE2("Cashed move %f %f\r\n",m_CachedMove.vx,m_CachedMove.vy);
	//if ( abs((xMove-m_CachedMove.vx)/m_CachedMove.vx) >0.1 || abs((yMove-m_CachedMove.vy)/m_CachedMove.vy) >0.1) {
	if ( xMove!=m_CachedMove.vx || abs(yMove-m_CachedMove.vy) > 1) {
		
		m_CachedMove.vx = xMove;
		m_CachedMove.vy = yMove;
		TRACE0("*** Sending ***\r\n");
		//TRACE2("Input %f %f \r\n",xMove,yMove);
			}
	else {
		return;
	}
	CString str;
	float vx = scale*0.2*xMove;
	float vy = scale*yMove;
	// scale backward speed by half
	vy = vy>0 ? vy/2 : vy;
	str.Format("DRIVE {Left %f} {Right %f} {Normalized true}",(vx-vy),(-vy-vx));

	
	
	if (m_arOut!=NULL&&str.GetLength()>1) {
		try {
			m_arOut->WriteString(str);
			m_arOut->WriteString("\r\n");
			m_arOut->Flush();
		}catch (CException* e) {
			e->Delete();
			ConnectionLost();
			m_pMainGUI->AddToLog(ERRORMSG, "The following message was not delivered:");
		}
		m_pMainGUI->AddToLog(OUTGOING, str);
	}

}

//! Querys the server for available starting poses.
/*!	\param n_poses returns the number of starting poses
	\param tags returns array of tags
	\param poses returns corresponding array of poses
	\retval TRUE if successfull
	\retval FALSE otherwise*/
bool CWheelchairApp::ProcessStartPoses(CString &resp, int & n_poses, CString* & tags, CString* & poses, CString* & rotations)
{
	TRACE0("ProcessStartPoses\r\n");
	


	CString str;	//stores various temporary values
	CString* str_poses;	//temporary startpose storage

	if (resp.IsEmpty()){
		CString msg = "GETSTARTPOSES";
		SendUTMessage(msg);
		ReadUTMessage(resp);

	}

	str = resp; //copy for manipulation

	//Get the number of poses defined in the map
	int end = 0;
	n_poses = atoi(LPCSTR(CMessageParser::GetString("StartPoses",resp,0, end)));

	//Set size of the array
	tags = new CString[n_poses];
	poses = new CString[n_poses];
	rotations = new CString[n_poses];
	//allocate an array of CStrings 3 times the size of n_poses
	int count = n_poses * 3;
	str_poses = new CString[count];

	if (!n_poses){
		MessageBox(m_pMainGUI->m_hWnd, "Error. No starting poses found.","Error",MB_OK|MB_ICONSTOP);
		return false;
	}


	str.Delete(0,end+2);	// delete the beginning of the string
	str.Insert(1,"a ");		//a hack to make GetStrings work nicely.
	
	CMessageParser::GetStrings("a", str, ' ', &count, str_poses,TRUE);

	for (int i = 0; i < n_poses; i++) {
		tags[i] = str_poses[3*i];
		poses[i] = str_poses[3*i + 1];
		rotations[i] = str_poses[3*i+2];
	}

	delete[] str_poses;
	return true;
}


//!Obtains the address of shared memory where the image is stored.
/*! This function is adopted from the SimpleUI sample in USARSim*/
bool CWheelchairApp::GetpfFrameData()
{
	TRACE0("GetpfFrameData\r\n");
	
	CString strMsg;
	CHAR szFullDLL[1024] = "\0";
	CHAR szMesg[1024]="\0";
	PCHAR pszFileDLL = NULL;

	//Find hook.dll
	SearchPath("C:\\", m_dllPath, ".dll", arrayof(szFullDLL), szFullDLL, &pszFileDLL);
	
	//Check if UT2004 is running  
	if (UTWND==NULL) {
		if (m_utclient != NULL)
			ConnectionLost();
		return false; 
	}
	if (m_utclient==NULL) return false; // Errors handled in LoadUT. Nothing to do here.

	//load hook.dll
	if ((m_hookDLL = LoadLibrary(szFullDLL))==NULL) {
		strMsg.Format(IDS_DLL_LOAD_ERROR,DLLFILE,GetLastError());
		AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
		return false;
	}
	// Get the function address of getFrameData()
	// Call getFrameData() to get memory address 
	if ((m_pfFrameData = (pfFrameData)GetProcAddress(m_hookDLL, "getFrameData"))==NULL) {
		strMsg.Format(IDS_DLL_PROCADDR_ERROR,"getFrameData",GetLastError());
		AfxMessageBox( strMsg, MB_OK|MB_ICONSTOP );
		FreeLibrary(m_hookDLL);
	}

	return true;
}
//! Starts the timer. 
bool CWheelchairApp::StartTimer(void) 
{	
	TRACE0("StartTimer\r\n");
	GetpfFrameData();	// Obtain pointer to FrameData function
	if (m_pfFrameData==NULL) 
		return false;			// Errors handled in GetpfFrameData. Nothing to do here.
	m_pFrameData = m_pfFrameData(); // Copy the pointer to the FrameData struct variable
	//HANDLE m_timerhandle = NULL;
	//bool success = CreateTimerQueueTimer(&m_timerhandle, NULL, (WAITORTIMERCALLBACK) QueueTimerProc_Wrapper, 0, 0, 1000, WT_EXECUTEINTIMERTHREAD);
	//if (!success) {
	//	MessageBox(m_pMainGUI->m_hWnd,"Couldn't create timer","Error",MB_OK|MB_ICONSTOP);
	//	return false;
	//}
	
	m_pTimer = SetTimer(NULL,NULL,(int)(1000/m_cFPS),(TIMERPROC) TimerProc_Wrapper); 
	return true;
}

//! Stops the timer
void CWheelchairApp::StopTimer(void)
{
	TRACE0("StopTimer\r\n");
	KillTimer(NULL,m_pTimer);
	m_pTimer = NULL;
}
//! Wrapper class for TimerProc. Circumfences requirement for static TimerProc function.
/*! The Timer implementation is based on CodeProject tutorial
http://www.codeproject.com/KB/cpp/SetTimer__non-static.aspx */
void CALLBACK CWheelchairApp::TimerProc_Wrapper( HWND hwnd, UINT uMsg, UINT_PTR idEvent, DWORD dwTime ) 
{
//	TRACE1("TimerProc_Wrapper: %d\r\n", clock());
	CWheelchairApp *pSomeClass = (CWheelchairApp*)pObject; // cast the void pointer

	pSomeClass->TimerProc(hwnd, uMsg, idEvent, dwTime); // call non-static function

}

//! Called on every Timer tick. Calls CSimWnd::Draw() and increases CWheelchairDlg::m_pFrameData sequence number. 
/*! This function is adopted from the Usar_UI sample in USARSim*/
void CALLBACK CWheelchairApp::TimerProc(HWND hwnd,UINT uMsg,UINT_PTR idEvent,DWORD dwTime)
{
	// If we ever go down the threading route, there will need to be a critical section here.
//	TRACE1("TimerProc: %d\r\n",clock());
	TRACE0("=======================\r\n");
	if (UTWND==NULL) {
		ConnectionLost();
		return;
	}
	if (m_pFrameData==NULL)
		//get a FrameData pointer if it's NULL. This should never happen.
		m_pFrameData = m_pfFrameData();
	if (!m_pFrameData) 
		return;
	int time = 0;
	clock_t t = clock();
	int time_out = (int) (0.8 * 1000/m_cFPS);
	while (m_pFrameData->state!=FRAME_OK){// && time < time_out) {
		//TRACE0("waiting...\r\n");
		Sleep(2);
		time+=2;
	}
	TRACE1("Slept: %d ms\r\n", time);
	TRACE1("Time to get frame: %d ms\r\n", (int) clock()-t);
	//if (time >= time_out) {
	//	TRACE0("Time-out. Skipping to next timer tick\r\n");
	//	return;
	//}
	t = clock();
	m_sequence = m_pFrameData->sequence;
	//TRACE1("At frame %d\r\n",m_sequence);
	// The DLL only acquires a new frame if the sequence number increases
	if(!m_bUTFreeze)
		m_pFrameData->sequence += 1;

	if (m_pSim->Draw(m_pFrameData,m_sequence))
		MessageBox(m_pMainGUI->m_hWnd, "Error in the Draw function","Error",MB_OK);
	TRACE1("Time to draw: %f sec\r\n",(float) (clock() - t)/CLOCKS_PER_SEC );


	if (m_lastClock==0) m_lastClock = clock();
	//every 5 seconds update the FPS estimate.
	++m_frameCount;
	t = clock();
	clock_t elapsed = t - m_lastClock;
	TRACE1("Timer: %f sec\r\n",(float) elapsed);


	if ( elapsed > 5*CLOCKS_PER_SEC) {
		m_pMainGUI->setFPS( (float)m_frameCount*CLOCKS_PER_SEC/elapsed );
		m_lastClock = t;
		m_frameCount = 0;
	}

};


/*void CALLBACK CWheelchairApp::QueueTimerProc_Wrapper(PVOID lpParam, BOOL TimerOrWaitFired) 
{
	//TRACE0("TimerProc_Wrapper\r\n");
	CWheelchairApp *pSomeClass = (CWheelchairApp*)pObject; // cast the void pointer

	pSomeClass->TimerProc2();// call non-static function

}
void CALLBACK CWheelchairApp::TimerProc2(void)
{
	// If we ever go down the threading route, there will need to be a critical section here.
	//TRACE0("TimerProc\r\n");
	if (UTWND==NULL) {
		ConnectionLost();
		return;
	}
	if (m_pFrameData==NULL)
		//get a FrameData pointer if it's NULL. This should never happen.
		m_pFrameData = m_pfFrameData();
	 if (m_pFrameData && m_pFrameData->state!=FRAME_PENDING) {
		if (m_lastClock==0) m_lastClock = clock();
		//every 5 seconds update the FPS estimate.
		++m_frameCount;
		if (clock()-m_lastClock>5*CLOCKS_PER_SEC) {
			m_pMainGUI->setFPS( (float)m_frameCount*CLOCKS_PER_SEC/(clock()-m_lastClock) );
			m_lastClock = clock();
			m_frameCount = 0;
		}

		if (m_pSim->Draw(m_pFrameData))
			MessageBox(m_pMainGUI->m_hWnd, "Error in the Draw function","Error",MB_OK);

		
		m_pFrameData->sequence += 1;
	}
	else {
		TRACE0("Pending\r\n");
		//MessageBeep(-1);
	}
	TRACE0("END\r\n");
};
*/
//! Creates and initialises instance of CSimWnd. \sa m_pSim
bool CWheelchairApp::StartSimWnd(void)
{
	TRACE0("StartSimWnd\r\n");
	if (m_pSim == NULL)
		m_pSim = new CSimWnd(this);
	
	m_pSim->ShowWindow(SW_SHOW);
	m_pSim->UpdateWindow();
	return true;
}
//! Closes and destroys the instance of CSimWnd. \sa m_pSim
bool CWheelchairApp::StopSimWnd(void)
{
	TRACE0("StopSimWnd\r\n");
	if (m_pSim!=NULL){
		m_pSim->SendMessage(WM_CLOSE); // calls the destructor. No need to delete
		m_pSim = NULL;
	}
	return true;
}
//! Testing function. Calls \ref CSimWnd::DrawCheckSPI displaying a checkboard
bool CWheelchairApp::TestSimWnd(void)
{
	TRACE0("TestSimWnd\r\n");
	if (m_pSim==NULL)
		return false;
	m_pSim->DrawCheckSPI();
	return true;
}

//! Opens SPI Setting Dialog
bool CWheelchairApp::OpenSettings(void)
{
	TRACE0("OpenSettings\r\n");
	if (m_pSettingsDlg == NULL)
		return false;
	m_pSettingsDlg->ShowWindow(SW_SHOW);
	return true;
}


//! Opens Condfiguration Dialog
bool CWheelchairApp::OpenConfiguration(void)
{
	TRACE0("OpenConfiguration\r\n");
	if (m_pConfigurationDlg == NULL){
		m_pConfigurationDlg = new CConfigurationDlg(this, m_pMainGUI); 
		m_pConfigurationDlg->Create(IDD_CONFIGURATIONDLG, m_pMainGUI);
	}
	if (m_pConfigurationDlg == NULL){
		TRACE0("Couldn't Open Configuration\r\n");
		return false;
	}
	m_pConfigurationDlg->ShowWindow(SW_SHOW);
	
	return true;
}
//! Closes UT2004 window (and server).
/*! \retval TRUE if closed successfuly \retval FALSE if cancelled by user or m_utclient was NULL */
bool CWheelchairApp::CloseUT(void)
{
	TRACE0("CloseUT\r\n");
	if(m_utclient!=NULL){
		if (AfxMessageBox(IDS_CLOSE_UTC,MB_YESNO)==IDYES){
			m_utclient->SendMessage(WM_CLOSE);
			m_utclient = NULL;
			return true;
		} else 
			return false;
	} else
		return false;
}
//! Positions the UT2004 window in the top left corner of the screen
void CWheelchairApp::ShowUT(void)
{
	TRACE0("ShowUT\r\n");
	RECT posW;
	m_utclient->GetWindowRect(&posW);
	m_utclient->SetWindowPos(m_pMainGUI,
							 0, 0,
							 0, 0,
							 SWP_FRAMECHANGED|SWP_NOACTIVATE|SWP_NOSIZE);	
}

//! Hides the UT2004 window by positioning it (almost) off-screen.
void CWheelchairApp::HideUT(void)
{
	TRACE0("HideUT\r\n");
	// put the UT2004 window off-screen
	HDC hScrDC = CreateDC("DISPLAY", NULL, NULL, NULL);
	m_utclient->SetWindowPos(m_pMainGUI,
		  				     GetDeviceCaps(hScrDC, HORZRES)-10,
							 GetDeviceCaps(hScrDC, VERTRES)-10,
							 0, 0,
							 SWP_FRAMECHANGED|SWP_NOACTIVATE|SWP_NOSIZE);
	DeleteDC(hScrDC);
}

//! Destroys corrupt CArchive members after connection loss without memory leaks
/*! After generic CFileException (occurs when UT window disappears) the CArchive
	objects are corrupt and cannot be deleted normally. This function calls Abort() 
	to clear all data, and then uses free() do prevent memory leaks, after 
	explicitly calling the CArchive destructor.*/
void CWheelchairApp::EmergencyArchiveDestroy() 
{
	TRACE0("EmergencyArchiveDestroy\r\n");
	// Clear the contents without throwing exceptions
	m_arOut->Abort();
	m_arIn->Abort();
	// Explicitly call the destructor
	m_arOut->~CArchive();
	m_arIn->~CArchive();
	// free the pointers (only thing left of the objects)
	free(m_arOut);
	free(m_arIn);
	// set the members to NULL
	m_arOut = NULL;
	m_arIn = NULL;
}

//! Resets the application after a connection loss
void CWheelchairApp::ConnectionLost(void)
{
	TRACE0("ConnectionLost\r\n");
	StopTimer();
	MessageBox(m_pMainGUI->m_hWnd,"Connection Lost!","Error",MB_OK|MB_ICONSTOP);
	EmergencyArchiveDestroy();
	CloseConnection();
	if (m_utclient!=NULL) {
		if (UTWND!=NULL)
			m_utclient->SendMessage(WM_CLOSE);
		m_utclient = NULL;
	}
	m_pMainGUI->AddToLog(ERRORMSG, "Connection Lost! ");
	m_pMainGUI->ConnectionLost();
	if (m_pSim != NULL) {
		if (AfxMessageBox("Do you want to close the simulation window?",MB_YESNO)==IDYES){
			StopSimWnd();
		}
	}
}

//! Checks if connection to the server is in order
/*! If something is wrong with the connection, this function attempts to recover.
	\retval TRUE if everything seems OK
	\retval FALSE otherwise*/
bool CWheelchairApp::CheckConnection(void) 
{
	TRACE0("CheckConnection\r\n");
	if (!m_bConnected)
		return false; // everything else must have been handled elsewhere

	// check if UT is running at all
	if (UTWND==NULL) {
		ConnectionLost();
		return false;
	}
	
	if (m_arIn == NULL || m_arOut == NULL || m_File == NULL) {
		StopTimer();
		MessageBox(m_pMainGUI->m_hWnd,"Connection Lost!","Error",MB_OK|MB_ICONSTOP);
		CloseConnection();
		m_pMainGUI->ConnectionLost();
		return false;
	} else {
		return true;
	}
}
//! Save the image captured from the backbuffer of UT2004
bool CWheelchairApp::SaveImg(void)
{
	TRACE0("SaveImg\r\n");

	if (m_pfFrameData == NULL) {
		GetpfFrameData();	// Obtain pointer to FrameData function
	}
	if (m_pfFrameData==NULL) 
		return false;		
	m_pFrameData = m_pfFrameData();
	
	if (m_pTimer == NULL)
		m_pFrameData->sequence += 1;

	/*******************
	 * OpenFile Dialog *
	 *******************/
	
	OPENFILENAME ofn;
	char FileName[512] = "log";
	char Extension[5] = "bmp";
	
	memset(&ofn, 0, sizeof(OPENFILENAME));
	ofn.lStructSize = sizeof(OPENFILENAME);
	ofn.hwndOwner = m_pMainGUI->m_hWnd;
	ofn.lpstrFilter = "Bitmap (*.bmp)\0*.bmp\0";
	ofn.lpstrFile = FileName;
	ofn.lpstrDefExt = ".bmp";
	ofn.nMaxFile = 512;
	ofn.Flags = OFN_CREATEPROMPT;
	ofn.lpstrInitialDir = NULL; //for the time being... 

	GetSaveFileName(&ofn);
	
	// Open the file
	FILE* f; 
	errno_t err;

	if ( (err  = fopen_s( &f, ofn.lpstrFile, "wb" )) !=0 ) {
		MessageBox(m_pMainGUI->m_hWnd, "Failed to open file.", "Error", MB_OK);
		return false;
	}

	/******************
	 * Prepare Bitmap *
	 ******************/

	BITMAPINFO bi;
    bi.bmiHeader.biSize = sizeof(bi.bmiHeader);
    bi.bmiHeader.biWidth = m_pFrameData->width;
    bi.bmiHeader.biHeight = m_pFrameData->height;
    bi.bmiHeader.biPlanes = 1;
    bi.bmiHeader.biBitCount = 24;
    bi.bmiHeader.biCompression = BI_RGB;
    bi.bmiHeader.biSizeImage = m_pFrameData->size;
    bi.bmiHeader.biXPelsPerMeter = 0;
    bi.bmiHeader.biYPelsPerMeter = 0;
    bi.bmiHeader.biClrUsed = 0;
    bi.bmiHeader.biClrImportant = 0;

    BITMAPFILEHEADER bmfh;
    bmfh.bfType = 'MB';
    bmfh.bfSize = sizeof(bmfh) + sizeof(BITMAPINFOHEADER) + bi.bmiHeader.biSizeImage;
    bmfh.bfReserved1 = bmfh.bfReserved2 = 0;
    bmfh.bfOffBits = sizeof(bmfh) + sizeof(BITMAPINFOHEADER);
	
	/*****************
	 * Save the file *
	 *****************/

	LPBYTE data = NULL;
		//if (m_sequence % 2)
		data = m_pFrameData->data1;
	//else
	//	data = m_pFrameData->data2;

	fwrite(reinterpret_cast<void*>(&bmfh), sizeof(bmfh), 1, f);
	fwrite(reinterpret_cast<void*>(&(bi.bmiHeader)), sizeof(BITMAPINFOHEADER), 1, f);
	fwrite(reinterpret_cast<void*>(data), sizeof(BYTE), bi.bmiHeader.biSizeImage, f);
	fclose(f);
	return true;
}

//! Save the image captured from the SPI backbuffer.
/*! If fname is passed, the image will be saved in the default directory 
	with the given name. Otherwise, a dialog box will pop up asking for 
	the directory and filename.*/
bool CWheelchairApp::SaveSPIImg(unsigned char* pRGBA, CString fname)
{
	TRACE0("SaveSPIImg\r\n");

	

	/*******************
	 * OpenFile Dialog *
	 *******************/
	
	OPENFILENAME ofn;
	char FileName[512] = "log";
	char Extension[5] = "txt";

	if (fname == "") {
		memset(&ofn, 0, sizeof(OPENFILENAME));
		ofn.lStructSize = sizeof(OPENFILENAME);
		ofn.hwndOwner = m_pMainGUI->m_hWnd;
		ofn.lpstrFilter = "Bitmap (*.bmp)\0*.bmp\0";
		ofn.lpstrFile = FileName;
		ofn.lpstrDefExt = ".bmp";
		ofn.nMaxFile = 512;
		ofn.Flags = OFN_CREATEPROMPT;
		ofn.lpstrInitialDir = NULL; //for the time being... 

		GetSaveFileName(&ofn);
		
		strcpy(FileName,ofn.lpstrFile);	
	} else {
		strcpy(FileName,m_AppPath+"//"+fname);

	}
	// Open the file
	FILE* f = NULL; 
	errno_t err = 0;
	err  = fopen_s( &f, FileName, "wb" );
	if ( err !=0 ) 
	{
		TRACE0("Failed to open file!\r\n");
	//	MessageBox(m_pMainGUI->m_hWnd, "Failed to open file.", "Error", MB_OK);
		return false;
	}

	/******************
	 * Prepare Bitmap *
	 ******************/

	BITMAPINFO bi;
    bi.bmiHeader.biSize = sizeof(bi.bmiHeader);
    bi.bmiHeader.biWidth = m_SimWidth;
    bi.bmiHeader.biHeight = m_SimHeight;
    bi.bmiHeader.biPlanes = 1;
    bi.bmiHeader.biBitCount = 24;
    bi.bmiHeader.biCompression = BI_RGB;
    bi.bmiHeader.biSizeImage = 3 * (m_SimWidth) * (m_SimHeight);
    bi.bmiHeader.biXPelsPerMeter = 0;
    bi.bmiHeader.biYPelsPerMeter = 0;
    bi.bmiHeader.biClrUsed = 0;
    bi.bmiHeader.biClrImportant = 0;

    BITMAPFILEHEADER bmfh;
    bmfh.bfType = 'MB';
    bmfh.bfSize = sizeof(bmfh) + sizeof(BITMAPINFOHEADER) + bi.bmiHeader.biSizeImage;
    bmfh.bfReserved1 = bmfh.bfReserved2 = 0;
    bmfh.bfOffBits = sizeof(bmfh) + sizeof(BITMAPINFOHEADER);
	
	/*****************
	 * Save the file *
	 *****************/

	fwrite(reinterpret_cast<void*>(&bmfh), sizeof(bmfh), 1, f);
	fwrite(reinterpret_cast<void*>(&(bi.bmiHeader)), sizeof(BITMAPINFOHEADER), 1, f);
	fwrite(reinterpret_cast<void*>(pRGBA), sizeof(BYTE), bi.bmiHeader.biSizeImage, f);
	fclose(f);
	return true;
}
//! Save the current log to file (append if exists)
bool CWheelchairApp::SaveLog(CString str) {

	TRACE0("SaveLog\r\n");

	OPENFILENAME ofn;
	char FileName[512] = "log";
	char Extension[5] = "txt";
	
	memset(&ofn, 0, sizeof(OPENFILENAME));
	ofn.lStructSize = sizeof(OPENFILENAME);
	ofn.hwndOwner = m_pMainGUI->m_hWnd;
	ofn.lpstrFilter = "Textfile (*.txt)\0*.txt\0";
	ofn.lpstrFile = FileName;
	ofn.lpstrDefExt = ".txt";
	ofn.nMaxFile = 512;
	ofn.Flags = OFN_CREATEPROMPT;
	ofn.lpstrInitialDir = m_AppPath;

	GetSaveFileName(&ofn);

	CFile file;

	if (!file.Open(ofn.lpstrFile, CFile::modeCreate | CFile::modeNoTruncate | CFile::modeReadWrite, NULL)){
		MessageBox(m_pMainGUI->m_hWnd, "Failed to open file.", "Error", MB_OK);
		return false;
	}
	file.SeekToEnd(); // forces append...
	file.Write((LPCSTR)str, str.GetLength());
	file.Close();

	return true;
}

bool CWheelchairApp::AppendToLog(CString str) {
	if (!m_logfile){
	// if (nullptr == static_cast<StreamWriter^>(m_logfile)){
		String ^path = gcnew String(m_AppPath);
		path += "\\Wheelchair" + (DateTime::Now).ToString("yyyyMMddHHmmss") + ".log";
		m_logfile = gcnew StreamWriter(path);
		m_logfile->AutoFlush = true;
		delete path;
	}
	String ^txt = gcnew String(str);
	m_logfile->Write(txt);
	delete txt;
	//m_logfile->Close();
	return true;
}
void CWheelchairApp::ProcessMessage(bool test) {
	TRACE0("ProcessMessage\r\n");

	if (test){
		if(m_pUSARSocket->IsBlocking())
			Beep(700,100);
			TRACE0("There was a blocking call");
	}

	CString str, msg_type,msg_content;
	ReadUTMessage(str);
	int start = 0;
	msg_type = str.Tokenize(" ",start);
	
	if (msg_type == "NFO") {
		msg_content = str.Tokenize(" ",start);
		msg_content.Delete(0,1);
		if (msg_content == "Gametype"){
			PostMessage(NULL,UWM_SERVER_READY,0,0);
		}
		else if (msg_content == "StartPoses"){
			m_pMainGUI->PopulateStartPose(str);	
		}
	
	//radhika start
	//send sensor data to SharedControl
	} else if (m_useSharedControl && (msg_type == "SEN")){
		TRACE1("Sensor Type! Calling SSC with string %s\r\n", str.GetString());
		m_pSharedControl->SetSensorRange(str);
		TRACE0("Returned from SSC!\r\n");
	}
	//end radhika

}

bool CWheelchairApp::ReadUTMessage(CString & str) {
	
	TRACE0("ReadUTMessage\r\n");

	CString temp;
	str = "";
	BOOL res;
	int count = 0;
	do{
		try{
			TRACE0("ReadString\r\n");
			res = m_arIn->ReadString(temp); 
		} catch (CArchiveException* e){
			TRACE1("!!EXCEPTION Cause = %d\n",e->m_cause);
			e->Delete();
			//MessageBox(m_pMainGUI->m_hWnd,"Can't read from socket. Disconnecting...","Connect",MB_OK);
			// Clean up for next execution:
			//CloseConnection();
			return false;
		}
		if (!res){
			TRACE0("ReadString returned FALSE");
			return false;
		}
		m_pMainGUI->AddToLog(INCOMING, temp);
		str.Append(temp);
		count ++;
	}while(!m_arIn->IsBufferEmpty());
	
	//TRACE1("The String is %s\n",str.GetString());
	//TRACE1("Count = %d\n", count);
	return true;
}

bool CWheelchairApp::SendUTMessage(CString msg){
	TRACE0("SendUTMessage\r\n");
	bool retval = true;

	// Check if buffers exist
	if (m_arIn==NULL || m_arOut==NULL || !m_bConnected) {
		m_bConnected = false;
		MessageBox(m_pMainGUI->m_hWnd,"There's no connection to the server, or connection is broken.","Error",MB_OK);
		return false; 
	}


	try {
		m_arOut->WriteString(msg);
		m_arOut->WriteString("\r\n");
		m_arOut->Flush();
	}catch (CArchiveException* e) {
		TRACE1("!!EXCEPTION Cause = %d\n",e->m_cause);
		e->Delete();
		ConnectionLost();
		m_pMainGUI->AddToLog(ERRORMSG, "The following message was not delivered:");
		retval = false;
	}
	m_pMainGUI->AddToLog(OUTGOING, msg);

	return retval;
}

void CWheelchairApp::OnServerReady(WPARAM, LPARAM){

	TRACE0("OnServerReady\r\n");
	m_bServerReady = true;
	
	CString msg = "GETSTARTPOSES";
	SendUTMessage(msg);
}

//radhika start
//Function to handle SharedControl check-box
void CWheelchairApp::OnSharedControl(BOOL m_startSharedCtrl)
{
	if(m_startSharedCtrl){
		if(!m_pSharedControl){
			TRACE0("Create shared control object\n");
			m_pSharedControl = new SharedControl(this); //Constructing shared control object
		}
		TRACE0("Shared Control Set\n");
		m_useSharedControl = true;
	}else{
		TRACE0("NO SHARED CONTROL\n");
		m_useSharedControl = false;
	}
}
//end radhika