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

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

static struct fastio_node fio_main;
static struct fastio_node fio_sub;

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) {
        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
        //fio.next = (struct fastio_node *) malloc(sizeof(struct fastio_node));
        hr = sub(&fio_sub);
    }

    return hr;
}


/// Itterate over the chain of connected boards and return a pointer to the board with the requested number
void fastio_match_board_number(uint8_t board_no, struct fastio_node root, struct fastio_node *board)
{
    assert(&root != NULL);
    // FIXME
    
    board = &root;
    while (board->next != NULL && board->next->board_no != board_no) {
        board = board->next;
    }

    if (board->board_no != board_no) {
        // We've exhausted our boards, give a warning and set boards to NULL
        dprintf("FastIO: Could not find board number %d\n", board_no);
        board = NULL;
    }
}

void fastio_digital_read(void* ctx, uint8_t board_no, uint32_t *out)
{
    //struct fastio_node *req_board = NULL;
    //fastio_match_board_number(board_no, fio_main, 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 (req_board != NULL && req_board->digital_in != NULL) {
    if (fio_main.digital_in != NULL) {
        fio_main.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 = NULL;
    fastio_match_board_number(board_no, fio_main, 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 (req_board != NULL && req_board->coins_in != NULL) {
        fio_main.coins_in(ctx, slot_no, out);
    }
}