/**
 * oled_control.c
 *
 * Low level control of the OLED panels over I2C.
 * 
 * Copyright (c) 2022 Bigscreen, Inc.
 */


#include <asf.h>
#include <string.h>
#include "i2c.h"
#include "oled_control.h"

#define CMD3_PAGE1_DATA_LENGTH			(2)
#define CMD3_PAGE1_CMD_ADDRESS			(0xFF)
const uint8_t cmd3_page1_data[CMD3_PAGE1_DATA_LENGTH] = {
	0x5A, 0x81
};

#define TC_TRIGGER_POINT_CMD_ADDRESS	(0xF9)
#define TC_TRIGGER_POINT_START_OFFSET	(0x0B)
#define TC_TRIGGER_POINT_DATA_LENGTH	(13)
// These values used for all display modes
const uint8_t oled_startup_tc_trigger_point[TC_TRIGGER_POINT_DATA_LENGTH] = {
	0x58, 0x5F, 0x66, 0x6D, 0x74, 0x7B, 0x82, 0x89, 0x90, 0x97, 0x9E, 0xA5, 0xAC
};

#define VERT_BARS_FIX_CMD_ADDRESS		(0xF3)
#define VERT_BARS_FIX_START_OFFSET		(0x09)
#define VERT_BARS_FIX_DATA_LENGTH		(1)
const uint8_t oled_startup_vert_bars_data[VERT_BARS_FIX_DATA_LENGTH] = {
	0x01
};

#define I2C_GPIO_CONTROL_ADDRESS		(0xF2)
#define I2C_GPIO_CONTROL_START_OFFSET	(0x01)
#define I2C_GPIO_CONTROL_DATA_LENGTH	(1)
const uint8_t oled_startup_i2c_gpio_control[I2C_GPIO_CONTROL_DATA_LENGTH] = {
	0x02
};

// Format - First byte is length of the command.
//				Note: length includes address, 
//				so for commands without data (like 
//				display on) length = 1.
// 				Reserved value = 0.
//				If the length is zero, stop. 
//				It marks the end of the sequence.
//			Next byte is the address to write to.
//			All following bytes are data to write.
const uint8_t oled_startup_native_75Hz_DSC[] = {
	1, 0x28, // display off
	1, 0x10, // sleep in
	2, 0x03, 0x81, // DSC On
	2, 0x53, 0x29, // Gamma selection
	3, 0x51, 0xFF, 0x01, // "Fixed"?
	2, 0x69, 0x00, // No scaling
	2, 0x6B, 0x10, // 2 mipi ports
	45, 0x70, // DSC settings for 2544x2544
	0x11, 0x00, 0x00, 0x89,
	0x10, 0x80, 0x09, 0xF0,
	0x04, 0xF8, 0x04, 0xF8,
	0x04, 0xF8, 0x04, 0xF8,
	0x02, 0x00, 0x04, 0x1B,
	0x00, 0x20, 0x91, 0xBF,
	0x00, 0x11, 0x00, 0x0F,
	0x00, 0x19, 0x00, 0x09,
	0x18, 0x00, 0x10, 0xF0,
	0x03, 0x0C, 0x20, 0x00,
	0x06, 0x0B, 0x0B, 0x33,
	5, 0x80, 0x01, 0x3E, 0x3E, 0x11, // Display resolution setting
	8, 0x81, 0x01, 0xE1, 0x00, 0x10, 0x00, 0x10, 0x00, // timing control for normal mode
	8, 0x82, 0x01, 0xE1, 0x00, 0x10, 0x00, 0x10, 0x00, // timing control for idle mode
	2, 0x25, 0x01, // Temperature compensation on
	
	1, 0x22,  // all pixels off
	
	3, 0xF0, 0xAA, 0x10, // command 2 page 0
	2, 0xD1, 0x02, // VESA OSC frequency
	
	3, 0xF0, 0xAA, 0x11, // command 2 page 1
	4, 0xC0, 0x00, 0x40, 0x00, // ??
	10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x02, // PWM brightness duty setting
	
	3, 0xF0, 0xAA, 0x12, // command 2 page 2
	2, 0xD3, 0x20, // vcom tracking setting
	3, 0xBF, 0x37, 0xBE, // thermal detection
	8, 0xC2, 0xC1, 0x01, 0xE1, 0x00, 0x07, 0x00, 0x78, // OSC tracking
	
	0 // stop.
};

const uint8_t oled_startup_native_72Hz_DSC[] = {
	1, 0x28, // display off
	1, 0x10, // sleep in
	2, 0x03, 0x81, // DSC On
	2, 0x53, 0x29, // Gamma selection
	3, 0x51, 0xFF, 0x01, // "Fixed"?
	2, 0x69, 0x00, // No scaling
	2, 0x6B, 0x10, // 2 mipi ports
	45, 0x70, // DSC settings for 2544x2544
	0x11, 0x00, 0x00, 0x89,
	0x10, 0x80, 0x09, 0xF0,
	0x04, 0xF8, 0x04, 0xF8,
	0x04, 0xF8, 0x04, 0xF8,
	0x02, 0x00, 0x04, 0x1B,
	0x00, 0x20, 0x91, 0xBF,
	0x00, 0x11, 0x00, 0x0F,
	0x00, 0x19, 0x00, 0x09,
	0x18, 0x00, 0x10, 0xF0,
	0x03, 0x0C, 0x20, 0x00,
	0x06, 0x0B, 0x0B, 0x33,
	5, 0x80, 0x01, 0x3E, 0x3E, 0x11, // Display resolution setting
	8, 0x81, 0x01, 0xE1, 0x00, 0x06, 0x00, 0x10, 0x00, // timing control for normal mode
	8, 0x82, 0x01, 0xE1, 0x00, 0x06, 0x00, 0x10, 0x00, // timing control for idle mode
	2, 0x25, 0x01, // Temperature compensation on
		
	1, 0x22,  // all pixels off

	3, 0xF0, 0xAA, 0x10, // command 2 page 0
	2, 0xD1, 0x02, // VESA OSC frequency
	
	3, 0xF0, 0xAA, 0x11, // command 2 page 1
	10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x02, // PWM brightness duty setting
	
	3, 0xF0, 0xAA, 0x12, // command 2 page 2
	2, 0xD3, 0x20, // vcom tracking setting
	3, 0xBF, 0x37, 0xBE, // thermal detection
	8, 0xC2, 0xC1, 0x01, 0xE1, 0x00, 0x07, 0x00, 0x78, // OSC tracking
	
	0 // stop.
};

const uint8_t oled_startup_native_60Hz_DSC[] = {
	1, 0x28, // display off
	1, 0x10, // sleep in
	2, 0x03, 0x81, // DSC On
	2, 0x53, 0x29, // Gamma selection
	3, 0x51, 0xFF, 0x01, // "Fixed"?
	2, 0x69, 0x00, // No scaling
	2, 0x6B, 0x10, // 2 mipi ports
	45, 0x70, // DSC settings for 2544x2544
	0x11, 0x00, 0x00, 0x89,
	0x10, 0x80, 0x09, 0xF0,
	0x04, 0xF8, 0x04, 0xF8,
	0x04, 0xF8, 0x04, 0xF8,
	0x02, 0x00, 0x04, 0x1B,
	0x00, 0x20, 0x91, 0xBF,
	0x00, 0x11, 0x00, 0x0F,
	0x00, 0x19, 0x00, 0x09,
	0x18, 0x00, 0x10, 0xF0,
	0x03, 0x0C, 0x20, 0x00,
	0x06, 0x0B, 0x0B, 0x33,
	5, 0x80, 0x01, 0x3E, 0x3E, 0x11, // Display resolution setting
	8, 0x81, 0x01, 0xE1, 0x00, 0x06, 0x00, 0x10, 0x00, // timing control for normal mode
	8, 0x82, 0x01, 0xE1, 0x00, 0x06, 0x00, 0x10, 0x00, // timing control for idle mode
	2, 0x25, 0x01, // Temperature compensation on
	
	3, 0xF0, 0xAA, 0x10, // command 2 page 0
	2, 0xD1, 0x02, // VESA OSC frequency
	
	3, 0xF0, 0xAA, 0x11, // command 2 page 1
	//10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x02, // PWM brightness duty setting
	10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x82, // PWM brightness duty setting, 4 pulse mode
	
	3, 0xF0, 0xAA, 0x12, // command 2 page 2
	2, 0xD3, 0x20, // vcom tracking setting
	3, 0xBF, 0x37, 0xBE, // thermal detection
	
	0 // stop.
};

const uint8_t oled_startup_1920_90Hz_DSC[] = {
	1, 0x28, // display off
	1, 0x10, // sleep in
	2, 0x03, 0x81, // DSC On
	2, 0x53, 0x29, // Gamma selection
	3, 0x51, 0xFF, 0x01, // "Fixed"?
	2, 0x69, 0x02, // Upscaling by 1.33 : 1
	2, 0x6B, 0x10, // 2 mipi ports
	45, 0x70, // DSC settings for 1920x1920
		0x11, 0x00, 0x00, 0x89,
		0x10, 0x80, 0x07, 0x80,
		0x03, 0xC0, 0x07, 0x80,
		0x03, 0xC0, 0x03, 0xC0,
		0x02, 0x00, 0x03, 0x58,
		0x00, 0x20, 0xC6, 0x37,
		0x00, 0x0D, 0x00, 0x0F,
		0x00, 0x11, 0x00, 0x08,
		0x18, 0x00, 0x10, 0xF0,
		0x03, 0x0C, 0x20, 0x00,
		0x06, 0x0B, 0x0B, 0x33,
	5, 0x80, 0x01, 0x40, 0x40, 0x11, // Display resolution setting
	8, 0x81, 0x02, 0x56, 0x00, 0x09, 0x00, 0x0F, 0x00, // timing control for normal mode
	8, 0x82, 0x02, 0x56, 0x00, 0x09, 0x00, 0x0F, 0x00, // timing control for idle mode
	2, 0x25, 0x01, // Temperature compensation on
	
	1, 0x22,  // all pixels off
	
	3, 0xF0, 0xAA, 0x10, // command 2 page 0
	2, 0xD1, 0x02, // VESA OSC frequency
	
	3, 0xF0, 0xAA, 0x11, // command 2 page 1
	4, 0xC0, 0x00, 0x40, 0x00, // dimming control
	10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x02, // PWM brightness duty setting

	3, 0xF0, 0xAA, 0x12, // command 2 page 2
	2, 0xD3, 0x20, // vcom tracking setting
	3, 0xBF, 0x37, 0xBE, // thermal detection
	8, 0xC2, 0xC1, 0x01, 0x8F, 0x00, 0x05, 0x00, 0x63, // OSC tracking

	0 // stop.
};

const uint8_t oled_startup_1920_60Hz[] = {
	1, 0x28, // display off
	1, 0x10, // sleep in
	2, 0x03, 0x80, // DSC off
	2, 0x53, 0x29, // Gamma selection
	3, 0x51, 0xFF, 0x01, // ???
	2, 0x69, 0x02, // Upscale 1.33:1
	2, 0x6B, 0x10, // 2 mipi ports
	5, 0x80, 0x00, 0x40, 0x40, 0x11, // Display resolution
	8, 0x81, 0x02, 0x56, 0x00, 0x09, 0x00, 0x0F, 0x00, // timing control, normal mode
	8, 0x82, 0x02, 0x56, 0x00, 0x09, 0x00, 0x0F, 0x00, // timing control, idle mode
	2, 0x25, 0x01, // temperature compensation on
	
	1, 0x22,  // all pixels off
	
	3, 0xF0, 0xAA, 0x11, // Select command 2 page 1
	4, 0xC0, 0x00, 0x40, 0x00, // ???
	10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x02, // PWM brightness duty setting 

	3, 0xF0, 0xAA, 0x12, // command 2 page 2
	2, 0xD3, 0x20, // vcom tracking setting
	3, 0xBF, 0x37, 0xBE, // temp sensor on
	8, 0xC2, 0xC1, 0x02, 0x57, 0x00, 0x08, 0x00, 0x95, // OSC tracking
	
	0 // stop.
};

const uint8_t oled_startup_2560_30Hz[] = {
	1, 0x28, // display off
	1, 0x10, // sleep in
	2, 0x03, 0x80, // DSC off
	2, 0x53, 0x29, // Gamma selection
	3, 0x51, 0xFF, 0x01, // ???
	2, 0x69, 0x00, // No scaling
	2, 0x6B, 0x10, // 2 mipi ports
	5, 0x80, 0x00, 0x40, 0x40, 0x11, // Display resolution setting, osc selection
	8, 0x81, 0x04, 0xAC, 0x00, 0x10, 0x00, 0x10, 0x00, // timing control for normal mode
	8, 0x82, 0x04, 0xAC, 0x00, 0x10, 0x00, 0x10, 0x00, // timing control for idle mode
	2, 0x25, 0x01, // temperature compensation on
	
	1, 0x22,  // all pixels off
	
	3, 0xF0, 0xAA, 0x11, // Select command 2 page 1
	4, 0xC0, 0x00, 0x40, 0x00, // ???
	10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x82, // PWM brightness duty setting, no black insertion (it looks weird at 30Hz)

	3, 0xF0, 0xAA, 0x12, // command 2 page 2
	2, 0xD3, 0x20, // vcom tracking setting
	3, 0xBF, 0x37, 0xBE, // thermal detection
	8, 0xC2, 0xC1, 0x03, 0x80, 0x00, 0x0D, 0x00, 0xE0, // OSC tracking
	
	0 // stop.
};

const uint8_t oled_startup_colorbar_test[] = {
	// CMD1
	1, 0x01, // software reset
	2, 0x03, 0x80, // DSC close
	2, 0x53, 0x29,
	3, 0x51, 0xFF, 0x01,
	2, 0x69, 0x00,
	2, 0x6B, 0x10,
	5, 0x80, 0x00, 0x40, 0x40, 0x11,
	8, 0x81, 0x02, 0x56, 0x00, 0x0A, 0x00, 0x10, 0x00,
	8, 0x82, 0x02, 0x56, 0x00, 0x0A, 0x00, 0x10, 0x00,
	2, 0x35, 0x00,
	2, 0x25, 0x01,

	// CMD2 P1
	3, 0xF0, 0xAA, 0x11,
	// 10, 0xC2, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x90, 0x82,
	// to match our default brightness: 
	10, 0xC2, 0x01, 0x0A, 0x01, 0x0A, 0x01, 0x0A, 0x00, 0x90, 0x02,

	// CMD2 P2
	3, 0xF0, 0xAA, 0x12,
	2, 0xD3, 0x20,
	3, 0xBF, 0x37, 0xBE, // 85deg

	// CMD3 P1
	//3, 0xFF, 0x5A, 0x81,
	//2, 0x65, 0x0B,
	//14, 0xF9, 0x58, 0x5F, 0x66, 0x6D, 0x74, 0x7B, 0x82, 0x89, 0x90, 0x97, 0x9E, 0xA5, 0xAC,

	// CMD2 P1
	3, 0xF0, 0xAA, 0x11,
	5, 0xC4, 0xAA, 0x55, 0x01, 0x80,
	4, 0xC5, 0x00, 0x1F, 0xFF,

	0 // stop.
};


// When this was global, multiple tasks trying to access OLED I2C would clobber each other
// uint8_t oled_data_buffer[64];

uint8_t OLED_send_cmd(OLED_Panel_T panel_sel, uint8_t reg_addr, uint8_t datalen, const uint8_t* data) {
	// Every write over I2C is 4 bytes long
	// [0] - register address (high byte) = the same as the MIPI register address
	// [1] - register address (low byte) = the data byte offset number, starting from zero
	// [2] - always zero
	// [3] - data byte
	
	// using the "internal write" mode so we only send 3 bytes after the register address
	uint8_t cmd_buf[3];
	I2C_Handle* oled_i2c_handle = (panel_sel == OLED_LEFT) ? (DDIC_I2C()) : (MCU_I2C());
	
	I2C_Transaction_Request i2ctxn;
	i2ctxn.cb = NULL;
	i2ctxn.dev_addr = OLED_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2ctxn.len = 3; // address not counted, so it's 3 for offset, zero, and data bytes
	i2ctxn.data = cmd_buf;
	i2ctxn.internal_addr = reg_addr;

	cmd_buf[1] = 0;
	
	i2c_lock(oled_i2c_handle);
	
	//i2c_lockup_clear_sequence(oled_i2c_handle); // this could help a little
	uint8_t i2c_retval = pdFAIL;
	for(uint8_t i = 0; i < datalen; i++) {
		cmd_buf[0] = i;
		cmd_buf[2] = data[i];
		i2c_retval = i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
		if(oled_i2c_handle->status != I2C_SUCCESS) {
			// try resetting the bus and going again
			i2c_lockup_clear_sequence(oled_i2c_handle);
			i2c_retval = i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
			if(oled_i2c_handle->status != I2C_SUCCESS) {
				i2c_unlock(oled_i2c_handle);
				return pdFAIL;
			}
		}
	}
	i2c_unlock(oled_i2c_handle);
	return i2c_retval;
}

uint8_t OLED_send_tc_cmd(OLED_Panel_T panel_sel, uint8_t reg_addr, uint8_t reg_start_offset, uint8_t datalen, uint8_t* data)
{
	// this is identical to the above OLED_send_cmd except for one key difference:
	// it allows for the offset to start at a value different than zero
	// This is specific to the Temperature Compensation trigger point command (register 0xF9 in cmd3 page 1)
	// Over MIPI, you can first send "0x65 0x0B" to set the starting register write address
	// so the startup would look like:
	// 0xFF 0x5A 0x81			(switch to cmd3 page 1)
	// 0x65 0x0B				(change starting byte in the next write)
	// 0xF9 0x58 0x5F 0x66 0x6D 0x74 0x7B 0x82 0x89 0x90 0x97 0x9E 0xA5 0xAC	(send TC trigger point)
	
	// But over I2C this changes to:
	// 0xFF 0x00 0x00 0x5A		(switch to cmd3 page 1, part 1/2)
	// 0xFF 0x01 0x00 0x81		(switch to cmd3 page 1, part 2/2)
	// 0x65 0x00 0x00 0x0B		(attempt to change starting address of next write, but sadly this doesn't work)
	// 0xF9 0x00 0x00 0x58		(set TC trigger point, part 1/13)
	// 0xF9 0x01 0x00 0x5F		(set TC trigger point, part 2/13)
	// ...
	// 0xF9 0x0B 0x00 0xA5		(set TC trigger point, part 12/13)
	// 0xF9 0x0C 0x00 0xAC		(set TC trigger point, part 13/13)
	
	// Problem is that the 0xF9 command needs to actually start its offset at 0x0B. 
	// And the 0x65 command apparently is ignored.
	// So the full sequence SHOULD be:
	// 0xFF 0x00 0x00 0x5A		(switch to cmd3 page 1, part 1/2)
	// 0xFF 0x00 0x00 0x81		(switch to cmd3 page 1, part 2/2)
	// 0xF9 0x0B 0x00 0x58		(set TC trigger point, part 1/13)
	// 0xF9 0x0C 0x00 0x5F		(set TC trigger point, part 2/13)
	// ...
	// 0xF9 0x16 0x00 0xA5		(set TC trigger point, part 12/13)
	// 0xF9 0x17 0x00 0xAC		(set TC trigger point, part 13/13)
	// Note that the offset byte for 0xF9 starts at 0x0B and not 0x00
	
	
	// using the "internal write" mode so we only send 3 bytes after the register address
	uint8_t cmd_buf[3];
	I2C_Handle* oled_i2c_handle = (panel_sel == OLED_LEFT) ? (DDIC_I2C()) : (MCU_I2C());
		
	I2C_Transaction_Request i2ctxn;
	i2ctxn.cb = NULL;
	i2ctxn.dev_addr = OLED_I2C_ADDR;
	i2ctxn.shift = I2C_Unshifted;
	i2ctxn.ttype = I2C_INT_ADDR_WRITE;
	i2ctxn.len = 3; // address not counted, so it's 3 for offset, zero, and data bytes
	i2ctxn.data = cmd_buf;
	i2ctxn.internal_addr = reg_addr;

	cmd_buf[1] = 0;
		
	i2c_lock(oled_i2c_handle);
		
	//i2c_lockup_clear_sequence(oled_i2c_handle); // this could help a little
	uint8_t i2c_retval = pdFAIL;
	for(uint8_t i = 0; i < datalen; i++) {
		cmd_buf[0] = i + reg_start_offset;
		cmd_buf[2] = data[i];
		i2c_retval = i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
		if(oled_i2c_handle->status != I2C_SUCCESS) {
			// try resetting the bus and going again
			i2c_lockup_clear_sequence(oled_i2c_handle);
			i2c_retval = i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
			if(oled_i2c_handle->status != I2C_SUCCESS) {
				i2c_unlock(oled_i2c_handle);
				return pdFAIL;
			}
		}
	}
	i2c_unlock(oled_i2c_handle);
	return i2c_retval;
}

uint8_t OLED_Vert_Bars_Fix(OLED_Panel_T panel_sel) {
	// Applies a fix to the faint vertical bars seen with a fully green display in 1920x1920 mode
	//	(not just fully green display, also can be detected in some other edge cases)
	//
	// Similar to the TC Compensation command, this has a different format in MIPI vs
	// the I2C that we're using
	// MIPI:
	//  - 0xFF,0x5A,0x81
	//  - 0x65,0x09
	//  - 0xF3,0x01
	// I2C:
	//   - 0xFF 0x00 0x00 0x5A		(switch to cmd3 page 1, part 1/2)
	//   - 0xFF 0x01 0x00 0x81		(switch to cmd3 page 1, part 2/2)
	//   - 0xF3 0x09 0x00 0x01		(super secret vertical bars fix)
	
	uint8_t retval;
	
	retval = OLED_send_cmd(panel_sel, CMD3_PAGE1_CMD_ADDRESS, CMD3_PAGE1_DATA_LENGTH, cmd3_page1_data);
	
	if(pdPASS == retval ) {
		retval = OLED_send_tc_cmd(panel_sel,
			VERT_BARS_FIX_CMD_ADDRESS,
			VERT_BARS_FIX_START_OFFSET,
			VERT_BARS_FIX_DATA_LENGTH,
			oled_startup_vert_bars_data);
	}

	return retval;
}

static uint8_t OLED_read_cmd(OLED_Panel_T panel_sel, uint8_t reg_addr, uint8_t datalen, uint8_t* data) {
	// Reads are done with two transactions
	// First transaction sets the internal register address (write two bytes)
	// Second transaction reads that address (read two bytes)
	
	I2C_Handle* oled_i2c_handle = (panel_sel == OLED_LEFT) ? (DDIC_I2C()) : (MCU_I2C());
	
	uint8_t read_buf[2];
	
	I2C_Transaction_Request i2ctxn;
	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(oled_i2c_handle);
	
	for(uint8_t i = 0; i < datalen; i++) {
		
		// Write the internal register address
		read_buf[0] = reg_addr;
		read_buf[1] = i;
		i2ctxn.ttype = I2C_SIMPLE_WRITE;
		i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
		if(oled_i2c_handle->status != I2C_SUCCESS) {
			// try resetting the bus and going again
			i2c_lockup_clear_sequence(oled_i2c_handle);
			i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
			if(oled_i2c_handle->status != I2C_SUCCESS) {
				i2c_unlock(oled_i2c_handle);
				return pdFAIL;
			}
		}
		
		// Read the two bytes (although the first one is always zero)
		i2ctxn.ttype = I2C_SIMPLE_READ;
		i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
		if(oled_i2c_handle->status != I2C_SUCCESS) {
			// try resetting the bus and going again
			i2c_lockup_clear_sequence(oled_i2c_handle);
			i2c_transact_blocking(oled_i2c_handle, &i2ctxn, DEFAULT_I2C_WAIT_TIME);
			if(oled_i2c_handle->status != I2C_SUCCESS) {
				i2c_unlock(oled_i2c_handle);
				return pdFAIL;
			}
		}
		// Save the single valid byte that was just read into the output array
		data[i] = read_buf[1];
	}
	i2c_unlock(oled_i2c_handle);
	return pdPASS;
}


uint8_t OLED_set_brightness(OLED_Panel_T panel_sel, uint16_t new_brightness, bool pulse_emission) {
	uint8_t hi;
	uint8_t lo;
	uint8_t retval = pdFAIL;
	uint8_t oled_data_buffer[12];
	// Brightness command:
	// 0xF0 0xAA 0x11 (set page)
	// 0xC2 hi lo hi lo hi lo 0x00 0x90 0x02 (set brightness)
	oled_data_buffer[0] = 0xAA;
	oled_data_buffer[1] = 0x11;
	if(OLED_send_cmd(panel_sel, 0xF0, 2, oled_data_buffer) != pdPASS) {
		return pdFAIL;
	}
	
	hi = (uint8_t)((new_brightness & 0xFF00) >> 8);
	lo = (uint8_t)(new_brightness & 0x00FF);
	
	oled_data_buffer[0] = hi;
	oled_data_buffer[1] = lo;
	oled_data_buffer[2] = hi;
	oled_data_buffer[3] = lo;
	oled_data_buffer[4] = hi;
	oled_data_buffer[5] = lo;	
	oled_data_buffer[6] = 0x00;
	oled_data_buffer[7] = 0x90;
	if(pulse_emission) {
		oled_data_buffer[8] = 0x82;
	}
	else {
		oled_data_buffer[8] = 0x02;
	}
	retval = OLED_send_cmd(panel_sel, 0xC2, 9, oled_data_buffer);
	
	// Try to retrieve the value to confirm it was sent correctly
	if(retval == pdPASS) {
		retval = OLED_read_cmd(panel_sel, 0xC2, 9, oled_data_buffer);
	}
	if(retval == pdPASS) {
		// Check the brightness settings match
		if((oled_data_buffer[0] != hi) || (oled_data_buffer[2] != hi) || (oled_data_buffer[4] != hi)
		|| (oled_data_buffer[1] != lo) || (oled_data_buffer[3] != lo) || (oled_data_buffer[5] != lo)
		|| (oled_data_buffer[6] != 0x00) || (oled_data_buffer[7] != 0x90) || (oled_data_buffer[8] != 0x02)) {
			retval = pdFAIL;
		}
	}
	
	return retval;
}

uint8_t OLED_sleep_in_out(OLED_Panel_T panel_sel, bool sleep_in) {
	uint8_t panel_cmd;
	uint8_t oled_data_buffer[4];
	oled_data_buffer[0] = 0; // not actually used, but we need to send a
							// parameter length of 1 just to 
							// send a command even without parameters
	if(sleep_in) {
		panel_cmd = 0x10;	
	} else {
		panel_cmd = 0x11;
	}
	return OLED_send_cmd(panel_sel, panel_cmd, 1, oled_data_buffer);
}

uint8_t OLED_display_on_off(OLED_Panel_T panel_sel, bool display_on) {
	uint8_t panel_cmd;
	uint8_t oled_data_buffer[4];
	oled_data_buffer[0] = 0; // not actually used, but we need to send a
							 // parameter length of 1 just to 
							 // send a command even without parameters
	if(display_on) {
		panel_cmd = 0x29;
	} else {
		panel_cmd = 0x28;
	}
	uint8_t errval = OLED_send_cmd(panel_sel, panel_cmd, 1, oled_data_buffer);
	
	return errval;
}

uint8_t OLED_all_pixels_off(OLED_Panel_T panel_sel, bool display_on) {
	uint8_t panel_cmd;
	uint8_t oled_data_buffer[4];
	oled_data_buffer[0] = 0; // not actually used, but we need to send a
							 // parameter length of 1 just to 
							 // send a command even without parameters
	if(display_on) {
		panel_cmd = 0x13; // Normal mode
	} else {
		panel_cmd = 0x22; // ALLPOFF command
	}
	uint8_t errval = OLED_send_cmd(panel_sel, panel_cmd, 1, oled_data_buffer);
	
	return errval;
}

uint8_t OLED_invert_mode(OLED_Panel_T panel_sel, bool invert_on) {
	uint8_t panel_cmd;
	uint8_t oled_data_buffer[4];
	oled_data_buffer[0] = 0; // not actually used, but we need to send a
							 // parameter length of 1 just to 
							 // send a command even without parameters
	if(invert_on) {
		panel_cmd = 0x20; // exit invert mode
	} else {
		panel_cmd = 0x21; // invert mode
	}
	uint8_t errval = OLED_send_cmd(panel_sel, panel_cmd, 1, oled_data_buffer);
	
	return errval;
}

uint8_t OLED_dimming_post_init(OLED_Panel_T panel_sel) {
	uint8_t errval = pdFAIL;
	uint8_t oled_data_buffer[4];
	// do post init (dimming control)
	// mipi.write 0x29 0xF0 0xAA 0x11 (select CMD2 page 1)
	// mipi.write 0x29 0xC0 0xFF (not sure what this is, but it needs to be sent 20ms after display on)
	
	// Set all pixels normal, to negate the all pixels off command sent in the initialization
	OLED_all_pixels_off(panel_sel, true);
	
	oled_data_buffer[0] = 0xAA;
	oled_data_buffer[1] = 0x11;
	errval = OLED_send_cmd(panel_sel, 0xF0, 2, oled_data_buffer);
	oled_data_buffer[0] = 0xFF;
	if(pdPASS == errval) {
		errval = OLED_send_cmd(panel_sel, 0xC0, 1, oled_data_buffer);
	}
	return errval;
}

uint8_t OLED_flip(OLED_Panel_T panel_sel, bool flip) {
	uint8_t oled_data_buffer[4];
	oled_data_buffer[0] = (flip) ? (0x03) : (0x00);
	return OLED_send_cmd(panel_sel, 0x36, 1, oled_data_buffer);
}

uint8_t OLED_temperature(OLED_Panel_T panel_sel) {
	uint8_t oled_data_buffer[4];
	
	// Temperature command:
	// write: 0xF0 0xAA 0x12 (set CMD2 page2)
	// read: 0xBF02 (or read 0xBF with a length of 3 and return the third byte)
	
	oled_data_buffer[0] = 0xAA;
	oled_data_buffer[1] = 0x12;
	
	if(OLED_send_cmd(panel_sel, 0xF0, 2, oled_data_buffer) != pdPASS) {
		return 0;
	}
	
	if(OLED_read_cmd(panel_sel, 0xBF, 3, oled_data_buffer) != pdPASS) {
		return 0;
	}
	
	return oled_data_buffer[2];
}

uint8_t OLED_read_ID(OLED_Panel_T panel_sel, uint8_t* id_buffer) {
	// Read ID command:
	// Gets 14 bytes at address 0x84
	// Calling function must pass in a uint8_t array of at least 14 in length
	// Returns number of bytes copied to id_buffer
	// Will be either 14 if no problem, or 0 if problem
	// Don't use the data if return value is 0

	uint8_t oled_data_buffer[16];
	
	// switch I2C SDA to open drain
	uint8_t retval = pdPASS;
	
	// switch to CMD3 page1
	retval = OLED_send_cmd(panel_sel, CMD3_PAGE1_CMD_ADDRESS, CMD3_PAGE1_DATA_LENGTH, cmd3_page1_data);
	
	if(retval == pdPASS) {
		// send the special trigger point command with the non-zero starting byte offset
		retval = OLED_send_tc_cmd(panel_sel,
		I2C_GPIO_CONTROL_ADDRESS,
		I2C_GPIO_CONTROL_START_OFFSET,
		I2C_GPIO_CONTROL_DATA_LENGTH,
		oled_startup_i2c_gpio_control);
	}
	if(retval == pdPASS) {
		if(OLED_read_cmd(panel_sel, 0x84, 14, oled_data_buffer) == pdPASS) {
			memcpy(id_buffer, oled_data_buffer, 14);
			return 14;
		}
	}
	
	return 0;
}

uint8_t OLED_set_pps(OLED_Panel_T panel_sel, uint8_t* pps_data) {
	return OLED_send_cmd(panel_sel, OLED_PPS_REG_ADDR, OLED_PPS_LEN, pps_data);
}

uint8_t OLED_Startup(OLED_Panel_T panel_sel, Video_Selection_T vidsel) {
	// Run through the startup commands to initialize and turn on the panel
	uint8_t cmd_ptr = 0;
	uint8_t cmd_length;
	uint8_t cmd_length_to_send;
	uint8_t cmd_addr;
	uint8_t null_byte = 0;
	const uint8_t* cmd;

	uint8_t retval = pdPASS;
	
	// Assign the correct startup commands
	const uint8_t* startup_cmds;
	switch(vidsel) {
		case Vid_H1920_V1920_90Hz:
			startup_cmds = oled_startup_1920_90Hz_DSC;
		break;
		
		case Vid_H2544_V2544_75Hz:
			startup_cmds = oled_startup_native_75Hz_DSC;
		break;
		
		case Vid_H2544_V2544_72Hz:
			startup_cmds = oled_startup_native_72Hz_DSC;
		break;
		/*
		case Vid_H2544_V2184_90Hz:
			startup_cmds = oled_startup_native_90Hz_DSC;
		break;
		*/
		case Vid_H1920_V1920_60Hz:
			startup_cmds = oled_startup_1920_60Hz;
		break;
		case Vid_H2560_V2560_30Hz:
			startup_cmds = oled_startup_2560_30Hz;
		break;
		case Vid_H2544_V2544_60Hz:
			startup_cmds = oled_startup_native_60Hz_DSC;	
		break;
		case Vid_Colorbar_Test:
			startup_cmds = oled_startup_colorbar_test;
		break;
		case Vid_Unknown:
		default:
			return pdFAIL;
		break;
	}

	// when the "length" byte is zero, stop.
	while((retval == pdPASS) && (startup_cmds[cmd_ptr] != 0)) {
		cmd_length = startup_cmds[cmd_ptr];
		cmd_addr = startup_cmds[cmd_ptr+1];
		if(cmd_length > 1) {
			// for any command with real data
			cmd = &(startup_cmds[cmd_ptr+2]);
			cmd_length_to_send = cmd_length - 1;
		} else {
			// just a single address to send, so use a dummy byte
			cmd = &null_byte;
			cmd_length_to_send = 1;
		}
		retval = OLED_send_cmd(panel_sel, cmd_addr, cmd_length_to_send, cmd);

		cmd_ptr += cmd_length + 1; // +1 to include the offset of the length byte itself!
	}
	
	// now perform the special TC trigger point command
	if(retval == pdPASS)
	{
		// switch to CMD3 page1
		retval = OLED_send_cmd(panel_sel, CMD3_PAGE1_CMD_ADDRESS, CMD3_PAGE1_DATA_LENGTH, cmd3_page1_data);
	}
	if(retval == pdPASS) {
		// send the special trigger point command with the non-zero starting byte offset
		retval = OLED_send_tc_cmd(panel_sel, 
				TC_TRIGGER_POINT_CMD_ADDRESS,
				TC_TRIGGER_POINT_START_OFFSET,
				TC_TRIGGER_POINT_DATA_LENGTH,
				oled_startup_tc_trigger_point);
	}
	
	// switch I2C SDA to open drain
	if(retval == pdPASS)
	{
		// switch to CMD3 page1
		retval = OLED_send_cmd(panel_sel, CMD3_PAGE1_CMD_ADDRESS, CMD3_PAGE1_DATA_LENGTH, cmd3_page1_data);
	}
	if(retval == pdPASS) {
		// send the special trigger point command with the non-zero starting byte offset
		retval = OLED_send_tc_cmd(panel_sel,
		I2C_GPIO_CONTROL_ADDRESS,
		I2C_GPIO_CONTROL_START_OFFSET,
		I2C_GPIO_CONTROL_DATA_LENGTH,
		oled_startup_i2c_gpio_control);
	}

	return retval;
}

uint8_t OLED_Colorbar_Test(OLED_Panel_T panel_sel) {
	return OLED_Startup(panel_sel, Vid_Colorbar_Test);
}