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

#include "libfb.h"
#include "flash.h"


#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf

/*
 * 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, contMessageQ;

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


#define SCREEN_LOW
#ifdef SCREEN_LOW
static u16 cfb[2][320*240] __attribute__((aligned(64)));
#else
static u16 cfb[2][640*480] __attribute__((aligned(64)));
#endif

static void
idleproc(char *argv)		/* priority 8 */
{
    osCreateViManager(OS_PRIORITY_VIMGR);
#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(&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;
static OSMesg		dummyMessage, retraceMessageBuf;
OSBbFs fs;

OSContStatus statusData[MAXCONTROLLERS];
OSContPad controllerData[MAXCONTROLLERS];
static u16 lastbutton[MAXCONTROLLERS];

static int no_controller = 1;
static void
initController(void)
{
    int i;
    u8 pattern;

    osCreateMesgQueue(&contMessageQ, &dummyMessage, 1);
    osSetEventMesg(OS_EVENT_SI, &contMessageQ, (OSMesg) 0);

    osContInit(&contMessageQ, &pattern, &statusData[0]);

    for (i = 0; i < MAXCONTROLLERS; i++) {
        if ((pattern & (1 << i)) &&
            !(statusData[i].errno & CONT_NO_RESPONSE_ERROR)) {
            no_controller = 0;
        }
    }
}

static void draw_map(BbFat16* fat, u16 nblocks, u16 bias);
static void draw_filemap(BbFat16* fat, u16 nblocks, u16 bias);

static void
processButton(u16 button, int lastbutton)
{
    int nblocks = osBbCardBlocks(0);
    static int bias, mode;
    extern BbFat16* __osBbFat;

    if (button & D_JPAD && !(lastbutton & D_JPAD)) {
	if (bias > 0)
	    bias -= 4096;
    }

    if (button & U_JPAD && !(lastbutton & U_JPAD)) {
	if (bias < nblocks-4096)
	    bias += 4096;
    }

    if (button & R_JPAD && !(lastbutton & R_JPAD)) {
    }

    if (button & L_JPAD && !(lastbutton & L_JPAD)) {
    }

    if (button & B_BUTTON && !(lastbutton & B_BUTTON)) {
	mode ^= 1;
    }

    if (button & A_BUTTON && !(lastbutton & A_BUTTON)) {
    }

    if (button & START_BUTTON && !(lastbutton & START_BUTTON)) {
	osExit();
    }

    if (mode)
	draw_filemap(__osBbFat, osBbCardBlocks(0), bias);
    else
	draw_map(__osBbFat, osBbCardBlocks(0), bias);
    {
    int i;
    u16 bad = 0;
    u16 used = 0;
    BbFat16* fat = __osBbFat;
    for(i = 0; i < osBbCardBlocks(0); i++) {
	u16 e = BB_FAT16_NEXT(fat, i);
	if (e == BB_FAT_BAD) bad++;
	else if (e != BB_FAT_AVAIL) used++;
    }
    fbPrintf(fbWhite, 8, 14, "bad %d used %.1f free %.1f", bad, used*16.f/1024, (osBbCardBlocks(0)-used)*16.f/1024);
    }
    osWritebackDCacheAll();
}

static void
readControllers(void)
{
    int i;
    OSContPad *pad;

    osRecvMesg(&contMessageQ, &dummyMessage, OS_MESG_BLOCK); 
    osContGetReadData(controllerData);

    for (i = 0; i < MAXCONTROLLERS; ++i) {
        pad = &controllerData[i];
        if (pad->button) {
            processButton(pad->button, lastbutton[i]);
        }
        lastbutton[i] = pad->button;
    }
}

#define fbhBlue		GPACK_RGBA5551(0,0,80,1)
#define fbhGreen	GPACK_RGBA5551(0,80,0,1)
#define fbhRed		GPACK_RGBA5551(80,0,0,1)
#define fbhYellow	GPACK_RGBA5551(80,80,0,1)
#define fbhMagenta	GPACK_RGBA5551(80,0,80,1)
#define fbhCyan		GPACK_RGBA5551(0,80,80,1)

#define fbLemonChiffon		GPACK_RGBA5551(255,250,205,1)
#define fbLightSlateBlue	GPACK_RGBA5551(132,112,255,1)
#define fbLightPink1		GPACK_RGBA5551(255,174,185,1)
#define fbThistle2		GPACK_RGBA5551(238,210,238,1)
#define fbSeaGreen4		GPACK_RGBA5551(105,139,105,1)
#define fbTomato1		GPACK_RGBA5551(255,99,71,1)
#define fbTurquoise2		GPACK_RGBA5551(0,229,238,1)
#define fbSienna4		GPACK_RGBA5551(139,71,38,1)

static void
draw_map(BbFat16* fat, u16 nblocks, u16 bias) {
    int x, y;
    u16 col;
    fbRect(64, 16, 64+191, 16+191, fbhGreen);
    for(y = 0; y < 4096/64; y++) {
	for(x = 0; x < 64; x++) {
	    u16 e = BB_FAT16_NEXT(fat, bias+y*64+x);
	    col = fbhGreen;
	    if (e == BB_FAT_RESERVED) col = fbhYellow;
	    else if (e == BB_FAT_BAD) col = fbhRed;
	    else if (e != BB_FAT_AVAIL) col = fbhCyan;
	    fbRect(64+3*x, 16+3*y, 64+3*x+2, 16+3*y+2, col);
	}
    }
}

const u16 rainbow[] = {
    fbLemonChiffon,
    fbLightSlateBlue,
    fbLightPink1,
    fbSeaGreen4,
    fbTomato1,
    fbGray,
    fbTurquoise2,
    fbSienna4
};

static void
draw_filemap(BbFat16* fat, u16 nblocks, u16 bias) {
    int x, y, i;
    u16 col;
    fbRect(64, 16, 64+191, 16+191, fbhGreen);
    for(y = 0; y < 4096/64; y++) {
	for(x = 0; x < 64; x++) {
	    u16 e = BB_FAT16_NEXT(fat, bias+y*64+x);
	    if (e == BB_FAT_RESERVED) col = fbhYellow;
	    else if (e == BB_FAT_BAD) col = fbhRed;
	    else continue;
	    fbRect(64+3*x, 16+3*y, 64+3*x+2, 16+3*y+2, col);
	}
    }
    for(i = 0; i < BB_INODE16_ENTRIES; i++) {
	u16 b = fat->inode[i].block;
	if (!fat->inode[i].type) continue;
	col = rainbow[i&7];
	while(b != BB_FAT_LAST) {
	    if (b >= bias && b < bias+4096) {
		x = (b-bias) & 63;
		y = (b-bias) >> 6;
		fbRect(64+3*x, 16+3*y, 64+3*x+2, 16+3*y+2, col);
	    }
	    b = BB_FAT16_NEXT(fat, b);
	}
    }
}

static void 
mainproc(char *argv)		{
    int rv, which;
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
    osViBlack(1);
    osViSwapBuffer(cfb);
    initController();
    fbSetBg(fbBlack);
    fbClear();
    osViBlack(0);
    osWritebackDCacheAll();
    osViSwapBuffer(cfb[which = 1]);
    fbClear();

    for(;;) {
	if (osBbCardUnhappy() && (rv = osBbFInit(&fs)) < 0) {
	    PRINTF("fs init failed %d\n", rv);
	}
        osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
        osContStartReadData(&contMessageQ);
	osViSwapBuffer(cfb[which]);
        readControllers();
	which ^= 1;
    }
}