// VNCPconsole.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <iostream>
#include <string>
#include <sstream>
#include <chrono>

#define ESC	0x1b
#define ENTER 0x0d
#define CTLX 0x18



#include "ViperInterface.h"
#include "VPif.h"
#include "USBfcns.h"

#define TX_BUF_SIZE  0x400
#define RX_BUF_SIZE  0x400
uint8_t g_txbuf[TX_BUF_SIZE];
int g_ntxcount;

uint8_t g_rxbuf[RX_BUF_SIZE];
int g_nrxcount;

libusb_device_handle *g_usbhnd = 0;
vp_usbdevinfo g_usbinfo;

int CmdStationMap( CStationMap & stamap);
int CmdSingle();
int CmdHemisphere(CHemisphereCfg & hemcfg);
int CmdStartCont();
int CmdStopCont();
void SrcSelectTest();
void ContPnoTest();

using namespace std;

int main()
{

	int r = 0;

	if (r = DiscoverVidPid(&g_usbhnd, g_usbinfo, POLHEMUS_USB_VID, VIPER_USB_PID))
	{
		//error connecting
		cout << "DiscoverVidPid error - device not found" << endl;
		return 1;
	}

	CStationMap cstamap;
	if (CmdStationMap(cstamap))
	{
		cout << "CmdStationMap error" << endl;
		return 1;
	}
	else
	{
		cout << "Station Count = " << cstamap.SnsDetectedCount() << endl;
		std::string str;
		cstamap.Report(str);
		std::cout << "CStationMap Report " << endl << str.c_str() << endl;

	}

	CHemisphereCfg hemcfg;
	float hemarr[3] = { 1.0f, 0.0f, 0.0f };
	hemcfg.Fill(hemarr, true);

	if (CmdHemisphere(hemcfg))
	{
		cout << "CmdHemisphere error" << endl;
		return 1;
	}
	else
	{
		cout << "+X Hemisphere set on sensor 0"  << endl;
	}


	if (CmdSingle())
	{
		cout << "CmdSingle error" << endl;
		return 1;
	}

	//SrcSelectTest();

	ContPnoTest();

    return 0;
}
void DisplayError(int32_t err, const char *szMsg)
{
	cout << szMsg << libusb_error_name(err);

	cout << endl;
}

void SrcSelectTest()
{
	CVPcmd cmd;
	int r = 0;
	int nSns;
	eCmdActions eAct = CMD_ACTION_SET;
	CSrcSelectCfg ssc;
	ssc.Fill(0x01);
	std::string s;
	ssc.Report(s, false, true);
	char szMsg[300];
	for (int i = 0; i <= 5; i++)
	{
		cmd.Init();
		cmd.Fill(0, CMD_SRC_SELECT, CMD_ACTION_SET, i, 0, ssc, sizeof(SRC_SEL_CONFIG));
		cmd.Prepare(g_txbuf, g_ntxcount);

		uint32_t timeout_ms = 50;
		int nBytes = g_ntxcount;
		uint8_t *pbuf = g_txbuf;

		if (r = WriteUSB(g_usbhnd, g_usbinfo, pbuf, nBytes))
		{
			//DisplayError(r, "Write CMD_ACTION_GET failed with error ");
		}
		else
		{
			g_nrxcount = RX_BUF_SIZE;
			r = ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, g_nrxcount, timeout_ms, true);
			//DisplayError(r, "CmdStationMap() Read : ");
			if (r == 0)
			{
				CFrameInfo fi(g_rxbuf, g_nrxcount);

				cout << cmd.cmdstr(fi.cmd()) << " " << cmd.actstr(CMD_ACTION_SET) << " sensor " << i << ", srcmap " << hex << ssc.uSrcSelMap << " response " << cmd.actstr(fi.action()) << endl;
			}
		}
	}
	ssc.Init();
	ssc.Fill(0x04);
	for (int i = 6; i <= 11; i++)
	{
		cmd.Init();
		cmd.Fill(0, CMD_SRC_SELECT, CMD_ACTION_SET, i, 0, ssc, sizeof(SRC_SEL_CONFIG));
		cmd.Prepare(g_txbuf, g_ntxcount);

		int nBytes = g_ntxcount;
		uint8_t *pbuf = g_txbuf;

		if (r = WriteUSB(g_usbhnd, g_usbinfo, pbuf, nBytes))
		{
			//DisplayError(r, "Write CMD_ACTION_GET failed with error ");
		}
		else
		{
			g_nrxcount = RX_BUF_SIZE;
			r = ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, g_nrxcount, true);
			//DisplayError(r, "CmdStationMap() Read : ");
			if (r == 0)
			{
				CFrameInfo fi(g_rxbuf, g_nrxcount);

				cout << cmd.cmdstr(fi.cmd()) << " " << cmd.actstr(CMD_ACTION_SET) << " sensor " << i << ", srcmap " << hex << ssc.uSrcSelMap << " response " << cmd.actstr(fi.action()) << endl;
			}
		}
	}
}

int CmdStationMap (CStationMap &stamap)
{
	int r = 0;

	CVPcmd cmd;
	cmd.Fill(0, CMD_STATION_MAP, CMD_ACTION_GET);
	cmd.Prepare(g_txbuf, g_ntxcount);


	uint32_t timeout_ms = 50;
	int nBytes = g_ntxcount;
	uint8_t *pbuf = g_txbuf;

	if (r = WriteUSB(g_usbhnd, g_usbinfo, pbuf, nBytes))
	{
		//DisplayError(r, "Write CMD_ACTION_GET failed with error ");
	}
	else
	{
		g_nrxcount = RX_BUF_SIZE;
		r = ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, g_nrxcount, timeout_ms, true);
		//DisplayError(r, "CmdStationMap() Read : ");
		if (r == 0)
		{

			CFrameInfo fi(g_rxbuf, g_nrxcount);
			
			if ((fi.cmd() == CMD_STATION_MAP) && (fi.IsAck()))
			{
				CStationMap cmap((STATION_MAP*)fi.PCmdPayload());
				stamap = cmap;

				std::string s;
				cmap.Report(s);
				cout << s << endl;
			}
		}
	}
	return r;

}

int CmdSingle()
{
	int r = 0;

	CVPcmd cmd;
	cmd.Fill(0, CMD_SINGLE_PNO, CMD_ACTION_GET);
	cmd.Prepare(g_txbuf, g_ntxcount);

	uint32_t timeout_ms = 50;

	int nBytes = g_ntxcount;
	uint8_t *pbuf = g_txbuf;

	if (r = WriteUSB(g_usbhnd, g_usbinfo, pbuf, nBytes))
	{
		//DisplayError(r, "Write CMD_ACTION_GET failed with error ");
	}
	else
	{
		g_nrxcount = RX_BUF_SIZE;
		r = ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, g_nrxcount, timeout_ms, true);
		//DisplayError(r, "CmdStationMap() Read : ");
		if (r == 0)
		{

			CFrameInfo fi(g_rxbuf, g_nrxcount);


			if (fi.cmd() != CMD_SINGLE_PNO)
			{
				cout << endl << "CmdSingle cmd mismatch: " << hex << fi.cmd() << " should be " << CMD_SINGLE_PNO << endl;
				r = -1;
			}
			else if (!(fi.IsAck()))
			{
				cout << endl << "CmdSingle ACK not received: " << hex << fi.action() << endl;
				r = -1;
			}
			else //if ((fi.cmd() == CMD_SINGLE_PNO) && (fi.IsAck()))
			{
				CVPSeuPno pno;
				int bytesextracted = pno.Extractseupno(fi.PCmdPayload());

				if (bytesextracted)
				{
					std::string s;
					s += "Single Pno Frame:\n";
					pno.Report(s);
					cout << s << endl;
				}
				else
				{
					cout << "CmdSingle Extractseupno parsing error" << endl;
					r = -1;
				}
			}
		}
		else
		{
			cout << "CmdSingle ReadUSB error" << endl;
		}
	}
	return r;

}

int CmdHemisphere(CHemisphereCfg & hemcfg)
{
	int r = 0;

	//NOTE Arg1=-1, setting hemisphere for all sensors.  Can be set individually if needed.
	CVPcmd cmd;
	cmd.Fill(0, CMD_HEMISPHERE, CMD_ACTION_SET, 0, 0, hemcfg, hemcfg.Size());
	cmd.Prepare(g_txbuf, g_ntxcount);


	uint32_t timeout_ms = 50;
	int nBytes = g_ntxcount;
	uint8_t *pbuf = g_txbuf;

	if (r = WriteUSB(g_usbhnd, g_usbinfo, pbuf, nBytes))
	{
		//DisplayError(r, "Write CMD_ACTION_SET failed with error ");
	}
	else
	{
		g_nrxcount = RX_BUF_SIZE;
		r = ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, g_nrxcount, timeout_ms, true);

		if (r == 0)
		{

			CFrameInfo fi(g_rxbuf, g_nrxcount);

			if (fi.cmd() != CMD_HEMISPHERE)
			{
				cout << endl << "CmdHemisphere cmd mismatch: " << hex << fi.cmd() << " should be " << CMD_HEMISPHERE << endl;
				r = -1;
			}
			else if (!(fi.IsAck()))
			{
				cout << endl << "CmdHemisphere ACK not received: " << hex << fi.action() << endl;
				r = -1;
			}
		}
	}
	return r;

}

int CmdContPno(eCmdActions eAct)
{
	int r = 0;

	CVPcmd cmd;
	cmd.Fill(0, CMD_CONTINUOUS_PNO, eAct);
	cmd.Prepare(g_txbuf, g_ntxcount);


	uint32_t timeout_ms = 50;
	int nBytes = g_ntxcount;
	uint8_t *pbuf = g_txbuf;

	if (r = WriteUSB(g_usbhnd, g_usbinfo, pbuf, nBytes))
	{
		//DisplayError(r, "Write CMD_ACTION_SET failed with error ");
	}
	else
	{
		g_nrxcount = RX_BUF_SIZE;
		r = ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, g_nrxcount, timeout_ms, true);

		if (r == 0)
		{

			CFrameInfo fi(g_rxbuf, g_nrxcount);

			if (fi.cmd() != CMD_CONTINUOUS_PNO)
			{
				cout << endl << "CmdContPno cmd mismatch: " << hex << fi.cmd() << " should be " << CMD_CONTINUOUS_PNO << endl;
				r = -1;
			}
			else if (!(fi.IsAck()))
			{
				cout << endl << "CmdContPno ACK not received: " << hex << fi.action() << endl;
				r = -1;
			}

		}
	}
	return r;

}
int CmdStartCont()
{
	return CmdContPno(CMD_ACTION_SET);
}
int CmdStopCont()
{
	return CmdContPno(CMD_ACTION_RESET);
}
bool StartCont()
{
	bool bRet = false;
	int r = 0;

	if (r = CmdStartCont())
	{
	}
	else
	{
		bRet = true;
	}

	return bRet;
}
bool StopCont()
{
	bool bRet = false;
	int r = 0;

	if (r = CmdStopCont())
	{
		//flush usb input
		int nBytes = RX_BUF_SIZE;
		ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, nBytes);
		CmdStopCont();
		bRet = true;
	}
	else
		bRet = true;

	return bRet;
}

uint8_t g_rxbuf_2[RX_BUF_SIZE];

bool ContLoop_S(int nGet /*=-1*/)
{
	bool bRet = true;
	bool bExit = false;
	int32_t r;
	int nCaptured = 0;
	int nReads = 0;

	uint32_t fc_last = 0;
	uint32_t fc_curr = 0;
	uint32_t totaldropcount = 0;
	bool bFirst = true;
	uint32_t ndiff = 0;
	uint32_t dropped = 0;
	uint32_t timeout_ms = 0;
	std::string rptstr;

	cout << "ContLoop_S: timeout_ms " << timeout_ms << endl;
	HANDLE hth = ::GetCurrentThread();

	//::SetThreadPriority(hth, THREAD_PRIORITY_TIME_CRITICAL);
	::SetThreadPriority(hth, THREAD_PRIORITY_ABOVE_NORMAL);

	auto start = std::chrono::high_resolution_clock::now();
	while (std::chrono::high_resolution_clock::now() - start < 4ms);

	do {

		//auto start = std::chrono::high_resolution_clock::now();

		if ((nGet > 0) && (nReads > 500 * nGet))
		{
			bRet = false;
			break;
		}

		uint8_t * pbuf = 0;
		int nBytes = RX_BUF_SIZE;
		CFrameInfo f;
		CVPSeuPno pno;

		g_nrxcount = RX_BUF_SIZE;
		if (r = ReadUSB(g_usbhnd, g_usbinfo, g_rxbuf, nBytes, timeout_ms, false))
		//if (r = g_dev.Read(g_rxbuf, nBytes, true))
		{
			DisplayError(r, "ContLoop_S Sync Read Error : ");
			bRet = false;

		}
		else
		{
			nReads++;
			if (nBytes)
			{
				CFrameInfo f(g_rxbuf, nBytes);
				if (!f.IsPno())
				{
					cout << "not pno" << endl;
					//DOUTHEXBUF(g_rxbuf, nBytes, true, __FUNCTION__);

				}
				else if (bFirst)
				{
					//nCaptured++;
					fc_curr = fc_last = f.devfc();
					//cout << "first read, " << "nBytes = " << nBytes << " fc_curr = fc_last = " << fc_last << endl;
					bFirst = false;
				}
				else
				{
					fc_curr = f.devfc();

					dropped = fc_curr - fc_last - 1;
					totaldropcount += dropped;

					++nCaptured;
					fc_last = fc_curr;

					// simulate work on the received data.
					memcpy(g_rxbuf_2, g_rxbuf, nBytes);
					//cout << " " << fc_curr;
					//if (dropped)
					//	cout << "*" << dropped << " ";
					
					//rptstr.clear();
					CVPSeuPno::Report((VP_PNO*)g_rxbuf, rptstr, true);
					//cout << rptstr.c_str() << endl;

					if (nCaptured % 480 == 0)
						std::cout << endl << "Read " << nCaptured << " total frames with " << totaldropcount << "  lost frames\n";

				}


			}
			else
			{
				std::cout << " .";
			}

		}

		if (nGet >= 0)
			bExit = (nCaptured >= nGet);

		else if (_kbhit() && (_gettche() == ESC))
		{
			bExit = true;
		}

		//while (std::chrono::high_resolution_clock::now() - start < 4ms);

	} while (!bExit);

	::SetThreadPriority(hth, THREAD_PRIORITY_NORMAL);
	cout << flush;
	cout << endl << "Cont S exiting. " << nCaptured << " captured, " << totaldropcount << " dropped.  USB timeout " << timeout_ms << " ms." << endl << flush;

	return bRet;
}

void ContPnoTest()
{
	if (!(StartCont())) //uses libusb sync bulk read and write
	{
	}
	else
	{
		//Collect/Display Tracker Data until ESC
		ContLoop_S(-1);
		StopCont();
	}

}
//
//int DiscoverVidPid(libusb_device_handle **usbhnd, vp_usbdevinfo &usbinfo, uint16_t vid, uint16_t pid)
//{
//
//	int r = 0;
//
//	if (libusb_init(NULL))
//		return -1;
//
//	if (*usbhnd)
//		ReleaseUSB(usbhnd, usbinfo);
//
//	vp_usbdevinfo arrDevInfo[VPUSB_MAX_DISCOVERABLE];
//	size_t arrcount = VPUSB_MAX_DISCOVERABLE;
//	libusb_device **devlist;
//
//	if ((r = CreateVidPidList(NULL, devlist, vid, pid, arrDevInfo, arrcount)) < 0)
//		return r;
//
//	for (int d = 0; d < (int)arrcount; d++)
//	{
//		libusb_device * dev = devlist[arrDevInfo[d].usbDevIndex];
//		libusb_device_handle * handle = 0;
//		libusb_config_descriptor *conf_desc = 0;
//		uint8_t nb_ifaces = 0, claimed_ifaces = 0;
//		uint8_t ep_in = 0, ep_out = 0;
//		uint16_t out_pktsize = 0;
//
//		if ((r = libusb_open(dev, &handle)))
//		{
//		}
//		else if ((r = libusb_get_config_descriptor(dev, 0, &conf_desc)))
//		{
//			libusb_close(handle);
//		}
//		else
//		{
//			nb_ifaces = conf_desc->bNumInterfaces;
//			claimed_ifaces = 0;
//			for (uint8_t i = 0; (i < nb_ifaces) && (r == 0); i++)
//			{
//				if ((r = libusb_claim_interface(handle, (int)i)))
//				{
//				}
//				else
//				{
//					claimed_ifaces++;
//
//					find_endpoints(conf_desc, (int)i, ep_in, ep_out, out_pktsize);
//				}
//			}
//
//			if (claimed_ifaces == nb_ifaces)
//			{
//				*usbhnd = handle;
//				usbinfo = arrDevInfo[d];
//				usbinfo.usbNumInterfaces = nb_ifaces;
//				usbinfo.ep_in = ep_in;
//				usbinfo.ep_out = ep_out;
//				usbinfo.epout_maxPktsize = out_pktsize;
//
//				break;
//			}
//		}
//
//		// clean up failed attempt before trying the next one.
//		if (claimed_ifaces != nb_ifaces)
//		{
//			for (int k = 0; k < nb_ifaces; k++)
//				libusb_release_interface(handle, k);
//		}
//		if (handle)
//			libusb_close(handle);
//
//		libusb_free_config_descriptor(conf_desc);
//	}
//
//	libusb_free_device_list(devlist, 1);
//
//	if (!usbhnd)
//		r = -99;
//
//	return r;
//}
//
//int CreateVidPidList(libusb_context* pctx, libusb_device ** & devlist, uint16_t vid, uint16_t pid, vp_usbdevinfo arrDevInfo[], size_t & arrcount)
//{
//	int r = 0;
//
//	ssize_t devcount = libusb_get_device_list(pctx, &devlist);
//	if (devcount < 0)
//		return (int)devcount; // returns error code < 0
//
//							  //struct libusb_device *found = NULL;
//							  //struct libusb_device_handle *found_dev_handle = NULL;
//	libusb_device *dev;
//	int iFoundCount = 0;
//
//	int iFoundArrIndex = -1;
//	int i = 0;
//	while ((dev = devlist[i]) != NULL)
//	{
//		struct libusb_device_descriptor desc;
//		r = libusb_get_device_descriptor(dev, &desc);
//		if (r < 0)
//			break;
//
//		if (desc.idVendor == vid && desc.idProduct == pid)
//		{
//			iFoundCount++; iFoundArrIndex++;
//			// populate the array if size permits
//			if (iFoundCount <= (int)arrcount)
//			{
//				vp_usbdevinfo * p = &arrDevInfo[iFoundArrIndex];
//				p->usbDevIndex = i;
//				p->usbBusNum = libusb_get_bus_number(dev);
//				p->usbDevNum = libusb_get_device_address(dev);
//				p->usbVID = vid; p->usbPID = pid;
//			}
//		}
//
//		i++;
//	}
//
//	arrcount = iFoundCount;
//
//	return r;
//}
//
//void find_endpoints(libusb_config_descriptor *conf_desc, int iface, uint8_t & ep_in, uint8_t & ep_out, uint16_t & out_pktsize)
//{
//	for (int j = 0; j < conf_desc->interface[iface].num_altsetting; j++)
//	{
//		for (int k = 0; k < conf_desc->interface[iface].altsetting[j].bNumEndpoints; k++)
//		{
//			const struct libusb_endpoint_descriptor *p_ep;
//			p_ep = &conf_desc->interface[iface].altsetting[j].endpoint[k];
//
//			if ((p_ep->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) & (LIBUSB_TRANSFER_TYPE_BULK | LIBUSB_TRANSFER_TYPE_INTERRUPT))
//			{
//				if (p_ep->bEndpointAddress & LIBUSB_ENDPOINT_IN)
//				{
//					if (!ep_in)
//						ep_in = p_ep->bEndpointAddress;
//				}
//				else
//				{
//					if (!ep_out)
//					{
//						ep_out = p_ep->bEndpointAddress;
//						out_pktsize = p_ep->wMaxPacketSize;
//					}
//				}
//			}
//
//		}
//	}
//}
//
//int WriteUSB(libusb_device_handle *hnd, vp_usbdevinfo &usbinfo, uint8_t *pbuf, int & count, uint32_t timeout)
//{
//	int nActual = 0;
//	int r = 0;
//
//	if ((r = libusb_bulk_transfer(hnd, usbinfo.ep_out, pbuf, count, &nActual, timeout)))
//	{
//		//r = (r << 16) | E_VPERR_LIBUSB
//	}
//	else if (nActual != count)
//	{
//		count = nActual;
//		r = -1;// E_VPUSB_ERR_WRITE_COUNT_WRONG;
//		
//	}
//	else if ((nActual % usbinfo.epout_maxPktsize) == 0)
//	{
//		r = libusb_bulk_transfer(hnd, usbinfo.ep_out, nullptr, 0, &nActual, timeout);
//	}
//
//	return r;
//
//}
//
//int ReadUSB(libusb_device_handle *hnd, vp_usbdevinfo &usbinfo, uint8_t *pbuf, int & count, uint32_t timeout, bool bTOisErr/*=false*/)
//{
//
//	int r = 0;
//	int nActual = 0;
//
//	r = libusb_bulk_transfer(hnd, usbinfo.ep_in, pbuf, count, &nActual, timeout);
//																						 // timeout is not an error
//																						 //if ((r == LIBUSB_ERROR_TIMEOUT) && !ReadCmdTOIsError())
//	if ((r == LIBUSB_ERROR_TIMEOUT) && !bTOisErr)
//	{
//		r = 0;
//		count = 0;
//	}
//	else
//		count = nActual;
//
//	return r;
//}
//
//void ReleaseUSB(libusb_device_handle **hnd, vp_usbdevinfo &usbinfo)
//{
//	if ((hnd == 0) || (*hnd == 0))
//		return;
//
//	int r = 0;
//	for (int i = 0; i < usbinfo.usbNumInterfaces; i++)
//		r = libusb_release_interface(*hnd, i);
//
//	libusb_close(*hnd);
//	*hnd = 0;
//	usbinfo = vp_usbdevinfo();
//}
