#include <stdlib.h>
#include <assert.h>

#include "idmac/fastio.h"
#include "util/dprintf.h"

static struct fastio_node *fio_main = NULL;
static struct fastio_node *fio_sub = NULL;

HRESULT fastio_hook_init(fastio_provider_t main, fastio_provider_t sub)
{
    struct fastio_node fio;
    HRESULT hr = S_OK;

    dprintf("FastIO: Init main %p sub %p\n", main, sub);

    if (main != NULL) {
        if (fio_main != NULL) {
            free(fio_main);
            fio_main = NULL;
        }
        
        fio_main = malloc(sizeof(struct fastio_node));
        fio_main->board_no = 0; // Maybe 0 should be the PCB itself and connected IO boards start at 1?
        hr = main(fio_main);
    }
    
    if (FAILED(hr)) {
        return hr;
    }

    if (sub != NULL) {
        // TODO: Unsure if "sub" is treated as input from another box, or just a 2nd link in the chain
        if (fio_sub != NULL) {
            free(fio_sub);
            fio_sub = NULL;
        }

        fio_sub = malloc(sizeof(struct fastio_node));
        fio_sub->board_no = 1;
        hr = sub(fio_sub);
    }

    return hr;
}

// FIXME This entire file is a horrific mess...
void fastio_find_board(uint8_t board_no, struct fastio_node **out)
{
    if (fio_main != NULL) {
        *out = fio_main;
        /* while (out->next != NULL && out->next->board_no != board_no) {
            out = out->next;
        }
        if (out->board_no == board_no) { return; } */
    }
    
    if (fio_sub != NULL) {
        *out = fio_sub;
        /*while (out->next != NULL && out->next->board_no != board_no) {
            out = out->next;
        }
        if (out->board_no != board_no) { out = NULL; }*/
    }
}

void fastio_reset(void* ctx, uint8_t board_no, uint32_t in)
{
    //struct fastio_node *req_board = board_no ? fio_sub : fio_main;
    //fastio_find_board(board_no, &req_board);

    // If we fail to find, or the board we found doesn't have the handler we need,
    // quietly return with out unmodified
    if (fio_main != NULL && fio_main->reset != NULL) {
        fio_main->reset(ctx, in);
    }
}

void fastio_digital_read(void* ctx, uint8_t board_no, uint32_t *out)
{
    //struct fastio_node *req_board = board_no ? fio_sub : fio_main;
    //fastio_find_board(board_no, &req_board);

    // If we fail to find, or the board we found doesn't have the handler we need,
    // quietly return with out unmodified
    if (board_no == 0 && fio_main != NULL && fio_main->digital_in != NULL) {
        fio_main->digital_in(ctx, out);
    } else if (board_no == 1 && fio_sub != NULL && fio_sub->digital_in != NULL) {
        fio_sub->digital_in(ctx, out);
    }
}

void fastio_coins_in(void* ctx, uint8_t board_no, uint8_t slot_no, uint32_t *out)
{
    //struct fastio_node *req_board = board_no ? fio_sub : fio_main;
    //fastio_find_board(board_no, &req_board);

    // If we fail to find, or the board we found doesn't have the handler we need,
    // quietly return with out unmodified
    if (fio_main != NULL && fio_main->coins_in != NULL) {
        fio_main->coins_in(ctx, slot_no, out);
    }
}
void fastio_analog_in(void* ctx, uint8_t board_no, uint8_t analog_no, uint32_t *out)
{
    //struct fastio_node *req_board = board_no ? fio_sub : fio_main;
    //fastio_find_board(board_no, &req_board);

    // If we fail to find, or the board we found doesn't have the handler we need,
    // quietly return with out unmodified
    if (fio_main != NULL && fio_main->analog_in != NULL) {
        fio_main->analog_in(ctx, analog_no, out);
    }
}
void fastio_coins_out(void* ctx, uint8_t board_no, uint8_t slot_no, uint32_t in) {
    //struct fastio_node *req_board = board_no ? fio_sub : fio_main;
    //fastio_find_board(board_no, &req_board);

    // If we fail to find, or the board we found doesn't have the handler we need,
    // quietly return with out unmodified
    if (fio_main != NULL && fio_main->coins_out != NULL) {
        fio_main->coins_out(ctx, slot_no, in);
    }
}

void fastio_gpio_out(void* ctx, uint8_t board_no, uint8_t channel, uint32_t in) {
    //struct fastio_node *req_board = board_no ? fio_sub : fio_main;
    //fastio_find_board(board_no, &req_board);

    // If we fail to find, or the board we found doesn't have the handler we need,
    // quietly return with out unmodified
    if (fio_main != NULL && fio_main->gpio_out != NULL) {
        fio_main->gpio_out(ctx, channel, in);
    }
}