#include <stdio.h>
#include <pico.h>
#include "pico/stdlib.h"
#include "hardware/clocks.h"
#include "pico/stdlib.h"
#include "pico/binary_info.h"
#include "hardware/i2c.h"
#include "pico/multicore.h"
#include "hardware/irq.h"
#include "hardware/adc.h"
#include "hardware/clocks.h"
#include "display.h"
#include "sd1306.h"

#define ADC_BUF_LEN 0x100
#define ADC_SAMPLE_CLOCK   3000     // 3000 sps
#define HEARTBEAT_SAMPLE_CLOCK  50  // 50Hz

uint16_t adc_buffer[ADC_BUF_LEN];   // ADC用リングバッファ
volatile uint16_t read_pointer = 0;          // バッファ用ポインタ
volatile uint16_t write_pointer = 0;

// ADC割り込み
void adc_handler(void)
{
    #define LOCAL_BUFFER_SIZE   (ADC_SAMPLE_CLOCK / HEARTBEAT_SAMPLE_CLOCK)
    static int local_counter = 0;
    static uint16_t local_buffer[LOCAL_BUFFER_SIZE];

    while(! adc_fifo_is_empty()) {
        local_buffer[local_counter++] = (adc_fifo_get() & 0xFFF);
        if(local_counter >= LOCAL_BUFFER_SIZE) {
            uint32_t total = 0;

            // 平均を取る
            for(int i = 0 ; i < local_counter; i++ )
                total += local_buffer[i];
            adc_buffer[write_pointer++] = (uint16_t)(total / local_counter);
            write_pointer &= ADC_BUF_LEN - 1;
            local_counter = 0;
        }
    }
}

int main(void)
{
    uint32_t adc_clk;

    stdio_init_all();

    // Display初期化
    multicore_reset_core1();
    multicore_launch_core1(display);

    // ADC初期化
    adc_init();
    adc_gpio_init(26);
    adc_select_input(0);
    adc_fifo_setup(true, false, 1, true, false);
    // ADCサンプリングクロック設定
    adc_clk = clock_get_hz(clk_adc);
    adc_set_clkdiv(adc_clk/ADC_SAMPLE_CLOCK);
    // ADC割り込み
	irq_set_exclusive_handler(ADC_IRQ_FIFO, adc_handler);
	irq_set_enabled(ADC_IRQ_FIFO, true);
    // ADC割り込み有効化&ADCスタート
    adc_irq_set_enabled(true);
    adc_run(true);

    // メインループ
    #define HEARTBEAT_THRESHOLD    1430    // 心拍のしきい値
    #define HYSTERESIS  50                  // ヒステリシス
    #define HEARTBEAT_COUNT 10              // 基準心拍数
    #define LOWEST_VALUE    1240            // 約1V

    uint32_t last_time = 0;                 // 前回の時間
    uint32_t heartbeat_counter = 0;         // 脈拍カウンタ
    bool ex_threshold = false;              // しきい値フラグ

    while(true) {
        if(write_pointer != read_pointer) {
            uint16_t pos_y = 0;
            uint8_t pixel;
            uint16_t offset;
            uint16_t val = adc_buffer[read_pointer++] & 0xFFF;
            read_pointer &= ADC_BUF_LEN - 1;

            // 心拍センサー用の調整
            if( val > LOWEST_VALUE ) {      // ADCの値から約1Vを切り捨てる
                val -= LOWEST_VALUE;
            }
            else val = 1;
            // ドット位置の計算
            pos_y = (val * OLED_HEIGHT) / (4096 - LOWEST_VALUE) ;
            offset = (pos_y / 8) * OLED_WIDTH;
            pixel = 0x01 << (pos_y % 8);
            // ディスプレイバッファ横スクロール
            memmove(&(display_buffer[1]),&(display_buffer[0]), OLED_BUF_LEN-(1+24));
            display_buffer[0] = 0x0;
            display_buffer[OLED_WIDTH] = 0x0;
            display_buffer[OLED_WIDTH*2] = 0x0;
            display_buffer[OLED_WIDTH*3] = 0x0;
            // ドットを打つ
            display_buffer[offset] = pixel;

            // 心拍カウンタ
            if((val > HEARTBEAT_THRESHOLD) && !ex_threshold)
                ex_threshold = true;
            if(val < (HEARTBEAT_THRESHOLD - HYSTERESIS)){
                if(ex_threshold) {
                    heartbeat_counter++;
                    if(heartbeat_counter >= HEARTBEAT_COUNT) {
                        // 現在時刻をミリ秒で得る
                        uint32_t current_time =  to_ms_since_boot(get_absolute_time());
                        if(last_time == 0) {
                            last_time = current_time;
                        }
                        else {
                            // 脈拍数を3桁の数で表示する
                            uint32_t past_ms = current_time - last_time;
                            uint32_t heartrate = 60 * 1000 * HEARTBEAT_COUNT / past_ms;
                            char s[16];
                            sprintf(s, "%3d", (int)heartrate);
                            display_putchar(s[0], 15, 3, true);
                            display_putchar(s[1], 14, 3, true);
                            display_putchar(s[2], 13, 3, true);
                            last_time = current_time;
                        }
                        heartbeat_counter = 0;
                    }
                }
                ex_threshold = false;
            }
        }
    }
}