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

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

#include "cvideo.h"
#include "text_mode.h"


// --------------------------------
// CORE 1
// --------------------------------

void core1_entry() {
    // Nothing yet
}

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


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

static video_framebuffer_t fb0, fb1;
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;
static uint console_row = 0;
static uint console_col = 0;

void framebuffer_switched_cb(){
    framebuffer_switched = true;
}

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

static void text_mode_scroll_up(void) {
    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 text_mode_newline(void) {
    console_col = 0;
    if (console_row + 1 >= CHAR_LINES) {
        text_mode_scroll_up();
    } else {
        console_row++;
    }
}

static void text_mode_put_char(char c) {
    if (c == '\r') {
        console_col = 0;
        return;
    }

    if (c == '\n') {
        text_mode_newline();
        return;
    }

    if (c == '\b' || c == 0x7f) {
        if (console_col > 0) {
            console_col--;
            text_mode_buffer[console_row][console_col] = 0;
        }
        return;
    }

    if ((unsigned char)c < 32 || (unsigned char)c > 126) {
        return;
    }

    text_mode_buffer[console_row][console_col] = c;

    console_col++;
    if (console_col >= CHARS_PER_LINE) {
        text_mode_newline();
    }
}

static void text_mode_puts(const char *s) {
    while (*s) {
        text_mode_put_char(*s++);
    }
}

static void poll_usb_console(void) {
    int ch;
    while ((ch = getchar_timeout_us(0)) != PICO_ERROR_TIMEOUT) {
        text_mode_put_char((char)ch);
        putchar(ch);
    }
}

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

    init_font_cache();

    video_set_framebuffer_switched_cb(framebuffer_switched_cb);
    video_init(fb0);

    text_mode_clear_buffer();
    text_mode_puts("+------------------------------+\r");
    text_mode_puts("|      Pico-PAL text mode      |\r");
    text_mode_puts("+------------------------------+\r");

    gpio_init(25);
    gpio_set_dir(25, GPIO_OUT);

    int fbnum = 1;
    video_framebuffer_ptr_t fbs[] = {fb0, fb1};
    while (true) {
        poll_usb_console();

        // Check if need to draw new frame
        if(framebuffer_switched){
            gpio_put(25, fbnum);

            if(text_mode){
                draw_text_mode(fbs[fbnum], text_mode_buffer);
            }

            fbnum = (fbnum+1)%2;
            framebuffer_switched = false;
        }

        tight_loop_contents();
    }
}

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