/**
 * test_and_debug.c
 * 
 * Copyright (c) 2022 Bigscreen, Inc.
 */

#include <string.h>
#include <asf.h>
#include <stdbool.h>
#include "test_and_debug.h"
#include "rgb_led.h"
#include "usbc_mux.h"
#include "power_control.h"
#include "prox_control.h"
#include "oled_control.h"
#include "vxr_interface.h"
#include "usb3803.h"
#include "usb2517.h"
#include "linkbox_hpd.h"
#include "eyetrack_fpga.h"

// I2C stuff
I2C_Control_T i2c_control_cmd;
RGB_Color_t newcolor;
uint16_t newbrightness;
uint8_t i2c_buffer[32];
I2C_Transaction_Type i2c_txn_type;

volatile uint8_t i2c_addr;
volatile uint8_t i2c_reg_addr;
volatile uint8_t i2c_len;

TaskHandle_t xI2CTaskHandle = NULL;
TaskHandle_t xTimedStartupTaskHandle = NULL;
TaskHandle_t xLedTaskHandle = NULL;
TaskHandle_t xFanTaskHandle = NULL;

I2C_Transaction_Request hi2c_txn_testing;

uint8_t oled_temp_left = OLED_ZERO_TEMP; // Approximately 0degC (71*.714 - 50.71429)
uint8_t oled_temp_right = OLED_ZERO_TEMP;

// Dynamic serial name for USB descriptor

uint8_t* usb_serial_name_ptr;
uint8_t usb_serial_name[USB_DEVICE_GET_SERIAL_NAME_LENGTH];

struct power_timer {
	uint32_t _10sec;
	uint32_t _10min;
};

struct power_timer time_power_on = {._10sec=0, ._10min=0};
struct power_timer time_display_on = {._10sec=0, ._10min=0};
struct power_timer time_longest_display_on = {._10sec=0, ._10min=0};
uint32_t time_display_off_10sec = 0;

struct I2C_Control_OLED_Command {
	uint8_t datalen;
	OLED_Panel_T left_or_right;
	uint8_t data[64];
};

struct I2C_Control_OLED_Command i2c_control_oled_cmd;

uint8_t expo_easing_in_schedule[100] = {
	0, 0, 0, 1, 1, 2, 2, 2, 2, 2,
	3, 3, 3, 3, 3, 3, 3, 4, 4, 4,
	4, 4, 5, 5, 5, 5, 5, 6, 6, 6,
	7, 7, 7, 8, 8, 8, 9, 9, 9, 10,
	10, 11, 11, 12, 12, 13, 13, 14, 15, 15,
	16, 17, 17, 18, 19, 20, 21, 21, 22, 23,
	24, 25, 27, 28, 29, 30, 32, 33, 34, 36,
	37, 39, 41, 42, 44, 46, 48, 50, 52, 55,
	57, 60, 62, 65, 68, 71, 74, 77, 80, 84,
	87, 91, 95, 99, 103, 108, 112, 117, 122, 128,
};

uint8_t cubic_easing_out_schedule[100] = {
	128, 124, 121, 118, 115, 112, 109, 106, 103, 100,
	97, 95, 92, 89, 87, 84, 82, 79, 77, 75,
	72, 70, 68, 66, 64, 62, 59, 58, 56, 54,
	52, 50, 48, 47, 45, 43, 42, 40, 39, 37,
	36, 34, 33, 32, 30, 29, 28, 27, 26, 24,
	23, 22, 21, 20, 19, 18, 17, 17, 16, 15,
	14, 13, 13, 12, 11, 11, 10, 9, 9, 8,
	8, 7, 7, 6, 6, 5, 5, 4, 4, 4,
	3, 3, 3, 3, 2, 2, 2, 2, 1, 1,
	1, 1, 1, 1, 0, 0, 0, 0, 0, 0,
};

#ifdef USE_MONITOR_TASK
/**
 * \brief This task, when activated, send every ten seconds on debug UART
 * the whole report of free heap and total tasks status
 */
static void task_monitor(void *pvParameters)
{
	static portCHAR szList[256];
	UNUSED(pvParameters);

	for (;;) {
		printf("--- task ## %u", (unsigned int)uxTaskGetNumberOfTasks());
		vTaskList((signed portCHAR *)szList);
		printf(szList);
		printf("\nBuffer difference %u\n",main_audio_buffer_difference);
		vTaskDelay(1000);
	}
}
#endif

/***********************************************************************
 * \brief This task runs the ADC routine to collect thermistor
 *		  and USB CC pin voltages.
 */
static void task_adc(void* pvParameters) {
	UNUSED(pvParameters);
	for (;;) {
		// Trigger an ADC reading
		trig_adc();
		vTaskDelay(100);
		// Read the adc results
		// Calculate the thermistor temperature
		temp_sense_calc_temperature();
		
		// Inform the fan that we have a new temperature
		xTaskNotify(get_fan_task_handle(), FAN_TASK_CODE_TEMPERATURE, eSetValueWithoutOverwrite);
	}
}

/***********************************************************************
 * \brief	Controls the fan and speed ramps 
 * 
 * \note	There are two HID commands to control fan speed:
 *			'F' - set fan speed immediately. Changes the speed right away
 *			'f' - set fan speed deferred. Changes the setpoint 
 *				  that will be used later
 *
 * \note	Implements a state machine. There are 3 states:
 *			- Idle: Fan remains off unless a temperature threshold is exceeded.
 *					If it does, fan turns on and doesn't shut off until temp
 *					drops below a threshold (with hysteresis).
 *					In Idle, setting fan speed deferred with 'f' will only
 *					change the speed that the fan will spin up to when
 *					video is enabled.
 *			- Video: Fan is turned on to the preset speed (from config memory)
 *					 or the deferred speed sent by HID. Setting either 'F' or
 *					 'f' HID commands will change the fan speed immediately.
 *					 Leaving video mode will change to Idle.
 *			- Debug: Fan speed is set directly by HID command 'F'.
 *					 Debug mode is exited when a 'f' HID command is
 *					 sent or video turns on or fan speed commanded to
 *					 zero with 'F' HID command.
 *
 * \note	Fan inputs can arrive from the following places:
 * 			- usbhid_interface: sends the 'F' (fan speed immediate) and
 * 				'f' (fan speed deferred) commands
 * 			- video_proc: sends video on and video off notifications
 * 			- task_adc: updates the current temperature
 */

enum fan_state{
	FS_Idle,
	FS_Video,
	FS_Debug
};

enum fan_overtemp_state{
	FOS_None   = 0, // under IDLE_FAN_THRESH1
	FOS_Level1 = 1, // between IDLE_FAN_THRESH1 and IDLE_FAN_THRESH2
	FOS_Level2 = 2, // between IDLE_FAN_THRESH2 and IDLE_FAN_THRESH3
	FOS_Level3 = 3, // over IDLE_FAN_THRESH3
	Num_FOS_Levels = 4
};

uint8_t Fan_Overtemp_Speeds[Num_FOS_Levels] = {
	IDLE_FAN_OFF,
	IDLE_FAN_SPEED_LEVEL1,
	IDLE_FAN_SPEED_LEVEL2,
	IDLE_FAN_SPEED_LEVEL3
};

static void task_fan(void* pvParameters) {
	UNUSED(pvParameters);
	uint32_t notify_val = 0;
	uint32_t notify_cmd = 0;
	uint32_t notify_arg = 0;
	enum fan_state fs = FS_Idle;
	enum fan_overtemp_state fos = FOS_None;
	bool video_state = false;
	uint8_t startup_fan_speed = DEFAULT_FAN_SPEED;
	uint8_t video_on_fan_speed = DEFAULT_FAN_SPEED;
	uint8_t debug_fan_speed = 0;
	uint8_t current_fan_speed = 0;
	float new_temp = 0.0f;
	
	// xFanTaskHandle = xTaskGetCurrentTaskHandle();

	// grab the NVM config settings at startup
	sigv2_find_tag(SigTag_Fan_Speed, &startup_fan_speed);
	video_on_fan_speed = startup_fan_speed;
	
	for(;;) {
		if(notify_val == 0) {
			// Only recheck the notification if currently is zero.
			// If non-zero, that means we received a notification
			// in the middle of a fan speed ramp up or down (later in this for loop).
			// So we should not retry taking the notification, as we
			// already have something to do!
			notify_val = ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
		}
		notify_cmd = notify_val & 0xFF00u;
		notify_arg = notify_val & 0x00FFu;
		
		if((notify_cmd) == FAN_TASK_CODE_VIDEO_STATE) {
			if((notify_arg) == 0) {
				video_state = false;
			}
			else {
				video_state = true;
			}
		}

		if((notify_cmd) == FAN_TASK_CODE_SPEED_DEFER) {
			if(notify_arg > 100) { 
				notify_arg = 100;
			}
			video_on_fan_speed = notify_arg;
		}

		if((notify_cmd) == FAN_TASK_CODE_SPEED_IMM) {
			if(notify_arg > 100) {
				notify_arg = 100;
			}
			if(notify_arg != 0) {
				video_on_fan_speed = notify_arg;
			}
			debug_fan_speed = notify_arg;
		}

		if(notify_cmd == FAN_TASK_CODE_TEMPERATURE) {
			// Temperature has been updated, let's check it
			new_temp = temp_sense_get_temperature();
			// Multi-level fan thresholds with hysteresis between levels.
			// Stepping up is done whenever the threshold is exceeded.
			// Stepping down only when the temperature reduces under (threshold - hysteresis).

			// If above threshold 3, always in level 3
			if(new_temp >= IDLE_FAN_THRESH3) { 
				fos = FOS_Level3; 
			}
			// Between threshold 3 and the hysteresis below it: 
			// - keep in level 3 if already in 3
			// - otherwise, in level 2
			else if((new_temp >= (IDLE_FAN_THRESH3 - IDLE_FAN_HYSTERESIS)) && (new_temp < IDLE_FAN_THRESH3)){
				if(fos != FOS_Level3) fos = FOS_Level2;
			}
			// Between thresh2 and the hysteresis below thresh3 - always level 2
			else if((new_temp >= IDLE_FAN_THRESH2) && (new_temp < (IDLE_FAN_THRESH3 - IDLE_FAN_HYSTERESIS))) {
				fos = FOS_Level2;
			}
			// And so on...
			else if((new_temp >= (IDLE_FAN_THRESH2 - IDLE_FAN_HYSTERESIS)) && (new_temp < IDLE_FAN_THRESH2)){
				if(fos != FOS_Level2) fos = FOS_Level1;
			}
			else if((new_temp >= IDLE_FAN_THRESH1) && (new_temp < (IDLE_FAN_THRESH2 - IDLE_FAN_HYSTERESIS))) {
				fos = FOS_Level1;
			}
			else if((new_temp >= (IDLE_FAN_THRESH1 - IDLE_FAN_HYSTERESIS)) && (new_temp < IDLE_FAN_THRESH1)){
				if(fos != FOS_Level1) fos = FOS_None;
			}
			else {
				fos = FOS_None;
			}
		}

		// state change logic
		switch(fs){
			case FS_Idle:
				if( (notify_cmd == FAN_TASK_CODE_SPEED_IMM) && (notify_arg != 0) ) {
					fs = FS_Debug;
				}
				else if(video_state) {
					fs = FS_Video;
				}

			break;
			case FS_Video:
				// Video off? back to idle
				if(!video_state) {
					fs = FS_Idle;
				}
			
			break;
			case FS_Debug:
				if(video_state) {
					fs = FS_Video;
				}
				else if( (notify_cmd == FAN_TASK_CODE_SPEED_IMM) && (notify_arg == 0)) {
					fs = FS_Idle;
				}

			break;
			default:
				// Shouldn't get here. Buf if we do, go back to idle.
				fs = FS_Idle;
				video_on_fan_speed = startup_fan_speed;
				current_fan_speed = 0;
				debug_fan_speed = 0;
				set_fan_pwm(current_fan_speed);
			break;
		}

		// Now figure out what our next fan speed should be
		uint8_t next_fan_speed = 0;
		switch(fs){
			case FS_Idle:
				next_fan_speed = Fan_Overtemp_Speeds[fos];
			break;
			case FS_Video:
				next_fan_speed = video_on_fan_speed;
			break;
			case FS_Debug:
				next_fan_speed = debug_fan_speed;
			break;
		}

		notify_val = 0; // clear it, now we can check if a new notification arrived
		notify_cmd = 0;
		notify_arg = 0;

		// Fan speed ramp up...
		while((notify_val == 0) && (current_fan_speed < next_fan_speed)) {
			set_fan_pwm(++current_fan_speed);
			notify_val = ulTaskNotifyTake(pdTRUE, FAN_UPDATE_DELAY);
		}

		// ...or ramp down.
		while((notify_val == 0) && (current_fan_speed > next_fan_speed)) {
			set_fan_pwm(--current_fan_speed);
			notify_val = ulTaskNotifyTake(pdTRUE, FAN_UPDATE_DELAY);
		}
		/*
		if(notify_val != 0) {
			notify_cmd = notify_val & 0xFF00u;
			notify_arg = notify_val & 0x00FFu;
		}

		if(notify_val == 0) {
			// if it is non-zero, it was probably set during a ramp up or down loop
			// then skip this wait and go straight to the next update
			notify_val = ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
		}
		
		// Check the notification code
		if((notify_val & 0xFF00u) == FAN_TASK_CODE_STOP) {
			set_fan_pwm(0);
			notify_val = 0;
		}
		if((notify_val & 0xFF00u) == FAN_TASK_CODE_NEW_SPEED) {
			
			uint16_t new_speed = notify_val & 0x00FFu;
			notify_val = 0;
			// ramp up
			while((notify_val == 0) && (current_fan_duty < new_speed)) {
				set_fan_pwm(++current_fan_duty);
				notify_val = ulTaskNotifyTake(pdTRUE, FAN_UPDATE_DELAY);
			} 
			// or ramp down
			while((notify_val == 0) && (current_fan_duty > new_speed)) {
				set_fan_pwm(--current_fan_duty);
				notify_val = ulTaskNotifyTake(pdTRUE, FAN_UPDATE_DELAY);
			}
		}
		*/
	}
}
/***********************************************************************
 * \brief	Controls the RBG led to perform dimming or breathing 
 *			animations
 */
static void task_led(void* pvParameters) {
	UNUSED(pvParameters);
	uint32_t notify_val = 0;
	uint32_t led_state = LED_TASK_CODE_STATIC_COLOR;
	uint8_t step = 0;
	uint8_t brightdir = 0; // 0 = increasing, 1 = wait at full, 2 = decreasing, 3 = wait at off
	// and in blinking mode: 0 = off, 1 = on
	
	// xLedTaskHandle = xTaskGetCurrentTaskHandle();
	
	// Wait until RGB LED is initialized
	while((LED_TASK_CODE_LED_INIT & notify_val) == 0) {
		notify_val = ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
		if((notify_val & LED_TASK_CODE_MODE_MASK) == LED_TASK_CODE_BREATHING) {
			led_state = LED_TASK_CODE_BREATHING;
		}
	}
	
	// Check if we have a override startup color from the user signature
	RGB_Color_t startup_color =
	{	.r = 255,
		.g = 0,
		.b = 255
	};
	sigv2_find_tag(SigTag_RGB_Color, (uint8_t*)&startup_color);
		
	if(led_state == LED_TASK_CODE_BREATHING) {
		rgb_led_set_dimming(MCU_I2C(), 0x00);
	}
	rgb_led_set_color(MCU_I2C(), &startup_color);
	
	for(;;) {
		notify_val = ulTaskNotifyTake(pdTRUE, LED_UPDATE_DELAY);
		if((notify_val & LED_TASK_CODE_MODE_MASK) == LED_TASK_CODE_BREATHING) {
			led_state = LED_TASK_CODE_BREATHING;
			step = 0;
			brightdir = 0;
		}
		if((notify_val & LED_TASK_CODE_MODE_MASK) == LED_TASK_CODE_STATIC_COLOR) {
			led_state = LED_TASK_CODE_STATIC_COLOR;
			rgb_led_set_dimming(MCU_I2C(), 0x80);
		}
		if((notify_val & LED_TASK_CODE_MODE_MASK) == LED_TASK_CODE_BLINKING) {
			led_state = LED_TASK_CODE_BLINKING;
			rgb_led_set_dimming(MCU_I2C(), 0x80);
			step = 0;
			brightdir = 1;
		}
		
		if(LED_TASK_CODE_BREATHING == led_state) {
			if(brightdir == 0) {
				rgb_led_set_dimming(MCU_I2C(), expo_easing_in_schedule[step++]);
				if(step >= 100) {
					step = 0;
					brightdir = 2;
				}
			}
			/*
			else if(brightdir == 1) {
				step++;
				if(step >= 25) {
					step=0;
					brightdir = 2;
				}
			}
			*/
			else if(brightdir == 2) {
				rgb_led_set_dimming(MCU_I2C(), cubic_easing_out_schedule[step++]);
				if(step >= 100) {
					step = 0;
					brightdir = 3;
				}
			} 
			else if(brightdir == 3) {
				step++;
				if(step >= 25) {
					step = 0;
					brightdir = 0;
				}
			}			
		}
		
		if(LED_TASK_CODE_BLINKING == led_state) {
			step++;
			if (step >= 5) {
				step = 0;
				if(brightdir == 0) {
					brightdir = 1;
					rgb_led_set_dimming(MCU_I2C(), 0x80);
				} else {
					brightdir = 0;
					rgb_led_set_dimming(MCU_I2C(), 0x00);
				}
			}
		}
	}
}

/***********************************************************************
 * \brief	This task performs startup routines that require timing, 
 *			including longish delays or sequenced starts.
 *			After startup this task is responsible for reading the 
 *			proximity sensor and OLED temperatures.
 */
static void task_timed_startup(void *pvParameters){
	UNUSED(pvParameters);

	bool fpga_led_current_error = false;
	
	// xTimedStartupTaskHandle = xTaskGetCurrentTaskHandle();
	
	// Initialize the emulated EEPROM
	EEPROM_Init();
	
	// Upgrade user signature region from v1 if we have that kind right now
	sigv2_upgrade_from_sigv1();
	
	// Correction on USB CC wire if we have an old Linkbox v1 connected.
	// Check user signature region for valid signature block, and if so,
	// if the Linkboxv1 flag is true we need to invert the CC logic.
	linkbox_hpd_set_active_level(true); // Default for Linkbox v2 and later
	
	bool linkbox_is_v1 = false;
	if(sigv2_find_tag(SigTag_Linkbox_v1, (uint8_t*)&linkbox_is_v1)) {
		if(linkbox_is_v1) {
			linkbox_hpd_set_active_level(false);
		}
	}
	// Now make sure to apply inactive HPD
	linkbox_hpd_set_inactive();

	
	// Start timers for fan control
	init_fan();
	uint16_t fpga_sw_ver = 0u; // checking against zero can be used to tell if improperly started
	// after startup, a real version number should be saved here

	// Set the serial number in the USB descriptor
	usb_serial_name_ptr = usb_serial_name;
	uint32_t serial_length = sigv2_tag_length(SigTag_Serial);
	if(serial_length > 0) {
		sigv2_find_tag(SigTag_Serial, usb_serial_name_ptr);
		// pad remainder of serial number with spaces
		for(uint8_t i = serial_length; i < USB_DEVICE_GET_SERIAL_NAME_LENGTH; i++) {
			usb_serial_name_ptr[i] = (uint8_t)0;
		}
	} else {
		memcpy(usb_serial_name, "1234            ", USB_DEVICE_GET_SERIAL_NAME_LENGTH);
	}

	// Initialize the hub chips
	// BS1 has 2x USB3803 chips (addresses 0x10 and 0x12)
	// BS2 EVT has 1x USB2517 chip (address 0x58)
	// BS2 has 3x USB3803 chips (addresses 0x10, 0x12, and 0x52)
	// Use hub chip responses on I2C to determine board version
	board_ver_t board_ver = board_ver_unknown;
	board_ver = usb3803_init(MCU_I2C());
	if(board_ver_unknown == board_ver) {
		// no USB3803's on board. Try initializing USB2517
		if(pdPASS == usb2517_init(MCU_I2C())) {
			board_ver = board_ver_BS2_evt;
		}
	}
	set_mainboardver(board_ver);

	// AUX USB (AUDIO) PORT
	ioport_set_pin_level(PIN_AUX_USB_EN, true);
	
	// Determine flip status to determine how to configure the USB-C crosspoint switch
	// Determine flip state to set the lane swap
	// If flip state currently undetermined, just wait a while.
	// The ADC task will eventually settle on the current flip state
	USB_Flip_State_t flip = get_flip_status();
	uint8_t repeat_counts = 0;
	while(flip == Flip_Undefined && repeat_counts < 20) {
		// Delay a bit, waiting for the flip status to be read
		vTaskDelay(5);
		flip = get_flip_status();
		repeat_counts++;
	}
	usbc_mux_flip(DDIC_I2C(), flip); // Will only enable the Mux if flip is Flip_CC1 or Flip_CC2
	
	// Power control - need to enable the 1.0V rail (for the VXR7200)
	VXR_Power_Control_On();
	
	// Tell the video task thread that the VXR chip is ready to access
	xTaskNotify(video_proc_task_handle(), VIDEOPROC_NOTIFY_1V0_READY, eSetBits);

	// Enable the 1.8V, OLED+, and OLED- power rails (for OLED panels and prox sensor)
	Panel_Power_Control(true);

	// Let the video processor know that all power rails are up now
	xTaskNotify(video_proc_task_handle(), VIDEOPROC_NOTIFY_1V8_AND_OLED_PWR_READY, eSetBits);

	// Initialize proximity sensor
	prox_init();

	// Init RGB LED
	rgb_led_init(MCU_I2C());

	// Tell the LED task that we're ready
	xTaskNotify(xLedTaskHandle, LED_TASK_CODE_LED_INIT | LED_TASK_CODE_BREATHING, eSetBits);
	
	// put FPGA IOs in a safe state
	if((board_ver == board_ver_BS2) || (board_ver == board_ver_BS2_evt ))
	{
		fpga_init();
		
		// Try to see if FPGA is in bootloader or application mode
		// if it's in bootloader, trigger RECONFIG_N to swap to application
		uint8_t addrbyte[4]; // needs to be a pointer with data length of 3 or more
		bool fpga_start_complete = false;
		bool did_reconfig = false;
		uint32_t startup_count = 0;
		while(!fpga_start_complete) {
			addrbyte[0] = FPGA_REG_CONFIG_ID; // this and next two bytes are the full config ID
		
			set_fpga_command_data(FPGA_ID_LEN, addrbyte);
		
			if(pdPASS == fpga_read_i2c())
			{
				uint8_t* read_data = get_fpga_read_data();
				if(read_data[0] == 'A') {
					fpga_sw_ver = (((uint16_t)read_data[1])<<8) + ((uint16_t)read_data[2]);
					fpga_start_complete = true;
				}
				else if(read_data[0] == 'B') {
					if(!did_reconfig) {
						fpga_reconfig();
						vTaskDelay(3000);
						did_reconfig = true;
					}
				}
				else {
					// not recognized, possibly still starting up
					// might as well delay a little and try again
					vTaskDelay(100);
				}
			}
			else {
				vTaskDelay(100);
			}
			
			startup_count++;
			if(startup_count >= 50) {
				fpga_start_complete = true;
			}
		}
		
	}
	
	/**	Startup is complete here. Now enter an infinite loop, which takes care
		of the proximity sensor and OLED temperature reading.
	**/

	uint32_t prox_oled_temp_delay_counter = 0;
	uint32_t power_on_timer_counter = 0;
	
	while(true) {
	/**	Check runtime counters and increment every 10 minutes.
		Total run time will always be incremented after every 10
		minute block. Granularity of the timer is 10 seconds.
		
		Displays on time will only be incremented if displays were on.
		Counter is not reset when displays turn off, so partial on-times
		will count. For example, display on 5 minutes, off 5 minutes, then
		on 5 minutes WILL count as a 10 minute increment.
		
		Longest display on time will only increase if it is greater than the
		value currently in EEPROM. The counter is reset when displays turn
		off and stay off longer than 1 minute.
	**/
		
	power_on_timer_counter++;
	if(power_on_timer_counter >= (POWER_TIMER_GRANULARITY / PROX_DISTANCE_DELAY)){
		power_on_timer_counter = 0;
		
		time_power_on._10sec++;
		if(time_power_on._10sec >= (TEN_MIN_IN_TEN_SEC_INTERVALS)) {
			time_power_on._10sec = 0;
			if(EEPROM_Found == EEPROM_Read(EEKEY_TotalPoweredOnTime10min, &(time_power_on._10min))) {
				time_power_on._10min++;
				EEPROM_Write(EEKEY_TotalPoweredOnTime10min, time_power_on._10min);
			} else {
				EEPROM_Write(EEKEY_TotalPoweredOnTime10min, 1);
			}
			
		}
		
		if(displays_are_on()) {
			time_display_off_10sec = 0;
			time_display_on._10sec++;
			if(time_display_on._10sec >= (TEN_MIN_IN_TEN_SEC_INTERVALS)) {
				time_display_on._10sec = 0;
				if(EEPROM_Found == EEPROM_Read(EEKEY_TotalDisplaysOnTime10min_Left, &(time_display_on._10min))) {
					time_display_on._10min++;
					EEPROM_Write(EEKEY_TotalDisplaysOnTime10min_Left, time_display_on._10min);
				} else {
					EEPROM_Write(EEKEY_TotalDisplaysOnTime10min_Left, 1);
				}
				if(EEPROM_Found == EEPROM_Read(EEKEY_TotalDisplaysOnTime10min_Right, &(time_display_on._10min))) {
					time_display_on._10min++;
					EEPROM_Write(EEKEY_TotalDisplaysOnTime10min_Right, time_display_on._10min);
					} else {
					EEPROM_Write(EEKEY_TotalDisplaysOnTime10min_Right, 1);
				}
			}
			
			time_longest_display_on._10sec++;
			if(time_longest_display_on._10sec >= (TEN_MIN_IN_TEN_SEC_INTERVALS)) {
				time_longest_display_on._10sec = 0;
				time_longest_display_on._10min++;
				uint32_t saved_longest_time;
				if(EEPROM_Found == EEPROM_Read(EEKEY_LongestDisplaysOnTime10min, &(saved_longest_time))) {
					if(time_longest_display_on._10min > saved_longest_time) {
						EEPROM_Write(EEKEY_LongestDisplaysOnTime10min, time_longest_display_on._10min);
					}
				} else {
					EEPROM_Write(EEKEY_LongestDisplaysOnTime10min, time_longest_display_on._10min);
				}
			}
		} else {
			// Reset the longest on timer if it's been off for more than 1 minute
			time_display_off_10sec++;
			if(time_display_off_10sec >= (ONE_MIN_IN_TEN_SEC_INTERVALS)) {
				time_longest_display_on._10sec = 0;
				time_longest_display_on._10min = 0;
				time_display_off_10sec = 0;
			}
		}
	}
		
	/** Perform proximity detection and OLED temperature sensing
		Uses a delay counter to determine if the slower period activities
		(OLED temp and enabling the proximity sensor) should get triggered.
	**/
		// OLED temp checking delay is longer than the prox checking delay
		// Can use a modulus to determine if we should perform a temperature check
		if(prox_oled_temp_delay_counter % (OLED_TEMP_DELAY / PROX_DISTANCE_DELAY) == 0) {		
			if(video_is_enabled()) {
				// oled temperature checking can only be performed if the 
				// initialization routine is done, which only happens after 
				// video is enabled
				oled_temp_left = OLED_temperature(OLED_LEFT);
				oled_temp_right = OLED_temperature(OLED_RIGHT);
			} else {
				oled_temp_left = OLED_ZERO_TEMP;
				oled_temp_right = OLED_ZERO_TEMP;
			}
		}
			
		if(prox_is_connected()) {
			prox_update();
		} else {
			// Check if prox sensor is now detected
			// Delay is longer than usual distance delay. 
			if(prox_oled_temp_delay_counter % (PROX_CONNECT_CHECK_DELAY / PROX_DISTANCE_DELAY) == 0) {

				prox_init();
			}
		}
		if(prox_oled_temp_delay_counter % (PROX_CONNECT_CHECK_DELAY / PROX_DISTANCE_DELAY) == 0) {
			if((board_ver == board_ver_BS2) || (board_ver == board_ver_BS2_evt )) {
				// also ensure we have LED current measurement on board
				if(fpga_sw_ver >= 0x0049) {
					// periodically check the IR LED overcurrent error latches
					uint8_t current_addrbyte[4]; // needs to be a pointer with length 4 or more
					current_addrbyte[0] = FPGA_REG_IR_LEFT_RUN_MEAS_HI; // first reg of the set of 9
					set_fpga_command_data(FPGA_CURRENT_MEAS_LENGTH, current_addrbyte);
					if(pdPASS == fpga_read_i2c())
					{
						uint8_t* led_current_regs = get_fpga_read_data();
						// check high 4 bits for any of them set
						if((led_current_regs[8] & 0xF0) != 0) {
							// Start the error notification via flashing the displays
							if(false == fpga_led_current_error) {
								xTaskNotify(video_proc_task_handle(), VIDEOPROC_EYETRACKING_ERROR, eSetBits);
								fpga_led_current_error = true;
							}
						}
						if((led_current_regs[8] & 0xF0) == 0) {
							if(fpga_led_current_error) {
						// we can release if the latched error is now cleared
						// note: this won't happen unless FPGA is reset.
								xTaskNotify(video_proc_task_handle(), VIDEOPROC_EYETRACKING_ERROR_CLEAR, eSetBits);
								fpga_led_current_error = false;
							}
						}
					}
				}
			}
		}
		
		
		vTaskDelay(PROX_DISTANCE_DELAY);
		prox_oled_temp_delay_counter++;
		// The longest delay is PROX_CONNECT_CHECK_DELAY
		// We can just reset the counter when that delay value has been hit, the modulus
		// operation will still trigger the function when the counter is zero.
		if(prox_oled_temp_delay_counter >= (PROX_CONNECT_CHECK_DELAY / PROX_DISTANCE_DELAY)) {
			prox_oled_temp_delay_counter = 0;
		}
	}	
}

void set_oled_command_data(uint8_t datalen, OLED_Panel_T left_or_right, uint8_t* data) {
	i2c_control_oled_cmd.datalen = datalen;
	i2c_control_oled_cmd.left_or_right = left_or_right;
	memcpy(i2c_control_oled_cmd.data, data, datalen);
}

/***********************************************************************
 * \brief This task manages the I2C communication triggered by an interrupt handler
 */
static void task_i2c(void *pvParameters) {
	UNUSED(pvParameters);
		
	// xI2CTaskHandle = xTaskGetCurrentTaskHandle();
	
	while(pdTRUE) {
		// Wait for notification
		ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
		
		switch(i2c_control_cmd) {
			case I2C_CTRL_OLED_UNFLIP:
				OLED_flip(OLED_LEFT, false);
				OLED_flip(OLED_RIGHT, false);
			break;
			case I2C_CTRL_OLED_FLIP:
				OLED_flip(OLED_LEFT, true);
				OLED_flip(OLED_RIGHT, true);
			break;
			case I2C_CTRL_OLED_BRIGHTNESS:
				video_set_brightness(newbrightness);
				// OLED_set_brightness(OLED_LEFT, newbrightness);
				// OLED_set_brightness(OLED_RIGHT, newbrightness);
			break;
			case I2C_CTRL_OLED_DISPOFF:
				OLED_display_on_off(OLED_LEFT, false);
				OLED_display_on_off(OLED_RIGHT, false);
			break;
			case I2C_CTRL_OLED_DISPON:
				OLED_display_on_off(OLED_LEFT, true);
				OLED_display_on_off(OLED_RIGHT, true);
				vTaskDelay(OLED_POST_INIT_DELAY);
				OLED_dimming_post_init(OLED_LEFT);
				OLED_dimming_post_init(OLED_RIGHT);
			break;
			case I2C_CTRL_RGB_COLOR:
				rgb_led_set_color(MCU_I2C(), &newcolor);
			break;
			case I2C_CTRL_SAVE_SIG:
				// So this isn't I2C per se, but this is a nice task you got there for doing slower things like this
				save_signature_raw();
			break;
			case I2C_CTRL_PROX_SETTINGS:
				// Update the proximity sensor with the new settings obtained from HID interface
				// The HID interrupt will have already loaded the new settings into the proxsettings struct in prox_control.c
				prox_apply_settings();
			break;
			case I2C_CTRL_OLED_CMD:
			
				// Send a OLED command directly to the display
				// Assume this command structure has already been filled by the task calling function
				OLED_send_cmd(i2c_control_oled_cmd.left_or_right, i2c_control_oled_cmd.data[0], 
					i2c_control_oled_cmd.datalen, &(i2c_control_oled_cmd.data[1]));
			
			break;
			case I2C_CTRL_FPGA_CMD:
				// Send an I2C command to the FPGA
				if((get_mainboardver() == board_ver_BS2) || (get_mainboardver() == board_ver_BS2_evt))
					fpga_send_i2c();
			break;
			case I2C_CTRL_NONE:
			default:
			
			// do nothing
			break;
		}
		
		// After processing, set the command back to none
		i2c_control_cmd = I2C_CTRL_NONE;
		
	}
}

void start_test_tasks(void) {
	#ifdef USE_MONITOR_TASK
	/* Create task to monitor processor activity */
	if (xTaskCreate(task_monitor, "Monitor", TASK_MONITOR_STACK_SIZE, NULL,
	TASK_MONITOR_STACK_PRIORITY, NULL) != pdPASS) {
		printf("Failed to create Monitor task\r\n");
	}
	#endif

	if(xTaskCreate(task_i2c, "I2C", TASK_I2C_STACK_SIZE, NULL,
	TASK_I2C_PRIORITY, &xI2CTaskHandle) != pdPASS) {
		printf("Failed to create I2C task\r\n");
	}
	
	if(xTaskCreate(task_timed_startup, "TIMED_START", TASK_TIMED_STARTUP_STACK_SIZE, NULL,
		TASK_TIMED_STARTUP_PRIORITY, &xTimedStartupTaskHandle) != pdPASS) {
		printf("Failed to create timed startup task\r\n");
	}
	if(xTaskCreate(task_adc, "ADC", TASK_ADC_STACK_SIZE, NULL,
		TASK_ADC_PRIORITY, NULL) != pdPASS) {
		printf("Failed to create ADC task\r\n");
	}
	if(xTaskCreate(task_led, "LED", TASK_LED_STACK_SIZE, NULL,
		TASK_LED_PRIORITY, &xLedTaskHandle) != pdPASS) {
		printf("Failed to create LED task\r\n");
	}
	if(xTaskCreate(task_fan, "FAN", TASK_FAN_STACK_SIZE, NULL,
		TASK_FAN_PRIORITY, &xFanTaskHandle) != pdPASS) {
		printf("Failed to create Fan task\r\n");	
	}
}


/** Helper functions for tasks **/

bool send_code_to_i2c_task_fromISR(I2C_Control_T code) {
	i2c_control_cmd = code;
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
	// vTaskNotifyGiveFromISR(xI2CTaskHandle, &xHigherPriorityTaskWoken);
	// Notification value is checked - if there is a notification pending 
	// (the notification value is not zero) then we simply fail out of here
	// I don't want to overwrite a previous notification
	uint32_t prev_notify_val = 0;
	xTaskNotifyAndQueryFromISR(xI2CTaskHandle, 1, eSetValueWithoutOverwrite, &prev_notify_val, &xHigherPriorityTaskWoken);
	if(prev_notify_val != 0) {
		return false;
	}
	portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	return true;
}

TaskHandle_t get_fan_task_handle(void) {
	return xFanTaskHandle;
}

TaskHandle_t get_led_task_handle(void) {
	return xLedTaskHandle;
}