#include <stdint.h>
#include "keypad.h"
#include "gpio.h"
#include "RotaryCoder.h"

static uint8_t key_status = 0;
static uint16_t encoder_value = 0;
static GPIO_TypeDef *ports[] = {
    KEY_1_GPIO_Port,
    KEY_2_GPIO_Port,
    KEY_3_GPIO_Port,
    KEY_4_GPIO_Port,
    KEY_5_GPIO_Port,
    KEY_6_GPIO_Port,
    ENCODER_KEY_GPIO_Port
};
static uint16_t pins[] = {
    KEY_1_Pin,
    KEY_2_Pin,
    KEY_3_Pin,
    KEY_4_Pin,
    KEY_5_Pin,
    KEY_6_Pin,
    ENCODER_KEY_Pin
};
static uint8_t keys[] = {
    kp_KEY1,
    kp_KEY2,
    kp_KEY3,
    kp_KEY4,
    kp_KEY5,
    kp_KEY6,
    kp_KEYENCODER
};

#define K_GET(x) (((0b1 << x) & key_status) >> x)
#define K_SET0(x) (key_status = (~(0b1 << x)) & key_status)
#define K_SET1(x) (key_status = (0b1 << x) | key_status)
#define K_HGET(x) (HAL_GPIO_ReadPin(ports[x], pins[x]) == GPIO_PIN_RESET)
#define K_EVENT(event, value) ((value << 4) | event)

uint8_t kp_query() {
    // Scan keypad
    uint16_t rota_value = EC11_COUNT();
    uint16_t differ = rota_value - encoder_value;
    uint8_t direction = kp_ROTATE_LEFT;
    if (differ > 0) {
        if (differ > 32768) {
            differ = 0xFFFF - differ + 1;
            direction = kp_ROTATE_RIGHT;
        }
        differ = differ / 2; // filter 2x
    }
    if (differ > 0) {
        encoder_value = rota_value;
        return K_EVENT(direction, differ);
    }
    uint8_t state, mstate, key_n;
    for (key_n = 0; key_n < sizeof(keys); key_n ++) {
        state = K_HGET(key_n);
        mstate = K_GET(key_n);
        if (state && (!mstate)) {
            K_SET1(key_n);
            return K_EVENT(kp_KEY_DOWN, keys[key_n]);
        } else if ((!state) && mstate) {
            K_SET0(key_n);
            return K_EVENT(kp_KEY_UP, keys[key_n]);
        }
    }
    return K_EVENT(kp_NOP, 0);
}