/**
 * SceneGraph が増えてくると動かなくなるかもしれない。
 * 一応 mainMem とかで動くようになるとは思うけど。
 * だめだったら、そこら辺が怪しいと思うべき
 */

#include "CreatePolygon.h"
#include "polygon_pack.h"
#include "scene_graph_pack.h"

SchedDefineTask(CreatePolygon);

#define SG_PACK_LOAD 10
#define SG_NODE_LOAD 11
#define PP_LOAD 12
#define PP_STORE 13

/**
 * あとで直す
 */
static void
rotate(float *xyz, float *matrix)
{
    float abc[4];

    abc[0] = xyz[0];
    abc[1] = xyz[1];
    abc[2] = xyz[2];
    abc[3] = xyz[3];

    for(int i=0; i<4; i++)
    {
	xyz[i] = abc[0]*matrix[i] + abc[1]*matrix[i+4] + abc[2]*matrix[i+8] + abc[3]*matrix[i+12];
    }
}

int 
CreatePolygon::run(void *rbuf, void *wbuf)
{
    float xyz1[4],xyz2[4],xyz3[4];

    SceneGraphPackPtr sgp = (SceneGraphPack*)smanager->get_input(0);
    SceneGraphPackPtr next_sgp = 
	(SceneGraphPackPtr)smanager->allocate(sizeof(SceneGraphPack));
    SceneGraphPackPtr free_sgp = next_sgp;
    SceneGraphPackPtr tmp_sgp;

    SceneGraphNodePtr node;
    SceneGraphNodePtr next_node
	= (SceneGraphNodePtr)smanager->allocate(sizeof(SceneGraphNode));
    SceneGraphNodePtr free_node = next_node;
    SceneGraphNodePtr tmp_node;

    PolygonPackPtr pp
	= (PolygonPackPtr)smanager->allocate(sizeof(PolygonPack));
    PolygonPackPtr send_pp
	= (PolygonPackPtr)smanager->allocate(sizeof(PolygonPack));
    PolygonPackPtr pp_addr = (PolygonPackPtr)smanager->get_param(0);
    PolygonPackPtr tmp_pp;
    
    pp->init();
    send_pp->init();

    do {
	if (sgp->next != NULL) {
	    smanager->dma_load(next_sgp, (uint32)sgp->next,
			       sizeof(SceneGraphPack), SG_PACK_LOAD);
	} else {
	    next_sgp = NULL;
	}

	for (int i = 0; i < sgp->info.size; i++) {
	    node = &sgp->node[i];

	    do {
		if (node->next != NULL) {
		    smanager->dma_load(next_node, (uint32)node->next,
				       sizeof(SceneGraphNode), SG_NODE_LOAD);
		} else {
		    next_node = NULL;
		}
    
		for (int n = 0, nt = 0; n < node->size*3; n+=9, nt+=6) {

		    if (pp->info.size >= MAX_SIZE_TRIANGLE) {
			PolygonPackPtr next;

			// smanager から Task を作る、0 ではなく PolygonPack->task_id が良い
			smanager->mainMem_alloc(0, sizeof(PolygonPack));
			smanager->mainMem_wait();
			next = (PolygonPackPtr)smanager->mainMem_get(0);

			pp->next = next; // この部分は TaskManager 側でやるべき

			tmp_pp = pp;
			pp = send_pp;
			send_pp = tmp_pp;

			smanager->dma_wait(PP_STORE);
			smanager->dma_store(send_pp, (uint32)pp_addr,
					    sizeof(PolygonPack), PP_STORE);
			
			pp_addr = next;

			smanager->dma_wait(PP_LOAD); // 多分不要
			smanager->dma_load(pp, (uint32)pp_addr,
					   sizeof(PolygonPack), PP_LOAD);
			// 次の dma_wait のコストが高いのでパイプラインで隠す必要がある
			
			smanager->dma_wait(PP_LOAD);
			pp->init();

		    }

		    TrianglePack *triangle = &pp->tri[pp->info.size++];

		    xyz1[0] = node->vertex[n];
		    xyz1[1] = node->vertex[n+1];
		    xyz1[2] = node->vertex[n+2]*-1;
		    xyz1[3] = 1;
		    xyz2[0] = node->vertex[n+3];
		    xyz2[1] = node->vertex[n+3+1];
		    xyz2[2] = node->vertex[n+3+2]*-1;
		    xyz2[3] = 1;
		    xyz3[0] = node->vertex[n+6];
		    xyz3[1] = node->vertex[n+6+1];
		    xyz3[2] = node->vertex[n+6+2]*-1;
		    xyz3[3] = 1;
		
		    rotate(xyz1, node->translation);
		    rotate(xyz2, node->translation);
		    rotate(xyz3, node->translation);
		
		    triangle->ver1.x = xyz1[0];
		    triangle->ver1.y = xyz1[1];
		    triangle->ver1.z = xyz1[2];
		    triangle->ver1.tex_x = node->texture[nt];
		    triangle->ver1.tex_y = node->texture[nt+1];
		
		    triangle->ver2.x = xyz2[0];
		    triangle->ver2.y = xyz2[1];
		    triangle->ver2.z = xyz2[2];
		    triangle->ver2.tex_x = node->texture[nt+2];
		    triangle->ver2.tex_y = node->texture[nt+2+1];
		
		    triangle->ver3.x = xyz3[0];
		    triangle->ver3.y = xyz3[1];
		    triangle->ver3.z = xyz3[2];
		    triangle->ver3.tex_x = node->texture[nt+4];
		    triangle->ver3.tex_y = node->texture[nt+4+1];
		
#if 1
		    triangle->tex_info.addr = node->tex_addr;
		    triangle->tex_info.width = node->tex_width;
		    triangle->tex_info.height = node->tex_height;
#else
		    triangle->tex_info.addr = node->texture_info.pixels;
		    triangle->tex_info.width = node->texture_info.t_w;
		    triangle->tex_info.height = node->texture_info.t_h;
#endif
		}

		smanager->dma_wait(SG_NODE_LOAD);

		tmp_node = node;
		node = next_node;
		next_node = tmp_node;
	    } while (node);

	    next_node = free_node;
	}

	smanager->dma_wait(SG_PACK_LOAD);

	tmp_sgp = sgp;
	sgp = next_sgp;
	next_sgp = tmp_sgp;
    } while (sgp);
    
    smanager->dma_wait(PP_STORE);
    smanager->dma_store(pp, (uint32)pp_addr,
			sizeof(PolygonPack), PP_STORE);
    smanager->dma_wait(PP_STORE);

    free(pp);
    free(send_pp);
    free(free_node);
    free(free_sgp);

    return 0;
}