/**
 * 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 "linkbox_hpd.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();
	}
}

/***********************************************************************
 * \brief	Controls the fan and speed ramps 
 */
static void task_fan(void* pvParameters) {
	UNUSED(pvParameters);
	uint32_t notify_val = 0;
	uint16_t current_fan_duty = 0;
	
	// xFanTaskHandle = xTaskGetCurrentTaskHandle();
	
	for(;;) {
		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
	
	// 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(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;
				}
			}			
		}
	}
}

/***********************************************************************
 * \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);
	
	// 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();


	// 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 USB3803 hub chip
	usb3803_init(MCU_I2C());

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