#include "display.h"
#include <stdlib.h>
#include "pico/stdlib.h"
#include "pico/binary_info.h"
#include "hardware/i2c.h"
#include "pico/util/queue.h"
#include "pico/multicore.h"
#include "pico/util/queue.h"

uint8_t display_buffer[OLED_BUF_LEN];

// ディスプレイコマンドqueue
queue_t command_queue;


void _d_putchar(char c, uint8_t col_x, uint8_t col_y, bool reverse)
{
    uint32_t offset = col_x * FONT_WIDTH + OLED_WIDTH * col_y + FONT_WIDTH;

    if( c < 0 ) return;
    if(col_x > 15 || col_y > 3) return;
    // フォントの縦横変換
    for(int i = 0; i < 8 ; i++ ) {
        volatile uint8_t f;
        f  = ((font8x8[(int)c][reverse ? 0 : 7] << (reverse ? 7 - i : i)) & 0x80) >> 0;
        f |= ((font8x8[(int)c][reverse ? 1 : 6] << (reverse ? 7 - i : i)) & 0x80) >> 1;
        f |= ((font8x8[(int)c][reverse ? 2 : 5] << (reverse ? 7 - i : i)) & 0x80) >> 2;
        f |= ((font8x8[(int)c][reverse ? 3 : 4] << (reverse ? 7 - i : i)) & 0x80) >> 3;
        f |= ((font8x8[(int)c][reverse ? 4 : 3] << (reverse ? 7 - i : i)) & 0x80) >> 4;
        f |= ((font8x8[(int)c][reverse ? 5 : 2] << (reverse ? 7 - i : i)) & 0x80) >> 5;
        f |= ((font8x8[(int)c][reverse ? 6 : 1] << (reverse ? 7 - i : i)) & 0x80) >> 6;
        f |= ((font8x8[(int)c][reverse ? 7 : 0] << (reverse ? 7 - i : i)) & 0x80) >> 7;
        display_buffer[offset - i] = f;
    }
}

void _d_putstr(char *str, uint8_t col_x, uint8_t col_y, bool reverse)
{
    int i = 0;
    while(str[i] != '\0') {
        _d_putchar(str[i++], col_x++, col_y, reverse);
    }
}

void _d_clear(void)
{
    fill(display_buffer, 0x00);
}

bool display_putstr(char *str, uint8_t col_x, uint8_t col_y, bool reverse)
{
    display_cmd_t       cmd;
    putstr_t    *p;

    cmd.command = DISPLAY_CMD_PUTSTR;

    p = (putstr_t *)malloc(sizeof(putstr_t));
    p->col = col_x;
    p->row = col_y;
    p->reverse = reverse;
    strcpy(p->str, str);
    cmd.data = (void *)p;

    return queue_try_add(&command_queue, &cmd);
}

bool display_clear(void)
{
    display_cmd_t cmd;
    cmd.command = DISPLAY_CMD_CLEAR;
    return queue_try_add(&command_queue, &cmd);
}

// ゴミ捨て
inline void disposer(void *garbage)
{
    multicore_fifo_push_blocking((uint32_t)garbage);
}

void display_main(void)
{
    // I2C初期化
    i2c_init(i2c_default, 800 * 1000);
    // I2Cピン初期化
    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);

    // OLED初期化
    oled_init();

    struct render_area frame_area = {start_col: 0, 
                                    end_col : OLED_WIDTH - 1, 
                                    start_page : 0, 
                                    end_page : OLED_NUM_PAGES - 1, 
                                    buflen : 0 }; 
    calc_render_area_buflen(&frame_area);
    // ディスプレイクリア
    fill(display_buffer, 0x00);

    // ディスプレイメインループ
    while(true) {
        display_cmd_t cmd;

        render(display_buffer, &frame_area);
        while(! queue_is_empty(&command_queue)) {
            queue_remove_blocking(&command_queue, &cmd);
            switch(cmd.command) {
             case DISPLAY_CMD_CLEAR:
                _d_clear();
                break;
                
             case DISPLAY_CMD_PUTSTR:
                putstr_t *p = (putstr_t *)cmd.data;
                _d_putstr(p->str, p->col, p->row, p->reverse);
                disposer((void *)p);
                break;
                
             default:
                break;
            };
        }
        sleep_us(4*1000);
    }
}

// FIFO割り込み
void sio_irq_handler(void)
{
    while(multicore_fifo_rvalid()) {
        void *garbage = (void *)multicore_fifo_pop_blocking();
        free(garbage);
    }
    multicore_fifo_clear_irq();
}

// ディスプレイサブシステム初期化
void display_init(void)
{
    //queue初期化
    queue_init(&command_queue, sizeof(display_cmd_t), 8);
    // display_main起動
    multicore_launch_core1(display_main);
    // FIFO割り込みEnable
    irq_set_exclusive_handler(SIO_FIFO_IRQ_NUM(0), sio_irq_handler);
    irq_set_enabled(SIO_FIFO_IRQ_NUM(0), true);
}