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

#include "cardstress.h"


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


/*
 * 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]);
#ifdef SCREEN_LOW
    fbInit(FB_LOW_RES);
#else
    fbInit(FB_HIGH_RES);
#endif

    /*
     * 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	retraceMessageQ;
static OSMesg		dummyMessage, retraceMessageBuf;
static u8 block[BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
static u8 spare[BB_FL_SPARE_SIZE];
//#define DESTRUCTIVE
#ifdef DESTRUCTIVE
static u8 blockw[BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
#endif

#ifdef SCREEN_LOW
static u16 cfb[2][320*240] __attribute__((aligned(64)));
#else
static u16 cfb[2][640*480] __attribute__((aligned(64)));
#endif
int dbpos, sbpos, mbpos;

static void 
mainproc(char *argv)		{
    u8 status;
    s32 rv, i, pass = 0, db = 0, sb = 0, mb = 0, sbcount = 0;
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
    osViBlack(1);
    osViSwapBuffer(cfb);
    fbSetBg(fbBlack);
    fbClear();
    osViBlack(0);
    osWritebackDCacheAll();

    osBbCardInit();
    if ((rv == osBbCardStatus(0, &status)) < 0) {
	PRINTF("osBbCardStatus failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbCardBlocks(0)) < 0) {
	PRINTF("osBbBlocks failed %d\n", rv);
	goto out;
    }

#ifdef DESTRUCTIVE
    fbClear();
    fbPrintf(fbWhite, 12, 2, "Destructive Card Test");
    fbPrintf(fbWhite, 12, 3, "Remove Current Card ...");
    osWritebackDCacheAll();
    while(!osBbCardUnhappy()) ;
    /* wait two seconds */
    for(i = 0; i < 2*30; i++)
	osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
    fbClear();
    osWritebackDCacheAll();
#endif


    for(;;) {
	int update = 1;
	int restart = 0;

again:
	osBbCardInit();
	if (osBbCardUnhappy()) {
	    osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
	    restart = 1;
	    goto again;
	}
#ifdef DESTRUCTIVE
	{
	static u8 pattern = 0xa5;
	pattern ^= 0xff;
	for(i = 0; i < BB_FL_BLOCK_SIZE; i++) {
	    blockw[i] = pattern;
	}
	}
#endif
	fbClear();
	fbPrintf(fbWhite, 12, 2, "          ");
	fbPrintf(fbWhite, 12, 2, "Pass %d", pass++);
#ifdef DESTRUCTIVE
	fbPrintf(fbWhite, 22, 2, "Destructive");
#endif

	db = sb = mb = 0;
	dbpos = sbpos = mbpos = 0;
	for(i = 0; i < osBbCardBlocks(0); i++) {
	    extern u32 __osBbCardSbErr;
	    int good = 1;
	    spare[BB_FL_BLOCK_STATUS_OFF] = 0;
	    if ((rv = osBbCardReadBlock(0, i, block, spare)) < 0) {
		//PRINTF("read block %d failed %d\n", i, rv);
		//goto out;
		PRINTF("@");
#ifdef DESTRUCTIVE
dberror:
#endif
		fbPrintf(fbWhite, dbpos >= 10 ? 10 : 4, 4+(dbpos++ % 10), "%c%4d", spare[BB_FL_BLOCK_STATUS_OFF] != 0xff ? '*' : ' ', i);
		db++;
		update = 1;
		good = 0;
	    }
	    if (spare[BB_FL_BLOCK_STATUS_OFF] != 0xff) {
		mb++;
		fbPrintf(fbWhite, mbpos >= 10 ? 33 : 28, 4+(mbpos++ % 10), "%4d", i);
		good = 0;

	    }
#ifdef DESTRUCTIVE
noerror:
#endif
	    if (sbcount != __osBbCardSbErr) {
		sbcount = __osBbCardSbErr;
		sb++;
		fbPrintf(fbWhite, sbpos >= 10 ? 21 : 16, 4+(sbpos++ % 10), "%4d", i);
		update = 1;
	    }
	    if (i % 256 == 0) {
		PRINTF(".");
		fbPrintf(fbWhite, 4, 2, "%4d", i);
		update = 1;
	    }
	    if (update) {
		fbPrintf(fbWhite, 4, 3, "db %4d      sb %4d       mb %4d", db, sb, mb);
		osWritebackDCacheAll();
	    }
	    update = 0;
#ifdef DESTRUCTIVE
	    if (good) {
		good = 0;
		if ((rv = osBbCardEraseBlock(0, i)) < 0) {
		    PRINTF("erase %d failed\n", i);
		} else if ((rv = osBbCardWriteBlock(0, i, blockw, 0)) < 0) {
		    PRINTF("write %d failed\n", i);
		} else {
		    if ((rv = osBbCardReadBlock(0, i, block, spare)) < 0) {
			PRINTF("verify read %d failed\n", i);
			goto dberror;
		    }
		    if (bcmp(block, blockw, BB_FL_BLOCK_SIZE)) {
			PRINTF("verify compare failed\n", i);
			goto dberror;
		    }
		    goto noerror;
		}
	    }
#endif
	    if (osBbCardUnhappy()) break;
	}
	for(i = 0; i < 120; i++) {
	    osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
	}
    }
out:
#ifdef _DEBUG
    osExit();
#else
#endif
}