#include "ssd1306.h"
#include "ssd1306_driver.h"
#include "esp_log.h"
#include "stdlib.h"
#include "string.h"
#include "config.h"
#include "fonts.h"

#ifdef ENABLE_SSD1306

static uint8_t SSD1306_Buffer[SSD1306_WIDTH * SSD1306_HEIGHT / 8];

typedef struct {
	uint16_t CurrentX;
	uint16_t CurrentY;
	uint8_t Inverted;
	uint8_t Initialized;
} SSD1306_t;

/* Private variable */
static SSD1306_t SSD1306;

void SSD1306_DrawPixel(uint16_t x, uint16_t y, SSD1306_COLOR_t color) {
	if (
		x >= SSD1306_WIDTH ||
		y >= SSD1306_HEIGHT
	) {
		return;
	}
	if (color == SSD1306_COLOR_WHITE) {
		SSD1306_Buffer[x + (y / 8) * SSD1306_WIDTH] |= 1 << (y % 8);
	} else {
		SSD1306_Buffer[x + (y / 8) * SSD1306_WIDTH] &= ~(1 << (y % 8));
	}
}

char SSD1306_Putc(char ch, const FontDef_t* Font, SSD1306_COLOR_t color) {
	uint32_t i, b, j;
	
	/* Check available space in LCD */
	if (
		SSD1306_WIDTH <= (SSD1306.CurrentX + Font->FontWidth) ||
		SSD1306_HEIGHT <= (SSD1306.CurrentY + Font->FontHeight)
	) {
		/* Error */
		return 0;
	}
	
	/* Go through font */
	for (i = 0; i < Font->FontHeight; i++) {
		b = Font->data[(ch - 32) * Font->FontHeight + i];
		for (j = 0; j < Font->FontWidth; j++) {
			if ((b << j) & 0x8000) {
				SSD1306_DrawPixel(SSD1306.CurrentX + j, (SSD1306.CurrentY + i), (SSD1306_COLOR_t) color);
			} else {
				SSD1306_DrawPixel(SSD1306.CurrentX + j, (SSD1306.CurrentY + i), (SSD1306_COLOR_t)!color);
			}
		}
	}
	
	/* Increase pointer */
	SSD1306.CurrentX += Font->FontWidth;
	
	/* Return character written */
	return ch;
}

char SSD1306_Puts(char* str, FontDef_t* Font, SSD1306_COLOR_t color) {
	/* Write characters */
	while (*str) {
		/* Write character by character */
		if (SSD1306_Putc(*str, Font, color) != *str) {
			/* Return error */
			return *str;
		}
		
		/* Increase string pointer */
		str++;
	}
	
	/* Everything OK, zero should be returned */
	return *str;
}

void SSD1306_UpdateScreen(void) {
	uint8_t m;
	
	for (m = 0; m < 8; m++) {
        uint8_t configMsg[] = {
            PAGE_START_ADDR+m,        // set page range:
            0x00,                      //   y / 8
            0x10,                      //   y / 8
        };
        i2c_tx_cmd(configMsg, sizeof configMsg);
        i2c_tx_data(&SSD1306_Buffer[SSD1306_WIDTH * m], SSD1306_WIDTH);
	}
}


esp_err_t oled_ssd1306_init() {

    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);

    i2c_master_write_byte(cmd, (SSD1306_OLED_ADDR << 1) | WRITE_BIT, ACK_CHECK_EN);

    // Initialization (page 64)
    // The next bytes are commands
    i2c_master_write_byte(cmd, COMMAND_MODE, ACK_CHECK_EN);

    // Mux Ratio
    i2c_master_write_byte(cmd, MULTIPLEX_RATIO, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x3F, ACK_CHECK_EN);

    // Set display offset
    i2c_master_write_byte(cmd, DISPLAY_OFFSET, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x00, ACK_CHECK_EN);

    // Set display line start
    i2c_master_write_byte(cmd, DISPLAY_LINE_START, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x00, ACK_CHECK_EN);

    // Set Segment re-map
    i2c_master_write_byte(cmd, SEGMENT_REMAP, ACK_CHECK_EN);

    // Set COM output scan dir
    i2c_master_write_byte(cmd, COM_OUTPUT_SCAN_DIR, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x00, ACK_CHECK_EN);

    // Set COM pins hardware config
    i2c_master_write_byte(cmd, COM_PINS_HARDWARE_CONFIG, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x12, ACK_CHECK_EN);

    // Set contrast Control
    i2c_master_write_byte(cmd, CONTRAST_CONTROL, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x7F, ACK_CHECK_EN);

    i2c_master_write_byte(cmd, DEACTIVATE_SCROLL, ACK_CHECK_EN);

    // Disable entire display ON
    i2c_master_write_byte(cmd, DISABLE_ENTIRE_DISPLAY, ACK_CHECK_EN);

    // Set normal display
    i2c_master_write_byte(cmd, NORMAL_DISPLAY, ACK_CHECK_EN);

    // Set OSC frequency
    i2c_master_write_byte(cmd, DISPLAY_CLK_RATIO, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x80, ACK_CHECK_EN);

    // Enable charge pump regulator
    i2c_master_write_byte(cmd, CHARGE_PUMP_SET, ACK_CHECK_EN);
    i2c_master_write_byte(cmd, 0x14, ACK_CHECK_EN);

    // Display on
    i2c_master_write_byte(cmd, DISPLAY_ON, ACK_CHECK_EN);

    // Stop bit
    i2c_master_stop(cmd);

    esp_err_t ret = i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, 1000 / portTICK_RATE_MS);
    i2c_cmd_link_delete(cmd);

    SSD1306.CurrentX = 30;
	SSD1306.CurrentY = 10;
	
	SSD1306.Initialized = 1;

    SSD1306_Putc('A',&Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_DrawPixel(50,40, SSD1306_COLOR_WHITE);
    SSD1306_Putc('X',&Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_Puts(" THOMAS WAS\nHERE", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_UpdateScreen();

    return ret;
}

#endif