#include <stdbool.h>

#include "main.h"
#include "usb_device.h"
#include "usbd_hid.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);
    }
}

void * _sbrk(ptrdiff_t __incr);
void * _sbrk(ptrdiff_t __incr) {
    /* FIXME Do we even need this? */
    return NULL;
}

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

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;
}

const uint8_t _asciimap[128] =
{
    0x00,             // NUL
    0x00,             // SOH
    0x00,             // STX
    0x00,             // ETX
    0x00,             // EOT
    0x00,             // ENQ
    0x00,             // ACK
    0x00,             // BEL
    0x2a,           // BS   Backspace
    0x2b,           // TAB  Tab
    0x28,           // LF   Enter
    0x00,             // VT
    0x00,             // FF
    0x00,             // CR
    0x00,             // SO
    0x00,             // SI
    0x00,             // DEL
    0x00,             // DC1
    0x00,             // DC2
    0x00,             // DC3
    0x00,             // DC4
    0x00,             // NAK
    0x00,             // SYN
    0x00,             // ETB
    0x00,             // CAN
    0x00,             // EM
    0x00,             // SUB
    0x00,             // ESC
    0x00,             // FS
    0x00,             // GS
    0x00,             // RS
    0x00,             // US

    0x2c,          //  ' '
    0x1e|0x80,    // !
    0x34|0x80,    // "
    0x20|0x80,    // #
    0x21|0x80,    // $
    0x22|0x80,    // %
    0x24|0x80,    // &
    0x34,          // '
    0x26|0x80,    // (
    0x27|0x80,    // )
    0x25|0x80,    // *
    0x2e|0x80,    // +
    0x36,          // ,
    0x2d,          // -
    0x37,          // .
    0x38,          // /
    0x27,          // 0
    0x1e,          // 1
    0x1f,          // 2
    0x20,          // 3
    0x21,          // 4
    0x22,          // 5
    0x23,          // 6
    0x24,          // 7
    0x25,          // 8
    0x26,          // 9
    0x33|0x80,      // :
    0x33,          // ;
    0x36|0x80,      // <
    0x2e,          // =
    0x37|0x80,      // >
    0x38|0x80,      // ?
    0x1f|0x80,      // @
    0x04|0x80,      // A
    0x05|0x80,      // B
    0x06|0x80,      // C
    0x07|0x80,      // D
    0x08|0x80,      // E
    0x09|0x80,      // F
    0x0a|0x80,      // G
    0x0b|0x80,      // H
    0x0c|0x80,      // I
    0x0d|0x80,      // J
    0x0e|0x80,      // K
    0x0f|0x80,      // L
    0x10|0x80,      // M
    0x11|0x80,      // N
    0x12|0x80,      // O
    0x13|0x80,      // P
    0x14|0x80,      // Q
    0x15|0x80,      // R
    0x16|0x80,      // S
    0x17|0x80,      // T
    0x18|0x80,      // U
    0x19|0x80,      // V
    0x1a|0x80,      // W
    0x1b|0x80,      // X
    0x1c|0x80,      // Y
    0x1d|0x80,      // Z
    0x2f,          // [
    0x31,          // bslash
    0x30,          // ]
    0x23|0x80,    // ^
    0x2d|0x80,    // _
    0x35,          // `
    0x04,          // a
    0x05,          // b
    0x06,          // c
    0x07,          // d
    0x08,          // e
    0x09,          // f
    0x0a,          // g
    0x0b,          // h
    0x0c,          // i
    0x0d,          // j
    0x0e,          // k
    0x0f,          // l
    0x10,          // m
    0x11,          // n
    0x12,          // o
    0x13,          // p
    0x14,          // q
    0x15,          // r
    0x16,          // s
    0x17,          // t
    0x18,          // u
    0x19,          // v
    0x1a,          // w
    0x1b,          // x
    0x1c,          // y
    0x1d,          // z
    0x2f|0x80,    // {
    0x31|0x80,    // |
    0x30|0x80,    // }
    0x35|0x80,    // ~
    0               // DEL
};


void sendChar(uint8_t ch){
    if( ch > 128 ) ch -=128;

    key.id = 1;
    key.keycode[0]=_asciimap[ch]&0x7F;
    key.keycode[1]=0;

    if ( _asciimap[ch] & 0x80) key.modifier |= 0x02;

    for(int i=0; i< 4;i++){
        USBD_HID_SendReport(&hUsbDeviceFS, (uint8_t *)&key, sizeof(key));
        HAL_Delay(10);
    }
    memset(key.keycode, 0 , sizeof(key.keycode));
    key.modifier = 0;
    USBD_HID_SendReport(&hUsbDeviceFS, (uint8_t *)&key, sizeof(key));
    HAL_Delay(10);
}

void sendCharWrong(uint8_t ch){
    key.id = 1;
    key.keycode[0]=ch&0x7F;
    key.keycode[1]=0;

    if ( _asciimap[ch] & 0x80) key.modifier |= 0x02;

    for(int i=0; i< 4;i++){
        USBD_HID_SendReport(&hUsbDeviceFS, (uint8_t *)&key, sizeof(key));
        HAL_Delay(10);
    }
    memset(key.keycode, 0 , sizeof(key.keycode));
    key.modifier = 0;
    USBD_HID_SendReport(&hUsbDeviceFS, (uint8_t *)&key, sizeof(key));
    HAL_Delay(10);
}

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);
    }
}