#include <stdio.h>
#include <string.h>

#include "pico/stdlib.h"
#include "pico/multicore.h"

#include "cvideo.h"

#include "font.xbm"

static video_framebuffer_t fb0, fb1;

#define START_X 35
#define END_X (VIDEO_WIDTH-40)
#define START_Y 20
#define END_Y (VIDEO_HEIGHT-40)

#define CHARS_PER_LINE 32
#define CHAR_LINES 20

static char text_mode_buffer[20][32];

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

static void set_bit(video_framebuffer_t fb, uint x, uint y, bool value) {
    if(x>=VIDEO_WIDTH || y>=VIDEO_HEIGHT)
        return;
    uint index_x = x / 32;
    uint pos_x = x % 32;

    uint flag = value << (31-pos_x);
    if(value)
        fb[y][index_x] |= flag;
    else
        fb[y][index_x] &= ~flag;
}

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

void draw_character(video_framebuffer_t fb, unsigned int x, unsigned int y, unsigned int scale, char character) {
    uint8_t row = character / 16;
    uint8_t column = character % 16;
    uint32_t font_x = column * 10;
    uint32_t font_y = row * 12;

    for (int j = font_y; j < font_y + CHAR_HEIGHT; j++) {
        for  (int i = font_x; i <  font_x + CHAR_WIDTH; i++) {
            // XBM images pad rows with zeros
            uint32_t array_position;
            if (font_width %8 != 0) {
                array_position = (j * (font_width + (8 - font_width % 8))) + i;
            }
            else {
                array_position = (j * font_width) + i;
            }
            uint32_t array_index = array_position / 8;
            // XBM image, low bits are first
            uint32_t byte_position = array_position % 8;

            uint8_t value = font_bits[array_index] >> byte_position & 1 ;

            for (int sx = 0; sx < scale; sx++) {
                for (int sy = 0; sy < scale; sy++) {
                    set_bit(fb, x + ((i - font_x) * scale) + sx, y + ((j- font_y) * scale) + sy, !value);
                }
            }
        }
    }
}

void core1_entry() {
    gpio_init(25);
    gpio_set_dir(25, GPIO_OUT);
    while (true) {
        gpio_put(25, 1);
        sleep_ms(500);
        gpio_put(25, 0);
        sleep_ms(500);
    }
}


void framebuffer_switched_cb(){
}

void main() {
    stdio_init_all();
    multicore_launch_core1(core1_entry);

    clear(fb0);
    clear(fb1);
    video_init(fb0);
    video_set_framebuffer_switched_cb(framebuffer_switched_cb);

    int fbnum = 1;
    video_framebuffer_ptr_t fbs[] = {fb0, fb1};
    int i = 0;
    while (true) {
        clear(fbs[fbnum]);
        for(int y=0; y<20; y++){
            for(int x=0; x<32; x++){
                draw_character(fbs[fbnum], START_X+x*(CHAR_WIDTH+1)*2, START_Y+y*(CHAR_HEIGHT+1)*2, 2, '0'+(i+x+y)%10);
            }
        }

        video_set_framebuffer(fbs[fbnum]);
        fbnum = (fbnum+1)%2;
        i = (i+1)%10;
        sleep_ms(500);

        tight_loop_contents();
    }
}