#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "pico/stdlib.h"

#include "tusb_config.h"
#include "bsp/board_api.h"
#include "tusb.h"

#include "logging/log.hh"

#include "blink.hh"
#include "hid.hh"
#include "i2c.hh"
#include "mcp23017.hh"

using mcp23017::reg;


extern "C" {
    // TinyUSB callbacks
    void tud_mount_cb();
    void tud_unmount_cb();
    void tud_suspend_cb(bool);
    void tud_resume_cb();
}

static constexpr unsigned i2c_baud = 400 * 1000;

static constexpr unsigned hid_update_ms = 1000/20; // 20Hz

static constexpr unsigned bank_leds = 0;
static constexpr unsigned bank_buttons = 1;
static constexpr unsigned buttons_per_group = 5;
static constexpr unsigned button_group_count = 2;
static const unsigned button_group_irqs[] = {4, 7};
mcp23017::extender button_groups[] ={ {0x20}, {0x21} };
uint8_t button_states[] = {0, 0};
uint8_t button_leds[] = {0, 0};

static_assert(sizeof(button_group_irqs) == button_group_count * sizeof(*button_group_irqs));
static_assert(sizeof(button_groups) == button_group_count * sizeof(*button_groups));

void irq_handler(unsigned pin, uint32_t events) {
    for (unsigned i = 0; i < button_group_count; ++i) {
        if (button_group_irqs[i] != pin) continue;
        button_states[i] = button_groups[i].get_gpios(bank_buttons);
        break;
    }
}

bool hid_update_callback(repeating_timer *timer) {
    uint32_t buttons = 0;
    for (unsigned i = 0; i < button_group_count; ++i)
        buttons |= uint32_t(button_states[i]) << (i*8);

    blink::task();
    hid::task(buttons);
    return true;
}

void set_leds_callback(uint32_t leds) {
    for (unsigned i = 0; i < button_group_count; ++i) {
        uint8_t desired = (leds >> (i*8)) & 0xff;
        if (desired != button_leds[i]) {
            button_groups[i].set_gpios(bank_leds, desired);
            button_leds[i] = desired;
        }
    }
    log::trace("set leds to %02x %02x", button_leds[0], button_leds[1]);
}

void test_irq_handler(unsigned pin, uint32_t events) {
    log::debug("irq received on %d", pin);
    irq_handler(pin, events);
    uint32_t leds = 0;
    for (unsigned i = 0; i < button_group_count; ++i)
        leds |= (uint32_t(button_states[i]) << (i*8));
    set_leds_callback(leds);
}

int main(void)
{
    setup_default_uart();
    stdio_init_all();
    puts("\n");

    gpio_init(PICO_DEFAULT_LED_PIN);
    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);

    board_init();
    log::info("board initialized");

    tud_init(BOARD_TUD_RHPORT);

    if (board_init_after_tusb)
        board_init_after_tusb();
    log::info("tinyusb initialized");

    i2c::init(i2c_baud);

    // set up interrupt handlers
    for (int pin : button_group_irqs) {
        gpio_init(pin);
        gpio_pull_up(pin);
        gpio_set_irq_enabled_with_callback(pin, GPIO_IRQ_EDGE_FALL, true, irq_handler);
    }

    bool have_any_buttons = false;
    bool start_test_mode = false;
    for (unsigned i = 0; i < button_group_count; ++i) {
        if (!button_groups[i].init()) {
            log::warn("failed to initialize button group %d", i);
            continue;
        }
        log::info("initializing button group %d", i);

        have_any_buttons = true;
        mcp23017::extender &buttons = button_groups[i];

        using mcp23017::direction;
        for (unsigned i = 0; i < buttons_per_group; ++i)
            buttons.set_direction(bank_leds, i, direction::out);

        for (unsigned i = 0; i < buttons_per_group; ++i) {
            buttons.set_direction(bank_buttons, i, direction::in);
            buttons.set_polarity(bank_buttons, i, true);
            buttons.set_pullup(bank_buttons, i, true);
            buttons.set_irq(bank_buttons, i, true);
        }

        button_states[i] = buttons.get_gpios(bank_buttons);
        if (button_states[i] != 0) {
            log::debug("button group %d has pressed buttons %02x, enabling test mode.",
                    i, button_states[i]);
            start_test_mode = true;
        }
    }

    if (!have_any_buttons) {
        i2c::scan();
        return 1;
    }

    repeating_timer hid_timer;
    if (start_test_mode) {
        // Replace IRQ handlers
        for (int pin : button_group_irqs)
            gpio_set_irq_enabled_with_callback(pin, GPIO_IRQ_EDGE_FALL, true, test_irq_handler);
    } else {
        // Regular mode
        hid::init(set_leds_callback);
        add_repeating_timer_ms(hid_update_ms, hid_update_callback, nullptr, &hid_timer);
    }

    // Let timers and iterrupts handle most things
    while (1) {
        tud_task(); // tinyusb device task
        sleep_ms(1);
    }
}

//--------------------------------------------------------------------+
// Device callbacks
//--------------------------------------------------------------------+

// Invoked when device is mounted
void tud_mount_cb() {
    blink::set_pattern(blink::pattern::mounted);
}

// Invoked when device is unmounted
void tud_umount_cb() {
    blink::set_pattern(blink::pattern::not_mounted);
}

// Invoked when usb bus is suspended
// argumen : if host allow us  to perform remote wakeup
// Within 7ms, device must draw an average of current less than 2.5 mA from bus
void tud_suspend_cb(bool) {
    blink::set_pattern(blink::pattern::suspended);
}

// Invoked when usb bus is resumed
void tud_resume_cb() {
    blink::pattern pat = tud_mounted() ? blink::pattern::mounted : blink::pattern::not_mounted;
    blink::set_pattern(pat);
}