/**
 * usbhid_interface.c
 *
 * Commands, queries, and reports over the USB HID connection.
 * 
 * Copyright (c) 2022 Bigscreen, Inc.
 */

#include <asf.h>
#include <string.h>
#include "qmath.h"

TaskHandle_t xSendHidTaskHandle = NULL;

volatile uint16_t report_rate; // Delay in milliseconds between periodic HID reports
// Globals for HID interaction
// written by "usbhid_set_feature", read by "usbhid_send_report"
// Direct task notification used to prevent data races
uint8_t user_signature_block_num; // User signature buffer - requested block number
uint8_t vxr_block_to_delete; // VXR flash deletion - which block to delete
uint8_t vxr_sector_to_delete; // VXR flash deletion - which sector to delete (if block = 0x80)
uint32_t vxr_start_addr; // VXR checksum calculation - starting address in flash. VXR Flash program - where to start programming bytes
uint32_t vxr_len; // VXR checksum calculation - length of flash to calculate over. VXR Flash program - number of bytes to program
uint8_t vxr_program_buf[32];
uint32_t new_timer_val; // Timer value overwrite - new value for a usage timer
Hid_Error_T hid_error_reply; // when responding with an error code, what error to reply with

// TODO: Get a better interface than passing externs around
extern uint8_t oled_temp_left;
extern uint8_t oled_temp_right;
extern I2C_Control_T i2c_control_cmd;
extern RGB_Color_t newcolor;
extern uint16_t newbrightness;
extern UBaseType_t task_watermarks[16];

static void usbhid_process_command(void *pvParameters);
static void usbhid_periodic_report(void *pvParameters);

/*
static void _app_exec(uint32_t app_address) {
	uint8_t i;
	// Disable IRQs
	__DMB();
	__ISB();
	__disable_irq();
	// Clear pending IRQs
	for(i = 0; i < 8; i++) {
		NVIC->ICER[i] = 0xFFFFFFFFu;
		NVIC->ICPR[i] = 0xFFFFFFFFu;
	}
	// Disable Systick, USB
	SysTick->CTRL = 0;
	// Modify vector table location
	__DSB();
	__ISB();
	SCB->VTOR = ((uint32_t)app_address & SCB_VTOR_TBLOFF_Msk);
	// Set new stack pointer and jump to app
	void (*pApp)(void) = (void(*)(void)) *((uint32_t *)(app_address + 4));
	__DMB();
	__ISB();
	__set_MSP(*((uint32_t *)(app_address)));
	pApp();
}
*/

void send_code_to_hid_task(uint32_t code) {
	xTaskNotify(xSendHidTaskHandle, code, eSetValueWithOverwrite);
}

void send_code_to_hid_task_fromISR(uint32_t code) {
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
	xTaskNotifyFromISR(xSendHidTaskHandle, code, eSetValueWithOverwrite, &xHigherPriorityTaskWoken);
	portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void start_usbhid_task(void) {
	if(xTaskCreate(usbhid_process_command, "HID Commands", TASK_REPORT_STACK_SIZE, NULL,
	TASK_REPORT_PRIORITY, &xSendHidTaskHandle) != pdPASS) {
		printf("Failed to create HID command processor task\r\n");
	}
	if(xTaskCreate(usbhid_periodic_report, "HID Report", TASK_REPORT_STACK_SIZE, NULL,
	TASK_REPORT_PRIORITY, NULL) != pdPASS) {
		printf("Failed to create HID periodic report task\r\n");
	}
}

static uint8_t try_send_hid_report_with_timeout(uint8_t* report_data, uint32_t timeout) {
	// Repeatedly call udi_hid_generic_send_report_in until it either succeeds or a timeout was reached
	
	uint32_t start_time = rtt_read_timer_value();
	bool report_succeeded = false;
	
	while(!report_succeeded) {
		if(rtt_timeout_check(start_time, timeout)) {
			return pdFAIL;
		}
		report_succeeded = udi_hid_generic_send_report_in(report_data);
	}
	
	return pdPASS;
}


static void processor_reset(void) {
	
	uint32_t rstc_mode_reg;
	rstc_mode_reg = RSTC->RSTC_MR;
	rstc_mode_reg &= ~(RSTC_MR_ERSTL_Msk | RSTC_MR_KEY_Msk);
	// rstc_mode_reg |= RSTC_MR_ERSTL(0xFF); // longest possible reset time (about 2 seconds)
	rstc_mode_reg |= RSTC_MR_ERSTL(0x07); // About 2^8 / 32768 seconds (7.8ms)
	rstc_mode_reg |= RSTC_MR_KEY_PASSWD;
	RSTC->RSTC_MR = rstc_mode_reg;
	RSTC->RSTC_CR = RSTC_CR_KEY_PASSWD | RSTC_CR_EXTRST; // assert reset pin
}

// "*report_data" is global to avoid using stack space. Stack is preallocated for FreeRTOS tasks.
uint8_t _command_report_data[UDI_HID_REPORT_FEATURE_SIZE];
static void usbhid_process_command(void *pvParameters) {
	UNUSED(pvParameters);	
	const uint8_t* sig_buf_reference;
	RC_Error_T vxr_retval;
	
	// xSendHidTaskHandle = xTaskGetCurrentTaskHandle();
	
	uint32_t notify_value;

	memset(_command_report_data, 0, UDI_HID_REPORT_FEATURE_SIZE); // Clear report
	
	while(pdTRUE) {
		// If there's a notification process it right away. Otherwise sit and wait.
		notify_value = ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
		if(notify_value != 0) {
			// Got here due to another task notifying this one
			if(notify_value == HID_REPORT_TASK_CODE_FOR_RESET) {
				// Shutdown all systems. This also stops the RTOS from firing, so we will
				// now be stuck in this function.
				main_system_shutdown();
				// apply reset
				processor_reset();
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_BOOTLOADER) {
				// Shutdown all systems. This also stops the RTOS from firing, so we will
				// now be stuck in this function.
				main_system_shutdown();
				// By setting a particular value in the backup registers, the bootloader
				// does not launch this application by default when it runs.
				GPBR->SYS_GPBR[0] = 0xBB;
				GPBR->SYS_GPBR[1] = 0x1A;
				// jump to bootloader by applying reset
				processor_reset();
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_SW_VER) {
				// Reply with software version number string
				_command_report_data[0] = HID_CODE_FOR_SW_VER_REPLY;
				strcpy(&(_command_report_data[1]), VERSION_STRING);
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_SERIAL) {
				if(sigv2_tag_length(SigTag_Serial) > 0) {
					_command_report_data[0] = HID_CODE_FOR_SERIAL_NUM_REPLY;
					sigv2_find_tag(SigTag_Serial, &(_command_report_data[1]));
				} else {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = Hid_Unsupported;
				}
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_HMD_SERIAL) {
				if(sigv2_tag_length(SigTag_HMD_Serial) > 0) {
					_command_report_data[0] = HID_CODE_FOR_HMD_SERIAL_REPLY;
					sigv2_find_tag(SigTag_HMD_Serial, &(_command_report_data[1]));
					} else {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = Hid_Unsupported;
				}
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_TRACKING_SERIAL) {
				if(sigv2_tag_length(SigTag_Tracking_Serial) > 0) {
					_command_report_data[0] = HID_CODE_FOR_TRACKING_SERIAL_REPLY;
					sigv2_find_tag(SigTag_Tracking_Serial, &(_command_report_data[1]));
					} else {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = Hid_Unsupported;
				}
			}
			if((notify_value & 0xFFu) == HID_REPORT_TASK_CODE_FOR_USAGE_TIMER_GET) {
				// Requested timer is placed in bits 15-8 (above the 8-bit report task code)
				// 0: Total power on time
				// 1: Displays on time (left module)
				// 2: Longest continuous display on time
				// 3: Displays on time (right module)
				uint32_t usage_data;
				EEPROM_Keys_T eekey;
				switch((notify_value & 0xFF00) >> 8) {
					case 0:
						eekey = EEKEY_TotalPoweredOnTime10min;
					break;
					case 1:
						eekey = EEKEY_TotalDisplaysOnTime10min_Left;
					break;
					case 2:
						eekey = EEKEY_LongestDisplaysOnTime10min;
					break;
					case 3:
						eekey = EEKEY_TotalDisplaysOnTime10min_Right;
					break;
					default:
						eekey = EEKEY_None;
					break;
				}
				if(EEKEY_None != eekey) {
					// Find the saved variable in EEPROM
					if(EEPROM_Found == EEPROM_Read(eekey, &usage_data)) {
						// Data was stored in EEPROM, return in the HID report
						_command_report_data[0] = HID_CODE_FOR_USAGE_TIMER_REPLY;
						memcpy(&(_command_report_data[1]), &usage_data, sizeof(uint32_t));
					} else {
						// Return an error, data wasn't found
						_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
						_command_report_data[1] = Hid_Unsupported;
					}
				} else {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = Hid_Invalid_Arg;
				}
			}
			if((notify_value & 0xFFu) == HID_REPORT_TASK_CODE_FOR_USAGE_TIMER_SET) {
				// Requested timer is placed in bits 15-8 (above the 8-bit report task code)
				// 0: Total power on time
				// 1: Displays on time (left module)
				// 2: Longest continuous display on time
				// 3: Displays on time (right module)
				EEPROM_Keys_T eekey;
				switch((notify_value & 0xFF00) >> 8) {
					case 0:
					eekey = EEKEY_TotalPoweredOnTime10min;
					break;
					case 1:
					eekey = EEKEY_TotalDisplaysOnTime10min_Left;
					break;
					case 2:
					eekey = EEKEY_LongestDisplaysOnTime10min;
					break;
					case 3:
					eekey = EEKEY_TotalDisplaysOnTime10min_Right;
					break;
					default:
					eekey = EEKEY_None;
					break;
				}
				if(EEKEY_None != eekey) {
					// Overwrite the data in EEPROM
					if(EEPROM_Write_Success == EEPROM_Write(eekey, new_timer_val)) {
						_command_report_data[0] = HID_CODE_FOR_SUCCESS_REPLY;
					} else {
						// Return an error, write was unsuccessful
						_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
						_command_report_data[1] = Hid_Unknown_Error;
					}					
				} else {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = Hid_Invalid_Arg;
				}
			}
			
			if(notify_value == HID_REPORT_TASK_CODE_FOR_SUCCESS) {
				// Simple response to show the last command was successful
				_command_report_data[0] = HID_CODE_FOR_SUCCESS_REPLY;
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_ERROR) {
				// Respond with the error code in byte 1
				_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
				_command_report_data[1] = (uint8_t) hid_error_reply;
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_SIG) {
				// Only allow blocks 0 to 15. There are 512 bytes total in the user signature region, 
				// and we read 32 at a time. So there are 16 blocks of 32 bytes we can reply with.
				if(user_signature_block_num > 15) {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = Hid_Invalid_Arg;
				} else {
				
					// Read the signature region and reply with the requested 
					sig_buf_reference = (const uint8_t*)get_signature_raw();
				
					_command_report_data[0] = HID_CODE_FOR_SIG_REPLY;
					_command_report_data[1] = HID_SIG_XFER_LENGTH; // Always 32 bytes long
					memcpy(&(_command_report_data[2]), &(sig_buf_reference[user_signature_block_num*HID_SIG_XFER_LENGTH]), HID_SIG_XFER_LENGTH);
				}
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_VXR_CHECKSUM) {
				// Grab the checksum from the VXR chip
				_command_report_data[0] = HID_CODE_FOR_VXR_CHECKSUM_REPLY;
				_command_report_data[1] = VXR_CHECKSUM_LENGTH; // always a 32-bit (4-byte) checksum value
				uint32_t temp;
				vxr_retval = VXR_Calc_Checksum(vxr_start_addr, vxr_len, &temp);
				if(RC_Success == vxr_retval) {
					memcpy(&(_command_report_data[2]), &temp, VXR_CHECKSUM_LENGTH);
				} else {
					memset(&(_command_report_data[2]), 0, VXR_CHECKSUM_LENGTH);
				}
				
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_VXR_TAGS) {
				// Figure out which banks are active - config and firmware - in the VXR7200
				// Response is a single byte, with bitfield definitions:
				// b0 - Config0 tag is valid
				// b1 - Config1 tag is valid
				// b2 - Firmware0 tag is valid
				// b3 - Firmware1 tag is valid
				// b4 to b7 reserved
				_command_report_data[0] = HID_CODE_FOR_VXR_TAGS_REPLY;
				_command_report_data[1] = 0;
				if(VXR_Tag_Valid(Vxr_Tag_Config_0)) _command_report_data[1] |= 0x01;
				if(VXR_Tag_Valid(Vxr_Tag_Config_1)) _command_report_data[1] |= 0x02;
				if(VXR_Tag_Valid(Vxr_Tag_Firmware_0)) _command_report_data[1] |= 0x04;
				if(VXR_Tag_Valid(Vxr_Tag_Firmware_1)) _command_report_data[1] |= 0x08;
				
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_VXR_DELETE) {
				if(vxr_block_to_delete <= 7) {
					vxr_retval = VXR_Erase_Firmware_Bank(vxr_block_to_delete);
					if(RC_Success == vxr_retval) {
						_command_report_data[0] = HID_CODE_FOR_SUCCESS_REPLY;
					} else {
						_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
						_command_report_data[1] = (uint8_t)vxr_retval;
					}
				} else if(vxr_block_to_delete == 0x80 && vxr_sector_to_delete <= 127){
					vxr_retval = VXR_Erase_Firmware_Sector(vxr_sector_to_delete);
					if(RC_Success == vxr_retval) {
						_command_report_data[0] = HID_CODE_FOR_SUCCESS_REPLY;
					} else {
						_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
						_command_report_data[1] = (uint8_t)vxr_retval;
					}
					
				} else {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = Hid_Invalid_Arg;
				}
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_VXR_PROGRAM) {
				vxr_retval = VXR_Program_Firmware(vxr_start_addr, vxr_len, vxr_program_buf);
				if(RC_Success == vxr_retval) {
					_command_report_data[0] = HID_CODE_FOR_SUCCESS_REPLY;
				} else {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
					_command_report_data[1] = (uint8_t)vxr_retval;
				}
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_VXR_FWNAME) {
				_command_report_data[0] = HID_CODE_FOR_VXR_FWNAME_REPLY;
				vxr_retval = VXR_Get_Firmware_Name(&(_command_report_data[1]));
				if(RC_Success != vxr_retval) {
					memset(&(_command_report_data[1]), ' ', 15); // Set as all spaces if error in retrieval
				}
			}
			if((notify_value & (0x00FFu))== HID_REPORT_TASK_CODE_FOR_OLED_ID) {
				// Panel selection is in the high byte. Zero if left, one if right (or really anything besides zero)
				OLED_Panel_T panel_sel = ((notify_value & 0xFF00u) == 0x00) ? OLED_LEFT : OLED_RIGHT;
				
				_command_report_data[0] = HID_CODE_FOR_OLED_ID_REPLY;
				_command_report_data[1] = OLED_read_ID(panel_sel, &(_command_report_data[2]));
				if(_command_report_data[1] == 0) {
					_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
				}
			}
			if((notify_value & (0x00FFu)) == HID_REPORT_TASK_CODE_FOR_HW_TEST) {
				_command_report_data[0] = HID_CODE_FOR_HW_TEST_REPLY;
				switch( (HW_Test_T)((notify_value & 0xFF00u) >> 8) ) {
					case HWTest_USB_Hub:
						_command_report_data[1] = test_usb_hub();
					break;
					case HWTest_RGB_LED:
						_command_report_data[1] = test_rgb_led();
					break;
					case HWTest_USBC_Switch:
						_command_report_data[1] = test_usbc_switch();
					break;
					case HWTest_VXR:
						_command_report_data[1] = test_vxr();
					break;
					case HWTest_Prox:
						_command_report_data[1] = test_prox();
					break;
					case HWTest_OLED:
						_command_report_data[1] = test_oled_panels();
					break;
					case HWTest_Fan:
						_command_report_data[1] = test_fan();
					break;
					default:
						_command_report_data[0] = HID_CODE_FOR_ERROR_REPLY;
						_command_report_data[1] = Hid_Invalid_Arg;
					break;
				}
				

			}
			if((notify_value) == HID_REPORT_TASK_CODE_FOR_COLORBARPATTERN) {
				video_set_colorbar();
				_command_report_data[0] = HID_CODE_FOR_SUCCESS_REPLY;
			}
			if(notify_value == HID_REPORT_TASK_CODE_FOR_STACK_LEVELS) {
				_command_report_data[0] = HID_CODE_FOR_STACK_LEVELS_REPLY;
				_command_report_data[1] = 4*taskid_NUM_TASKS; // Number of bytes is tasks * size of UBaseType_t (uint32_t)
				for(uint8_t i = 0; i < taskid_NUM_TASKS; i++) {
					memcpy(&(_command_report_data[4*i+2]), &(task_watermarks[i]), 4);
				}
			}
			
			if(vbus_is_present()) {
				//udi_hid_generic_send_report_in(_command_report_data);
				try_send_hid_report_with_timeout(_command_report_data, DEFAULT_HID_WAIT_TIME);
			}
			memset(_command_report_data, 0, UDI_HID_REPORT_FEATURE_SIZE); // Clear report
		}
	}
}

// "*report_data" is global to avoid using stack space. Stack is preallocated for FreeRTOS tasks.
uint8_t _periodic_report_data[UDI_HID_REPORT_FEATURE_SIZE];
static void usbhid_periodic_report(void *pvParameters) {
	UNUSED(pvParameters);
	uint16_t temp_val;
	
	report_rate = 1000; // 1 second reports to begin
	
	while(pdTRUE) {
		vTaskDelay(report_rate);
		_periodic_report_data[0] = HID_CODE_FOR_DATA_REPLY;
		_periodic_report_data[1] = 20; // currently has 4x 2-bytes (16-bit) values:
		// fan speed, prox distance, CC1, and CC2
		// then 3x 4-byte floating point values:
		// board temperature, left eye temp, right eye temp
		temp_val = get_fan_speed();
		_periodic_report_data[2] = (temp_val & 0xFF00) >> 8;
		_periodic_report_data[3] = temp_val & 0x00FF;
		temp_val = prox_get_distance();
		_periodic_report_data[4] = (temp_val & 0xFF00) >> 8;
		_periodic_report_data[5] = temp_val & 0x00FF;
				
		// CC1 / CC2 values
		temp_val = get_cc1_val();
		_periodic_report_data[6] = (temp_val & 0xFF00) >> 8;
		_periodic_report_data[7] = temp_val & 0x00FF;
		temp_val = get_cc2_val();
		_periodic_report_data[8] = (temp_val & 0xFF00) >> 8;
		_periodic_report_data[9] = temp_val & 0x00FF;
				
		// Temperatures
		float temp_float = temp_sense_get_temperature();
		memcpy(&(_periodic_report_data[10]), &temp_float, 4);
				
		temp_float = -50.71429f + ((float)oled_temp_left)*0.714f;
		memcpy(&(_periodic_report_data[14]), &temp_float, 4);
				
		temp_float = -50.71429f + ((float)oled_temp_right)*0.714f;
		memcpy(&(_periodic_report_data[18]), &temp_float, 4);
				
		if(vbus_is_present()) {
			//udi_hid_generic_send_report_in(_periodic_report_data);
			try_send_hid_report_with_timeout(_periodic_report_data, DEFAULT_HID_WAIT_TIME);
		}
		memset(_periodic_report_data, 0, UDI_HID_REPORT_FEATURE_SIZE); // Clear report
			
	}
}
	
/************************************************************************/
/*  Uses HID communication channel to access internal features and      */
/*  settings.															*/
/*	Responds to a "set feature report" command from the host computer.	*/
/*																		*/
/*	This function is called from an interrupt service routine, so all   */
/*	FreeRTOS functions must use the _FromISR variant if available.		*/
/************************************************************************/

void usbhid_set_feature(uint8_t* report) {
	BaseType_t xHigherPriorityTaskWoken;
	uint32_t report_task_code = 0;
	if(report[0] == HID_CODE_FOR_SW_VER){
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SW_VER);
	}
	if(report[0] == HID_CODE_FOR_SERIAL_NUM) {
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SERIAL);
	}
	if(report[0] == HID_CODE_FOR_HMD_SERIAL_NUM) {
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_HMD_SERIAL);
	}
	if(report[0] == HID_CODE_FOR_TRACKING_SERIAL) {
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_TRACKING_SERIAL);
	}
	if(report[0] == HID_CODE_FOR_USAGE_TIMER_GET) {
		// First byte after the tag is the requested timer
		// 0: Total power on time
		// 1: Displays on time (left module)
		// 2: Longest continuous display on time
		// 3: Displays on time (right module)
		// Shifted 8 bits left and OR'd with the HID task code as the task notification
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_USAGE_TIMER_GET | (report[1] << 8));
	}
	if(report[0] == HID_CODE_FOR_USAGE_TIMER_SET) {
		// First byte after the tag is the requested timer
		//		0: Total power on time
		//		1: Displays on time (left module)
		//		2: Longest continuous display on time
		//		3: Displays on time (right module)
		// Next 4 bytes are the new timer value. Timer is in units of 10-minutes. So 1 hour would be "6".
		//		Data is sent LSB first (little-endian). 
		new_timer_val = ((uint32_t)report[2]) + (((uint32_t)report[3])<<8) + (((uint32_t)report[4])<<16) + (((uint32_t)report[5])<<24);
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_USAGE_TIMER_SET | (report[1] << 8));
	}
	if(report[0] == HID_CODE_FOR_STEREO) {
		// Toggle audio modes (mono/stereo)
		if(report[1] == 0) {
			// Mono
			pdmmic_set_mixstate(MIX_STATE_ADD);
			} else {
			// Stereo
			pdmmic_set_mixstate(MIX_STATE_STEREO);
		}
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
	}
	if(report[0] == HID_CODE_FOR_GAIN) {
		// Gain
		// 15 bit number, so anything over 32767 will be clipped to that value
		uint16_t newgain = (((uint16_t)report[1]) << 8) + report[2];
		if(newgain > 32767) newgain = 32767;
		set_pdm_gain(newgain);
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
	}
	if(report[0] == HID_CODE_FOR_FRAC_GAIN) {
		// Fractional gain
		// Q16 number, from -1 to allllmost +1
		uint16_t newgain = (((uint16_t)report[1]) << 8) + report[2];
		set_pdm_fractional_gain((q16_t)newgain);
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
	}
	if(report[0] == HID_CODE_FOR_FAN) {
		// Fan control
		// Second byte is duty cycle, zero to 100
		set_fan_pwm(report[1]); // values above 100 will be clipped to 100%
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
	}
	if(report[0] == HID_CODE_FOR_RATE) {
		// Report rate
		// Sets how frequently the Interrupt IN report is sent (in millisec)
		uint16_t new_rate = (((uint16_t)report[1]) << 8) + report[2];
		if(new_rate >= MIN_REPORT_RATE) {
			report_rate = new_rate;
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
			} else {
			hid_error_reply = Hid_Invalid_Arg;
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_ERROR);
		}
	}
	if(report[0] == HID_CODE_FOR_OLED_FLIP) {
		// Debug commands
		// Set DDIC registers
		// report bytes:
		// - 0 = 'P' 0x50
		// - 1 = flip (1) or no flip (0)
		if(report[1] == 0) {
			if(send_code_to_i2c_task_fromISR(I2C_CTRL_OLED_UNFLIP)) {
				report_task_code = HID_REPORT_TASK_CODE_FOR_SUCCESS;
			} else {
				report_task_code = HID_REPORT_TASK_CODE_FOR_ERROR;
				hid_error_reply = Hid_Busy_Error;
			}
			send_code_to_hid_task_fromISR(report_task_code);
		} else if(report[1] == 1) {
			if(send_code_to_i2c_task_fromISR(I2C_CTRL_OLED_FLIP)) {
				report_task_code = HID_REPORT_TASK_CODE_FOR_SUCCESS;
			} else {
				report_task_code = HID_REPORT_TASK_CODE_FOR_ERROR;
				hid_error_reply = Hid_Busy_Error;
			}
			send_code_to_hid_task_fromISR(report_task_code);
		} else {
			// Invalid
			i2c_control_cmd = I2C_CTRL_NONE;
			hid_error_reply = Hid_Invalid_Arg;
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_ERROR);
		}
	}
	if(report[0] == HID_CODE_FOR_OLED_ID) {
		// Immediately trigger reading the ID from the selected display
		// report bytes:
		// - 0 = '^' 0x5E
		// - 1 = panel selection, 0 = left, 1 = right
		switch(report[1]) {
			case 0:
				send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_OLED_ID);
			break;
			case 1:
				send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_OLED_ID | (0x0100u));
			break;
			default:
				send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_ERROR);
			break;
		}
	}
	if(report[0] == HID_CODE_FOR_OLED_BRIGHNTESS) {
		// Debug commands
		// Set DDIC registers
		// report bytes:
		// - 0 = 'I' 0x49
		// - 1 = brightness high byte
		// - 2 = brightness low byte
		
		newbrightness = (((uint16_t) report[1]) << 8) + report[2];
		if(send_code_to_i2c_task_fromISR(I2C_CTRL_OLED_BRIGHTNESS)) {
			report_task_code = HID_REPORT_TASK_CODE_FOR_SUCCESS;
		} else {
			report_task_code = HID_REPORT_TASK_CODE_FOR_ERROR;
			hid_error_reply = Hid_Busy_Error;
		}
		send_code_to_hid_task_fromISR(report_task_code);
	}
	if(report[0] == HID_CODE_FOR_RESET) {
		// Triggers a device restart
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_RESET);
		// Cannot reply - will immediately restart
	}
	if(report[0] == HID_CODE_FOR_BOOTLOADER) {
		// Triggers a restart into bootloader
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_BOOTLOADER);
		// Cannot reply - will immediately restart into bootloader
	}
	if(report[0] == HID_CODE_FOR_READ_SIG) {
		// Reads the user signature region. Returns a requested block of 32 bytes
		// from the 512 byte total region. That means the allowable block addresses
		// are 0 to 15.
		user_signature_block_num = report[1];
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SIG);
	}
	
	if(report[0] == HID_CODE_FOR_WRITE_SIG) {
		// Writes 32 bytes in one of the 16 segments (16*32 = 512 total bytes) of the user
		// signature region in flash. Allowable block addresses are 0 to 15. Followed up
		// by 32 bytes of data to write.
		// This request only writes to the temporary ram buffer. After writing all the signature
		// bytes, user should then send HID_CODE_FOR_SAVE_SIG to commit it to flash
		
		if(report[1] <= 15) {
			update_signature_raw(&(report[2]), report[1]*HID_SIG_XFER_LENGTH, HID_SIG_XFER_LENGTH);
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
			} else {
			hid_error_reply = Hid_Invalid_Arg;
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_ERROR);
		}
	}
	
	if(report[0] == HID_CODE_FOR_SAVE_SIG) {
		// Saves the current signature buffer into flash
		if(send_code_to_i2c_task_fromISR(I2C_CTRL_SAVE_SIG)) {
			report_task_code = HID_REPORT_TASK_CODE_FOR_SUCCESS;
		} else {
			report_task_code = HID_REPORT_TASK_CODE_FOR_ERROR;
			hid_error_reply = Hid_Busy_Error;
		}
		send_code_to_hid_task_fromISR(report_task_code);
	}
	
	if(report[0] == HID_CODE_FOR_RGB_LED) {
		newcolor.r = report[1];
		newcolor.g = report[2];
		newcolor.b = report[3];
		if(send_code_to_i2c_task_fromISR(I2C_CTRL_RGB_COLOR)) {
			report_task_code = HID_REPORT_TASK_CODE_FOR_SUCCESS;
		} else {
			report_task_code = HID_REPORT_TASK_CODE_FOR_ERROR;
			hid_error_reply = Hid_Busy_Error;
		}
		send_code_to_hid_task_fromISR(report_task_code);
	}
	if(report[0] == HID_CODE_FOR_PROX_SETTINGS) {
		// Grab the new settings from HID message, populate our struct
		// Queue up the new settings for loading
		Prox_Settings* newproxsettings = prox_get_settings();
		newproxsettings->pwen = report[1];
		newproxsettings->prate = report[2];
		newproxsettings->pwlong = report[3];
		newproxsettings->pgain = report[4];
		newproxsettings->ppulse = report[5];
		newproxsettings->plen = report[6];
		newproxsettings->pldrive = report[7];
		newproxsettings->pwtime = report[8];
		newproxsettings->pdselect = report[9];
		newproxsettings->pmavg = report[10];
		newproxsettings->proxavg = report[11];
		
		// Trigger the load
		if(send_code_to_i2c_task_fromISR(I2C_CTRL_PROX_SETTINGS)) {
			report_task_code = HID_REPORT_TASK_CODE_FOR_SUCCESS;
		} else {
			report_task_code = HID_REPORT_TASK_CODE_FOR_ERROR;
			hid_error_reply = Hid_Busy_Error;
		}
		send_code_to_hid_task_fromISR(report_task_code);
	}
	
	if(report[0] == HID_CODE_FOR_VXR_CHECKSUM) {
		// Calculates the checksum of installed firmware on the VXR7200 chip.
		// requires a length and starting address
		// Both are 4 byte values, most significant byte last
		// for example, 0x01020304 is sent as 0x04, 0x03, 0x02, 0x01
		vxr_start_addr = ((uint32_t)report[1]) + (((uint32_t)report[2])<<8) + (((uint32_t)report[3])<<16) + (((uint32_t)report[4])<<24);
		vxr_len = ((uint32_t)report[5]) + (((uint32_t)report[6])<<8) + (((uint32_t)report[7])<<16) + (((uint32_t)report[8])<<24);
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_VXR_CHECKSUM);
	}
	if(report[0] == HID_CODE_FOR_VXR_TAGS) {
		// Checks the config and firmware banks on the VXR7200 to see which
		// is currently being used. This should be used for firmware updates.
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_VXR_TAGS);
	}
	if(report[0] == HID_CODE_FOR_VXR_DELETE) {
		// Deletes a block of Flash memory on the VXR7200.
		// Two size deletes are available: 64kB or 4kB
		// There are 8x 64kB blocks - so only values 0->7 are allowed
		// There are 128x 4kB sectors - uses a special code for the "block"
		// of 0x80 and the sector number is given in the next byte
		// report[0] = code word for delete ('D')
		// report[1] = block to delete (0->7) or 0x80 if deleting a sector
		// report[2] = sector to delete (0->127) or ignore if deleting a block
		// report[3:11] = "VXRDELETE" as a safety measure to prevent accidental deletes
		if(memcmp(&(report[3]),"VXRDELETE",9) == 0) {
			vxr_block_to_delete = report[1];
			vxr_sector_to_delete = report[2];
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_VXR_DELETE);
		} else {
			hid_error_reply = Hid_Invalid_Arg;
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_ERROR);
		}
	}
	if(report[0] == HID_CODE_FOR_VXR_PROGRAM) {
		// programs 1-32 bytes in the VXR7200 Flash memory
		// report[0] = code word for program ('A')
		// report[1] = length in bytes (1 to 32)
		// report[2:5] = starting address where to program
		// report[6:6+length-1] = bytes to program
		if((report[1] == 0) || (report[1] > 32)) {
			hid_error_reply = Hid_Invalid_Arg;
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_ERROR);
			} else {
			vxr_len = (uint32_t)report[1];
			vxr_start_addr = ((uint32_t)report[2]) + (((uint32_t)report[3])<<8) + (((uint32_t)report[4])<<16) + (((uint32_t)report[5])<<24);
			memcpy(vxr_program_buf, &(report[6]), vxr_len);
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_VXR_PROGRAM);
		}
	}
	if(report[0] == HID_CODE_FOR_VXR_RESET) {
		xHigherPriorityTaskWoken = pdFALSE;
		xTaskNotifyFromISR(video_proc_task_handle(), VIDEOPROC_NOTIFY_VXR_RESET, eSetBits, &xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
	
	if(report[0] == HID_CODE_FOR_VXR_FWNAME) {
		// Responds with the firmware version name currently loaded on the VXR7200
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_VXR_FWNAME);
	}
	
	if(report[0] == HID_CODE_FOR_HW_TEST) {
		// The test type is the 2nd byte.
		// Send the hid task this message with its 32 bit notification value
		// Lowest 8 bits are the notification type (Hardware test), next 8 bits
		// are the test to run.
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_HW_TEST + (((uint32_t)report[1]) << 8));
	}
	if(report[0] == HID_CODE_FOR_FATP_MODE) {
		// Enables the EDID for FATP test mode. There are three modes, set by the second byte.
		// Byte[1] = 0: Mode 0
		// --- Two non-DSC video modes, 3840x1920 @60Hz and 5120x2560 @30Hz
		// Byte[1] = 1: Mode 1
		// --- One non-DSC video mode, 3840x1920 @60Hz
		// Byte[1] = 2: Mode 2
		// --- One DSC video mode, 5088x2544 @60Hz
		// Byte[1] = 0xFF: back to normal mode.
		// Alternatively to leave FATP mode you can send the HID_CODE_FOR_EDID_SWITCH command
		// Any other value than the above for Byte[1] will also leave FATP and go back to normal mode.
		video_proc_set_fatp_edid(report[1]);
		xTaskNotifyFromISR(video_proc_task_handle(), VIDEOPROC_NOTIFY_VXR_RESET, eSetBits, &xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	}
	
	if(report[0] == HID_CODE_FOR_OLED_COMMAND) {
		// Sends a command directly to the OLEDs.
		// Command format:
		// Byte 0: HID_CODE_FOR_OLED_COMMAND (ascii 'o')
		// Byte 1: Left display (0) or Right display (1)
		// Byte 2: Length in bytes to send, including the register address
		//			So a simple display_on command would still be one byte
		// Bytes 3->(n+3): Command data
		
		set_oled_command_data(report[2], report[1], &(report[3]));
		if(send_code_to_i2c_task_fromISR(I2C_CTRL_OLED_CMD)) {
			report_task_code = HID_REPORT_TASK_CODE_FOR_SUCCESS;
		} else {
			report_task_code = HID_REPORT_TASK_CODE_FOR_ERROR;
			hid_error_reply = Hid_Busy_Error;
		}
		send_code_to_hid_task_fromISR(report_task_code);
	}
	
	if(report[0] == HID_CODE_FOR_EDID_SWITCH) {
		// Byte 0: HID_CODE_FOR_EDID_SWITCH (ascii '
		// Byte 1: Select between 3 possible EDIDs
		//			0 - default, both 75Hz and 90Hz 
		//			1 - only 90Hz
		//			2 - only 75Hz
		if(pdPASS == video_proc_select_edid(report[1])) {
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
			xTaskNotifyFromISR(video_proc_task_handle(), VIDEOPROC_NOTIFY_VXR_RESET, eSetBits, &xHigherPriorityTaskWoken);
			portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
		} else {
			send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_ERROR);
		}
	}
	
	if(report[0] == HID_CODE_FOR_PROX_DISABLE) {
		// Simply disables proximity (always believes it's set on)
		proximity_disable();
		// The task notification allows for the displays to be turned on if they are currently off due to proximity.
		// If video is on, but displays are off, disabling the prox sensor will never turn the displays on again
		// until video is off and back on again. So we do this notification to take care of that case.
		xTaskNotifyFromISR(video_proc_task_handle(), VIDEOPROC_NOTIFY_DEBUG_PROX_DISABLED, eSetBits, &xHigherPriorityTaskWoken);
		portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
	}
	
	if(report[0] == HID_CODE_FOR_PROX_ENABLE) {
		proximity_enable();
		// Don't need the special case like above in the prox-disable. If the proximity is re-enabled but the proximity
		// sensor isn't triggered (e.g. off your face) then the video processing loop will just take care of turning off
		// the displays normally.
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_SUCCESS);
	}
	
	if(report[0] == HID_CODE_FOR_COLORBARPATTERN) {
		// disconnects the VXR from displayport, tells the OLED
		// panels to show a colorbar pattern instead of video content
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_COLORBARPATTERN);
	}
	
	if(report[0] == HID_CODE_FOR_STACK_LEVELS) {
		// Send the current FreeRTOS task "high watermark" (remaining
		// stack words) values to the connected PC
		send_code_to_hid_task_fromISR(HID_REPORT_TASK_CODE_FOR_STACK_LEVELS);
	}
	
	if(report[0] == HID_CODE_FOR_HARDFAULT){
		// runs an illegal instruction, which will immediately hard fault.
		// Bytes 1 through 9 should spell out "CRASHNOW", as a little
		// safety measure against accidentally triggering a crash
		if(memcmp(&(report[1]),"CRASHNOW",8) == 0) {
			__builtin_trap();
		}
	}
	
	if(report[0] == HID_CODE_FOR_UNUSED_IRQ){
		// Trigger another IRQ that's not set. It will go to Dummy_Handler
		// and call our crash handler
		if(memcmp(&(report[1]),"CRASHNOW",8) == 0) {
			NVIC_EnableIRQ(MEM2MEM_IRQn);
			NVIC_SetPriority(MEM2MEM_IRQn, 4); // doesn't really matter.
			NVIC_SetPendingIRQ(MEM2MEM_IRQn);
		}
	}
	
	if(report[0] == HID_CODE_FOR_STACK_OVERFLOW) {
		// Stack overflow code performed in another task, not
		// right here as we're in the USB ISR
		if(memcmp(&(report[1]),"CRASHNOW",8) == 0) {
			// Let's do it in the video_proc task 
			// as it's already pretty low on stack
			xTaskNotifyFromISR(video_proc_task_handle(), VIDEOPROC_STACK_OVERFLOW, eSetBits, &xHigherPriorityTaskWoken);
			portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
			
		}
	}
}
