/////////////////////////////////////////////////////////////////////
// This open-source software sample is offered free of charge and without guarantee.
// Polhemus accepts no responsibility or liability for any use of this code.
/////////////////////////////////////////////////////////////////////
/* VSS $Header: /Viper/VPCmdIF/VPCmdIF/VPTrace.cpp 22    6/01/20 3:54p Suzanne $
*/
///@cond
/////////////////////////////////////////////////////////////////////
#if defined(_WIN32) || defined(__CYGWIN__) || defined(_WIN32_WCE)
#include <windows.h>
/*#if defined(interface)
#undef interface
#endif
*/
#if !defined(__CYGWIN__)
#include <winsock.h>
#endif
#endif
/////////////////////////////////////////////////////////////////////

#include "VPTracei.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <thread>
#include "VPcontext.h"
#include "VPdevIO.h"
#include "PDebug.h"
#include "CriticalSection.h"


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


/**
*  Global Trace state.
*/
bool g_bVPTrace = false;
eVPcmdTraceLevel g_eVPTraceLev = VPCMD_NOTRACE;


#define VP_TRACEBUFFER_SIZE 0x1000 //0x800
char g_szTraceBuffer[VP_TRACEBUFFER_SIZE] = "";

#ifndef STATETRACE
#define STATETRACE 	DOUT3(_T("%s : %s : %d\r\n"), __FILE__, __FUNCTION__, __LINE__)
#endif
//VC140
//inline uint32_t trTraceThIDu()
//{
//	//return (uint32_t)(std::this_thread::get_id());
//	return _Thrd_id();
//	//return _Thrd_current()._Id;
//}
//VC142:
inline std::thread::id trTraceThID()
{
	return (std::this_thread::get_id());
	//return _Thrd_id();
	//return _Thrd_current()._Id;
}

//const char * g_szActions[CMD_ACTION_MAX] =
//{
//	"CMD_ACTION_SET"
//	, "CMD_ACTION_GET"
//	, "CMDP_ACTION_RESET"
//	, "CMD_ACTION_ACK"
//	, "CMD_ACTION_NAK"
//  , "CMD_ACTION_NAK_WARNING"
//};
//const char * g_szCmds[CMD_MAX] =
//{
//	"CMD_HEMISPHERE"
//	, "CMD_FILTER"
//	, "CMD_TIP_OFFSET"
//	, "CMD_BORESIGHT"
//	, "CMD_INCREMENT"
//	, "CMD_FRAMERATE"
//	, "CMD_UNITS"
//	, "CMD_SRC_ROTATION"
//	, "CMD_SYNC_MODE"
//	, "CMD_STATION_MAP"
//	, "CMD_STYLUS"
//	, "CMD_SEUID"
//	, "CMD_DUAL_OUTPUT"
//	, "CMD_RS232_CONFIG"
//
//	, "CMD_CONTINUOUS_PNO"
//	, "CMD_SINGLE_PNO"
//	, "CMD_FRAME_COUNT"
//	, "CMD_PERSIST"
//	, "CMD_BIT"
//	, "CMD_WHOAMI"
//	, "CMD_INITIALIZE"
//	, "CMD_BLOCK_CFG"
//	, "CMD_SENSOR_WHOAMI"
//
//
//};
//
typedef struct _errinfo
{
	int err;
	char * szerr;
	wchar_t* wszerr;
}errinfo;
errinfo g_errinfo[VPCMDERR_MAX + 1] =
{ VPCMDERR_MAX, "VPCMDERR_MAX", L"VPCMDERR_MAX" };

void InitErrInfo()
{
	if (g_errinfo[0].err != VPCMDERR_MAX)
		return;

	for (int i = 0; i <= VPCMDERR_MAX; i++)
	{
		switch (i)
		{
		default:
			g_errinfo[i] = { VPCMDERR_MAX, "VP_ERR NOT DEFINED", L"VP_ERR NOT DEFINED" };
			break;
		case E_VP_SUCCESS: // = 0           /**< 0, No error occured. */
			g_errinfo[i] = { E_VP_SUCCESS, "E_VP_SUCCESS", L"E_VP_SUCCESS" };
			break;
		case E_VPERR_NAK_RESP: //             /**< 1, Tracker rejected the command. */
			g_errinfo[i] = { E_VPERR_NAK_RESP, "E_VPERR_NAK_RESP", L"E_VPERR_NAK_RESP" };
			break;
		case E_VPERR_NAKWARN_RESP: //             /**< 2, Tracker responded with NAK_WARNING. */
			g_errinfo[i] = { E_VPERR_NAKWARN_RESP, "E_VPERR_NAKWARN_RESP", L"E_VPERR_NAKWARN_RESP" };
			break;
		case E_VPERR_MEMORY_ALLOCATION: //    /**< 3, Memory could not be allocated. */
			g_errinfo[i] = { E_VPERR_MEMORY_ALLOCATION, "E_VPERR_MEMORY_ALLOCATION", L"E_VPERR_MEMORY_ALLOCATION" };
			break;
		case E_VPERR_INVALID_CONTEXT: //      /**< 4, The specified context is invalid. */
			g_errinfo[i] = { E_VPERR_INVALID_CONTEXT, "E_VPERR_INVALID_CONTEXT", L"E_VPERR_INVALID_CONTEXT" };
			break;
		case E_VPERR_INVALID_ACTION: //       /**< 5, The specified action is invalid. */
			g_errinfo[i] = { E_VPERR_INVALID_ACTION, "E_VPERR_INVALID_ACTION", L"E_VPERR_INVALID_ACTION" };
			break;
		case E_VPERR_INVALID_SENSOR: //       /**< 6, The specified sensor is invalid. */
			g_errinfo[i] = { E_VPERR_INVALID_SENSOR, "E_VPERR_INVALID_SENSOR", L"E_VPERR_INVALID_SENSOR" };
			break;
		case E_VPERR_INVALID_SENSOR_ACT: //   /**< 7, The specified sensor and action combination is invalid.*/
			g_errinfo[i] = { E_VPERR_INVALID_SENSOR_ACT, "E_VPERR_INVALID_SENSOR_ACT", L"E_VPERR_INVALID_SENSOR_ACT" };
			break;
		case E_VPERR_INVALID_PARAMS: //       /**< 8, An additional specified parameter is invalid. */
			g_errinfo[i] = { E_VPERR_INVALID_PARAMS, "E_VPERR_INVALID_PARAMS", L"E_VPERR_INVALID_PARAMS" };
			break;
		case E_VPERR_INVALID_SIZE: //         /**< 9, The size field of the received frame is invalid. */
			g_errinfo[i] = { E_VPERR_INVALID_SIZE, "E_VPERR_INVALID_SIZE", L"E_VPERR_INVALID_SIZE" };
			break;
		case E_VPERR_INVALID_CRC: //          /**< 10, The CRC of the received frame is invalid */
			g_errinfo[i] = { E_VPERR_INVALID_CRC, "E_VPERR_INVALID_CRC", L"E_VPERR_INVALID_CRC" };
			break;
		case E_VPERR_TIMEOUT: //              /**< 11, A timeout was reached before all the data could be recieved. */
			g_errinfo[i] = { E_VPERR_TIMEOUT, "E_VPERR_TIMEOUT", L"E_VPERR_TIMEOUT" };
			break;
		case E_VPERR_PLATFORM: //             /**< 12, Indicates a system error in hi bits. */
			g_errinfo[i] = { E_VPERR_PLATFORM, "E_VPERR_PLATFORM", L"E_VPERR_PLATFORM" };
			break;
		case E_VPERR_LIBUSB: // 			  /**< 13, Indicates an error received from libusb library in high bits */
			g_errinfo[i] = { E_VPERR_LIBUSB, "E_VPERR_LIBUSB", L"E_VPERR_LIBUSB" };
			break;
		case E_VPERR_UNKNOWN: //              /**< 14, The error is unknown. */
			g_errinfo[i] = { E_VPERR_UNKNOWN, "E_VPERR_UNKNOWN", L"E_VPERR_UNKNOWN" };
			break;
		case E_VPERR_NOTFOUND: // 			  /**< 15, Device not found */
			g_errinfo[i] = { E_VPERR_NOTFOUND, "E_VPERR_NOTFOUND", L"E_VPERR_NOTFOUND" };
			break;
		case E_VPERR_SERIO:				  /**< 16, Error received from VPserIO library in high bits */
			g_errinfo[i] = { E_VPERR_SERIO, "E_VPERR_SERIO", L"E_VPERR_SERIO" };
			break;
		case E_VPERR_CMDRSP_MISMATCH:		  /**< 17, Command-Response mismatch */
			g_errinfo[i] = { E_VPERR_CMDRSP_MISMATCH, "E_VPERR_CMDRSP_MISMATCH", L"E_VPERR_CMDRSP_MISMATCH" };
			break;
		case E_VPERR_BUF_TOO_SMALL:			  /**< 18, Supplied buffer is too small */
			g_errinfo[i] = { E_VPERR_BUF_TOO_SMALL, "E_VPERR_BUF_TOO_SMALL", L"E_VPERR_BUF_TOO_SMALL" };
			break;
		case E_VPERR_NOT_IMPLEMENTED:		  /**< 19, Cmd or operation not implemented */
			g_errinfo[i] = { E_VPERR_NOT_IMPLEMENTED, "E_VPERR_NOT_IMPLEMENTED", L"E_VPERR_NOT_IMPLEMENTED" };
			break;


				//case E_VPERR_MAX/**< 15, Not an actual error, marks the end of the error list. */
		case E_CTXERR_INVALID_DEVHND: //  = E_VPERR_MAX
			g_errinfo[i] = { E_CTXERR_INVALID_DEVHND, "E_CTXERR_INVALID_DEVHND Invalid Device Handle", L"E_CTXERR_INVALID_DEVHND Invalid Device Handle" };
			break;
		case E_CTXERR_INVALID_CONDITION: // 
			g_errinfo[i] = { E_CTXERR_INVALID_CONDITION, "E_CTXERR_INVALID_CONDITION", L"E_CTXERR_INVALID_CONDITION" };
			break;
		case E_CTXERR_MEMALLOC_ERROR: // 
			g_errinfo[i] = { E_CTXERR_MEMALLOC_ERROR, "E_CTXERR_MEMALLOC_ERROR", L"E_CTXERR_MEMALLOC_ERROR" };
			break;
		case E_CTXERR_DEV_RECV_QUEUE_EMPTY: // 
			g_errinfo[i] = { E_CTXERR_DEV_RECV_QUEUE_EMPTY, "E_CTXERR_DEV_RECV_QUEUE_EMPTY (NO RESPONSE DETECTED)", L"E_CTXERR_DEV_RECV_QUEUE_EMPTY (NO RESPONSE DETECTED)" };
			break;
		case E_CTXERR_NO_MORE_DATA: // 
			g_errinfo[i] = { E_CTXERR_NO_MORE_DATA, "E_CTXERR_NO_MORE_DATA  Incoming p&o data queue empty", L"E_CTXERR_NO_MORE_DATA  Incoming p&o data queue empty" };
			break;

				//, E_CTXERR_MAX
				//E_DEVIOERR_NONE
		case E_DEVIOERR_NO_CNX: //  = E_CTXERR_MAX
			g_errinfo[i] = { E_DEVIOERR_NO_CNX, "E_DEVIOERR_NO_CNX", L"E_DEVIOERR_NO_CNX" };
			break;
		case E_DEVIOERR_FRAME_HEADER: // 
			g_errinfo[i] = { E_DEVIOERR_FRAME_HEADER, "E_DEVIOERR_FRAME_HEADER", L"E_DEVIOERR_FRAME_HEADER" };
			break;
		case E_DEVIOERR_USB_WRITERR: // 
			g_errinfo[i] = { E_DEVIOERR_USB_WRITERR, "E_DEVIOERR_USB_WRITERR", L"E_DEVIOERR_USB_WRITERR" };
			break;
		case E_DEVIOERR_USB_READERR: // 
			g_errinfo[i] = { E_DEVIOERR_USB_READERR, "E_DEVIOERR_USB_READERR", L"E_DEVIOERR_USB_READERR" };
			break;
		case E_DEVIOERR_SER_WRITERR: // 
			g_errinfo[i] = { E_DEVIOERR_SER_WRITERR, "E_DEVIOERR_SER_WRITERR", L"E_DEVIOERR_SER_WRITERR" };
			break;
		case E_DEVIOERR_SER_READERR: // 
			g_errinfo[i] = { E_DEVIOERR_SER_READERR, "E_DEVIOERR_SER_READERR", L"E_DEVIOERR_SER_READERR" };
			break;

				//case E_DEVIOERR_MAX

		}
	}
}

//const char * g_szErrors[VPCMDERR_MAX] =
//{
//	"E_VP_SUCCESS"					/**< 0, No error occured. */
//	, "E_VPERR_NAK_RESP"			/**< 1, An error occured on the tracker. */
//	, "E_VPERR_NAKWARN_RESP"		/**< 2, Tracker responded with NAK_WARNING. */
//	, "E_VPERR_MEMORY_ALLOCATION"   /**< 3, Memory could not be allocated. */
//	, "E_VPERR_INVALID_CONTEXT"     /**< 4, The specified context is invalid. */
//	, "E_VPERR_INVALID_ACTION"      /**< 5, The specified action is invalid. */
//	, "E_VPERR_INVALID_SENSOR"      /**< 6, The specified sensor is invalid. */
//	, "E_VPERR_INVALID_SENSOR_ACT"  /**< 7, The specified sensor and action combination is invalid.*/
//	, "E_VPERR_INVALID_PARAMS"      /**< 8, An additional specified parameter is invalid. */
//	, "E_VPERR_INVALID_SIZE"        /**< 9, The size field of the received frame is invalid. */
//	, "E_VPERR_INVALID_CRC"         /**< 10,The CRC of the received frame is invalid */
//	, "E_VPERR_TIMEOUT"             /**< 11, A timeout was reached before all the data could be recieved. */
//	, "E_VPERR_PLATFORM"			/**< 12, Indicates a system error in hi bits. */
//	, "E_VPERR_LIBUSB"				/**< 13, Indicates an error received from libusb library in high bits */
//	, "E_VPERR_UNKNOWN"				/**< 14, The error is unknown. */
//	, "E_VPERR_NOTFOUND"  		    /**< 15, Device not found */
//
//	, "E_CTXERR_INVALID_DEVHND"// = E_VPERR_MAX
//	, "E_CTXERR_THREAD_ALREADY_RUNNING"
//	, "E_CTXERR_THREAD_ALREADY_STOPPED"
//	, "E_CTXERR_INVALID_CONDITION"
//	, "E_CTXERR_FRAME_HEADER_ERROR"
//	, "E_CTXERR_FRAME_CRC_ERROR"
//	, "E_CTXERR_MEMALLOC_ERROR"
//	, "E_CTXERR_HUB_DUPLICATE"
//	, "E_CTXERR_CMDRSP_NOMATCH"
//	, "E_CTXERR_HUBIO_NOT_STARTED"
//	, "E_CTXERR_HUBDISC_NOT_STARTED"
//	, "E_CTXERR_INVALID_MASK"
//	, "E_CTXERR_NO_HUB_DISCOVERED"
//	, "E_CTXERR_CMDIO_THREAD_ERROR"
//	, "E_CTXERR_HUB_RECV_QUEUE_EMPTY"
//	, "E_CTXERR_HUB_FILTERED"
//	, "E_CTXERR_HUB_BUFFIO_NOT_READY"
//	, "E_CTXERR_HUB_SOCKET_ERROR"
//	, "E_CTXERR_HUB_CMD_SOCKET_NOT_READY"
//	, "E_CTXERR_NO_MORE_DATA"
//
//	, "E_DEVIOERR_NO_CNX" // = E_CTXERR_MAX
//	, "E_DEVIOERR_FRAME_HEADER"
//	, "E_DEVIOERR_WRITERR"
//
//};

#define VP_COMPOUNDERR_SIZE 0x1000 //0x800
char g_szCompoundError[VP_COMPOUNDERR_SIZE];
wchar_t g_wszCompoundError[VP_COMPOUNDERR_SIZE];

// string versions of WSA standard Berkeley errors
const char* g_szWSAstdErrs[] =
{
	"WSAEWOULDBLOCK"		//      (WSABASEERR+35)
	, "WSAEINPROGRESS"		//      (WSABASEERR+36)
	, "WSAEALREADY"			//      (WSABASEERR+37)
	, "WSAENOTSOCK"			//      (WSABASEERR+38)
	, "WSAEDESTADDRREQ"		//      (WSABASEERR+39)
	, "WSAEMSGSIZE"			//      (WSABASEERR+40)
	, "WSAEPROTOTYPE"		//      (WSABASEERR+41)
	, "WSAENOPROTOOPT"		//      (WSABASEERR+42)
	, "WSAEPROTONOSUPPORT"	//      (WSABASEERR+43)
	, "WSAESOCKTNOSUPPORT"	//      (WSABASEERR+44)
	, "WSAEOPNOTSUPP"		//      (WSABASEERR+45)
	, "WSAEPFNOSUPPORT"		//      (WSABASEERR+46)
	, "WSAEAFNOSUPPORT"		//      (WSABASEERR+47)
	, "WSAEADDRINUSE"		//      (WSABASEERR+48)
	, "WSAEADDRNOTAVAIL"	//      (WSABASEERR+49)
	, "WSAENETDOWN"			//      (WSABASEERR+50)
	, "WSAENETUNREACH"		//      (WSABASEERR+51)
	, "WSAENETRESET"		//      (WSABASEERR+52)
	, "WSAECONNABORTED"		//      (WSABASEERR+53)
	, "WSAECONNRESET"		//      (WSABASEERR+54)
	, "WSAENOBUFS"			//      (WSABASEERR+55)
	, "WSAEISCONN"			//      (WSABASEERR+56)
	, "WSAENOTCONN"			//      (WSABASEERR+57)
	, "WSAESHUTDOWN"		//      (WSABASEERR+58)
	, "WSAETOOMANYREFS"		//      (WSABASEERR+59)
	, "WSAETIMEDOUT"		//      (WSABASEERR+60)
	, "WSAECONNREFUSED"		//      (WSABASEERR+61)
	, "WSAELOOP"			//      (WSABASEERR+62)
	, "WSAENAMETOOLONG"		//      (WSABASEERR+63)
	, "WSAEHOSTDOWN"		//      (WSABASEERR+64)
	, "WSAEHOSTUNREACH"		//      (WSABASEERR+65)
	, "WSAENOTEMPTY"		//      (WSABASEERR+66)
	, "WSAEPROCLIM"			//      (WSABASEERR+67)
	, "WSAEUSERS"			//      (WSABASEERR+68)
	, "WSAEDQUOT"			//      (WSABASEERR+69)
	, "WSAESTALE"			//      (WSABASEERR+70)
	, "WSAEREMOTE"			//      (WSABASEERR+71)
};

const char* g_szWSAextErrs[] =
{
	"WSASYSNOTREADY"			//  (WSABASEERR+91)
	, "WSAVERNOTSUPPORTED"		//  (WSABASEERR+92)
	, "WSANOTINITIALISED"		//  (WSABASEERR+93)
	, "WSAEDISCON"				//  (WSABASEERR+101)
	, "WSAENOMORE"				//  (WSABASEERR+102)
	, "WSAECANCELLED"			//  (WSABASEERR+103)
	, "WSAEINVALIDPROCTABLE"	//  (WSABASEERR+104)
	, "WSAEINVALIDPROVIDER"		//  (WSABASEERR+105)
	, "WSAEPROVIDERFAILEDINIT"	//  (WSABASEERR+106)
	, "WSASYSCALLFAILURE"		//  (WSABASEERR+107)
	, "WSASERVICE_NOT_FOUND"	//  (WSABASEERR+108)
	, "WSATYPE_NOT_FOUND"		//  (WSABASEERR+109)
	, "WSA_E_NO_MORE"			//  (WSABASEERR+110)
	, "WSA_E_CANCELLED"			//  (WSABASEERR+111)
	, "WSAEREFUSED"				//  (WSABASEERR+112)

};


//Platform-dependent
void trPlatform_FormatSysErr(int32_t err, std::string &s)
{
#ifdef _WINDOWS_
	//defined in Winbase.h
	FormatMessageA(FORMAT_MESSAGE_FROM_SYSTEM, NULL, err, 0, g_szTraceBuffer, _countof(g_szTraceBuffer), NULL);
	//#else
	sprintf_s(g_szTraceBuffer, VP_TRACEBUFFER_SIZE, "Non-Windows system error 0x%x", err);
#endif
	std::string stmp(g_szTraceBuffer);

	s = stmp;
}
//Platform-dependent
const char * trPlatform_WSAstdErrStr(int32_t iWSAerr)
{
#ifdef _WINDOWS_
	if ((iWSAerr >= WSAEWOULDBLOCK) && (iWSAerr <= WSAEREMOTE))
	{
		return g_szWSAstdErrs[(iWSAerr - WSAEWOULDBLOCK)];
	}
	else if ((iWSAerr >= WSASYSNOTREADY) && (iWSAerr <= WSAEREFUSED))
	{
		return g_szWSAextErrs[(iWSAerr - WSASYSNOTREADY)];
	}
	else
#endif
	{
		return "WSAE NOT DEFINED";
	}
}


inline std::string to_errstring(int32_t iErr)
{
	std::string s;
	s += trErrStr(vp_loerr(iErr));
	if (vp_loerr(iErr) == E_VPERR_PLATFORM)
	{
		s += " : "; s += std::to_string((uint32_t)(vp_hierr(iErr)));
//		s += " : "; s += trWSAstdErrStr(g4p_hierr(iErr));
	}
	return (std::string(s));


}


void trTraceEnable(bool bTrace, int32_t iLev/*=2*/)
{
	g_bVPTrace = bTrace;
	g_eVPTraceLev = (eVPcmdTraceLevel)iLev;

	PiDebug::PIDSetLevel(iLev);
}

//////////////////////////////////////////
//////////////////////////////////////////
void trLogEnable(bool bLog, TCHAR * fp)
{
	if (bLog)
	{
		if (fp)
		{
			PiDebug::SetLogFile(fp);
		}
		PiDebug::StartLog();
	}
	else
	{
		PiDebug::StopLog();
	}
}

std::mutex mtxTraceBuffer;

//void trLogRx(g4p_cmdlib_context * pCtx, int32_t iCount)
//{
//	if ((pCtx != 0) && pCtx->bTrace && (iCount != 0))
//	{
//		pCtx->last_cmd_bytes_recv += iCount;
//		pCtx->last_cmd_recv_msg_pos += 
//			trConvert_data_to_ascii_hex(pCtx->cmd.buffer, pCtx->cmd.bp, pCtx->last_cmd_recv_msg + pCtx->last_cmd_recv_msg_pos, 16384 - pCtx->last_cmd_recv_msg_pos, "\t");
//	}
//}
//////////////////////////////////////////
//////////////////////////////////////////
//void trLogTx(g4p_cmdlib_context * pCtx, int32_t iCount)
//{
//	if ((pCtx != 0) && pCtx->bTrace)
//	{
//		pCtx->last_cmd_bytes_sent = iCount;
//		trConvert_data_to_ascii_hex(pCtx->cmd.buffer, pCtx->last_cmd_bytes_sent, pCtx->last_cmd_sent_msg, 16384, 0);
//	}
//}
//////////////////////////////////////////
//////////////////////////////////////////
//trace frame buf
void trTraceFBuf(void * pBuf, int32_t iCount, bool bCR, const char *szPre, int32_t minlev/*=2*/)
{
	if (!g_bVPTrace || (pBuf == 0) || (iCount == 0) || (DOUT_LEV_LT(minlev)) )
		return;

	char szThread[100];
	sprintf_s(szThread, 100, "[%08x] : ", (int)GetCurrentThreadId());

	trConvert_data_to_ascii_hex(pBuf, iCount, g_szTraceBuffer, VP_TRACEBUFFER_SIZE, 0);
	fprintf(stdout, "%s%s \r\n%s%s", szThread, (szPre != 0) ? szPre : "", g_szTraceBuffer, bCR ? "\r\n" : "" );
	#ifdef _WINDOWS_
		::OutputDebugStringA(g_szTraceBuffer);
	#endif
	g_szTraceBuffer[0] = 0;
}

void trTraceFBufcmd(void* pBuf, int32_t iCount, bool bCR, const char* szPre, int32_t minlev/*=2*/)
{
	if (!g_bVPTrace || (pBuf == 0) || (iCount == 0) || (DOUT_LEV_LT(minlev)))
		return;
	char szThread[100];
	sprintf_s(szThread, 100, "[%08x] : ", (int)GetCurrentThreadId());

	std::string s;
	CVPcontext::traceBufcmd(s, (uint8_t*)pBuf, iCount, bCR, 0);


	//CTRKctxBase::traceBufcmd(pBuf, iCount, bCR, szPre);
	fprintf(stdout, "%s%s \r\n%s%s", szThread, (szPre != 0) ? szPre : "", s.c_str(), bCR ? "\r\n" : "");
#ifdef _WINDOWS_
	::OutputDebugStringA(s.c_str());
#endif
	g_szTraceBuffer[0] = 0;


}
void trTraceFBufpno(void* pBuf, int32_t iCount, bool bCR, const char* szPre, int32_t minlev/*=2*/)
{
	if (!g_bVPTrace || (pBuf == 0) || (iCount == 0) || (DOUT_LEV_LT(minlev)))
		return;

	char szThread[100];
	sprintf_s(szThread, 100, "[%08x] : ", (int)GetCurrentThreadId());

	std::string s;
	CVPcontext::traceBufpno(s, (uint8_t*)pBuf, iCount, bCR, szPre);

	fprintf(stdout, "%s%s \r\n%s%s", szThread, (szPre != 0) ? szPre : "", s.c_str(), bCR ? "\r\n" : "");
#ifdef _WINDOWS_
	::OutputDebugStringA(s.c_str());
#endif
	g_szTraceBuffer[0] = 0;

}
//////////////////////////////////////////
//////////////////////////////////////////
// Output Function, Line, Count in preamble
void trTraceHexBufFLC(uint8_t * pBuf, int32_t iCount, bool bCR, const char *szFunc, int32_t iLine)
{
	char * pre = 0;
	char szFLC[200];
	if (szFunc)
	{
		sprintf_s(szFLC, _countof(szFLC), "%s:%d: %d : ", szFunc, iLine, iCount);
		pre = szFLC;
	}
	trTraceHexBuf(pBuf, iCount, bCR, pre);
}
//Uses DOUT5
void trTraceHexBuf5(uint8_t * pBuf, int32_t iCount, bool bCR, const char *szPre)
{
	if (!g_bVPTrace || (pBuf == 0) || (iCount == 0))
		return;

	int32_t iOffset = 0;
	for (int32_t i = 0; (i < iCount) && (iOffset < VP_TRACEBUFFER_SIZE - 3); i++)
	{
		iOffset += sprintf_s(&(g_szTraceBuffer[iOffset]), _countof(g_szTraceBuffer) - iOffset, "%02x ", pBuf[i]);
	}
	DOUT5("%s%s%s", (szPre != 0) ? szPre : "", g_szTraceBuffer, bCR ? "\r\n" : "");
	//fprintf(stdout, "%s%s%s", (szPre != 0) ? szPre : "", g_szTraceBuffer, bCR ? "\r\n" : "");
	g_szTraceBuffer[0] = 0;
}
//Uses DOUT5, Output Function, Line, Count in preamble
void trTraceHexBuf5FLC(uint8_t * pBuf, int32_t iCount, bool bCR, const char *szFunc, int32_t iLine)
{
	char * pre = 0;
	char szFLC[200];
	if (szFunc)
	{
		sprintf_s(szFLC, _countof(szFLC), "%s:%d: %d : ", szFunc, iLine, iCount);
		pre = szFLC;
	}
	trTraceHexBuf5(pBuf, iCount, bCR, pre);
}

char g_szTraceBufferII[VP_TRACEBUFFER_SIZE] = "";
std::mutex g_mtxTraceBuffII;
void trTraceRxII0(void* pCtx, uint8_t * pBuf, int32_t iCount, const char * szFunc, int32_t iLine, bool bMicroTrace/* = false*/)
{
	trTraceRxII(pCtx, pBuf, iCount, szFunc, iLine, bMicroTrace, 0);
}

void trTraceRxII(void* pCtx, uint8_t * pBuf, int32_t iCount, const char * szFunc, int32_t iLine, bool bMicroTrace/* = false*/, int32_t iLev/* = 1*/)
{
	std::string s;

	if (g_bVPTrace && pBuf)
	{
		std::lock_guard<std::mutex> lck(g_mtxTraceBuffII);
		//VC140:
		//void* hndTh = ::get_Thrd_current()._Hnd;
		//uint32_t uiTh = trTraceThIDu();// _Thrd_current()._Id;
		//VC142:
		std::thread::id _thid = trTraceThID();

		SEUCMD_HDR* pF = (SEUCMD_HDR*)(pBuf);
		if (bMicroTrace)
		{
			//DOUT1(".RXID(%02d)CMD(%02X)ACT(%02X).",
			DOUTL( iLev, ".RXID(%02d)CMD(%02X)ACT(%02X).",
				pF->seucmd.seuid,
				pF->seucmd.cmd,
				pF->seucmd.action);
		}
		else if (PID_COMPACT)
		{
			//DOUT1("%x   <=== RX:%3d  ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d\r\n",
			DOUTL(iLev, "%x   <=== RX:%3d  ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d\r\n",
				_thid,//uiTh,
				iCount,
				pF->seucmd.seuid,
				trCmdStr(pF->seucmd.cmd), pF->seucmd.cmd,
				trActionStr(pF->seucmd.action), pF->seucmd.action,
				trDTbrief().c_str(),
				szFunc, iLine);
		}
		else
		{
			trConvert_data_to_ascii_hex(pBuf, iCount, g_szTraceBufferII, sizeof(g_szTraceBufferII), "\t");
			s = (g_szTraceBufferII);
			g_szTraceBufferII[0] = 0;

			//DOUT1("\r\n%x   <=== RX:%3d  ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d\r\n%s\r\n",
			DOUTL(iLev, "\r\n%x   <=== RX:%3d  ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d\r\n%s\r\n",
				_thid,//uiTh,
				iCount,
				pF->seucmd.seuid,
				trCmdStr(pF->seucmd.cmd), pF->seucmd.cmd,
				trActionStr(pF->seucmd.action), pF->seucmd.action,
				trDTbrief().c_str(),
				szFunc, iLine, s.c_str());
		}
	}
}
//////////////////////////////////////////
//////////////////////////////////////////
void trTraceTxII0(void* pCtx, uint8_t * pBuf, int32_t iCount, const char * szFunc, int32_t iLine, bool bMicroTrace/*=false*/)
{
	trTraceTxII(pCtx, pBuf, iCount, szFunc, iLine, bMicroTrace, 0);
}
void trTraceTxII(void* pCtx, uint8_t * pBuf, int32_t iCount, const char * szFunc, int32_t iLine, bool bMicroTrace/*=false*/, int32_t iLev/* = 1*/)
{
	std::string s;
	if (g_bVPTrace && pBuf)
	{
		std::lock_guard<std::mutex> lck(g_mtxTraceBuffII);
		//VC140
		//void * hndTh = _Thrd_current()._Hnd;
		//uint32_t uiTh = trTraceThIDu();// _Thrd_current()._Id;
		//VC142:
		std::thread::id _thid = trTraceThID();

		SEUCMD_HDR* pF = (SEUCMD_HDR*)(pBuf);
		if (bMicroTrace)
		{
			//DOUT1(".TXID(%02d)CMD(%02X)ACT(%02X).",
			DOUTL(iLev, ".TXID(%02d)CMD(%02X)ACT(%02X).",
				pF->seucmd.seuid,
				pF->seucmd.cmd,
				pF->seucmd.action);
		}
		else if (PID_COMPACT)
		{
			//DOUT1("\r\n%x===> TX:%3d    ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d \r\n",
			DOUTL(iLev, "\r\n%x===> TX:%3d    ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d \r\n",
				_thid,//uiTh,
				iCount,
				pF->seucmd.seuid,
				trCmdStr(pF->seucmd.cmd), pF->seucmd.cmd,
				trActionStr(pF->seucmd.action), pF->seucmd.action,
				trDTbrief().c_str(),
				szFunc, iLine);
		}
		else
		{
			trConvert_data_to_ascii_hex(pBuf, iCount, g_szTraceBufferII, sizeof(g_szTraceBufferII), 0);
			s = (g_szTraceBufferII);
			g_szTraceBufferII[0] = 0;

			//DOUT1("\r\n%x===> TX:%3d    ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d\r\n%s\r\n",
			DOUTL(iLev, "\r\n%x===> TX:%3d    ID(%02d)   %s(%02X)   %s(%02X)\t%s %s:%d\r\n%s\r\n",
				_thid,//uiTh,
				iCount,
				pF->seucmd.seuid,
				trCmdStr(pF->seucmd.cmd), pF->seucmd.cmd,
				trActionStr(pF->seucmd.action), pF->seucmd.action,
				trDTbrief().c_str(),
				szFunc, iLine, 
				s.c_str());
		}
	}
}

///////////////////////////////////////////////
///////////////////////////////////////////////
#define VP_SWAP_16(l)        \
( ( ((l) >> 8) & 0x00ff ) |  \
  ( ((l) << 8) & 0xff00 ) )

inline uint16_t vpNetwToHost16(uint16_t val)
{
	uint16_t r = VP_SWAP_16(val);
	return r;
}
void trSAStr(struct sockaddr_in * pSAin, char * szMsg, std::string & s)
{

	if (pSAin == 0)
		return;

	if (szMsg != 0)
	{
		s += szMsg;
		s += " : ";
	}

	char szAddr[40];

	sprintf_s(szAddr, 40, "%d.%d.%d.%d:%d",
		pSAin->sin_addr.S_un.S_un_b.s_b1,
		pSAin->sin_addr.S_un.S_un_b.s_b2,
		pSAin->sin_addr.S_un.S_un_b.s_b3,
		pSAin->sin_addr.S_un.S_un_b.s_b4,
		vpNetwToHost16(pSAin->sin_port));	//		ntohs(pSAin->sin_port));

	s += szAddr;
}

void trMacStr(uint64_t mac, std::string & s)
{
	char szMac[80];
	DWORD len = 80;

	memset(szMac, 0, sizeof(szMac));

	sprintf_s(szMac, 80, "%02x:%02x:%02x:%02x:%02x:%02x", (unsigned char)(mac & 0xff),
		(unsigned char)((mac & 0xff00) >> 8),
		(unsigned char)((mac & 0xff0000) >> 16),
		(unsigned char)((mac & 0xff000000) >> 24),
		(unsigned char)((mac & 0xff00000000) >> 32),
		(unsigned char)((mac & 0xff0000000000) >> 40));

	s = szMac;
}

//////////////////////////////////////////
//////////////////////////////////////////

void trTraceErr(void* pCtx, int32_t err, const char * msg, const char *szFunc, int32_t iLine, int32_t minlev/*=0*/)
{
	if ((minlev != 0) && ((!g_bVPTrace) || DOUT_LEV_LT(minlev)))
		return;
	//if (((pCtx == 0) && g_bVPTrace) || ((pCtx != 0) && (((g4p_cmdlib_context*)pCtx)->bTrace)))
	if (g_bVPTrace) 
	{
		DOUT( "0x%x: Error 0x%0x %s : %s : %s:%d\r\n", 
			pCtx, err, trErrStr(err), ((msg==0)? "" :msg), szFunc, iLine);
	}
}


//////////////////////////////////////////
//////////////////////////////////////////

void trTraceSysErrSEU(int32_t sid, const char * ascid, int32_t err, const char * msg, const char *szFunc, int32_t iLine)
{
	{
		DOUT0("%x : %d-%s : %s:%d : SysErr %dL : %s : %s\r\n",
			trTraceThID()/*_Thrd_current()._Id*/, sid, ascid, szFunc, iLine, err, ((msg == 0) ? "" : msg), trPlatform_WSAstdErrStr(err));
	}
}
void trTraceSysErrSEU3(int32_t sid, const char * ascid, int32_t err, const char * msg, const char *szFunc, int32_t iLine)
{
	{
		DOUT3("%x : %d-%s : %s:%d : SysErr %dL : %s : %s\r\n",
			trTraceThID()/*_Thrd_current()._Id*/, sid, ascid, szFunc, iLine, err, ((msg == 0) ? "" : msg), trPlatform_WSAstdErrStr(err));
	}
}

void trTraceSysErr(void* pCtx, int32_t err, const char * msg, const char *szFunc, int32_t iLine)
{
	std::string s;

	trPlatform_FormatSysErr(err, s);

	{
		DOUT0("%x : %s:%d : SysErr %dL : %s : %s : %s\r\n",
			trTraceThID()/*_Thrd_current()._Id*/, szFunc, iLine, err, trPlatform_WSAstdErrStr(err), ((msg == 0) ? "" : msg), s.c_str());
	}

}
void trTraceSysErr3(void* pCtx, int32_t err, const char * msg, const char *szFunc, int32_t iLine)
{
	std::string s;

	trPlatform_FormatSysErr(err, s);

	{
		DOUT3("%x : %s:%d : SysErr %dL : %s : %s : %s\r\n",
			trTraceThID()/*_Thrd_current()._Id*/, szFunc, iLine, err, trPlatform_WSAstdErrStr(err), ((msg == 0) ? "" : msg), s.c_str());
	}

}


///@endcond
//////////////////////////////////////////
//////////////////////////////////////////
// Exposed VPTrace methods
//////////////////////////////////////////
//////////////////////////////////////////
//////////////////////////////////////////
//////////////////////////////////////////
const char * trActionStr(uint32_t action)
{
	//if (action > _countof(g_szActions))
	//	return "CMD_ACTION NOT DEFINED";
	return CVPcmd::actstr(action);// g_szActions[action];
}
//////////////////////////////////////////
//////////////////////////////////////////
const char * trCmdStr(uint32_t cmd)
{
	//if (cmd > _countof(g_szCmds))
	//	return "CMD NOT DEFINED";

	return CVPcmd::cmdstr(cmd);// g_szCmds[cmd];
}
//////////////////////////////////////////
//////////////////////////////////////////
const char * trErrStr(int32_t iErr)
{
	if (g_errinfo[0].err == VPCMDERR_MAX)
		InitErrInfo();

	if (vp_loerr(iErr) == E_VPERR_INVALID_CRC)
	{
		sprintf_s(g_szCompoundError, _countof(g_szCompoundError), "%s : %s",
			g_errinfo[vp_loerr(iErr)].szerr, trCmdStr(vp_hierr(iErr))); //g_szCmds[vp_hierr(iErr)]);

		return g_szCompoundError;
	}

	else if (io_err(vp_loerr(iErr)))// == E_VPERR_LIBUSB)
	{
		sprintf_s(g_szCompoundError, _countof(g_szCompoundError), "%s : %s",
			g_errinfo[CLAMP(vp_loerr(iErr), 0, VPCMDERR_MAX)].szerr, 
			CVPdevIO::ErrorStr(iErr) );
		return g_szCompoundError;
	}

	return g_errinfo[CLAMP(vp_loerr(iErr), 0, VPCMDERR_MAX)].szerr;
}
const wchar_t* trErrStrW(int32_t iErr)
{
	if (g_errinfo[0].err == VPCMDERR_MAX)
		InitErrInfo();

	if (vp_loerr(iErr) == E_VPERR_INVALID_CRC)
	{
		swprintf_s(g_wszCompoundError, _countof(g_wszCompoundError), L"%s : %s",
			g_errinfo[vp_loerr(iErr)].wszerr, trCmdStr(vp_hierr(iErr))); //g_szCmds[vp_hierr(iErr)]);

		return g_wszCompoundError;
	}

	else if (io_err(vp_loerr(iErr)))// == E_VPERR_LIBUSB)
	{
		swprintf_s(g_wszCompoundError, _countof(g_wszCompoundError), L"%s : %s",
			g_errinfo[CLAMP(vp_loerr(iErr), 0, VPCMDERR_MAX)].wszerr,
			CVPdevIO::ErrorStr(iErr));
		return g_wszCompoundError;
	}

	return g_errinfo[CLAMP(vp_loerr(iErr), 0, VPCMDERR_MAX)].wszerr;
}
//////////////////////////////////////////
//////////////////////////////////////////
/**
*  Converts binary data to formated ASCII hex and ASCII characters. Each line contains eight hex values and
*  the 8 corresponding characters, if they are displayable. Characters such as line feed are printed as a '.'.
*  Example output:
*  ---------------
*  <tt>50 4C 55 43  18 00 00 00  PLUC....</tt><br>
*  <tt>01 00 00 00  0B 00 00 00  ........</tt><br>
*  <tt>01 00 00 00  00 00 00 00  ........</tt><br>
*  <tt>00 00 00 00  B7 26 00 00  .....&..</tt><br>
*
*  @note The bytes are printed as they are sent. This means that on a little endian machine the hex fields will
*        appear as if they are reversed.
*  @note ascii output ends with "\r\n"
*/
size_t trConvert_data_to_ascii_hex( const void * data	/**< A pointer to the binary data to convert. */,
									size_t data_size	/**< Specifies the size in bytes of memory pointed to by data. */,
									char * out			/**< A pointer to a memory location where the ASCII hex and ASCII characters will be written. */,
									size_t out_size		/**< Specifies the size in bytes of memory pointed to by out. */,
									const char * pre	/**< ptr to char string to prepend to each line. May be zero. */ )
{
	size_t i, j, k;
	uint32_t bytes[2];


	for (i = 0, j = 0; i < data_size && j + 15 < out_size - 1; ++i)
	{
		if ((pre) && (i % 8 == 0))
		{
			j += sprintf_s(&(out[j]), (out_size-j-15), "%s", pre);
		}
		bytes[0] = ((unsigned char*)data)[i] / 16;
		bytes[1] = ((unsigned char*)data)[i] - 16 * bytes[0];

		for (k = 0; k < 2; ++k)
		{
			switch (bytes[k])
			{
			case 0:  out[j + k] = '0'; break;
			case 1:  out[j + k] = '1'; break;
			case 2:  out[j + k] = '2'; break;
			case 3:  out[j + k] = '3'; break;
			case 4:  out[j + k] = '4'; break;
			case 5:  out[j + k] = '5'; break;
			case 6:  out[j + k] = '6'; break;
			case 7:  out[j + k] = '7'; break;
			case 8:  out[j + k] = '8'; break;
			case 9:  out[j + k] = '9'; break;
			case 10: out[j + k] = 'A'; break;
			case 11: out[j + k] = 'B'; break;
			case 12: out[j + k] = 'C'; break;
			case 13: out[j + k] = 'D'; break;
			case 14: out[j + k] = 'E'; break;
			case 15: out[j + k] = 'F'; break;
			default: out[j + k] = '!';
			}
		}

		j += 2;

		if ((i + 1) % 8 == 0)
		{
			out[j] = ' '; ++j;
			out[j] = ' '; ++j;

			for (k = i - 7; k <= i; ++k, ++j)
			{
				out[j] = ((unsigned char*)data)[k];
				out[j] = (out[j] >= ' ' && out[j] <= '~') ? (out[j]) : ('.');
			}
		}
		else
		if ((i + 1) % 4 == 0)
		{
			out[j] = ' '; ++j;
			out[j] = ' '; ++j;

			if (i == data_size - 1)
			{
				for (k = 0; k < 13; ++k, ++j)
					out[j] = ' ';

				for (k = i - 3; k <= i; ++k, ++j)
				{
					out[j] = ((unsigned char*)data)[k];
					out[j] = (out[j] >= ' ' && out[j] <= '~') ? (out[j]) : ('.');
				}
			}
		}
		else
		{
			out[j] = ' '; ++j;
		}

		if ((i + 1) % 8 == 0 && i != data_size - 1)
		{
			out[j] = '\r'; ++j;
			out[j] = '\n'; ++j;
		}

		if (i == data_size - 1)
		{
			out[j] = '\r'; ++j;
			out[j] = '\n'; ++j;
			//out[j] = '\r'; ++j;
			//out[j] = '\n'; ++j;
		}
	}

	out[j] = '\0';

	return j;
}

//////////////////////////////////////////
//////////////////////////////////////////
char g_szTraceBufferIII[VP_TRACEBUFFER_SIZE] = "";
std::mutex g_mtxTraceBuffIII;
//formatted trace, debug level 3
void trTracef(void* pCtx, const char *fmt, ...)
{
	if ((pCtx == 0) && g_bVPTrace)
	{
		va_list argptr;
		va_start(argptr, fmt);

		std::lock_guard<std::mutex> lck(g_mtxTraceBuffIII);
		std::string s;
		{

			//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
			//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
			int iFormattedLen = _vscprintf(fmt, argptr);
			if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
				vsprintf_s(g_szTraceBufferIII, VP_TRACEBUFFER_SIZE, fmt, argptr);
			else
				sprintf_s(g_szTraceBufferIII, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
				__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);
			s += g_szTraceBufferIII;
			g_szTraceBufferIII[0] = 0;
		}
		DOUT3("%s", s.c_str());
		//DOUT(fmt, argptr);
	}
}
//////////////////////////////////////////
//////////////////////////////////////////
// formatted trace, level 1
void trTracef1(void* pCtx, const char *fmt, ...)
{
	if ((pCtx == 0) && g_bVPTrace)
	{
		va_list argptr;
		va_start(argptr, fmt);

		std::lock_guard<std::mutex> lck(g_mtxTraceBuffIII);
		std::string s;
		{
			//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
			//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
			int iFormattedLen = _vscprintf(fmt, argptr);
			if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
				vsprintf_s(g_szTraceBufferIII, VP_TRACEBUFFER_SIZE, fmt, argptr);
			else
				sprintf_s(g_szTraceBufferIII, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
				__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);
			s += g_szTraceBufferIII;
			g_szTraceBufferIII[0] = 0;
		}
		DOUT1("%s", s.c_str());
		//DOUT(fmt, argptr);
	}
}
//////////////////////////////////////////
//////////////////////////////////////////
char g_szTraceBufferIV[VP_TRACEBUFFER_SIZE] = "";
std::mutex g_mtxTraceBuffIV;
// formatted trace, debug level 5, High Priority: does not check if Trace is enabled.
void trTracefHP(const char *fmt, ...)
{
	if (DOUT_LEV_LT(5))
		return;

	va_list argptr;
	va_start(argptr, fmt);

	std::lock_guard<std::mutex> lck(g_mtxTraceBuffIV);
	std::string s;
	char szThread[100];
	sprintf_s(szThread, 100, "[%08x] : ", (int)GetCurrentThreadId());
	s += szThread;
	{
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		int iFormattedLen = _vscprintf(fmt, argptr);
		if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
			vsprintf_s(g_szTraceBufferIV, VP_TRACEBUFFER_SIZE, fmt, argptr);
		else
			sprintf_s(g_szTraceBufferIV, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
			__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);

		s += g_szTraceBufferIV;
		g_szTraceBufferIV[0] = 0;
	}
	const char *pstr = s.c_str();
	DOUT5("%s", pstr);// s.c_str());

}
//////////////////////////////////////////////////
//////////////////////////////////////////////////
char g_szTraceBufferV[VP_TRACEBUFFER_SIZE] = "";
std::mutex g_mtxTraceBuffV;
// USES DOUT - overrides any trace level
// formatted trace, High Priority: does not check if Trace is enabled.
void trTracefHPx(const char *fmt, ...)
{
	va_list argptr;
	va_start(argptr, fmt);

	std::lock_guard<std::mutex> lck(g_mtxTraceBuffIV);
	std::string s;
	char szThread[100];
	sprintf_s(szThread, 100, "[%08x] : ", (int)GetCurrentThreadId());
	s += szThread;
	{
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		int iFormattedLen = _vscprintf(fmt, argptr);
		if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
			vsprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, fmt, argptr);
		else
			sprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
			__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);
		s += g_szTraceBufferV;
		g_szTraceBufferV[0] = 0;
	}
	DOUT("%s", s.c_str());

}
//Uses level 0 trace out for error
// USES DOUT0 - Minimal trace + errors
// formatted trace, High Priority: does not check if Trace is enabled.
void trTracefHP0(const char *fmt, ...)
{
	va_list argptr;
	va_start(argptr, fmt);

	std::lock_guard<std::mutex> lck(g_mtxTraceBuffIV);
	std::string s;
	{
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		int iFormattedLen = _vscprintf(fmt, argptr);
		if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
			vsprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, fmt, argptr);
		else
			sprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
			__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);
		s += g_szTraceBufferV;
		g_szTraceBufferV[0] = 0;
	}
	DOUT0("%s", s.c_str());

}
//USES DOUT1 - Normal I/O trace + errors
// formatted trace, High Priority: does not check if Trace is enabled.
void trTracefHP1(const char *fmt, ...)
{
	va_list argptr;
	va_start(argptr, fmt);

	std::lock_guard<std::mutex> lck(g_mtxTraceBuffIV);
	std::string s;
	{
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		int iFormattedLen = _vscprintf(fmt, argptr);
		if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
			vsprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, fmt, argptr);
		else
			sprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
			__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);
		s += g_szTraceBufferV;
		g_szTraceBufferV[0] = 0;
	}
	DOUT1("%s", s.c_str());

}
//USES DOUT5 - verbose
// formatted trace, High Priority: does not check if Trace is enabled.
void trTracefHP5(const char *fmt, ...)
{
	va_list argptr;
	va_start(argptr, fmt);

	std::lock_guard<std::mutex> lck(g_mtxTraceBuffIV);
	std::string s;
	char szThread[100];
	sprintf_s(szThread, 100, "[%08x] : ", (int)GetCurrentThreadId());
	s += szThread;
	{
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		int iFormattedLen = _vscprintf(fmt, argptr);
		if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
			vsprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, fmt, argptr);
		else
			sprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
			__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);
		s += g_szTraceBufferV;
		g_szTraceBufferV[0] = 0;
	}
	DOUT5("%s", s.c_str());

}
//////////////////////////////////////////
//////////////////////////////////////////
//USES DOUT7 - extra verbose - used in continuous streaming situs
// formatted trace, High Priority: does not check if Trace is enabled.
void trTracefHP7(const char *fmt, ...)
{
	va_list argptr;
	va_start(argptr, fmt);

	std::lock_guard<std::mutex> lck(g_mtxTraceBuffIV);
	std::string s;
	char szThread[100];
	sprintf_s(szThread, 100, "[%08x] : ", (int)GetCurrentThreadId());
	s += szThread;
	{
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		//std::lock_guard<std::mutex> lck(mtxTraceBuffer);
		int iFormattedLen = _vscprintf(fmt, argptr);
		if (iFormattedLen + 1 <= VP_TRACEBUFFER_SIZE)
			vsprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, fmt, argptr);
		else
			sprintf_s(g_szTraceBufferV, VP_TRACEBUFFER_SIZE, "%s:%i : TraceBuffer size %i too small for formatted len %i\r\n",
				__FUNCTION__, __LINE__, VP_TRACEBUFFER_SIZE, iFormattedLen);
		s += g_szTraceBufferV;
		g_szTraceBufferV[0] = 0;
	}
	DOUT7("%s", s.c_str());

}
//////////////////////////////////////////
//////////////////////////////////////////
// only displayed if Trace is enabled.
void trTraceHexBuf(uint8_t * pBuf, int32_t iCount, bool bCR, const char *szPre)
{
	if (!g_bVPTrace || (pBuf == 0) || (iCount == 0))
		return;

	int32_t iOffset = 0;
	for (int32_t i = 0; (i < iCount) && (iOffset < VP_TRACEBUFFER_SIZE - 3); i++)
	{
		iOffset += sprintf_s(&(g_szTraceBuffer[iOffset]), _countof(g_szTraceBuffer) - iOffset, "%02x ", pBuf[i]);
	}
	DOUT3("%s%s%s", (szPre != 0) ? szPre : "", g_szTraceBuffer, bCR ? "\r\n" : "");
	//fprintf(stdout, "%s%s%s", (szPre != 0) ? szPre : "", g_szTraceBuffer, bCR ? "\r\n" : "");
	g_szTraceBuffer[0] = 0;
}
//////////////////////////////////////////
//////////////////////////////////////////

