#define CODE_ROM
#define ISV64
#define __MAIN_C
#include "common.h"
#include <PR/sched.h>
#include "ks_nes.h"


GAME_NES game_nes, game_nes_audio;

PUBLIC int emulator_running;
PUBLIC OSTask my_task[MY_TASK_MAX];
PUBLIC u32 ks_memblock_begin, ks_memblock_end;
/* カラーフレームバッファ */
PUBLIC u16 *cfbp; /* 次に表示したい フレームバッファポインタ */
PUBLIC u16 *cfb_oldp; /* cfb_oldp が retrace 時に osViSwapBuffer される */
PUBLIC u16 *menu_cfbp;
PUBLIC u16 *menu_cfb_oldp;
PUBLIC vu32 frame_count;
PUBLIC task_request_object task_request[MY_TASK_MAX];
static RSPTask *rsp_taskp;
/* boot_stack は PUBLIC というわけではないが static であっては ならない */
u64 boot_stack[BOOT_STACKSIZE / sizeof(u64)];
#ifdef PTN64
static OSThread rmonThread;
static u64      rmonStack[RMON_STACKSIZE/sizeof(u64)];
#endif
static OSThread audio_thread;
static u64      audio_thread_stack[AUDIO_STACKSIZE / sizeof(u64)];
PUBLIC OSThread main_thread;
static u64      main_thread_stack[MAIN_STACKSIZE / sizeof(u64)];
static OSThread tman_thread;
static u64      tman_thread_stack[TMAN_STACKSIZE / sizeof(u64)];
static OSThread idle_thread;
static u64      idle_thread_stack[IDLE_STACKSIZE / sizeof(u64)];
static OSMesgQueue pi_msgq;
static OSMesg      pi_msgbuf[PI_MSGS_MAX];
PUBLIC OSMesgQueue dma_msgq;
static OSMesg dma_msgbuf[DMA_MSGS_MAX];
PUBLIC OSMesgQueue audio_timing_msgq;
static OSMesg audio_timing_msgbuf[AUDIO_TIMING_MSGS_MAX];
static OSMesgQueue main_timing_msgq;
static OSMesg main_timing_msgbuf[MAIN_TIMING_MSGS_MAX];
static OSMesgQueue taskman_msgq;
static OSMesg taskman_msgbuf[TASKMAN_MSGS_MAX];
PUBLIC OSIoMesg dma_io_msgbuf[DMA_IO_MSGS_MAX];
#define NUM_LEO_MESGS 8
static OSMesg LeoMessages[NUM_LEO_MESGS];
static OSPiHandle *cart_epi_handle;
static OSPiHandle *disk_epi_handle;
/* コントローラ 関係 */
PUBLIC OSMesgQueue pad_msgq;
static OSMesg pad_msgbuf[PAD_MSGS_MAX];
static OSContStatus pad_status[MAXCONTROLLERS];
PUBLIC OSContPad os_pad_data[MAXCONTROLLERS];
/* npad: 有効な コントローラの 数 */
PUBLIC u32 npad;
PUBLIC pad_object pad[MAXCONTROLLERS];
PUBLIC u8 pad_index[MAXCONTROLLERS];
/* sequence: シーケンス 関数ポインタ */
PUBLIC sq sequencep;
PUBLIC u32 blanking_flag; /* retrace 時に この値を osViBlack 引数とする */

/* メモリブロックを得る。64 byte alignment される。
 * 初期化は与えても良いブロック全体の開始(+0)、終了アドレス(+1)を
 * ks_memblock_begin と ks_memblock_end に それぞれセットする。
 * ks_memblock_begin は メモリを与えるほど ks_memblock_end に近づいていく。
 * garbage collection はしない。ブロックにヘッダは一切付かない。
 */
PUBLIC u8 *ks_get_memblock(u32 size, char *purpose)
{
  u32 tmp;

  if (purpose == NULL) {
    purpose = "?";
  }

  tmp = ALIGN64B(ks_memblock_begin);
  ks_memblock_begin = tmp + size;
  if (ks_memblock_begin >= ks_memblock_end) {
    osSyncPrintf("ks_get_memblock: failed to get 0x%x bytes as %s, 0x%x bytes free\n",
		 size, purpose, ks_memblock_end - tmp);
    while (TRUE)
      ;
  }
  osSyncPrintf("ks_get_memblock: from 0x%x, 0x%x bytes as %s\n",
	       tmp, size, purpose);
  return (u8 *)tmp;
}
  
/* 各種グローバル変数の 初期化 */
static void init_globals(void)
{
  int i;
  extern u8 _nes_codeSegmentEnd[];

  /* メモリブロック begin, end */
  ks_memblock_begin = (u32)_nes_codeSegmentEnd;
  ks_memblock_end = 0x80400000;

  blanking_flag = TRUE;
  cfb_oldp = (u16 *)ks_get_memblock(SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8, "CFB0");
  bzero(cfb_oldp, SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8);
  cfbp = (u16 *)ks_get_memblock(SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8, "CFB1");
  bzero(cfbp, SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8);
  menu_cfb_oldp = (u16 *)ks_get_memblock(SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8, "MENU_CFB0");
  bzero(menu_cfb_oldp, SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8);
  menu_cfbp = (u16 *)ks_get_memblock(SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8, "MENU_CFB1");
  bzero(menu_cfbp, SCREEN_WD * SCREEN_HT_FPAL * SCREEN_BPP / 8);
  frame_count = 0;
  
  rsp_taskp = NULL;

  return;
}

PUBLIC void rom_load(u8 *src, u8 *dst, u32 length)
{
  if(MQ_IS_FULL(&dma_msgq)) {
    osRecvMesg(&dma_msgq, NULL, OS_MESG_BLOCK);
  }
  if (length < 0x2000) {
    osWritebackDCache(dst, length);
    osInvalDCache(dst, length);
  } else {
    osWritebackDCacheAll();
  }
  dma_io_msgbuf[0].hdr.pri = OS_MESG_PRI_NORMAL;
  dma_io_msgbuf[0].hdr.retQueue = &dma_msgq;
  dma_io_msgbuf[0].dramAddr = dst;
  dma_io_msgbuf[0].devAddr = (u32)src;
  dma_io_msgbuf[0].size = length;
  
  osEPiStartDma(cart_epi_handle, &dma_io_msgbuf[0], OS_READ);

  osRecvMesg(&dma_msgq, NULL, OS_MESG_BLOCK);
}

extern void ScanController(void)
{
  osContStartReadData(&pad_msgq);
}  

extern void ReadController(void)
{
  osRecvMesg(&pad_msgq, NULL, OS_MESG_BLOCK);
  osContGetReadData(&os_pad_data[0]);
  if (npad) {
    int j;
    pad_object *padp;
    OSContPad *os_padp;
    
    j = npad - 1;
    padp = &pad[j];
    for (j = npad - 1; j >= 0; j--) {
      os_padp = &os_pad_data[pad_index[j]];
      padp -> trig = (padp -> button ^ os_padp -> button) & os_padp -> button;
      padp -> button = os_padp -> button;
      padp -> stick_x = os_padp -> stick_x;
      padp -> stick_y = os_padp -> stick_y;
      padp--;
    }
  }
}  

/* audioproc: オーディオスレッド。
 * RSP は使わないが ダミーで RSP タスクを起動する。そして、ダミー RSPDONE を SendMesg する。
 * yield する必要があるときは… taskman があらかじめ yield しておくべき。
 */
PUBLIC int audioproc_enabled;
static void audioproc(void *dummyp)
{
  while (TRUE) {
    osRecvMesg(&audio_timing_msgq, NULL, OS_MESG_BLOCK);
    /* dummy RSP task, and dummy RSPDONE */
    osSendMesg(&taskman_msgq, (OSMesg)MSG_RSPDONE, OS_MESG_NOBLOCK);
    if (audioproc_enabled) {
      game_nes_audio.g.exec(&game_nes_audio);
    }
  }
}

/* mainproc: メインスレッド
 * main_timing_msgq のメッセージを待たなくても
 * コントローラデータの更新を待っているので、結局1フレーム1ループの処理となる。
 */
static void mainproc(void *dummyp)
{
  u32 count;
  sq sequencep_next, sequencep_old; /* 前の シーケンスと 変わったかどうか */
  OSTask *taskp;
  int i;
  
  cart_epi_handle = osCartRomInit();
#ifndef CODE_ROM
  disk_epi_handle = sub_osDriveRomInit();
#endif
  emulator_running = 0;
  sequencep_next = sq_init;


  while (TRUE) {
/*     osSyncPrintf("mainproc: waiting main_timing_msgq\n"); */
    if (MQ_IS_FULL(&main_timing_msgq)) {
/*       osSyncPrintf("main_timing_msgq: MQ_FULL\n"); */
      osRecvMesg(&main_timing_msgq, NULL, OS_MESG_BLOCK);
    }
    osRecvMesg(&main_timing_msgq, NULL, OS_MESG_BLOCK);
/*     osSyncPrintf("  mainproc: got main_timing_msgq\n"); */

    /* emu ルーチン実行中なら パッドデータを読まない。
     * エミュレータスレッドが 必要に応じて 読む。
     */
    if ( emulator_running ) {
/*       osSyncPrintf("mainproc: emuproc active at frame 0x%x\n", frame_count); */
    } else {
      osContStartReadData(&pad_msgq);
      /* syncram コマンド作成 */
      osRecvMesg(&pad_msgq, NULL, OS_MESG_BLOCK);
      osContGetReadData(&os_pad_data[0]);
      if (npad) {
	int j;
	pad_object *padp;
	OSContPad *os_padp;
      
	j = npad - 1;
	padp = &pad[j];
	for (j = npad - 1; j >= 0; j--) {
	  os_padp = &os_pad_data[pad_index[j]];
	  padp -> trig = (padp -> button ^ os_padp -> button) & os_padp -> button;
	  padp -> button = os_padp -> button;
	  padp -> stick_x = os_padp -> stick_x;
	  padp -> stick_y = os_padp -> stick_y;
	  padp--;
	}
      }
      /* ここで syncram/verify コマンド発行 */
    }

    sequencep = sequencep_next;
    sequencep_next = sequencep((u32)sequencep ^ (u32)sequencep_old, sequencep);
    sequencep_old = sequencep;

    {
      u16 *tmp;
      /* フレームバッファポインタの swap */
      tmp = cfb_oldp;
      cfb_oldp = cfbp;
      cfbp = tmp;
    }
  }
}

/* taskman: タスクマネージャ */
static void taskman(void *dummyp)
{
  u32 task_search_index;
  s32 task_search_index_rdp; /* DP を使う task index (M_GFXTASK) */
  u32 mesg;
  u32 count_tmp, count_tmp_bak;
  int i;
  
  /* VI 初期化
   * NTSC, Low reso., Anti alias(opp. Point sample), Non-interlaced,
   * 1 = 16 bpp (opp. 2 = 32 bpp) */
  osCreateViManager(OS_PRIORITY_VIMGR);
  osViSetMode(&osViModeNtscLpn1);
  osViBlack(TRUE);
  osViSetSpecialFeatures(
			 OS_VI_GAMMA_OFF |
			 OS_VI_GAMMA_DITHER_OFF |
			 OS_VI_DIVOT_OFF
			 );
  /* OS bug workaround */
  osViSetSpecialFeatures(OS_VI_DITHER_FILTER_OFF);

  /* PI 初期化 */
  osCreatePiManager(OS_PRIORITY_PIMGR, &pi_msgq, pi_msgbuf, PI_MSGS_MAX);

  /* 各種 グローバル変数の 初期設定 */
  init_globals();

#if 0
#ifdef PTN64
  /**** Start rmonThread so you can do printf's ****/
  osCreateThread(&rmonThread, 0, rmonMain, (void *)0,
		 (void *)(rmonStack+(RMON_STACKSIZE/sizeof(u64))),
		 (OSPri) OS_PRIORITY_RMON );
  osStartThread(&rmonThread);
#endif
#endif

  for (i = 0; i != MY_TASK_MAX; i++) {
    task_request[i].status = MY_TASK_STATUS_DONE;
  }
  task_search_index = 0;
  task_search_index_rdp = -1;
  /* initialize up RCP tasks */
  /* dummy audio task */
  
  /* dma, rcp, リトレース メッセージキューの設定 */
  /* それぞれ キューは 1 つのみ */
  osCreateMesgQueue(&dma_msgq, dma_msgbuf, DMA_MSGS_MAX);
  osCreateMesgQueue(&audio_timing_msgq, audio_timing_msgbuf, AUDIO_TIMING_MSGS_MAX);
  osCreateMesgQueue(&main_timing_msgq, main_timing_msgbuf, MAIN_TIMING_MSGS_MAX);
  osCreateMesgQueue(&taskman_msgq, taskman_msgbuf, TASKMAN_MSGS_MAX);
  osCreateMesgQueue(&pad_msgq, pad_msgbuf, PAD_MSGS_MAX); /* バッファは 一つ */

  
  osSetEventMesg(OS_EVENT_SP, &taskman_msgq, (OSMesg)MSG_RSPDONE);
  osSetEventMesg(OS_EVENT_DP, &taskman_msgq, (OSMesg)MSG_RDPDONE);
  osSetEventMesg(OS_EVENT_PRENMI, &taskman_msgq, (OSMesg)MSG_PRENMI);
  osSetEventMesg(OS_EVENT_SI, &pad_msgq, (OSMesg)MSG_PAD);
  osViSetEvent(               &taskman_msgq, (OSMesg)MSG_RETRACE,
			      1); /* 1 = 毎フレーム呼び出し */

  /* 各スレッド起動 */
  osCreateThread(&audio_thread, TID_AUDIO, audioproc, NULL,
		 &audio_thread_stack[AUDIO_STACKSIZE / sizeof(u64)], PRI_AUDIO);
  osStartThread(&audio_thread);

  osCreateThread(&main_thread, TID_MAIN, mainproc, NULL,
		 &main_thread_stack[MAIN_STACKSIZE / sizeof(u64)], PRI_MAIN);
  osStartThread(&main_thread);

  /* コントローラ初期化 */
  {
    u8 pattern;
    osContInit(&pad_msgq, &pattern, &pad_status[0]);
/*     osContStartQuery(&pad_msgq); */
/*     osRecvMesg(&pad_msgq, NULL, OS_MESG_BLOCK); */
/*     osContGetQuery(&pad_status[0]); */
    npad = 0;
    for (i = 0; i < MAXCONTROLLERS; i++) {
      pad[i].button =
	pad[i].trig =
	pad[i].stick_x =
	pad[i].stick_y = 0;
      if ((pattern & (1 << i))
	  && !(pad_status[i].errno & CONT_NO_RESPONSE_ERROR)) {
	pad_index[npad] = i;
	npad++;
      }
    }
  }

/*   osSyncPrintf("taskman: created sio, audio, main threads\n"); */
  
  while (TRUE) {
wait_new_msg:
/*     osSyncPrintf("taskman: waiting taskman_msgq\n"); */
    osRecvMesg(&taskman_msgq, (OSMesg)&mesg, OS_MESG_BLOCK);
    count_tmp = osGetCount();
/*     osSyncPrintf("  taskman: got taskman_msgq=%c%c%c%c, count_tmp=0x%x, diff=0x%x\n", */
/* 		 (mesg>>24)&0xff, (mesg>>16)&0xff, (mesg>>8)&0xff, mesg&0xff, count_tmp, count_tmp-count_tmp_bak); */
/*     osSyncPrintf("%c%c%c%c, 0x%x\n",(mesg>>24)&0xff, (mesg>>16)&0xff, (mesg>>8)&0xff, mesg&0xff, count_tmp-count_tmp_bak); */
    count_tmp_bak = count_tmp;
    switch (mesg) {
    case MSG_RETRACE:
      if (emulator_running == 0) {
       	osViSwapBuffer(cfb_oldp);
	osViBlack(blanking_flag);
      }
      frame_count++;
      /* サウンド処理は 毎フレーム 行う
       * が、audioproc < sioproc なので SP が 無駄になる?
       */
      osWritebackDCacheAll();
      osSendMesg(&audio_timing_msgq, (OSMesg)MSG_AUDIO_GO, OS_MESG_NOBLOCK);

      /* main スレッド 呼び出し。*/
      osSendMesg(&main_timing_msgq, (OSMesg)MSG_MAIN_GO, OS_MESG_NOBLOCK);
      break;
    case MSG_RSPDONE:
      /* 何らかの RSP タスクが 終わった。
       * もし yield 終了なら それなりに 処理する。
       */
      task_request[task_search_index].status &= ~MY_TASK_STATUS_REQUESTED;
      task_request[task_search_index].rspdone = osGetCount() - task_request[task_search_index].rspdone;
      task_search_index++;
      
      /* search for new task */
      while (task_search_index != MY_TASK_MAX) {
	if (task_request[task_search_index].status) {
	  /* writeback
	   * ...All(); は 重いのかな? でもしょうがないでしょ?
	   */
	  /* execute */
	  task_request[task_search_index].rspdone = osGetCount();
	  /* needs DP ? */
	  if (task_request[task_search_index].taskp -> t.type == M_GFXTASK) {
	    task_request[task_search_index].rdpdone = task_request[task_search_index].rspdone | 1;
	    task_search_index_rdp = task_search_index;
	  } else {
	    task_search_index_rdp = -1;
	  }
	  osSpTaskStart(task_request[task_search_index].taskp);
	  goto wait_new_msg; /* continue では 一番近くの while に なってしまう! */
	}
	task_search_index++;
      }
      task_search_index = 0;
      break;
    case MSG_RDPDONE: /* rdp done */
      task_request[task_search_index_rdp].rdpdone = (osGetCount() - task_request[task_search_index_rdp].rdpdone) & 0xfffffffe;
      break;
    case MSG_PRENMI:
/*       sub_LeoReset(); */
      while (osAfterPreNMI()) {
        IO_WRITE(SP_STATUS_REG, SP_CLR_RSPSIGNAL);  /* これを入れないとリセット後 暴走することがある */
      }
      while (TRUE); /* for debug */
      break;
    default:
/*       sub_unknown_msgq(mesg); */
      osSyncPrintf("unknown message taskman_msgq = 0x%x\n\n\n\n", mesg);
      for (;;);
      break;
    }
  }
}

/* idle: アイドルスレッド */
static void idle(void *dummyp)
{
  /* タスクマネージャ 起動 */
  osCreateThread(&tman_thread, TID_TMAN, taskman, NULL,
		 &tman_thread_stack[TMAN_STACKSIZE / sizeof(u64)], PRI_TMAN);
  osStartThread(&tman_thread);

  /* bye. */
  osSetThreadPri(NULL, OS_PRIORITY_IDLE);
  while (TRUE) {
    ;
  }
}

#if 0
#ifdef PTN64
#include "pt.c"
#endif
#endif

/* boot: ブート */
void boot(void)
{
  osInitialize();

#if 0
#ifdef PTN64
   ptstart();
#endif
#endif
   
  /* アイドルスレッドの生成・起動 引数は NULL */
  osCreateThread(&idle_thread, TID_IDLE, idle, NULL,
		 &idle_thread_stack[IDLE_STACKSIZE / sizeof(u64)], PRI_IDLE);
  osStartThread(&idle_thread);
}