#include "bottom_half.h"
#include <esp_log.h>

static const char *TAG = "bottom_half";

static uint16_t keypad_state;
static uint8_t button_state;
static uint8_t switch_state;
static uint8_t switch_touch_state;
static uint8_t touch_state;

static uint16_t keypad_pressed;
static uint8_t button_pressed;
static uint8_t switch_flipped_up;
static uint8_t switch_touch_pressed;
static uint8_t touch_pressed;

static uint16_t keypad_released;
static uint8_t button_released;
static uint8_t switch_flipped_down;
static uint8_t switch_touch_released;
static uint8_t touch_released;

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

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

// TODO: add intrupt on bottom half delta pin
// static void IRAM_ATTR gpio_isr_handler(void* arg)
// {
//     // TODO: do somthing
//     ESP_LOGI("BOTTOM_HALF", "Hit");
// }

void init_bottom_half() {
    ESP_LOGI(TAG, "Initializing bottom half...");

    ESP_ERROR_CHECK(gpio_set_direction(BOTTOM_PIN_INTERUPT, GPIO_MODE_INPUT));
    ESP_ERROR_CHECK(gpio_set_pull_mode(BOTTOM_PIN_INTERUPT, GPIO_PULLUP_ONLY));

    // TODO: do interupt stuff.
    // ESP_ERROR_CHECK(gpio_intr_enable(BOTTOM_PIN_INTERUPT));
    // ESP_ERROR_CHECK(gpio_install_isr_service(ESP_INTR_FLAG))
    //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_switch();
    receive_touch();

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

    ESP_LOGI(TAG, "Bottom half initialized!");
}

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_switch(void) {
    uint8_t reg = 3;
    ESP_ERROR_CHECK_WITHOUT_ABORT(i2c_master_write_read_device(BOTTOM_I2C_NUM, BOTTOM_I2C_ADDR, &reg, 1, buf, 2, (100 / portTICK_PERIOD_MS)));
    uint8_t new_button_state = buf[1] & 0xF;
    uint8_t new_switch_state = (~buf[0]) & 0xF;
    uint8_t new_switch_touch_state = (buf[1] >> 4) & 0xF;

    // button
    uint8_t just_pressed = new_button_state & ~button_state;
    button_pressed |= just_pressed;

    uint8_t just_released = ~new_button_state & button_state;
    button_released |= just_released;

    button_state = new_button_state;

    // switch
    uint8_t just_flipped_up = new_switch_state & ~switch_state;
    switch_flipped_up |= just_flipped_up;

    uint8_t just_flipped_down = ~new_switch_state & switch_state;
    switch_flipped_down |= just_flipped_down;
    
    switch_state = new_switch_state;

    // switch touch
    uint8_t touch_just_pressed = new_switch_touch_state & ~switch_touch_state;
    switch_touch_pressed |= touch_just_pressed;

    uint8_t touch_just_released = ~new_switch_touch_state & switch_touch_state;
    switch_touch_released |= touch_just_released;

    switch_touch_state = new_switch_touch_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) {
    // TODO: if using an interupt, switch this to use a queue
    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_SWITCH)) receive_button_switch();
            if (delta & (1 << DELTA_BIT_TOUCH)) receive_touch();
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }

    vTaskDelete(NULL);
}

void clear_all_pressed_released(void) {
    keypad_pressed = 0;
    button_pressed = 0;
    switch_flipped_up = 0;
    touch_pressed = 0;
    
    keypad_released = 0;
    button_released = 0;
    switch_flipped_down = 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_keypad_pressed(KeypadKey* kp) {
    return _take_key(kp, &keypad_pressed);
}
bool get_keypad_released(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, uint8_t* button_bitfield) {
    if (((*button_bitfield) & 0xF) == 0) return false;

    // scan the 4 button bits for one that is set
    for (int i = 0; i < 4; i++) {
        int bit_selector = (1 << i);
        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_, uint8_t* switch_bitfield) {
    if (((*switch_bitfield) & 0xF) == 0) return false;

    // scan the 4 switch bits for one that is set
    for (int i = 0; i < 4; i++) {
        int bit_selector = (1 << i);
        if ((*switch_bitfield) & bit_selector) {
            if (switch_ != nullptr) {
                *switch_ = (SwitchKey) i;
            }
            // clear bit
            *switch_bitfield &= ~bit_selector;
            return true;
        }
    }
    return false;
}

bool get_button_pressed(ButtonKey* button) {
    return _take_button(button, &button_pressed);
}
bool get_button_released(ButtonKey* button) {
    return _take_button(button, &button_released);
}
uint8_t get_button_state() {
    return button_state;
}

bool get_switch_flipped_up(SwitchKey* switch_) {
    return _take_switch(switch_, &switch_flipped_up);
}
bool get_switch_flipped_down(SwitchKey* switch_) {
    return _take_switch(switch_, &switch_flipped_down);
}
bool get_switch_flipped(SwitchKey* switch_) {
    for (int i = 0; i < 4; i++) {
        int bit_selector = (1 << i);
        if (
            ((switch_flipped_up & bit_selector) != 0) || 
            ((switch_flipped_down & bit_selector) != 0)
        ) {
            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 switch_state;
}

bool get_switch_touch_pressed(SwitchKey* switch_) {
    return _take_switch(switch_, &switch_touch_pressed);
}
bool get_switch_touch_released(SwitchKey* switch_) {
    return _take_switch(switch_, &switch_touch_released);
}
uint8_t get_switch_touch_state(){
    return switch_touch_state;
}


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