#include "st7567.h"
#include <string.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

static const char *TAG = "ST7567";

// I2C control byte: bit7 = Co (0 = stream, no further control bytes needed),
// bit6 = D/C# (0 = command, 1 = data). Same convention as SSD1306-family displays.
#define ST7567_I2C_CTRL_CMD  0x00
#define ST7567_I2C_CTRL_DATA 0x40

#define ST7567_I2C_TIMEOUT_MS 100
#define ST7567_DEFAULT_CLOCK_HZ 400000
#define ST7567_DEFAULT_CONTRAST 0x20

static esp_err_t st7567_i2c_write(const st7567_device_t *dev, uint8_t ctrl, const uint8_t *data, size_t len) {
    // small stack buffer, framebuffer flush is chunked by page (128B) so this is plenty
    uint8_t buf[ST7567_WIDTH + 1];
    if (len + 1 > sizeof(buf))
        return ESP_ERR_INVALID_SIZE;

    buf[0] = ctrl;
    memcpy(&buf[1], data, len);
    return i2c_master_transmit(dev->i2c_dev, buf, len + 1, ST7567_I2C_TIMEOUT_MS);
}

esp_err_t st7567_send_cmd(const st7567_device_t *dev, const uint8_t cmd) {
    return st7567_i2c_write(dev, ST7567_I2C_CTRL_CMD, &cmd, 1);
}

esp_err_t st7567_send_data(const st7567_device_t *dev, const uint8_t *data, size_t len) {
    return st7567_i2c_write(dev, ST7567_I2C_CTRL_DATA, data, len);
}

esp_err_t st7567_set_contrast(const st7567_device_t *dev, uint8_t contrast) {
    if (contrast > 63)
        contrast = 63;

    esp_err_t ret = st7567_send_cmd(dev, ST7567_CMD_CONTRAST_MODE);
    if (ret != ESP_OK)
        return ret;

    return st7567_send_cmd(dev, contrast);
}

esp_err_t st7567_init(const st7567_config_t *config, st7567_device_t *device) {
    memset(device, 0, sizeof(st7567_device_t)); // clears framebuffer too

    // Add device on the (already-initialized) I2C bus
    const i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = config->i2c_addr ? config->i2c_addr : ST7567_I2C_ADDR_DEFAULT,
        .scl_speed_hz = config->i2c_clock_speed_hz ? config->i2c_clock_speed_hz : ST7567_DEFAULT_CLOCK_HZ,
    };

    const esp_err_t ret = i2c_master_bus_add_device(config->i2c_bus, &dev_cfg, &device->i2c_dev);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to add I2C device: %s", esp_err_to_name(ret));
        return ret;
    }

    // internal software reset + init sequence (standard ST7567 sequence)
    st7567_send_cmd(device, ST7567_CMD_RESET);
    vTaskDelay(pdMS_TO_TICKS(10));
    st7567_send_cmd(device, ST7567_CMD_BIAS_1_9);
    st7567_send_cmd(device, ST7567_CMD_ADC_NORMAL);
    st7567_send_cmd(device, ST7567_CMD_COM_REVERSE);
    st7567_send_cmd(device, ST7567_CMD_RESISTOR_RATIO | 0x03);
    st7567_send_cmd(device, ST7567_CMD_POWER_CTRL | 0x07); // booster, regulator, follower all on
    vTaskDelay(pdMS_TO_TICKS(10)); // let the charge pump stabilize before touching contrast

    st7567_set_contrast(device, config->contrast ? config->contrast : ST7567_DEFAULT_CONTRAST);

    st7567_send_cmd(device, ST7567_CMD_START_LINE | 0x00);
    st7567_send_cmd(device, ST7567_CMD_ALL_PIXEL_NORMAL);
    st7567_send_cmd(device, ST7567_CMD_DISPLAY_NORMAL);
    st7567_send_cmd(device, ST7567_CMD_DISPLAY_ON);

    ESP_LOGI(TAG, "display init done");
    return ESP_OK;
}

void st7567_clear_buffer(st7567_device_t *dev) {
    memset(dev->framebuffer, 0, sizeof(dev->framebuffer));
}

esp_err_t st7567_clear(st7567_device_t *dev) {
    st7567_clear_buffer(dev);
    return st7567_update_screen(dev);
}

esp_err_t st7567_update_screen(const st7567_device_t *dev) {
    // ST7567 is page-addressed: 8 pages of 8 vertical px each, byte = one column
    // within a page (LSB = top row of the page). Same layout as SSD1306.
    for (uint8_t page = 0; page < ST7567_PAGES; page++) {
        esp_err_t ret;
        if ((ret = st7567_send_cmd(dev, ST7567_CMD_PAGE_ADDR | page)) != ESP_OK) return ret;
        if ((ret = st7567_send_cmd(dev, ST7567_CMD_COL_ADDR_HIGH | 0x00)) != ESP_OK) return ret;
        if ((ret = st7567_send_cmd(dev, ST7567_CMD_COL_ADDR_LOW | 0x00)) != ESP_OK) return ret;
        if ((ret = st7567_send_data(dev, &dev->framebuffer[page * ST7567_WIDTH], ST7567_WIDTH)) != ESP_OK) return ret;
    }

    return ESP_OK;
}

static void st7567_set_pixel_color(st7567_device_t *dev, const uint8_t x, const uint8_t y, const uint8_t color) {
    if (x >= ST7567_WIDTH || y >= ST7567_HEIGHT)
        return;

    const uint16_t byte_index = (y / 8) * ST7567_WIDTH + x;
    const uint8_t bit_index = y % 8;

    if (color)
        dev->framebuffer[byte_index] |= (1 << bit_index);
    else
        dev->framebuffer[byte_index] &= ~(1 << bit_index);
}

void st7567_draw_pixel(void *dev, const uint8_t x, const uint8_t y) {
    st7567_set_pixel_color(dev, x, y, 1);
}

void st7567_clear_pixel(void *dev, const uint8_t x, const uint8_t y) {
    st7567_set_pixel_color(dev, x, y, 0);
}