#include "blk_box_drivers/expander.hpp"

#include "pins.h"
#include "blk_box_drivers/i2c.h"
#include "driver/i2c_master.h"
#include "driver/gpio.h"
#include "esp_log.h"
#include "esp_err.h"

static const char *TAG = "EXPANDER";

i2c_master_dev_handle_t expander_i2c_dev_handle;

const static uint8_t REG_WHOAMI = 0x01;
const static uint8_t REG_SW_VERSION = 0x02;
const static uint8_t REG_EVENT_QUEUE_POP = 0x10;
const static uint8_t REG_EVENT_QUEUE_LEN = 0x11;
const static uint8_t REG_STATE_BUTTONS = 0x20;
const static uint8_t REG_STATE_SWITCHES = 0x21;
const static uint8_t REG_STATE_KEYPAD = 0x22;
const static uint8_t REG_STATE_TOUCH = 0x23;
const static uint8_t REG_STATE_RFID = 0x24;
const static uint8_t REG_STATE_HALL = 0x25;
const static uint8_t REG_STATE_CLOSE = 0x26;
const static uint8_t REG_RESET = 0x30;
const static uint8_t REG_HALL_SENSITIVITY = 0x31;
const static uint8_t REG_CLOSE_SENSITIVITY = 0x32;
const static uint8_t REG_SWITCH_TOUCH_EVENT = 0x33;

ExpanderPeripheral expander_peripheral;

// forward declarations
static void handle_event(uint8_t event);
static void handle_button_switch_event(uint8_t event);
static void handle_keypad_event(uint8_t event);
static void handle_touch_event(uint8_t event);
static void handle_rfid_event(uint8_t event);
static void handle_close_hal_event(uint8_t event);

void init_expander() {
    ESP_LOGI(TAG, "Initializing expander...");

    i2c_device_config_t dev_config = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = EXPANDER_I2C_ADDR,
        .scl_speed_hz = EXPANDER_I2C_SPEED,
        .scl_wait_us = 0, // default
        .flags = {
            .disable_ack_check = 0,
        }
    };

    ESP_ERROR_CHECK(i2c_master_bus_add_device(i2c_main_bus_handle, &dev_config, &expander_i2c_dev_handle));
    ESP_LOGD(TAG, "Expander I2C device added to bus");

    // TODO: setup interrupt on PIN_EXPANDER_INT

    uint8_t read_buf[2] = {0};

    ESP_ERROR_CHECK(i2c_master_transmit_receive(expander_i2c_dev_handle, &REG_WHOAMI, 1, read_buf, 1, EXPANDER_TIMEOUT_MS));

    if (read_buf[0] != EXPANDER_WHOAMI_VALUE) {
        ESP_LOGE(TAG, "WHOAMI mismatch, expected 0x%02X, got 0x%02X", EXPANDER_WHOAMI_VALUE, read_buf[0]);
        return;
    }

    ESP_LOGD(TAG, "Expander WHOAMI check passed");

    ESP_ERROR_CHECK(i2c_master_transmit_receive(expander_i2c_dev_handle, &REG_SW_VERSION, 1, read_buf, 2, EXPANDER_TIMEOUT_MS));

    // init the peripheral struct
    expander_peripheral.state_mutex = xSemaphoreCreateMutex();
    expander_peripheral.button_press_events= xQueueCreate(EXPANDER_EVENT_QUEUE_SIZE, sizeof(Button));
    expander_peripheral.button_release_events= xQueueCreate(EXPANDER_EVENT_QUEUE_SIZE, sizeof(Button));
    expander_peripheral.switch_flip_events= xQueueCreate(EXPANDER_EVENT_QUEUE_SIZE, sizeof(SwitchFlip));
    expander_peripheral.switch_touch_events= xQueueCreate(EXPANDER_EVENT_QUEUE_SIZE, sizeof(SwitchTouch));
    expander_peripheral.touch_events= xQueueCreate(EXPANDER_EVENT_QUEUE_SIZE, sizeof(TouchedReleased));
    expander_peripheral.keypad_press_events= xQueueCreate(EXPANDER_KEYPAD_QUEUE_SIZE, sizeof(KeypadKey));
    expander_peripheral.keypad_release_events= xQueueCreate(EXPANDER_KEYPAD_QUEUE_SIZE, sizeof(KeypadKey));

    ESP_LOGI(TAG, "Expander initialized! SW version: v%d.%d", read_buf[0], read_buf[1]);
}

void get_events() {
    uint8_t recv;
    do {
        ESP_ERROR_CHECK(i2c_master_transmit_receive(expander_i2c_dev_handle, &REG_EVENT_QUEUE_POP, 1, &recv, 1, EXPANDER_TIMEOUT_MS));
        handle_event(recv);
    } while (gpio_get_level(PIN_EXPANDER_INT) == 0);
}

static void handle_event(uint8_t event) {
    const uint8_t BUTTON_SWITCH = 0b000;
    const uint8_t KEYPAD = 0b001;
    const uint8_t TOUCH = 0b010;
    const uint8_t RFID = 0b011;

    ESP_LOGI(TAG, "Expander event: 0b%08b (0x%02X)", event, event);

    if (event == 0) {
        ESP_LOGE(TAG, "We read from event queue while it was empty!");
        return;
    }

    uint8_t type_bits = event >> 5;

    switch (type_bits) {
        case BUTTON_SWITCH:
            handle_button_switch_event(event);
            break;
        case KEYPAD:
            handle_keypad_event(event);
            break;
        case TOUCH:
            handle_touch_event(event);
            break;
        case RFID:
            handle_rfid_event(event);
            break;
        default:
            handle_close_hal_event(event);
            break;
    }
}

static void handle_button_switch_event(uint8_t event) {
    const uint8_t PRESSED_NOT_RELEASED_BIT = 0b10000;
    const uint8_t SWITCH_NOT_BUTTON_BIT = 0b01000;
    const uint8_t SWITCH_UP_NOT_DOWN_BIT = 0b00100;
    const uint8_t NUMBER_MASK = 0b00011;

    bool pressed = (event & PRESSED_NOT_RELEASED_BIT) != 0;
    uint8_t number = event & NUMBER_MASK;

    if ((event & SWITCH_NOT_BUTTON_BIT) != 0) {
        // For now, we support two position switches by only looking at the switch up events
        bool switch_up = (event & SWITCH_UP_NOT_DOWN_BIT) != 0;
        if (!switch_up) {
            return;
        }

        Switch sw = static_cast<Switch>(number);
        SwitchFlip sw_flip = SwitchFlip(sw, pressed);
        if (xQueueSendToBack(expander_peripheral.switch_flip_events, &sw_flip, 0) != pdTRUE) {
            ESP_LOGE(TAG, "Failed to send switch flip event!");
        }
        xSemaphoreTake(expander_peripheral.state_mutex, portMAX_DELAY);
        if (pressed) {
            // set
            expander_peripheral.state.switch_state |= 1 << number;
        } else {
            // clear
            expander_peripheral.state.switch_state &= ~(1 << number);
        }
        xSemaphoreGive(expander_peripheral.state_mutex);
    } else {
        // button
        Button button = static_cast<Button>(number);
        if (pressed) {
            if (xQueueSendToBack(expander_peripheral.button_press_events, &button, 0) != pdTRUE) {
                ESP_LOGE(TAG, "Failed to send button press event!");
            }
            xSemaphoreTake(expander_peripheral.state_mutex, portMAX_DELAY);
            expander_peripheral.state.button_state |= 1 << number;
            xSemaphoreGive(expander_peripheral.state_mutex);
        } else {
            if (xQueueSendToBack(expander_peripheral.button_release_events, &button, 0) != pdTRUE) {
                ESP_LOGE(TAG, "Failed to send button release event!");
            }
            xSemaphoreTake(expander_peripheral.state_mutex, portMAX_DELAY);
            expander_peripheral.state.button_state &= ~(1 << number);
            xSemaphoreGive(expander_peripheral.state_mutex);
        }
    }
}

static void handle_keypad_event(uint8_t event) {
    const uint8_t PRESSED_NOT_RELEASED_BIT = 0b10000;
    const uint8_t KEY_MASK = 0b1111;

    bool pressed = (event & PRESSED_NOT_RELEASED_BIT) != 0;
    uint8_t number = event & KEY_MASK;
    KeypadKey key = static_cast<KeypadKey>(number);

    // starcode system gets first dibs
    // TODO: do starcode inbetweener
    // if starcode_handle_keypad(key, pressed).await {
    //     return;
    // }

    if (pressed) {
        if (xQueueSendToBack(expander_peripheral.keypad_press_events, &key, 0) != pdTRUE) {
            ESP_LOGE(TAG, "Failed to send keypad press event!");
        }
        xSemaphoreTake(expander_peripheral.state_mutex, portMAX_DELAY);
        expander_peripheral.state.keypad_state |= 1 << number;
        xSemaphoreGive(expander_peripheral.state_mutex);
    } else {
        if (xQueueSendToBack(expander_peripheral.keypad_release_events, &key, 0) != pdTRUE) {
            ESP_LOGE(TAG, "Failed to send keypad release event!");
        }
        xSemaphoreTake(expander_peripheral.state_mutex, portMAX_DELAY);
        expander_peripheral.state.keypad_state &= ~(1 << number);
        xSemaphoreGive(expander_peripheral.state_mutex);
    }
}

static void handle_touch_event(uint8_t event) {
    const uint8_t TOUCHED_NOT_UNTOUCHED_BIT = 0b10000;
    const uint8_t SENSOR_MASK = 0b0111;
    const uint8_t FINGERPRINT_BIT = 0b0100;

    bool touched = (event & TOUCHED_NOT_UNTOUCHED_BIT) != 0;
    uint8_t sensor = event & SENSOR_MASK;

    if ((sensor & FINGERPRINT_BIT) != 0) {
        TouchedReleased touch_state = static_cast<TouchedReleased>(touched);
        if (xQueueSendToBack(expander_peripheral.touch_events, &touch_state, 0) != pdTRUE) {
            ESP_LOGE(TAG, "Failed to send touch event!");
        }
    } else {
        Switch sw = static_cast<Switch>(sensor);
        SwitchTouch sw_touch = SwitchTouch(sw, touched);
        if (xQueueSendToBack(expander_peripheral.switch_touch_events, &sw_touch, 0) != pdTRUE) {
            ESP_LOGE(TAG, "Failed to send switch touch event!");
        }
    }

    xSemaphoreTake(expander_peripheral.state_mutex, portMAX_DELAY);
    if (touched) {
        expander_peripheral.state.touch_state |= 1 << sensor;
    } else {
        expander_peripheral.state.touch_state &= ~(1 << sensor);
    }
    xSemaphoreGive(expander_peripheral.state_mutex);
}

static void handle_rfid_event(uint8_t event) {
    // TODO: impl
    (void)event;
}

static void handle_close_hal_event(uint8_t event) {
    // TODO: impl
    (void)event;
}