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

#include "videocb.h"
#include "graph.h"

// #define PAL 1

#define DMA_QUEUE_SIZE	200

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

static u32 cfb[640*480] __attribute__((aligned(64)));

static OSMesgQueue	retraceMessageQ;
static OSMesg		dummyMessage, retraceMessageBuf;

static int numTests = 0;
static int current = 0;
static int imageMode = 0;
static char testImage[20][32];

OSContStatus statusData[MAXCONTROLLERS];
OSContPad controllerData[MAXCONTROLLERS];

static int no_controller = 1;
static u16 lastbutton[MAXCONTROLLERS];

static u32 viHighResMode;
static u32 viLowResMode;

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

#ifdef PAL
    viHighResMode = OS_VI_PAL_HPF2;
    viLowResMode = OS_VI_PAL_LPN1;
    __osBbVideoPllInit(OS_TV_PAL);
#else
    viHighResMode = OS_VI_NTSC_HPF2;
    viLowResMode = OS_VI_NTSC_LPN1;
    __osBbVideoPllInit(OS_TV_NTSC);
#endif

    osCreateViManager(OS_PRIORITY_VIMGR);
    osViSetMode(&osViModeTable[viLowResMode]);

    /*
     * 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 void drawLine(int i) 
{
    printstr(i == current ? GPACK_RGBA5551(255,0,255,1): white, i >= numTests/2 ? 20 : 5, 4+(i>=numTests/2?i-numTests/2:i), testImage[i]);
}

static void redraw(void) {
    int i;
    char *title =  "J&B's Video Tests";

    // Make sure mode gets reset
    osViSetMode(&osViModeTable[viLowResMode]);
    gcls();
    printstr(markcolor, 4+16-strlen(title)/2, 2, title);
    for (i = 0; i < numTests; ++i) {
        drawLine(i);
    }

    osWritebackDCacheAll();
}

static u32 bars[8] = {0xbfbfbf00, // grey
                      0xbfbf0000, // yellow
                      0x00bfbf00, // cyan
                      0x00bf0000, // green
                      0xbf00bf00, // magenta
                      0xbf000000, // red
                      0x0000bf00, // blue
                      0x00000000  // black
};

static void colorBars()
{
    int i, j;
    for (i=0; i<640; i++) {
        for (j=0; j<480; j++) {
            cfb[j*640 + i] = bars[(i*8) / 640];
        }
    }
}

static void clear32(u32 color)
{
    int i, j;
    for (i=0; i<640; i++) {
        for (j=0; j<480; j++) {
            cfb[j*640 + i] = color;
        }
    }

}

static void resTest(u32 c0, u32 c1)
{
    float endWidth = 20.0;
    float lineWidth;
    u32 lineCount = 0;
    int i, j;

    for (j=0; j<480; j++) {
        i = 0;
        while (i<640) {
            lineWidth = 1.0 + i*(endWidth-1)/640;
            lineCount = 0;
            while ((lineCount < (u32) lineWidth) && (i<640)) {
                cfb[j*640 + i] = c0;
                i++;
                lineCount++;
            }
            lineCount = 0;
            while ((lineCount < (u32) lineWidth) && (i<640)) {
                cfb[j*640 + i] = c1;
                i++;
                lineCount++;
            }
        }
    }
}

static void fieldTest(u32 c0, u32 c1)
{
    // Alternating c0, c1 vertically
    // XXX Need to set the video mode differently for this test
    int i, j;
    for (j=0; j<480; j++) {
        for (i=0; i<640; i++) {
            if (j & 1) {
                cfb[j*640 + i] = c0;
            } else {
                cfb[j*640 + i] = c1;
            }                
        }
    }
}

static void ramp(u32 c0, u32 c1)
{
    int i, j;
    u32 r0, g0, b0;
    u32 r1, g1, b1;
    u32 rgb;

    r0 = (c0 >> 24) & 0xff;
    g0 = (c0 >> 16) & 0xff;
    b0 = (c0 >> 8) & 0xff;

    r1 = (c1 >> 24) & 0xff;
    g1 = (c1 >> 16) & 0xff;
    b1 = (c1 >> 8) & 0xff;

    for (j=0; j<480; j++) {
        for (i=0; i<640; i++) {
            rgb = (r0 + ((r1 - r0)*i) / 640) << 24;
            rgb |= ((g0 + ((g1 - g0)*i) / 640) & 0xff) << 16;
            rgb |= ((b0 + ((b1 - b0)*i) / 640) & 0xff) << 8;
            cfb[j*640 + i] = rgb;
        }
    }
}

static void drawImage(void) 
{
    /*
     * Switch video mode
     */
    osViSetMode(&osViModeTable[viHighResMode]);
    switch (current) {
    case 0:
        // Color bars
        colorBars();
        break;
    case 1:
        // White
        clear32(0xffffff00);
        break;
    case 2:
        // Black
        clear32(0x0);
        break;
    case 3:
        // W&B lines
        resTest(0xffffff00, 0x0);
        break;
    case 4:
        // R&B lines
        resTest(0xbf000000, 0x0);
        break;
    case 5:
        // W&B fields
        fieldTest(0xffffff00, 0x0);
        break;
    case 6:
        // B&W Ramp
        ramp(0x0, 0xbfbfbf00);
        break;
    case 7:
        // B&Y Ramp
        ramp(0x0, 0xbfbf0000);
        break;
    case 8:
        // 75 % red
        clear32(0xbf000000);
        break;
    default:
        break;

    }
}

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 processButton(u16 button, int lastbutton)
{
    int old = current;;
    if (current == -1) return;

    // If we are in the high res mode just get out of it and return;
    if (imageMode && (lastbutton != button)) {
        imageMode = 0;
        redraw();
        return;
    }

    if (button & D_JPAD && !(lastbutton & D_JPAD)) {
        current = (current + 1) % numTests;
    }

    if (button & U_JPAD && !(lastbutton & U_JPAD)) {
        current = (current - 1) < 0? numTests - 1: current-1;
    }

    if (button & R_JPAD && !(lastbutton & R_JPAD)) {
        if (current + numTests/2 < numTests) {
            current += numTests/2;
        }
    }

    if (button & L_JPAD && !(lastbutton & L_JPAD)) {
        if (current >= numTests/2) {
            current -= numTests/2;
        }
    }

    if ((button & A_BUTTON && !(lastbutton & A_BUTTON)) ||
        (button & START_BUTTON && !(lastbutton & START_BUTTON))) {
        printstr(red, 4, 13, "Launching: ");
        printstr(red, 15, 13, testImage[current]);
        drawImage();
        imageMode = 1;
        osWritebackDCacheAll();
    }

    if (old != current) {
        redraw();
    }
}

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

static void 
mainproc(char *argv)		
{

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

    osViBlack(1);
    osViSwapBuffer(cfb);

    init_graphics(LOW_RES);

    strcpy(testImage[numTests++], "Color bars");
    strcpy(testImage[numTests++], "White");
    strcpy(testImage[numTests++], "Black");
    strcpy(testImage[numTests++], "W&B lines");
    strcpy(testImage[numTests++], "R&W lines");
    strcpy(testImage[numTests++], "W&B fields");
    strcpy(testImage[numTests++], "B&W Ramp");
    strcpy(testImage[numTests++], "B&Y Ramp");
    strcpy(testImage[numTests++], "75% Red");

    redraw();

    for(;;) {
        osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
        osContStartReadData(&contMessageQ);
        readControllers();
    }

}