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

#include "d_pad.h"
#include "nu64sys.h"
#include "graph.h"


#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf
//#define IDE_PRINT

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 *);

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 */
}

#define __REG(x) "$" #x
#define __REG1(x) #x
#define getcp0reg(source)          	                        \
({ int __res;                                                   \
        __asm__ __volatile__(                                   \
	".set\tpush\n\t"					\
	".set\treorder\n\t"					\
        "mfc0\t%0,"__REG(source)"\n\t"                          \
	".set\tpop"						\
        : "=r" (__res));                                        \
        __res;})

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 BUT_DEBOUCE_TIME        5000
#define BUT_STABLE_TIME         1000000
#define BUT_RELEASED (!((IO_READ(MI_EINTR_REG)&MI_EINTR_BUTTON_PRESSED)))
#define BUT_STATE_CHANGED \
             (IO_READ(MI_EINTR_REG)&MI_INTR_BUT)
#define BUT_CLEAR_STATE_CHANGED \
        IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_CLR_BUT)
static void power_debouce()
{
    u64 end;

    /* debounce button pressed */
    do{
         while(BUT_STATE_CHANGED) {
             BUT_CLEAR_STATE_CHANGED;

             end = OS_CYCLES_TO_USEC(osGetTime()) + BUT_DEBOUCE_TIME;
             while (OS_CYCLES_TO_USEC(osGetTime()) < end);
         }
     } while(!BUT_RELEASED || BUT_STATE_CHANGED);

    IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_CLR_BUT);
    end = OS_CYCLES_TO_USEC(osGetTime()) + BUT_STABLE_TIME / 3;
    while (OS_CYCLES_TO_USEC(osGetTime()) < end);
    IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_SET_BUT);

    return;
}

#define JOY_XY_MIN  52
static void d_pad_test(void) 
{
    OSContStatus     sdata[MAXCONTROLLERS];
    OSContPad        ctrl_data[MAXCONTROLLERS];
    char pattern, joy[32];
    int key, down[4], color, i, bd_key=CONT_A|CONT_B|CONT_G;

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

#ifdef SYS_APP
    power_debouce();
#endif

#ifdef JOY_CH
    printstr(white, 12, 3, "BB Joy test 1.3");
    osWritebackDCacheAll();

    printstr(red, 6, 11, "Red: current reach max");
    printstr(gray, 6, 12,  "Gray: already reach max");
    printstr(white, 6, 13, "White: not reach max yet");
#else
    printstr(white, 12, 3, "BB D-PAD test 1.3");
    osWritebackDCacheAll();

    printstr(red, 6, 11, "Red: current pressed");
    printstr(gray, 6, 12,  "Gray: already pressed");
    printstr(white, 6, 13, "White: not pressed yet");
#endif

    for (key=0; key<4; key++) down[key] = 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);

        for (i=key=0; i<MAXCONTROLLERS; i++) {
             if (!(ctrl_data[i].errno &CONT_NO_RESPONSE_ERROR))
                 key |= ctrl_data[i].button;
        }

#ifdef JOY_CH
        color = (ctrl_data[0].stick_y  > JOY_XY_MIN) ? red : (down[0] ? gray : white);
        printstr(color, 18, 5, "Up");
        if (ctrl_data[0].stick_y > JOY_XY_MIN) down[0] = 1;

        color = (ctrl_data[0].stick_x < -JOY_XY_MIN) ? red : (down[1] ? gray : white);
        printstr(color, 6, 7, "Left");
        if (ctrl_data[0].stick_x < -JOY_XY_MIN) down[1] = 1;

        color = (ctrl_data[0].stick_x > JOY_XY_MIN) ? red : (down[2] ? gray : white);
        printstr(color, 30, 7, "Right");
        if (ctrl_data[0].stick_x > JOY_XY_MIN) down[2] = 1;

        color = (ctrl_data[0].stick_y < -JOY_XY_MIN) ? red : (down[3] ? gray : white);
        printstr(color, 18, 9, "Down");
        if (ctrl_data[0].stick_y < -JOY_XY_MIN) down[3] = 1;
 
        sprintf(joy, "(%d, %d)     ", ctrl_data[0].stick_x, ctrl_data[0].stick_y);
        printstr(gray, 15, 7, joy);
#else
        color = (key & CONT_UP) ? red : (down[0] ? gray : white);
        printstr(color, 20, 5, "Up");
        if (key & CONT_UP) down[0] = 1;

        color = (key & CONT_LEFT) ? red : (down[1] ? gray : white);
        printstr(color, 10, 7, "Left");
        if (key & CONT_LEFT) down[1] = 1;

        color = (key & CONT_RIGHT) ? red : (down[2] ? gray : white);
        printstr(color, 30, 7, "Right");
        if (key & CONT_RIGHT) down[2] = 1;

        color = (key & CONT_DOWN) ? red : (down[3] ? gray : white);
        printstr(color, 20, 9, "Down");
        if (key & CONT_DOWN) down[3] = 1;
#endif

        osWritebackDCacheAll();

        if ((key & bd_key) == bd_key) 
             for (i=0; i<4; i++) down[i] = 0;
    }

    return;
}

static void 
mainproc(char *argv)		{
    __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);

    d_pad_test();

    for (;;);
}