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

#include "bandwidth.h"


#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf
#define MSG_FAULT	0x10


/*
 * 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];
static OSMesgQueue      PiMessageQ;

static OSMesg           SiMessages[DMA_QUEUE_SIZE];
static OSMesgQueue      SiMessageQ;

/*
 * Local variables and routines
 */

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

void
boot(void)
{
    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);
    osViSetMode(&osViModeTable[OS_VI_NTSC_LPN1]);

    /*
     * Start PI Mgr for access to cartridge - start before the debugger
     */
    osCreatePiManager((OSPri) OS_PRIORITY_PIMGR, &PiMessageQ, PiMessages,
            DMA_QUEUE_SIZE);

    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	controllerMsgQ, retraceMessageQ;
static OSMesg		controllerMsgBuf, dummyMessage, retraceMessageBuf;
static OSContStatus	statusdata[MAXCONTROLLERS];
static OSContPad	controllerdata[MAXCONTROLLERS];

static int
initControllers(void) {
    OSMesgQueue     serialMsgQ;
    OSMesg          serialMsg;
    int             i, rv = -1;
    u8              pattern;

    osCreateMesgQueue(&serialMsgQ, &serialMsg, 1);
    osSetEventMesg(OS_EVENT_SI, &serialMsgQ, (OSMesg)1);

    osContInit(&serialMsgQ, &pattern, &statusdata[0]);

    osCreateMesgQueue(&controllerMsgQ, &controllerMsgBuf, 1);
    osSetEventMesg(OS_EVENT_SI, &controllerMsgQ, (OSMesg)0);

    for (i = 0; i < MAXCONTROLLERS; i++) {
	PRINTF("%d type 0x%x status 0x%x\n", i, statusdata[i].type, statusdata[i].status);
        if ((pattern & (1<<i)) &&
                !(statusdata[i].errno & CONT_NO_RESPONSE_ERROR))
            rv = i;
    }
    return rv;
}

static void
ReadController(int thecontroller) {
    static u16 button, lastbutton;
    static s16 stickX, stickY;      
    
    (void)osRecvMesg(&controllerMsgQ, NULL, OS_MESG_BLOCK);
    osContGetReadData(controllerdata);

    button = controllerdata[thecontroller].button;
    stickX = controllerdata[thecontroller].stick_x;
    stickY = controllerdata[thecontroller].stick_y;

    //PRINTF("%04d %04d ", stickX, stickY);
    if (button & CONT_UP)
	PRINTF("UP ");

	    
    if (button & CONT_DOWN)
	PRINTF("DOWN ");

    if (button & CONT_RIGHT)
	PRINTF("RIGHT ");
	    
    if (button & CONT_LEFT)
	PRINTF("LEFT ");


    if ((button & CONT_A) && !(lastbutton & CONT_A))
	PRINTF("A ");

    if ((button & CONT_B) && !(lastbutton & CONT_B))
	PRINTF("B ");

    if ((button & CONT_C) && !(lastbutton & CONT_C))
	PRINTF("C ");

    if ((button & CONT_F) && !(lastbutton & CONT_F))
	PRINTF("F ");

    if ((button & CONT_D) /* && !(lastbutton & CONT_D) */)
	PRINTF("D ");

    if ((button & CONT_E) /* && !(lastbutton & CONT_E) */)
	PRINTF("E ");


    if ((button & CONT_L) && !(lastbutton & CONT_L))
	PRINTF("L ");

    if ((button & CONT_R) && !(lastbutton & CONT_R))
	PRINTF("R ");

    if ((button & CONT_G) && !(lastbutton & CONT_G))
	PRINTF("G ");

    if ((button & CONT_START) && !(lastbutton & CONT_START))
	PRINTF("START ");
  
    PRINTF("\n");
    lastbutton = button;
}

#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;})

#define setcp0reg(register,value)                		\
        __asm__ __volatile__(                                   \
        "mtc0\t%0,"__REG(register)"\n\t"			\
	"nop"							\
        : : "r" (value));

#define ADDR		0x80040000
#define CMEM_WORD       (*(vu32*)ADDR)
#define UMEM_WORD       (*(vu32*)K0_TO_K1(ADDR))

#define CMEM64_WORD     (*(vu32*)PHYS_TO_K0(DDRRAM64_START))
#define UMEM64_WORD     (*(vu32*)PHYS_TO_K1(DDRRAM64_START))

#define STRIDE	(1*1024*1024/4)
static void
run(void) {
    int i, pass = 0;
    u32* x = (u32*)&UMEM_WORD;
    u32* y = (u32*)&UMEM_WORD+STRIDE;
    unsigned z;
again:
    i = STRIDE;
    z = getcp0reg(C0_COUNT);
    do {
	*x++ = 0;
    } while(i-- >= 0);
    PRINTF("%f\n", STRIDE*4.0/OS_CYCLES_TO_USEC(getcp0reg(C0_COUNT)-z));

    i = STRIDE; x = y - STRIDE;
    z = getcp0reg(C0_COUNT);
    do {
	*(volatile unsigned *)x++;
    } while(i-- >= 0);
    PRINTF("%f\n", STRIDE*4.0/OS_CYCLES_TO_USEC(getcp0reg(C0_COUNT)-z));

    i = STRIDE; x = y - STRIDE;
    z = getcp0reg(C0_COUNT);
    do {
	*y++ = *x++;
    } while(i-- > 0);
    PRINTF("%f\n", STRIDE*4.0/OS_CYCLES_TO_USEC(getcp0reg(C0_COUNT)-z));

    if (pass++ == 0) {
	x = (u32*)&CMEM_WORD;
	y = (u32*)&CMEM_WORD+STRIDE;
	goto again;
    }
    IO_READ(MI_SEC_MODE_REG);
}

static void 
mainproc(char *argv)		{
    int thecontroller;

    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);

    PRINTF("\n=> mainproc...\n");
    run();
    for(;;) ;
}