#include "hiduniversal.h"

HIDUniversal::HIDUniversal(USB *p) : 
		HID(p),
		qNextPollTime(0),
		bPollEnable(false),
		bHasReportId(false)
{
	Initialize();

	if (pUsb)
		pUsb->RegisterDeviceClass(this);
}

uint16_t HIDUniversal::GetHidClassDescrLen(uint8_t type, uint8_t num)
{
	for (uint8_t i=0, n=0; i<HID_MAX_HID_CLASS_DESCRIPTORS; i++)
	{
		if (descrInfo[i].bDescrType == type)
		{
			if (n == num)
				return descrInfo[i].wDescriptorLength;
			n ++;
		}
	}
	return 0;
}

void HIDUniversal::Initialize()
{
	for (uint8_t i=0; i<MAX_REPORT_PARSERS; i++)
	{
		rptParsers[i].rptId		= 0;
		rptParsers[i].rptParser = NULL;
	}
	for (uint8_t i=0; i<HID_MAX_HID_CLASS_DESCRIPTORS; i++)
	{
		descrInfo[i].bDescrType			= 0;
		descrInfo[i].wDescriptorLength	= 0;
	}
	for (uint8_t i=0; i<maxHidInterfaces; i++)
	{
		hidInterfaces[i].bmInterface	= 0;
		hidInterfaces[i].bmProtocol		= 0;

		for (uint8_t j=0; j<maxEpPerInterface; j++)
			hidInterfaces[i].epIndex[j] = 0;
	}
	for(uint8_t i=0; i<totalEndpoints; i++)
	{
		epInfo[i].epAddr		= 0;
		epInfo[i].maxPktSize	= (i) ? 0 : 8;
		epInfo[i].epAttribs		= 0;
		epInfo[i].bmNakPower	= (i) ? USB_NAK_NOWAIT : USB_NAK_MAX_POWER;
	}
	bNumEP		= 1;
	bNumIface	= 0;
	bConfNum	= 0;

	ZeroMemory(constBuffLen, prevBuf);
}

bool HIDUniversal::SetReportParser(uint8_t id, HIDReportParser *prs)
{
	for (uint8_t i=0; i<MAX_REPORT_PARSERS; i++)
	{
		if (rptParsers[i].rptId == 0 && rptParsers[i].rptParser == NULL)
		{
			rptParsers[i].rptId		= id;
			rptParsers[i].rptParser = prs;
			return true;
		}
	}
	return false;
}

HIDReportParser* HIDUniversal::GetReportParser(uint8_t id)
{
	if (!bHasReportId)
		return ((rptParsers[0].rptParser) ? rptParsers[0].rptParser : NULL);

	for (uint8_t i=0; i<MAX_REPORT_PARSERS; i++)
	{
		if (rptParsers[i].rptId == id)
			return rptParsers[i].rptParser;
	}
	return NULL;
}

uint8_t HIDUniversal::Init(uint8_t parent, uint8_t port, bool lowspeed)
{
	const uint8_t constBufSize = sizeof(USB_DEVICE_DESCRIPTOR);

	uint8_t		buf[constBufSize];
	uint8_t		rcode;
	UsbDevice	*p = NULL;
	EpInfo		*oldep_ptr = NULL;
	uint8_t		len = 0;

	uint8_t		num_of_conf;	// number of configurations
	uint8_t		num_of_intf;	// number of interfaces

	AddressPool	&addrPool = pUsb->GetAddressPool();

	USBTRACE("HU Init\r\n");

	if (bAddress)
		return USB_ERROR_CLASS_INSTANCE_ALREADY_IN_USE;

	// Get pointer to pseudo device with address 0 assigned
	p = addrPool.GetUsbDevicePtr(0);

	if (!p)
		return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;

	if (!p->epinfo)
	{
		USBTRACE("epinfo\r\n");
		return USB_ERROR_EPINFO_IS_NULL;
	}

	// Save old pointer to EP_RECORD of address 0
	oldep_ptr = p->epinfo;

	// Temporary assign new pointer to epInfo to p->epinfo in order to avoid toggle inconsistence
	p->epinfo = epInfo;

	p->lowspeed = lowspeed;

	// Get device descriptor
	rcode = pUsb->getDevDescr( 0, 0, 8, (uint8_t*)buf );

	if  (!rcode)
		len = (buf[0] > constBufSize) ? constBufSize : buf[0];

	if( rcode ) 
	{
		// Restore p->epinfo
		p->epinfo = oldep_ptr;

		goto FailGetDevDescr;
	}

	// Restore p->epinfo
	p->epinfo = oldep_ptr;

	// Allocate new address according to device class
	bAddress = addrPool.AllocAddress(parent, false, port);

	if (!bAddress)
		return USB_ERROR_OUT_OF_ADDRESS_SPACE_IN_POOL;

	// Extract Max Packet Size from the device descriptor
	epInfo[0].maxPktSize = (uint8_t)((USB_DEVICE_DESCRIPTOR*)buf)->bMaxPacketSize0; 

	// Assign new address to the device
	rcode = pUsb->setAddr( 0, 0, bAddress );

	if (rcode)
	{
		p->lowspeed = false;
		addrPool.FreeAddress(bAddress);
		bAddress = 0;
		USBTRACE2("setAddr:",rcode);
		return rcode;
	}

	USBTRACE2("Addr:", bAddress);

	p->lowspeed = false;

	p = addrPool.GetUsbDevicePtr(bAddress);

	if (!p)
		return USB_ERROR_ADDRESS_NOT_FOUND_IN_POOL;

	p->lowspeed = lowspeed;

	if (len)
		rcode = pUsb->getDevDescr( bAddress, 0, len, (uint8_t*)buf );

	if(rcode) 
		goto FailGetDevDescr;

	num_of_conf = ((USB_DEVICE_DESCRIPTOR*)buf)->bNumConfigurations;

	// Assign epInfo to epinfo pointer
	rcode = pUsb->setEpInfoEntry(bAddress, 1, epInfo);

	if (rcode)
		goto FailSetDevTblEntry;

	USBTRACE2("NC:", num_of_conf);

	for (uint8_t i=0; i<num_of_conf; i++)
	{
		//HexDumper<USBReadParser, uint16_t, uint16_t>		HexDump;
		ConfigDescParser<USB_CLASS_HID, 0, 0, 
			CP_MASK_COMPARE_CLASS>							confDescrParser(this);

		//rcode = pUsb->getConfDescr(bAddress, 0, i, &HexDump);
		rcode = pUsb->getConfDescr(bAddress, 0, i, &confDescrParser);
		
		if (bNumEP > 1)
			break;
	} // for
	
	if (bNumEP < 2)
		return USB_DEV_CONFIG_ERROR_DEVICE_NOT_SUPPORTED;

	// Assign epInfo to epinfo pointer
	rcode = pUsb->setEpInfoEntry(bAddress, bNumEP, epInfo);

	USBTRACE2("\r\nCnf:", bConfNum);

	// Set Configuration Value
	rcode = pUsb->setConf(bAddress, 0, bConfNum);

	if (rcode)
		goto FailSetConfDescr;

	for (uint8_t i=0; i<bNumIface; i++)
	{
		if (hidInterfaces[i].epIndex[epInterruptInIndex] == 0)
			continue;

		rcode = SetIdle(hidInterfaces[i].bmInterface, 0, 0);

		if (rcode && rcode != hrSTALL)
			goto FailSetIdle;
	}

	USBTRACE("HU configured\r\n");

	{
		HexDumper<USBReadParser, uint16_t, uint16_t>    Hex;
		ReportDescParser                                Rpt;

		if (rcode = GetReportDescr(0, &Hex))
			goto FailGetReportDescr;
	        
		if (rcode = GetReportDescr(0, &Rpt))
			goto FailGetReportDescr;
	}
	bPollEnable = true;
	return 0;

FailGetDevDescr:
	USBTRACE("getDevDescr:");
	goto Fail;

FailSetDevTblEntry:
	USBTRACE("setDevTblEn:");
	goto Fail;

FailGetConfDescr:
	USBTRACE("getConf:");
	goto Fail;

FailSetConfDescr:
	USBTRACE("setConf:");
	goto Fail;

FailSetProtocol:
	USBTRACE("SetProto:");
	goto Fail;

FailSetIdle:
	USBTRACE("SetIdle:");
	goto Fail;

FailGetReportDescr:
	USBTRACE("GetReportDescr:");
	goto Fail;

Fail:
	Serial.println(rcode, HEX);
	Release();
	return rcode;
}

HIDUniversal::HIDInterface* HIDUniversal::FindInterface(uint8_t iface, uint8_t alt, uint8_t proto)
{
	for (uint8_t i=0; i<bNumIface && i<maxHidInterfaces; i++)
		if (hidInterfaces[i].bmInterface == iface && hidInterfaces[i].bmAltSet == alt
			&& hidInterfaces[i].bmProtocol == proto)
			return hidInterfaces + i;
	return NULL;
}

void HIDUniversal::EndpointXtract(uint8_t conf, uint8_t iface, uint8_t alt, uint8_t proto, const USB_ENDPOINT_DESCRIPTOR *pep) 
{
	// If the first configuration satisfies, the others are not concidered.
	if (bNumEP > 1 && conf != bConfNum)
		return;

	//ErrorMessage<uint8_t>(PSTR("\r\nConf.Val"), conf);
	//ErrorMessage<uint8_t>(PSTR("Iface Num"), iface);
	//ErrorMessage<uint8_t>(PSTR("Alt.Set"), alt);

	bConfNum = conf;

	uint8_t			index	= 0;
	HIDInterface	*piface = FindInterface(iface, alt, proto);
	
	// Fill in interface structure in case of new interface 
	if (!piface)
	{
		piface				= hidInterfaces + bNumIface;
		piface->bmInterface = iface;
		piface->bmAltSet	= alt;
		piface->bmProtocol	= proto;
		bNumIface ++;
	}

	if ((pep->bmAttributes & 0x03) == 3 && (pep->bEndpointAddress & 0x80) == 0x80)
		index = epInterruptInIndex;
	else
		index = epInterruptOutIndex;

	if (index)
	{
		// Fill in the endpoint info structure
		epInfo[bNumEP].epAddr		= (pep->bEndpointAddress & 0x0F);
		epInfo[bNumEP].maxPktSize	= (uint8_t)pep->wMaxPacketSize;
		epInfo[bNumEP].epAttribs	= 0;
		epInfo[bNumEP].bmNakPower	= USB_NAK_NOWAIT;

		// Fill in the endpoint index list
		piface->epIndex[index] = bNumEP;			//(pep->bEndpointAddress & 0x0F);

		bNumEP ++;
	}
	//PrintEndpointDescriptor(pep);
}


uint8_t HIDUniversal::Release()
{
	pUsb->GetAddressPool().FreeAddress(bAddress);

	bNumEP				= 1;
	bAddress			= 0;
	qNextPollTime		= 0;
	bPollEnable			= false;
	return 0;
}

bool HIDUniversal::BuffersIdentical(uint8_t len, uint8_t *buf1, uint8_t *buf2)
{
	for (uint8_t i=0; i<len; i++)
		if (buf1[i] != buf2[i])
			return false;
	return true;
}

void HIDUniversal::ZeroMemory(uint8_t len, uint8_t *buf)
{
	for (uint8_t i=0; i<len; i++)	
		buf[i] = 0;
}
void HIDUniversal::SaveBuffer(uint8_t len, uint8_t *src, uint8_t *dest)
{
	for (uint8_t i=0; i<len; i++)
		dest[i] = src[i];
}

uint8_t HIDUniversal::Poll()
{
	uint8_t rcode = 0;

	if (!bPollEnable)
		return 0;

	if (qNextPollTime <= millis())
	{
		qNextPollTime = millis() + 50;

		uint8_t buf[constBuffLen];

		for (uint8_t i=0; i<bNumIface; i++)
		{
			uint8_t		index	= hidInterfaces[i].epIndex[epInterruptInIndex];
			uint16_t	read	= (uint16_t)epInfo[index].maxPktSize;

			ZeroMemory(constBuffLen, buf);

			uint8_t rcode = pUsb->inTransfer(bAddress, epInfo[index].epAddr, &read, buf);

			if (rcode)
			{
				if (rcode != hrNAK)
					USBTRACE2("Poll:", rcode);
				return rcode;
			}

			if (read > constBuffLen)
				read = constBuffLen;

			bool identical = BuffersIdentical(read, buf, prevBuf);

			SaveBuffer(read, buf, prevBuf);

			if (identical)
				return 0;

			Serial.print("\r\nBuf: ");

			for (uint8_t i=0; i<read; i++)
				PrintHex<uint8_t>(buf[i]);

			Serial.println("");

			HIDReportParser		*prs = GetReportParser( ((bHasReportId) ? *buf : 0) );

			if (prs)
				prs->Parse(this, bHasReportId, (uint8_t)read, buf);
		}
	}
	return rcode;
}