#include "ultra64.h"
#include <PR/os_bb.h>
#include <PR/bcp.h>
#include <libfb.h>

#include "state.h"

#define DMA_QUEUE_SIZE	200

/*
 * Thread and stack structures
 */
char   bootStack[STACKSIZE] __attribute__ ((aligned (8)));
static u16 cfb[320*240] __attribute__((aligned(64)));

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)
{
    /* 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);
    /*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 */
    {
    }
}

#define NUM_MESSAGE 1

static OSMesgQueue  pifMesgQueue;
static OSMesg       pifDummyMessage, pifMesgBuf[NUM_MESSAGE];
static OSContStatus sdata[MAXCONTROLLERS];

static OSMesgQueue	retraceMessageQ;
static OSMesg		dummyMessage, retraceMessageBuf;
static u8 errormsg[1024];

static char auxdatabuf[2*BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
static char buf[BB_MAX_AUXDATA_SIZE]  __attribute__((aligned(16)));
static char buf2[BB_MAX_AUXDATA_SIZE] __attribute__((aligned(16)));
static u8 long_buffer[EEPROM_BLOCK_SIZE * EEPROM_MAXBLOCKS] __attribute__((aligned(16)));

extern u32   __osBbStashMagic;
extern void* __osBbEepromAddress;
extern u32   __osBbEepromSize;
extern void* __osBbFlashAddress;
extern u32   __osBbFlashSize;
extern void* __osBbSramAddress;
extern u32   __osBbSramSize;
extern void* __osBbPakAddress[MAXCONTROLLERS];
extern u32   __osBbPakSize;
extern s32   __osBbPakBindings[MAXCONTROLLERS];
extern u8    __osBbStateName[];
extern u32   __osBbStateDirty;
extern u32   __osBbAuxDataLimit;

static void 
mainproc(char *argv)	
{
    u8 ids[BB_MAX_AUXDATA], p;
    int rv, i, n, size;
    u32 len;
    int* a = (int*) __osBbStateName;
    s32 eeprom = 0;
    u32 addr1, addr2;
    OSPiHandle* handler;

#ifdef DEBUG
    if ((rv = osBbUsbInit()) < 0) {
        return;
    }

    __osBbDelay(1500000);
    osSyncPrintf("state testing\n");
#endif

    /* setup text output to screen */
    fbInit(FB_LOW_RES);
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
    osViBlack(1);
    osViSwapBuffer(cfb);
    fbSetBg(fbBlack);
    fbClear();
    osViBlack(0);
    osWritebackDCacheAll();
    osViSwapBuffer(cfb);
    fbClear();

    /* Initialization for testing main game state */
	osCreateMesgQueue(&pifMesgQueue, pifMesgBuf, NUM_MESSAGE);
	osSetEventMesg(OS_EVENT_SI, &pifMesgQueue, pifDummyMessage);
	osContInit(&pifMesgQueue, &p, &sdata[0]);
    eeprom = osEepromProbe(&pifMesgQueue);
    
    /* Read from rom to test where the FS calls are corrupting
       the DMA */
    handler = osCartRomInit();
    osEPiReadIo(handler, 0x0, &addr1);

    /* Initialize buffer */
    bzero(buf, sizeof(buf));
    bzero(buf2, sizeof(buf2));
    len = sizeof(buf);

    fbPrintf(fbWhite, 3, 4, "M: %08x, S: %d, U: %d,%d",
             __osBbStashMagic, __osBbStateDirty,
             osBbAuxDataGetLimit(), __osBbAuxDataLimit);
    fbPrintf(fbWhite, 3, 5, "N: %08x %08x", a[0], a[1]);
    fbPrintf(fbWhite, 3, 6, "   %08x %08x", a[2], a[3]);
    fbPrintf(fbWhite, 3, 7, "EE: %d %08x %d", __osBbEepromSize,
             __osBbEepromAddress, eeprom);
    fbPrintf(fbWhite, 3, 8, "F: %d %08x, S: %d %08x", 
             __osBbFlashSize, __osBbFlashAddress, 
             __osBbSramSize, __osBbSramAddress);
    fbPrintf(fbWhite, 3, 9, "P: %d %d %d %d %d",
             __osBbPakSize, __osBbPakBindings[0], __osBbPakBindings[1],
             __osBbPakBindings[2],  __osBbPakBindings[3]);
    fbPrintf(fbWhite, 3, 10, "   %08x %08x",  __osBbPakAddress[0], __osBbPakAddress[1]);
    fbPrintf(fbWhite, 3, 11, "   %08x %08x",  __osBbPakAddress[2], __osBbPakAddress[3]);    

    osWritebackDCacheAll();

    fbPrintf(fbWhite, 3, 2, "Test osBbAuxDataInit");
    /* Test error conditions */
    /* osBbAuxDataInit */
    /* Buffer NULL */
    rv = osBbAuxDataInit(NULL, sizeof(auxdatabuf));
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataInit = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Buffer not 16 byte aligned */
    rv = osBbAuxDataInit(auxdatabuf + 1, sizeof(auxdatabuf));
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataInit = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Buffer length too small */
    rv = osBbAuxDataInit(auxdatabuf, sizeof(auxdatabuf)-1);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataInit = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }

    /* Initialize aux data properly */
    rv = osBbAuxDataInit(auxdatabuf, sizeof(auxdatabuf));
    if (rv != 0) {
        sprintf(errormsg, "%d: osBbAuxDataInit = %d, %d expected", 
                __LINE__, rv, 0);
        goto out;
    }

    /* Do main test data test */
    fbPrintf(fbWhite, 3, 2, "Test osBbGameCommitState");
    fbPrintf(fbWhite, 3, 3, "Test eeprom                ");
    if (eeprom) {
        for (i  = 0; i < EEPROM_BLOCK_SIZE * EEPROM_MAXBLOCKS; i++) {
            long_buffer[i] = i & 0xff;
        }
        rv = osEepromLongWrite(&pifMesgQueue, 0, long_buffer, 
                               EEPROM_BLOCK_SIZE * EEPROM_MAXBLOCKS);
    }

    fbPrintf(fbWhite, 3, 3, "Test Commit                ");
    rv = osBbGameCommitState();
    fbPrintf(fbWhite, 3, 13, "Commit: %d", rv);
    osWritebackDCacheAll();

    /* Starting aux data test */
    fbPrintf(fbWhite, 3, 2, "Test aux data state saving");
    fbPrintf(fbWhite, 3, 3, "Getting aux data ids         ");
    osWritebackDCacheAll();

    /* Delete all aux data */
    n = osBbAuxDataIds(ids);

    fbPrintf(fbWhite, 3, 3, "Delete old aux data  %d      ", n);
    osWritebackDCacheAll();

    for (i = 0; i < n; i++) {
        size = osBbAuxDataSize(ids[i]);        
        osBbAuxDataDelete(ids[i]);
    }
    n = osBbAuxDataIds(ids);

    if (n != 0) {
        sprintf(errormsg, "%d: osBbAuxDataIds = %d, 0 expected", 
                __LINE__, n);
        goto out;
    }

    fbPrintf(fbWhite, 3, 3, "Test errors                ");
    osWritebackDCacheAll();

    /* Test error conditions */
    /* osBbAuxDataSize */
    /* Invalid Aux Data id */
    rv = osBbAuxDataSize(-1);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSize = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid Aux Data id */
    rv = osBbAuxDataSize(BB_MAX_AUXDATA);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSize = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* No such Aux Data id */
    rv = osBbAuxDataSize(9);
    if (rv != BBFS_ERR_ENTRY) {
        sprintf(errormsg, "%d: osBbAuxDataSize = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_ENTRY);
        goto out;
    }

    /* osBbAuxDataDelete */
    /* Invalid Aux Data id */
    rv = osBbAuxDataDelete(-1);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataDelete = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid Aux Data id */
    rv = osBbAuxDataDelete(BB_MAX_AUXDATA);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataDelete = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* No such Aux Data id */
    rv = osBbAuxDataDelete(9);
    if (rv != BBFS_ERR_ENTRY) {
        sprintf(errormsg, "%d: osBbAuxDataDelete = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_ENTRY);
        goto out;
    }

    /* osBbAuxDataSave */
    /* Over Aux Id Limit */
    rv = osBbAuxDataSave(osBbAuxDataGetLimit(), buf, len);
    if (rv != BBFS_ERR_STATE_LIMIT) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_STATE_LIMIT);
        goto out;
    }
    /* Invalid Aux Data id */
    rv = osBbAuxDataSave(-4, buf, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid Aux Data id */
    rv = osBbAuxDataSave(BB_MAX_AUXDATA, buf, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid buffer address - NULL */
    rv = osBbAuxDataSave(1, NULL, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid buffer address (not 16 byte aligned) */
    rv = osBbAuxDataSave(0, buf+1, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid length - not multiple of 16K */
    rv = osBbAuxDataSave(3, buf, 734);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid length - negative */
    rv = osBbAuxDataSave(2, buf, -1);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid length - too big */
    rv = osBbAuxDataSave(3, buf, BB_MAX_AUXDATA_SIZE*2);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }

    /* osBbAuxDataLoad */
    /* No such Aux Data id */
    rv = osBbAuxDataLoad(1, buf, len);
    if (rv != BBFS_ERR_ENTRY) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_ENTRY);
        goto out;
    }
    /* Invalid Aux Data id */
    rv = osBbAuxDataLoad(-4, buf, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid Aux Data id */
    rv = osBbAuxDataLoad(BB_MAX_AUXDATA, buf, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid buffer address - NULL */
    rv = osBbAuxDataLoad(1, NULL, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid buffer address (not 16 byte aligned) */
    rv = osBbAuxDataLoad(0, buf+1, len);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid length - not a multiple of 16K */
    rv = osBbAuxDataLoad(3, buf, 734);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid length - negative */
    rv = osBbAuxDataLoad(2, buf, -1);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }
    /* Invalid length - too big */
    rv = osBbAuxDataLoad(3, buf, BB_MAX_AUXDATA_SIZE*2);
    if (rv != BBFS_ERR_INVALID) {
        sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                __LINE__, rv, BBFS_ERR_INVALID);
        goto out;
    }

    fbPrintf(fbWhite, 3, 3, "Create aux data           ");
    osWritebackDCacheAll();

    /* Create aux data */
    i = 0;
    while (i < len) {
        memset(buf+i, i/1024, 1024);
        i += 1024;
    }

    for (i = 1; i <= 2; i++) {
        len = i*16384;
        rv = osBbAuxDataSave(i, buf, len);
        if (rv != len) {
            sprintf(errormsg, "%d: osBbAuxDataSave = %d, %d expected", 
                    __LINE__, rv, len);
            goto out;
        }
        rv = osBbAuxDataSize(i);
        if (rv != len) {
            sprintf(errormsg, "%d: osBbAuxDataSize = %d, %d expected", 
                    __LINE__, rv, len);
            goto out;
        }
        rv = osBbAuxDataLoad(i, buf2, len);
        if (rv != len) {
            sprintf(errormsg, "%d: osBbAuxDataLoad = %d, %d expected", 
                    __LINE__, rv, len);
            goto out;
        }
        if (memcmp(buf, buf2, len) != 0) {
            sprintf(errormsg, "%d: osBbAuxDataLoad does not match saved data",
                    __LINE__);
            goto out;
        }
    }

    n = osBbAuxDataIds(ids);
    if (n != 2) {
        sprintf(errormsg, "%d: osBbAuxDataIds = %d, %d expected", 
                __LINE__, n, 2);
        goto out;
    }
    
    /* TODO: Fill up card and test */


    /* Everything okay */
    sprintf(errormsg, "State saving tests passed");

out:
    osEPiReadIo(handler, 0x0, &addr2);
    fbPrintf(fbWhite, 3, 12, "RA: %08x %08x", addr1, addr2);
    if (addr1 != addr2) 
        sprintf(errormsg, "%d: rom addr differs", __LINE__);

    fbPrintf(fbWhite, 3, 13, errormsg);
    osWritebackDCacheAll();

#ifdef DEBUG
    osSyncPrintf("%s\n", errormsg);
#endif

    while (1) {
        osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
    }
}