/*
 * hw_test.c
 *
 * Runs internal tests to check board components.
 *
 * Copyright (c) 2022 Bigscreen, Inc.
 */ 

#include <asf.h>
#include "usb3803.h"
#include "usbc_mux.h"
#include "oled_control.h"

uint16_t fan_window_average[TEST_FAN_AVERAGING_WINDOW_SIZE];

/**	Test items
 *	
 *	I2C connected devices:
 *		USB hub
 *		RGB led
 *		USB-C crossbar switch
 *		VXR7200
 *		Proximity (off-board)
 *		OLED panels (off-board)
 *
 *	GPIO controlled devices:
 *		Fan (off-board)
 *
 *	Note: SteamVR tracking is not connected through
 *	the microcontroller, so it can't be tested here.
 */

Test_Error_T test_usb_hub(void) {
	uint8_t hub_init_response;
	// Since the hub will disable I2C access when it's running normally,
	// we shouldn't try to communicate with it if already initialized
	if(usb3803_is_initialized(USB_HUB_1)) {
		return TE_Success;
	}
	
	ioport_set_pin_level(PIN_USBHUB_RESET, false); // RESET_N asserted, hub resets
	vTaskDelay(10);
	
	hub_init_response = usb3803_init(MCU_I2C());
	
	if(hub_init_response == pdPASS) {
		return TE_Success;
	} else {
		return TE_Fail;
	}
}

Test_Error_T test_rgb_led(void) {
	// Reads a register from the AW2033 chip to check
	// if it is responding
	// Register 0x00 (Chip ID and reset) will
	// read as 0x09 if all is well
	
	Test_Error_T retval;
	I2C_Handle* hi2c = MCU_I2C();
	I2C_Transaction_Request i2ctxn;
	uint8_t id_byte;
	
	i2ctxn.dev_addr = RGB_LED_I2C_ADDR;
	i2ctxn.cb = NULL; // no callback needed
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.len = 1;
	i2ctxn.data = &id_byte;
		
	i2c_lock(hi2c);
		
	// Check ID register
	i2ctxn.internal_addr = RGB_RESET_REG;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
	i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	
	if(i2c_status(hi2c) != I2C_SUCCESS) {
		retval = TE_Fail;
	} else {
		if(0x09 == id_byte) {
			retval = TE_Success;
		} else {
			retval = TE_Fail;
		}
	}
	
	i2c_unlock(hi2c);
	
	return retval;
}

Test_Error_T test_usbc_switch(void) {
	// Reads from the PI3USB31532 chip to check
	// if it is responding
	// No internal registers, simply read or write
	// 2 bytes to the chip's I2C port for control
	
	Test_Error_T retval;
	I2C_Transaction_Request i2ctxn;
	I2C_Handle* hi2c = DDIC_I2C();
	uint8_t id_bytes[2];
	
	i2ctxn.cb = NULL;
	i2ctxn.dev_addr = USBC_MUX_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.internal_addr = 0; // Using 0 as internal address, this byte is actually ignored
	i2ctxn.data = id_bytes;
	i2ctxn.len = 2;
	i2ctxn.ttype = I2C_SIMPLE_READ;
	
	i2c_lock(hi2c);
	
	i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
	
	if(i2c_status(hi2c) != I2C_SUCCESS) {
		retval = TE_Fail;
	} else {
		if((0 == id_bytes[0]) && (0 == (id_bytes[1] & 0xF8))) {
			retval = TE_Success;
		} else {
			retval = TE_Fail;
		}
	}
	i2c_unlock(hi2c);
	
	return retval;
}

Test_Error_T test_vxr(void) {
	if(RC_Success == VXR_Check_Connection()) {
		return TE_Success;
	}
	
	return TE_Fail;
}

Test_Error_T test_prox(void) {
	// Reads a register on the TMD2635 proximity sensor
	// to check that it is present and responding
	
	Test_Error_T retval;
	I2C_Transaction_Request i2ctxn;
	I2C_Handle* hi2c = MCU_I2C();
	uint8_t id_byte;
		
	i2ctxn.cb = NULL;
	i2ctxn.dev_addr = TMD2635_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.internal_addr = PROX_REG_DEVID;
	i2ctxn.data = &id_byte;
	i2ctxn.len = 1;
	i2ctxn.ttype = I2C_INT_ADDR_READ;
		
	i2c_lock(hi2c);
		
	i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
		
	if(i2c_status(hi2c) != I2C_SUCCESS) {
		retval = TE_Fail;
		} else {
		if(0x44 == id_byte) {
			retval = TE_Success;
			} else {
			retval = TE_Fail;
		}
	}
	i2c_unlock(hi2c);
		
	return retval;
}

Test_Error_T test_oled_panels(void) {
	Test_Error_T left_result = test_oled_panel(OLED_LEFT);
	Test_Error_T right_result = test_oled_panel(OLED_RIGHT);
	if((TE_Success == left_result) && (TE_Success == right_result)) {
		return TE_Success;
	} else {
		return TE_Fail;
	}
}

Test_Error_T test_oled_panel(OLED_Panel_T panel_sel) {
	// Reads a register on the SY103WAM01 oled panel
	// to check that it is present and responding
	
	Test_Error_T retval;
	I2C_Transaction_Request i2ctxn;
	uint8_t read_buf[2];
	
	// Left panel = DDIC_I2C, right panel = MCU_I2C
	I2C_Handle* hi2c = (panel_sel == OLED_LEFT) ? (DDIC_I2C()) : (MCU_I2C());
	
	i2ctxn.cb = NULL;
	i2ctxn.dev_addr = OLED_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.ttype = I2C_SIMPLE_WRITE;
	i2ctxn.len = 2;
	i2ctxn.data = read_buf;
	i2ctxn.internal_addr = 0;
	
	i2c_lock(hi2c);
	read_buf[0] = 0x36; // MADCTL (image flip) register. Doesn't really matter what the value is, 
						// we just need to check if the I2C bus gets a result.
	read_buf[1] = 0;
	i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME); // Write 2 bytes (internal register address)
	i2ctxn.ttype = I2C_SIMPLE_READ;
	i2c_transact_blocking(hi2c, &i2ctxn, DEFAULT_I2C_WAIT_TIME); // Read 2 bytes (value of register)
	
	if(i2c_status(hi2c) == I2C_SUCCESS) {
		retval = TE_Success;
	} else {
		retval = TE_Fail;
	}
	i2c_unlock(hi2c);

	return retval;
}

Test_Error_T test_fan(void) {
	// Attempts to spin up the fan at various power levels
	// Checks that the fan speed response makes sense for those power settings
	
	uint16_t this_fan_sample;
	uint32_t averaging_working_value;
	uint32_t timer_start_time; 
	uint8_t rolling_window_place;
	uint8_t within_deviation_count;
	bool timed_out;
	bool average_reached;
	
	// Save old fan speed 
	uint32_t old_fan_speed = TC0->TC_CHANNEL[1].TC_RB;
	// Set test power level
	set_fan_pwm(TEST_FAN_PWM_SETTING_1); // Should be above 10-15 to ensure it starts up
	// Wait a little while to make sure it's spinning
	vTaskDelay(TEST_FAN_STARTUP_TIME_MS);
	// Begin averaging in a window
	for(uint8_t i = 0; i < TEST_FAN_AVERAGING_WINDOW_SIZE; i++) {
		fan_window_average[i] = get_fan_speed();
		vTaskDelay(TEST_FAN_SAMPLING_TIME_MS);
	}
	
	// Check that a number of samples does not deviate more that some amount
	// from the average. If we time out before stabilizing, we fail.
	timer_start_time = rtt_read_timer_value();
	timed_out = false;
	average_reached = false;
	rolling_window_place = 0;
	within_deviation_count = 0;
	while((!timed_out) && (!average_reached)) {
		
		averaging_working_value = 0;
		for(uint8_t j = 0; j < TEST_FAN_AVERAGING_WINDOW_SIZE; j++) {
			averaging_working_value += fan_window_average[j];
		}
		averaging_working_value /= TEST_FAN_AVERAGING_WINDOW_SIZE;
		
		// Check fan is running. If this average speed is zero, we fail.
		if(averaging_working_value <= TEST_FAN_MIN_AVERAGE_SPEED) {
			// go back to last fan speed
			if(old_fan_speed == 0) {
				// Special case needed for speed = 0
				set_fan_pwm(0);
			} else {
				TC0->TC_CHANNEL[1].TC_RB = old_fan_speed;
			}
			return TE_Fail;
		}
		
		this_fan_sample = get_fan_speed();
		
		fan_window_average[rolling_window_place++] = this_fan_sample; // Add current sample to the rolling window
		if(rolling_window_place >= TEST_FAN_AVERAGING_WINDOW_SIZE) {
			rolling_window_place = 0;
		}
		
		// Did we get within deviation?
		if( (this_fan_sample >= (averaging_working_value - TEST_FAN_DEVIATION_ALLOWED )) && 
			(this_fan_sample <= (averaging_working_value + TEST_FAN_DEVIATION_ALLOWED )) ) {
			within_deviation_count++;
			if(within_deviation_count > TEST_FAN_AVERAGING_WINDOW_SIZE) {
				average_reached = true;
			}
		} else {
			// reset the counter on any large deviation
			within_deviation_count = 0;
		}
		
		vTaskDelay(TEST_FAN_SAMPLING_TIME_MS);
		timed_out = rtt_timeout_check(timer_start_time, TEST_FAN_TIMEOUT);
	}
	
	// go back to last fan speed
	if(old_fan_speed == 0) {
		// Special case needed for speed = 0
		set_fan_pwm(0);
	} else {
		TC0->TC_CHANNEL[1].TC_RB = old_fan_speed;
	}
	
	if(timed_out || (!average_reached)) {
		return TE_Fail;
	}
	else {
		return TE_Success;
	}
}