uint8_t Endpoint_Write_Control_Stream_LE(const void* Buffer, uint16_t Length)
{
	uint8_t* DataStream     = (uint8_t*)Buffer;
	bool     LastPacketFull = false;
	
	if (Length > USB_ControlRequest.wLength)
	  Length = USB_ControlRequest.wLength;
	
	while (Length && !(Endpoint_IsOUTReceived()))
	{
		while (!(Endpoint_IsINReady()));
		
		while (Length && (Endpoint_BytesInEndpoint() < USB_ControlEndpointSize))
		{
			Endpoint_Write_Byte(*(DataStream++));
			Length--;
		}
		
		LastPacketFull = (Endpoint_BytesInEndpoint() == USB_ControlEndpointSize);
		Endpoint_ClearIN();
	}
	
	if (Endpoint_IsOUTReceived())
	  return ENDPOINT_RWCSTREAM_HostAborted;
	
	if (LastPacketFull)
	{
		while (!(Endpoint_IsINReady()));
		Endpoint_ClearIN();
	}
	
	while (!(Endpoint_IsOUTReceived()));

	return ENDPOINT_RWCSTREAM_NoError;
}

/** Event handler for the USB_ControlRequest event. This is used to catch and process control requests sent to
 *  the device from the USB host before passing along unhandled control requests to the library for processing
 *  internally.
 */
void EVENT_USB_Device_ControlRequest(void)
{
	/* Ignore any requests that aren't directed to the HID interface */
	if ((USB_ControlRequest.bmRequestType & (CONTROL_REQTYPE_TYPE | CONTROL_REQTYPE_RECIPIENT)) !=
	    (REQTYPE_CLASS | REQREC_INTERFACE))
	{
		return;
	}

	/* Process HID specific control requests */
	switch (USB_ControlRequest.bRequest)
	{
		case HID_REQ_SetReport:
			Endpoint_ClearSETUP();
			
			/* Wait until the command has been sent by the host */
			while (!(Endpoint_IsOUTReceived()));
		
			/* Read in the write destination address */
			uint16_t PageAddress = Endpoint_Read_16_LE();
			
			/* Check if the command is a program page command, or a start application command */
			if (PageAddress == COMMAND_STARTAPPLICATION)
			{
				RunBootloader = false;
			}
			else
			{
				/* Erase the given FLASH page, ready to be programmed */
				boot_page_erase(PageAddress);
				boot_spm_busy_wait();
				
				/* Write each of the FLASH page's bytes in sequence */
				for (uint8_t PageWord = 0; PageWord < (SPM_PAGESIZE / 2); PageWord++)				
				{
					/* Check if endpoint is empty - if so clear it and wait until ready for next packet */
					if (!(Endpoint_BytesInEndpoint()))
					{
						Endpoint_ClearOUT();
						while (!(Endpoint_IsOUTReceived()));
					}

					/* Write the next data word to the FLASH page */
					boot_page_fill(PageAddress + ((uint16_t)PageWord << 1), Endpoint_Read_16_LE());
				}

				/* Write the filled FLASH page to memory */
				boot_page_write(PageAddress);
				boot_spm_busy_wait();

				/* Re-enable RWW section */
				boot_rww_enable();
			}

			Endpoint_ClearOUT();

			Endpoint_ClearStatusStage();
			break;
	}
}

/** Event handler for the USB_UnhandledControlPacket event. This is used to catch standard and class specific
 *  control requests that are not handled internally by the USB library (including the HID commands, which are
 *  all issued via the control endpoint), so that they can be handled appropriately for the application.
 */
void EVENT_USB_UnhandledControlPacket(void)
{
	/* Handle HID Class specific requests */
	switch (USB_ControlRequest.bRequest)
	{
		case REQ_SetReport:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				
				/* Wait until the command (report) has been sent by the host */
				while (!(Endpoint_IsOUTReceived()));

				/* Read in the write destination address */
				uint16_t PageAddress = Endpoint_Read_Word_LE();
				
				/* Check if the command is a program page command, or a start application command */
				if (PageAddress == TEENSY_STARTAPPLICATION)
				{
					RunBootloader = false;
				}
				else
				{
					/* Erase the given FLASH page, ready to be programmed */
					boot_page_erase(PageAddress);
					boot_spm_busy_wait();
					
					/* Write each of the FLASH page's bytes in sequence */
					for (uint8_t PageByte = 0; PageByte < 128; PageByte += 2)
					{
						/* Check if endpoint is empty - if so clear it and wait until ready for next packet */
						if (!(Endpoint_BytesInEndpoint()))
						{
							Endpoint_ClearOUT();
							while (!(Endpoint_IsOUTReceived()));
						}

						/* Write the next data word to the FLASH page */
						boot_page_fill(PageAddress + PageByte, Endpoint_Read_Word_LE());
					}

					/* Write the filled FLASH page to memory */
					boot_page_write(PageAddress);
					boot_spm_busy_wait();

					/* Re-enable RWW section */
					boot_rww_enable();
				}

				Endpoint_ClearOUT();

				/* Acknowledge status stage */
				while (!(Endpoint_IsINReady()));
				Endpoint_ClearIN();
			}
			
			break;
	}
}

uint8_t TEMPLATE_FUNC_NAME (const void* const Buffer,
                            uint16_t Length)
{
	uint8_t* DataStream     = ((uint8_t*)Buffer + TEMPLATE_BUFFER_OFFSET(Length));
	bool     LastPacketFull = false;

	Endpoint_SelectEndpoint(USB_Endpoint_SelectedEndpoint | ENDPOINT_DIR_IN);

	if (Length > USB_ControlRequest.wLength)
	  Length = USB_ControlRequest.wLength;
	else if (!(Length))
	  Endpoint_ClearIN();

	while (Length || LastPacketFull)
	{
		uint8_t USB_DeviceState_LCL = USB_DeviceState;

		if (USB_DeviceState_LCL == DEVICE_STATE_Unattached)
		  return ENDPOINT_RWCSTREAM_DeviceDisconnected;
		else if (USB_DeviceState_LCL == DEVICE_STATE_Suspended)
		  return ENDPOINT_RWCSTREAM_BusSuspended;
		else if (Endpoint_IsSETUPReceived())
		  return ENDPOINT_RWCSTREAM_HostAborted;
		else if (Endpoint_IsOUTReceived())
		  break;

		if (Endpoint_IsINReady())
		{
			uint16_t BytesInEndpoint = Endpoint_BytesInEndpoint();

			while (Length && (BytesInEndpoint < USB_Device_ControlEndpointSize))
			{
				TEMPLATE_TRANSFER_BYTE(DataStream);
				TEMPLATE_BUFFER_MOVE(DataStream, 1);
				Length--;
				BytesInEndpoint++;
			}

			LastPacketFull = (BytesInEndpoint == USB_Device_ControlEndpointSize);
			Endpoint_ClearIN();
		}
	}

	while (!(Endpoint_IsOUTReceived()))
	{
		uint8_t USB_DeviceState_LCL = USB_DeviceState;

		if (USB_DeviceState_LCL == DEVICE_STATE_Unattached)
		  return ENDPOINT_RWCSTREAM_DeviceDisconnected;
		else if (USB_DeviceState_LCL == DEVICE_STATE_Suspended)
		  return ENDPOINT_RWCSTREAM_BusSuspended;
	}

	return ENDPOINT_RWCSTREAM_NoError;
}

void HID_Task(void)
{
    if (USB_DeviceState != DEVICE_STATE_Configured)
      return;

    Endpoint_SelectEndpoint(GENERIC_OUT_EPNUM);

    if (Endpoint_IsOUTReceived())
    {
        if (Endpoint_IsReadWriteAllowed())
        {
            Endpoint_Read_Stream_LE(&HIDReportInData, sizeof(HIDReportInData), NULL);
        }
        Endpoint_ClearOUT();
    }

    Endpoint_SelectEndpoint(GENERIC_IN_EPNUM);

    if (Endpoint_IsINReady())
    {
        Endpoint_Write_Stream_LE(&HIDReportOutData, sizeof(HIDReportOutData), NULL);
        memset(&HIDReportOutData, 0, GENERIC_REPORT_SIZE + 1);
        Endpoint_ClearIN();
    }
}

// From USB/Host to Arduino/Serial
void MIDI_To_Arduino(void)
{
	// Device must be connected and configured for the task to run
	if (USB_DeviceState != DEVICE_STATE_Configured) return;

	// Select the MIDI OUT stream
	Endpoint_SelectEndpoint(MIDI_STREAM_OUT_EPADDR);

	/* Check if a MIDI command has been received */
	if (Endpoint_IsOUTReceived())
	{
		MIDI_EventPacket_t MIDIEvent;

		/* Read the MIDI event packet from the endpoint */
		Endpoint_Read_Stream_LE(&MIDIEvent, sizeof(MIDIEvent), NULL);

		// Passthrough to Arduino
		Serial_SendByte(MIDIEvent.Data1);
		Serial_SendByte(MIDIEvent.Data2); 
		Serial_SendByte(MIDIEvent.Data3); 

		LEDs_TurnOffLEDs(LEDS_LED1);
		rx_ticks = TICK_COUNT;

		/* If the endpoint is now empty, clear the bank */
		if (!(Endpoint_BytesInEndpoint()))
		{
			/* Clear the endpoint ready for new packet */
			Endpoint_ClearOUT();
		}
	}

}

uint8_t RNDIS_Device_ReadPacket(USB_ClassInfo_RNDIS_Device_t* const RNDISInterfaceInfo,
                                void* Buffer,
                                uint16_t* const PacketLength)
{
	if ((USB_DeviceState != DEVICE_STATE_Configured) ||
	    (RNDISInterfaceInfo->State.CurrRNDISState != RNDIS_Data_Initialized))
	{
		return ENDPOINT_RWSTREAM_DeviceDisconnected;
	}

	Endpoint_SelectEndpoint(RNDISInterfaceInfo->Config.DataOUTEndpoint.Address);

	*PacketLength = 0;

	if (!(Endpoint_IsOUTReceived()))
		return ENDPOINT_RWSTREAM_NoError;

	RNDIS_Packet_Message_t RNDISPacketHeader;
	Endpoint_Read_Stream_LE(&RNDISPacketHeader, sizeof(RNDIS_Packet_Message_t), NULL);

	if (le32_to_cpu(RNDISPacketHeader.DataLength) > ETHERNET_FRAME_SIZE_MAX)
	{
		Endpoint_StallTransaction();

		return RNDIS_ERROR_LOGICAL_CMD_FAILED;
	}

	*PacketLength = (uint16_t)le32_to_cpu(RNDISPacketHeader.DataLength);

	Endpoint_Read_Stream_LE(Buffer, *PacketLength, NULL);
	Endpoint_ClearOUT();

	return ENDPOINT_RWSTREAM_NoError;
}

/** Function to manage CDC data transmission and reception to and from the host. */
void CDC_Task(void)
{
	char*       ReportString    = NULL;
	uint8_t     JoyStatus_LCL   = Joystick_GetStatus();
	static bool ActionSent      = false;

	/* Device must be connected and configured for the task to run */
	if (USB_DeviceState != DEVICE_STATE_Configured)
	  return;

	/* Determine if a joystick action has occurred */
	if (JoyStatus_LCL & JOY_UP)
	  ReportString = "Joystick Up\r\n";
	else if (JoyStatus_LCL & JOY_DOWN)
	  ReportString = "Joystick Down\r\n";
	else if (JoyStatus_LCL & JOY_LEFT)
	  ReportString = "Joystick Left\r\n";
	else if (JoyStatus_LCL & JOY_RIGHT)
	  ReportString = "Joystick Right\r\n";
	else if (JoyStatus_LCL & JOY_PRESS)
	  ReportString = "Joystick Pressed\r\n";
	else
	  ActionSent = false;

	/* Flag management - Only allow one string to be sent per action */
	if ((ReportString != NULL) && (ActionSent == false) && LineEncoding.BaudRateBPS)
	{
		ActionSent = true;

		/* Select the Serial Tx Endpoint */
		Endpoint_SelectEndpoint(CDC_TX_EPADDR);

		/* Write the String to the Endpoint */
		Endpoint_Write_Stream_LE(ReportString, strlen(ReportString), NULL);

		/* Remember if the packet to send completely fills the endpoint */
		bool IsFull = (Endpoint_BytesInEndpoint() == CDC_TXRX_EPSIZE);

		/* Finalize the stream transfer to send the last packet */
		Endpoint_ClearIN();

		/* If the last packet filled the endpoint, send an empty packet to release the buffer on
		 * the receiver (otherwise all data will be cached until a non-full packet is received) */
		if (IsFull)
		{
			/* Wait until the endpoint is ready for another packet */
			Endpoint_WaitUntilReady();

			/* Send an empty packet to ensure that the host does not buffer data sent to it */
			Endpoint_ClearIN();
		}
	}

	/* Select the Serial Rx Endpoint */
	Endpoint_SelectEndpoint(CDC_RX_EPADDR);

	/* Throw away any received data from the host */
	if (Endpoint_IsOUTReceived())
	  Endpoint_ClearOUT();
}

/** Attempts to read in the TMC message header from the TMC interface.
 *
 *  \param[out] MessageHeader  Pointer to a location where the read header (if any) should be stored
 *
 *  \return Boolean true if a header was read, false otherwise
 */
bool ReadTMCHeader(TMC_MessageHeader_t* const MessageHeader)
{
	uint16_t BytesTransferred;
	uint8_t  ErrorCode;

	/* Select the Data Out endpoint */
	Endpoint_SelectEndpoint(TMC_OUT_EPADDR);

	/* Abort if no command has been sent from the host */
	if (!(Endpoint_IsOUTReceived()))
	  return false;

	/* Read in the header of the command from the host */
	BytesTransferred = 0;
	while ((ErrorCode = Endpoint_Read_Stream_LE(MessageHeader, sizeof(TMC_MessageHeader_t), &BytesTransferred)) ==
	       ENDPOINT_RWSTREAM_IncompleteTransfer)
	{
		if (IsTMCBulkOUTReset)
		  break;
	}

	/* Store the new command tag value for later use */
	CurrentTransferTag = MessageHeader->Tag;

	/* Indicate if the command has been aborted or not */
	return (!(IsTMCBulkOUTReset) && (ErrorCode == ENDPOINT_RWSTREAM_NoError));
}

uint8_t Endpoint_WaitUntilReady(void)
{
	#if (USB_STREAM_TIMEOUT_MS < 0xFF)
	uint8_t  TimeoutMSRem = USB_STREAM_TIMEOUT_MS;	
	#else
	uint16_t TimeoutMSRem = USB_STREAM_TIMEOUT_MS;
	#endif

	for (;;)
	{
		if (Endpoint_GetEndpointDirection() == ENDPOINT_DIR_IN)
		{
			if (Endpoint_IsINReady())
			  return ENDPOINT_READYWAIT_NoError;
		}
		else
		{
			if (Endpoint_IsOUTReceived())
			  return ENDPOINT_READYWAIT_NoError;		
		}
		
		if (USB_DeviceState == DEVICE_STATE_Unattached)
		  return ENDPOINT_READYWAIT_DeviceDisconnected;
		else if (Endpoint_IsStalled())
		  return ENDPOINT_READYWAIT_EndpointStalled;
			  
		if (USB_INT_HasOccurred(USB_INT_SOFI))
		{
			USB_INT_Clear(USB_INT_SOFI);

			if (!(TimeoutMSRem--))
			  return ENDPOINT_READYWAIT_Timeout;
		}
	}
}

int16_t Device_ReceiveByte(USB_EPInfo_Device_t* const EPInfo)
{
	if (USB_DeviceState != DEVICE_STATE_Configured)
		return -1;

	int16_t ReceivedByte = -1;

	Endpoint_SelectEndpoint(EPInfo->DataOUTEPAddress);

	if (Endpoint_IsOUTReceived())
	// Endpoint_AVR8.h
	// Determines if the selected OUT endpoint has received new packet from 
	// the host.
	{
		if (Endpoint_BytesInEndpoint())
		// Endpoint_AVR8.h
		// Indicates the number of bytes currently stored in the current 
		// endpoint's selected bank.
			ReceivedByte = Endpoint_Read_8();
			// Endpoint_AVR8.h
			// Reads one byte from the currently selected endpoint's bank, 
			// for OUT direction endpoints.
		if (!(Endpoint_BytesInEndpoint()))
			Endpoint_ClearOUT();
			// Endpoint_AVR8.h
			// Acknowledges an OUT packet to the host on the currently selected
			// endpoint, freeing up the endpoint for the next packet and 
			// switching to the alternative endpoint bank if double banked.
	}

	return ReceivedByte;

}

/** Main program entry point. This routine configures the hardware required by the application, then
 *  enters a loop to run the application tasks in sequence.
 */
int main(void)
{
	SetupHardware();

	LEDs_SetAllLEDs(LEDMASK_USB_NOTREADY);
	GlobalInterruptEnable();

	for (;;)
	{
		USB_USBTask();

		uint8_t ReceivedData[VENDOR_IO_EPSIZE];
		memset(ReceivedData, 0x00, sizeof(ReceivedData));

		Endpoint_SelectEndpoint(VENDOR_OUT_EPADDR);
		if (Endpoint_IsOUTReceived())
		{
			Endpoint_Read_Stream_LE(ReceivedData, VENDOR_IO_EPSIZE, NULL);
			Endpoint_ClearOUT();

			Endpoint_SelectEndpoint(VENDOR_IN_EPADDR);
			Endpoint_Write_Stream_LE(ReceivedData, VENDOR_IO_EPSIZE, NULL);
			Endpoint_ClearIN();
		}
	}
}

void usb_check_incoming()
{
	uint8_t dataReceived = -1;

	if (USB_DeviceState != DEVICE_STATE_Configured)
		return;

	uint8_t GenericData[BUFFER_EPSIZE];

	Endpoint_SelectEndpoint(GENERIC_OUT_EPADDR);

	// packet received?
	if ( Endpoint_IsOUTReceived() )
	{

		if ( Endpoint_IsReadWriteAllowed())
		{
			Endpoint_Read_Stream_LE( &GenericData, sizeof(GenericData), NULL);

			dataReceived = 1;
		}

		Endpoint_ClearOUT();
	}

	if ( dataReceived == 1 )
	{
		usb_process_incoming(GenericData);		
		//processDataCallBack(GenericData);
	}
}

/** Event handler for the USB_UnhandledControlPacket event. This is used to catch standard and class specific
 *  control requests that are not handled internally by the USB library (including the HID commands, which are
 *  all issued via the control endpoint), so that they can be handled appropriately for the application.
 */
void EVENT_USB_Device_UnhandledControlRequest(void)
{
	uint8_t* ReportData;
	uint8_t  ReportSize;

	/* Handle HID Class specific requests */
	switch (USB_ControlRequest.bRequest)
	{
		case REQ_GetReport:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
	
				/* Determine if it is the mouse or the keyboard data that is being requested */
				if (!(USB_ControlRequest.wIndex))
				{
					ReportData = (uint8_t*)&KeyboardReportData;
					ReportSize = sizeof(KeyboardReportData);
				}
				else
				{
					ReportData = (uint8_t*)&MouseReportData;
					ReportSize = sizeof(MouseReportData);
				}

				/* Write the report data to the control endpoint */
				Endpoint_Write_Control_Stream_LE(ReportData, ReportSize);

				/* Clear the report data afterwards */
				memset(ReportData, 0, ReportSize);
				
				/* Finalize the stream transfer to send the last packet or clear the host abort */
				Endpoint_ClearOUT();
			}
		
			break;
		case REQ_SetReport:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				
				/* Wait until the LED report has been sent by the host */
				while (!(Endpoint_IsOUTReceived()))
				{
					if (USB_DeviceState == DEVICE_STATE_Unattached)
					  return;
				}

				/* Read in and process the LED report from the host */
				Keyboard_ProcessLEDReport(Endpoint_Read_Byte());

				/* Clear the endpoint data */
				Endpoint_ClearOUT();

				Endpoint_ClearStatusStage();
			}
			
			break;
	}
}

uint16_t CDC_Device_BytesReceived(USB_ClassInfo_CDC_Device_t* const CDCInterfaceInfo)
{
	uint16_t bytes = 0;
	if ((USB_DeviceState == DEVICE_STATE_Configured) &&	(CDCInterfaceInfo->State.LineEncoding.BaudRateBPS != 0))
	{

		Endpoint_SelectEndpoint(CDCInterfaceInfo->Config.DataOUTEndpoint.Address);

		if (Endpoint_IsOUTReceived())
		{
			if (!(Endpoint_BytesInEndpoint()))
			{
				Endpoint_ClearOUT();
				bytes = 0;
			}
			else
			{
				bytes = Endpoint_BytesInEndpoint();
			}
		}
		else
		{
			bytes = 0;
		}
	}

	return bytes;
}

uint8_t TEMPLATE_FUNC_NAME (void* Buffer, uint16_t Length)
{
	uint8_t* DataStream = ((uint8_t*)Buffer + TEMPLATE_BUFFER_OFFSET(Length));
	
	if (!(Length))
	  Endpoint_ClearOUT();
	
	while (Length)
	{
		if (Endpoint_IsSETUPReceived())
		  return ENDPOINT_RWCSTREAM_HostAborted;

		if (USB_DeviceState == DEVICE_STATE_Unattached)
		  return ENDPOINT_RWCSTREAM_DeviceDisconnected;
		  
		if (Endpoint_IsOUTReceived())
		{
			while (Length && Endpoint_BytesInEndpoint())
			{
				TEMPLATE_TRANSFER_BYTE(DataStream);
				Length--;
			}
			
			Endpoint_ClearOUT();
		}		  
	}
	
	while (!(Endpoint_IsINReady()))
	{
		if (USB_DeviceState == DEVICE_STATE_Unattached)
		  return ENDPOINT_RWCSTREAM_DeviceDisconnected;
	}
	
	return ENDPOINT_RWCSTREAM_NoError;
}

void ecmd_lufa_rx(void) {
  /* Select the Serial Rx Endpoint */
  Endpoint_SelectEndpoint(CDC_RX_EPNUM);
  
  /* Check to see if a packet has been received from the host */
  if (Endpoint_IsOUTReceived())	{
    
    /* Read the bytes in from the endpoint into the buffer while space is available */
    while (Endpoint_BytesInEndpoint() && !must_parse) {

      /* Store each character from the endpoint */
      uint8_t data = Endpoint_Read_Byte();
      
      if (data == '\n' || data == '\r' || recv_len == sizeof(recv_buffer)) {
	recv_buffer[recv_len] = 0;
	must_parse = 1;
      } else {
 	recv_buffer[recv_len++] = data;
      }
      
    }
    
    /* Check to see if all bytes in the current packet have been read */
    if (!(Endpoint_BytesInEndpoint())) {
      /* Clear the endpoint buffer */
      Endpoint_ClearOUT();
    }
  }
}

int USBSerial::available(void)
{
        Endpoint_SelectEndpoint(CDC_RX_EPADDR);
	
	if (Endpoint_IsOUTReceived()) {
		if (Endpoint_BytesInEndpoint()) {
			return (unsigned int)Endpoint_BytesInEndpoint() + peekByteCnt;
		} else {
			Endpoint_ClearOUT();
			if (Endpoint_IsOUTReceived()) {
				return (unsigned int)Endpoint_BytesInEndpoint() + peekByteCnt;
			}
		}
	}
	
	return 0 + peekByteCnt;
}

bool Audio_Device_IsSampleReceived(USB_ClassInfo_Audio_Device_t* const AudioInterfaceInfo)
{
	if ((USB_DeviceState != DEVICE_STATE_Configured) || !(AudioInterfaceInfo->State.InterfaceEnabled))
	  return false;
	
	Endpoint_SelectEndpoint(AudioInterfaceInfo->Config.DataOUTEndpointNumber);	
	return Endpoint_IsOUTReceived();
}

uint8_t Endpoint_Read_Control_Stream_LE(void* const Buffer,
			                                        uint16_t Length)
{
	while(!Endpoint_IsOUTReceived());				//FIXME: this safe checking is fine for LPC18xx
	Endpoint_Read_Stream_LE(Buffer,Length,NULL);	//but hangs LPC17xx --> comment out
	Endpoint_ClearOUT();
	return ENDPOINT_RWCSTREAM_NoError;
}