#include <stdio.h>
#include <stdlib.h>
#include <gccore.h>
#include <wiiuse/wpad.h>
#include <unistd.h>
#include <string.h>
#include <network.h>
#include <ogc/machine/processor.h>

static void *xfb = NULL;
static GXRModeObj *rmode = NULL;

#define FAILURE(x) printf("\33[31mError: %d\n\33[39m", x)
#define SUCCESS printf("\33[32msuccess\n\33[39m")

void init_crap() {
    VIDEO_Init();
    WPAD_Init();
    PAD_Init();

    // Obtain the preferred video mode from the system
    // This will correspond to the settings in the Wii menu
    rmode = VIDEO_GetPreferredMode(NULL);

    // Allocate memory for the display in the uncached region
    xfb = MEM_K0_TO_K1(SYS_AllocateFramebuffer(rmode));

    // Initialise the console, required for printf
    console_init(xfb,20,20,rmode->fbWidth,rmode->xfbHeight,rmode->fbWidth*VI_DISPLAY_PIX_SZ);

    // Set up the video registers with the chosen mode
    VIDEO_Configure(rmode);

    // Tell the video hardware where our display memory is
    VIDEO_SetNextFramebuffer(xfb);

    // Make the display visible
    VIDEO_SetBlack(FALSE);

    // Flush the video register changes to the hardware
    VIDEO_Flush();

    // Wait for Video setup to complete
    VIDEO_WaitVSync();
    if(rmode->viTVMode&VI_NON_INTERLACE) VIDEO_WaitVSync();
}

void end() {
    int columns = 0, rows = 0;
    CON_GetMetrics(&columns, &rows);
    printf("\x1b[%i;0H",rows);
    printf("Done... press Home/Start to exit.");
    while(1) {
        WPAD_ScanPads(); PAD_ScanPads();
        if ((WPAD_ButtonsDown(0) & WPAD_BUTTON_HOME) || (PAD_ButtonsDown(0) & PAD_BUTTON_START))
            exit(0);
        VIDEO_WaitVSync();
    }
}

void do_bt_stuff() {
    s32 ret, fd;
    ioctlv vec[1];
    STACK_ALIGN(char, buffer, 32, 32);
    int x = 0;

    //udp_init();

    printf("Opening /dev/usb/oh1/57e/305...");
    do {
        ret = IOS_Open("/dev/usb/oh1/57e/305", 0);
        if (ret < 0) {
            sleep(1);
            printf(".");
            ++x;
        }
    }   while (ret == -6 && x < 10);
    if (ret < 0) {
        if (x >= 10)
            printf("\33[31mTimed out.\n\33[39m");
        else
            FAILURE(ret);
        goto deinit;
    }
    SUCCESS;
    fd = ret;

    printf("Calling IOS_Ioctlv...");
    memset(buffer, 0, 32);
    buffer[0] = 0x12; buffer[1] = 0x34; buffer[2] = 0x56;   buffer[3] = 0x78;
    buffer[4] = 0x12; buffer[5] = 0x34; buffer[6] = 0x56;   buffer[7] = 0x78;
    buffer[8] = 0x12; buffer[9] = 0x34; buffer[10] = 0x56;  buffer[11] = 0x78;
    buffer[12] = 0x12; buffer[13] = 0x34;   buffer[14] = 0x56;  buffer[15] = 0x78;
    buffer[16] = 0x12; buffer[17] = 0x34;   buffer[18] = 0x56;  buffer[19] = 0x78;
    buffer[20] = 0x12; buffer[21] = 0x34;   buffer[22] = 0x56;  buffer[23] = 0x78;
    buffer[24] = 0x12; buffer[25] = 0x34;   buffer[26] = 0x56;  buffer[27] = 0x78;
    buffer[28] = 0x12; buffer[29] = 0x34;   buffer[30] = 0x56;  buffer[31] = 0x78;
    vec[0].data = buffer;
    vec[0].len = 32;
    ret = IOS_Ioctlv(fd, 3, 0, 1, vec);
    if (ret < 0) {
        FAILURE(ret);
    } else {
        int i;
        printf("\33[32mReturned %d\n\33[39m", ret);
        for (i = 0; i < 32; i++)
            printf("%02x ", buffer[i]);
        printf("\n");
        //udp_printf("IOS_Ioctlv, size=%x", ret);
        //udp_send(buffer, 32);
    }

    deinit:

    printf("De-Initializing Network...");
    //udp_deinit();
    SUCCESS;
}

int main(int argc, char **argv) {
    init_crap();

    do_bt_stuff();

    end();

    return 0;
}