#include "text_mode.h"

#include <stdint.h>
#include <string.h>
#include "pico/stdlib.h"

#include "font.xbm"

static void clear(video_framebuffer_t fb){
    memset(fb, 0, sizeof(video_framebuffer_t));
}

#define CHAR_WIDTH font_width / 16
#define CHAR_HEIGHT font_height / 16

static uint16_t glyph_rows[256][CHAR_HEIGHT];
static uint32_t glyph_rows_x2[256][CHAR_HEIGHT];

void init_font_cache() {
    for (int ch = 0; ch < 256; ++ch) {
        uint8_t row = ch / 16;
        uint8_t column = ch % 16;
        uint32_t font_x = column * CHAR_WIDTH;
        uint32_t font_y = row * CHAR_HEIGHT;

        for (int j = 0; j < CHAR_HEIGHT; ++j) {
            uint16_t packed = 0;

            for (int i = 0; i < CHAR_WIDTH; ++i) {
                uint32_t src_x = font_x + i;
                uint32_t src_y = font_y + j;
                uint32_t array_position;

                if ((font_width % 8) != 0) {
                    array_position = (src_y * (font_width + (8 - font_width % 8))) + src_x;
                } else {
                    array_position = (src_y * font_width) + src_x;
                }

                uint32_t array_index = array_position / 8;
                uint32_t byte_position = array_position % 8;
                uint8_t value = (font_bits[array_index] >> byte_position) & 1u;

                if (!value) {
                    packed |= (uint16_t)(1u << (CHAR_WIDTH - 1 - i));
                }
            }

            glyph_rows[ch][j] = packed;

            uint32_t packed_x2 = 0;
            for (int i = 0; i < CHAR_WIDTH; ++i) {
                if (packed & (1u << (CHAR_WIDTH - 1 - i))) {
                    packed_x2 |= (uint32_t)(3u << ((CHAR_WIDTH - 1 - i) * 2));
                }
            }
            glyph_rows_x2[ch][j] = packed_x2;
        }
    }
}

static inline void blit_row_bits(video_framebuffer_t fb, unsigned int x, unsigned int y, uint64_t bits, unsigned int width) {
    if (y >= VIDEO_HEIGHT || x >= VIDEO_WIDTH || width == 0) {
        return;
    }

    if (x + width > VIDEO_WIDTH) {
        unsigned int old_width = width;
        width = VIDEO_WIDTH - x;
        bits >>= (old_width - width);
    }

    uint word = x >> 5;
    uint shift = x & 31u;
    uint64_t aligned = bits << (64u - width - shift);
    fb[y][word] |= (uint32_t)(aligned >> 32);
    if (shift + width > 32u && word + 1u < WORDS_PER_LINE) {
        fb[y][word + 1u] |= (uint32_t)aligned;
    }
}

void draw_character(video_framebuffer_t fb, unsigned int x, unsigned int y, unsigned int scale, char character) {
    if(character==0)
        return;
    unsigned char glyph = (unsigned char)character;

    if (scale == 1) {
        for (int j = 0; j < CHAR_HEIGHT; ++j) {
            blit_row_bits(fb, x, y + j, glyph_rows[glyph][j], CHAR_WIDTH);
        }
        return;
    }

    if (scale == 2) {
        for (int j = 0; j < CHAR_HEIGHT; ++j) {
            uint32_t row_bits = glyph_rows_x2[glyph][j];
            blit_row_bits(fb, x, y + (j * 2u), row_bits, CHAR_WIDTH * 2u);
            blit_row_bits(fb, x, y + (j * 2u) + 1u, row_bits, CHAR_WIDTH * 2u);
        }
        return;
    }

    for (int j = 0; j < CHAR_HEIGHT; ++j) {
        uint16_t row_bits = glyph_rows[glyph][j];
        for (int sy = 0; sy < (int)scale; ++sy) {
            for (int i = 0; i < CHAR_WIDTH; ++i) {
                if (row_bits & (1u << (CHAR_WIDTH - 1 - i))) {
                    for (int sx = 0; sx < (int)scale; ++sx) {
                        unsigned int px = x + (unsigned int)(i * scale + sx);
                        unsigned int py = y + (unsigned int)(j * scale + sy);
                        if (px < VIDEO_WIDTH && py < VIDEO_HEIGHT) {
                            uint word = px >> 5;
                            uint bit = 31u - (px & 31u);
                            fb[py][word] |= (1u << bit);
                        }
                    }
                }
            }
        }
    }
}

void draw_text_mode(video_framebuffer_t fb, text_mode_buffer_t buffer){
    clear(fb);
    for(int y=0; y<20; y++){
        for(int x=0; x<32; x++){
            draw_character(fb, START_X+x*(CHAR_WIDTH+1)*2, START_Y+y*(CHAR_HEIGHT+1)*2, 2, buffer[y][x]);
        }
    }
    video_set_framebuffer(fb);
}