#include "bottom_half.h"

static const char *TAG = "bottom_half";

static uint16_t keypad_state;
static uint16_t button_state;
static uint8_t touch_state;

static uint16_t keypad_pressed;
static uint16_t button_pressed;
static uint8_t touch_pressed;

static uint16_t keypad_released;
static uint16_t button_released;
static uint8_t touch_released;

/// read buffer
static uint8_t buf[8];

void clear_all_pressed_released(void) {
    keypad_pressed = 0;
    button_pressed = 0;
    touch_pressed = 0;
    
    keypad_released = 0;
    button_released = 0;
    touch_released = 0;
}

static bool _take_key(KeypadKey* kp, uint16_t* keypad_bitfield) {
    for (int i = 0; i < 16; i++) {
        int bit_selector = (1 << i);
        if ((*keypad_bitfield) & bit_selector) {
            if (kp != nullptr) {
                *kp = (KeypadKey) i;
            }
            // clear bit
            *keypad_bitfield &= ~bit_selector;
            return true;
        }
    }
    return false;
}

bool get_pressed_keypad(KeypadKey* kp) {
    return _take_key(kp, &keypad_pressed);
}
bool get_released_keypad(KeypadKey* kp) {
    return _take_key(kp, &keypad_released);
}

char char_of_keypad_key(KeypadKey kp) {
    switch (kp) {
        case KeypadKey::k1:
            return '1';
        case KeypadKey::k2:
            return '2';
        case KeypadKey::k3:
            return '3';
        case KeypadKey::k4:
            return '4';
        case KeypadKey::k5:
            return '5';
        case KeypadKey::k6:
            return '6';
        case KeypadKey::k7:
            return '7';
        case KeypadKey::k8:
            return '8';
        case KeypadKey::k9:
            return '9';
        case KeypadKey::k0:
            return '0';
        case KeypadKey::ka:
            return 'A';
        case KeypadKey::kb:
            return 'B';
        case KeypadKey::kc:
            return 'C';
        case KeypadKey::kd:
            return 'D';
        case KeypadKey::star:
            return '*';
        case KeypadKey::pound:
            return '#';
        default:
            return ' ';
    }
}

static bool _take_button(ButtonKey* button, uint16_t* button_bitfield) {
    for (int i = 0; i < 4; i++) {
        int bit_selector = (1 << (i+8));
        if ((*button_bitfield) & bit_selector) {
            if (button != nullptr) {
                *button = (ButtonKey) i;
            }
            // clear bit
            *button_bitfield &= ~bit_selector;
            return true;
        }
    }
    return false;
}
static bool _take_switch(SwitchKey* switch_, uint16_t* button_bitfield) {
    for (int i = 0; i < 4; i++) {
        int bit_selector = (1 << i);
        if ((*button_bitfield) & bit_selector) {
            if (switch_ != nullptr) {
                *switch_ = (SwitchKey) i;
            }
            // clear bit
            *button_bitfield &= ~bit_selector;
            return true;
        }
    }
    return false;
}

bool get_pressed_button(ButtonKey* button) {
    return _take_button(button, &button_pressed);
}
bool get_released_button(ButtonKey* button) {
    return _take_button(button, &button_released);
}

uint8_t get_button_state() {
    return (uint8_t)((button_state >> 8) & 0xF);
}

bool get_flipped_up_switch(SwitchKey* switch_) {
    return _take_switch(switch_, &button_released);
}
bool get_flipped_down_switch(SwitchKey* switch_) {
    return _take_switch(switch_, &button_released);
}
bool get_flipped_switch(SwitchKey* switch_) {
    for (int i = 0; i < 4; i++) {
        int bit_selector = (1 << (i+4));
        if (button_pressed & bit_selector || button_released & bit_selector) {
            if (switch_ != nullptr) {
                *switch_ = (SwitchKey) i;
            }
            // clear bit
            button_pressed &= ~bit_selector;
            button_released &= ~bit_selector;
            return true;
        }
    }
    return false;
}

uint8_t get_switch_state() {
    return (uint8_t)(~button_state & 0xF);
}

bool get_touch_state(void) {
    return touch_state;
}
bool get_touch_pressed(void) {
    bool return_ = touch_pressed;
    touch_pressed = false;
    return return_;
}
bool get_touch_released(void) {
    bool return_ = touch_released;
    touch_released = false;
    return return_;
}

static void poll_bottom_task(void *arg);
static void receive_keypad(void);
static void receive_button(void);
static void receive_touch(void);

// static void IRAM_ATTR gpio_isr_handler(void* arg)
// {
//     // TODO: do somthing
//     ESP_LOGI("BOTTOM_HALF", "Hit");
// }

void init_bottom_half() {
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = GPIO_NUM_5,
        .scl_io_num = GPIO_NUM_6,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master = {
            .clk_speed = 100000,
        }
    };

    ESP_ERROR_CHECK(i2c_param_config(BOTTOM_I2C_NUM, &conf));
    ESP_ERROR_CHECK(i2c_driver_install(BOTTOM_I2C_NUM, conf.mode, 0, 0, 0));

    gpio_config_t delta_pin_conf = {};
    // delta_pin_conf.intr_type = GPIO_INTR_LOW_LEVEL;
    delta_pin_conf.mode = GPIO_MODE_INPUT;
    // GPIO 0
    delta_pin_conf.pin_bit_mask = (1ULL<<BOTTOM_PIN_INTERUPT);
    delta_pin_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    gpio_config(&delta_pin_conf);

    //install gpio isr service
    // gpio_install_isr_service(0);
    //hook isr handler for specific gpio pin
    // gpio_isr_handler_add(BOTTOM_INTERUPT_PIN, gpio_isr_handler, NULL);

    receive_keypad();
    receive_button();
    receive_touch();

    xTaskCreate(poll_bottom_task, "poll_bottom", 4096, NULL, 10, NULL);
}

static uint8_t receive_delta(void) {
    uint8_t reg = 1;
    buf[0] = 0;
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_write_read_device(BOTTOM_I2C_NUM, BOTTOM_I2C_ADDR, &reg, 1, buf, 1, (100 / portTICK_PERIOD_MS)));
    return buf[0];
}

static void receive_keypad(void) {
    uint8_t reg = 2;
    buf[0] = 0;
    buf[1] = 0;
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_write_read_device(BOTTOM_I2C_NUM, BOTTOM_I2C_ADDR, &reg, 1, buf, 2, (100 / portTICK_PERIOD_MS)));
    uint16_t new_keypad_state = buf[0] | (buf[1] << 8);

    uint16_t just_pressed = new_keypad_state & ~keypad_state;
    keypad_pressed |= just_pressed;

    uint16_t just_released = ~new_keypad_state & keypad_state;
    keypad_released |= just_released;

    keypad_state = new_keypad_state;
}

static void receive_button(void) {
    uint8_t reg = 3;
    buf[0] = 0;
    buf[1] = 0;
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_write_read_device(BOTTOM_I2C_NUM, BOTTOM_I2C_ADDR, &reg, 1, buf, 2, (100 / portTICK_PERIOD_MS)));
    uint16_t new_button_state = buf[0] | (buf[1] << 8);

    uint16_t just_pressed = new_button_state & ~button_state;
    button_pressed |= just_pressed;

    uint16_t just_released = ~new_button_state & button_state;
    button_released |= just_released;
    
    button_state = new_button_state;
}

static void receive_touch(void) {
    uint8_t reg = 4;
    buf[0] = 0;
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_write_read_device(BOTTOM_I2C_NUM, BOTTOM_I2C_ADDR, &reg, 1, buf, 1, (100 / portTICK_PERIOD_MS)));
    bool new_touch_state = buf[0] != 0;

    bool just_pressed = new_touch_state & !touch_state;
    touch_pressed |= just_pressed;

    bool just_released = (!new_touch_state) & touch_state;
    touch_released |= just_released;
    
    touch_state = new_touch_state;
}

static void poll_bottom_task(void *arg) {
    while (1) {
        bool new_data = gpio_get_level(BOTTOM_PIN_INTERUPT) == 0;
        if (new_data) {
            uint8_t delta = receive_delta();
            // ESP_LOGI(_TAG, "delta: %d", delta);
            if (delta == 0) ESP_LOGW(TAG, "delta pin was low, but delta register returned 0");

            if (delta & (1 << DELTA_BIT_KP)) receive_keypad();
            if (delta & (1 << DELTA_BIT_BUTTON)) receive_button();
            if (delta & (1 << DELTA_BIT_TOUCH)) receive_touch();
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }

    vTaskDelete(NULL);
}