#include <assert.h>
#include <stdint.h>

#include "ll3hook/usbio.h"
#include "ll3hook/ll3-dll.h"
#include "ll3io/ll3io.h"
#include "util/dprintf.h"

static void get_buttons(void *ctx, uint8_t out[4], uint32_t *coin_ct);

HRESULT ll3_usbio_init(const struct usbio_config *cfg)
{
    if (!cfg->enabled) {
        return S_FALSE;
    }

    ll3_dll.init();

    struct usbio_ops ops = {
        .digital_in = get_buttons,
        .digital_out = NULL,
        .analog_in = NULL,
        .analog_out = NULL,
        .reader_poll = NULL,
        .reader_led_out = NULL,
    };

    return k9101258_hook_init(&ops, NULL);
}

static void get_buttons(void *ctx, uint8_t out[4], uint32_t *coin_ct)
{
    uint16_t gamebtn = 0;
    uint8_t sysbtn = 0;
    ll3_dll.poll(&gamebtn, &sysbtn, coin_ct);

    // Door switches
    out[3] |= 0xFC;

    if (sysbtn & LL3_SYSBTN_TEST) {
        out[0] |= 0x20;
    }
    if (sysbtn & LL3_SYSBTN_SERVICE) {
        out[0] |= 0x40;
    }
    if (sysbtn & LL3_SYSBTN_SELECT) {
        out[0] |= 0x10;
    }
    if (sysbtn & LL3_SYSBTN_ENTER) {
        out[2] |= 0x80;
    }
    if (sysbtn & LL3_SYSBTN_COIN) {
        out[0] |= 0x80;
    }

    if (gamebtn & LL3_BTN1) {
        out[2] |= 0x01;
    }
    if (gamebtn & LL3_BTN2) {
        out[2] |= 0x02;
    }
    if (gamebtn & LL3_BTN3) {
        out[2] |= 0x04;
    }
    if (gamebtn & LL3_BTN4) {
        out[2] |= 0x08;
    }
    if (gamebtn & LL3_BTN5) {
        out[2] |= 0x10;
    }
    if (gamebtn & LL3_BTN6) {
        out[2] |= 0x20;
    }
    if (gamebtn & LL3_BTN7) {
        out[2] |= 0x40;
    }
    if (gamebtn & LL3_BTN8) {
        out[0] |= 0x01;
    }
    if (gamebtn & LL3_BTN9) {
        out[0] |= 0x02;
    }
    if (gamebtn & LL3_BTN_CANCEL) {
        out[0] |= 0x04;
    }
    if (gamebtn & LL3_BTN_DECIDE) {
        out[0] |= 0x08;
    }

}