#include <ultra64.h>
#include "main.h"	// Various things defined here
#include "static.h"	// Static display lists
#include "stdio.h"	// For printf
#include "display.h"

FB              fb;
dynamic_stuff   *generate;
OSTask          *gentask;
Gfx             *glistp0;
Gfx             *glistp;
int             draw_buffer=0;
dynamic_stuff   dynamic[2];
OSTask          task[2];
u64             dram_stack[SP_DRAM_STACK_SIZE64];

char   *staticSegment = _codeSegmentEnd;


InitTextDisplay(void)
{
  fb.startx=2;
  fb.starty=1;
  fb.color=WHITE;
  fb.screen_wd=SCREEN_WD;
  fb.screen_ht=SCREEN_HT;
  fb.fb0=_cfb[0];
  fb.fb1=_cfb[1];
  fb.x=fb.startx;
  fb.y=fb.starty;
}

InitDisplay(void)
{
  generate = &(dynamic[draw_buffer]);
  glistp0  = &(generate->glist[0]);
  glistp   = glistp0;

  /* Tell RCP where each segment is */
  gSPSegment(glistp++, 0, 0x0);
  gSPSegment(glistp++, STATIC_SEGMENT, osVirtualToPhysical(staticSegment));
}

ClearScreenDL(void)
{
  /* Clear zbuffer */
  gSPDisplayList(glistp++, clearzbuffer_dl);

  /* Clear frame buffer */
  gDPSetColorImage(glistp++, G_IM_FMT_RGBA, G_IM_SIZ_16b, SCREEN_WD,
                   OS_K0_TO_PHYSICAL(_cfb[draw_buffer]));

  gDPSetFillColor(glistp++, GPACK_RGBA5551(255,255,255,255) << 16 |
                  GPACK_RGBA5551(255,255,255,255));

  gSPDisplayList(glistp++, clearcfb_dl);
}

InitDL(void)
{
  /* Initialize RSP and RDP state */

  gSPDisplayList(glistp++, init_dl);

  /* RSP and RDP setup, and screen clear */

  gSPDisplayList(glistp++, rdpinit2d_dl);
  gSPDisplayList(glistp++, rspinit2d_dl);
}

ExecDL(void)
{
  gDPFullSync(glistp++);
  gSPEndDisplayList(glistp++);

  /* Build graphics task to execute the display list we just built */
  gentask = &(task[draw_buffer ? 0 : 1]);
  gentask->t.type            = M_GFXTASK;
  gentask->t.flags           = 0x0;
  gentask->t.ucode_boot      = (u64*) rspbootTextStart;
  gentask->t.ucode_boot_size = ((int)rspbootTextEnd - (int)rspbootTextStart);
  gentask->t.ucode           = (u64*) gspSprite2DTextStart; /* use XBUS */
  gentask->t.ucode_data      = (u64*) gspSprite2DDataStart;
  gentask->t.ucode_size      = 4096;
  gentask->t.ucode_data_size = 2048;
  gentask->t.dram_stack      = (u64*) dram_stack;
  gentask->t.dram_stack_size = 1024;
  gentask->t.output_buff     = (u64*) NULL;
  gentask->t.output_buff_size= (u64*) NULL;
  gentask->t.yield_data_ptr  = (u64*) NULL; /* Graphics only - no yielding */
  gentask->t.yield_data_size = 0x0;
  gentask->t.data_ptr        = (u64*) glistp0;
  gentask->t.data_size       = ((int) glistp - (int) glistp0);
    
  /* Write back dirty cache lines that need to be read by the RCP */
  osWritebackDCache(generate, sizeof(dynamic_stuff));

  /* Fire up SP task */
  osSpTaskStart(gentask);

  /* wait for SP completion */
  (void)osRecvMesg(&rspMessageQ, NULL, OS_MESG_BLOCK);
  (void)osRecvMesg(&rdpMessageQ, NULL, OS_MESG_BLOCK);
}

EndDisplay(void)
{
  /* setup to swap buffers */
  osViSwapBuffer(_cfb[draw_buffer]);

  /* Make sure there isn't an old retrace in queue */
  if (MQ_IS_FULL(&retraceMessageQ))
     (void)osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
  /* Wait for Vertical retrace to finish swap buffers */
  (void)osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);

  draw_buffer ^= 1;
}


InitImage(void)
{
  fb.startx=2;
  fb.starty=1;
  fb.color=WHITE;
  fb.screen_wd=SCREEN_WD;
  fb.screen_ht=SCREEN_HT;
  fb.fb0=_cfb[0];
  fb.fb1=_cfb[1];

//  osViBlack(0);

  fb.x=fb.startx;
  fb.y=fb.starty;

  generate = &(dynamic[draw_buffer]);
  glistp0  = &(generate->glist[0]);
  glistp   = glistp0;

  /* Tell RCP where each segment is */
  gSPSegment(glistp++, 0, 0x0);
  gSPSegment(glistp++, STATIC_SEGMENT, osVirtualToPhysical(staticSegment));

  /* Clear zbuffer */
  gSPDisplayList(glistp++, clearzbuffer_dl);

  /* Clear frame buffer */
  gDPSetColorImage(glistp++, G_IM_FMT_RGBA, G_IM_SIZ_16b, SCREEN_WD,
                   OS_K0_TO_PHYSICAL(_cfb[draw_buffer]));

  gDPSetFillColor(glistp++, GPACK_RGBA5551(128,0,128,255) << 16 |
                  GPACK_RGBA5551(128,0,128,255));

  gSPDisplayList(glistp++, clearcfb_dl);

  /* Initialize RSP and RDP state */

  gSPDisplayList(glistp++, init_dl);

  /* RSP and RDP setup, and screen clear */

  gSPDisplayList(glistp++, rdpinit2d_dl);
  gSPDisplayList(glistp++, rspinit2d_dl);
}

DrawImage(void)
{
  gDPFullSync(glistp++);
  gSPEndDisplayList(glistp++);

  /* Build graphics task to execute the display list we just built */
  gentask = &(task[draw_buffer ? 0 : 1]);
  gentask->t.type            = M_GFXTASK;
  gentask->t.flags           = 0x0;
  gentask->t.ucode_boot      = (u64*) rspbootTextStart;
  gentask->t.ucode_boot_size = ((int)rspbootTextEnd - (int)rspbootTextStart);
  gentask->t.ucode           = (u64*) gspSprite2DTextStart; /* use XBUS */
  gentask->t.ucode_data      = (u64*) gspSprite2DDataStart;
  gentask->t.ucode_size      = 4096;
  gentask->t.ucode_data_size = 2048;
  gentask->t.dram_stack      = (u64*) dram_stack;
  gentask->t.dram_stack_size = 1024;
  gentask->t.output_buff     = (u64*) NULL;
  gentask->t.output_buff_size= (u64*) NULL;
  gentask->t.yield_data_ptr  = (u64*) NULL; /* Graphics only - no yielding */
  gentask->t.yield_data_size = 0x0;
  gentask->t.data_ptr        = (u64*) glistp0;
  gentask->t.data_size       = ((int) glistp - (int) glistp0);
    
  /* Write back dirty cache lines that need to be read by the RCP */
  osWritebackDCache(generate, sizeof(dynamic_stuff));

  /* Fire up SP task */
  osSpTaskStart(gentask);

  /* wait for SP completion */
  (void)osRecvMesg(&rspMessageQ, NULL, OS_MESG_BLOCK);
  (void)osRecvMesg(&rdpMessageQ, NULL, OS_MESG_BLOCK);

  /* setup to swap buffers */
  osViSwapBuffer(_cfb[draw_buffer]);

  /* Make sure there isn't an old retrace in queue */
  if (MQ_IS_FULL(&retraceMessageQ))
     (void)osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
  /* Wait for Vertical retrace to finish swap buffers */
  (void)osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);

  draw_buffer ^= 1;
}

InitDraw(void)
{
  fb.startx=2;
  fb.starty=1;
  fb.color=WHITE;
  fb.screen_wd=SCREEN_WD;
  fb.screen_ht=SCREEN_HT;
  fb.fb0=_cfb[0];
  fb.fb1=_cfb[1];

  osViBlack(0);

  fb.x=fb.startx;
  fb.y=fb.starty;

  generate = &(dynamic[draw_buffer]);
  glistp0  = &(generate->glist[0]);
  glistp   = glistp0;

  /* Tell RCP where each segment is */
  gSPSegment(glistp++, 0, 0x0);
  gSPSegment(glistp++, STATIC_SEGMENT, osVirtualToPhysical(staticSegment));

  /* Clear zbuffer */
  gSPDisplayList(glistp++, clearzbuffer_dl);

  /* Clear frame buffer */
  gDPSetColorImage(glistp++, G_IM_FMT_RGBA, G_IM_SIZ_16b, SCREEN_WD,
                   OS_K0_TO_PHYSICAL(_cfb[draw_buffer]));

  gDPSetFillColor(glistp++, GPACK_RGBA5551(128,0,128,255) << 16 |
                  GPACK_RGBA5551(128,0,128,255));

  gSPDisplayList(glistp++, clearcfb_dl);

  /* Initialize RSP and RDP state */

  gSPDisplayList(glistp++, init_dl);

  /* RSP and RDP setup, and screen clear */

  gSPDisplayList(glistp++, rdpinit2d_dl);
  gSPDisplayList(glistp++, rspinit2d_dl);

  gDPFullSync(glistp++);
  gSPEndDisplayList(glistp++);

  /* Build graphics task to execute the display list we just built */
  gentask = &(task[draw_buffer ? 0 : 1]);
  gentask->t.type            = M_GFXTASK;
  gentask->t.flags           = 0x0;
  gentask->t.ucode_boot      = (u64*) rspbootTextStart;
  gentask->t.ucode_boot_size = ((int)rspbootTextEnd - (int)rspbootTextStart);
  gentask->t.ucode           = (u64*) gspSprite2DTextStart; /* use XBUS */
  gentask->t.ucode_data      = (u64*) gspSprite2DDataStart;
  gentask->t.ucode_size      = 4096;
  gentask->t.ucode_data_size = 2048;
  gentask->t.dram_stack      = (u64*) dram_stack;
  gentask->t.dram_stack_size = 1024;
  gentask->t.output_buff     = (u64*) NULL;
  gentask->t.output_buff_size= (u64*) NULL;
  gentask->t.yield_data_ptr  = (u64*) NULL; /* Graphics only - no yielding */
  gentask->t.yield_data_size = 0x0;
  gentask->t.data_ptr        = (u64*) glistp0;
  gentask->t.data_size       = ((int) glistp - (int) glistp0);
    
  /* Write back dirty cache lines that need to be read by the RCP */
  osWritebackDCache(generate, sizeof(dynamic_stuff));

  /* Fire up SP task */
  osSpTaskStart(gentask);

  /* wait for SP completion */
  (void)osRecvMesg(&rspMessageQ, NULL, OS_MESG_BLOCK);
  (void)osRecvMesg(&rdpMessageQ, NULL, OS_MESG_BLOCK);
}


DrawIt(void)
{
  /* setup to swap buffers */
  osViSwapBuffer(_cfb[draw_buffer]);

  /* Make sure there isn't an old retrace in queue */
  if (MQ_IS_FULL(&retraceMessageQ))
     (void)osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);
  /* Wait for Vertical retrace to finish swap buffers */
  (void)osRecvMesg(&retraceMessageQ, NULL, OS_MESG_BLOCK);

  draw_buffer ^= 1;
}
