#include <stdbool.h>

#include "main.h"
#include "usb_device.h"
#include "usbd_hid.h"
#include "assert.h"

#define  HID_MEDIA_REPORT  2
#define  HYSTERESIS 200

ADC_HandleTypeDef hadc;
DMA_HandleTypeDef hdma_adc;

void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void key_matrix_select(int row);
static int key_matrix_query(int debounce_time);
static uint32_t poll_encoders(void);
static uint32_t poll_keys(void);

enum keybits {
    /* These match up with the record descriptor in usbd_hid.c */
    KEYBITS_NEXT        = 0x01,
    KEYBITS_PREV        = 0x02,
    KEYBITS_STOP        = 0x04,
    KEYBITS_PLAY_PAUSE  = 0x08,
    KEYBITS_MUTE        = 0x10,
    KEYBITS_VOL_UP      = 0x20,
    KEYBITS_VOL_DOWN    = 0x40,
};

enum key_names {
    KEY_2   = 0,
    KEY_3   = 1,
    KEY_1   = 2,
    KEY_ENC = 3,
    KEY_4   = 4
};

enum keymap_rows {
    KEYMAP_BOTTOM_LEFT_ENC = 0,
    KEYMAP_TOP_RIGHT_ENC = 1,
};

enum key_matrix_params {
    KEY_MATRIX_ROWS = 5,
    KEY_MATRIX_COLS = 2,
};

void sendKeybits(uint8_t keybits);

struct key_t
{
    uint8_t id;
    uint8_t modifier;
    uint8_t reserved;
    uint8_t keycode[6];
} key;

uint16_t ADCreg[8];
uint16_t ADCval[8];
uint16_t ADClast[8];

/* FIXME debug remove */
TIM_TypeDef *tim1 = TIM1;
TIM_TypeDef *tim3 = TIM3;

int main(void)
{
    HAL_Init();

    SystemClock_Config();

    MX_GPIO_Init();
    MX_USB_HID_INIT();

    __HAL_RCC_TIM1_CLK_ENABLE();
    __HAL_RCC_TIM3_CLK_ENABLE();

    TIM1->SMCR = 3;          // Encoder mode 3
    TIM1->CCER = 0;          // rising edge polarity
    TIM1->ARR = 0xFFFF;      // count from 0-ARR or ARR-0
    TIM1->CCMR1 = 0x0101;    // f_DTS/16, N=8, IC1->TI1, IC2->TI2
    TIM1->CNT = 0;           // Initialize counter
    TIM1->EGR = 1;           // Generate an update event
    TIM1->CR1 = 1;           // Enable the counter

    TIM3->SMCR = 3;          // Encoder mode 3
    TIM3->CCER = 0;          // rising edge polarity
    TIM3->ARR = 0xFFFF;      // count from 0-ARR or ARR-0
    TIM3->CCMR1 = 0x0101;    // f_DTS/16, N=8, IC1->TI1, IC2->TI2
    TIM3->CNT = 0;           // Initialize counter
    TIM3->EGR = 1;           // Generate an update event
    TIM3->CR1 = 1;           // Enable the counter

    while (1) {
        uint32_t keybits = poll_encoders();
        for (int i=0; i<10; i++) {
            keybits |= poll_keys();
            HAL_Delay(1);
        }
        sendKeybits(keybits);
    }
}

static uint32_t poll_encoders() {
    static bool tx_vol_reset = 0;
    static uint16_t tim1_last = 0, tim3_last = 0; /* timers init to 0 */
    static int vol_delta = 0;

    uint16_t tim1_now = TIM1->CNT, tim3_now = TIM3->CNT;
    int16_t tim1_delta = (int16_t)(tim1_now - tim1_last);
    int16_t tim3_delta = (int16_t)(tim3_now - tim3_last);

    /* Gang both encoders */
    vol_delta += tim3_delta - tim1_delta;

#define VOL_DELTA_INC 4
    uint8_t keybits = 0;
    if (!tx_vol_reset) {
        /* Customize encoder action here */
        if (vol_delta >= VOL_DELTA_INC) {
            keybits |= KEYBITS_VOL_UP;
            vol_delta -= VOL_DELTA_INC;
            tx_vol_reset = 1;
        } else if (vol_delta <= -VOL_DELTA_INC) {
            keybits |= KEYBITS_VOL_DOWN;
            vol_delta += VOL_DELTA_INC;
            tx_vol_reset = 1;
        }
    } else {
        tx_vol_reset = 0;
    }

    tim1_last = tim1_now;
    tim3_last = tim3_now;
    return keybits;
}

static uint32_t poll_keys() {
    int debounce_time = 5; /* 5 * 10 ms loop timing increments */
    uint32_t val = key_matrix_query(debounce_time);
    uint32_t state = val&0xffff, edges = val>>16;
    uint32_t pressed = state & edges;
    (void)edges; /* unused */
    uint32_t keybits = 0;

    if (pressed & (1 << KEY_3))
        keybits |= KEYBITS_PLAY_PAUSE;
    if (pressed & (1 << KEY_3 << KEY_MATRIX_ROWS))
        keybits |= KEYBITS_PREV;
    if (pressed & (1 << KEY_4 << KEY_MATRIX_ROWS))
        keybits |= KEYBITS_NEXT;
    if (pressed & (1 << KEY_ENC << KEY_MATRIX_ROWS))
        keybits |= KEYBITS_MUTE;

    return keybits;
}

void sendKeybits(uint8_t keybits){
    uint8_t report[2];
    report[0]= HID_MEDIA_REPORT;
    report[1]= keybits;
    USBD_HID_SendReport(&hUsbDeviceFS, report, 2);
}

void SystemClock_Config(void)
{
    RCC_OscInitTypeDef RCC_OscInitStruct = {0};
    RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
    RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI14|RCC_OSCILLATORTYPE_HSI48;
    RCC_OscInitStruct.HSI48State = RCC_HSI48_ON;
    RCC_OscInitStruct.HSI14State = RCC_HSI14_ON;
    RCC_OscInitStruct.HSI14CalibrationValue = 16;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
    HAL_RCC_OscConfig(&RCC_OscInitStruct);

    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
        |RCC_CLOCKTYPE_PCLK1;
    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI48;
    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
    HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1);

    PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USB;
    PeriphClkInit.UsbClockSelection = RCC_USBCLKSOURCE_HSI48;
    HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit);

}

static void MX_GPIO_Init(void) {
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();

    /* Left  encoder (SW2) A/B inputs: PA9/PA8 (TIM1 CH1/2)
     * Right encoder (SW7) A/B inputs: PB4/PB5 (TIM3 CH1/2)
     * Key matrix:  PB2  -> encoder switches left/right
     *              PB8  -> SW3/8
     *              PB1  -> SW4/9
     *              PA15 -> SW5/10
     *              PA0  -> SW6/11
     *              key matrix inputs: PA1/PA4
     *
     * Physical key layout:
     *
     * +------------|USB|------------+
     * | SW9     SW11    SW10    SW8 |
     * |     SW2             SW7     |
     * | SW4     SW6     SW5     SW3 |
     * +-----------------------------+
     *
     * Logical key layout:
     *
     * +------------|USB|------------+
     * |  KT1    KT2     KT3    KT4  |
     * |      EL             ER      |
     * |  KB1    KB2     KB3    KB4  |
     * +-----------------------------+
     *
     */
    GPIOA->MODER = 0x28000000 | (2<<(8*2)) | (2<<(9*2)) | (1<<(15*2))| (1<<(0*2));
    GPIOB->MODER =              (2<<(4*2)) | (2<<(5*2)) | (1<<(1*2)) | (1<<(2*2)) | (1<<(8*2));
    GPIOA->PUPDR = 0x24000000 | (1<<(8*2)) | (1<<(9*2)) | (2<<(1*2)) | (2<<(4*2));
    GPIOB->PUPDR =              (1<<(4*2)) | (1<<(5*2));
    GPIOA->AFR[1]=              0x00000022;
    GPIOB->AFR[0]=              0x00110000;
}

static void key_matrix_select(int row) {
    uint16_t bsrr_a = 0, bsrr_b = 0;
    /* A0 -> A15 -> B1 -> B2 -> B8 */
    switch (row) {
        case 0: bsrr_a = 1<<0; break;
        case 1: bsrr_a = 1<<15; break;
        case 2: bsrr_b = 1<<1; break;
        case 3: bsrr_b = 1<<2; break;
        case 4: bsrr_b = 1<<8; break;
    }
    uint16_t mask_a = (1<<15) | (1<<0), mask_b = (1<<1) | (1<<2) | (1<<8);
    /* Reset all pins except for selected pin */
    GPIOA->BSRR = (mask_a<<16) ^ ((bsrr_a<<16) | bsrr_a) ;
    GPIOB->BSRR = (mask_b<<16) ^ ((bsrr_b<<16) | bsrr_b) ;
}

static int key_matrix_query(int debounce_time) {
    static int debounce_states[KEY_MATRIX_COLS][KEY_MATRIX_ROWS] = {0};
    static int key_matrix_row = -1;
    static uint32_t matrix_state = 0;
    uint32_t matrix_state_edges = 0;
    if (key_matrix_row < 0) { /* On first iteration just set outputs and return */
        key_matrix_row = 0;
        key_matrix_select(0);
        return 0;
    }

    int input = GPIOA->IDR;
    int pressed[KEY_MATRIX_COLS] = { input & (1<<4), input & (1<<1) };
    for (int i=0; i<KEY_MATRIX_COLS; i++) {
        if (debounce_states[i][key_matrix_row] > 0) {
            /* debounce timer running */
            debounce_states[i][key_matrix_row]--;
        } else {
            uint32_t bit = 1 << key_matrix_row << (KEY_MATRIX_ROWS * i); 
            uint32_t old_matrix_state = matrix_state;
            if (pressed[i])
                matrix_state |= bit;
            else
                matrix_state &= ~bit;

            uint32_t edge = old_matrix_state ^ matrix_state;
            if (edge)
                debounce_states[i][key_matrix_row] = debounce_time;
            matrix_state_edges |= edge;
        }
    }

    key_matrix_row = (key_matrix_row+1) % KEY_MATRIX_ROWS;
    key_matrix_select(key_matrix_row);
    return (matrix_state_edges<<16) | matrix_state;
}

void Error_Handler(void)
{
    while(1){
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,1);
        HAL_Delay(100);
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_13,0);
        HAL_Delay(100);
    }
}

void __libc_init_array(void);
void __libc_init_array() {
    /* FIXME Do we even need this? */
}

void _init(void);
void _init() {
    /* FIXME Do we even need this? */
}

void __assert_func (const char *file, int line, const char * func, const char * exp) {
    asm volatile ("bkpt");
    while (1);
}