#include "bottom_half.h"
#include "inputs.hpp"

#include <array>

static uint8_t reverse_4_bits(uint8_t value) {
    return static_cast<uint8_t>(((value & 0x1) << 3) |
                                ((value & 0x2) << 1) |
                                ((value & 0x4) >> 1) |
                                ((value & 0x8) >> 3));
}

static KeypadKey map_input_keypad_key(InputKeypadKey key) {
    switch (key) {
        case InputKeypadKey::K0: return KeypadKey::k0;
        case InputKeypadKey::K1: return KeypadKey::k1;
        case InputKeypadKey::K2: return KeypadKey::k2;
        case InputKeypadKey::K3: return KeypadKey::k3;
        case InputKeypadKey::K4: return KeypadKey::k4;
        case InputKeypadKey::K5: return KeypadKey::k5;
        case InputKeypadKey::K6: return KeypadKey::k6;
        case InputKeypadKey::K7: return KeypadKey::k7;
        case InputKeypadKey::K8: return KeypadKey::k8;
        case InputKeypadKey::K9: return KeypadKey::k9;
        case InputKeypadKey::A:  return KeypadKey::ka;
        case InputKeypadKey::B:  return KeypadKey::kb;
        case InputKeypadKey::C:  return KeypadKey::kc;
        case InputKeypadKey::D:  return KeypadKey::kd;
        case InputKeypadKey::STAR:  return KeypadKey::star;
        case InputKeypadKey::POUND: return KeypadKey::pound;
        default: return KeypadKey::k0;
    }
}

static uint8_t get_fingerprint_touch_state() {
    InputsState current = InputsController::get_input_state();
    return static_cast<uint8_t>((current.touch_state >> 4) & 0x1);
}

static bool touch_state_initialized = false;
static bool touch_state_last = false;

static bool update_fingerprint_transition(bool want_pressed) {
    bool current = get_fingerprint_touch_state();
    if (!touch_state_initialized) {
        touch_state_last = current;
        touch_state_initialized = true;
        return false;
    }

    bool transition = want_pressed ? (current && !touch_state_last)
                                   : (!current && touch_state_last);
    touch_state_last = current;
    return transition;
}

static std::array<SwitchFlip, 8> pending_switch_flips;
static size_t pending_switch_flip_count = 0;

static void push_pending_switch_flip(const SwitchFlip& event) {
    if (pending_switch_flip_count < pending_switch_flips.size()) {
        pending_switch_flips[pending_switch_flip_count++] = event;
        return;
    }

    // Drop the oldest event if the buffer is full.
    for (size_t i = 1; i < pending_switch_flips.size(); ++i) {
        pending_switch_flips[i - 1] = pending_switch_flips[i];
    }
    pending_switch_flips.back() = event;
}

static bool pop_pending_switch_flip(bool want_up, SwitchFlip& out) {
    for (size_t i = 0; i < pending_switch_flip_count; ++i) {
        if (pending_switch_flips[i].is_up() == want_up) {
            out = pending_switch_flips[i];
            for (size_t j = i + 1; j < pending_switch_flip_count; ++j) {
                pending_switch_flips[j - 1] = pending_switch_flips[j];
            }
            --pending_switch_flip_count;
            return true;
        }
    }
    return false;
}

static void clear_pending_switch_flips() {
    pending_switch_flip_count = 0;
}

static std::array<SwitchTouch, 8> pending_switch_touches;
static size_t pending_switch_touch_count = 0;

static void push_pending_switch_touch(const SwitchTouch& event) {
    if (pending_switch_touch_count < pending_switch_touches.size()) {
        pending_switch_touches[pending_switch_touch_count++] = event;
        return;
    }

    for (size_t i = 1; i < pending_switch_touches.size(); ++i) {
        pending_switch_touches[i - 1] = pending_switch_touches[i];
    }
    pending_switch_touches.back() = event;
}

static bool pop_pending_switch_touch(bool want_touched, SwitchTouch& out) {
    for (size_t i = 0; i < pending_switch_touch_count; ++i) {
        if (pending_switch_touches[i].is_touched() == want_touched) {
            out = pending_switch_touches[i];
            for (size_t j = i + 1; j < pending_switch_touch_count; ++j) {
                pending_switch_touches[j - 1] = pending_switch_touches[j];
            }
            --pending_switch_touch_count;
            return true;
        }
    }
    return false;
}

static void clear_pending_switch_touches() {
    pending_switch_touch_count = 0;
}

void init_bottom_half() {
    init_expander();
    clear_all_pressed_released();
}

void clear_all_pressed_released() {
    InputsController::clear_all_events();
    clear_pending_switch_flips();
    clear_pending_switch_touches();
    touch_state_initialized = false;
}

bool get_keypad_pressed(KeypadKey* kp) {
    auto opt = InputsController::get_keypad_press();
    if (!opt.has_value()) {
        return false;
    }

    if (kp != nullptr) {
        *kp = map_input_keypad_key(opt.value());
    }
    return true;
}

bool get_keypad_released(KeypadKey* kp) {
    auto opt = InputsController::get_keypad_release();
    if (!opt.has_value()) {
        return false;
    }

    if (kp != nullptr) {
        *kp = map_input_keypad_key(opt.value());
    }
    return true;
}

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, std::optional<Button> opt) {
    if (!opt.has_value()) {
        return false;
    }
    if (button != nullptr) {
        *button = static_cast<ButtonKey>(static_cast<uint8_t>(opt.value()));
    }
    return true;
}

bool get_button_pressed(ButtonKey* button) {
    return take_button(button, InputsController::get_button_press());
}

bool get_button_released(ButtonKey* button) {
    return take_button(button, InputsController::get_button_release());
}

uint8_t get_button_state() {
    return reverse_4_bits(InputsController::button_state() & 0xF);
}

static bool take_switch_key(SwitchKey* switch_, const SwitchFlip& event) {
    if (switch_ != nullptr) {
        *switch_ = static_cast<SwitchKey>(static_cast<uint8_t>(event.get_switch()));
    }
    return true;
}

bool get_switch_flipped_up(SwitchKey* switch_) {
    SwitchFlip event;
    if (pop_pending_switch_flip(true, event)) {
        return take_switch_key(switch_, event);
    }

    while (true) {
        auto opt = InputsController::get_switch_flip();
        if (!opt.has_value()) {
            return false;
        }
        if (opt->is_up()) {
            return take_switch_key(switch_, *opt);
        }
        push_pending_switch_flip(*opt);
    }
}

bool get_switch_flipped_down(SwitchKey* switch_) {
    SwitchFlip event;
    if (pop_pending_switch_flip(false, event)) {
        return take_switch_key(switch_, event);
    }

    while (true) {
        auto opt = InputsController::get_switch_flip();
        if (!opt.has_value()) {
            return false;
        }
        if (!opt->is_up()) {
            return take_switch_key(switch_, *opt);
        }
        push_pending_switch_flip(*opt);
    }
}

bool get_switch_flipped(SwitchKey* switch_) {
    if (pending_switch_flip_count > 0) {
        SwitchFlip event = pending_switch_flips[0];
        for (size_t i = 1; i < pending_switch_flip_count; ++i) {
            pending_switch_flips[i - 1] = pending_switch_flips[i];
        }
        --pending_switch_flip_count;
        return take_switch_key(switch_, event);
    }

    auto opt = InputsController::get_switch_flip();
    if (!opt.has_value()) {
        return false;
    }

    return take_switch_key(switch_, *opt);
}

uint8_t get_switch_state() {
    return reverse_4_bits(InputsController::switch_state() & 0xF);
}

static bool take_switch_touch(SwitchKey* switch_, const SwitchTouch& event) {
    if (switch_ != nullptr) {
        *switch_ = static_cast<SwitchKey>(static_cast<uint8_t>(event.get_switch()));
    }
    return true;
}

bool get_switch_touch_pressed(SwitchKey* switch_) {
    SwitchTouch event;
    if (pop_pending_switch_touch(true, event)) {
        return take_switch_touch(switch_, event);
    }

    while (true) {
        auto opt = InputsController::get_switch_touch();
        if (!opt.has_value()) {
            return false;
        }
        if (opt->is_touched()) {
            return take_switch_touch(switch_, *opt);
        }
        push_pending_switch_touch(*opt);
    }
}

bool get_switch_touch_released(SwitchKey* switch_) {
    SwitchTouch event;
    if (pop_pending_switch_touch(false, event)) {
        return take_switch_touch(switch_, event);
    }

    while (true) {
        auto opt = InputsController::get_switch_touch();
        if (!opt.has_value()) {
            return false;
        }
        if (!opt->is_touched()) {
            return take_switch_touch(switch_, *opt);
        }
        push_pending_switch_touch(*opt);
    }
}

uint8_t get_switch_touch_state() {
    return reverse_4_bits(InputsController::switch_touch_state() & 0xF);
}

bool get_touch_state() {
    return get_fingerprint_touch_state() != 0;
}

bool get_touch_pressed() {
    return update_fingerprint_transition(true);
}

bool get_touch_released() {
    return update_fingerprint_transition(false);
}