/////////////////////////////////////////////////////////////////////
// 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.
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
// Polhemus Inc., A Rockwell Collins Company www.polhemus.com
// © 2002-07 Polhemus Inc., All Rights Reserved
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
//
//  Filename:           $Workfile: PDebug.cpp $
//
//  Project Name:       Polhemus Tracker Command int32_terface API 
//
//  Original Author:    S. Gagnon
//
//  Description:        DOUT defs - replacement for OutputDebugString calls
//
//  VSS $Header: /Viper/VPCmdIF/VPCmdIF/PDebug.cpp 2     9/01/17 10:22p Suzanne $  
//
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
///@cond

//#ifdef _DEBUG
//#include "Windows.h"
//#include "stringapiset.h"
//#endif

#include "PDebug.h"
#include <stdio.h>
#include <iostream>
#include <iomanip>

//#include "EInvalParamHandler.h"
#include "CriticalSection.h"

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

/////////////////////////////////////////////////////////////////////
// CLASS PiDebug 
/////////////////////////////////////////////////////////////////////
char  PiDebug::m_szDebug[PI_DEBUG_BUFFER_SIZE] = "";
uint16_t PiDebug::m_usDebugLevel = 0;
bool PiDebug::m_bDebugCompact = false;
bool PiDebug::m_bLog = false;
CLogFile PiDebug::m_LogFile;
//bool	PiDebug::g_bLogAscii = FALSE;
//bool	PiDebug::g_bFlushIO = TRUE;

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
void
PiDebug::PIDOUT_ERR0( LPCSTR szFormat, ... )
{
	va_list argptr;
	va_start( argptr, szFormat );

	PIDOUT_ERR( 0, szFormat, argptr );
}
void
PiDebug::PIDOUT_ERR1( LPCSTR szFormat, ... )
{
	va_list argptr;
	va_start( argptr, szFormat );

	PIDOUT_ERR( 1, szFormat, argptr );
}
void
PiDebug::PIDOUT_ERR3( LPCSTR szFormat, ... )
{
	va_list argptr;
	va_start( argptr, szFormat );

	PIDOUT_ERR( 3, szFormat, argptr );
}
void
PiDebug::PIDOUT_ERR5( LPCSTR szFormat, ... )
{
	va_list argptr;
	va_start( argptr, szFormat );

	PIDOUT_ERR( 5, szFormat, argptr );
}
void
PiDebug::PIDOUT_ERR7( LPCSTR szFormat, ... )
{
	va_list argptr;
	va_start( argptr, szFormat );

	PIDOUT_ERR( 7, szFormat, argptr );
}

void
PiDebug::PIDOUT_ERR( int32_t nLev, LPCSTR szFormat, ... )
{
	va_list argptr;
	va_start( argptr, szFormat );

	CHAR	szErrMsg[200];
	_strerror_s( szErrMsg, _countof(szErrMsg), szFormat );

//#ifdef _UNICODE
//	TCHAR * tstr = MBToWS( szErrMsg );
//	PIDebugOutL( nLev, tstr, argptr );
//	delete [] tstr;
//#else
	PIDebugOutL( nLev, szErrMsg, argptr );
//#endif
}

void
PiDebug::PIDOUT0( LPCSTR szFormat, ... )
{
    va_list argptr;
    va_start( argptr, szFormat );

    PIDebugOutL( 0, szFormat, argptr );
}
/////////////////////////////////////////////////////////////////////
void
PiDebug::PIDOUT1( LPCSTR szFormat, ... )
{
    va_list argptr;
    va_start( argptr, szFormat );

    PIDebugOutL( 1, szFormat, argptr );
}
/////////////////////////////////////////////////////////////////////
void
PiDebug::PIDOUT3( LPCSTR szFormat, ... )
{
    va_list argptr;
    va_start( argptr, szFormat );

    PIDebugOutL( 3, szFormat, argptr );
}
/////////////////////////////////////////////////////////////////////
void
PiDebug::PIDOUT5( LPCSTR szFormat, ... )
{
    va_list argptr;
    va_start( argptr, szFormat );

    PIDebugOutL( 5, szFormat, argptr );
}
/////////////////////////////////////////////////////////////////////
void
PiDebug::PIDOUT7( LPCSTR szFormat, ... )
{
    va_list argptr;
    va_start( argptr, szFormat );

    PIDebugOutL( 7, szFormat, argptr );
}

void 
PiDebug::PIDebugOut( int32_t level, LPCSTR szFormat, ... ) 
{
    if (level <= m_usDebugLevel)
    {
        int32_t iResult = 0;
        va_list argptr;
        va_start( argptr, szFormat );

        PIDOUTL( szFormat, argptr );
    }
}

void 
PiDebug::PIDebugOutL( int32_t level, LPCSTR szFormat, va_list argptr ) 
{
    if (level <= m_usDebugLevel)
    {
        PIDOUTL( szFormat, argptr );
    }
}

//void
//PiDebug::PIDOUTHEXBUF1( LPCTSTR szTitle, uint8_t * pBuf, uint32_t dwSize )
//{
//	if (g_usDebugLevel > 1)
//	{
//		PIDOUTHEXBUF( szTitle, pBuf, dwSize );
//	}
//}
//
//void
//PiDebug::PIDOUTHEXBUF( LPCTSTR szTitle, uint8_t * pBuf, uint32_t dwSize )
//{
//	CPiEInvalParamHandler iph(_T("PIDOUTHEXBUF"));
//	int32_t iOffset = 0;
//	//memset( (PVOID)g_szDebug, 0xAA, sizeof(g_szDebug) );
//	if (szTitle)
//	{
//		iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%s "), szTitle );
//	}
//
//	if (PiDebug::g_bLogAscii)
//	{
//		for (uint32_t i=0; (i<dwSize) && (iOffset<PI_DEBUG_BUFFER_SIZE-3); i++)
//		{
//			iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%c"), pBuf[i] );
//		}
//	}
//	else
//	{
//		for (uint32_t i=0; (i<dwSize) && (iOffset<PI_DEBUG_BUFFER_SIZE-3); i++)
//		{
//			iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%02x "), (BYTE)(pBuf[i]) );
//		}
//	}
//	//sprint32_tf( &(PiDebug::g_szDebug[iOffset]), "\n" );
//
//	g_szDebug[PI_DEBUG_BUFFER_SIZE-1] = 0;
//    ::OutputDebugString( PiDebug::g_szDebug );
//    ::OutputDebugString( _T("\n") );
//}
//
//void
//PiDebug::PILOGERR( LPCSTR szFormat, ... )
//{
//	CPiEInvalParamHandler iph(_T("PILOGERR"));
//
//	va_list argptr;
//	va_start( argptr, szFormat );
//
//	CHAR szErrMsg[200];
//	_strerror_s(szErrMsg, _countof(szErrMsg), szFormat );
//
//#ifdef _UNICODE
//	TCHAR * tstr = MBToWS( szErrMsg );
//	_stprint32_tf_s( PiDebug::g_szDebug, _countof( PiDebug::g_szDebug ), _T("%s "), tstr );
//	delete [] tstr;
//#else
//	_stprint32_tf_s( PiDebug::g_szDebug, _countof( PiDebug::g_szDebug ), _T("%s "), szErrMsg );
//#endif
//
//	LOGOUT( PiDebug::g_szDebug, argptr );
//}
//
//void
//PiDebug::PILOGHEXBUF( LPCTSTR szTitle, uint8_t * pBuf, uint32_t dwSize )
//{
//#ifdef _PIDEBUG_LOG
//	CPiEInvalParamHandler iph(_T("PILOGHEXBUF"));
//
//	int32_t iOffset = 0;
//	if (szTitle)
//	{
//		iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%s "), szTitle );
//	}
//
//	if (PiDebug::g_bLogAscii)
//	{
//		for (uint32_t i=0; (i<dwSize) && (iOffset<PI_DEBUG_BUFFER_SIZE-3); i++)
//		{
//			iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%c"), (BYTE)(pBuf[i]) );
//		}
//	}
//	else
//	{
//		for (uint32_t i=0; (i<dwSize) && (iOffset<PI_DEBUG_BUFFER_SIZE-3); i++)
//		{
//			iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%02x "), (BYTE)(pBuf[i]) );
//		}
//	}
//	//sprint32_tf( &(PiDebug::g_szDebug[iOffset]), "\n" );
//
//	g_szDebug[PI_DEBUG_BUFFER_SIZE-1] = 0;
//	g_LogFile.Write(_T("%s"), PiDebug::g_szDebug );
//	//FlushLog();
//#endif
//}
//
//void
//PiDebug::PIDOUTASCBUF( LPCTSTR szTitle, uint8_t * pBuf, uint32_t dwSize )
//{
//	CPiEInvalParamHandler iph(_T("PIDOUTASCBUF"));
//
//	int32_t iOffset = 0;
//	if (szTitle)
//	{
//		iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%s "), szTitle );
//	}
//
//	for (uint32_t i=0; (i<dwSize) && (iOffset<PI_DEBUG_BUFFER_SIZE-3); i++)
//	{
//		iOffset += _stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("%c"), pBuf[i] );
//	}
//
//	g_szDebug[PI_DEBUG_BUFFER_SIZE-1] = 0;
//	_stprint32_tf_s( &(PiDebug::g_szDebug[iOffset]), _countof(PiDebug::g_szDebug)-iOffset, _T("\n") );
//
//    ::OutputDebugString( PiDebug::g_szDebug );
//}
//

void 
PiDebug::PIDOUT( LPCSTR szFormat, ... )
{
// 	CPiEInvalParamHandler iph(_T("PIDOUT"));

    int32_t iResult = 0;
    va_list argptr;
    va_start( argptr, szFormat );

	int iFormattedLen = _vscprintf(szFormat, argptr);
	if (iFormattedLen + 1 <= PI_DEBUG_BUFFER_SIZE)
		iResult = vsprintf_s(PiDebug::m_szDebug, _countof(PiDebug::m_szDebug), szFormat, argptr);
	else
		sprintf_s(PiDebug::m_szDebug, sizeof(PiDebug::m_szDebug), "%s:%i : PiDebug::m_szDebug size %i too small for formatted len %i\r\n",
		__FUNCTION__, __LINE__, PI_DEBUG_BUFFER_SIZE, iFormattedLen);
//#endif

#ifdef _WINDOWS_
	::OutputDebugStringA( PiDebug::m_szDebug );
#endif
	fprintf(stdout, "%s", PiDebug::m_szDebug);

}
//void myInvalidParameterHandler(const wchar_t* expression,
//   const wchar_t* function, 
//   const wchar_t* file, 
//   uint32_t line, 
//   uint64_t pReserved)
//{
//	TCHAR msg[500];
//	memset(msg, 0, sizeof(msg));
//
//
//#ifndef _UNICODE
//	TCHAR * texpression;
//	TCHAR * tfunction;
//	TCHAR * tfile;
//	texpression = PiDebug::WSToMB( expression );
//	tfunction = PiDebug::WSToMB( function );
//	tfile = PiDebug::WSToMB( file );
//	_sntprint32_tf_s( msg, 499, _TRUNCATE, _T("Invalid parameter detected in function %s.")
//									   _T(" File: %s Line: %d\n")
//									   _T("Expression: %s\n"), tfunction, tfile, line, texpression );
//	delete [] texpression;
//	delete [] tfunction;
//	delete [] tfile;
//#else
//	_sntprint32_tf_s( msg, 499, _TRUNCATE, _T("Invalid parameter detected in function %s.")
//									   _T(" File: %s Line: %d\n")
//									   _T("Expression: %s\n"), function, file, line, expression );
//#endif
//
//	MessageBox(NULL, msg, _T("PIDOUTL"), MB_OK );
//}
void 
PiDebug::PIDOUTL( LPCSTR szFormat, va_list argptr )
{
//	CPiEInvalParamHandler ph(_T("PIDOUTL"));
#ifdef _DEBUG
 	CPiCriticalSection cs;
#endif
		
	int32_t iResult = 0;

	int iFormattedLen = _vscprintf(szFormat, argptr);
	if (iFormattedLen + 1 <= PI_DEBUG_BUFFER_SIZE)
		iResult = vsprintf_s(PiDebug::m_szDebug, _countof(PiDebug::m_szDebug), szFormat, argptr);
	else
		sprintf_s(PiDebug::m_szDebug, _countof(PiDebug::m_szDebug), "%s:%i : PiDebug::m_szDebug size %i too small for formatted len %i\r\n",
		__FUNCTION__, __LINE__, PI_DEBUG_BUFFER_SIZE, iFormattedLen);

#ifdef _WINDOWS_
    ::OutputDebugStringA( PiDebug::m_szDebug );
#endif
	fprintf(stdout, "%s", PiDebug::m_szDebug);
	std::cout << m_szDebug;

	if (m_bLog)
		m_LogFile.VWrite(szFormat, argptr);

	PiDebug::m_szDebug[0] = 0;
}


LPWSTR 
PiDebug::MBToWS( LPCSTR szStr )
{
	WCHAR *	wstr = new WCHAR[ strlen(szStr)+1 ];

	mbtowc(wstr, szStr, strlen(szStr));

	wstr[strlen(szStr)]=0;

	return wstr;
}

LPSTR 
PiDebug::WSToMB( LPCWSTR szWStr )
{
	char * mbstr = 0; 
	size_t newsize = 0;
	errno_t r = wcstombs_s(&newsize, NULL, 0, szWStr, newsize);

	if (newsize)
	{
		mbstr = new char[newsize+1];
		size_t ns_II = 0;

		r = wcstombs_s(&ns_II, mbstr, newsize+1, szWStr, _TRUNCATE);
		mbstr[newsize] = 0;
	}
	return mbstr;
}

/////////////////////////////////////////////////////////////////////
// END $Workfile: PDebug.cpp $
/////////////////////////////////////////////////////////////////////
///@endcond
