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

// #define REGISTER_TEST
// #define BYPASS

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

/*
 * Local variables and routines
 */
static u16 cfb[640*480] __attribute__((aligned(64)));

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

void __osBbDelay(u32 usec);
u32 __osDisableInt(void);
void __osRestoreInt(u32 mask);
void __osBbVideoPllInit(s32 tvType);

#ifdef BYPASS
static void vpll_bypass()
{
    u32 avctrl;
    u32 mask;

    mask = __osDisableInt();

    /* Stop the VI and AI - in case we are running */
    IO_WRITE(VI_CONTROL_REG, 0x0);
    IO_WRITE(AI_CONTROL_REG, 0x0);
    __osBbDelay(50);

    /* Reset the VI and AI, but leave PLL running */
    avctrl = IO_READ(MI_AVCTRL_REG);
    avctrl &= ~MI_AVCTRL_AV_RESET;

    IO_WRITE(MI_AVCTRL_REG, avctrl);
    __osBbDelay(50);

    /* Put the PLL in standby mode and bypass it */
    IO_WRITE(MI_AVCTRL_REG, avctrl | MI_AVCTRL_PLL_BYPASS | MI_AVCTRL_VPLL_STANDBY | MI_AVCTRL_DAC_POWER);
    __osBbDelay(50);
    avctrl = IO_READ(MI_AVCTRL_REG);

    /* Take the AI/VI out of reset */
    IO_WRITE(MI_AVCTRL_REG, avctrl |
             MI_AVCTRL_AV_RESET);
    IO_READ(MI_AVCTRL_REG);
           
    __osRestoreInt(mask);
}
#endif

void
boot(void)
{
#ifdef BYPASS
    vpll_bypass(OS_TV_NTSC);
#else
    __osBbVideoPllInit(OS_TV_NTSC);
#endif
    osInitialize();
    osCreateThread(&idleThread, 1, (void(*)(void *))idleproc, (void *)0,
                   idleThreadStack+STACKSIZE, 8);
    osStartThread(&idleThread);
}


static void
idleproc(char *argv)		/* priority 8 */
{
    /*
     * 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 SIGNATURE 0x43210123
static void
sig_put()
{
    u32 ctrl = IO_READ(VI_CONTROL_REG);

    IO_WRITE(VI_CONTROL_REG, 0x880);
    IO_WRITE(VI_SPANADDR_REG, 0);
    IO_WRITE(VI_SPANDATA_REG, SIGNATURE);

    IO_WRITE(VI_SPANADDR_REG, 1);
    IO_WRITE(VI_SPANDATA_REG, 0);
    IO_WRITE(VI_SPANADDR_REG, 2);
    IO_WRITE(VI_SPANDATA_REG, 0);
    IO_WRITE(VI_SPANADDR_REG, 3);
    IO_WRITE(VI_SPANDATA_REG, 0);

    IO_WRITE(VI_CONTROL_REG, ctrl);

}

static int
sig_check()
{
    u32 sig;

    u32 ctrl = IO_READ(VI_CONTROL_REG);
    IO_WRITE(VI_CONTROL_REG, 0x880);
    IO_WRITE(VI_SPANADDR_REG, 0);
    sig = IO_READ(VI_SPANDATA_REG);

    IO_WRITE(VI_CONTROL_REG, ctrl);

    if (sig == SIGNATURE) {
        return 1;
    } else {
        return 0;
    }

}

#ifdef REGISTER_TEST
static void wack_registers()
{
    u32 delay;

    IO_WRITE(VI_CONTROL_REG, 0x0);
    __osBbDelay(10);

    // Fill up the with some randoms
    IO_WRITE(VI_ORIGIN_REG, 0x0);
    IO_WRITE(VI_WIDTH_REG, 0x0);
    IO_WRITE(VI_INTR_REG, 0x3ff);
    IO_WRITE(VI_BURST_REG, 0xd1);
    IO_WRITE(VI_V_SYNC_REG, 0x0);
    IO_WRITE(VI_H_SYNC_REG, 0xd2047);
    IO_WRITE(VI_LEAP_REG, 0x0);
    IO_WRITE(VI_H_START_REG, 0x0);
    IO_WRITE(VI_V_START_REG, 0x0);
    IO_WRITE(VI_V_BURST_REG, 0x0);
    IO_WRITE(VI_X_SCALE_REG, 0x0);
    IO_WRITE(VI_Y_SCALE_REG, 0x0);

    delay = osGetCount() & 0xff;
    __osBbDelay(delay);

    // Fill up the registers
    IO_WRITE(VI_ORIGIN_REG, &cfb[0]);
    IO_WRITE(VI_WIDTH_REG, 0x140);
    IO_WRITE(VI_INTR_REG, 0x208);
    IO_WRITE(VI_BURST_REG, 0x3e52239);
    IO_WRITE(VI_V_SYNC_REG, 0x20d);
    IO_WRITE(VI_H_SYNC_REG, 0xc15);
    IO_WRITE(VI_LEAP_REG, 0xc150c15);
    IO_WRITE(VI_H_START_REG, 0x6c02ec);
    IO_WRITE(VI_V_START_REG, 0x2501ff);
    IO_WRITE(VI_V_BURST_REG, 0xe0204);
    IO_WRITE(VI_X_SCALE_REG, 0x200);
    IO_WRITE(VI_Y_SCALE_REG, 0x400);

    sig_put();

    IO_WRITE(VI_CURRENT_REG, 0x0);
    IO_WRITE(VI_CONTROL_REG, 0x120e);

}
#endif

/*
 * Note this should return with the VI correctly out of reset and also
 * disabled. You can do video set up using libultra routines after this.
 * It needs some frame buffer area to work. Right now it should only use
 * the first 10 lines of the buffer.
 *
 * To keep things simple we use a 320 wide x 16 bits/pixel framebuffer
 *
 * Before this function is called the PLL should be set up.
 *
 * Interrupts are disabled. It may not matter, but just in case. This routine
 * should be called before osInitialize() anyway
 *
 */
static int reset_video(u16 *cfb)
{
    u32 avctrl;
    int cnt = 0;
    u32 mask;

    mask = __osDisableInt();
    /*
     * Clear a few lines in the Framebuffer first
     */
    memset(cfb, 0, 320*10*2);
  
    do {
        IO_WRITE(VI_CONTROL_REG, 0x0);
        __osBbDelay(10);
        
        // Reset the video subsystem
        avctrl = IO_READ(MI_AVCTRL_REG);
        avctrl &= ~MI_AVCTRL_AV_RESET;
        IO_WRITE(MI_AVCTRL_REG, avctrl);
        __osBbDelay(1);
        IO_WRITE(MI_AVCTRL_REG, avctrl | MI_AVCTRL_AV_RESET);
        
        // Fill up the registers
        IO_WRITE(VI_ORIGIN_REG, cfb);
        IO_WRITE(VI_WIDTH_REG, 0x140);
        IO_WRITE(VI_INTR_REG, 0x208);
        IO_WRITE(VI_BURST_REG, 0x3e52239);
        IO_WRITE(VI_V_SYNC_REG, 0x20d);
        IO_WRITE(VI_H_SYNC_REG, 0xc15);
        IO_WRITE(VI_LEAP_REG, 0xc150c15);
        IO_WRITE(VI_H_START_REG, 0x6c02ec);
        IO_WRITE(VI_V_START_REG, 0x2501ff);
        IO_WRITE(VI_V_BURST_REG, 0xe0204);
        IO_WRITE(VI_X_SCALE_REG, 0x200);
        IO_WRITE(VI_Y_SCALE_REG, 0x400);
        
        sig_put();
        
        IO_WRITE(VI_CURRENT_REG, 0x0);
        IO_WRITE(VI_CONTROL_REG, 0x120e);
        
        /*
         * Wait until some active lines have gone by
         */
        while (IO_READ(VI_CURRENT_REG) < 0x30) ;

        cnt++;
    } while (sig_check());
    
    __osRestoreInt(mask);

    return cnt-1;
        
}

static void wait_frame()
{
    u32 x;

    do {
        x = IO_READ(MI_INTR_REG);
    } while ((x & MI_INTR_VI) == 0);

    // clear interrupt
    IO_WRITE(VI_CURRENT_REG, 0x0);

}

static void 
mainproc(char *argv) 
{
    int i;
    char buf[64];
    int loop = 0;
    int stripe_counter = 0;
    int dstripe_counter = 0;
    int tstripe_counter = 0;
    int cnt;

    __osDisableInt();

    init_graphics(LOW_RES, &cfb[0]);

    while (1) {
#ifdef REGISTER_TEST
        wack_registers();
#else
        cnt = reset_video(&cfb[0]);
#endif
        switch (cnt) {
        case 1:
            stripe_counter++;
            break;
        case 2:
            dstripe_counter++;
            break;
        case 3:
            tstripe_counter++;
        default:
        }

        sprintf(buf, "Loop: %d", loop++);
        printstr(white, 2, 3, buf);
        sprintf(buf, "Single Stripe counter: %d", stripe_counter);
        printstr(white, 2, 4, buf);
        sprintf(buf, "Double Stripe counter: %d", dstripe_counter);
        printstr(white, 2, 5, buf);
        sprintf(buf, "Triple Stripe counter: %d", tstripe_counter);
        printstr(white, 2, 6, buf);
        osWritebackDCacheAll();

        for (i=0; i<30; i++) {
            wait_frame();
        }
    }
}