#include <string>

#include "driver/gpio.h"
#include "driver/i2c.h"
#include "esp_err.h"
#include "esp_log.h"
#include "Oled.h"
#include "font8x8_basic.h"
#include "ssd1306.h"

#define TAG "OLED"

// 128x64 pixels
// 8 pixels per byte
// 128 bytes per page -> ((128*64)/8)/128 = 8 pages.
#define DISPLAY_WIDTH 128
#define DISPLAY_HEIGHT 64
#define PAGE_SIZE 128
#define PAGE_COUNT 8
#define I2C_PORT I2C_NUM_0

void Oled::init(uint8_t sda_pin, uint8_t scl_pin) {
    i2c_config_t i2c_config = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = sda_pin,
        .scl_io_num = scl_pin,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master = {
            .clk_speed = 1000000
        },
        .clk_flags = 0,
    };
    i2c_param_config(I2C_PORT, &i2c_config);

    i2c_driver_install(I2C_PORT, I2C_MODE_MASTER, 0, 0, 0);

	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
	i2c_master_write_byte(cmd, OLED_CMD_SET_CHARGE_PUMP, true);
	i2c_master_write_byte(cmd, 0x14, true);
	i2c_master_write_byte(cmd, OLED_CMD_SET_SEGMENT_REMAP, true);
	i2c_master_write_byte(cmd, OLED_CMD_SET_COM_SCAN_MODE, true);
	i2c_master_write_byte(cmd, OLED_CMD_DISPLAY_ON, true);
	i2c_master_stop(cmd);

	if (ESP_OK != i2c_master_cmd_begin(I2C_PORT, cmd, 10 / portTICK_PERIOD_MS)) {
		ESP_LOGE(TAG, "configuration failed");
	}

	i2c_cmd_link_delete(cmd);

	clear_display();
}

void Oled::clear_display() {
    ESP_LOGI(TAG, "clear_display");
	i2c_cmd_handle_t cmd;

	uint8_t page[PAGE_SIZE] = {0};
	for (uint8_t i = 0; i < PAGE_COUNT; i++) {
		cmd = i2c_cmd_link_create();
		i2c_master_start(cmd);
		i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
		i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_SINGLE, true);
		i2c_master_write_byte(cmd, 0xB0 | i, true);

		i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_DATA_STREAM, true);
		i2c_master_write(cmd, page, PAGE_SIZE, true);
		i2c_master_stop(cmd);
		i2c_master_cmd_begin(I2C_PORT, cmd, 10 / portTICK_PERIOD_MS);
		i2c_cmd_link_delete(cmd);
	}
}

void Oled::set_contrast(uint8_t contrast) {
	i2c_cmd_handle_t cmd;
	cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);
	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
	i2c_master_write_byte(cmd, OLED_CMD_SET_CONTRAST, true);
	i2c_master_write_byte(cmd, contrast, true);
	i2c_master_stop(cmd);
	i2c_master_cmd_begin(I2C_NUM_0, cmd, 10 / portTICK_PERIOD_MS);
	i2c_cmd_link_delete(cmd);
}

void Oled::display_text(const std::string& text) {
    ESP_LOGI(TAG, "display_text %s", text.c_str());
	i2c_cmd_handle_t cmd;

	uint8_t cur_page = 0;
	uint8_t col = 0;
	auto col_width = DISPLAY_WIDTH / 8;  // using an 8x8 font
	auto num_rows = DISPLAY_HEIGHT / 8;  // using an 8x8 font
	auto max_size = col_width * num_rows;

	cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);

	i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
	i2c_master_write_byte(cmd, 0x00, true); // reset column
	i2c_master_write_byte(cmd, 0x10, true);
	i2c_master_write_byte(cmd, 0xB0 | cur_page, true); // reset page

	i2c_master_stop(cmd);
	i2c_master_cmd_begin(I2C_NUM_0, cmd, 10 / portTICK_PERIOD_MS);
	i2c_cmd_link_delete(cmd);

	for (uint8_t i = 0; i < text.size() && i < max_size; i++) {
	    ESP_LOGI(TAG, "%d: %c", i, text[i]);
		if (text[i] == '\n' || col >= col_width) {
		    ESP_LOGI(TAG, "new line");
			cmd = i2c_cmd_link_create();
			i2c_master_start(cmd);
			i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);

			i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_CMD_STREAM, true);
			i2c_master_write_byte(cmd, 0x00, true); // reset column
			i2c_master_write_byte(cmd, 0x10, true);
			i2c_master_write_byte(cmd, 0xB0 | ++cur_page, true); // increment page

			i2c_master_stop(cmd);
			i2c_master_cmd_begin(I2C_NUM_0, cmd, 10/portTICK_PERIOD_MS);
			i2c_cmd_link_delete(cmd);
			col = 0;
		} else {
			cmd = i2c_cmd_link_create();
			i2c_master_start(cmd);
			i2c_master_write_byte(cmd, (OLED_I2C_ADDRESS << 1) | I2C_MASTER_WRITE, true);

			i2c_master_write_byte(cmd, OLED_CONTROL_BYTE_DATA_STREAM, true);
			uint8_t rotated[8];
			rotate8x8((uint8_t*)font8x8_basic[(uint8_t)text[i]], rotated);
			i2c_master_write(cmd, rotated, 8, true);

			i2c_master_stop(cmd);
			i2c_master_cmd_begin(I2C_NUM_0, cmd, 10/portTICK_PERIOD_MS);
			i2c_cmd_link_delete(cmd);
			col++;
		}
	}
}

void Oled::rotate8x8(uint8_t in[8], uint8_t out[8]) {
    for (int x = 0; x < 8; x++) {
        out[x] = 0;
        for (int y = 0; y < 8; y++) {
            if (in[y] & (1 << x)) {
                out[x] |= (1 << (7 - y));
            }
        }
    }
}