#include "ultra64.h"
#ifdef IS_BB
#include "os_bb.h"
#include "bcp.h"
#endif

#include "usbshow.h"
#include "graph.h"

#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf
//#define PRINTF		dramPrintf
//#define PRINTF(f,a...)
//#define drambufswitch(i)

//#define IDE_PRINT

/*
 * Thread and stack structures
 */
char   bootStack[STACKSIZE] __attribute__ ((aligned (8)));

static OSThread idleThread;
static char     idleThreadStack[STACKSIZE] __attribute__ ((aligned (8)));

static OSThread mainThread;
static char     mainThreadStack[STACKSIZE] __attribute__ ((aligned (8)));

/*
 * Message queues and message buffers used by this app 
 */
static OSMesg           PiMessages[DMA_QUEUE_SIZE], dmaMessageBuf;
static OSMesgQueue      PiMessageQ, dmaMessageQ;
//static OSIoMesg         dmaIOMessageBuf;

static OSMesg           SiMessages[DMA_QUEUE_SIZE];
static OSMesgQueue      SiMessageQ, contMessageQ;
OSPiHandle              *handler;

/*
 * Local variables and routines
 */

static void		idleproc(char *);
static void		mainproc(char *);

void
boot(void)
{
    __osBbVideoPllInit(OS_TV_NTSC);
    osInitialize();
    osCreateThread(&idleThread, 1, (void(*)(void *))idleproc, (void *)0,
                   idleThreadStack+STACKSIZE, 8);
    osStartThread(&idleThread);
}


static void
idleproc(char *argv)		/* priority 8 */
{
    osCreateViManager(OS_PRIORITY_VIMGR);
    /*
     * Start PI Mgr for access to cartridge - start before the debugger
     */
    osCreatePiManager((OSPri) OS_PRIORITY_PIMGR, &PiMessageQ, PiMessages,
            DMA_QUEUE_SIZE);

    osCreateMesgQueue(&dmaMessageQ, &dmaMessageBuf, 1);
    osCreateMesgQueue(&SiMessageQ, SiMessages, DMA_QUEUE_SIZE);
    osSetEventMesg(OS_EVENT_SI, &SiMessageQ, (OSMesg)DMA_QUEUE_SIZE);

    /*
     * The main thread's priority must be the same or lower than the original
     * idle's thread priority. This allows the idle thread to change its
     * priority to 0 before the main thread starts execution.
     */
    osCreateThread(&mainThread, 3, (void(*)(void *))mainproc, argv,
           mainThreadStack+STACKSIZE/8, (OSPri)7);
    osStartThread(&mainThread);

    osSetThreadPri(0, OS_PRIORITY_IDLE);
    for(;;) {
	/* idle thread */
    }
}

static OSMesgQueue	retraceMessageQ;
static OSMesg		dummyMessage, retraceMessageBuf;

#ifdef IS_BB
//static u8 block[BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
//static u16 blockList[4096];		/* file block list */
//static OSBbFs fs;
#else
typedef struct {
    char name[256];
    u8 type;
    int size;
} OSBbDirEnt;

#endif
static OSBbDirEnt dir[400];
static int num_files = 0;
static int current = 0;

#ifdef IS_BB
static char *cause = (char *)(0x80500000-4096);
#else
static char *cause = (char *)(0x80300000-4096);
#endif

#define MAX_USB_DEVICES 2

static OSBbUsbInfo UsbInfo[MAX_USB_DEVICES];

#define FRAMEBUFSIZE	(640*480)

static u16 cfb_a[FRAMEBUFSIZE] __attribute__((aligned(64)));
static u16 cfb_b[FRAMEBUFSIZE] __attribute__((aligned(64)));
static int drawbuf = 0;	/* keep track of buffer to draw */
static void *cfb_ptr[2] = {cfb_a, cfb_b};

char strbuf[256];

#define	ECHO_DATA_SIZE	1024
unsigned char outbuf[ECHO_DATA_SIZE] __attribute__ ((aligned(16)));
unsigned char inbuf[ECHO_DATA_SIZE] __attribute__ ((aligned(16)));

void
do_echo_test(s32 which, OSBbUsbHandle handle, int x, int y)
{
    s32 i, nc, errs, loop, fail;
    u32 start, gap, min, max;

    min = 0x10000000;
    max = 0;

    /*
     * Get the current buffer displayed so that this routine
     * can draw in the front buffer
     */
    osViSwapBuffer(cfb_ptr[drawbuf]);
    osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);

    fail = 0;
    for (loop = 0; ; loop++) {
	start = osGetCount();
        for (i = 0; i < sizeof outbuf; i++) {
	    outbuf[i] = ((u8)i) + ((start>>12)&0xff);
        }
	start = osGetCount();
        nc = osBbUsbDevWrite(handle, &outbuf[0], sizeof outbuf, (u64)0);
	/* If device was detached, bail */
	if (nc < 0) {
	    PRINTF("osBbUsbDevWrite returns err %d\n", nc);
	    return;
	}
        bzero((void *)inbuf, sizeof inbuf);
        nc = osBbUsbDevRead(handle, &inbuf[0], sizeof inbuf, (u64)0);
	/* If device was detached, bail */
	if (nc < 0) {
	    PRINTF("osBbUsbDevRead returns err %d\n", nc);
	    return;
	}
	gap = osGetCount();
	/*
	 * Track min and max loop times
	 * Ignore the gap when the count register wraps
	 */
	if (gap > start) {
	    gap -= start;
	    if (gap < min) {min = gap; max = 0;}
	    if (gap > max) max = gap;
	}

        errs = 0;
        for (i = 0; i < sizeof outbuf; i++) {
	    if (inbuf[i] != outbuf[i]) {
	        errs++;
	    }
        }
        bzero(strbuf, sizeof strbuf);
        sprintf(strbuf, "USB echo test loop %d %s      ",
            (int)loop, (errs == 0 ? "succeeded" : "failed"));
        if (errs) {
	    fail++;
	    printstr(blue, x, y+1, strbuf);
	    /* print results to DRAM for analysis */
	    PRINTF("*** Loop %d nc %d fails with bad values:\n", loop, nc);
            for (i = 0; i < sizeof outbuf; i++) {
	        if (inbuf[i] != outbuf[i]) {
	            PRINTF("*** Index %d should be 0x%x is 0x%x\n", i, outbuf[i] & 0xff, inbuf[i] & 0xff);
	        }
            }
	} else
	    printstr(blue, x, y, strbuf);
        bzero(strbuf, sizeof strbuf);
        sprintf(strbuf, "Loop time min %d, max %d", (int)min, (int)max);
	printstr(blue, x, (fail ? y+2 : y+1), strbuf);
        bzero(strbuf, sizeof strbuf);
        sprintf(strbuf, "Total failures %d", (int)fail);
	printstr(blue, x, (fail ? y+3 : y+2), strbuf);
        osWritebackDCacheAll();
    }

    return;
}

#include "rdb.h"

void
do_rdb_test(s32 which, OSBbUsbHandle handle, int x, int y)
{
#if 0
    unsigned char data[4];
    unsigned char in[64];
    int rv;

    data[0] = RDB_TYPE_HtoG_DATA << 2 | 3;
    data[1] = 0xa1;
    data[2] = 0xb2;
    data[3] = 0xc3;

    rv = osBbUsbDevWrite(handle, data, 4, 0);
    bzero((void *)in, sizeof in);
    rv = osBbUsbDevRead(handle, in, sizeof in, 0);

    /*
     * Now try big buffers
     */
    do_echo_test(which, handle, x, y);
#else
    PRINTF("do_rdb_test: disabled\n");
#endif
    return;
}

static void
usbshow(int nusb)
{
    static int loops = 0;
    int i, j;
    int ndev = 0;
    int x = 2, y = 3;
    OSBbUsbHandle h;

    /*
     * tell the graph.c routines where to draw
     */
    set_curfb(cfb_ptr[drawbuf]);

    gcls();
    bzero(strbuf, sizeof strbuf);
    sprintf(strbuf, "USB Status (%d controllers)", nusb);
    printstr(red, 2+18-strlen(strbuf)/2, 2, strbuf);
    osWritebackDCacheAll();

    for (i = 0; i < nusb; i++) {
        ndev = osBbUsbDevQuery(i, &UsbInfo[0], MAX_USB_DEVICES);
        bzero(strbuf, sizeof strbuf);
	sprintf(strbuf, "USB%d (%s): %d device%s found",
	    i, (ndev == 0) ? "host" : (UsbInfo[0].ua_type == OS_USB_TYPE_DEVICE) ? "device" : "host",
	    ndev, (ndev == 1) ? "" : "s");
        printstr(red, x, y, strbuf);
        osWritebackDCacheAll();
	y += 1;
	if (ndev <= 0)
	    continue;
	for (j = 0; j < ndev; j++) {
            bzero(strbuf, sizeof strbuf);
	    sprintf(strbuf, "%d: vendor 0x%x, product 0x%x", j,
	  	UsbInfo[j].ua_vendor,
		UsbInfo[j].ua_product);
            printstr(white, x, y, strbuf);
	    y += 1;
            bzero(strbuf, sizeof strbuf);
	    sprintf(strbuf, "%d: %s", j, UsbInfo[j].ua_mfr_str);
            printstr(white, x, y, strbuf);
	    y += 1;
            bzero(strbuf, sizeof strbuf);
	    sprintf(strbuf, "%d: %s", j, UsbInfo[j].ua_prod_str);
            printstr(white, x, y, strbuf);
	    y += 1;
            bzero(strbuf, sizeof strbuf);
	    if (UsbInfo[0].ua_type == OS_USB_TYPE_HOST) {
	        sprintf(strbuf, "%d: %s", j, UsbInfo[j].ua_driver_name);
	    } else {
	        sprintf(strbuf, "%d: %s", j, (UsbInfo[j].ua_state == OS_USB_STATE_ATTACHED) ? "attached" : "no host");
	    }
            printstr(white, x, y, strbuf);
            osWritebackDCacheAll();
	    y += 1;
	    if (UsbInfo[0].ua_type != OS_USB_TYPE_HOST)
	        continue;
            h = NULL;
            if (osBbUsbDevGetHandle(i, &UsbInfo[j], &h) == 0) {
                /*
                 * Assume the echo device for now
                 */
                if (UsbInfo[j].ua_vendor == 0x05a3 &&
                    UsbInfo[j].ua_product == 0x0001) {
                   do_echo_test(i, h, x, y);
                } else if (UsbInfo[j].ua_vendor == 0x3dbb &&
                    UsbInfo[j].ua_product == 0xdbbb) {
                   do_rdb_test(i, h, x, y);
                }
                osBbUsbDevFreeHandle(h);
	    }
	}
    }
    bzero(strbuf, sizeof strbuf);
    sprintf(strbuf, "Loop count %d", ++loops);
    printstr(blue, x, y, strbuf);
    if ((loops & 0xff) == 0) {
	PRINTF("Loop count %d\n", loops);
    }
    /* flush the cache and wait for retrace */
    osWritebackDCacheAll();
    osViSwapBuffer(cfb_ptr[drawbuf]);
    osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
    osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
    drawbuf ^= 1;
}

OSContStatus statusData[MAXCONTROLLERS];
OSContPad controllerData[MAXCONTROLLERS];

static int no_controller = 1;
static void
initController(void)
{
    int i;
    u8 pattern;

    osCreateMesgQueue(&contMessageQ, &dummyMessage, 1);
    osSetEventMesg(OS_EVENT_SI, &contMessageQ, (OSMesg) 0);

    osContInit(&contMessageQ, &pattern, &statusData[0]);

    for (i = 0; i < MAXCONTROLLERS; i++) {
        if ((pattern & (1 << i)) &&
            !(statusData[i].errno & CONT_NO_RESPONSE_ERROR)) {
            no_controller = 0;
        }
    }
}

static u16 lastbutton[MAXCONTROLLERS];

static void
processButton(u16 button, int lastbutton)
{
    if (current == -1) return;
    if (button & D_JPAD && !(lastbutton & D_JPAD)) {
        current = (current + 1) % num_files;
    }

    if (button & U_JPAD && !(lastbutton & U_JPAD)) {
        current = (current - 1) < 0? num_files - 1: current-1;
    }

    if (button & R_JPAD && !(lastbutton & R_JPAD)) {
        if (current + (num_files+1)/2 < num_files) {
            current += (num_files+1)/2;
        }
    }

    if (button & L_JPAD && !(lastbutton & L_JPAD)) {
        if (current >= (num_files+1)/2) {
            current -= (num_files+1)/2;
        }
    }

    if ((button & A_BUTTON && !(lastbutton & A_BUTTON)) ||
        (button & START_BUTTON && !(lastbutton & START_BUTTON))) {
        if (*cause) {
#ifdef IS_BB
            IO_WRITE(MI_CTRL_REG, MI_CTRL_HARD_RESET|MI_CTRL_DIV_MODE_1_5);
#endif
        } else {
            int fd;
            //printstr(red, 4, 13, "Launching: ");
            //printstr(red, 15, 13, dir[current].name);
            osWritebackDCacheAll();
#ifdef IS_BB
            fd = osBbFOpen(dir[current].name, "r");
            if (fd >= 0) {
		//restore_state(dir[current].name);
                //execFile(fd, dir[current].name);
            }
#endif
        }
    }

}

static void
readControllers(void)
{
    int i;
    OSContPad *pad;

    osRecvMesg(&contMessageQ, &dummyMessage, OS_MESG_BLOCK); 
    osContGetReadData(controllerData);

    for (i = 0; i < MAXCONTROLLERS; ++i) {
        pad = &controllerData[i];
        if (pad->button) {
            processButton(pad->button, lastbutton[i]);
        }
        lastbutton[i] = pad->button;
    }
}

/*
 * Busy wait for n ticks (where 1 tick = 10ms = 10,000us)
 */
static void
__delay(int ticks)
{
	OSTime et, ct;

	et = osGetTime() + (OSTime)OS_USEC_TO_CYCLES(ticks * 10000);
	do {
		ct = osGetTime();
	} while (ct < et);
}

static void 
mainproc(char *argv) {
    s32 nusb;
    s32 which, ndev;

    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
    set_curfb(cfb_ptr[drawbuf]);
    osViBlack(1);
    osViSwapBuffer(cfb_ptr[drawbuf]);
    drawbuf ^= 1;
    osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);

    initController();

    /*
     * Try USB0 as device first
     */
    which = 0;
retry:
    osBbUsbSetCtlrModes(which, 2);
    osBbUsbSetCtlrModes(~which & 1, 0);
    nusb = osBbUsbInit();
    osSyncPrintf("After osBbUsbInit and no delay, this one disappears ...\n");

    osBbSetErrorLed(1);  
    __delay(80);
    osSyncPrintf("After osBbUsbInit and wait .8 sec\n");
    osBbSetErrorLed(0);  

    /* wait 5 seconds */
    __delay(500);
    ndev = osBbUsbDevQuery(which, &UsbInfo[0], MAX_USB_DEVICES);
    if (ndev != 1
       || (UsbInfo[0].ua_type != OS_USB_TYPE_DEVICE)
       || (UsbInfo[0].ua_state != OS_USB_STATE_ATTACHED)) {

        osBbSetErrorLed(1);  
        __delay(40);
        osBbSetErrorLed(0);  
        __delay(40);
        osBbSetErrorLed(1);  
        __delay(40);
        osBbSetErrorLed(0);  
        __delay(40);
        which = ~which & 1;
        goto retry;
    }

    *cause = '\0';
    init_graphics(LOW_RES);

    osViBlack(0);
    osWritebackDCacheAll();
    osViSwapBuffer(cfb_ptr[drawbuf]);
    drawbuf ^= 1;
    osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);

    for(;;) {
        usbshow(nusb);
        //osContStartReadData(&contMessageQ);
        //readControllers();
    }
}