// VPserWinConsole.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <Windows.h>
#include <conio.h>
#include <iostream>
#include <string>
#include <sstream>

#include "VPserIO.h"

using namespace std;
#ifndef tstring
typedef basic_string<TCHAR> tstring;
#if defined(UNICODE) || defined(_UNICODE)
#define tstringstream std::wstringstream
#else
#define tstringstream std::stringstream
#endif
#endif

#if defined(UNICODE) || defined(_UNICODE)
#define tcout std::wcout
#define tcin std::wcin
#define tcslen std::wcslen
#else
#define tcout std::cout
#define tcout std::cin
#define tcslen std::strlen
#endif

typedef enum
{
	CHOICE_CONT
	, CHOICE_CONT_ASYNC
	, CHOICE_DISCONNECT
	, CHOICE_ASCII
	, CHOICE_SINGLE
	, CHOICE_SINGLE_LOOP
	, CHOICE_QUIT
	, CHOICE_ENTER
	, CHOICE_RESTART
	, CHOICE_OPTIONS
	, CHOICE_TRACEMODE

	, CHOICE_NONE = -1
} eMenuChoice;
#define ESC	0x1b
#define ENTER 0x0d
#define CTLX 0x18


//
//The following functions execute VPserIO function calls
bool		GetSingle(bool);
bool		SingleTestLoop(int nGet=-1 );
bool		Connect();
void		ToggleTraceMode();
void		Disconnect();
void		AsciiMode();
bool		StartCont();
bool		StopCont();
bool		ContLoop(int nGet = -1);
bool		StartCont_A(int nGet = -1);
bool		ContLoop_A();

//The following functions parse and display tracker and cmdlib output
void		DisplayError(int32_t err, const TCHAR *szMsg);
void		DisplayNativeViperFrame(uint8_t * pBuf, uint32_t iSize);
void		DisplayBinaryLegacyFrame(uint8_t * pBuf, uint32_t iSize);
//The following functions display option menus and prompt for input
void		ShowMainMenu();
eMenuChoice MainPrompt();
eMenuChoice ConnectPrompt();
eMenuChoice TrackerPrompt();

int g_device_count = 0;
CVPserIO g_dev;
bool g_bTrace = false;
int g_tracelev = 3;


int main()
{
	g_dev.SetTrace(g_bTrace, g_tracelev);

	eMenuChoice eChoice = CHOICE_NONE;

	SetConsoleTitle(_T("VPserWinConsole"));
	if (TrackerPrompt() == CHOICE_QUIT)
	{
		eChoice = CHOICE_QUIT;
	}

	if (ConnectPrompt() == CHOICE_QUIT)
	{
		eChoice = CHOICE_QUIT;
	}
	else
	{
		ShowMainMenu();
		do
		{
			eChoice = MainPrompt();

			switch (eChoice)
			{
			case CHOICE_CONT_ASYNC:
				//Start Cont Mode Asyncronously.
				// Must use Binary frame and preamble to check frame at the interface
				if (!(StartCont_A()))
				{
				}
				else
				{
					//Collect/Display Tracker Data until ESC
					ContLoop_A();
					StopCont();
				}

				break;			
			case CHOICE_CONT:
				//Start Cont Mode ASCII: no frame checking at the interface.
				if (!(StartCont()))
				{
				}
				else
				{
					//Collect/Display Tracker Data until ESC
					ContLoop();
					StopCont();
				}

				break;

			case CHOICE_SINGLE:
				GetSingle(true);
				break;

			case CHOICE_SINGLE_LOOP:
				SingleTestLoop();
				break;

			case CHOICE_TRACEMODE:
				ToggleTraceMode();
				break;

			case CHOICE_OPTIONS:
				//OptionsPrompt();
				break;

			case CHOICE_ENTER:
				ShowMainMenu();
				break;

			case CHOICE_RESTART:
				eChoice = ConnectPrompt();
				break;

			case CHOICE_DISCONNECT:
				Disconnect();
				break;

			case CHOICE_ASCII:
				AsciiMode();
				break;

			default:
				break;
			}

		} while (eChoice != CHOICE_QUIT);

		Disconnect();

	}

	return 0;
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void ShowMainMenu()
{
	tcout << _T("\n+++Main Menu: \n\n");
	tcout << _T("A or a:  Enter ASCII mode\n");
	tcout << _T("D or d:  Disconnect\n");
	tcout << _T("L or l:  Single frame capture, P cmd loop\n");
	tcout << _T("P or p:  Get Single Frame\n");
	tcout << _T("C or c:  Continuous mode - Synchronous, Ascii\n");
	tcout << _T("K or k:  Continuous mode - Asynchronous, binary (uses callback) (will not work with FT)\n");
	tcout << _T("T or t:  Toggle Trace mode \n");
	tcout << endl;
	tcout << _T("ESC:     Quit\n");
}
/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
eMenuChoice MainPrompt()
{
	eMenuChoice eRet = CHOICE_NONE;
	INT			ch;

	do
	{
		tcout << _T("\nAa//Dd//Ll//Pp//Cc//Kk//Tt//ESC>> ");
		ch = _gettche();
		ch = toupper(ch);

		switch (ch)
		{
		case 'A':
			eRet = CHOICE_ASCII;
			break;
		case 'D':
			eRet = CHOICE_DISCONNECT;
			break;
		case 'C':
			eRet = CHOICE_CONT;
			break;
		case 'K':
			eRet = CHOICE_CONT_ASYNC;
			break;
		case 'P':
			eRet = CHOICE_SINGLE;
			cout << endl;
			break;
		case 'L':
			eRet = CHOICE_SINGLE_LOOP;
			break;
		case 'O':
			eRet = CHOICE_OPTIONS;
			break;
		case 'R':
			eRet = CHOICE_RESTART;
			break;
		case 'T':
			eRet = CHOICE_TRACEMODE;
			break;
		case ENTER:
			eRet = CHOICE_ENTER;
			break;
		case ESC: // ESC
			eRet = CHOICE_QUIT;
			break;
		default:
			break;
		}
	} while (eRet == CHOICE_NONE);

	return eRet;
}

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
//uint16_t g_vid = POLHEMUS_VID;
//uint16_t g_pid = TEST_PID;
//TCHAR * g_vid_name = POLHEMUS_MFR_STR;
//TCHAR * g_pid_name = TEST_DEV_STR;
/**
*  Command frame preamble. All command frames begin with 'V''P''R''C'.
*/
#define VIPER_CMD_PREAMBLE 0x43525056
// 'V' 'P' 'R'
#define VIPER_PREAMBLE 0x00525056
#define LIBERTY_PREAMBLE   0x594C
#define PATRIOT_PREAMBLE   0x4150
#define FASTRAK_PREAMBLE   0x30
uint32_t g_preamble = VIPER_PREAMBLE;

enum {
	VIPER_TRACKER
	, LIBERTY_TRACKER
	, PATRIOT_TRACKER
	, FASTRAK_TRACKER

	, TRACKER_MAX

}eTracker;

TCHAR * g_szTrackerType[TRACKER_MAX] =
{
	_T("VIPER")
	,_T("LIBERTY")
	,_T("PATRIOT")
	,_T("FASTRAK")
};
char * g_TrackerStartContCmd[TRACKER_MAX] =
{
	"C"
	,"C\r"
	,"C\r"
	,"C"
};
char * g_TrackerStopContCmd[TRACKER_MAX] =
{
	"P"
	,"p"
	,"p"
	,"c"
};
char * g_TrackerSetAsciiCmd[TRACKER_MAX] =
{
	""
	,"F0\r"
	,"F0\r"
	,"F"
};
char * g_TrackerSetBinaryCmd[TRACKER_MAX] =
{
	""
	,"F1\r"
	,"F1\r"
	,"f"
};
int32_t g_TrackerType = VIPER_TRACKER;
int32_t g_PortSelection = 1;
eMenuChoice ConnectPrompt()
{
	BOOL bQuit = FALSE;
	eMenuChoice eChoice = CHOICE_NONE;
	INT ch;

	vp_serportinfo *portlist = 0;
	size_t portcount = 0;

	size_t count;
	int r = 0;
	tstring strlist;

	if (r = g_dev.ListSerPorts(0, portcount))
	{
		DisplayError(r, _T("ListSerPorts failed with error "));
	}
	else if (portcount != 0)
	{
		count = portcount;
		portlist = new vp_serportinfo[++count];

		if (r = g_dev.ListSerPorts(portlist, count))
		{
			DisplayError(r, _T("ListSerPorts failed with error "));
		}
		else
		{
			g_PortSelection = portlist[0].serPort;

			//g_dev.PortListReport(strlist);
			
			for (int i = 0; i < (int)count; i++)
			{
				tstringstream ss;
				//ss.str(s);
				ss << i << " : port : " << portlist[i].serPort << " : " << portlist[i].strPortName;
				ss << " : " << portlist[i].strFriendlyName;
				strlist += ss.str();

				//tstring tmp;
				//PortReport(list[i], tmp);
				strlist += _T("\r\n");
			}
		}
	}

	if (r)
	{
		delete portlist;
		return CHOICE_NONE;
	}

	tcout << _T("Commencing VP serial Discovery when ready.\n") << endl;
	tcout << _T("Serial ports:") << endl << strlist.c_str() << endl;
	tcout << _T("Default port selection is ") << g_PortSelection << _T(".") << endl;
	tcout << _T("To select different port, type the port number, or") << endl;
	tcout << _T("L or l:  List Serial Ports available. ") << endl;
	tcout << _T("ENTER :  Continue with connection on port ") << g_PortSelection << _T(".") << endl;
	tcout << _T("ESC   :  Abort\n") << endl;
	TCHAR *szPrompt = _T("ENTER//ESC/L/[1-9]>> ");
	tcout << szPrompt;

	uint32_t preamble = g_preamble;
	bool bLY = false;
	do
	{
		ch = _gettche();
		if (ch == ESC)
		{
			eChoice = CHOICE_QUIT;
			break;
		}

		else if ((ch == 'L') || (ch == 'l'))
		{
			tstring s;
			g_dev.PortListReport(s);
			tcout << endl;
			tcout << s.c_str();
			tcout << endl;
		}
		else if (isdigit(ch))
		{
			int tmpindex = ch - '0';
			int tmpport = portlist[tmpindex].serPort;
			tcout << endl << _T("You selected port index ") << tmpindex << _T(", COM") << tmpport << endl;
			g_PortSelection = tmpport;
		}
		else if (ch == ENTER)
		{
			eChoice = CHOICE_ENTER;
			break;
		}

		tcout << szPrompt;

	} while (ch != ENTER);

	if (portlist)
		delete[] portlist;

	if (eChoice == CHOICE_ENTER)
	{
		Connect();

		// set mode after successful connection.
		if (g_TrackerType != VIPER_TRACKER)
		{
			g_dev.SetModeLY();
		}
		g_dev.FramePreamble() = g_preamble;

	}
	return eChoice;
}

bool Connect()
{
	bool bRet = false;
	int  r = 0;
	tstring str;

	if (g_dev.IsOpen())
	{
		g_dev.DevReport(str);
		std::cout << "VPserIO object already connected to " << str.c_str() << endl;
	}
	//else if (r = g_dev.DiscoverPort())
	else if (r = g_dev.Open(g_PortSelection))
	{
		DisplayError(r, _T("Open port 5 failed with error "));
	}
	else
	{
		tstring s; g_dev.DevReport(s);
		str = _T(" DiscoverPort : ");
		str += s;
		bRet = true;
		DisplayError(r, str.c_str());
	}

	return bRet;
}

eMenuChoice TrackerPrompt()
{
	BOOL bQuit = FALSE;
	eMenuChoice eChoice = CHOICE_NONE;
	INT ch;

	tcout << _T("Default tracker is ");
	tcout << g_szTrackerType[g_TrackerType];
	tcout << _T(". To change, Select:\n") << endl;
	tcout << _T("L or l:  Liberty. ") << endl;
	tcout << _T("P or p:  Patriot. ") << endl;
	tcout << _T("F or f:  Fastrak. ") << endl;
	tcout << _T("ENTER :  Continue with selection\n");
	tcout << _T("ESC   :  Abort\n") << endl;
	TCHAR * szPrompt = _T("ENTER//ESC/Ll/Pp/Ff>> ");
	tcout << szPrompt;

	uint32_t preamble = g_preamble;
	bool bLY = false;
	do
	{
		ch = _gettche();
		if (ch == ESC)
			return CHOICE_NONE;

		else if ((ch == 'L') || (ch == 'l'))
		{
			g_preamble = LIBERTY_PREAMBLE;
			g_TrackerType = LIBERTY_TRACKER;
			tcout << endl << g_szTrackerType[g_TrackerType] << _T(" Selected.") << endl;
			bLY = true;
			eChoice = CHOICE_ENTER;
			break;
		}
		else if ((ch == 'P') || (ch == 'p'))
		{
			g_preamble = PATRIOT_PREAMBLE;
			g_TrackerType = PATRIOT_TRACKER;
			tcout << endl << g_szTrackerType[g_TrackerType] << _T(" Selected.") << endl;
			bLY = true;
			eChoice = CHOICE_ENTER;
			break;
		}
		else if ((ch == 'F') || (ch == 'f'))
		{
			// Can't really use preamble with Fastrak..
			g_preamble = FASTRAK_PREAMBLE;
			g_TrackerType = FASTRAK_TRACKER;
			tcout << endl << g_szTrackerType[g_TrackerType] << _T(" Selected.") << endl;
			bLY = true;
			eChoice = CHOICE_ENTER;
			break;
		}

		else if (ch == ENTER)
		{
			eChoice = CHOICE_ENTER;
			break;
		}

		else
		{
			tcout << _T("Invalid selection") << endl;
		}

		tcout << szPrompt;
	} while (ch != ENTER);

	if (bLY)
	{
		g_dev.SetModeLY();
		g_dev.FramePreamble() = g_preamble;

	}

	return eChoice;

}
void ToggleTraceMode()
{
	g_dev.SetTrace(!g_bTrace, g_tracelev);
	g_bTrace = !g_bTrace;

	cout << "Trace Mode " << (g_bTrace ? "Enabled" : "Disabled") << endl << endl;
}

void Disconnect()
{
	tstring str;
	if (g_dev.IsOpen())
	{
		g_dev.DevReport(str);
		tcout << _T("Disconnecting : ") << str.c_str() << endl;
		int r = g_dev.Close();
		DisplayError(r, _T("Close() "));
	}
	else
	{
		tcout << _T("Disconnect : Device not open!") << endl;
	}
	
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
enum
{
	ASC_MODE_LIB
	, ASC_MODE_FT
};
int g_AscMode = 0;
void AsciiOptionsMenu()
{
	tcout << _T("\n+++Ascii Options Menu:\n\n");
	//tcout << _T("T or t:  Select Read Timeout. Current (ms): ") << g_dev.ReadCmdTO() << endl;
	tcout << _T("L or l:  Lib/Pat Ascii mode: Enter key include in most commands.  Current value is ");
	tcout << (g_AscMode ? _T("FASTRAK") : _T("LIBERTY")) << endl;
	tcout << _T("F or f:  FasTrak ASCII mode: Enter key included where required by Fastrak.") << endl;
	tcout << _T("ESC   :  Exit Ascii Options Menu\n");

}
void AsciiOptionsPrompt()
{
	AsciiOptionsMenu();
	bool bExit = false;
	do
	{
		INT ch;

		tcout << _T("\nTt//ESC>> ");
		ch = _gettche();
		ch = toupper(ch);

		switch (ch)
		{
		case ESC:
			bExit = true;
			break;
		case ENTER:
			AsciiOptionsMenu();
			break;
		case 'L':
			g_AscMode = ASC_MODE_LIB;
			tcout << _T("Ascii Mode set to ");
			tcout << (g_AscMode ? _T("FASTRAK") : _T("LIBERTY")) << endl;
			break;
		case 'F':
			g_AscMode = ASC_MODE_FT;
			tcout << _T("Ascii Mode set to ");
			tcout << (g_AscMode ? _T("FASTRAK") : _T("LIBERTY")) << endl;
			break;
		//case 'T':
		//{
		//	int ch;
		//	TCHAR input[20];
		//	tcin.getline(input, 20);
		//	if (tcslen(input) == 0) // just Enter was typed
		//		ch = ENTER;
		//	else
		//	{
		//		int tin;
		//		tstring strinput(input);
		//		tstringstream(strinput) >> tin;

		//		tcout << _T("You selected ") << tin << _T("milliseconds read timeout.") << endl;
		//		g_dev.ReadCmdTO() = tin;
		//		bExit = true;
		//	}
		//}
		//break;
		default:
			tcout << _T("\nUnsupported Selection\n");
			break;
		}

	} while (bExit == FALSE);

}
void AsciiMenu()
{

	tcout << _T("\n+++Ascii Mode: \n\n");
	tcout << _T("At the prompt, type an ASCII character or string command to be sent to the device.\n");
	tcout << _T("Terminate the command with the ENTER key.\r\n");
	tcout << _T("When the ENTER key is typed, the command will be sent to the device.\n");
	//tcout << _T("A device read with a timeout of ") << g_dev.ReadCmdTO() << _T("ms will begin.");
	tcout << _T("Any response received will be displayed\n");
	tcout << _T("Any error condition will exit Ascii Mode\n");
	tcout << _T("A read timeout is not treated as an error.\n");
	tcout << _T("Timeout duration can be configured from the Ascii Mode Options menu\n") << endl;
	tcout << _T("To exit Ascii Mode, type the Ctrl-X.\n\n") << endl;
	tcout << _T("ENTER  :  Proceed to Ascii Mode\n");
	tcout << _T("O or o :  Ascii Mode options\n");
	tcout << _T("ESC    :  Return to Main Menu\n");
	tcout << endl;

}
eMenuChoice AsciiPrompt()
{
	AsciiMenu();
	eMenuChoice eRet = CHOICE_NONE;
	INT			ch;

	do
	{
		tcout << _T("ENTER//Oo//ESC>> ");
		ch = _gettche();
		ch = toupper(ch);

		switch (ch)
		{
		case 'O':
			AsciiOptionsPrompt();
			eRet = CHOICE_NONE;
			break;
		case ENTER:
			eRet = CHOICE_ENTER;
			break;
		case ESC: // ESC
			eRet = CHOICE_QUIT;
			break;
		default:
			break;
		}
	} while (eRet == CHOICE_NONE);

	return eRet;
}
void AsciiMode()
{
	if (!g_dev.IsOpen())
	{
		std::cout << "Not connected" << endl;
		return;
	}

	eMenuChoice eChoice = AsciiPrompt();

	if (eChoice == CHOICE_QUIT)
		return;

	bool bExit = false;
	do
	{
		tcout << _T("\n>> ");
		int ch;
		char input[100];
		cin.getline(input, 100);
		ch = input[0];

		if (ch == CTLX)
		{
			bExit = true;
			break;
		}
		int count = strlen(input);

		if (g_AscMode == ASC_MODE_LIB)
		{
			if (toupper(input[0]) != 'P')
			{ 
				input[count] = ENTER;
				count++;
			}
		}
		else
		{
			switch (ch)
			{
			case 'C':
			case 'c':
			case 'D':
			case 'd':
			case 'F':
			case 'f':
			case 'P':
			case 'S':
			case 'U':
			case 'u':
			case 'W':
			case 0x0b: //^K
			case 0x11: //^Q
			case 0x13: //^X
			case 0x19: //^Y
				break;
			default:
				input[count] = ENTER;
				count++;
				break;
			}
		}
		
		int r = g_dev.Write((uint8_t*)input, count);
		if (r)
		{
			DisplayError(r, _T("Write : "));
			bExit = true;
			break;
		}
		char readbuf[200];
		int  readcount = 200;

		r = g_dev.Read((uint8_t*)readbuf, readcount);
		if (r)
		{
			DisplayError(r, _T("Read Error : "));
			bExit = true;
		}
		readbuf[readcount] = 0;
		cout << endl << readbuf << endl;

	} while (!bExit);

	tcout << _T("\nLeaving Ascii Mode..\n");
}
const char *g_pCmdAsciiStartCont = "C\r";
const char *g_pCmdAsciiStopCont = "P";
const char *g_pCmdSetAscii = "F0\r";
const char *g_pCmdSetBinary = "F1\r";

typedef struct _SEUCMD_HDR
{
	uint32_t preamble;
	uint32_t size;
}SEUCMD_HDR;
typedef struct _cmdstruct {
	SEUCMD_HDR hdr;
	uint32_t seuid;
	uint32_t cmd;
	uint32_t action;
	uint32_t arg1;
	uint32_t arg2;
	//cmd payload goes here, if any.
	uint32_t crc;
} cmdstruct;
cmdstruct g_Cmd;

bool StartContinuousCmd(bool bAscii)
{
	std::cout << "Entering StartContinuousCmd" << endl;

	bool bRet = false;
	int r = 0;
	int nBytes = 0;
	uint8_t *pbuf = 0;

	switch (g_TrackerType)
	{
	case LIBERTY_TRACKER:
	case PATRIOT_TRACKER:
	case FASTRAK_TRACKER:
	case VIPER_TRACKER:
		g_pCmdAsciiStartCont = g_TrackerStartContCmd[g_TrackerType];
		g_pCmdAsciiStopCont = g_TrackerStopContCmd[g_TrackerType];
		break;

	default:
		break;
	}

		nBytes = strlen(g_pCmdAsciiStartCont);
		pbuf = (uint8_t*)g_pCmdAsciiStartCont;
		g_dev.AsciiContMode() = bAscii;

	//if (bAscii)
	//{
	//	
	//}
	//else
	//{
	//	nBytes = sizeof(cmdstruct);
	//	pbuf = (uint8_t*)&g_Cmd;
	//}

	if (r = g_dev.Write(pbuf, nBytes))
	{
		DisplayError(r, _T("Write CmdAsciiStartCont failed with error "));
	}
	else
		bRet = true;

	std::cout << "Leaving StartContinuousCmd" << endl;
	return bRet;
}

bool StopContinuousCmd(bool bAscii)
{
	std::cout << "Entering StopContinuousCmd bAscii = " << (bAscii ? "true" : "false") << endl;
	bool bRet = false;
	int r = 0;
	int nBytes = 0;
	uint8_t *pbuf = 0;

	if (bAscii)
	{
		nBytes = strlen(g_pCmdAsciiStopCont);
		pbuf = (uint8_t*)g_pCmdAsciiStopCont;
	}
	else
	{
		nBytes = sizeof(cmdstruct);
		pbuf = (uint8_t*)&g_Cmd;
	}
	if (r = g_dev.Write(pbuf, nBytes))
	{
		DisplayError(r, _T("Write CmdAsciiStopCont failed with error "));
	}
	else
		bRet = true;

	std::cout << "Leaving StopContinuousCmd" << endl;
	return bRet;
}

bool SetAsciiCmd()
{
	if (g_TrackerType == VIPER_TRACKER)
		return true;

	std::cout << "Entering SetAsciiCmd" << endl;

	bool bRet = false;
	int r = 0;
	int nBytes = 0;
	uint8_t *pbuf = 0;

	switch (g_TrackerType)
	{
	case LIBERTY_TRACKER:
	case PATRIOT_TRACKER:
	case FASTRAK_TRACKER:
		g_pCmdSetAscii = g_TrackerSetAsciiCmd[g_TrackerType];
		break;
	case VIPER_TRACKER:

	default:
		break;
	}

	nBytes = strlen(g_pCmdSetAscii);
	pbuf = (uint8_t*)g_pCmdSetAscii;

	if (r = g_dev.Write(pbuf, nBytes))
	{
		DisplayError(r, _T("Write SetAsciiCmd failed with error "));
	}
	else
		bRet = true;

	std::cout << "Leaving SetAsciiCmd" << endl;
	return bRet;
}
bool SetBinaryCmd()
{
	if (g_TrackerType == VIPER_TRACKER)
		return true;
	std::cout << "Entering SetBinaryCmd" << endl;

	bool bRet = false;
	int r = 0;
	int nBytes = 0;
	uint8_t *pbuf = 0;

	switch (g_TrackerType)
	{
	case LIBERTY_TRACKER:
	case PATRIOT_TRACKER:
	case FASTRAK_TRACKER:
		g_pCmdSetBinary = g_TrackerSetBinaryCmd[g_TrackerType];
		break;
	case VIPER_TRACKER:

	default:
		break;
	}

	nBytes = strlen(g_pCmdSetBinary);
	pbuf = (uint8_t*)g_pCmdSetBinary;

	if (r = g_dev.Write(pbuf, nBytes))
	{
		DisplayError(r, _T("Write SetBinaryCmd failed with error "));
	}
	else
		bRet = true;

	std::cout << "Leaving SetBinaryCmd" << endl;
	return bRet;
}

bool StartCont()
{
	bool bRet = false;
	int r = 0;

	if (!(SetAsciiCmd()))
	{
	}
	else if (!StartContinuousCmd(true))
	{
	}
	else if (r = g_dev.ReadCont())
	{
		DisplayError(r, _T("ReadCont failed with error "));
	}
	else
		bRet = true;

	return bRet;
}

bool StopCont()
{
	bool bRet = false;
	int r = 0;

	if (!StopContinuousCmd(true))
	{
	}
	else if (r = g_dev.ReadContStop())
	{
		DisplayError(r, _T("ReadContStop failed with error "));
	}
	else
		bRet = true;

	return bRet;
}

bool g_bAsyncRunning = false;
int  g_nReads = 0;
int  g_nCaptured = 0;
bool g_printing = false;
char capbuf[500];

bool ContLoop(int nGet /*=-1*/)
{
	if (nGet < 0)
	{
		std::cout << "\nPress any key to start continuous display\r\n";
		std::cout << "\nPress ESC to stop.\r\n";
		std::cout << "\nReady? ";
		_gettche();
		std::cout << std::endl;
	}

	bool bRet = true;
	bool bExit = false;
	int32_t r;
	int nCaptured = 0;
	int nReads = 0;

	do {

		if ((nGet > 0) && (nReads > 500 * nGet))
		{
			bRet = false;
			break;
		}

		//g4pFrameInfo *pFrames = 0;	///pointer to frame structs
		//int32_t iCount = 0;			///num frames grabbed

		uint8_t * pbuf = 0;
		int nBytes = 0;

		if (r = g_dev.LastAsyncRead(pbuf, nBytes))
		{
			DisplayError(r, _T("LastAsyncRead failed with error "));
			bRet = false;
		}
		else
		{
			nReads++;
			if (nBytes && pbuf)
			{
				nCaptured++;
				memcpy(capbuf, pbuf, min(nBytes, 499));
				capbuf[min(nBytes, 499)] = 0;

				std::cout << "Capture " << nCaptured << " : " << capbuf << endl;
			}
			else
				std::cout << " .";

		}

		if (nGet >= 0)
			bExit = (nCaptured >= nGet);

		if (_kbhit() && (_gettche() == ESC))
		{
			bExit = true;
		}

	} while (!bExit);

	return bRet;
}

void CALLBACK AsyncDataReady(void *pInfo, uint8_t *pdata, uint32_t nbytes)
{
	if (g_bTrace)
		std::cout << "Entering AsyncDataReady, g_printing = " << (g_printing ? "true" : "false") << endl;
	int nGet = (int)pInfo;

	if (g_printing)
	{
		g_nReads++;
		if (nbytes && pdata)
		{
			g_nCaptured++;
			memcpy(capbuf, pdata, min(nbytes, 499));
			capbuf[min(nbytes, 499)] = 0;

			switch (g_TrackerType)
			{
			case LIBERTY_TRACKER:
			case PATRIOT_TRACKER:
			case FASTRAK_TRACKER:
				//std::cout << capbuf << endl;
				DisplayBinaryLegacyFrame(pdata, nbytes);
				break;
			case VIPER_TRACKER:
				DisplayNativeViperFrame(pdata, nbytes);
				break;
			default:
				break;
			}
		}
		else
			std::cout << " .";

		if ((nGet > 0) && (g_nReads > 500 * nGet))
		{
			g_bAsyncRunning = false;
		}
	}
}

//asynchronous, using binary header frame

bool StartCont_A(int nGet /*=-1*/)
{
	std::cout << "Entering StartCont_A" << std::endl;

	bool bRet = false;
	int r = 0;

	if (!SetBinaryCmd())
	{
	}
	else if (!StartContinuousCmd(false))
	{
	}
	else
	{
		g_dev.SetMsgCallback(AsyncDataReady, (void *)nGet);
		if (r = g_dev.ReadCont())
		{
			DisplayError(r, _T("ReadCont failed with error "));
		}
		else
		{
			g_bAsyncRunning = true;
			g_nReads = 0;
			g_printing = false;
			g_nCaptured = 0;
			bRet = true;
		}
	}

	return bRet;
}


bool ContLoop_A()
{
	std::cout << "Entering ContLoop_A" << std::endl;
	//if (nGet < 0)
	{
		std::cout << "\nPress any key to start continuous display\r\n";
		std::cout << "\nPress ESC to stop.\r\n";
		std::cout << "\nReady? ";
		_gettche();
		std::cout << std::endl;
		g_printing = true;
	}

	bool bRet = true;
	bool bExit = false;
	//int32_t r;
	//int nCaptured = 0;
	//int nReads = 0;

	do {

		if (!g_bAsyncRunning)
		{
			std::cout << "ContLoop_A g_bAsyncRunning = false" << endl;
			bRet = false;
			break;
		}

		if (_kbhit() && (_gettche() == ESC))
		{
			bExit = true;
		}

	} while (!bExit);

	g_printing = false;

	std::cout << "Leaving ContLoop_A g_printing = false" << endl;
	return bRet;
}

const char *g_pCmdAsciiSingleCmd = "P";
char * g_TrackerSingleCmd[TRACKER_MAX] =
{
	"P"
	,"P"
	,"P"
	,"P"
};

int AsciiSingleCmd()
{
	//std::cout << "Entering AsciiSingleCmd" << endl;

	int r = 0;
	int nBytes = 0;
	uint8_t *pbuf = 0;

	nBytes = strlen(g_pCmdAsciiSingleCmd);
	pbuf = (uint8_t*)g_pCmdAsciiSingleCmd;

	if (r = g_dev.Write(pbuf, nBytes))
	{
		DisplayError(r, _T("Write AsciiSingleCmd failed with error "));
	}

	//std::cout << "Leaving AsciiSingleCmd" << endl;
	return r;
}

// currently only displays ascii output
bool GetSingle(bool bAscii)
{
	//std::cout << "Entering GetSingle" << endl;
	bool bRet = false;

	int r = 0;
	uint8_t readbuf[200];
	int nBytes = 200;

	if (r = AsciiSingleCmd())
	{
	}
	else if (r = g_dev.Read((uint8_t*)readbuf, nBytes))
	{
		DisplayError(r, _T("Read failed with error "));
	}
	else if (nBytes)
	{
		memcpy(capbuf, readbuf, min(nBytes, 499));
		capbuf[min(nBytes, 499)] = 0;

		switch (g_TrackerType)
		{
		case LIBERTY_TRACKER:
		case PATRIOT_TRACKER:
		case FASTRAK_TRACKER:
			std::cout << capbuf << endl;
			break;
		case VIPER_TRACKER:
			DisplayNativeViperFrame(readbuf, nBytes);
			break;
		default:
			break;
		}		

		bRet = true;
	}
	else
		cout << " . " << endl;

	return bRet;
}

bool SingleTestLoop(int nGet /*=-1*/)
{
	bool bRet = true;
	bool bExit = false;
	int nCaptured = 0;
	int nReads = 0;

	if (nGet < 0)
	{
		if (g_TrackerType == VIPER_TRACKER)
		{
			std::cout << "\nPress any key to start viper single P loop \r\n";
		}
		else
		{
			std::cout << "\nPress any key to start legacy single P loop \r\n";
		}

		std::cout << "\nPress ESC to stop.\r\n";
		std::cout << "\nReady? ";
		_gettche();
		std::cout << std::endl;
	}

	do {

		if ((nGet > 0) && (nReads > 500 * nGet))
		{
			bRet = false;
			break;
		}

		if (GetSingle(true))
		{
			nCaptured++;
		}

		if (nGet >= 0)
			bExit = (nCaptured >= nGet);

		if (_kbhit() && (_gettche() == ESC))
		{
			bExit = true;
		}

	} while (!bExit);

	return bRet;
}

bool SingleLegacyTestLoop(int nGet /*=-1*/)
{
	if (nGet < 0)
	{
		std::cout << "\nPress any key to start legacy single P loop \r\n";
		std::cout << "\nPress ESC to stop.\r\n";
		std::cout << "\nReady? ";
		_gettche();
		std::cout << std::endl;
	}

	return SingleTestLoop(nGet);
}

bool SingleViperTestLoop(int nGet /*=-1*/)
{
	if (nGet < 0)
	{
		std::cout << "\nPress any key to start viper single P loop \r\n";
		std::cout << "\nPress ESC to stop.\r\n";
		std::cout << "\nReady? ";
		_gettche();
		std::cout << std::endl;
	}

	return SingleTestLoop(nGet);
}

bool TempTestLoop(int nGet)
{
	bool bRet = true;
	bool bExit = false;
	int32_t r;
	int nCaptured = 0;
	int nReads = 0;

	do {

		if ((nGet > 0) && (nReads > 500 * nGet))
		{
			bRet = false;
			break;
		}

		//g4pFrameInfo *pFrames = 0;	///pointer to frame structs
		//int32_t iCount = 0;			///num frames grabbed

		uint8_t readbuf[200];
		int nBytes = 200;

		if (r = AsciiSingleCmd())
		{	}
		else
		if (r = g_dev.Read((uint8_t*)readbuf, nBytes))
		{
			DisplayError(r, _T("Read failed with error "));
			bRet = false;
		}
		else
		{
			nReads++;
			if (nBytes)
			{
				nCaptured++;
				memcpy(capbuf, readbuf, min(nBytes, 499));
				capbuf[min(nBytes, 499)] = 0;

				DisplayNativeViperFrame(readbuf, nBytes);
			}
			else
				std::cout << " .";

		}

		if (nGet >= 0)
			bExit = (nCaptured >= nGet);

		if (_kbhit() && (_gettche() == ESC))
		{
			bExit = true;
		}

	} while (!bExit);

	return bRet;
}

//////////////////////////////////////
////////////////////////////////////// pasted from ViperInterface.h
typedef struct _SEUPNO {
	uint32_t seuid;
	uint32_t frame;
	/**
	*  Each seu has 8 bits of digital IO which may have changed and also maintains a source lock.
	*   This structure contains information signaling the states of these items.
	*  byte(s)   | name       | format         | description
	*  ----------|------------|----------------|-----------------------------------------------
	*   0        | bfDig_IO   | integer        | Each bit in this byte signals an on(1)/off(0) state.
	*   1        | bfSrcLock  | integer        | A seu may go out of range of any source, which is signaled here.
	*   2-3      | bfReserved | raw            | Reserved.
	*/
	struct HPinfo {
		uint32_t bfDig_IO : 8;
		uint32_t bfSrcLock : 8;
		uint32_t bfReserved : 16;
	} HPinfo;
	uint32_t sensorCount;
} SEUPNO, *LPSEUPNO;
/**
*  Every PNO frame begins with this structure. Following this structure is a number of SENFRAMEDATA structures,
*  determined by the number of sensors connected to the seu and specified in the sensorCount field of the
*  SENFRAMEDATA structure.
*  byte(s)   | name     | format     | description
*  ----------|----------|------------|-----------------------------------------------
*  0-3       | preamble | integer    | A constant of value VPRLUC_PNO_PREAMBLE.
*  4-7       | size     | integer    | Total number of bytes to follow.
*  8-23      | seudata  | structure  | See SEUPNO.
*/
typedef struct _SEUPNO_HDR
{
	uint32_t preamble;
	uint32_t size;
	SEUPNO seupno;
}SEUPNO_HDR;
/**
*  This is the payload of a PNO frame. Each frame can have 0-3 sensor's worth of data and is therefor variable in size:
*  the size is 32 bytes (this accounts for the header at the beginning and CRC at the end) plus a multiple of 32 bytes
*  times the number of active sensors.
*  byte(s)   | name        | format         | description
*  ----------|-------------|----------------|-----------------------------------------------
*   0-3      | SFinfo      | bitfield       | Contains sensor information.
*   4-31     | pno         | structure      | Contains the sensors position and orientation.
*/
typedef struct _SENFRAMEDATA {
	/**
	*  Each sensor has its own number and they all share the same units. This structure specifies
	*  both values.
	*  byte(s)   | name       | format         | description
	*  ----------|------------|----------------|-----------------------------------------------
	*   0        | bfSnum     | integer        | The 0-based sensor number.
	*   1        | bfPosUnits | integer        | A constant enumeration of type eViperPosUnits.
	*   2        | bfOriUnits | integer        | A constant enumeration of type eViperOriUnits.
	*   3        | bfReserved | raw            | Reserved.
	*/
	struct SFinfo
	{
		uint32_t	bfSnum : 8;
		uint32_t	bfPosUnits : 4;
		uint32_t	bfOriUnits : 4;
		uint32_t	bfReserved : 16;
	} SFinfo;
	/**
	*  Position and orientation data is stored here. If the units are eulers, then the last 4 bytes
	*  are unused.
	*  byte(s)   | name       | format         | description
	*  ----------|------------|----------------|----------------------------------
	*   0-3      | pos[0]     | floating point | Position x.
	*   4-7      | pos[1]     | floating point | Position y.
	*   8-11     | pos[2]     | floating point | Position z.
	*   12-15    | ori[0]     | floating point | Quaternion spin or euler azimuth.
	*   16-19    | ori[1]     | floating point | Quaternion x or euler elevation.
	*   20-23    | ori[2]     | floating point | Quaternion y or euler roll.
	*   24-27    | ori[3]     | floating point | Quaternion z or not applicable.
	*/
	struct pno {
		float pos[3];
		float ori[4];
	} pno;
}SENFRAMEDATA;
typedef SEUPNO_HDR *LPSEUPNO_HDR;
typedef SENFRAMEDATA *LPSENFRAMEDATA;
typedef enum eViperOriUnits
{
	ORI_EULER_DEGREE = 0 /**< Euler angles in degrees. */
	, ORI_EULER_RADIAN   /**< Euler angles in radians. */
	, ORI_QUATERNION     /**< Quaternions. */

	, ORI_MAX
} eViperOriUnits;
//////////////////////////////////////
////////////////////////////////////// End viperinterface.h

void DisplayNativeViperFrame(uint8_t * pBuf, uint32_t iSize)
{
	std::string s;

	if ((!pBuf) || (!iSize))
	{
		char szMsg[300];
		sprintf_s(szMsg, _countof(szMsg), "%s : pBuf %p, iSize %d", __FUNCTION__, pBuf, iSize);
		s += szMsg;
		std::cout << (s.c_str()) << std::endl;
	}
	else
	{
		uint32_t i = 0;
		LPSEUPNO_HDR	pdevFrame;

		while (i < iSize)
		{
			pdevFrame = (LPSEUPNO_HDR)(&pBuf[i]);

			uint32_t	nSeuID = pdevFrame->seupno.seuid;// hubpno.hubid;
			uint32_t	nFrameNum = pdevFrame->seupno.frame;
			uint32_t	nSCount = pdevFrame->seupno.sensorCount;
			uint32_t	nDigIO = pdevFrame->seupno.HPinfo.bfDig_IO;

			i += sizeof(SEUPNO_HDR);

			char szData[300];
			for (uint32_t j = 0; j<nSCount; j++)
			{
				LPSENFRAMEDATA  pSD = (LPSENFRAMEDATA)&(pBuf[i]);

				if (pSD->SFinfo.bfOriUnits == ORI_QUATERNION)
				{
					sprintf_s(szData, _countof(szData), "%3d %3d |  %07d |  0x%04x |  % 7.3f % 7.3f % 7.3f | % 8.4f % 8.4f % 8.4f % 8.4f\r\n",
						nSeuID, pSD->SFinfo.bfSnum,
						nFrameNum, nDigIO,
						pSD->pno.pos[0], pSD->pno.pos[1], pSD->pno.pos[2],
						pSD->pno.ori[0], pSD->pno.ori[1], pSD->pno.ori[2], pSD->pno.ori[3]);
				}
				else
				{
					sprintf_s(szData, _countof(szData), "%3d %3d |  %07d |  0x%04x |  % 7.3f % 7.3f % 7.3f | % 8.3f % 8.3f % 8.3f\r\n",
						nSeuID, pSD->SFinfo.bfSnum,
						nFrameNum, nDigIO,
						pSD->pno.pos[0], pSD->pno.pos[1], pSD->pno.pos[2],
						pSD->pno.ori[0], pSD->pno.ori[1], pSD->pno.ori[2]);
				}
				s += szData;
				i += sizeof(SENFRAMEDATA);
				if (i > iSize) // error condition
					break;
			}
			i += sizeof(uint32_t); // crc

		} // end while isize
		std::cout << s.c_str();
	}
}

VOID DisplayBinaryLegacyFrame(uint8_t *  pBuf, uint32_t dwSize)
{
	TCHAR	szFrame[200];
	DWORD		i = 0;

	while (i<dwSize)
	{
		BYTE ucSensor = pBuf[i + 2];
		SHORT shSize = pBuf[i + 6];

		// skip rest of header
		i += 8;

		PFLOAT pPno = (PFLOAT)(&pBuf[i]);

		_sntprintf_s(szFrame, _countof(szFrame), _T("%2d   %+ 11.6f %+ 11.6f %+ 11.6f   %+ 11.6f %+ 11.6f %+ 11.6ff\r"),
			ucSensor, pPno[0], pPno[1], pPno[2], pPno[3], pPno[4], pPno[5]);
		tstring sF = tstring(szFrame);
		sF += tstring(_T("\n"));

		tcout << sF.c_str();

		i += shSize;
	}
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////
void DisplayError(int32_t err, const TCHAR *szMsg)
{
	tcout << szMsg << g_dev.ErrorStr(err);

	tcout << endl;
}