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

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

#include "cvideo.h"
#include "core_comm.h"
#include "text_mode.h"
#include "core1.h"


// --------------------------------
// CORE 0
// --------------------------------

static video_framebuffer_t fb0, fb1;
static video_framebuffer_t pixel_mode_buffer;
static text_mode_buffer_t text_mode_buffer;

static volatile bool framebuffer_switched = true;
static bool text_mode = true;
static bool text_mode_dirty = true;

void framebuffer_switched_cb(){
    framebuffer_switched = true;
}

static void text_mode_clear_buffer() {
    memset(text_mode_buffer, 0, sizeof(text_mode_buffer));
}

static void text_mode_scroll_up() {
    memmove(&text_mode_buffer[0][0],
            &text_mode_buffer[1][0],
            (CHAR_LINES - 1) * CHARS_PER_LINE * sizeof(text_mode_buffer[0][0]));
    memset(text_mode_buffer[CHAR_LINES - 1], 0, CHARS_PER_LINE * sizeof(text_mode_buffer[0][0]));
}

static void pixel_mode_write_byte(uint16_t x, uint16_t y, uint8_t pixels) {
    if (x >= VIDEO_WIDTH || y >= VIDEO_HEIGHT) {
        return;
    }

    for (uint bit_index = 0; bit_index < 8 && x + bit_index < VIDEO_WIDTH;
         bit_index++) {
        uint pixel_x = x + bit_index;
        uint word = pixel_x >> 5;
        uint bit = 31u - (pixel_x & 31u);
        uint32_t mask = 1u << bit;

        if ((pixels & (1u << (7u - bit_index))) != 0) {
            pixel_mode_buffer[y][word] |= mask;
        } else {
            pixel_mode_buffer[y][word] &= ~mask;
        }
    }
}

void main() {
    multicore_launch_core1(core1_entry);

    init_font_cache();

    video_set_framebuffer_switched_cb(framebuffer_switched_cb);
    video_init(fb0);

    text_mode_clear_buffer();
    memset(pixel_mode_buffer, 0, sizeof(pixel_mode_buffer));

    int fbnum = 1;
    video_framebuffer_ptr_t fbs[] = {fb0, fb1};
    while (true) {
        video_command_t cmd;
        while (core0_try_read_from_core1((uint64_t*)&cmd)) {
            switch(cmd.cmd){
                case VIDEO_COMMAND_MODE_SET:{
                    switch(cmd.data[0]){
                        case VIDEO_MODE_TEXT:
                            text_mode = true;
                            break;
                        case VIDEO_MODE_PIXEL:
                            text_mode = false;
                            break;
                        default:
                    }
                } break;
                case VIDEO_WRITE_TEXT:{
                    char c = (char)cmd.data[0];
                    int posx = ((int)cmd.data[1] >= CHARS_PER_LINE) ? CHARS_PER_LINE-1 : (int)cmd.data[1];
                    int posy = ((int)cmd.data[2] >= CHAR_LINES) ? CHAR_LINES-1 : (int)cmd.data[2];
                    text_mode_buffer[posy][posx] = c;
                } break;
                case VIDEO_WRITE_PIXEL:{
                    pixel_mode_write_byte(cmd.data[1], cmd.data[2],
                                          (uint8_t)cmd.data[0]);
                } break;
                case VIDEO_SCROLL_TEXT:{
                    text_mode_scroll_up();
                } break;
                case VIDEO_CLR_TEXT:{
                    text_mode_clear_buffer();
                } break;
                case VIDEO_CLR_PIXEL:{
                    memset(pixel_mode_buffer, 0, sizeof(pixel_mode_buffer));
                } break;
                default:
            }
        }

        // Check if need to draw new frame
        if(framebuffer_switched){
            if(text_mode){
                draw_text_mode(fbs[fbnum], text_mode_buffer);
            } else {
                memcpy(fbs[fbnum], pixel_mode_buffer,
                       sizeof(pixel_mode_buffer));
                video_set_framebuffer(fbs[fbnum]);
            }
            fbnum = (fbnum+1)%2;
            framebuffer_switched = false;
        }

        tight_loop_contents();
    }
}

// --------------------------------