#include <stdlib.h>
#include <string.h>
#include <spu_intrinsics.h>
#include "DrawSpan.h"
#include "polygon_pack.h"
#include "texture.h"
#include "viewer_types.h"
#include "Func.h"

SchedDefineTask(DrawSpan);

#define TEX_LOAD1      0
#define TEX_LOAD2      1
#define SPAN_PACK_LOAD 2
#define FB_STORE       3

DrawSpan::~DrawSpan(void)
{
    smanager->dma_wait(FB_STORE);
    free((void*)((int)linebuf*doneWrite));
}

inline vector float
spu_re_nrm(vector float a)
{
    vector float unit = (vector float){1.0, 1.0, 1.0, 1.0};
    vector float approximation;

    approximation = spu_re(a);
    return spu_madd(spu_nmsub(approximation, a, unit),
                    approximation, approximation);
}


vector signed int
getLocalPositionVec(vector signed int d, signed int offset)
{
    return spu_and(d, spu_splats(offset-1));
}

vector signed int
getLocalXVec(vector signed int x)
{
    return getLocalPositionVec(x, split_screen_w);
}


/**
 * テクスチャは、TEXTURE_SPLIT_PIXEL^2 のブロックに分割する
 *
 * +---+---+---+---+---+---+
 * | 0 | 1 | 2 | 3 | 4 | 5 |
 * +---+---+---+---+---+---+
 * |   |   |   |   |   |11 |
 * +---+---+---+---+---+---+
 * |   |   |   |   |   |17 |
 * +---+---+---+---+---+---+
 * |   |   |   |   |   |23 |
 * +---+---+---+---+---+---+
 * |   |   |   |   |   |29 |
 * +---+---+---+---+---+---+
 * |   |   |   |   |   |35 |
 * +---+---+---+---+---+---+
 *
 * 一辺を TEXTURE_SPLIT とする
 * 各ブロックの数字がブロックIDとなる。
 */

/**
 * テクスチャの座標から、
 * テクスチャのどのブロックかを求める
 *
 * @param[in] tx X coordinates of texture
 * @param[in] tx Y coordinates of texture
 * @param[in] twidth  Width of texture
 * @return block ID
 */
int
DrawSpan::getTexBlock(int tx, int ty, int twidth)
{
     int blockX, blockY;

     blockX = tx / TEXTURE_SPLIT_PIXEL;
     blockY = ty / TEXTURE_SPLIT_PIXEL;

     return blockX + (twidth/TEXTURE_SPLIT_PIXEL)*blockY;
}

/**
 * block ID と、テクスチャの TOP address から
 * (tx,ty) で使われるテクスチャの Tile addres を求める
 *
 * @param[in] tx X coordinates of texture
 * @param[in] tx Y coordinates of texture
 * @param[in] tw Width of texture
 * @param[in] tex_addr_top (tx,ty) で使うテクスチャの先頭address
 * @return block ID
 */
uint32*
DrawSpan::getTile(int tx, int ty, int tw, uint32 *tex_addr_top)
{
    int block = getTexBlock(tx, ty, tw);
    return tex_addr_top + block*TEXTURE_BLOCK_SIZE;
}

/**
 * FrameBuffer に書き込む rgb の領域初期化
 *
 * @param width  Width of Buffer
 * @param height Height of Buffer
 * @param rgb    Initial value of RGB at Buffer
 * @return Buffer
 */
int*
DrawSpan::linebuf_init(int width, int height, int rgb)
{
    int *buf = (int*)smanager->allocate(sizeof(int)*width*height);

    for (int i = 0; i < width*height; i++) {
	buf[i] = rgb;
    }

    return buf;
}

/**
 * Z-Buffer の初期化
 *
 * @param width  Width of Z-Buffer
 * @param height Height of Z-Buffer
 * @return Z-Buffer
 */
float*
DrawSpan::zRow_init(int width, int height)
{
    float *buf = (float*)smanager->allocate(sizeof(float)*width*height);
    float def = 65535.0f;

#if 0
    for (int i = 0; i < width*height; i++) {
	buf[i] = def;
    }
#else 
    vector float init = spu_splats(0.0f);
    vector float defi = spu_splats(def);

    for (int i = 0; i < width*height; i += 4) {
	vector float *out = (vector float *)&buf[i];

	*out = spu_add(init, defi);
    }
#endif

    return buf;
}


/**
 * Span が使う Texture Tile があるか
 *
 * @retval != NULL 存在する
 * @retval NULL    存在しない
 */
TilePtr
DrawSpan::isAvailableTile(uint32 *addr)
{
    return hash->get(addr);
}

void
DrawSpan::set_rgb(uint32 *addr, int tag)
{
    TilePtr tile;

    if (isAvailableTile(addr)) {
	return;
    }

    tile = tileList->nextTile();
    /**
     * FIFO なので、もし前のが残っていれば削除
     */
    hash->remove(tile->texture_addr);
    
    tile->texture_addr = addr;
    
    hash->put(tile->texture_addr, tile);
    smanager->dma_load(tile->pixel, (uint32)addr,
		       sizeof(uint32)*TEXTURE_BLOCK_SIZE, tag);
}

/**
 * 
 */
void
DrawSpan::set_rgbs(uint32 *cur_addr, uint32 *max_addr, int wait_tag)
{
    uint32 curp   = (uint32)cur_addr;
    uint32 maxp   = (uint32)max_addr;
    uint32 startp = curp;
    uint32 diff   = sizeof(int)*TEXTURE_BLOCK_SIZE;
    int length = (int)maxp-(int)curp;

    int cmp = (length < 0);

#if 1
    length += cmp*(-1)*length*2;
    startp = cmp*maxp + !cmp*curp;
#else
    if (length < 0) {
	length = -length;
	startp = maxp;
    }
#endif

    for (int i = 0; i <= length; i += diff) {
	set_rgb((uint32*)(startp + i), wait_tag);
    }
}

uint32
DrawSpan::get_rgb(int tx, int ty, uint32 *addr)
{
    TilePtr tile;

    tile = hash->get(addr);
    return tile->pixel[(TEXTURE_SPLIT_PIXEL)*ty+tx];
}

/**
 * DrawSpan の再起動 (DrawSpanRenew 生成)
 *
 * @param[in] spack 現在処理している SpanPack
 * @param[in] cur_span_x span->length_x != 1 の時の Span の処理で
 *                       どこまで進んでいるか
 */
void
DrawSpan::reboot(SpanPackPtr spack, int cur_span_x)
{
    DrawSpanArgPtr args =
	(DrawSpanArgPtr)smanager->allocate(sizeof(DrawSpanArg));
    TaskPtr renew_task = smanager->create_task(TASK_DRAW_SPAN2);

    // 数が多いので構造体で渡す
    args->display      = smanager->get_param(0);
    args->screen_width = smanager->get_param(1);
    args->rangex_start = smanager->get_param(2);
    args->rangex_end   = smanager->get_param(3);
    args->rangey       = smanager->get_param(4);
    renew_task->add_param((int)args);

    /**
     * SpanPack は続きから開始するので、
     * 現在の状態をコピーしておく。
     * spack は rbuf から取得してる可能性があり
     * rbuf はシステムが自動的に free() するため
     * アドレスだけ渡すのはNG
     */ 
    SpanPackPtr curr = (SpanPackPtr)smanager->allocate(sizeof(SpanPack));
    memcpy(curr, spack, sizeof(SpanPack));
    renew_task->add_param((int)curr);
    renew_task->add_param(cur_span_x);

    // linebuf と zRow も引き継がせる
    renew_task->add_param((int)linebuf);
    renew_task->add_param((int)zRow);

    /**
     * 再起動したタスクを待つ
     */ 
    smanager->wait_task(renew_task);

    // next_spack は free() するので wait する
    smanager->dma_wait(SPAN_PACK_LOAD);
}

void
DrawSpan::writebuffer(unsigned int display, int buf_width, int height,
		      int screen_width)
{
    for (int i = 0; i < height; i++) {
	smanager->dma_store(&linebuf[i*buf_width],
			    display + (sizeof(int)*screen_width*i),
			    sizeof(int)*buf_width, FB_STORE);
    }

    doneWrite = 1;
}

/**
 * zRow と Linebuf を更新する
 *
 * @param zpos     更新する pixel のZ座標
 * @param rangex   このタスクが処理する描画領域の x の長さ
 * @param x        pixel の、描画領域内での x 座標
 * @param y        〃 の、y 座標
 * @param tex_x    pixel が使用するテクスチャの、Tile (8x8) 内での x 座標
 * @param tex_y    〃 の y 座標
 * @param tex_addr テクスチャのアドレス(MainMemory)
 */
void
DrawSpan::updateBuffer(float zpos, int rangex, int x, int y,
		       int tex_x, int tex_y, uint32 *tex_addr)
{
    int rgb = get_rgb(tex_x, tex_y, tex_addr);
		    
    zRow[x + (rangex*y)] = zpos;
    linebuf[x + (rangex*y)] = rgb;
}

/**
 * 長さが 1 の Span の描画 (要するに 1 pixel)
 *
 * @param span Span
 * @param startx 描画開始範囲
 * @param endx 描画終了範囲
 */
int
DrawSpan::drawDot1(SpanPtr span, int startx, int endx, int wait_tag)
{
    int rangex = endx - startx + 1;

    /* span->x に対応する Texture の座標 (tex_xpos, tex_ypos) */
    int tex_xpos, tex_ypos;

    // span の始点に対応する Texture の座標 (tex1, tey1)
    float tex = span->tex_x1;
    float tey = span->tex_y1;

    // span の始点に対応する z 座標
    float zpos = span->start_z;

    /* Tile 内での座標 */
    int localx = getLocalX(span->x-1);
    int localy = getLocalY(span->y-1);

    /**
     * (tex_xpos, tex_ypos) の、Tile 内(上の図参照)での座標と
     * そのブロックのアドレス(MainMemory)
     */
    int tex_localx;
    int tex_localy;
    uint32 *tex_addr;

    if (span->x < startx || endx < span->x) {
	return -1;
    }

    tex_xpos = (int)((span->tex_width-1) * tex);
    tex_ypos = (int)((span->tex_height-1) * tey);

    if (zpos < zRow[localx + (rangex*localy)]) {
	tex_addr = getTile(tex_xpos, tex_ypos,
			   span->tex_width, span->tex_addr);
	tex_localx = tex_xpos % TEXTURE_SPLIT_PIXEL;
	tex_localy = tex_ypos % TEXTURE_SPLIT_PIXEL;

	if (!isAvailableTile(tex_addr)) {
	    set_rgb(tex_addr, wait_tag);
	    smanager->dma_wait(wait_tag);
	    //return startx;
	}

	updateBuffer(zpos, rangex, localx, localy,
		     tex_localx, tex_localy, tex_addr);
    }

    return -1;
}

void
DrawSpan::drawDot2(SpanPtr span, int startx, int end, int js, int wait_tag)
{
    //printf("%d\n", js);
}

/**
 * 長さが 1 より大きい Span の描画
 *
 * 本来の目的として、この関数(drawLine1) では
 *   : 既に SPE 上に Tile のある pixel だけ描画
 *   : それ以外は、ここで予め DMA load しておき、
 *   : drawLine2 で一気に描画する
 * ってものだったんだけど、どうも上手く行かなかったので
 * 今は drawLine1 で load -> wait -> rendering を全部やってます
 * (rendering といっても、rendering buffer に書き込むだけで
 *  まだ main memory (frame buffer) に dma store してるわけではない)
 *      
 * @param span Span
 * @param startx 描画開始範囲
 * @param endx 描画終了範囲
 * @return 「span のどの位置まで rendering が終わったか」の x 座標
 */
int
DrawSpan::drawLine1(SpanPtr span, int startx, int endx, int wait_tag)
{
    int x = span->x;
    int rangex = endx - startx + 1;
    int x_len = span->length_x;

    int js = (x < startx) ? startx - x : 0;
    int je = (x + x_len > endx) ? endx - x : x_len;

    /* span->x に対応する Texture の座標 (tex_xpos, tex_ypos) */
    int tex_xpos, tex_ypos;

    // span の始点に対応する座標 (tex1, tey1)
    float tex1 = span->tex_x1;
    float tey1 = span->tex_y1;

    // span の終点に対応する座標 (tex2, tey2)
    float tex2 = span->tex_x2;
    float tey2 = span->tex_y2;

    // span の始点、終点に対応する z 座標
    float zpos1 = span->start_z;
    float zpos2 = span->end_z;

    // Tile 内での座標
    int localx, localy = getLocalY(span->y-1);

    int ret = je+1;

    //for (int j = js; j <= je; j++) {
    for (int j = je; j >= js; j--) {
	float tex_x, tex_y, tex_z;

	localx = getLocalX(x-1+j);

	tex_z = zpos1*(x_len-1-j)/(x_len-1) + zpos2*j/(x_len-1);

	tex_x = tex1*(x_len-1-j)/(x_len-1) + tex2*j/(x_len-1);
	tex_y = tey1*(x_len-1-j)/(x_len-1) + tey2*j/(x_len-1);
	if (tex_x > 1) tex_x = 1;
	if (tex_x < 0) tex_x = 0;
	if (tex_y > 1) tex_y = 1;
	if (tex_y < 0) tex_y = 0;
	tex_xpos = (int)((span->tex_width-1) * tex_x);
	tex_ypos = (int)((span->tex_height-1) * tex_y);
		    
	if (tex_z < zRow[localx + (rangex*localy)]) {
	    // (tex_xpos, tex_ypos) の、Tile 内(上の図参照)での座標と
	    // そのブロックのアドレス(MainMemory)
	    uint32 *tex_addr;
	    int tex_localx;
	    int tex_localy;

	    tex_addr = getTile(tex_xpos, tex_ypos,
			       span->tex_width, span->tex_addr);
	    tex_localx = tex_xpos % TEXTURE_SPLIT_PIXEL;
	    tex_localy = tex_ypos % TEXTURE_SPLIT_PIXEL;

	    if (!isAvailableTile(tex_addr)) {
#if 0
		// span が必要とするであろう tile を予想紙
		// set_rgbs で複数同時に load しようとしている図
		// まあ上手くいかなかったんだけど。。
		tex_x = tex1*(x_len-1-js)/(x_len-1) + tex2*js/(x_len-1);
		if (tex_x > 1) tex_x = 1;
		if (tex_x < 0) tex_x = 0;
		tex_xpos = (int)((span->tex_width-1) * tex_x);

		uint32 *max_addr = getTile(tex_xpos, tex_ypos,
					   span->tex_width, span->tex_addr);

		set_rgbs(tex_addr, max_addr, wait_tag);
		return js;
#else
		set_rgb(tex_addr, wait_tag);
		smanager->dma_wait(wait_tag);
#endif
	    }

	    updateBuffer(tex_z, rangex, localx, localy,
			 tex_localx, tex_localy, tex_addr);
	}
    }

    return ret;
}

/**
 * 長さが 1 より大きい Span の描画 (2段階目)
 *
 * 上にあるように、drawLine2 は今は動いてないです
 * 正確には、js が startx ~ endx 範囲外にあり開始されないってところ
 *
 * @param span Span
 * @param startx 描画開始範囲
 * @param endx 描画終了範囲
 * @param js 前回(drawLine1) で span のどこまで終わっているか
 */
void
DrawSpan::drawLine2(SpanPtr span, int startx, int endx, int js, int wait_tag)
{
    int x = span->x;
    int rangex = endx - startx + 1;
    int x_len = span->length_x;

    //int js = startx;
    int je = (x + x_len > endx) ? endx - x : x_len;

    /* span->x に対応する Texture の座標 (tex_xpos, tex_ypos) */
    int tex_xpos, tex_ypos;

    // span の始点に対応する座標 (tex1, tey1)
    float tex1 = span->tex_x1;
    float tey1 = span->tex_y1;

    // span の終点に対応する座標 (tex2, tey2)
    float tex2 = span->tex_x2;
    float tey2 = span->tex_y2;

    // span の始点、終点に対応する z 座標
    float zpos1 = span->start_z;
    float zpos2 = span->end_z;

    // Tile 内での座標
    int localx, localy = getLocalY(span->y-1);

    // (tex_xpos, tex_ypos) の、Tile 内(上の図参照)での座標と
    // そのブロックのアドレス(MainMemory)
    int tex_localx;
    int tex_localy;
    uint32 *tex_addr;

    float tex_x, tex_y, tex_z;

    smanager->dma_wait(wait_tag);

    for (int j = js; j <= je; j++) {
	localx = getLocalX(x-1+j);

	tex_z = zpos1*(x_len-1-j)/(x_len-1) + zpos2*j/(x_len-1);

	tex_x = tex1*(x_len-1-j)/(x_len-1) + tex2*j/(x_len-1);
	tex_y = tey1*(x_len-1-j)/(x_len-1) + tey2*j/(x_len-1);
	if (tex_x > 1) tex_x = 1;
	if (tex_x < 0) tex_x = 0;
	if (tex_y > 1) tex_y = 1;
	if (tex_y < 0) tex_y = 0;
	tex_xpos = (int)((span->tex_width-1) * tex_x);
	tex_ypos = (int)((span->tex_height-1) * tex_y);

	if (tex_z < zRow[localx + (rangex*localy)]) {
	    tex_addr = getTile(tex_xpos, tex_ypos,
			       span->tex_width, span->tex_addr);
	    tex_localx = tex_xpos % TEXTURE_SPLIT_PIXEL;
	    tex_localy = tex_ypos % TEXTURE_SPLIT_PIXEL;

	    updateBuffer(tex_z, rangex, localx, localy,
			 tex_localx, tex_localy, tex_addr);
	}
    }    
}

int
DrawSpan::run(void *rbuf, void *wbuf)
{
    SpanPackPtr spack = (SpanPackPtr)smanager->get_input(0);
    SpanPackPtr next_spack = (SpanPackPtr)smanager->allocate(sizeof(SpanPack));
    SpanPackPtr free_spack = next_spack; // next_spack の free() 用
    Span *span;

    Span nop_span;
    nop_span.length_x = 1;

    int (DrawSpan::*drawFunc1[2])(SpanPtr, int, int, int) = {
	&DrawSpan::drawDot1, &DrawSpan::drawLine1
    };

    void (DrawSpan::*drawFunc2[2])(SpanPtr, int, int, int, int) = {
	&DrawSpan::drawDot2, &DrawSpan::drawLine2
    };

    uint32 display   = smanager->get_param(0);
    int screen_width = smanager->get_param(1);
    int rangex_start = smanager->get_param(2);
    int rangex_end   = smanager->get_param(3); 
    
    // このタスクが担当する x の範囲
    int rangex = rangex_end - rangex_start + 1;

    // y の範囲
    int rangey = smanager->get_param(4);

    hash = (TileHashPtr)smanager->global_get(GLOBAL_TEXTURE_HASH);
    tileList = (TileListPtr)smanager->global_get(GLOBAL_TILE_LIST);

    zRow = zRow_init(rangex, rangey);
    //linebuf = linebuf_init(rangex, rangey, 0x00ffffff);
    linebuf = linebuf_init(rangex, rangey, 0);

    doneWrite = 0;

    int tl_tag[2] = {TEX_LOAD1, TEX_LOAD2};
    int tl_tag_flg1 = 0;
    int tl_tag_flg2 = 1;
    
    do {
	/**
	 * SpanPack->next が存在する場合、
	 * 現在の SpanPack を処理してる間に
	 * 次の SpanPack の DMA 転送を行う
	 */
	if (spack->next != NULL) {
	    smanager->dma_load(next_spack, (uint32)spack->next,
			       sizeof(SpanPack), SPAN_PACK_LOAD);
	} else {
	    next_spack = NULL;
	}

	SpanPtr resume_span = &nop_span;
	int resume_span_x = 0;

	for (int t = 0; t < spack->info.size; t++) {	  
	    SpanPtr next_span;
	    int next_span_x;

	    span = &spack->span[t];

	    /**
	     * span の長さによって、drawLine か drawDot を選択している
	     */ 
	    next_span_x
		= (this->*drawFunc1[(span->length_x != 1)])(
		    span, rangex_start, rangex_end, tl_tag[tl_tag_flg1]);
	    next_span = span;

	    (this->*drawFunc2[(resume_span->length_x != 1)])(
		resume_span, rangex_start, rangex_end, resume_span_x,
		tl_tag[tl_tag_flg2]);

	    resume_span = next_span;
	    resume_span_x = next_span_x;
	    
	    //smanager->dma_wait(tl_tag[tl_tag_flg1]);

	    tl_tag_flg1 ^= 1;
	    tl_tag_flg2 ^= 1;
	}
	
	// 現在 drawLine2、drawDot2 は機能してないので
	//(this->*drawFunc2[(resume_span->length_x != 1)])(
	//resume_span, rangex_start, rangex_end, resume_span_x,
	//tl_tag[tl_tag_flg1]);

	smanager->dma_wait(SPAN_PACK_LOAD);

	SpanPackPtr tmp_spack = spack;
	spack = next_spack;
	next_spack = tmp_spack;
    } while (spack);

    writebuffer(display, rangex, rangey, screen_width);

    // linebuf は、writebuffer() の dma_store を wait する
    // DrawSpan::~DrawSpan() 内で free する。
    //free(linebuf);
    free(zRow);

//FINISH:
    /**
     * goto FINISH; の時は reboot なので
     * linebuf, zRow は free() しない
     */

    free(free_spack);
    return 0;
}