#include <string>

#include "driver/gpio.h"
#include <driver/i2c_master.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_TIMEOUT_MS 100

void Oled::init(uint8_t sda_pin, uint8_t scl_pin) {
    i2c_master_bus_config_t bus_config = {
        .i2c_port = I2C_NUM_0,
        .sda_io_num = (gpio_num_t)sda_pin,
        .scl_io_num = (gpio_num_t)scl_pin,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 7,
        .intr_priority = 0,
        .trans_queue_depth = 0,
        .flags = {
            .enable_internal_pullup = true,
        },
    };
    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, &bus_handle));

    i2c_device_config_t dev_config = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = OLED_I2C_ADDRESS,
        .scl_speed_hz = 400000,
        .scl_wait_us = 0,
        .flags = {
            .disable_ack_check = false,
        },
    };
    ESP_ERROR_CHECK(i2c_master_bus_add_device(bus_handle, &dev_config, &dev_handle));

    // Full SSD1306 initialisation sequence
    uint8_t init_cmd[] = {
        OLED_CONTROL_BYTE_CMD_STREAM,
        OLED_CMD_DISPLAY_OFF,               // display off during init
        OLED_CMD_SET_DISPLAY_CLK_DIV, 0x80, // clock divide ratio / oscillator frequency
        OLED_CMD_SET_MUX_RATIO, 0x3F,       // 1/64 duty (64 rows)
        OLED_CMD_SET_DISPLAY_OFFSET, 0x00,  // no vertical shift
        OLED_CMD_SET_DISPLAY_START_LINE,    // start line = 0
        OLED_CMD_SET_CHARGE_PUMP, 0x14,     // enable charge pump
        OLED_CMD_SET_MEMORY_ADDR_MODE, 0x02,// page addressing mode
        OLED_CMD_SET_SEGMENT_REMAP,         // mirror horizontally (0xA1)
        OLED_CMD_SET_COM_SCAN_MODE,         // mirror vertically   (0xC8)
        OLED_CMD_SET_COM_PIN_MAP, 0x12,     // alt COM pin config for 128x64
        OLED_CMD_SET_CONTRAST, 0x7F,        // mid contrast
        OLED_CMD_SET_PRECHARGE, 0xF1,       // pre-charge period
        OLED_CMD_SET_VCOMH_DESELCT, 0x30,  // VCOMH deselect level
        OLED_CMD_DISPLAY_RAM,               // display follows RAM
        OLED_CMD_DISPLAY_NORMAL,            // non-inverted
        OLED_CMD_DISPLAY_ON,                // turn display on
    };

    if (ESP_OK != i2c_master_transmit(dev_handle, init_cmd, sizeof(init_cmd), I2C_TIMEOUT_MS)) {
        ESP_LOGE(TAG, "configuration failed");
    }

    clear_display();
}

void Oled::clear_display() {

    // Data packet: control byte + PAGE_SIZE zero bytes
    uint8_t data_buf[1 + PAGE_SIZE] = { OLED_CONTROL_BYTE_DATA_STREAM };
    // rest is already zero-initialised

    for (uint8_t i = 0; i < PAGE_COUNT; i++) {
        // Set page address and reset column to 0
        uint8_t page_cmd[] = {
            OLED_CONTROL_BYTE_CMD_STREAM,
            (uint8_t)(0xB0 | i), // page address
            0x00,                // column low nibble = 0
            0x10,                // column high nibble = 0
        };
        i2c_master_transmit(dev_handle, page_cmd, sizeof(page_cmd), I2C_TIMEOUT_MS);
        i2c_master_transmit(dev_handle, data_buf, sizeof(data_buf), I2C_TIMEOUT_MS);
    }
}

void Oled::set_contrast(uint8_t contrast) {
    uint8_t cmd[] = {
        OLED_CONTROL_BYTE_CMD_STREAM,
        OLED_CMD_SET_CONTRAST,
        contrast,
    };
    i2c_master_transmit(dev_handle, cmd, sizeof(cmd), I2C_TIMEOUT_MS);
}

void Oled::display_text(const std::string& text) {

    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;

    // Reset cursor to top-left
    uint8_t reset_cmd[] = {
        OLED_CONTROL_BYTE_CMD_STREAM,
        0x00,           // reset column low nibble
        0x10,           // reset column high nibble
        (uint8_t)(0xB0 | cur_page), // reset page
    };
    i2c_master_transmit(dev_handle, reset_cmd, sizeof(reset_cmd), I2C_TIMEOUT_MS);

    for (uint8_t i = 0; i < text.size() && i < max_size; i++) {
        if (text[i] == '\n' || col >= col_width) {
            // new line
            cur_page++;
            col = 0;
            uint8_t newline_cmd[] = {
                OLED_CONTROL_BYTE_CMD_STREAM,
                0x00,
                0x10,
                (uint8_t)(0xB0 | cur_page),
            };
            i2c_master_transmit(dev_handle, newline_cmd, sizeof(newline_cmd), I2C_TIMEOUT_MS);
        } else {
            uint8_t rotated[8];
            rotate8x8((uint8_t*)font8x8_basic[(uint8_t)text[i]], rotated);

            // Prepend data-stream control byte
            uint8_t data_buf[9];
            data_buf[0] = OLED_CONTROL_BYTE_DATA_STREAM;
            for (int b = 0; b < 8; b++) {
                data_buf[1 + b] = rotated[b];
            }
            i2c_master_transmit(dev_handle, data_buf, sizeof(data_buf), I2C_TIMEOUT_MS);
            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[7 - y] & (1 << x)) {
                out[x] |= (1 << y);  // bit 0 = top row, iterate rows bottom-to-top to correct orientation
            }
        }
    }
}