#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <avr/pgmspace.h>

#ifdef DEBUG
#include <avr/io.h>
#endif

#include "HAL.h"
#include "ssd1306.h"
#include "i2c.h"
#include "fonts.h"
#include "image.h"

#define	HEIGHT	32		//no more than 255!
#define MEM_EVEN_BIT_ONLY	//if the height is 32 pixels, but only even bits are used
#define MEM_HEIGHT_FULL	64	//all memory
#define	WIDTH	128		//no more than 255!

static const uint8_t ssd_init[] PROGMEM = {
	OLED_CMD_DISPLAY_OFF,   // Display off
	0x20,   // Set Memory Addressing Mode
	0x10,   // 00,Horizontal Addressing Mode;01,Vertical Addressing Mode;10,Page Addressing Mode (RESET,11,Invalid
	0xB0,   // Set Page Start Address for Page Addressing Mode,0-7
	0xC8,   // Set COM Output Scan Direction
	0x00,   // Set low column address
	0x10,   // Set high column address
	0x40,   // Set start line address
	0x81,   // set contrast control register
	0xFF,
	#ifdef SSD1306_COM_TB_REMAP
	0xA1,   // Set segment re-map 0 to 127
	#else
	0xA0,   // Set segment re-map 0 to 127
	#endif
	0xA6,   // Set normal display
	0xA8,   // Set multiplex ratio(1 to 64)
	#ifdef MEM_EVEN_BIT_ONLY
	63,		//for 32 pixel height!
	#else
	(HEIGHT-1),
	#endif
	0xA4,   // 0xa4,Output follows RAM content;0xa5,Output ignores RAM content
	0xD3,   // Set display offset
	0x00,   // No offset
	0xD5,   // Set display clock divide ratio/oscillator frequency
	0xF0,   // Set divide ratio
	0xD9,   // Set pre-charge period
	0x22,

	0xDA,   // Set com pins hardware configuration
	#ifdef SSD1306_COM_LR_REMAP
	0x32,   // Enable COM left/right remap
	#else
	0x12,   // Do not use COM left/right remap
	#endif // SSD1306_COM_LR_REMAP

	0xDB,   // Set vcomh
	0x20,   // 0x20,0.77xVcc
	0x8D,   // Set DC-DC enable
	0x14,   //
	OLED_CMD_DISPLAY_ON   // Turn on SSD1306 panel
};

static uint8_t screen_buf[WIDTH*HEIGHT/8]; //TODO: volatile ?

#define ssd1306_cmd(buf, len)	do {while(!i2c_pgm_trans(DISPLAY_BUS, OLED_I2C_ADDRESS, i2c_write_cmd, OLED_CONTROL_BYTE_CMD_STREAM, buf, len, NULL));} while (0)
#define ssd1306_data(buf, len)	do {while(!i2c_pgm_trans(DISPLAY_BUS, OLED_I2C_ADDRESS, i2c_write_cmd, OLED_CONTROL_BYTE_DATA_STREAM, buf, len, NULL));} while (0)
	

static void ssd1306_command(uint8_t value)
{
	uint8_t buf[1];
	buf[0] = value;
	ssd1306_cmd(buf, 1);
}

void screen_update(void){
	uint8_t page2div = 0, draw[WIDTH], PosX;

	for (page2div = 0; page2div < HEIGHT/8; page2div++)
	{
		#ifdef MEM_EVEN_BIT_ONLY
		//set even line
		for (PosX=0; PosX<WIDTH; PosX++){
			draw[PosX] = (screen_buf[PosX+(WIDTH*page2div)] & 0x01)<<1;
			draw[PosX] |= (screen_buf[PosX+(WIDTH*page2div)] & 0x02)<<2;
			draw[PosX] |= (screen_buf[PosX+(WIDTH*page2div)] & 0x04)<<3;
			draw[PosX] |= (screen_buf[PosX+(WIDTH*page2div)] & 0x08)<<4;
		}
		ssd1306_command(0xB0 + page2div*2);//set even page address
		ssd1306_command(0x00);	//start address page - low nibble
		ssd1306_command(0x10);	//start address page - hight nibble
		ssd1306_data(draw, sizeof(draw));
		//set odd line
		for (PosX=0; PosX<WIDTH; PosX++){
			draw[PosX] = ((screen_buf[PosX+(WIDTH*page2div)] & 0x10) >> 3);
			draw[PosX] |= ((screen_buf[PosX+(WIDTH*page2div)] & 0x20) >> 2);
			draw[PosX] |= ((screen_buf[PosX+(WIDTH*page2div)] & 0x40) >> 1);
			draw[PosX] |= ((screen_buf[PosX+(WIDTH*page2div)] & 0x80) >> 0);
		}
		ssd1306_command(0xB0 + page2div*2 + 1);//set odd page address
		ssd1306_command(0x00);	//start address page - low nibble
		ssd1306_command(0x10);	//start address page - hight nibble
		ssd1306_data(draw, sizeof(draw));
		#else
		//TODO: 64 pixel height
		#endif
	}
}

void ssd1306_display_clear(void)
{
	uint16_t i;

	for (i = 0; i < (WIDTH*HEIGHT/8); i++) {
		screen_buf[i] = 0;
	}
	for (i = 0; i < 8; i++) {
		ssd1306_command(0xB0 + i);//set page address
		ssd1306_command(0x00);	//start address page - low nibble
		ssd1306_command(0x10);	//start address page - hight nibble
		ssd1306_data(screen_buf, WIDTH);
	}
}

void ssd1306_draw_pixel(uint8_t x, uint8_t y, bool set){
	if ((x>= WIDTH) || (y>=HEIGHT)){
		return;
	}
	if (set){
		screen_buf[x+(y/8*WIDTH)] |= 1<<(y%8);
		return;
	}
	screen_buf[x+(y/8*WIDTH)] &= ~(1<<(y%8));
}

/**
*  @brief: this draws a character on the pattern buffer but not refresh
*          returns the x position of the end character
*/
int ssd1306_char(int x, int y, char ascii_char, const sFONT* font, int colored){
	int i, j;
	unsigned int char_offset = 0;
	sPROP_SYMBOL symbol;

	#define NO_SYMBOL			0xff
	#define OffsetCalc(w)		(font->Height * (w / 8 + (w % 8 ? 1 : 0)))

	//get symbol and offset if table not full ascii table
	if (font->tableSymbolSize){
		for (i=0; i<= font->tableSymbolSize; i++){
			memcpy_P(&symbol, &(font->tableSymbol[i]), sizeof(sPROP_SYMBOL));
			symbol.Width = (font->FontType == monospaced)?font->Width:symbol.Width; //proportional font may be
			if(ascii_char == symbol.Symbol){
				break;
			}
			char_offset += OffsetCalc(symbol.Width);
		}
		if(ascii_char != symbol.Symbol){
			char_offset = 0;
		}
	}
	else{
		char_offset = (ascii_char-' ') * OffsetCalc(font->Width);//Only fixed font
		symbol.Width = font->Width;
	}
	
	const unsigned char* ptr = &font->table[char_offset];

	int set_pixel = (colored == COLORED)?COLORED:UNCOLORED;
	int clear_pixel = (colored == COLORED)?UNCOLORED:COLORED;
	for (j = 0; j < font->Height; j++) {
		for (i = 0; i < symbol.Width; i++) {
			if (pgm_read_byte(ptr) & (0x80 >> (i % 8))) {
				ssd1306_draw_pixel(x + i, y + j, set_pixel);
			}
			else {
				ssd1306_draw_pixel(x + i, y + j, clear_pixel);
			}
			if (i % 8 == 7) {
				ptr++;
			}
		}
		if (symbol.Width % 8 != 0) {
			ptr++;
		}
	}

	return x+symbol.Width;
}

int ssd1306_string(int x, const int y, const sFONT *font, const char *str){
	for (uint8_t i = 0; i<strlen(str);i++){
		x = ssd1306_char(x, y, str[i], font, COLORED);
	}
	return x;
}

void ssd1306_contrast(uint8_t contrast)
{
	uint8_t buf[2];
	buf[0] = OLED_CMD_SET_CONTRAST;
	buf[1] = contrast;
	ssd1306_cmd(buf, 2);
}

void ssd1306_display_show(bool show){
	if (show) ssd1306_command(OLED_CMD_DISPLAY_ON);
	else ssd1306_command(OLED_CMD_DISPLAY_OFF);
}

void ssd1306_image_pgm(uint8_t x, uint8_t y, const tImage* image)
{
	uint8_t posX, posY = 0, img_byte = 0;
	const uint8_t bytes_per_line = (image->width/image->dataSize) + (image->width % image->dataSize?1:0);
	for(;posY<image->height;posY++){
		for(posX = 0; posX < image->width; posX++){
			if (!(posX % 8)){
				img_byte = pgm_read_byte(image->data+((bytes_per_line * posY) + (posX/8)));
			}
			ssd1306_draw_pixel(x+posX, y+posY, ((img_byte & (1<<(posX % 8)))? COLORED : UNCOLORED));
		}
	}
}

bool ssd1306_init()
{
	uint8_t i = 0, buf[1] = {OLED_CMD_NOP};
	//checking the device on the bus
	bool res = false;
	while(i2c_is_busy());
	res = i2c_pgm_trans(DISPLAY_BUS, OLED_I2C_ADDRESS, i2c_write_cmd, OLED_CONTROL_BYTE_CMD_STREAM, buf, 1, false);
	if (res){
		i2c_white_ready();
		res = i2c_is_ok();
		if (res){
			for(;i < sizeof(ssd_init);i++){
				ssd1306_command(pgm_read_byte(ssd_init+i));
			}
		}
	}
	return res;
}