/////////////////////////////////////////////////////////////////////
// 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: /PiUpdater.root/PiUpdater/PiUpdaterLib/stdformat.cpp 1     6/21/19 4:55p Suzanne $  
//#include "stdafx.h"
#include "stdformat.h"
#include <cstdio>
#include <cstdarg>
#include <algorithm>
#include <varargs.h>

//#include <alloca.h>

#include <string>
//#include <iostream>

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

namespace stdformat
{
	errno_t convertfromws(std::string &s, const std::wstring & ws)
	{
		std::wstring wsc = ws;
		wchar_t const* wsz = (wchar_t const*)wsc.c_str();
		size_t len = wsc.length() + 10;
		char *sz = new char[len];
		size_t converted = 0;
		mbstate_t mbstate;
		// Reset to initial shift state
		::memset((void*)&mbstate, 0, sizeof(mbstate));

		errno_t err = wcsrtombs_s(&converted, sz, len, &wsz, _TRUNCATE, &mbstate);
		s = sz;

		delete sz;
		return err;
	}
	errno_t converttows(std::wstring &ws, const std::string & s)
	{
		std::string sc = s;
		char const* sz = sc.c_str();
		size_t len = sc.length() + 10;
		wchar_t *wsz = new wchar_t[len];
		size_t converted = 0;
		//mbstate_t mbstate;
		// Reset to initial shift state
		//::memset((void*)&mbstate, 0, sizeof(mbstate));

		errno_t err = mbstowcs_s(&converted, wsz, len, sz, _TRUNCATE);
		ws = wsz;

		delete wsz;
		return err;
	}

	std::wstring wstrformat(const wchar_t* format, ...)
	{
		va_list arg_list;
		va_start(arg_list, format);

		// SUSv2 version doesn't work for buf NULL/size 0, so try printing
		// into a small buffer that avoids the double-rendering and alloca path too...
		wchar_t short_buf[256];
		memset(short_buf, 0, sizeof(short_buf));
		//int iSize = vsnprintf(short_buf, sizeof(short_buf)-1, format, arg_list) ;
		int iSize = _vscwprintf(format, arg_list);
		if (iSize < 0) // means it is truncated, need more space
		{
			return std::wstring();
		}
		else
		{
			// OPTION 1
			const size_t needed = iSize + 1;
			std::wstring result(needed, ' ');
			_vsnwprintf_s((wchar_t*)result.data(), result.size(), needed, format, arg_list);
			return result;  // RVO ensures this is cheap
							//OR
							//	// OPTION 2
							//	char* p = static_cast<char*>(alloca(needed)); // on stack
							//vsnprintf(p, needed, format, arg_list);
							//return p;  // text copied into returned string
		}
		/*else if (iSize < sizeof(short_buf))
		{
		return short_buf;
		}*/

		const size_t needed = iSize + 1;

		// need more space...

		// OPTION 1
		std::wstring result(needed, ' ');
		_vsnwprintf_s((wchar_t*)result.data(), result.size(), needed, format, arg_list);
		return result;  // RVO ensures this is cheap
						//OR
						//	// OPTION 2
						//	char* p = static_cast<char*>(alloca(needed)); // on stack
						//vsnprintf(p, needed, format, arg_list);
						//return p;  // text copied into returned string
	}


	std::wstring wstrformathexbuf(uint8_t *pbuf, size_t size, bool bcrlf, size_t trunc)
	{
		std::wstring s;
		size_t fsize = 0;
		size_t tsize = (trunc ? trunc - wcslen(L"Truncated\r\n") - 1 : size * 3);
		size_t i = 0;
		for (i = 0; (i < size) && (fsize < tsize); i++)
		{
			s += wstrformat(L"%02X ", pbuf[i]).c_str();
			fsize += 3;
		}
		if (trunc && i < size) // it was truncated
			s += L"Truncated";

		if (bcrlf)
			s += L"\r\n";

		return s;
	}

	std::wstring wstrformatasciibuf(uint8_t *pbuf, size_t size, bool bcrlf)
	{
		std::wstring s;

		for (unsigned int i = 0; i < size; i++)
		{
			char ch = pbuf[i];
			//ch = isprint((int)ch) ? ch : '.';
			ch = ((ch >= ' ') && (ch <= '~')) ? ch : '.';

			s += wstrformat(L"%c", ch).c_str();
		}
		if (bcrlf)
			s += L"\r\n";

		return s;
	}
	std::wstring wstrformatasciihexblanks(size_t size)
	{
		std::wstring s;
		for (unsigned int i = 0; i < size; i++)
		{
			s += L"   ";
		}
		return s;
	}
	std::wstring wstrformatasciihexbuf(uint8_t *pbuf, size_t size, bool bcrlf, size_t trunc)
	{
		std::wstring s;
		size_t remaining = size;
		size_t index = 0;
		size_t indexII = 0;
		size_t remainingII;
		size_t count, countII, countA;
		size_t four = 4;
		size_t eight = 8;

		size_t fsize = 0;
		size_t tsize = trunc ? (trunc - wcslen(L"Truncated\r\n") - 1) : 0;

		while (remaining > 0)
		{
			// each row is 20 characters
			if ((tsize) && (fsize + 20 > tsize))
			{
				s += L"Truncated\r\n";
				break;
			}

			count = std::min(remaining, four);// (remaining < 4) ? remaining : max(remaining, 4);
			size_t blank_count = four - count;
			if (count > 0)
				s += wstrformathexbuf(&pbuf[index], count, false, 0);
			wstrformatasciihexblanks(blank_count);

			s += L" ";  fsize += (count + blank_count) * 3 + 1;

			indexII = index + count;
			remainingII = remaining - count;
			countII = std::min(remainingII, four); //(remainingII < 4) ? remainingII : max(remainingII, 4);
			size_t blank_countII = four - countII;
			if (countII > 0)
				s += wstrformathexbuf(&pbuf[indexII], countII, false, 0);
			wstrformatasciihexblanks(blank_countII);

			s += L" "; fsize += (countII + blank_countII) * 3 + 1;

			countA = std::min(remaining, eight);// (remaining < 8) ? remaining : max(remaining, 8);
			size_t blank_countA = eight - countA;
			if (countA > 0)
				s += wstrformatasciibuf(&pbuf[index], countA, true);

			fsize += countA + 2;
			remaining -= countA;
			index += countA;

		}


		return s;
	}



	std::string strformat(const char* format, ...)
	{
		va_list arg_list;
		va_start(arg_list, format);

		// SUSv2 version doesn't work for buf NULL/size 0, so try printing
		// into a small buffer that avoids the double-rendering and alloca path too...
		char short_buf[256];
		memset(short_buf, 0, sizeof(short_buf));
		//int iSize = vsnprintf(short_buf, sizeof(short_buf)-1, format, arg_list) ;
		int iSize = _vscprintf(format, arg_list);
		if (iSize < 0) // means it is truncated, need more space
		{
			return std::string();
		}
		else
		{
			// OPTION 1
			const size_t needed = iSize + 1;
			std::string result(needed, ' ');
			vsnprintf((char*)result.data(), needed, format, arg_list);
			return result;  // RVO ensures this is cheap
			//OR
			//	// OPTION 2
			//	char* p = static_cast<char*>(alloca(needed)); // on stack
			//vsnprintf(p, needed, format, arg_list);
			//return p;  // text copied into returned string
		}
		/*else if (iSize < sizeof(short_buf))
		{
			return short_buf;
		}*/

		const size_t needed = iSize + 1;

		// need more space...

		// OPTION 1
		std::string result(needed, ' ');
		vsnprintf((char*)result.data(), needed, format, arg_list);
		return result;  // RVO ensures this is cheap
		//OR
		//	// OPTION 2
		//	char* p = static_cast<char*>(alloca(needed)); // on stack
		//vsnprintf(p, needed, format, arg_list);
		//return p;  // text copied into returned string
	}


	std::string strformathexbuf(uint8_t *pbuf, size_t size, bool bcrlf, size_t trunc)
	{
		std::string s;
		size_t fsize = 0;
		size_t tsize = (trunc ? trunc - strlen("Truncated\r\n") - 1 : size * 3);
		size_t i = 0;
		for (i = 0; (i < size) && (fsize < tsize); i++)
		{
			s += strformat("%02X ", pbuf[i]).c_str();
			fsize += 3;
		}
		if (trunc && i < size) // it was truncated
			s += "Truncated";

		if (bcrlf)
			s += "\r\n";

		return s;
	}

	std::string strformatasciibuf(uint8_t *pbuf, size_t size, bool bcrlf)
	{
		std::string s;

		for (unsigned int i = 0; i < size; i++)
		{
			char ch = pbuf[i];
			//ch = isprint((int)ch) ? ch : '.';
			ch = ((ch >= ' ') && (ch <= '~')) ? ch : '.';

			s += strformat("%c", ch).c_str();
		}
		if (bcrlf)
			s += "\r\n";

		return s;
	}
	std::string strformatasciihexblanks(size_t size)
	{
		std::string s;
		for (unsigned int i = 0; i < size; i++)
		{
			s += "   ";
		}
		return s;
	}
	std::string strformatasciiblanks(size_t size)
	{
		std::string s;
		for (unsigned int i = 0; i < size; i++)
		{
			s += " ";
		}
		return s;
	}
	std::string strformatasciihexbuf(uint8_t *pbuf, size_t size, bool bcrlf, size_t trunc)
	{
		std::string s;
		size_t remaining = size;
		size_t index = 0;
		size_t indexII = 0;
		size_t remainingII;
		size_t count, countII, countA;
		size_t four = 4;
		size_t eight = 8;

		size_t fsize = 0;
		size_t tsize = trunc ? (trunc - strlen("Truncated\r\n") - 1) : 0;

		while (remaining > 0)
		{
			// each row is 20 characters
			if ((tsize) && (fsize + 20 > tsize))
			{
				s += "Truncated\r\n";
				break;
			}

			//column 1
			count = std::min(remaining, four);// (remaining < 4) ? remaining : max(remaining, 4);
			size_t blank_count = four - count;
			if (count > 0)
				s += strformathexbuf(&pbuf[index], count, false, 0);
			//strformatasciihexblanks(blank_count);

			s += " ";  fsize += (count + blank_count) * 3 + 1;

			//column 2
			indexII = index + count;
			remainingII = remaining - count;
			countII = std::min(remainingII, four); //(remainingII < 4) ? remainingII : max(remainingII, 4);
			size_t blank_countII = four - countII;
			if (countII > 0)
				s += strformathexbuf(&pbuf[indexII], countII, false, 0);
			else
				s += strformatasciihexblanks(blank_countII);

			s += " "; fsize += (countII + blank_countII) * 3 + 1;

			// column 3 ascii
			countA = std::min(remaining, eight);// (remaining < 8) ? remaining : max(remaining, 8);
			size_t blank_countA = eight - countA;
			if (countA > 0)
				s += strformatasciibuf(&pbuf[index], countA, bcrlf /*true*/);
			if (blank_countA)
				s += strformatasciiblanks(blank_countA);

			fsize += countA + 2 + blank_countA;
			remaining -= countA;
			index += countA;

		}


		return s;
	}

	std::string strformatasciihexbufn(uint8_t* pbuf, size_t size, std::string & notes, bool bcrlf, size_t trunc)
	{
		std::string s;
		size_t remaining = size;
		size_t index = 0;
		size_t indexII = 0;
		size_t remainingII;
		size_t count, countII, countA;
		size_t four = 4;
		size_t eight = 8;

		size_t fsize = 0;
		size_t tsize = trunc ? (trunc - strlen("Truncated\r\n") - 1) : 0;
		bool bTruncated = false;

		while (remaining > 0)
		{
			// each row is 20 characters
			if ((tsize) && (fsize + 20 > tsize))
			{
				s += "Truncated\r\n";
				bTruncated = true;
				break;
			}

			//column 1
			count = std::min(remaining, four);// (remaining < 4) ? remaining : max(remaining, 4);
			size_t blank_count = four - count;
			if (count > 0)
				s += strformathexbuf(&pbuf[index], count, false, 0);
			strformatasciihexblanks(blank_count);

			s += " ";  fsize += (count + blank_count) * 3 + 1;

			//column 2
			indexII = index + count;
			remainingII = remaining - count;
			countII = std::min(remainingII, four); //(remainingII < 4) ? remainingII : max(remainingII, 4);
			size_t blank_countII = four - countII;
			if (countII > 0)
				s += strformathexbuf(&pbuf[indexII], countII, false, 0);
			strformatasciihexblanks(blank_countII);

			s += " "; fsize += (countII + blank_countII) * 3 + 1;

			// column 3 ascii
			countA = std::min(remaining, eight);// (remaining < 8) ? remaining : max(remaining, 8);
			size_t blank_countA = eight - countA;
			if (countA > 0)
				s += strformatasciibuf(&pbuf[index], countA, bcrlf /*true*/);

			fsize += countA + 2;
			remaining -= countA;
			index += countA;

		}
		if (!bTruncated && ((tsize) && (fsize + notes.length() < tsize)) )
		{
			s += notes;
		}

		return s;
	}

} // end namespace stdformat