#include "lcd_oled.h"
#include "ssd1306_i2c.h"

i2c_t i2c_device =  {	I2C_CH_1,					/*!< The harware channel to be used	*/	
						i2c_mode_master,			/*!< Master Mode */
						0x00,						/*!< First and Main address of the device */ 
						0x00,						/*!< Second address if dual addresse mode is configured	*/
						i2c_address_count_single,	/*!< Single address */ 
						i2c_address_size_7b,		/*!< 10 or 7 bit address size */
						i2c_clk_speed_standart,		/*!< Bus Speed: standart */  
						i2c_clk_stretching_disable,	/*!< Clock Streching disabeled */
						i2c_wake_disabled};			/*!< Wake up condition : None */
	

#define pgm_read_byte(addr) (*(const uint8_t *)(addr))
#define ssd1306_swap(a, b) { int16_t t = a; a = b; b = t; }
#define ROTATION 0 // TODO: This shoudl be initiated at the function 

uint8_t display_buffer[LCD_OLED_SIZE_X * LCD_OLED_SIZE_Y / 8] = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80,
	0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x80, 0x80, 0xC0, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0xF8, 0xE0, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80,
	0x80, 0x80, 0x00, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0xFF,
	0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00, 0x80, 0x80, 0x00, 0x00,
	0x80, 0xFF, 0xFF, 0x80, 0x80, 0x00, 0x80, 0x80, 0x00, 0x80, 0x80, 0x80, 0x80, 0x00, 0x80, 0x80,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x00, 0x00, 0x8C, 0x8E, 0x84, 0x00, 0x00, 0x80, 0xF8,
	0xF8, 0xF8, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xE0, 0xE0, 0xC0, 0x80,
	0x00, 0xE0, 0xFC, 0xFE, 0xFF, 0xFF, 0xFF, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0xFF, 0xC7, 0x01, 0x01,
	0x01, 0x01, 0x83, 0xFF, 0xFF, 0x00, 0x00, 0x7C, 0xFE, 0xC7, 0x01, 0x01, 0x01, 0x01, 0x83, 0xFF,
	0xFF, 0xFF, 0x00, 0x38, 0xFE, 0xC7, 0x83, 0x01, 0x01, 0x01, 0x83, 0xC7, 0xFF, 0xFF, 0x00, 0x00,
	0x01, 0xFF, 0xFF, 0x01, 0x01, 0x00, 0xFF, 0xFF, 0x07, 0x01, 0x01, 0x01, 0x00, 0x00, 0x7F, 0xFF,
	0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x7F, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x01, 0xFF,
	0xFF, 0xFF, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x03, 0x0F, 0x3F, 0x7F, 0x7F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xE7, 0xC7, 0xC7, 0x8F,
	0x8F, 0x9F, 0xBF, 0xFF, 0xFF, 0xC3, 0xC0, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFC, 0xFC, 0xFC,
	0xFC, 0xFC, 0xFC, 0xFC, 0xFC, 0xF8, 0xF8, 0xF0, 0xF0, 0xE0, 0xC0, 0x00, 0x01, 0x03, 0x03, 0x03,
	0x03, 0x03, 0x01, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x01, 0x01,
	0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x01, 0x01, 0x03, 0x03, 0x00, 0x00,
	0x00, 0x03, 0x03, 0x00, 0x00, 0x00, 0x03, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
	0x03, 0x03, 0x03, 0x03, 0x03, 0x01, 0x00, 0x00, 0x00, 0x01, 0x03, 0x01, 0x00, 0x00, 0x00, 0x03,
	0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF9, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x3F, 0x1F, 0x0F,
	0x87, 0xC7, 0xF7, 0xFF, 0xFF, 0x1F, 0x1F, 0x3D, 0xFC, 0xF8, 0xF8, 0xF8, 0xF8, 0x7C, 0x7D, 0xFF,
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x3F, 0x0F, 0x07, 0x00, 0x30, 0x30, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0xFE, 0xFE, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xE0, 0xC0, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x30, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0xC0, 0xFE, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x7F, 0x3F, 0x1F,
	0x0F, 0x07, 0x1F, 0x7F, 0xFF, 0xFF, 0xF8, 0xF8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFE, 0xF8, 0xE0,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00,
	0x00, 0xFC, 0xFE, 0xFC, 0x0C, 0x06, 0x06, 0x0E, 0xFC, 0xF8, 0x00, 0x00, 0xF0, 0xF8, 0x1C, 0x0E,
	0x06, 0x06, 0x06, 0x0C, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0x00, 0x00, 0xFC,
	0xFE, 0xFC, 0x00, 0x18, 0x3C, 0x7E, 0x66, 0xE6, 0xCE, 0x84, 0x00, 0x00, 0x06, 0xFF, 0xFF, 0x06,
	0x06, 0xFC, 0xFE, 0xFC, 0x0C, 0x06, 0x06, 0x06, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0xC0, 0xF8,
	0xFC, 0x4E, 0x46, 0x46, 0x46, 0x4E, 0x7C, 0x78, 0x40, 0x18, 0x3C, 0x76, 0xE6, 0xCE, 0xCC, 0x80,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x01, 0x07, 0x0F, 0x1F, 0x1F, 0x3F, 0x3F, 0x3F, 0x3F, 0x1F, 0x0F, 0x03,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00,
	0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00, 0x03, 0x07, 0x0E, 0x0C,
	0x18, 0x18, 0x0C, 0x06, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x01, 0x0F, 0x0E, 0x0C, 0x18, 0x0C, 0x0F,
	0x07, 0x01, 0x00, 0x04, 0x0E, 0x0C, 0x18, 0x0C, 0x0F, 0x07, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00,
	0x00, 0x0F, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x0F, 0x00, 0x00, 0x00, 0x07,
	0x07, 0x0C, 0x0C, 0x18, 0x1C, 0x0C, 0x06, 0x06, 0x00, 0x04, 0x0E, 0x0C, 0x18, 0x0C, 0x0F, 0x07,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

void lcd_oled_init(i2c_t *i2c_dev, uint8_t address) 
{
	
	i2c_init(&i2c_device);
	ssd1306_i2c_set_display_off(i2c_dev);
	ssd1306_i2c_set_display_clkDiv_oscFreq(i2c_dev, 0, 15); // Working but to be refined
	ssd1306_i2c_set_multiplex_ratio(i2c_dev, LCD_OLED_SIZE_Y - 1); // Pages start at 0 and ends at 63
	ssd1306_i2c_set_display_offset(i2c_dev, 0x00);  // No Offset
	ssd1306_i2c_set_display_start_line(i2c_dev, 0); // Start line 0 
	ssd1306_i2c_set_chage_pump(i2c_dev, SSD1306_SWITCHCAPVCC);
	ssd1306_i2c_set_memory_addressing_mode(i2c_dev, SSD1306_MEMORY_MODE_HORIZONTAL);
	ssd1306_i2c_set_segment_remap(i2c_dev, 1);
	ssd1306_i2c_set_com_scan_direction(i2c_dev, 0); //Decremental TODO : Make a define for it
	ssd1306_i2c_set_com_pins(i2c_dev, SSD1306_COM_SEQ_LR_REMAP_ON);
	ssd1306_i2c_set_contrast(i2c_dev,0x9F);
	ssd1306_i2c_set_prechage_period(i2c_dev, 0xF0); // TODO : I don't know what he exxect is
	ssd1306_i2c_set_com_deselect_level(i2c_dev, 0x40); // TODO : PLeanty of things can influance this capacitor value being the most importnat one
	ssd1306_i2c_set_display_entire_on(i2c_dev, SSD1306_DISPLAYALLON_RESUME);
	ssd1306_i2c_set_display_invert_pixel(i2c_dev, 0); // Normal pilex printing. Warning this hurts the eye
	ssd1306_i2c_set_scroll(i2c_dev, 0);// Turn scrolling off
	ssd1306_i2c_set_display_on(i2c_dev);
}

uint8_t lcd_oled_clear()
{
	memset(display_buffer, 0,(LCD_OLED_SIZE_X * LCD_OLED_SIZE_Y / 8) * sizeof(uint8_t));
	return 1;
}

uint8_t lcd_oled_display()
{
	ssd1306_i2c_send_command(&i2c_device,SSD1306_COLUMNADDR);
	ssd1306_i2c_send_command(&i2c_device,0);	// Column start address (0 = reset)
	ssd1306_i2c_send_command(&i2c_device,LCD_OLED_SIZE_X - 1);	// Column end address (127 
	// = reset)

	ssd1306_i2c_send_command(&i2c_device,SSD1306_PAGEADDR);
	ssd1306_i2c_send_command(&i2c_device,0);	// Page start address (0 = reset)
	ssd1306_i2c_send_command(&i2c_device,7);	// Page end address

	uint8_t i2cDataLenght = 1;	// Co = 0, D/C = 0
	uint16_t address = SSD1306_I2C_ADDRESS;   	// Co = 0, D/C = 0
	uint8_t reg = 0x40;	
	// I2C
	int16_t i;
	for (i = 0; i < (LCD_OLED_SIZE_X * SSD1306_LCDHEIGHT / 8); i++) {
		i2c_write(&i2c_device, &address, &reg, &display_buffer[i], &i2cDataLenght);
		//This sends byte by byte. 
		//Better to send all buffer
		//Should be optimized
	}
/*
	uint16_t i2cDataLenght = (LCD_OLED_SIZE_X * SSD1306_LCDHEIGHT / 8) -1 ;	// Co = 0, D/C = 0
	uint16_t address = SSD1306_I2C_ADDRESS;   	// Co = 0, D/C = 0
	uint8_t reg = 0x40;	
	i2c_write(&i2c_device, &address, &reg, &display_buffer, &i2cDataLenght);
	*/
	return 1;
}

void lcd_oled_print_char(uint16_t x, uint16_t y, uint8_t c, uint8_t color)
{
	uint8_t size = 1; 
	if ((x >= WIDTH) ||	// Clip right
	    (y >= HEIGHT) ||	// Clip bottom
	    ((x + 6 * size - 1) < 0) ||	// Clip left
	    ((y + 8 * size - 1) < 0))	// Clip top
		return;
	int16_t i;
	int16_t j;
	for (i = 0; i < 6; i++) {
		int16_t line;
		if (i == 5)
			line = 0x0;
		else
			line = (*(const uint8_t *)(font + (c * 5) + i));
		for (j = 0; j < 8; j++) {
			if (line & 0x1) {
				if (size == 1)	// default size
					lcd_oled_draw_pixel(x + i, y + j, color);
				else {	// big size To implement
					//ssd1306_fillRect(x + (i * size),
					//		 y + (j * size), size,
					//		 size, color);
				}
			}
			line >>= 1;
		}
	}
}

void lcd_oled_draw_pixel(uint16_t x, uint16_t y, uint8_t color)
{
	if ((x < 0) || (x >= LCD_OLED_SIZE_X) || (y < 0) || (y >= LCD_OLED_SIZE_Y))
		return;

	// check rotation, move pixel around if necessary
	switch (ROTATION) {
	case 1:
		ssd1306_swap(x, y);
		x = LCD_OLED_SIZE_X - x - 1;
		break;
	case 2:
		x = LCD_OLED_SIZE_X - x - 1;
		y = LCD_OLED_SIZE_Y - y - 1;
		break;
	case 3:
		ssd1306_swap(x, y);
		y = LCD_OLED_SIZE_Y - y - 1;
		break;
	}

	// x is which column
	switch (color) {
	case WHITE:
		display_buffer[x + (y / 8) * LCD_OLED_SIZE_X] |= (1 << (y & 7));
		break;
	case BLACK:
		display_buffer[x + (y / 8) * LCD_OLED_SIZE_X] &= ~(1 << (y & 7));
		break;
	case INVERSE:
		display_buffer[x + (y / 8) * LCD_OLED_SIZE_X] ^= (1 << (y & 7));
		break;
	}
}

void lcd_oled_print_text(uint16_t x, uint16_t y, uint8_t *text, uint16_t length, uint8_t color)
{
	uint8_t pos, i = 0;
	for (i = 0; i < length; i++)
	{
		pos = x + (i * 6);
		lcd_oled_print_char(pos,y,text[i],color);
	}
}

void lcd_oled_draw_line(uint16_t x, uint16_t y, uint16_t angle, uint16_t lenght, uint8_t color)
{
	int16_t i = 0; 

	switch(angle)
	{
		case 0: 
		case 360: 
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x+i,y,color);
			}
			break; 
		case 45:
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x+i,y-i,color);
			}
			break; 
		case 90:
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x,y-i,color);
			}
			break; 
		case 135:
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x-i,y-i,color);
			}
			break; 
		case 180:
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x-i,y,color);
			}
			break; 
		case 225:
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x-i,y+i,color);
			}
			break; 
		case 270:
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x,y+i,color);
			}
			break; 
		case 315:
			for(i = 0; i < lenght; i++)
			{
				lcd_oled_draw_pixel(x+i,y+i,color);
			}
			break; 
	}
}

void lcd_oled_draw_circle(uint16_t x, uint16_t y, uint16_t radius, uint8_t fill, uint8_t color)
{
	uint8_t i = 0; 
	int16_t x_cicle, y_circle, d = 0;
	if(fill == 0)
	{
		x_cicle = 0;
		y_circle = radius;
		d = 3 - 2 * radius;
		lcd_oled_draw_circle_bresenham(x, y, x_cicle, y_circle, color);
		while (y_circle >= x_cicle)
		{
			x_cicle++;
			if (d > 0)
			{
				y_circle--;
				d = d + 4 * (x_cicle - y_circle) + 10;
			}
			else
				d = d + 4 * x_cicle + 6;
			lcd_oled_draw_circle_bresenham(x, y, x_cicle, y_circle, color);
		}
	}
	else // To be optimized
	{
		for(i=radius; i > 0; i--)
		{
			x_cicle = 0; 
			y_circle = i;
			d = 3 - 2 * i;
			lcd_oled_draw_circle_bresenham(x, y, x_cicle, y_circle, color);
			while (y_circle >= x_cicle)
			{
				x_cicle++;
				if (d > 0)
				{
					y_circle--;
					d = d + 4 * (x_cicle - y_circle) + 10;
				}
				else
					d = d + 4 * x_cicle + 6;
				lcd_oled_draw_circle_bresenham(x, y, x_cicle, y_circle, color);
			}
		}
	}
}

void lcd_oled_draw_circle_bresenham(int16_t xc, int16_t yc, uint8_t x, uint8_t y, uint8_t color)
{
    lcd_oled_draw_pixel(xc+x, yc+y, color);
    lcd_oled_draw_pixel(xc-x, yc+y, color);
    lcd_oled_draw_pixel(xc+x, yc-y, color);
    lcd_oled_draw_pixel(xc-x, yc-y, color);
    lcd_oled_draw_pixel(xc+y, yc+x, color);
    lcd_oled_draw_pixel(xc-y, yc+x, color);
    lcd_oled_draw_pixel(xc+y, yc-x, color);
    lcd_oled_draw_pixel(xc-y, yc-x, color);
}


void lcd_oled_draw_rectangle(uint16_t x, uint16_t y, uint16_t length, uint16_t width, uint8_t fill, uint8_t color)
{
	uint16_t i = 0;
	if(fill == EMPTY)
	{
		lcd_oled_draw_line(x, y, 0, length, color);
		lcd_oled_draw_line(x, y+width-1, 0, length, color);
		lcd_oled_draw_line(x, y, 270, width, color);
		lcd_oled_draw_line(x+length-1, y, 270, width, color);
	}
	else
	{
		for(i=0 ; i < width; i++)
		{
			lcd_oled_draw_line(x, y+i, 0, length, color);
		}
	}
}