#pragma once
#include "font.hpp"

#include <cstring>
#include <format>
#include <string>
#include <cstdint>

#include "hardware/i2c.h"

namespace display
{

// I2C1 on GPIO6 (SDA) and GPIO7 (SCL) running at 400KHz
constexpr auto I2C_PORT = i2c1;
constexpr unsigned I2C_SDA = 6;
constexpr unsigned I2C_SCL = 7;

// SSD1036 configuration
constexpr auto SSD1306_ADDR = 0x3c;
constexpr auto SSD1036_BYTES_PER_PAGE = 128;

constexpr unsigned DISPLAY_WIDTH = 128;
constexpr unsigned DISPLAY_HEIGHT = 64;

constexpr unsigned MAX_W = (DISPLAY_WIDTH - 1);
constexpr unsigned MAX_H = (DISPLAY_HEIGHT - 1);

// FRAME BUFFER
static uint8_t framebuffer[DISPLAY_HEIGHT * DISPLAY_WIDTH / 8];

typedef enum
{
  // Fundamental commands
  SET_CONTRAST = 0x81,         // Double byte command to select 1 out of
                               // 256 contrast steps.
  SET_ENTIRE_DISP_ON = 0xa4,   // Bit0 = 0: Output follows RAM content.
                               // Bit0 = 1: Output ignores RAM content,
                               //           all pixels are turned on.
  SET_NORMAL_INVERTED = 0xa6,  // Bit0 = 0: Normal display.
                               // Bit0 = 1: Inverted display.
  SET_DISP_ON_OFF = 0xae,      // Bit0 = 0: Display off, sleep mode.
                               // Bit0 = 1: Display on, normal mode.

  // Addressing setting Commands
  SET_ADDRESSING_MODE = 0x20,  // Double byte command to set memory
                               // addressing mode.
  SET_COLUMN_ADDRESS = 0x21,   // Tripple byte command to setup column start
                               // and end address.
  SET_PAGE_ADDRESS = 0x22,     // Tripple byte command to setup page start and
                               // end address.

  // Hardware configuration (panel resolution and layout related) commands
  SET_DISP_START_LINE = 0x40,  // Set display RAM display start line
                               // register. Valid values are 0 to 63.
  SET_SEGMENT_REMAP = 0xa0,    // Bit 0 = 0: Map col addr 0 to SEG0.
                               // Bit 0 = 1: Map col addr 127 to SEG0.
  SET_MUX_RATIO = 0xa8,        // Double byte command to configure display
                               // height. Valid height values are 15 to 63.
  SET_COM_OUTPUT_DIR = 0xc0,   // Bit 3 = 0: Scan from 0 to N-1.
                               // Bit 3 = 1: Scan from N-1 to 0. (N=height)
  SET_DISP_OFFSET = 0xd3,      // Double byte command to configure vertical
                               // display shift. Valid values are 0 to 63.
  SET_COM_PINS_CONFIG = 0xda,  // Double byte command to set COM pins
                               // hardware configuration.

  // Timing and driving scheme setting commands
  SET_DCLK_FOSC = 0xd5,        // Double byte command to set display clock
                               // divide ratio and oscillator frequency.
  SET_PRECHARGE_PERIOD = 0xd9, // Double byte command to set pre-charge
                               // period.
  SET_VCOM_DESEL_LEVEL = 0xdb, // Double byte command to set VCOMH deselect
                               // level.

  // Charge pump command
  SET_CHARGE_PUMP = 0x8d,      // Double byte command to enable/disable
                               // charge pump.
                               // Byte2 = 0x10: Disable charge pump.
                               // Byte2 = 0x14: Enable charge pump.
} ssd1306_commands;

static bool
write_commands(uint8_t command)
{
    uint8_t data[] = {0x00, command};
    // send command via I2C
    auto write_result = i2c_write_blocking(I2C_PORT, SSD1306_ADDR, data, sizeof(data), false);
    return write_result == sizeof(data);
}

static bool
write_framebuffer(const uint8_t *data, size_t length) {
    // SSD1306 data-control byte followed by pixel data.
    // Send in chunks to keep the transfer buffer small.
    uint8_t packet[17];
    packet[0] = 0x40;

    size_t offset = 0;

    while (offset < length) {
        size_t chunk = length - offset;

        if (chunk > 16) {
            chunk = 16;
        }

        memcpy(&packet[1], &data[offset], chunk);

        int result = i2c_write_blocking(
            I2C_PORT,
            SSD1306_ADDR,
            packet,
            chunk + 1,
            false
        );

        if (result != (int)(chunk + 1)) {
            return false;
        }

        offset += chunk;
    }

    return true;
}

static bool
diplay_init()
{
    // Initialize the SDD1306 controller
    const uint8_t commands[] = {
    SET_DISP_ON_OFF | 0x00,         // Display off.
    SET_DCLK_FOSC, 0x80,            // Set clock divide ratio and oscillator
                                    //   frequency.
    SET_MUX_RATIO, 0x3f,            // Set display height.
    SET_DISP_OFFSET, 0x00,          // Set vertical display shift to 0.
    SET_DISP_START_LINE,            // Set display RAM display start line
                                    //   register to 0.
    SET_CHARGE_PUMP, 0x14,          // Enable charge pump.
    SET_SEGMENT_REMAP | 0x01,       // Map col addr 127 to SEG0.
    SET_COM_OUTPUT_DIR | 0x08,      // Scan from N-1 to 0. (N=height)
    SET_COM_PINS_CONFIG, 0x12,      // Set COM pins hardware configuration to
                                    //   0x12.
    SET_CONTRAST, 0xcf,             // Set contrast to 0xcf
    SET_PRECHARGE_PERIOD, 0xf1,     // Set pre-charge to 0xf1
    SET_VCOM_DESEL_LEVEL, 0x40,     // Set VCOMH deselect to 0x40
    SET_ENTIRE_DISP_ON,             // Output follows RAM content.
    SET_NORMAL_INVERTED | 0x00,     // Normal display.
    SET_ADDRESSING_MODE, 0x00,      // Set addressing mode to horizontal mode.
    SET_COLUMN_ADDRESS, 0x00, 0x7f, // Set column start and end address.
    SET_PAGE_ADDRESS, 0x00, 0x07,   // Set page start and end address.
    SET_DISP_ON_OFF | 0x01,         // Display on.
    };

    for (uint8_t command : commands) {
        if (!write_commands(command)) {
            return false;
        }
    }

    memset(framebuffer, 0, sizeof(framebuffer));

    return true;
}

static bool
display_update()
{
    const uint8_t commands[] = {
        SET_COLUMN_ADDRESS, 0x00, 0x7f, // Column range: 0-127
        SET_PAGE_ADDRESS, 0x00, 0x07, // Page range: 0-7
    };

    for (uint8_t command : commands) {
        if (!write_commands(command)) {
            return false;
        }
    }

    return write_framebuffer(framebuffer, sizeof(framebuffer));

}

void
init()
{
    // Initialize I2C protocol
    i2c_init(I2C_PORT, 400*1000);
    
    gpio_set_function(I2C_SDA, GPIO_FUNC_I2C);
    gpio_set_function(I2C_SCL, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SDA);
    gpio_pull_up(I2C_SCL);

    // Initialize the display
    if (!diplay_init()) {
        while (true) {
            // Display not responding, stop here.
            sleep_ms(1000);
        }
    }

}



    static void draw_char(int x, int y, char character, int scale = 1)
{
    const Glyph *glyph = find_glyph(character);

    if (glyph == nullptr) {
        glyph = find_glyph(' ');
    }

    for (int col = 0; col < 5; ++col) {
        uint8_t bits = glyph->columns[col];

        for (int row = 0; row < 7; ++row) {
            if (bits & (1u << row)) {
                for (int dx = 0; dx < scale; ++dx) {
                    for (int dy = 0; dy < scale; ++dy) {
                        int px = x + col * scale + dx;
                        int py = y + row * scale + dy;

                        if (px < 0 || px >= DISPLAY_WIDTH ||
                            py < 0 || py >= DISPLAY_HEIGHT) {
                            continue;
                            }

                        int index = px + (py / 8) * DISPLAY_WIDTH;

                        framebuffer[index] |=
                            (uint8_t)(1u << (py % 8));
                    }
                }
            }
        }
    }
}
static void draw_text(int x, int y, const char *text, int scale = 1)
{
    while (*text) {
        draw_char(x, y, *text++, scale);
        x += 6 * scale;
    }
}

static void
render_framebuffer(uint8_t pot_a, uint8_t pot_b) {
    memset(framebuffer, 0, sizeof(framebuffer));
    draw_text(10, 8, "XIAO RP2040", 2);
    draw_text(10, 30, std::format("POT A: {}\n", static_cast<std::uint32_t>(pot_a)).c_str(), 1);
    draw_text(10, 50, std::format("POT C: {}\n", static_cast<std::uint32_t>(pot_b)).c_str(), 1);
}

}