#include "VPdevIO.h"
#include "VPtracei.h"
#include "VPusbIO.h"
#include "VPserIO.h"
#include "VPcmdErr.h"

//#define VALIDATE_USBCNX {if ((m_pusb == 0) || !(m_pusb->IsOpen())) {CODETRACEEX; return E_DEVIOERR_NO_CNX;} }
///void CALLBACK USB_CB(void * pinfo, uint8_t *pbuf, uint32_t nbytes);
//#define VALIDATE_SERCNX {if ((m_pser == 0) || !(m_pser->IsOpen())) {CODETRACEEX; return E_DEVIOERR_NO_CNX;} }

#define VALIDATE_CNX { 	if (m_pusb) { if (!(m_pusb->IsOpen())) { CODETRACEEX; return E_DEVIOERR_NO_CNX; } }		\
						else if (m_pser) { if (!(m_pser->IsOpen())){ CODETRACEEX; return E_DEVIOERR_NO_CNX; } }	\
						else { CODETRACEEX; return E_DEVIOERR_NO_CNX; }	}



//int CVPdevIO::validate_cnx()
//{
//	if ((m_pusb) && !(m_pusb->IsOpen())) 
//		{ CODETRACEEX; return E_DEVIOERR_NO_CNX; }
//	else if ((m_pser) && !(m_pser->IsOpen())) 
//		{ CODETRACEEX; return E_DEVIOERR_NO_CNX; }
//	else 
//		{ CODETRACEEX; return E_DEVIOERR_NO_CNX; }
//
//	return 0;
//}
 
CVPdevIO::CVPdevIO(CVPcontext *pctx) 
	: m_pctx(pctx)
	, m_pusb(0)
	, m_pser(0)
	, m_uiPnoHostFrameCount(0)
	, m_uiPnoDropCount(0)
	, m_cmdTimeOutMS(VPUSB_DEFAULT_CMD_TIMEOUT_MS)
	, m_bNewRxCmdReady(false)
	, m_bNewRxPnoReady(false)
	, m_eTraceLev(0)
	, m_uiOwnerTag(0)
	, m_uiCmdPreamble(VIPER_CMD_PREAMBLE)
	, m_uiPnoPreamble(VIPER_PNO_PREAMBLE)
	, m_bUseTimestamp(false)
	, m_bAllowFrameErrs(false)
	, m_uiUSBBufcount(0)
	, m_pClientPnoCallback(0)
	, m_pCCBInfo(0)


{
//#ifdef _DEBUG
	SetTraceLev(VPCMD_TRACE_3_NORMALIO);
//#endif	

	InitIO();
	Initdev();
}

CVPdevIO::~CVPdevIO()
{
	DisconnectUSB();
	DisconnectSerial();
}

void CVPdevIO::SetTraceLev(int t)
{
	m_eTraceLev = t;
	int io_t = -1;

	trTracefHP0(__FUNCTION__ " : nT = %d\r\n", t);
	switch (t)
	{
		case VPCMD_NOTRACE:					//!< No Trace output
		case VPCMD_TRACE_1_MIN:				//!< Minimal trace + errors
		case VPCMD_TRACE_2_COMPACT:
			io_t = VPCMD_NOTRACE;
			break;

		case VPCMD_TRACE_3_NORMALIO:		//!< Normal I/O trace + errors, no P&O 
		case VPCMD_TRACE_4_ALLIO:		    //!< All Normal I/O trace + errors, includes P&O
			io_t = VPCMD_TRACE_1_MIN;
			break;

		case VPCMD_TRACE_5_CODETRACE:	//= 5	//!< Code trace + I/O + errors
		case VPCMD_TRACE_7_VERBOSE:		// = 7	//!< Verbose trace output, from VPcmdIF and underlying USB & serial IO packakges.
			io_t = t;
			break;
	}

	if (m_pusb)
		m_pusb->SetTrace((t > 0), io_t);
		//m_pusb->SetTrace((t > 0), CLAMP(t - 2, 0, t - 1));

	if (m_pser)
		m_pser->SetTrace((t > 0), io_t);
		//m_pser->SetTrace((t > 0), CLAMP(t - 2, 0, t - 1));
}

void CVPdevIO::InitCmdTimeoutMS()
{
	CmdTimeoutMS() = VPUSB_DEFAULT_CMD_TIMEOUT_MS;
}

inline int CVPdevIO::Write(uint8_t * pbuf, int & count)
{
	// succeeds if either m_pusb or m_pser non zero and is OPEN
	VALIDATE_CNX; 
	int r = 0;
	if (m_pusb)
	{  
		r = m_pusb->Write(pbuf, count);
		if (r)
		{
			r = (r << 16) | E_DEVIOERR_USB_WRITERR;
		}
	}
	else
	{
		r = m_pser->Write(pbuf, count);
		if (r)
		{
			r = (r << 16) | E_DEVIOERR_SER_WRITERR;
		}
	}

	return r;
}

inline int CVPdevIO::Read(uint8_t * pbuf, int & count)
{
	// succeeds if either m_pusb or m_pser non zero and is OPEN
	VALIDATE_CNX;
	int r = 0;
	if (m_pusb)
	{
		r = m_pusb->Read(pbuf, count);
		if (r)
			r = (r << 16) | E_VPERR_LIBUSB;

	}
	else
	{
		r = m_pser->Read(pbuf, count);
		if (r)
			r = (r << 16) | E_VPERR_SERIO;
	}
	return r;
}


void CVPdevIO::InitIO()
{
	//memset(hdr_inbuffer, 0, sizeof(hdr_inbuffer));
	memset(pno_inbuffer, 0, sizeof(pno_inbuffer));
	memset(cmd_inbuffer, 0, sizeof(cmd_inbuffer));

	iPnoRawIndex = 0;
	iCmdRawIndex = 0;
	p_nextPno_inbuffer = pno_inbuffer;
	p_nextCmd_inbuffer = cmd_inbuffer;

	m_uiPnoHostFrameCount = 0;
	m_uiPnoDropCount = 0;

	PurgePnoFrames();
	PurgeCmdFrames();

	DisconnectUSB();	// closes and deletes
	DisconnectSerial(); // closes and deletes

}

void CVPdevIO::Initdev()
{
	memset((vpDevInfo *)this, 0, sizeof(vpDevInfo));
}

const char * CVPdevIO::ErrorStr(int32_t ierr)
{

	if (usb_err(vp_loerr(ierr)))
	{
		return ErrorStr_USB(vp_hierr(ierr));
	}
	else if (ser_err(vp_loerr(ierr)))
	{
		return ErrorStr_SER(vp_hierr(ierr));
	}
	else
		return "";

}
const char * CVPdevIO::ErrorStr_USB(int16_t e)
{
	return CVPusbIO::ErrorStr(e);
}
const char * CVPdevIO::ErrorStr_SER(int16_t e)
{
	return CVPserIO::ErrorStr(e);
}
void CVPdevIO::Report(std::string & s)
{
	char buf[200];
	memset(buf, 0, sizeof(buf));

	sprintf_s(buf, sizeof(buf), "% 4u   %-16s   %s", SEUID(), SerialNum(), AscID());

	s += buf;
}

#define USBERR(r) (r ? ((r << 16) | E_VPERR_LIBUSB) : r)
#define SERERR(r) (r ? ((r << 16) | E_VPERR_SERIO) : r)
// Find loaded vid:pid USB device
// If USB connection or instance already exists, destroy it and then create new connection
int CVPdevIO::ConnectUSB(uint16_t vid, uint16_t pid)
{
	MAXCOMMAND max;
	max.cmd.action = 0;
	uint32_t maxreadbufsize = sizeof(MAXCOMMAND);
	uint32_t maxpno = sizeof(MAXPNO);
	uint32_t minpno1 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (1 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno2 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (2 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno3 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (3 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno4 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (4 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno5 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (5 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno6 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (6 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno7 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (7 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno8 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (8 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno9 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (9 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno10 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (10 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno11 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (11 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno12 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (12 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno13 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (13 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno14 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (14 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);
	uint32_t minpno15 = sizeof(VP_FRAME_HDR) + sizeof(SEUPNO) + (15 * sizeof(SENFRAMEDATA)) + sizeof(VPCRC);

	uint32_t maxpay = sizeof(max.payload);
	uint32_t paysize1 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(HEMISPHERE_CONFIG  ) + sizeof(VPCRC);
	uint32_t paysize2 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(FILTER_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize3 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(INCREMENT_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize4 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(TIP_OFFSET_CONFIG  ) + sizeof(VPCRC);
	uint32_t paysize5 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(BORESIGHT_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize6 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(WHOAMI_STRUCT	  ) + sizeof(VPCRC);
	uint32_t paysize7 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(FRAMERATE_CONFIG   ) + sizeof(VPCRC);
	uint32_t paysize8 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(UNITS_CONFIG       ) + sizeof(VPCRC);
	uint32_t paysize9 = sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(SRCROT_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize10 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(BINARY_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize11 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(STATION_MAP		  ) + sizeof(VPCRC);
	uint32_t paysize12 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(STYLUS_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize13 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(VIPER_SEUID		  ) + sizeof(VPCRC);
	uint32_t paysize14 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(BINARY_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize15 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(SERIAL_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize16 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(BIT_STRUCT		  ) + sizeof(VPCRC);
	uint32_t paysize17 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(CONT_FC_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize18 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(FTTMODE_CONFIG	  ) + sizeof(VPCRC);
	uint32_t paysize19 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(SRC_CONFIG		  ) + sizeof(VPCRC);
	uint32_t paysize20 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(PF_CONFIG)		    + sizeof(VPCRC);
	uint32_t paysize21 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(PF_CONFIG_EXT)	    + sizeof(VPCRC);
	uint32_t paysize22 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(SRC_SEL_CONFIG)	    + sizeof(VPCRC);
	uint32_t paysize23 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(SNS_ORIG_CONFIG)    + sizeof(VPCRC);
	uint32_t paysize24 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(VIRTUAL_SNS_CONFIG) + sizeof(VPCRC);
	uint32_t paysize25 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(MAP_INFO_CONFIG)    + sizeof(VPCRC);
	uint32_t paysize26 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(COMP2_PAYLOAD)	    + sizeof(VPCRC);
	uint32_t paysize27 =sizeof(VP_FRAME_HDR) + sizeof(SEUCMD) + sizeof(NAK_INFO)		    + sizeof(VPCRC);
		Disconnect();	 

	int r = 0;
	CVPusbIO *pusb = new CVPusbIO();

	if (pusb == 0)
		return E_CTXERR_MEMALLOC_ERROR;

	pusb->SetTrace((m_eTraceLev>0), m_eTraceLev-1); // so only codetrace and above will cause usbio hexdumps

	if (r = pusb->DiscoverVidPid(vid, pid))
	{
		if (r <= -99)
			r = E_VPERR_NOTFOUND;
		else
			r = USBERR(r);

		delete pusb;
	}
	else if (r = pusb->ReadCont())
	{
		pusb->Close();
		delete pusb;
		r = USBERR(r);
	}
	else
	{
		pusb->SetMsgCallback((VP_DATAREADY_CALLBACK)USBSer_CB, (void*)this);
		m_pusb = pusb;

	}

	return r;
}
int CVPdevIO::Disconnect()
{
	if (m_pusb)
		return DisconnectUSB();

	else
		return DisconnectSerial();
}
int CVPdevIO::DisconnectUSB()
{
	int r = 0;
	if (m_pusb)
	{
		//r = m_pusb->ReadContStop(); // Close() will stop the cont loop.  Inadviseable to do it here.
		r = m_pusb->Close();
		delete m_pusb;
		m_pusb = 0;
	}
	return USBERR(r);
}
int CVPdevIO::ConnectSerial( int32_t serport, uint32_t sercfg)
{
	Disconnect();
	int r = 0;
	CVPserIO *pser = new CVPserIO( sercfg );

	if (pser == 0)
		return E_CTXERR_MEMALLOC_ERROR;

	pser->SetTrace((m_eTraceLev>0), m_eTraceLev-1); // so only codetrace and above will cause serio hexdumps

	if (serport > 0)
	{
		r = pser->Open(serport);
	}
	else
		r = pser->DiscoverPort();

	if (r)
	{
		delete pser;
	}
	else if (r = pser->ReadCont())
	{
		pser->Close();
		delete pser;
	}
	else
	{
		pser->SetMsgCallback((VP_DATAREADY_CALLBACK)USBSer_CB, (void*)this);
		m_pser = pser;
	}

	return SERERR(r);
}
int CVPdevIO::DisconnectSerial()
{
	int r = 0;
	if (m_pser)
	{
		//r = m_pusb->ReadContStop(); // Close() will stop the cont loop.  Inadviseable to do it here.
		r = m_pser->Close();
		delete m_pser;
		m_pser = 0;
	}
	return SERERR(r);
}

int CVPdevIO::EnableTimestamp(bool bEn)
{
	m_bUseTimestamp = bEn;
	return 0;
	//std::ratio tsdur = std::ratio<1,ratio>;

	//typedef duration<long long, micro> microseconds;
	//typedef duration<long long, milli> milliseconds;
}
int CVPdevIO::AllowPnoFramerrs(bool bAllow)
{
	m_bAllowFrameErrs = bAllow;
	return 0;

}
int CVPdevIO::USBbuffer(uint32_t count)
{
	m_uiUSBBufcount = count;
	return 0;
}
/**
* @param[in]  Fcmd      outbound command frame fully constructed ready to send
* @param[out] Frsp      inbound command response frame obj to be populated with command response
* @param[in]  bWantRsp  if false, return immediately, do not read response from device. Default true.
*
* device i/o checks for valid response frame CRC, command, and NAK
* on successful read, response buffer info is placed in Frsp object param
*/
int CVPdevIO::CmdIO(const CFrameInfo & Fcmd, CFrameInfo & Frsp, bool bWantRsp /*= true*/)
{
	CODETRACE;

	VALIDATE_CNX;

	int r = 0;
	int iSize = Fcmd.uiSize;

	std::lock_guard<std::mutex> lck(mtxCmdIO);
	m_bNewRxCmdReady = false;  

	if (bWantRsp)
	{
		RxCmdVecAdd((eViperCmds)Fcmd.cmd());
	}


	trTraceFBufcmd(Fcmd.pF, Fcmd.uiSize, true, __FUNCTION__ " writing ");

	//if (r = m_pusb->Write(Fcmd.pF, iSize))
	if (r = Write(Fcmd.pF, iSize))
	{

	}
	else if (bWantRsp)
	{
		CFrameInfo lfin;
		//for now, doing synchronous cmd i/o.  goal: async using cmd & pno queues
		//if (r = WaitCmdRespSync(lfin, Fcmd))
		if (r = WaitCmdRespA(lfin, Fcmd))
		{
		}
		else if (lfin.IsNull())
		{
		}
		else if (lfin.IsNak())
		{
			r = E_VPERR_NAK_RESP;
		}
		else
			Frsp = lfin;

		if (r)
		{
			trTraceErr(m_pctx, r, "io err begin", __FUNCTION__, __LINE__); 
			trTraceTxII0(m_pctx, Fcmd.pF, Fcmd.uiSize, __FUNCTION__, __LINE__);
			trTraceRxII0(m_pctx, lfin.pF, lfin.uiSize, __FUNCTION__, __LINE__);
			//trTraceFBuf(Fcmd.pF, Fcmd.uiSize, true, __FUNCTION__ " wrote ");
			//trTraceFBuf(lfin.pF, lfin.uiSize, true, __FUNCTION__ " read ");
			trTraceErr(m_pctx, r, "io err end", __FUNCTION__, __LINE__);
		}
	}

	CODETRACEEX;
	return r;
}
/**
* @param[out] Frsp      inbound empty frame obj will be populated with command response 
* @param[in]  Fcmd      previously sent cmd frame used for validating response
*
* device i/o checks for valid response frame header, size, CRC, matching command field
* NOT USED ??
*/
int CVPdevIO::WaitCmdRespSync(CFrameInfo & Frsp, const CFrameInfo & Fcmd)
{
	VALIDATE_CNX;

	int r = 0;

	do
	{
		int iSize = RAWBUFSIZE;

		//if (r = m_pusb->Read(p_nextCmd_inbuffer, iSize))
		if (r = Read(p_nextCmd_inbuffer, iSize))
		{

		}
		else if (!(CVPcmd::ValidateLeader(p_nextCmd_inbuffer, m_uiCmdPreamble, m_uiPnoPreamble)))
		{
			r = E_DEVIOERR_FRAME_HEADER;
		}
		else if (!(CVPcmd::ValidateCRC(p_nextCmd_inbuffer)))
		{
			r = ( E_VPERR_INVALID_CRC | (Fcmd.cmd() << 16) );
		}
		else
		{
			CFrameInfo lfin(p_nextCmd_inbuffer, iSize);
			if (!(lfin.IsCmd()))  
			{
				// it's pno.. dropping it for now, could copy it into pno buffer though...
				continue;
			}
			// its a command, leave it in the cmd buf
			iCmdRawIndex += iSize;
			p_nextCmd_inbuffer = &(cmd_inbuffer[iCmdRawIndex]);

			if  (lfin.cmd() == Fcmd.cmd())
			{
				Frsp = lfin;
				break;
			}
		}

	} while (r == 0);

	return r;
}

int CVPdevIO::WaitCmdRespA(CFrameInfo &Fin, const CFrameInfo & Fcmd)
{
	CODETRACE;

	VALIDATE_CNX;
	//Cg4pPerfCounter lperf; lperf.Start();
	//m_perfCmd.Start();
	//std::cout << __FUNCTION__ << __LINE__ << " m_perfCmd: " << m_perfCmd.IntervalNR() << " Cg4pBuffIO::WaitCmdResp Entered" << std::endl;
	int r = 0;

	CFrameInfo ftmp;
	eViperCmds eCmd = (eViperCmds)Fcmd.cmd();


	std::unique_lock<std::mutex> lck(mtxRxCmd);
	if (!(cvRxCmd.wait_for(lck, std::chrono::milliseconds(CmdTimeoutMS()), [this] {return m_bNewRxCmdReady; })))// == std::cv_status::timeout)
	{
		CODETRACE;

		r = (int)E_VPERR_TIMEOUT;// E_CTXERR_CMDRSP_TIMEOUT;
		trTracefHP("%d-%s : %s : %s waiting for response to %s(%x) %s(%x)\r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			trErrStr(r),
			trCmdStr(Fcmd.cmd()), Fcmd.cmd(),
			trActionStr(Fcmd.action()), Fcmd.action());

		// check the queue just in case the frame arrived before we tried to wait for it.
		if (!(RxCmdDequeue(Fin, Fcmd.cmd(), r)))
		{
			//_INVALID_CONDITION(r, 0, 0)
		}
		else if (Fin.err())
		{
			r = Fin.err();
		}
		else
		{
			r = 0;
		}

		RxCmdVecRemove(eCmd);
	}
	else
	{
		CODETRACE;
		trTracefHP5("%d-%s : %s : Waiting for response to %s(%x) %s(%x) : m_bNewRxCmdReady = %s\r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			//trErrStr(r),
			trCmdStr(Fcmd.cmd()), Fcmd.cmd(),
			trActionStr(Fcmd.action()), Fcmd.action(),
			m_bNewRxCmdReady ? "true" : "false");

		if (!(RxCmdDequeue(Fin, Fcmd.cmd(), r)))
		{
			//_INVALID_CONDITION(r, 0, 0)
		}
		else if (Fin.err())
		{ 
			r = Fin.err();
		}
		else
		{
		}
	}

	CODETRACEEX;

	return r;
}

int CVPdevIO::WaitPnoRespA(CFrameInfo &Fin)
{
	CODETRACE;

	VALIDATE_CNX;
	int r = 0;

	m_bNewRxPnoReady = false;


	std::unique_lock<std::mutex> lck(mtxRxPno);
	if (!(cvRxPno.wait_for(lck, std::chrono::milliseconds(CmdTimeoutMS()), [this] {return m_bNewRxPnoReady; })))// == std::cv_status::timeout)
	{
		CODETRACE;

		r = (int)E_VPERR_TIMEOUT;// E_CTXERR_CMDRSP_TIMEOUT;
		trTracefHP("%d-%s : %s : %s waiting for next pno frame\r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			trErrStr(r));

	}
	else
	{
		CODETRACE;
		trTracefHP5("%d-%s : %s : %s waiting next pno frame : m_bNewRxPnoReady = %s\r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			trErrStr(r),
			m_bNewRxPnoReady ? "true" : "false");

		if (!(RxPnoDequeueLAST(Fin)))
		{
			_INVALID_CONDITION(r, 0, 0)
		}
		else if (Fin.err())
		{
			r = Fin.err();
		}
		else
		{

			//if (Fin.pF)
			//{
			//	memcpy(Fin.pF, ftmp.pF, ftmp.uiSize);
			//	Fin.uiSize = ftmp.uiSize;
			//	Fin.iFrameErr = ftmp.iFrameErr;
			//}
			//else
			//{
			//	Fin.uiSize = 0;
			//}
		}
	}

	CODETRACEEX;

	return r;
}

// raw data buffer intake. Handles inbound data from any source (USB/Serial/WiFi etc)
int CVPdevIO::DataReady(CFrameInfo &fin)
{
	CODETRACE;
	VALIDATE_CNX;
	// test for valid frame and copy it into the cmd or pno buffer.
	// put frameinfo into cmd or pno rxqueue
	int r = 0;

	//if (!(CVPcmd::ValidateLeader(fin.pF, m_uiCmdPreamble, m_uiPnoPreamble)))
	if (!(CVPcmd::ValidatePreamble((uint32_t*)fin.pF, m_uiCmdPreamble, m_uiPnoPreamble)))
	{
		r = E_DEVIOERR_FRAME_HEADER; // this error is never captured
		trTraceErr(NULL, r, "", __FUNCTION__, __LINE__, 5);
		//trTraceFBuf(fin.pF, (size_t)fin.uiSize, true, __FUNCTION__ " received " ); // 
		trTraceFBuf(fin.pF, (size_t)8, true, __FUNCTION__ " received "); // 
		if (m_bAllowFrameErrs)
		{
			(*(uint32_t*)fin.pF) = m_uiPnoPreamble;
			fin.setpnoframerr();
		}
	}
	if ((r == 0) || (m_bAllowFrameErrs))
	{
		if (!(CVPcmd::ValidateSize((uint32_t*)fin.pF))) //assumes preamble is good.
		{
			r = E_DEVIOERR_FRAME_HEADER;
			trTraceErr(NULL, r, "", __FUNCTION__, __LINE__, 5);
			trTraceFBuf(fin.pF, (size_t)8, true, __FUNCTION__ " received "); // 
			if (m_bAllowFrameErrs)
			{
				fin.setpnoframerr();
			}
		}
	}
	//else
	if ((r == 0) || (m_bAllowFrameErrs))
	{
		//if (!(CVPcmd::ValidateCRC(fin.pF)))
		// only test crc if preamble and size are good.
		if ( !(fin.iFrameErr) && !(CVPcmd::ValidateCRC(fin.pF)) )
		{
			if (((uint32_t*)fin)[2] != 0xffff )
				r = E_VPERR_INVALID_CRC;

			if (m_bAllowFrameErrs)
				fin.setpnocrcerr();
//			trTraceErr(NULL, r, "", __FUNCTION__, __LINE__, 5);
		}

		uint8_t *p_inbuffer = 0;
		uint32_t inbytes = 0;

		if (fin.IsCmd())
		{
			if (r)
				trTraceErr(NULL, r, "", __FUNCTION__, __LINE__, 2);
			trTraceFBufcmd(fin.pF, (size_t)fin.uiSize, true, __FUNCTION__ " received ");

			// copy to cmd buffer
			memcpy(p_nextCmd_inbuffer, fin.pF, (size_t)fin.uiSize);

			// if cmd frame has an error, mark it so that waiting thread can report it
			if (r)
				r |= (fin.cmd() << 16);

			RxCmdEnqueue(CFrameInfo(p_nextCmd_inbuffer, fin.uiSize, r));

			//advance pointers
			iCmdRawIndex += fin.uiSize;
			if (iCmdRawIndex >= RAWBUFSIZE)
			{
				iCmdRawIndex = 0;
			}
			p_nextCmd_inbuffer = &(cmd_inbuffer[iCmdRawIndex]);
		}
		else if ((r == 0) || (m_bAllowFrameErrs))  // don't keep pno frames with errors 
		{
			fin.iFrameErr = r;

			if (m_pClientPnoCallback)
			{
				m_pClientPnoCallback(m_pCCBInfo, fin.pF, fin.uiSize);
			}
			else
			{
				//trTraceFBuf(fin.pF, (size_t)fin.uiSize, true, __FUNCTION__ " received ", 4);
				trTraceFBufpno(fin.pF, (size_t)fin.uiSize, true, __FUNCTION__ " received ", 4);
				// copy to pno buffer
				memcpy(p_nextPno_inbuffer, fin.pF, (size_t)fin.uiSize);

				RxPnoEnqueue(CFrameInfo(p_nextPno_inbuffer, fin.uiSize));

				// copy to pno buffer
				iPnoRawIndex += fin.uiSize;
				if (iPnoRawIndex >= RAWBUFSIZE)
				{
					iPnoRawIndex = 0;
				}
				p_nextPno_inbuffer = &(pno_inbuffer[iPnoRawIndex]);
			}
		}
		//else
		if (r != 0)
		{
			//possibly pno cmd, CRC error
			//trTraceHexBuf(fin.pF, fin.uiSize, true, "CRC_ERROR :");
			trTraceErr(NULL, r, "", __FUNCTION__, __LINE__, 7);
			trTraceFBuf(fin.pF, (size_t)fin.uiSize, true, __FUNCTION__ " received ", 7); // 

		}
	}

	CODETRACEEX;
	return r;
}

// Also used for Serial
void CALLBACK CVPdevIO::USBSer_CB(void * pinfo, uint8_t *pbuf, uint32_t nbytes)
{

	//trTraceFBuf(pbuf, nbytes, true, __FUNCTION__ " received ");
	if ((pinfo == 0) || (pbuf == 0) || (nbytes == 0))
		return;

	//CFrameInfo fin(pbuf, nbytes);
	((CVPdevIO*)pinfo)->DataReady(CFrameInfo(pbuf, nbytes));
	CODETRACEEX;

}

void CVPdevIO::RxCmdEnqueue(CFrameInfo & F)
{
	CODETRACE;
	//Cg4pPerfCounter lperf; lperf.Start();
	//std::cout << "lperf: " << lperf.Interval() << " Cg4pBuffIO::RxCmdENqueue Entered" << std::endl;
	//std::cout << __FUNCTION__ << __LINE__ << " m_perfCmd: " << m_perfCmd.IntervalNR() << " Cg4pBuffIO::RxCmdENqueue Entered " << std::endl;

	std::lock_guard<std::mutex> lck(mtxRxCmdQueue);
	//std::cout << "lperf: " << lperf.Interval() << " Cg4pBuffIO::RxCmdENqueue lock acquired" << std::endl;

	// only que up this cmd response if we are actually waiting for it.
	//if (RxCmdVecFind((eViperCmds)F.cmd()))
	{
		queRxCmdFrames.push(F);// CFrameInfo(p_nextPno_inbuffer, uiFrameSize, uiRxPnoFrameCount, uiCrcErrCount, uiFrameErrCount));
								//std::cout << __FUNCTION__ << __LINE__ << " m_perfCmd: " << m_perfCmd.IntervalNR() << " frame pushed onto vector " << std::endl;
		if (queRxCmdFrames.size() > FINFO_QUE_SIZE)
		{
			queRxCmdFrames.pop();
		}
		trTracefHP5("%d-%s : %s: Enqued response to cmd %s(%x) %s(%x) : Setting m_bNewRxCmdReady=true, calling notify_all()\r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			trCmdStr(F.cmd()), F.cmd(),
			trActionStr(F.action()), F.action(),
			queRxCmdFrames.size());
		m_bNewRxCmdReady = true;
		cvRxCmd.notify_all();
	}
	/*else
	{
		trTracefHP("%d-%s : %s: Not waiting for cmd %s(%x) %s(%x): Discarding\r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			trCmdStr(F.cmd()), F.cmd(),
			trActionStr(F.action()), F.action());
	}*/

	CODETRACEEX;
}

void CVPdevIO::RxPnoEnqueue(CFrameInfo & F)
{
	CODETRACE7;
	std::lock_guard<std::mutex> lck(mtxRxPnoQueue);

	//F.uiFCountRx = ++m_uiPnoHostFrameCount;

	if (m_bUseTimestamp)
	{
		using namespace std::chrono;
		//auto now = std::chrono::system_clock::now();
		//auto now_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(now);
		//auto value = now_ms.time_since_epoch(); //value has type std::chrono::milliseconds
		//F.TS() = value.count();

		const auto timeNowSteadyClock = std::chrono::steady_clock::now();
		auto timeDurSteadyClock = timeNowSteadyClock.time_since_epoch(); //duration
		F.TS() = timeDurSteadyClock.count(); //steady clock counts since epoch

		//const auto nowstd = std::chrono::steady_clock::now();
		//auto nowstd_ms = std::chrono::time_point_cast<std::chrono::milliseconds>(nowstd);
		//auto valuestd = nowstd_ms.time_since_epoch(); //value has type std::chrono::milliseconds
		//F.TS() = valuestd.count();

		//std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
		//time_t now_t = std::chrono::system_clock::to_time_t(now);
		//F.TS() = now_t;
		

		//auto value = std::chrono::duration_cast<std::chrono::milliseconds>(epoch);
		//uint64_t duration = value.count(); //duration = num milliseconds since epoch of system_clock

		//::QueryPerformanceCounter((LARGE_INTEGER*)&(F.qpc));
	}

	//m_lastPnoFInfo = F;

	queRxPnoFrames.push_back(F);// CFrameInfo(p_nextPno_inbuffer, uiFrameSize, uiRxPnoFrameCount, uiCrcErrCount, uiFrameErrCount));
	if (queRxPnoFrames.size() > FINFO_QUE_SIZE)
	{
		queRxPnoFrames.pop_front();
		trTracefHP0("%d-%s : %s: Unclaimed Rx Pno Frames exceeded %d : popping frame FC %d\r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			FINFO_QUE_SIZE, F.devfc());
	}

	// perf metric stuff used previously by g4plus
	uint32_t drops;
	if (m_uiPnoHostFrameCount != 0)
	{
		drops = (F.devfc() - m_lastPnoFInfo.devfc() - 1);
		m_uiPnoDropCount += drops;
		if (m_bAllowFrameErrs)
			F.setpnodrops(drops);

		//if (drops != 0)
		//{
		//	trTracefHP0("%d-%s : %s: Enqued pno : %u dropped %u totaldropped, frame %u, prev %u, size %u\r\n",
		//		tag_cfg.seuid, tag_AscID, __FUNCTION__,
		//		drops, m_uiPnoDropCount, F.devfc(),
		//		m_lastPnoFInfo.devfc(), F.uiSize);
		//}
	}
	F.uiFCountRx = ++m_uiPnoHostFrameCount;
	m_lastPnoFInfo = F;

	m_bNewRxPnoReady = true;
	cvRxPno.notify_all();

	CODETRACEEX7;

}

bool CVPdevIO::RxCmdDequeue(CFrameInfo & Fin, uint32_t cmd, int32_t & iErr)
{
	CODETRACE;
	//Cg4pPerfCounter lperf; lperf.Start();
	//std::cout << "lperf: " << lperf.Interval() << " Cg4pBuffIO::RxCmdDequeue Entered" << std::endl;
	//std::cout << __FUNCTION__ << __LINE__ << " m_perfCmd: " << m_perfCmd.IntervalNR() << " Cg4pBuffIO::RxCmdDequeue Entered " << std::endl;

	bool bRet = false;
	iErr = E_CTXERR_NONE; // 0

	std::lock_guard<std::mutex> lck(mtxRxCmdQueue);
	//std::cout << "lperf: " << lperf.Interval() << " Cg4pBuffIO::RxCmdDequeue lock acquired" << std::endl;

	if (queRxCmdFrames.empty())
	{
		//_INVALID_CONDITION(iErr, 0)
		iErr = E_CTXERR_DEV_RECV_QUEUE_EMPTY;
		std::string s; Report(s);
		trTracefHPx("%d-%s : %s : cmd %s(%x) : Error : %s \r\n",
			tag_cfg.seuid, tag_AscID, __FUNCTION__,
			trCmdStr(cmd), cmd,
			trErrStr(iErr));
	}
	else
	{
		std::queue<CFrameInfo> queTemp;
		//check all available rx'd cmd frames or until match found
		while ((!(queRxCmdFrames.empty())) && !bRet)
		{
			CFrameInfo f = queRxCmdFrames.front();
			if (f.uiSize && f.cmd() == cmd)
			{
				bRet = true;
				Fin = f;
				queRxCmdFrames.pop();
				trTracefHP5("%d-%s : %s:%d : cmd %s(%x) MATCH popped\r\n", tag_cfg.seuid, tag_AscID, __FUNCTION__, __LINE__,
					trCmdStr(cmd), cmd);

				RxCmdVecRemove((eViperCmds)f.cmd());
			}
			else
			{
				//pop off frames that don't match
				iErr = E_VPERR_CMDRSP_MISMATCH;
				trTracefHP0("%d-%s : %s:%d: cmd %s(%x) NO MATCH (%0x) popped\r\n", tag_cfg.seuid, tag_AscID, __FUNCTION__, __LINE__,
					trCmdStr(cmd), cmd, f.cmd());
				trTraceRxII0(m_pctx, f.pF, f.uiSize, __FUNCTION__, __LINE__);
				queRxCmdFrames.pop();
			}

		}
	}

	CODETRACEEX;
	//std::cout << lperf.Interval() << " Cg4pBuffIO::RxCmdDequeue Exiting" << std::endl;
	return bRet;
}

bool CVPdevIO::RxCmdVecAdd(eViperCmds cmd)
{
	bool bRet = true;

	std::lock_guard<std::mutex> lck(m_mtxVecRxCmds);
	m_vecRxCmds.push_back((eViperCmds)cmd);

	return bRet;
}

bool CVPdevIO::RxCmdVecFind(eViperCmds cmd)
{
	bool bRet = false;
	std::lock_guard<std::mutex> lck(m_mtxVecRxCmds);
	std::vector<eViperCmds>::iterator it;

	for (it = m_vecRxCmds.begin(); it != m_vecRxCmds.end(); it++)
	{
		if (*it == cmd)
		{
			bRet = true;
			break;
		}
	}

	return bRet;
}

bool CVPdevIO::RxCmdVecRemove(eViperCmds cmd)
{
	bool bRet = false;
	std::lock_guard<std::mutex> lck(m_mtxVecRxCmds);
	std::vector<eViperCmds>::iterator it;

	for (it = m_vecRxCmds.begin(); it != m_vecRxCmds.end(); it++)
	{
		if (*it == cmd)
		{
			m_vecRxCmds.erase(it);

			bRet = true;
			break;
		}
	}

	return bRet;
}

int CVPdevIO::RxPnoDequeueFIFO(CFrameInfo & Fin)
{
	CODETRACE7;
	int r = 0;

	{
		std::lock_guard<std::mutex> lck(mtxRxPnoQueue);

		if (!(queRxPnoFrames.empty()))
		{
			Fin = queRxPnoFrames.front();
			queRxPnoFrames.pop_front();

			// there are still more in there, newer frames
			if (!(queRxPnoFrames.empty()))
			{
				CFrameInfo Ftmp = queRxPnoFrames.back();
				trTracefHP5("%s:%d : SEUID %d: %d newer Frames remain in queRxPnoFrames. Fin.FC = %d, Newest Ftmp.FC = %d\r\n",
					__FUNCTION__, __LINE__, SEUID(), queRxPnoFrames.size(), Fin.devfc(), Ftmp.devfc());
			}

		}
		else
			r = E_CTXERR_NO_MORE_DATA;
	}

	CODETRACEEX7;
	return r;
}

int CVPdevIO::RxPnoDequeueLAST(CFrameInfo & Fin)
{
	CODETRACE7;
	int r = 0;

	{
		std::lock_guard<std::mutex> lck(mtxRxPnoQueue);

		if (!(queRxPnoFrames.empty()))
		{
			Fin = queRxPnoFrames.back();
			queRxPnoFrames.pop_back();

			// there are still more in there, older frames
			if (!(queRxPnoFrames.empty()))
			{
				CFrameInfo Ftmp = queRxPnoFrames.front();
				trTracefHP5("%s:%d : SEUID %d: DISCARDING %d older Frames remaining in queRxPnoFrames. Fin.FC = %d, Oldest Ftmp.FC = %d\r\n",
					__FUNCTION__, __LINE__, SEUID(), queRxPnoFrames.size(), Fin.devfc(), Ftmp.devfc());

				queRxPnoFrames.clear();

			}

		}
		else
			r = E_CTXERR_NO_MORE_DATA;
	}

	CODETRACEEX7;
	return r;
}

int CVPdevIO::FIFOPnoCount(uint32_t & fc, uint32_t & bc)
{
	CODETRACE7;
	fc = (uint32_t)queRxPnoFrames.size();

	int r = 0;
	bc = 0;
	{
		std::lock_guard<std::mutex> lck(mtxRxPnoQueue);

		for (uint32_t i = 0; i < fc; i++)
		{
			bc += queRxPnoFrames[i].uiSize;
		}
	}

	CODETRACEEX7;
	return r;
}

int CVPdevIO::FIFORxPnoCount(CFrameInfo & Fin, uint32_t & fc, uint32_t maxbytes)
{
	CODETRACE7;
	std::lock_guard<std::mutex> lck(mtxRxPnoQueue);

	int r = 0;
	uint32_t bc = 0;
	uint32_t frames_avail = (uint32_t)queRxPnoFrames.size();
	uint32_t frames_taken = 0;

	CFrameInfo ftmp;
	//fc = ;
	if (frames_avail)
	{
		ftmp = queRxPnoFrames.front();
		if (ftmp.uiSize > maxbytes)
		{
			r = E_VPERR_BUF_TOO_SMALL;
		}
		else
		{
			queRxPnoFrames.pop_front();
			Fin = ftmp;
			frames_taken = 1;

			bc = ftmp.uiSize;
			uint8_t * plast = ftmp.pF;
			for (uint32_t i = 0; i < fc-1; i++)
			{
				ftmp = queRxPnoFrames.front();
				if ((ftmp.uiSize + bc) > maxbytes)
					break;
				else if (ftmp.pF < plast) //wrapped
					break; 
				else
				{
					queRxPnoFrames.pop_front();
					bc += ftmp.uiSize;
					frames_taken++;
				}
				
			}
		
			Fin.uiSize = bc;
			fc = frames_taken;
		}

	}

	CODETRACEEX7;
	return r;
}
