#include "text_mode.h"

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

#include "font/ucs_6x13.xbm"

static void clear(video_framebuffer_t fb){
#if TEXT_INVERT_COLOURS
    memset(fb, 0xff, sizeof(video_framebuffer_t));
#else
    memset(fb, 0, sizeof(video_framebuffer_t));
#endif
}

#define SOURCE_GLYPH_WIDTH (font_width / 16)
#define SOURCE_GLYPH_HEIGHT (font_height / 16)
#define GLYPH_CROP_LEFT (GLYPH_PADDING_X / 2)
#define GLYPH_CROP_RIGHT (GLYPH_PADDING_X - GLYPH_CROP_LEFT)
#define GLYPH_CROP_TOP (GLYPH_PADDING_Y / 2)
#define CHAR_WIDTH (SOURCE_GLYPH_WIDTH - GLYPH_PADDING_X)
#define CHAR_HEIGHT (SOURCE_GLYPH_HEIGHT - GLYPH_PADDING_Y)
#define TEXT_MODE_WIDTH \
    (((CHARS_PER_LINE - 1) * (CHAR_WIDTH + 1) + CHAR_WIDTH) * CHAR_SCALE)
#define TEXT_MODE_HEIGHT \
    (((CHAR_LINES - 1) * (CHAR_HEIGHT + 1) + CHAR_HEIGHT) * CHAR_SCALE)

_Static_assert(CHAR_SCALE > 0, "CHAR_SCALE must be at least 1");
_Static_assert(GLYPH_PADDING_X >= 0 && GLYPH_PADDING_X < SOURCE_GLYPH_WIDTH,
               "GLYPH_PADDING_X must leave at least one glyph pixel");
_Static_assert(GLYPH_PADDING_Y >= 0 && GLYPH_PADDING_Y < SOURCE_GLYPH_HEIGHT,
               "GLYPH_PADDING_Y must leave at least one glyph pixel");
_Static_assert(TEXT_MARGIN_LEFT >= 0 && TEXT_MARGIN_TOP >= 0,
               "text margins cannot be negative");
_Static_assert(TEXT_MARGIN_LEFT + TEXT_MODE_WIDTH <= VIDEO_WIDTH,
               "left margin, CHAR_SCALE, and CHARS_PER_LINE do not fit");
_Static_assert(TEXT_MARGIN_TOP + TEXT_MODE_HEIGHT <= VIDEO_HEIGHT,
               "top margin, CHAR_SCALE, and CHAR_LINES do not fit");

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 * SOURCE_GLYPH_WIDTH;
        uint32_t font_y = row * SOURCE_GLYPH_HEIGHT;

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

            for (int i = 0; i < SOURCE_GLYPH_WIDTH; ++i) {
                uint32_t src_x = font_x + i;
                uint32_t src_y = font_y + j + GLYPH_CROP_TOP;
                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 << (SOURCE_GLYPH_WIDTH - 1 - i));
                }
            }

            packed = (uint16_t)((packed >> GLYPH_CROP_RIGHT) &
                                ((1u << CHAR_WIDTH) - 1u));
            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);
#if TEXT_INVERT_COLOURS
    fb[y][word] &= ~((uint32_t)(aligned >> 32));
#else
    fb[y][word] |= (uint32_t)(aligned >> 32);
#endif
    if (shift + width > 32u && word + 1u < WORDS_PER_LINE) {
#if TEXT_INVERT_COLOURS
        fb[y][word + 1u] &= ~((uint32_t)aligned);
#else
        fb[y][word + 1u] |= (uint32_t)aligned;
#endif
    }
}

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);
#if TEXT_INVERT_COLOURS
                            fb[py][word] &= ~(1u << bit);
#else
                            fb[py][word] |= (1u << bit);
#endif
                        }
                    }
                }
            }
        }
    }
}

void draw_text_mode(video_framebuffer_t fb, text_mode_buffer_t buffer){
    clear(fb);
    for(int y=0; y<CHAR_LINES; y++){
        for(int x=0; x<CHARS_PER_LINE; x++){
            draw_character(fb,
                           TEXT_MARGIN_LEFT + x * (CHAR_WIDTH + 1) * CHAR_SCALE,
                           TEXT_MARGIN_TOP + y * (CHAR_HEIGHT + 1) * CHAR_SCALE,
                           CHAR_SCALE,
                           buffer[y][x]);
        }
    }
    video_set_framebuffer(fb);
}