#include "VPContext.h"
#include "VPCmdIF.h"
#include "VPusbIO.h"
#include "VPserIO.h"
#include "VPdevIO.h"
#include "VPtracei.h"
#include "stdformat.h"


std::vector<CVPcontext *> CVPcontext::g_vecActiveCtx;

CVPcontext::CVPcontext()
	: m_eTraceLev(VPCMD_NOTRACE)
	, m_dev_USBBufcount(0)
{
	CVPcmd::InitCmdInfo();
//#ifdef _DEBUG
//	//vpcmdif_trace(true, 3);
//	SetTraceLev(VPCMD_TRACE_3_NORMALIO);
//#endif
}

void CVPcontext::traceBufcmd(std::string& s, uint8_t* pBuf, int32_t iCount, bool bCR, const char* szPre)
{
	//s = stdformat::strformatasciihexbuf(pBuf, (size_t)iCount, true);

	uint32_t* p32 = (uint32_t*)pBuf;

	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[0], 8, false);
	s += stdformat::strformat("  PREAMBLE     SIZE %04d\r\n", p32[1]).c_str();

	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[2], 8, false);
	s += stdformat::strformat("  SEUID %04d   %s\r\n", p32[2], CVPcmd::cmdstr(p32[3])).c_str();

	int act = p32[4];
	eViperCmds ecmd = (eViperCmds)(CLAMP(p32[3], 0, CMD_MAX));
	size_t paysize = CVPcmd::paysize(ecmd);
	uint32_t size = p32[1];
	uint32_t payloadbyteindex = 7 * 4;
	size_t expectedcountwithpayload = payloadbyteindex + paysize + 4; //current+paysize+crc
	bool bnopayload = (paysize == 0) || ((size_t)iCount < expectedcountwithpayload);

	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[4], 8, false);
	s += stdformat::strformat("  %-11s  ARG1 %04d\r\n", CVPcmd::actstr(p32[4], true), p32[5]).c_str();
	//s += stdformat::strformat("  %-11s  ARG1 %04d   paysize=%d, expcountwpay=%d, iCount=%d\r\n", 
	//	CVPcmd::actstr(p32[4], true), p32[5],
	//	paysize, expectedcountwithpayload, iCount	).c_str();

	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[6], 8, false);
	s += stdformat::strformat("  ARG2 %04d    ", p32[6]).c_str();

	//int act = p32[4];
	//eG4BoostCmds ecmd = (eG4BoostCmds)(CLAMP(p32[3], 0, CMD_MAX));
	//size_t paysize = CVPcmd::paysize(ecmd);
	//uint32_t size = p32[1];
	//uint32_t payloadbyteindex = 7*4;
	//size_t expectedcountwithpayload = payloadbyteindex + paysize + 4; //current+paysize+crc
	//bool bnopayload = (paysize == 0) || (iCount < expectedcountwithpayload);

	if (bnopayload)
	{
		s += stdformat::strformat("CRC\r\n");
		return;
	}
	if (paysize == 4)
	{
		s += stdformat::strformat("%s %04d\r\n", CVPcmd::cmdpaystr(ecmd), p32[7]).c_str();
		s += stdformat::strformatasciihexbuf((uint8_t*)&p32[8], 4, false).c_str();
		s += stdformat::strformat("  CRC          \r\n").c_str();
		//return;
	}
	else if (ecmd == CMD_SINGLE_PNO)
	{
		//typedef struct _SEUPNO {
		//	uint32_t seuid;
		//	uint32_t frame;
		//	HPINFO   HPinfo;		// 4 bytes
		//	uint32_t sensorCount;
		//} SEUPNO;		

		s += stdformat::strformat("%s seuid %04d\r\n", CVPcmd::cmdpaystr(ecmd), p32[7]).c_str();
		int i32 = 8;
		s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
		s += stdformat::strformat("  F %-7d    HPINFO %08x\r\n", p32[i32], p32[i32 + 1]).c_str();
		i32 += 2;
		s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
		int snsct = p32[i32];
		if (snsct == 0)
		{
			s += stdformat::strformat("  SNSCNT %02d     CRC\r\n", p32[i32], p32[i32 + 1]).c_str();
		}
		else
		{
			int sns = ((SFINFO*)(&p32[i32 + 1]))->bfSnum;
			s += stdformat::strformat("  SNSCNT %02d    SFINFO %08x   SENSOR % d\r\n", p32[i32], p32[i32 + 1], ((SFINFO*)(&p32[i32 + 1]))->bfSnum).c_str();
			i32 += 2;
			for (int i = 0; i < snsct; i++)
			{
				s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
				s += stdformat::strformat("  % 9.5f    % 9.5f   \r\n", *(float*)(&p32[i32]), *(float*)(&p32[i32 + 1])).c_str(); // x y
				i32 += 2;
				s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
				s += stdformat::strformat("  % 9.5f    % 9.5f   \r\n", *(float*)(&p32[i32]), *(float*)(&p32[i32 + 1])).c_str(); // z a
				i32 += 2;
				s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
				s += stdformat::strformat("  % 9.5f    % 9.5f   \r\n", *(float*)(&p32[i32]), *(float*)(&p32[i32 + 1])).c_str(); // e r
				i32 += 2;
				s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
				if (i == (snsct - 1))
				{
					s += stdformat::strformat("  % 9.5f    CRC   \r\n", *(float*)(&p32[i32])).c_str(); // e r
				}
				else
				{
					s += stdformat::strformat("  % 9.5f    SFINFO %08x   SENSOR % d\r\n", (float)p32[i32], p32[i32 + 1], ((SFINFO*)(&p32[i32 + 1]))->bfSnum).c_str(); // e r
					i32 += 2;
				}
			}
		}

	}
	else
	{
		s += stdformat::strformat("%s payload\r\n", CVPcmd::cmdpaystr(ecmd)).c_str();
		if (((paysize - 4) % 8) == 0)
		{
			// does complete rows of 8 for remaining payload data
			// then does crc
			s += stdformat::strformatasciihexbuf((uint8_t*)&p32[8], paysize - 4, true);
			s += stdformat::strformatasciihexbuf(&pBuf[(8 * 4) + paysize - 4], 4, false).c_str();
			s += stdformat::strformat("  CRC          \r\n").c_str();
			//return;
		}
		else
		{
			// does remaining payload data + 4 byte crc
			s += stdformat::strformatasciihexbuf((uint8_t*)&p32[8], paysize - 8, true);
			s += stdformat::strformatasciihexbuf(&pBuf[(8 * 4) + paysize - 8], 8, false).c_str();
			s += stdformat::strformat("               CRC\r\n").c_str();
			//return;
		}
	}
}

void CVPcontext::traceBufpno(std::string& s, uint8_t* pBuf, int32_t iCount, bool bCR, const char* szPre)
{
	//s = stdformat::strformatasciihexbuf(pBuf, (size_t)iCount, true);

	uint32_t* p32 = (uint32_t*)pBuf;

	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[0], 8, false);
	s += stdformat::strformat("  PREAMBLE          SIZE %04d\r\n", p32[1]).c_str();

	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[2], 8, false);
	s += stdformat::strformat("  SEUID %04d        F % 7d\r\n", p32[2], p32[3]).c_str();

	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[4], 8, false);
	s += stdformat::strformat("  HPINFO %08x   SNSCNT %02d\r\n", p32[4], p32[5]).c_str();
	/*
	PREAMBLE     SIZE 0084
	00 00 00 00  04 00 00 00  ........   SEUID 0000        F       4
	00 00 00 00  02 00 00 00  ........   HPINFO 00000000   SNSCNT 02
	00 C0 04 00  67 1C 71 41  ....g.qA   SFINFO 00000006    0.00000  SENSOR - 536870912  i32 =
	64 BC 08 40  B5 E4 01 41  d..@...A    2.13650           8.11834
	CC D2 8B 42  0D A2 3F 41  ...B.. ?A   69.91171         11.97706
	2B 2E 93 C2  00 00 00 00  + ......  -73.59017     0.00000
	01 80 0E 00  F3 A4 6B 41  ......kA  SFINFO 0000000e      0.00000  SENSOR  1610612736  i32 =
	78 0A A0 40  7E 53 5D 41  x..@~S]A  5.00128     13.83288
	AC D7 5B 42  0D 8A 55 C2  ..[B..U.  54.96062 - 53.38482
	1A 43 BD C2  00 00 00 00  .C......  -94.63106     0.00000
	4B C0 00 00               K...      CRC
	*/
	int snscount = p32[5];
	int snsbegin32 = 6;
	int snssz32 = 8;
	int i32 = snsbegin32;
	for (int i = 0; i < snscount; i++)
	{
		i32 = snsbegin32 + (i * snssz32);
		//SFINFO* psfi = (SFINFO*)(&p32[i32]);
		//int sns = psfi->bfSnum;

		s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
		s += stdformat::strformat("  SFINFO %08x   % 9.5f  SENSOR % d  \r\n",
			p32[i32], *(float*)(&p32[i32 + 1]), ((SFINFO*)(&p32[i32]))->bfSnum).c_str();
		i32 += 2;
		s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
		s += stdformat::strformat("  % 9.5f         % 9.5f\r\n", *(float*)(&p32[i32]), *(float*)(&p32[i32 + 1])).c_str();
		i32 += 2;
		s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
		s += stdformat::strformat("  % 9.5f         % 9.5f\r\n", *(float*)(&p32[i32]), *(float*)(&p32[i32 + 1])).c_str();
		i32 += 2;
		s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 8, false);
		s += stdformat::strformat("  % 9.5f         % 9.5f\r\n", *(float*)(&p32[i32]), *(float*)(&p32[i32 + 1])).c_str();
	}
	i32 += 2;
	s += stdformat::strformatasciihexbuf((uint8_t*)&p32[i32], 4, false).c_str();
	s += stdformat::strformat("  CRC          \r\n").c_str();
}

void CVPcontext::SetTraceLev(int nT)
{
	int r = 0;
	trTracefHP0(__FUNCTION__ " : nT = %d\r\n", nT);

	m_eTraceLev = nT;
	//vpcmdif_trace(nT);

	std::vector<CVPdevIO *>::const_iterator it;
	for (it = m_vecDevCnx.begin(); it != m_vecDevCnx.end(); ++it)
	{
		CVPdevIO *pDev = *it;
		pDev->SetTraceLev(nT);
	}
}

CVPcontext::~CVPcontext()
{
	destroyAlldevs();
}

bool CVPcontext::findPctx(CVPcontext *p)
{
	bool bRet = false;
	std::vector<CVPcontext *>::iterator it;
	for (it = g_vecActiveCtx.begin(); it != g_vecActiveCtx.end(); ++it)
	{
		if (*it == p)
		{
			bRet = true;
			break;
		}
	}
	return bRet;
}

void CVPcontext::destroyPctx(CVPcontext *pCtx)
{
	std::vector<CVPcontext *>::const_iterator it;
	for (it = g_vecActiveCtx.begin(); it != g_vecActiveCtx.end(); ++it)
	{
		if (*it == pCtx)
		{
			g_vecActiveCtx.erase(it);
			delete pCtx;
			break;
		}
	}
}

void CVPcontext::pushPctx(CVPcontext *pCtx)
{
	g_vecActiveCtx.push_back(pCtx);
}
int CVPcontext::connectNewUSB(vpdev_hnd & hnd, uint16_t vid, uint16_t pid, uint32_t tag)
{
	CODETRACE;
	trTracefHP1("%s : %s : vid 0x%x, pid 0x%x \r\n", __FILE__, __FUNCTION__, vid, pid);

	int r = 0;

	CVPdevIO *pdev = new CVPdevIO( this );


	if (pdev == 0)
		return E_CTXERR_MEMALLOC_ERROR;

	pdev->SetTraceLev(m_eTraceLev);

	if (r = pdev->ConnectUSB(vid, pid))
	{
		delete pdev;
	}
	else
	{
		if (tag)
		{
			pdev->OwnerTag() = tag;
			pdev->CmdPreamble() = CVPcmd::TrkCmdPre(pid);
			pdev->PnoPreamble() = CVPcmd::TrkPnoPre(pid);
		}

		pdev->SetTraceLev(m_eTraceLev);
		pdev->CmdTimeoutMS() = VPUSB_DEFAULT_CMD_TIMEOUT_MS*10;

		pushPdev(pdev);
		hnd = (vpdev_hnd)pdev;
		//CONT_FC_CONFIG fc;
		if (r = cmd_dev_contpno(hnd, CMD_ACTION_RESET, 0, E_PNOMODE_STD))
		{
			trTraceErr(this, r, "cmd_dev_contpno returned", __FUNCTION__, __LINE__);
			//if (r == E_VPERR_NAK_RESP)
		}
		//else
		//	pdev->InitCmdTimeoutMS();
		else
		if (r = cmd_dev_seuid(hnd, CMD_ACTION_GET, (SEUID_CONFIG*)&(pdev->tag_cfg.seuid)))
		{}
		else
			r = cmd_dev_whoami(hnd, &(pdev->tag_wai));

		pdev->InitCmdTimeoutMS();
	}

#ifdef _MOCKUP
	strncpy_s(pdev->tag_wai.device_name, NAME_SIZE, "Viper", _TRUNCATE);
	strncpy_s(pdev->tag_wai.dsp_app_fw_pn, PN_SIZE, "FWC-75809-15-100", _TRUNCATE);
	strncpy_s(pdev->tag_wai.dsp_bt_fw_pn, PN_SIZE, "FWC-75809-14-100", _TRUNCATE);
#endif
	CODETRACEEX;
	return r;
}

int CVPcontext::connectNewSer(vpdev_hnd & hnd, int32_t serport, uint32_t sercfg)
{
	CODETRACE;
	trTracefHP("%s : %s : serport %d, sercfg %d \r\n", __FILE__, __FUNCTION__, serport, sercfg);

	int r = 0;

	CVPdevIO *pdev = new CVPdevIO(this);

	if (pdev == 0)
		return E_CTXERR_MEMALLOC_ERROR;

	pdev->SetTraceLev(m_eTraceLev);
	if (r = pdev->ConnectSerial(serport, sercfg))
	{
		
		delete pdev;
	}
	else
	{

		vpdev_hnd l_hnd = (vpdev_hnd)pdev;
		pushPdev(pdev);


		if (r = cmd_dev_seuid(l_hnd, CMD_ACTION_GET, (SEUID_CONFIG*)&(pdev->tag_cfg.seuid)))
		{
			//connection to port will succeed even if device not there.
			// convert to NOT FOUND if no response to command.
			if (r == E_CTXERR_DEV_RECV_QUEUE_EMPTY)
				r = E_VPERR_NOTFOUND;

			destroyPdev( pdev );
		}
		else if (r = cmd_dev_whoami(l_hnd, &(pdev->tag_wai)))
		{
			// Got this far so we know the device is there.  Leave this error code alone.
			destroyPdev(pdev);

		}
		else
		{
			hnd = l_hnd;
		}

	}

	return r;
}


bool CVPcontext::findPdev(CVPdevIO *p)
{
	bool bRet = false;
	std::vector<CVPdevIO *>::iterator it;
	for (it = m_vecDevCnx.begin(); it != m_vecDevCnx.end(); ++it)
	{
		if (*it == p)
		{
			bRet = true;
			break;
		}
	}
	return bRet;
}

void CVPcontext::destroyPdev(CVPdevIO *pdev)
{
	CODETRACE;
	std::vector<CVPdevIO *>::const_iterator it;
	for (it = m_vecDevCnx.begin(); it != m_vecDevCnx.end(); ++it)
	{
		if (*it == pdev)
		{
			m_vecDevCnx.erase(it);
			delete pdev;
			break;
		}
	}
	CODETRACEEX;

	trTracefHP("=======================================================\r\n");
	trTracefHP("=======================================================\r\n");

}

void CVPcontext::destroyAlldevs()
{
	std::vector<CVPdevIO *>::const_iterator it;
	for (it = m_vecDevCnx.begin(); it != m_vecDevCnx.end(); ++it)
	{
		CVPdevIO *pDev = *it;
		//pDev->DisconnectUSB();
		{
			//m_vecDevCnx.erase(it);
			delete pDev;
		}
	}
	m_vecDevCnx.clear();
}

void CVPcontext::reportAlldevs()
{
	std::vector<CVPdevIO *>::const_iterator it;
	for (it = m_vecDevCnx.begin(); it != m_vecDevCnx.end(); ++it)
	{
		CVPdevIO *pDev = *it;
		pDev->Report(m_strReports);
		m_strReports += "\r\n";
	}
	m_wstrReports.clear();
	stdformat::converttows(m_wstrReports, m_strReports);
}
void CVPcontext::pushPdev(CVPdevIO *pdev)
{
	m_vecDevCnx.push_back(pdev);
}

#define VALIDATE_DEVHND(pdev, hnd)  \
							{	pdev=(CVPdevIO*)hnd;						\
								if (!(findPdev(pdev)))						\
								{	CODETRACEEX; return E_CTXERR_INVALID_DEVHND; } }

#define VALIDATE_ACTION(eCmd, eAct) \
							{	if (!(CVPcmd::ValidateAct(eCmd, eAct))) \
								{	CODETRACEEX;  return E_VPERR_INVALID_ACTION; } }

#define VALIDATE_0PARAMS(eAct, pbuf, bufsize) \
							{	if (((eAct == CMD_ACTION_SET) || (eAct == CMD_ACTION_GET)) && (!pbuf || !bufsize)) \
								{	CODETRACEEX; return E_VPERR_INVALID_PARAMS; } }
// tests for valid payload pointer & size.. if payload not required (RESET), does not matter if its there or not
#define VALIDATE_PARAMS(eCmd, eAct, pbuf, bufsize) \
							{	if ( CVPcmd::paysize(eCmd) && ((eAct != CMD_ACTION_RESET) && (!pbuf || !bufsize))) \
								{	CODETRACEEX; return E_VPERR_INVALID_PARAMS; }\
								else if ( (eAct != CMD_ACTION_RESET) && (CVPcmd::paysize(eCmd) != bufsize)) \
								{	CODETRACEEX; return E_VPERR_INVALID_SIZE; } }

#define VALIDATE_BUF_PARAMS( pbuf, bufsize )\
							{	if  (!pbuf || !bufsize) \
								{	CODETRACEEX; return E_VPERR_INVALID_PARAMS; } }


#define VALIDATE_SENSOR(sns, eAct)												\
								{	if ((sns < 0) && (eAct == CMD_ACTION_GET))	\
									{	CODETRACEEX; return E_VPERR_INVALID_SENSOR_ACT;	}	\
									else if (sns > SENSORS_PER_SEU)				\
									{	CODETRACEEX; return E_VPERR_INVALID_SENSOR; } }

int CVPcontext::dev_release(vpdev_hnd hnd)
{
	int r = 0;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	r = pdev->Disconnect();

	destroyPdev(pdev);

	return r;
}
int CVPcontext::dev_SetPnoCB(vpdev_hnd hnd, VP_PNOREADY_CALLBACK cb, PVOID pInfo)
{
	int r = 0;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	pdev->SetPnoCB(cb, pInfo);
	return r;

}

int CVPcontext::dev_tracelev(vpdev_hnd hnd, int iT)
{
	int r = 0;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	pdev->SetTraceLev(iT);

	return r;
}
int CVPcontext::dev_timeoutms(vpdev_hnd hnd, eCmdActions eAct, int ms)
{
	int r = 0;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	if (eAct == CMD_ACTION_SET)
	{
		pdev->CmdTimeoutMS() = ms;
	}
	else
	{
		pdev->InitCmdTimeoutMS();

	}
	return r;
}

int CVPcontext::dev_report(vpdev_hnd hnd)
{
	int r = 0;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	pdev->Report(m_strReports);
	m_strReports += "\r\n";

	m_wstrReports.clear();
	stdformat::converttows(m_wstrReports, m_strReports);
	return r;
}

int CVPcontext::dev_info(vpdev_hnd hnd, const WHOAMI_STRUCT *& wai,  VIPER_SEUID & seuid)
{
	int r = 0;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	wai = pdev->WhoAmI();
	seuid = pdev->SEUID(); 

	return r;
}

int CVPcontext::dev_tsenable(vpdev_hnd hnd, bool tsEn)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	return pdev->EnableTimestamp(tsEn);
}
int CVPcontext::dev_pnofrmerr_allow(vpdev_hnd hnd, bool ballow)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	return pdev->AllowPnoFramerrs(ballow);
}
int CVPcontext::dev_pnousb_buffer(vpdev_hnd hnd, uint32_t count)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);
	m_dev_USBBufcount = count;
	return pdev->USBbuffer(count);
}

int CVPcontext::cmd_dev_gen(vpdev_hnd hnd, uint32_t uCmd, eCmdActions eAct, uint8_t* pbuf /*= 0*/, uint32_t bufsize /*= 0*/, uint32_t a1 /*= 0*/, uint32_t a2 /*= 0*/)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	CVPcmd cmd;
	cmd.Fill(pdev->SEUID(), uCmd, eAct, a1, a2, (uint8_t*)pbuf, bufsize);

	if (pdev->OwnerTag())
	{
		//Do nothing
		// Tag can be set during context connectNewUSB();  
		// Useful for experimental or custom adaptations.  
		// cmd.vp2tag(); // convert to a different command possible use
	}

	CFrameInfo Frsp;

	int r = 0;

	if (r = SendRecv(hnd, cmd, Frsp))
	{
	}
	else if (cmd.IsGet())
	{
		if (pbuf && bufsize)
			memcpy(pbuf, Frsp.PCmdPayload(), std::min<uint32_t>(bufsize, Frsp.CmdPayloadSize()));

		//TODO: Add fcn to backup cmd data into devinfo  on Get or on SET+Ack
	}

	CODETRACEEX;
	return r;
}

int CVPcontext::cmd_dev(vpdev_hnd hnd, eViperCmds eCmd, eCmdActions eAct, uint8_t* pbuf /*= 0*/, uint32_t bufsize /*= 0*/, uint32_t a1/*=0*/, uint32_t a2/*=0*/)
{
	VALIDATE_ACTION(eCmd, eAct);
	VALIDATE_PARAMS(eCmd, eAct, pbuf, bufsize);

	uint32_t uCmd = eCmd;

	return cmd_dev_gen(hnd, uCmd, eAct, pbuf, bufsize, a1, a2);
}

int  CVPcontext::cmd_sns_gen(vpdev_hnd hnd, int sns, uint32_t uCmd, eCmdActions eAct, uint8_t* pbuf/* = 0*/, uint32_t bufsize/* = 0*/, uint32_t a2/* = 0*/)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);
	VALIDATE_SENSOR(sns, eAct);

	CVPcmd cmd;
	cmd.Fill(pdev->SEUID(), uCmd, eAct, sns, a2, (uint8_t*)pbuf, bufsize);

	if (pdev->OwnerTag())
	{
		//Do nothing
		// Tag can be set during context connectNewUSB();  
		// Useful for experimental or custom adaptations.  
		// cmd.vp2tag(); // convert to a different command possible use
	}

	CFrameInfo Frsp;

	int r = 0;

	if (r = SendRecv(hnd, cmd, Frsp))
	{
	}
	else if (cmd.IsGet())
	{
		memcpy(pbuf, Frsp.PCmdPayload(), std::min<uint32_t>(bufsize, Frsp.CmdPayloadSize()));
	}

	return r;

}

int CVPcontext::cmd_sns(vpdev_hnd hnd, int sns, eViperCmds eCmd, eCmdActions eAct, uint8_t* pbuf /*= 0*/, uint32_t bufsize /*= 0*/, uint32_t a2/*=0*/)
{
	VALIDATE_ACTION(eCmd, eAct);
	VALIDATE_PARAMS(eCmd, eAct, pbuf, bufsize);

	uint32_t uCmd = eCmd;

	return cmd_sns_gen(hnd, sns, uCmd, eAct, pbuf, bufsize, a2);
}

// for all commands, out_wai should point to caller's buffer.
// for SET action, the out_wai buffer will be copied into the outbound command buffer.
// for GET action, the command response payload will be copied into the out_wai buffer.
// for RESET action, the out_wai buffer pointer is ignored unless the tracker's command syntax requires it.
int CVPcontext::cmd_dev_bit(vpdev_hnd hnd, BIT_STRUCT * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_BIT, CMD_ACTION_GET, (uint8_t*)out_wai, sizeof(BIT_STRUCT));
}
int CVPcontext::cmd_dev_bit_report(vpdev_hnd hnd)
{
	CODETRACE;
	BIT_STRUCT out_wai;
	int r = 0;
	if (r = cmd_dev_bit(hnd, &out_wai))
	{
	}
	else
	{
		//Not saving bit results in dev object
		//CVPdevIO * pdev = (CVPdevIO*)hnd;  // this was validated by the call to cmd_dev
		//pdev->SetWhoAmI(&out_wai);
		CBitResult(&out_wai).Report(m_strReports);
		stdformat::converttows(m_wstrReports, m_strReports);
	}
	return r;
}
int CVPcontext::cmd_dev_resetblockcfg(vpdev_hnd hnd)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_BLOCK_CFG, CMD_ACTION_RESET);
}
int CVPcontext::cmd_dev_framerate(vpdev_hnd hnd, eCmdActions eact, FRAMERATE_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_FRAMERATE, eact, (uint8_t*)out_wai, sizeof(FRAMERATE_CONFIG));
}
int CVPcontext::cmd_dev_persist(vpdev_hnd hnd)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_PERSIST, CMD_ACTION_SET);
}
int CVPcontext::cmd_dev_selfcalstate(vpdev_hnd hnd, eCmdActions eact, SELFCAL_CONFIG* out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SELFCAL_STATE, eact, (uint8_t*)out_wai, sizeof(SELFCAL_CONFIG), 0, 0);
}
int CVPcontext::cmd_dev_selfcalsave(vpdev_hnd hnd, eCmdActions eact, BINARY_CONFIG* out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SELFCAL_SAVE, eact, (uint8_t*)out_wai, sizeof(BINARY_CONFIG));
}
int CVPcontext::cmd_dev_resetfc(vpdev_hnd hnd)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_FRAME_COUNT, CMD_ACTION_RESET);
}
//int CVPcontext::cmd_dev_restart(vpdev_hnd hnd)
//{
//	CODETRACE;
//	return cmd_dev(hnd, CMD_INITIALIZE, CMD_ACTION_SET);
//}
int CVPcontext::cmd_dev_serialif(vpdev_hnd hnd, eCmdActions eact, SERIAL_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SERIAL_CONFIG, eact, (uint8_t*)out_wai, sizeof(SERIAL_CONFIG));
}
int CVPcontext::cmd_dev_seuid(vpdev_hnd hnd, eCmdActions eAct, SEUID_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SEUID, eAct, (uint8_t*)out_wai, sizeof(SEUID_CONFIG));
}
int CVPcontext::cmd_dev_sourcerot(vpdev_hnd hnd, int src, eCmdActions eact, SRCROT_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SRC_ROTATION, eact, (uint8_t*)out_wai, sizeof(SRCROT_CONFIG), src);
}
int CVPcontext::cmd_dev_sourcecfg(vpdev_hnd hnd, int src, eCmdActions eact, SRC_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SOURCE_CFG, eact, (uint8_t*)out_wai, sizeof(SRC_CONFIG), src);
}
int CVPcontext::cmd_dev_stationmap(vpdev_hnd hnd, eCmdActions eact, STATION_MAP * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_STATION_MAP, eact, (uint8_t*)out_wai, sizeof(STATION_MAP));
}
int CVPcontext::cmd_dev_stylusmode(vpdev_hnd hnd, eCmdActions eact, STYLUS_CONFIG *out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_STYLUS, eact, (uint8_t*)out_wai, sizeof(STYLUS_CONFIG));
}
int CVPcontext::cmd_dev_syncmode(vpdev_hnd hnd, eCmdActions eact, BINARY_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SYNC_MODE, eact, (uint8_t*)out_wai, sizeof(BINARY_CONFIG));
}
int CVPcontext::cmd_dev_units(vpdev_hnd hnd, eCmdActions eact, UNITS_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_UNITS, eact, (uint8_t*)out_wai, sizeof(UNITS_CONFIG));
}
int CVPcontext::cmd_dev_dualoutput(vpdev_hnd hnd, eCmdActions eact, BINARY_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_DUAL_OUTPUT, eact, (uint8_t*)out_wai, sizeof(BINARY_CONFIG));
}
int CVPcontext::cmd_dev_whoami(vpdev_hnd hnd, WHOAMI_STRUCT * out_wai )
{
	CODETRACE;
	return cmd_dev(hnd, CMD_WHOAMI, CMD_ACTION_GET, (uint8_t*)out_wai, sizeof(WHOAMI_STRUCT));
}
int CVPcontext::cmd_dev_whoami_report(vpdev_hnd hnd)
{
	CODETRACE;
	WHOAMI_STRUCT out_wai;
	int r = 0;
	if (r = cmd_dev(hnd, CMD_WHOAMI, CMD_ACTION_GET, (uint8_t*)&out_wai, sizeof(WHOAMI_STRUCT)))
	{}
	else
	{
		CVPdevIO * pdev = (CVPdevIO*)hnd;  // this was validated by the call to cmd_dev
		pdev->SetWhoAmI(&out_wai);
		CWhoAmI(&out_wai).Report(m_strReports);
		stdformat::converttows(m_wstrReports, m_strReports);
	}
	return r;
}
int CVPcontext::cmd_dev_sourcewhoami(vpdev_hnd hnd, int src, WHOAMI_STRUCT * out_wai)
{
	CODETRACE;
	return cmd_dev(hnd, CMD_SRC_WHOAMI, CMD_ACTION_GET, (uint8_t*)out_wai, sizeof(WHOAMI_STRUCT), src);
}
int CVPcontext::cmd_dev_sourcewhoami_report(vpdev_hnd hnd, int src)
{
	CODETRACE;
	WHOAMI_STRUCT out_wai;
	int r = 0;
	if (r = cmd_dev(hnd, CMD_SRC_WHOAMI, CMD_ACTION_GET, (uint8_t*)&out_wai, sizeof(WHOAMI_STRUCT), src))
	{
	}
	else
	{
		CWhoAmI(&out_wai).ReportSnum(m_strReports);
		stdformat::converttows(m_wstrReports, m_strReports);
	}
	return r;
}
int CVPcontext::cmd_sns_boresight(vpdev_hnd hnd, int sns, eCmdActions eact, BORESIGHT_CONFIG *out_wai, uint32_t arg2)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_BORESIGHT, eact, (uint8_t*)out_wai, sizeof(BORESIGHT_CONFIG), arg2);
}
int CVPcontext::cmd_sns_filter(vpdev_hnd hnd, int sns, eCmdActions eact, FILTER_CONFIG *out_wai, eViperFilterTargets etar)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_FILTER, eact, (uint8_t*)out_wai, sizeof(FILTER_CONFIG), etar);
}
int CVPcontext::cmd_sns_ftt(vpdev_hnd hnd, int sns, eCmdActions eact, ENUM_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_FTT_MODE, eact, (uint8_t*)out_wai, sizeof(ENUM_CONFIG));
}
int CVPcontext::cmd_sns_ftthome(vpdev_hnd hnd, int sns, eCmdActions eact, PNODATA * out_wai, eFTTHomeMethod eMethod)
{
	CODETRACE;
	return E_VPERR_NOT_IMPLEMENTED;// cmd_sns(hnd, sns, CMD_SNS_FTTHOME, eact, (uint8_t*)out_wai, sizeof(PNODATA), eMethod);
}

int CVPcontext::cmd_sns_srcselect(vpdev_hnd hnd, int sns, eCmdActions eact, SRC_SEL_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_SRC_SELECT, eact, (uint8_t*)out_wai, sizeof(SRC_SEL_CONFIG));
}
int CVPcontext::cmd_sns_origin(vpdev_hnd hnd, int sns, eCmdActions eact, ENUM_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_SNS_ORIGIN, eact, (uint8_t*)out_wai, sizeof(ENUM_CONFIG));
}

int CVPcontext::cmd_sns_virtual(vpdev_hnd hnd, int sns, eCmdActions eact, VIRTUAL_SNS_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_SNS_VIRTUAL, eact, (uint8_t*)out_wai, sizeof(VIRTUAL_SNS_CONFIG));
}

int CVPcontext::cmd_sns_blockcfg(vpdev_hnd hnd, int sns, eCmdActions eact, SENSOR_CONFIG *out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_SENSOR_BLOCKCFG, eact, (uint8_t*)out_wai, sizeof(SENSOR_CONFIG));
}
int CVPcontext::cmd_sns_predfilter(vpdev_hnd hnd, int sns, eCmdActions eact, PF_CONFIG * out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_PREDFILTER_CFG, eact, (uint8_t*)out_wai, sizeof(PF_CONFIG));
}
int CVPcontext::cmd_sns_predfilterext(vpdev_hnd hnd, int sns, eCmdActions eact, PF_CONFIG_EXT * out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_PREDFILTER_EXT, eact, (uint8_t*)out_wai, sizeof(PF_CONFIG_EXT));
}
int CVPcontext::cmd_sns_hemisphere(vpdev_hnd hnd, int sns, eCmdActions eact, HEMISPHERE_CONFIG *out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_HEMISPHERE, eact, (uint8_t*)out_wai, sizeof(HEMISPHERE_CONFIG));
}
int CVPcontext::cmd_sns_increment(vpdev_hnd hnd, int sns, eCmdActions eact, INCREMENT_CONFIG *out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_INCREMENT, eact, (uint8_t*)out_wai, sizeof(INCREMENT_CONFIG));
}
int CVPcontext::cmd_sns_tipoffset(vpdev_hnd hnd, int sns, eCmdActions eact, TIP_OFFSET_CONFIG *out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_TIP_OFFSET, eact, (uint8_t*)out_wai, sizeof(TIP_OFFSET_CONFIG));
}
int CVPcontext::cmd_sns_whoami(vpdev_hnd hnd, int sns, WHOAMI_STRUCT * out_wai)
{
	CODETRACE;
	return cmd_sns(hnd, sns, CMD_SENSOR_WHOAMI, CMD_ACTION_GET, (uint8_t*)out_wai, sizeof(WHOAMI_STRUCT));
}
int CVPcontext::cmd_sns_whoami_report(vpdev_hnd hnd, int sns)
{
	CODETRACE;
	WHOAMI_STRUCT out_wai;
	int r = 0;
	if (r = cmd_sns(hnd, sns, CMD_SENSOR_WHOAMI, CMD_ACTION_GET, (uint8_t*)&out_wai, sizeof(WHOAMI_STRUCT)))
	{
	}
	else
	{
		//CVPdevIO * pdev = (CVPdevIO*)hnd;  // this was validated by the call to cmd_dev
		//pdev->SetWhoAmI(&out_wai);
		CWhoAmI(&out_wai).ReportSnum(m_strReports);
		stdformat::converttows(m_wstrReports, m_strReports);
	}

	return r;
}

//{
//	CODETRACE;
//
//	uint8_t * ppayload_out = (uint8_t*)out_wai;
//	uint32_t szout = sizeof(SEUID_CONFIG);
//
//	CVPdevIO * pdev = 0;
//	VALIDATE_DEVHND(pdev, hnd);
//
//	VALIDATE_PARAMS(CMD_SEUID, eAct, out_wai, szout);
//
//	CVPcmd cmd;
//	cmd.Fill(0, CMD_SEUID, eAct, 0, 0, ppayload_out, szout);
//
//	CFrameInfo Frsp;
//
//	int r = 0;
//
//	if (r = SendRecv(hnd, cmd, Frsp))
//	{	}
//	else if (cmd.IsGet())
//	{
//		memcpy(out_wai, Frsp.PCmdPayload(), std::min<uint32_t>(szout, Frsp.CmdPayloadSize()));
//	}
//
//	return r;
//}


int CVPcontext::cmd_dev_contpno(vpdev_hnd hnd, eCmdActions eAct, CONT_FC_CONFIG * infc, ePnoMode eMode)
{
	CODETRACE;
	// putting infc into arg1 here also so it will work with G4Plus
	return cmd_dev(hnd, CMD_CONTINUOUS_PNO, eAct, (uint8_t*)infc, sizeof(CONT_FC_CONFIG), m_dev_USBBufcount/*(infc)? *infc : 0*/, eMode);
}
int CVPcontext::cmd_dev_singlepno(vpdev_hnd hnd, vpFrameInfo &f, ePnoMode eMode)
{
	CODETRACE;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	CVPcmd cmd;
	cmd.Fill(0, CMD_SINGLE_PNO, CMD_ACTION_GET, 0, eMode); 
	if (pdev->OwnerTag())
	{
		//Do nothing
		// Tag can be set during context connectNewUSB();  
		// Useful for experimental or custom adaptations.  
		// cmd.vp2tag(); // convert to a different command possible use
	}
	CFrameInfo Frsp;

	int r = 0;

	if (r = SendRecv(hnd, cmd, Frsp))
	{
	}
	else 
	{
		f = CFrameInfo(Frsp.PCmdPayload(), Frsp.CmdPayloadSize());
	}

	CODETRACEEX;
	return r;
}

int CVPcontext::dev_fifopnof(vpdev_hnd hnd, CFrameInfo &F)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	int r = 0;
	CFrameInfo Fin;
	if (r = pdev->FIFORxPno(Fin))
	{	}
	else
	{
		F = Fin;
		F.pF = Fin.PPnoBody(); F.uiSize = Fin.PnoBodySize();
		// (Fin.PPnoBody(), Fin.PnoBodySize(), Fin.devfc(), Fin.err(), Fin.TS());
	}

	return r;
}

int CVPcontext::dev_fifopnofraw(vpdev_hnd hnd, CFrameInfo &F)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	return pdev->FIFORxPno(F);
}

int CVPcontext::dev_fifo_count(vpdev_hnd hnd, uint32_t & fc, uint32_t & bc)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	return pdev->FIFOPnoCount( fc, bc );
}

int CVPcontext::dev_fifo_readrawbuf(vpdev_hnd hnd, uint32_t & fc, uint8_t *& pbuf, uint32_t & size)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);
	VALIDATE_BUF_PARAMS(pbuf, size);

	int r = 0;
	uint32_t bytes_copied = 0;
	uint32_t i = 0;

	CFrameInfo Fin;

	if (r = pdev->FIFORxPnoCount(Fin, fc, size))
	{	}
	else
	{
		memcpy(pbuf, Fin.pF, Fin.uiSize);
		size = Fin.uiSize;
	}

	return r;
}

int CVPcontext::dev_lastpnof(vpdev_hnd hnd, CFrameInfo &F)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	int r = 0;
	CFrameInfo Fin;
	if (r = pdev->LastRxPno(Fin))
	{
	}
	else
	{
		F = Fin;
		F.pF = Fin.PPnoBody(); F.uiSize = Fin.PnoBodySize();
		//F = CFrameInfo(Fin.PPnoBody(), Fin.PnoBodySize());
		//F.uiFCountRx = Fin.uiFCountRx;
	}

	return r;
}

int CVPcontext::dev_lastpnofraw(vpdev_hnd hnd, CFrameInfo &F)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	return pdev->LastRxPno(F);
}

int CVPcontext::dev_resetpno(vpdev_hnd hnd)
{
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	pdev->PurgePnoFrames();
	return 0;
}

int CVPcontext::dev_nextpnof(vpdev_hnd hnd, CFrameInfo &F)
{
	CODETRACE;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	CODETRACEEX;
	return pdev->NextRxPno(F);
}

/**
* @param[in] hnd device handle
* @param[in] cmd raw command struct (crc not calculated)
* @param[out] Frsp frame info of returned cmd response
*
* outbound buffer is owned by context, prepared from cmd struct prior to sending.
* inbound buffer is owned by dev object, populated with response received.
* 
* Frsp param points to dev's inbound buffer on exit
*/
int CVPcontext::SendRecv(vpdev_hnd hnd, CVPcmd& cmd, CFrameInfo & Frsp, bool bWantRsp)
{
	int r = 0;
	CVPdevIO * pdev = 0;
	VALIDATE_DEVHND(pdev, hnd);

	bool bTry = true;
	int retry_count = 0;

	while (bTry)
	{
		retry_count++;
		bTry = false;

		std::lock_guard<std::mutex> lck(m_mtxTxData);

		cmd.Prepare(m_txbuf, m_ntxcount);

		CFrameInfo Fout(m_txbuf, m_ntxcount);

		if (r = pdev->CmdIO(Fout, Frsp, bWantRsp))
		{
			/*if (r == E_VPERR_CMDRSP_MISMATCH)
			{
				if (retry_count < 2)
					bTry = true;
			}*/
		}
		else
		{
		}
	}
	return r;
}
