#include "ultra64.h"
#include "os_bb.h"
#include "bcp.h"

#include "controller.h"
#include "nu64sys.h"
#include "graph.h"


#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf

extern u32 __osBbIsBb;
extern u32 __osBbHackFlags;

/*
 * 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 OSMesg           SiMessages[DMA_QUEUE_SIZE];
static OSMesgQueue      SiMessageQ;

/*
 * Local variables and routines
 */

static void		idleproc(char *);
static void		mainproc(char *);
extern void             __osBbVideoPllInit(s32 tvType);

void
boot(void)
{
    /* Init the video PLL */
    __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);
#if	SCREEN_LOW
    osViSetMode(&osViModeTable[OS_VI_NTSC_LPN1]);
#else
    osViSetMode(&osViModeTable[OS_VI_NTSC_HPF1]);
#endif

    /*
     * 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, siMessageQ;;
static OSMesg		dummyMessage, retraceMessageBuf;
OSMesg      siMessageBuf;

static u16 cfb[SCREEN_WD*SCREEN_HT] __attribute__((aligned(64)));
static void clear(u16 bg) {
    int i;
    for (i = 0; i < SCREEN_WD*SCREEN_HT; ++i) {
            cfb[i] = bg;
    }
}

int my_poweroff()
{
    osBbPowerOff();

    /* Busy loop to let CPU spin */
    for (; ;);
}

#define JOY_XY_MIN  52
static int controller_test(int which) 
{
    OSContStatus     sdata[MAXCONTROLLERS];
    OSContPad        ctrl_data[MAXCONTROLLERS];
    int key, i, done[18], color[18], x=0, y=0;
    char title[64], pattern, sp[8], no_response_before=1;
    int key_pre, key_post, key_reset, key_exit;
    u64 end;

    osSetEventMesg(OS_EVENT_SI, &siMessageQ, (OSMesg)NULL);
    osContInit(&siMessageQ, &pattern, &sdata[0]);

    key_pre = CONT_A | CONT_B | CONT_L;
    key_post = CONT_A | CONT_B | CONT_R;
    key_reset = CONT_A | CONT_B | CONT_G;
    key_exit = CONT_A | CONT_B | CONT_START;

    if (which == 0)
        sprintf(title, " Local Controller Test");
    else 
        sprintf(title, "Dummy Controller%d Test", which);
    printstr(white, 8, 2, title);

    printstr(red, 3, 13, "Red:current");
    printstr(gray, 15, 13,  "Gray:done");
    printstr(white, 25, 13, "White:not yet");
    osWritebackDCacheAll();

    end = OS_CYCLES_TO_USEC(osGetTime()) + 500000;
    while (OS_CYCLES_TO_USEC(osGetTime()) < end);

    for (i=0; i<18; i++) done[i] = 0;
    for (; ;) {
        if (IO_READ(MI_EINTR_REG) & MI_EINTR_BUT_STATUS) 
            my_poweroff(); 

        osContStartReadData(&siMessageQ);
        osRecvMesg(&siMessageQ, NULL, OS_MESG_BLOCK);
        osContGetReadData(ctrl_data);

        /* check back door */
        if ((ctrl_data[0].button & key_pre) == key_pre) return -1;
        if ((ctrl_data[0].button & key_post) == key_post) return 1;
        if ((ctrl_data[0].button & key_exit) == key_exit) return 0x100;
        if ((ctrl_data[0].button & key_reset) == key_reset) 
            for (i=0; i<18; i++) done[i] = 0;

        if (ctrl_data[which].errno &CONT_NO_RESPONSE_ERROR) {
            for (i=0; i<18; i++) 
                done[i] = 0;

           if (!no_response_before) { /* not need to clean screen */
               for (i=0; i<40; i++) title[i] = 'A';
               title[40] = '\0';
               for (i=3; i<12; i++)
                   printstr(BGCOLOR, 0, i, title);
           }

            printstr(red, 5, 7, "ERROR:");
            if (which) {
                sprintf(title, "Pls plug Dummy controller %d", which);
                printstr(red, 7, 8, title);
            } else printstr(red, 10, 8, "Local controller not response");
            osWritebackDCacheAll();
            no_response_before = 1;
            continue;
        } else {
            if (no_response_before) {
                for (i=0; i<40; i++) title[i] = 'A';
                title[40] = '\0';
                printstr(BGCOLOR, 0, 7, title);
                printstr(BGCOLOR, 0, 8, title);
            }

            no_response_before = 0;
            x = ctrl_data[which].stick_x ;
            y = ctrl_data[which].stick_y ;
            key = ctrl_data[which].button;

            color[0] = (y > JOY_XY_MIN) ? red : (done[0] ? gray : white);
            done[0] |= (y > JOY_XY_MIN) ? 1 : 0;
            color[1] = (x < -JOY_XY_MIN) ? red : (done[1] ? gray : white);
            done[1] |= (x < -JOY_XY_MIN) ? 1 : 0;
            color[2] = (x > JOY_XY_MIN) ? red : (done[2] ? gray : white);
            done[2] |= (x > JOY_XY_MIN) ? 1 : 0;
            color[3] = (y < -JOY_XY_MIN) ? red : (done[3] ? gray : white);
            done[3] |= (y < -JOY_XY_MIN) ? 1 : 0;
       
            color[4] = (key & CONT_UP) ? red : (done[4] ? gray : white);
            done[4] |= (key & CONT_UP) ? 1 : 0;
            color[5] = (key & CONT_LEFT) ? red : (done[5] ? gray : white);
            done[5] |= (key & CONT_LEFT) ? 1 : 0;
            color[6] = (key & CONT_RIGHT) ? red : (done[6] ? gray : white);
            done[6] |= (key & CONT_RIGHT) ? 1 : 0;
            color[7] = (key & CONT_DOWN) ? red : (done[7] ? gray : white);
            done[7] |= (key & CONT_DOWN) ? 1 : 0;

            color[8] = (key & CONT_L) ? red : (done[8] ? gray : white);
            done[8] |= (key & CONT_L) ? 1 : 0;
            color[9] = (key & CONT_R) ? red : (done[9] ? gray : white);
            done[9] |= (key & CONT_R) ? 1 : 0;
            color[10] = (key & CONT_G) ? red : (done[10] ? gray : white);
            done[10] |= (key & CONT_G) ? 1 : 0;
            color[11] = (key & CONT_START) ? red : (done[11] ? gray : white);
            done[11] |= (key & CONT_START) ? 1 : 0;

            color[12] = (key & CONT_E) ? red : (done[12] ? gray : white);
            done[12] |= (key & CONT_E) ? 1 : 0;
            color[13] = (key & CONT_C) ? red : (done[13] ? gray : white);
            done[13] |= (key & CONT_C) ? 1 : 0;
            color[14] = (key & CONT_F) ? red : (done[14] ? gray : white);
            done[14] |= (key & CONT_F) ? 1 : 0;
            color[15] = (key & CONT_D) ? red : (done[15] ? gray : white);
            done[15] |= (key & CONT_D) ? 1 : 0;

            color[16] = (key & CONT_A) ? red : (done[16] ? gray : white);
            done[16] |= (key & CONT_A) ? 1 : 0;
            color[17] = (key & CONT_B) ? red : (done[17] ? gray : white);
            done[17] |= (key & CONT_B) ? 1 : 0;
        }

        printstr(color[0], 10, 6, "JT"); 
        printstr(color[1], 3, 7,  "JL");
        printstr(color[2], 17, 7, "JR");
        printstr(color[3], 10, 8, "JB");
        if (color[0] != black) {
            key=sprintf(title, "(%d,", x);
            for (i=key; i<6; i++) sp[i-key] = 'A';
            sp[6-key] = '\0';
            printstr(BGCOLOR, 6, 7, sp);
            printstr(gray, 12-key, 7, title);
            
            key=sprintf(title, "%d)", y);
            for (i=key; i<5; i++) sp[i-key] = 'A';
            sp[5-key] = '\0';
            printstr(gray, 12, 7, title);
            printstr(BGCOLOR, 12+key, 7, sp);
        } else printstr(BGCOLOR, 6, 7, "(-128,-128)");
         
        printstr(color[4], 10, 10, "Top");
        printstr(color[5], 3,  11, "Left");
        printstr(color[6], 17, 11, "Right");
        printstr(color[7], 10, 12, "Bottom");

        printstr(color[8], 5, 4, "L");
        printstr(color[9], 32, 4, "R");
        printstr(color[10], 8, 4, "Z");
        printstr(color[11], 20, 5, "Start");

        printstr(color[12], 32, 6, "E");
        printstr(color[13], 29, 7, "C");
        printstr(color[14], 35, 7, "F");
        printstr(color[15], 32, 8, "D");

        printstr(color[16], 28, 10, "A");
        printstr(color[17], 25, 9, "B");

        osWritebackDCacheAll();
        for (i=key=0; i<18; i++) key += done[i];
        if (key == 18) return 0;
    }

    return 0;
}

int my_strcat(char *dst, const char *append)
{
    char *stmp = dst, *sadd=(char *) append;

    while (*stmp) stmp++;
    while (*sadd)
        *stmp++ = *sadd++;

    *stmp = '\0';
    return 0;
}

int my_strlen(char *s)
{
    int len=0;

    if (s == NULL)
        return 0;

    while (*s) {
        len++;
        s++;
    }
    return len;
}

static void
controller_test_all()
{
    OSContStatus     sdata[MAXCONTROLLERS];
    OSContPad        ctrl_data[MAXCONTROLLERS];
    char pattern, msg[128], sp[128];
    char *cont[MAXCONTROLLERS]= {"Local", "Dummy1", "Dummy2", "Dummy3"};
    int  i, key, len, wh;

    osSetEventMesg(OS_EVENT_SI, &siMessageQ, (OSMesg)NULL);
    osContInit(&siMessageQ, &pattern, &sdata[0]);
    printstr(white, 8, 2, "BB Controllers Test");
    printstr(white, 3, 13, "Press power button to poweroff");
    osWritebackDCacheAll();

    for (; ;) {
        if (IO_READ(MI_EINTR_REG) & MI_EINTR_BUT_STATUS)
            my_poweroff();

        osContStartReadData(&siMessageQ);
        osRecvMesg(&siMessageQ, NULL, OS_MESG_BLOCK);
        osContGetReadData(ctrl_data);

        for (wh=0; wh<MAXCONTROLLERS; wh++) {
            if (ctrl_data[wh].errno & CONT_NO_RESPONSE_ERROR) {
                len = sprintf(msg, "%s:  not response", cont[wh]);
                key = 0;
            } else {
                len = sprintf(msg, "%s:  (%d, %d)", 
                              cont[wh], ctrl_data[wh].stick_x, ctrl_data[wh].stick_y);
                key = ctrl_data[wh].button;
            }

            if (len >= 36) msg[36] = '\0';
            printstr(white, 4, 4+(wh<<1), msg);
            if (len < 36) {
                for (i=len; i<36; i++) sp[i-len] = 'A';
                sp[36-len] = '\0';
                printstr(BGCOLOR, 4+len, 4+(wh<<1), sp); 
            }
         
            msg[0] ='\0';
            if (key & CONT_A) my_strcat(msg, " A");
            if (key & CONT_B) my_strcat(msg, " B");
            if (key & CONT_G) my_strcat(msg, " Z");
            if (key & CONT_L) my_strcat(msg, " L");
            if (key & CONT_R) my_strcat(msg, " R");
            if (key & CONT_E) my_strcat(msg, " E");
            if (key & CONT_C) my_strcat(msg, " C");
            if (key & CONT_F) my_strcat(msg, " F");
            if (key & CONT_D) my_strcat(msg, " D");
            if (key & CONT_UP) my_strcat(msg, " Up");
            if (key & CONT_DOWN) my_strcat(msg, " Down");
            if (key & CONT_LEFT) my_strcat(msg, " Left");
            if (key & CONT_RIGHT) my_strcat(msg, " Right");
            if (key & CONT_START) my_strcat(msg, " Start");
            if (key) my_strcat(msg, "  pressed");

            len  =  my_strlen(msg);
            if (len >= 33) msg[33] = '\0';
            printstr(white, 7, 5+(wh<<1), msg);
            if (len < 33) {
                for (i=len; i<33; i++) sp[i-len] = 'A';
                sp[33 - len] = '\0';
                printstr(BGCOLOR, 7+len, 5+(wh<<1), sp);
            }
        }
        osWritebackDCacheAll();
    }

    return;
    
}

static void 
mainproc(char *argv)		{
    int dummy, ret;
    __osBbHackFlags = 0;
    osCreateMesgQueue(&siMessageQ, &siMessageBuf, 1);
    osSetEventMesg(OS_EVENT_SI, &siMessageQ, dummyMessage);

    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
    osViBlack(1);
    osViSwapBuffer(cfb);
    clear(0x0);
    osViBlack(0);
    osWritebackDCacheAll();
    osViSwapBuffer(cfb);

    for (dummy=1;;) {
        ret = controller_test(dummy);

        osViBlack(1);
        osViSwapBuffer(cfb);
        clear(0x0);
        osViBlack(0);

        if (((ret == 0) && (dummy >= 3)) || (ret  > 3)) 
           break;

        dummy += (ret == -1) ? -1 : 1;
        if (dummy > 3) dummy = 1;
        if (dummy < 1) dummy = 3;
    }

    controller_test_all();
    for (;;);
}