/*
 * SSD1306_oled.c
 *
 *  Created on: Apr 5, 2021
 *      Author: bartool
 */

#include "main.h"
#include "SSD1306_oled.h"
#include "string.h"

I2C_HandleTypeDef *oled_i2c;

#ifdef TEST
uint8_t buffer_oled[SSD1306_BUF_SIZE];
#else
static uint8_t buffer_oled[SSD1306_BUF_SIZE];
#endif

static void SSD1306_SendCommand(uint8_t cmd)
{
	HAL_I2C_Mem_Write(oled_i2c, SSD1306_ADDRESS<<1, 0x00, 1, &cmd, 1, SSD1306_TIMEOUT);
}

static void SSD1306_SendData(uint8_t *data, uint16_t size)
{
	HAL_I2C_Mem_Write(oled_i2c, SSD1306_ADDRESS<<1, 0x40, 1, data, size, SSD1306_TIMEOUT);
}

void SSD1306_clear(uint8_t color)
{
	switch(color)
	{
	case WHITE:
		memset(buffer_oled, 0xFF, SSD1306_BUF_SIZE);
		break;
	case BLACK:
		memset(buffer_oled, 0x00, SSD1306_BUF_SIZE);
		break;
	}
}

void SSD1306_display_all(void)
{
	SSD1306_SendCommand(SSD1306_PAGEADDR);
	SSD1306_SendCommand(0);
	SSD1306_SendCommand(0xFF);
	SSD1306_SendCommand(SSD1306_COLUMNADDR);
	SSD1306_SendCommand(0);
	SSD1306_SendCommand(SSD1306_LCDWIDTH - 1);

	SSD1306_SendData(buffer_oled, SSD1306_BUF_SIZE);
}

uint8_t SSD1306_display_page(void)
{
	static uint8_t page = 0;

	SSD1306_SendCommand(SSD1306_PAGEADDR);
	SSD1306_SendCommand(page);
	SSD1306_SendCommand(page);
	SSD1306_SendCommand(SSD1306_COLUMNADDR);
	SSD1306_SendCommand(0);
	SSD1306_SendCommand(SSD1306_LCDWIDTH - 1);

	SSD1306_SendData(buffer_oled + (page * SSD1306_LCDWIDTH), SSD1306_LCDWIDTH);
//	page++;
	if (++page > 7)
	{
		page = 0;
		return SSD1306_SENDALL;
	}
	else
	{
		return SSD1306_SENDPAGE;
	}
}

void SSD1306_Init(I2C_HandleTypeDef *i2c)
{
	oled_i2c = i2c;
	SSD1306_SendCommand(SSD1306_DISPLAYOFF);
	SSD1306_SendCommand(SSD1306_SETDISPLAYCLOCKDIV);
	SSD1306_SendCommand(0x80);
	SSD1306_SendCommand(SSD1306_SETMULTIPLEX);
	SSD1306_SendCommand(SSD1306_LCDHEIGHT - 1);

	SSD1306_SendCommand(SSD1306_SETDISPLAYOFFSET);
	SSD1306_SendCommand(0x0);
	SSD1306_SendCommand(SSD1306_SETSTARTLINE | 0x0);
	SSD1306_SendCommand(SSD1306_CHARGEPUMP);

	SSD1306_SendCommand(0x14);

	SSD1306_SendCommand(SSD1306_MEMORYMODE);
	SSD1306_SendCommand(0x00);
	SSD1306_SendCommand(SSD1306_SEGREMAP | 0x1);
	SSD1306_SendCommand(SSD1306_COMSCANDEC);

	uint8_t comPins = 0x02;
	uint8_t contrast = 0x8F;

	if ((SSD1306_LCDWIDTH == 128) && (SSD1306_LCDHEIGHT == 32)) {
		comPins = 0x02;
		contrast = 0x0F;
	} else if ((SSD1306_LCDWIDTH == 128) && (SSD1306_LCDHEIGHT == 64)) {
		comPins = 0x12;
		contrast = 0xCF;
	} else if ((SSD1306_LCDWIDTH == 96) && (SSD1306_LCDHEIGHT == 16)) {
		comPins = 0x2; // ada x12
		contrast = 0xAF;
	}

	SSD1306_SendCommand(SSD1306_SETCOMPINS);
	SSD1306_SendCommand(comPins);
	SSD1306_SendCommand(SSD1306_SETCONTRAST);
	SSD1306_SendCommand(contrast);

	SSD1306_SendCommand(SSD1306_SETPRECHARGE); // 0xd9
	SSD1306_SendCommand(0xF1);

	SSD1306_SendCommand(SSD1306_SETVCOMDETECT);
	SSD1306_SendCommand(0x40);
	SSD1306_SendCommand(SSD1306_DISPLAYALLON_RESUME);
	SSD1306_SendCommand(SSD1306_NORMALDISPLAY);
	SSD1306_SendCommand(SSD1306_DEACTIVATE_SCROLL);

	SSD1306_SendCommand(SSD1306_DISPLAYON);


}

void SSD1306_set_pixel(uint8_t x, uint8_t y, uint8_t bw)
{
	if (x > SSD1306_LCDWIDTH || y > SSD1306_LCDHEIGHT) return;

	switch (bw)
	{
	case WHITE:
		buffer_oled[(y/8) * SSD1306_LCDWIDTH + x] |= (1<<(y%8));
		break;
	case BLACK:
		buffer_oled[(y/8) * SSD1306_LCDWIDTH + x] &= ~(1<<(y%8));
		break;
	case INVERSE:
		buffer_oled[(y/8) * SSD1306_LCDWIDTH + x] ^= (1<<(y%8));
		break;
	}
}

// void ssd1306_write_to_buffer(const uint8_t* data, uint8_t width, uint8_t height, int8_t pos_x, int8_t pos_y)
// {
// 	int16_t max_x, max_y;
// 	uint8_t shift_x = 0, temp, row = 0;
// 	uint16_t buf_idx = 0, index = 0;


// 	// right boundry
// 	if (width + pos_x > SSD1306_LCDWIDTH) max_x = SSD1306_LCDWIDTH;
// 	else max_x = width + pos_x;

// 	// left boundry
// 	if (pos_x < 0)
// 	{
// 		shift_x = pos_x * -1;
// 		pos_x = 0;
// 	}

// 	// bottom boundry
// 	if (height + pos_y > SSD1306_LCDHEIGHT) max_y = SSD1306_LCDHEIGHT;
// 	else max_y = height + pos_y;
// 	max_y = max_y / 8 + (max_y % 8 == 0 ? 0 : 1);

// 	// top boundry
// 	if (pos_y < 0)
// 	{
// 		uint8_t abs_pos_y = pos_y * -1;
// 		row = abs_pos_y/8 + (abs_pos_y % 8 == 0 ? 0 : 1);
// 		pos_y = (8 - abs_pos_y%8)%8;
// 	}

// 	uint8_t shift = pos_y % 8;
// 	uint8_t mask_lsb = 0xFF >> (8 - shift);
// 	uint8_t mask_msb = 0xFF << shift;

// 	for (uint8_t y = pos_y / 8; y < max_y; y++)
// 	{
// 		index = width * row + shift_x;
// 		for (uint8_t x = pos_x; x < max_x; x++, index++)
// 		{
// 			buf_idx = y * SSD1306_LCDWIDTH + x;
// 			temp = buffer_oled[buf_idx];

// 			if (index < width * ((height + 7) / 8))
// 				temp = data[index] << shift | (temp & mask_lsb);
// //			else if (height%8 != 0)
// //				mask_msb = 0xFF << ((pos_y + height)%8);

// 			if (shift != 0 && index >= width)
// 				temp = data[index - width] >> (8 - shift) | (temp & mask_msb);

// 			buffer_oled[buf_idx] = temp;
// 		}
// 		row++;
// 	}
// }

// void ssd1306_clear_buffer (uint8_t width, uint8_t height, uint8_t pos_x, uint8_t pos_y)
// {
// 	uint16_t max_x, max_y, index = 0, temp, row = 0;
// 	if (width + pos_x > SSD1306_LCDWIDTH) max_x = SSD1306_LCDWIDTH;
// 	else max_x = width + pos_x;

// 	if (height + pos_y > SSD1306_LCDHEIGHT) max_y = SSD1306_LCDHEIGHT;
// 	else max_y = height + pos_y;
// 	max_y = max_y / 8 + (max_y % 8 == 0 ? 0 : 1);

// 	uint8_t shift = pos_y % 8;
// 	uint8_t mask_right = 0xFF >> shift;
// 	uint8_t mask_left = 0xFF << (8 - shift);
// 	for (uint8_t y = pos_y / 8; y < max_y; y++, index = width * row)
// 	{
// 		for (uint8_t x = pos_x; x < max_x; x++, index++)
// 		{
// 			temp = buffer_oled[y * SSD1306_LCDWIDTH + x];
// 			if (index < width * height/8)
// 				temp &= mask_left;

// 			if (index >= width)
// 				temp &= mask_right;

// 			buffer_oled[y * SSD1306_LCDWIDTH + x] = temp;
// 		}
// 		row++;
// 	}

// }

typedef struct 
{
	uint16_t bitmap_max_idx;
	uint8_t buf_row_first;
	uint8_t buf_row_last;
	uint8_t buf_col_first;
	uint8_t buf_col_last;
	uint8_t bitmap_col;
	uint8_t bitmap_row;
	uint8_t bitmap_row_first;
	uint8_t shift;
}buf_bitmap_boundry_t;

static void get_boundry (buf_bitmap_boundry_t* boundry, uint8_t bitmap_width, uint8_t bitmap_height, int8_t pos_x, int8_t pos_y)
{
	if (pos_x < 0) {
		boundry->bitmap_col = pos_x * -1;
		boundry->buf_col_first = 0;
	} else {
		boundry->bitmap_col = 0;
		boundry->buf_col_first = pos_x;
	}

	if (pos_y < 0) {
		boundry->shift = (pos_y * -1);
		boundry->bitmap_row_first = boundry->shift / 8;
		boundry->buf_row_first = 0;
	} else {
		boundry->shift = pos_y;
		boundry->bitmap_row_first = 0;
		boundry->buf_row_first = pos_y / 8;
	}

	if ((bitmap_width + pos_x) > SSD1306_LCDWIDTH) {
		boundry->buf_col_last = SSD1306_LCDWIDTH;
	} else {
		boundry->buf_col_last = bitmap_width + pos_x;
	}
	
	if (bitmap_height + pos_y > SSD1306_LCDHEIGHT) {
		boundry->buf_row_last = SSD1306_LCDHEIGHT / 8;
	} else {
		boundry->buf_row_last = (bitmap_height + pos_y + 7) / 8;
	}
	
	boundry->bitmap_max_idx = bitmap_width * ((bitmap_height + 7) / 8);
}
/**
 * @brief A function that writes a bitmap into the buffer at the given position.
 * 			0,0 -------->x
 * 			 |
 * 			 |
 * 			\ /
 * 			 y
 * @param bitmap A pointer to bitmap array.
 * @param bitmap_width Bitmap witdh in pixels.
 * @param bitmap_height Bitmap height in pixels.
 * @param pos_x Position x in the display
 * @param pos_y Position y in the display
 * @param color WHITE (1) normal mode, others will cause inverse mode
 */
void SSD1306_write_to_buffer(const uint8_t* bitmap, uint8_t bitmap_width, uint8_t bitmap_height, int8_t pos_x, int8_t pos_y, uint8_t color)
{
	if (bitmap_width + pos_x < 0 || bitmap_height + pos_y < 0) return;

	uint16_t tmp_buf16, bitmap_idx;
	uint8_t mask_buf, tmp_bitmap;

	buf_bitmap_boundry_t b;
	get_boundry(&b, bitmap_width, bitmap_height, pos_x, pos_y);

	for(uint8_t col = b.buf_col_first; col < b.buf_col_last; col++, b.bitmap_col++)
	{
		tmp_buf16 = 0;
		b.bitmap_row = b.bitmap_row_first;
		for( uint8_t buf_row = b.buf_row_first; buf_row < b.buf_row_last; buf_row++, b.bitmap_row++ )
		{
			bitmap_idx = bitmap_width * b.bitmap_row + b.bitmap_col;
			mask_buf = 0;

			if (b.bitmap_row == 0 && pos_y > 0) {
				mask_buf |= 0xFF >> (8 - (b.shift % 8));
			}

			uint8_t shifted_pixels_left = (pos_y%8 + bitmap_height) - b.bitmap_row * 8;
			if (shifted_pixels_left < 8) {
				mask_buf |= (0xFF << shifted_pixels_left);
			}

			tmp_buf16 |= buffer_oled[buf_row * SSD1306_LCDWIDTH + col] & mask_buf;

			if (bitmap_idx < b.bitmap_max_idx)
			{
				switch (color)
				{
				case WHITE:
					tmp_bitmap = bitmap[bitmap_idx];
					break;
				default:
					tmp_bitmap = ~(bitmap[bitmap_idx]);
					break;
				}

				if (pos_y < 0) {
					tmp_buf16 |= (tmp_bitmap  >> (b.shift % 8)) & ~(mask_buf);
				} else {
					tmp_buf16 |= (tmp_bitmap  << (b.shift % 8)) & ~(mask_buf);
				}
			}

			buffer_oled[buf_row * SSD1306_LCDWIDTH + col] = (uint8_t) tmp_buf16;
			tmp_buf16 = tmp_buf16 >> 8;
		}
	}
}

void SSD1306_clear_buffer(uint8_t width, uint8_t height, int8_t pos_x, int8_t pos_y, uint8_t color)
{
	if (width + pos_x < 0 || height + pos_y < 0) return;

	uint16_t tmp_buf16, bitmap_idx;
	uint16_t mask_buf;
	uint8_t bitmap_row;
	
	switch(color)
	{
		case WHITE:
			color = 0xFF;
			break;
		case BLACK:
			color = 0x00;
			break;
	}

	buf_bitmap_boundry_t b;
	get_boundry(&b, width, height, pos_x, pos_y);

	for(uint8_t col = b.buf_col_first; col < b.buf_col_last; col++, b.bitmap_col++)
	{
		tmp_buf16 = 0;
		bitmap_row = b.bitmap_row_first;
		for( uint8_t buf_row = b.buf_row_first; buf_row < b.buf_row_last; buf_row++, bitmap_row++ )
		{
			bitmap_idx = width * bitmap_row + b.bitmap_col;
			mask_buf = 0;

			if (bitmap_row == 0 && pos_y > 0) {
				mask_buf |= 0xFF >> (8 - (b.shift % 8));
			}

			uint8_t shifted_pixels_left = (pos_y%8 + height) - bitmap_row * 8;
			if (shifted_pixels_left < 8) {
				mask_buf |= (0xFF << shifted_pixels_left);
			}

			tmp_buf16 |= buffer_oled[buf_row * SSD1306_LCDWIDTH + col] & mask_buf;

			if (bitmap_idx < b.bitmap_max_idx)
			{
				if (pos_y < 0) {
					tmp_buf16 |= (color >> (b.shift % 8)) & ~(mask_buf);
				} else {
					tmp_buf16 |= (color << (b.shift % 8)) & ~(mask_buf);
				}
			}

			buffer_oled[buf_row * SSD1306_LCDWIDTH + col] = (uint8_t) tmp_buf16;
			tmp_buf16 = tmp_buf16 >> 8;
		}
	}
}