#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "pico/multicore.h"
#include "keypad_scanner.pio.h"
#include "keypad.h"

volatile uint   write_p = 0;    // バッファ書き込みインデックス
volatile uint   read_p  = 0;    // バッファ読み出しインデックス

// キーバッファ
volatile key_t    keystate_buffer[KEYPAD_BUFFER_SIZE];
// LED
const uint LED_PIN = PICO_DEFAULT_LED_PIN;

// PIO割り込みハンドラ
void pio_irq_handler(void)
{
    static uint32_t prev_keycode = 0;
    uint32_t keycode;
    // IRQクリア
    pio_interrupt_clear(pio0, 0);

    while(! pio_sm_is_rx_fifo_empty(pio0, 0)) {
        keycode = pio_sm_get(pio0, 0);
        uint32_t changed_bit = keycode ^ prev_keycode;
        prev_keycode = keycode;

        for(uint16_t idx = 0; idx < 16; idx++) {
            if(changed_bit & 1) {
                keystate_buffer[write_p & 0xF].code = idx;
                keystate_buffer[write_p & 0xF].state = keycode & 1;
                gpio_put(LED_PIN, keystate_buffer[write_p & 0xF].state);
                write_p++;
            }
            changed_bit >>= 1;
            keycode >>= 1;
        }
    }
}

// キーパッド初期化
void keypad_init()
{
    // オンボードLED
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    gpio_put(LED_PIN, 0);

    // PIO
    PIO pio = pio0;
    // PIO割り込みの設定
    irq_set_exclusive_handler(PIO0_IRQ_0, pio_irq_handler);
    irq_set_enabled(PIO0_IRQ_0, true);
    pio_set_irq0_source_enabled(pio,pis_interrupt0, pio_irq_handler );
    uint offset = pio_add_program(pio, &keypad_scanner_program);
    keypad_scanner_program_init(pio, 0, offset, ROW_BASE, COLUMN_BASE, 16000);

    // SM起動
    pio_sm_set_enabled(pio, 0, true);
}

// キーバッファからデータを取り出す
key_t get_key(void)
{
    key_t   retval;
    uint wp;

    wp = write_p;
    if(wp != read_p) {
        retval.code = keystate_buffer[read_p & 0x0F].code;
        retval.state = keystate_buffer[read_p & 0xF].state;
        read_p++;
    }
    else {
        retval.code = 0;
        retval.state = KEYPAD_INVALID;
    }
    return retval;
}