void MS_Device_ProcessControlRequest(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo)
{
	if (!(Endpoint_IsSETUPReceived()))
	  return;

	if (USB_ControlRequest.wIndex != MSInterfaceInfo->Config.InterfaceNumber)
	  return;

	switch (USB_ControlRequest.bRequest)
	{
		case MS_REQ_MassStorageReset:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				Endpoint_ClearStatusStage();

				MSInterfaceInfo->State.IsMassStoreReset = true;
			}

			break;
		case MS_REQ_GetMaxLUN:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				while (!(Endpoint_IsINReady()));
				Endpoint_Write_8(MSInterfaceInfo->Config.TotalLUNs - 1);
				Endpoint_ClearIN();
				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 Mass Storage class-specific
 *  requests) so that they can be handled appropriately for the application.
 */
void EVENT_USB_Device_UnhandledControlRequest(void)
{
	/* Process UFI specific control requests */
	switch (USB_ControlRequest.bRequest)
	{
		case REQ_MassStorageReset:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				Endpoint_ClearStatusStage();

				/* Indicate that the current transfer should be aborted */
				IsMassStoreReset = true;
			}

			break;
		case REQ_GetMaxLUN:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();

				/* Indicate to the host the number of supported LUNs (virtual disks) on the device */
				Endpoint_Write_Byte(TOTAL_LUNS - 1);

				Endpoint_ClearIN();
				Endpoint_ClearStatusStage();
			}

			break;
	}
}

void PRNT_Device_ProcessControlRequest(USB_ClassInfo_PRNT_Device_t* const PRNTInterfaceInfo)
{
	if (!(Endpoint_IsSETUPReceived()))
	  return;

	if (USB_ControlRequest.wIndex != PRNTInterfaceInfo->Config.InterfaceNumber)
	  return;

	switch (USB_ControlRequest.bRequest)
	{
		case PRNT_REQ_GetDeviceID:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();

				while (!(Endpoint_IsINReady()))
				{
					if (USB_DeviceState == DEVICE_STATE_Unattached)
					  return;
				}

				uint16_t IEEEStringLen = strlen(PRNTInterfaceInfo->Config.IEEE1284String);
				Endpoint_Write_16_BE(IEEEStringLen);
				Endpoint_Write_Control_Stream_LE(PRNTInterfaceInfo->Config.IEEE1284String, IEEEStringLen);
				Endpoint_ClearStatusStage();
			}

			break;
		case PRNT_REQ_GetPortStatus:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();

				while (!(Endpoint_IsINReady()))
				{
					if (USB_DeviceState == DEVICE_STATE_Unattached)
					  return;
				}

				Endpoint_Write_8(PRNTInterfaceInfo->State.PortStatus);
				Endpoint_ClearStatusStage();
			}

			break;
		case PRNT_REQ_SoftReset:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				Endpoint_ClearStatusStage();

				PRNTInterfaceInfo->State.IsPrinterReset = true;

				EVENT_PRNT_Device_SoftReset(PRNTInterfaceInfo);
			}

			break;
	}
}

void CDC_Device_ProcessControlRequest(USB_ClassInfo_CDC_Device_t* const CDCInterfaceInfo)
{
	if (!(Endpoint_IsSETUPReceived()))
	  return;
	  
	if (USB_ControlRequest.wIndex != CDCInterfaceInfo->Config.ControlInterfaceNumber)
	  return;

	switch (USB_ControlRequest.bRequest)
	{
		case CDC_REQ_GetLineEncoding:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				Endpoint_Write_Control_Stream_LE(&CDCInterfaceInfo->State.LineEncoding, sizeof(CDCInterfaceInfo->State.LineEncoding));
				Endpoint_ClearOUT();
			}
			
			break;
		case CDC_REQ_SetLineEncoding:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				Endpoint_Read_Control_Stream_LE(&CDCInterfaceInfo->State.LineEncoding, sizeof(CDCInterfaceInfo->State.LineEncoding));
				Endpoint_ClearIN();

				EVENT_CDC_Device_LineEncodingChanged(CDCInterfaceInfo);
			}
	
			break;
		case CDC_REQ_SetControlLineState:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{				
				Endpoint_ClearSETUP();
				Endpoint_ClearStatusStage();

				CDCInterfaceInfo->State.ControlLineStates.HostToDevice = USB_ControlRequest.wValue;

				EVENT_CDC_Device_ControLineStateChanged(CDCInterfaceInfo);
			}
	
			break;
		case CDC_REQ_SendBreak:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{				
				Endpoint_ClearSETUP();
				Endpoint_ClearStatusStage();

				EVENT_CDC_Device_BreakSent(CDCInterfaceInfo, (uint8_t)USB_ControlRequest.wValue);
			}

			break;
	}
}

/** Event handler for the USB_UnhandledControlRequest event. This is used to catch standard and class specific
 *  control requests that are not handled internally by the USB library, so that they can be handled appropriately
 *  for the application.
 */
void EVENT_USB_Device_UnhandledControlRequest(void)
{
	uint8_t* LineCodingData = (uint8_t*)&LineCoding;

	/* Process CDC specific control requests */
	switch (USB_ControlRequest.bRequest)
	{
		case REQ_GetLineEncoding:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();

				for (uint8_t i = 0; i < sizeof(LineCoding); i++)
				  Endpoint_Write_Byte(*(LineCodingData++));	
				
				Endpoint_ClearIN();
				
				Endpoint_ClearStatusStage();
			}
			
			break;
		case REQ_SetLineEncoding:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();

				while (!(Endpoint_IsOUTReceived()))
				{				
					if (USB_DeviceState == DEVICE_STATE_Unattached)
					  return;
				}
			
				for (uint8_t i = 0; i < sizeof(LineCoding); i++)
				  *(LineCodingData++) = Endpoint_Read_Byte();

				Endpoint_ClearOUT();

				Endpoint_ClearStatusStage();
			}
	
			break;
		case REQ_SetControlLineState:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				Endpoint_ClearSETUP();
				
				Endpoint_ClearStatusStage();
			}
	
			break;
	}
}

/** 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_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;
	}
}

static void USB_Device_SetConfiguration(void)
{
	USB_Descriptor_Device_t* DevDescriptorPtr;


	if (CALLBACK_USB_GetDescriptor((DTYPE_Device << 8), 0, (void*)&DevDescriptorPtr
	                               ) == NO_DESCRIPTOR)
	{
		return;
	}

	if ((uint8_t)USB_ControlRequest.wValue > DevDescriptorPtr->NumberOfConfigurations)
	  return;

	Endpoint_ClearSETUP();

	USB_Device_ConfigurationNumber = (uint8_t)USB_ControlRequest.wValue;

	Endpoint_ClearStatusStage();

	if (USB_Device_ConfigurationNumber)
	  USB_DeviceState = DEVICE_STATE_Configured;
	else
	  USB_DeviceState = (USB_Device_IsAddressSet()) ? DEVICE_STATE_Configured : DEVICE_STATE_Powered;

	EVENT_USB_Device_ConfigurationChanged();
}

static void USB_Device_SetConfiguration(void)
{
#if defined(FIXED_NUM_CONFIGURATIONS)
	if ((uint8_t)USB_ControlRequest.wValue > FIXED_NUM_CONFIGURATIONS)
	  return;
#else
	#if !defined(USE_FLASH_DESCRIPTORS) && !defined(USE_EEPROM_DESCRIPTORS) && !defined(USE_RAM_DESCRIPTORS)
	uint8_t MemoryAddressSpace;
	#endif
	
	USB_Descriptor_Device_t* DevDescriptorPtr;

	if (CALLBACK_USB_GetDescriptor((DTYPE_Device << 8), 0, (void*)&DevDescriptorPtr
	#if !defined(USE_FLASH_DESCRIPTORS) && !defined(USE_EEPROM_DESCRIPTORS) && !defined(USE_RAM_DESCRIPTORS)
	                               , &MemoryAddressSpace
	#endif
	                               ) == NO_DESCRIPTOR)
	{
		return;
	}
	
	#if defined(USE_RAM_DESCRIPTORS)
	if ((uint8_t)USB_ControlRequest.wValue > DevDescriptorPtr->NumberOfConfigurations)
	  return;
	#elif defined (USE_EEPROM_DESCRIPTORS)
	if ((uint8_t)USB_ControlRequest.wValue > eeprom_read_byte(&DevDescriptorPtr->NumberOfConfigurations))
	  return;
	#elif defined (USE_FLASH_DESCRIPTORS)
	if ((uint8_t)USB_ControlRequest.wValue > pgm_read_byte(&DevDescriptorPtr->NumberOfConfigurations))
	  return;
	#else
	if (MemoryAddressSpace == MEMSPACE_FLASH)
	{
		if (((uint8_t)USB_ControlRequest.wValue > pgm_read_byte(&DevDescriptorPtr->NumberOfConfigurations)))
		  return;
	}
	else if (MemoryAddressSpace == MEMSPACE_EEPROM)
	{
		if (((uint8_t)USB_ControlRequest.wValue > eeprom_read_byte(&DevDescriptorPtr->NumberOfConfigurations)))
		  return;
	}
	else
	{
		if ((uint8_t)USB_ControlRequest.wValue > DevDescriptorPtr->NumberOfConfigurations)
		  return;
	}
	#endif
#endif
	
	Endpoint_ClearSETUP();

	USB_ConfigurationNumber = (uint8_t)USB_ControlRequest.wValue;

	Endpoint_ClearStatusStage();

	USB_DeviceState = (USB_ConfigurationNumber) ? DEVICE_STATE_Configured : DEVICE_STATE_Addressed;

	EVENT_USB_Device_ConfigurationChanged();
}

void process_GET_POSE(){
	uint16_t myPose[6];
	myPose[0] = 0;//Do This Right
	Endpoint_ClearSETUP();
	Endpoint_Write_Stream_LE(myPose, 16, 0);
	Endpoint_ClearIN();
	Endpoint_ClearStatusStage();
}

void process_SET_STOP(){
	/* Accept command */
	Endpoint_ClearSETUP();
	/* and mark the whole request as successful: */
	Endpoint_ClearStatusStage();
	/* Process Command Parameters: */
	//Call fuckingStop();
}

/**
 * \brief LOCK request
 *
 * The LOCK request disables focus changes 
 */
void	process_lock() {
	Endpoint_ClearSETUP();
	Endpoint_ClearStatusStage();
	if (USB_ControlRequest.wIndex) {
		recv_lock();
	} else {
		recv_unlock();
	}
}

void process_GET_STATUS(){
	uint16_t myStatus = 0;
	Endpoint_ClearSETUP();
	Endpoint_Write_16_LE(myStatus);
	/* send packet */
	Endpoint_ClearIN();
	/* and mark the whole request as successful: */
	Endpoint_ClearStatusStage();	
}

static void USB_Device_GetConfiguration(uint8_t corenum)
{
	Endpoint_ClearSETUP(corenum);

	Endpoint_Write_8(corenum, USB_Device_ConfigurationNumber);
	Endpoint_ClearIN(corenum);

	Endpoint_ClearStatusStage(corenum);
}

static void USB_Device_GetConfiguration(void)
{
	Endpoint_ClearSETUP();

	Endpoint_Write_Byte(USB_ConfigurationNumber);
	Endpoint_ClearIN();

	Endpoint_ClearStatusStage();
}

/**
 * \brief SET request
 *
 * The SET request moves the focuser to a position indicated by the wValue
 * field in the request. If wIndex is nonzero, then the movement is done
 * in high speed mode, otherwise in slow speed.
 */
void	process_set() {
	Endpoint_ClearSETUP();
	Endpoint_ClearStatusStage();
	if ((USB_ControlRequest.wValue == 0)
		|| (USB_ControlRequest.wValue == 0xffff)) {
		return;
	}
	motor_moveto(USB_ControlRequest.wValue,
		(USB_ControlRequest.wIndex) ? SPEED_FAST : SPEED_SLOW);
}

void EVENT_USB_Device_ControlRequest(void){
    void* buffer = NULL;
    uint16_t length =  0;
    if(change != NULL) {
        usb_set_device(change, 1);
        Endpoint_ClearSETUP();
        Endpoint_ClearIN();
        Endpoint_ClearOUT();
    } else { 
        Endpoint_ClearSETUP();
    }
    uart_print("Type: %x, req: %x, val: %x, idx: %x, len: %d\n",
                USB_ControlRequest.bmRequestType,
                USB_ControlRequest.bRequest,
                USB_ControlRequest.wValue,
                USB_ControlRequest.wIndex,
                USB_ControlRequest.wLength);

    device->handle_configuration(   device,
                                    USB_ControlRequest.bmRequestType,
                                    USB_ControlRequest.bRequest,
                                    USB_ControlRequest.wValue,
                                    USB_ControlRequest.wIndex,
                                    USB_ControlRequest.wLength,
                                    &buffer,
                                    &length);

    if(buffer) {
        if(length == 0xFFFF) {
            return;
        }

        if(length) {
            if(length & CONST_PTR) {
                Endpoint_Write_Control_PStream_LE(buffer, length & ~CONST_PTR);
            } else {
                Endpoint_Write_Control_Stream_LE(buffer, length);
            }
        }

        Endpoint_ClearStatusStage();

        if(deferred_addr >= 0) {
            _usb_set_addr(deferred_addr);
            uart_print("Setting addr to %d\n", deferred_addr);
            deferred_addr = -1;
        }

    } else {
        Endpoint_StallTransaction();
        uart_print("STALLED!\n");
    }

}

/**
 * \brief RESET request
 *
 * The RESET request is handled by simply setting the whatdog timer to
 * such a short interval that it is bound to be triggered.
 */
void	process_reset() {
	Endpoint_ClearSETUP();
	Endpoint_ClearStatusStage();
	// the usual method to set the watchdog timer timeout to
	// a short time does not really work, because interrupts happen
	// so often in this application. We use the reset flag from
	// the timer, which tells the timer interrupt no longer to reset
	// the watchdog timer, which will utlimately cause a watch dog timer
	// reset
	resetflag = 0;
}

//LUFA USB Event Processer Functions
void process_SET_GOAL(){
	uint16_t newGoal[6];
	/* Accept command */
	Endpoint_ClearSETUP();
	/* read data from endpoint */
	Endpoint_Read_Control_Stream_LE(newGoal, 16);
	/* and mark the whole request as successful: */
	Endpoint_ClearStatusStage();
	/* process command parameters: */
	//Call changeGoal(newGoal);
}

/** Event handler for the USB_UnhandledControlRequest event. This is used to catch standard and class specific
 *  control requests that are not handled internally by the USB library (including the CDC control 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)
{
	/* Process CDC specific control requests */
	switch (USB_ControlRequest.bRequest)
	{
		case REQ_GetLineEncoding:
			if (USB_ControlRequest.bmRequestType == (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE))
			{	
				/* Acknowledge the SETUP packet, ready for data transfer */
				Endpoint_ClearSETUP();

				/* Write the line coding data to the control endpoint */
				Endpoint_Write_Control_Stream_LE((CDC_Line_Coding_t*)&LineEncoding, sizeof(CDC_Line_Coding_t));
				
				/* Finalize the stream transfer to send the last packet or clear the host abort */
				Endpoint_ClearOUT();
			}
			
			break;
		case REQ_SetLineEncoding:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{
				/* Acknowledge the SETUP packet, ready for data transfer */
				Endpoint_ClearSETUP();

				/* Read the line coding data in from the host into the global struct */
				Endpoint_Read_Control_Stream_LE((CDC_Line_Coding_t*)&LineEncoding, sizeof(CDC_Line_Coding_t));

				/* Reconfigure the USART with the new settings */
				ReconfigureUSART();

				/* Finalize the stream transfer to clear the last packet from the host */
				Endpoint_ClearIN();
			}
	
			break;
		case REQ_SetControlLineState:
			if (USB_ControlRequest.bmRequestType == (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE))
			{				
				/* Acknowledge the SETUP packet, ready for data transfer */
				Endpoint_ClearSETUP();
				
				/* NOTE: Here you can read in the line state mask from the host, to get the current state of the output handshake
				         lines. The mask is read in from the wValue parameter in USB_ControlRequest, and can be masked against the
						 CONTROL_LINE_OUT_* masks to determine the RTS and DTR line states using the following code:
				*/

				Endpoint_ClearStatusStage();
			}
	
			break;
	}
}