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

#include "CreatePolygonFromSceneGraph.h"
#include "polygon_pack.h"
#include "scene_graph_pack.h"

SchedDefineTask(CreatePolygonFromSceneGraph);

#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 
CreatePolygonFromSceneGraph::run(void *rbuf, void *wbuf)
{
    float xyz1[4], xyz2[4], xyz3[4];

    SceneGraphPtr sg_top = (SceneGraphPtr)smanager->get_param(0);
    SceneGraphPtr sg = sg_top;

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

    while (sg) {
	for (int i = 0; i < sg->size; i += 3) {
	    if (pp->info.size >= MAX_SIZE_TRIANGLE) {
		PolygonPackPtr next;
	    
		smanager->mainMem_alloc(0, sizeof(PolygonPack));
		smanager->mainMem_wait();
		next = (PolygonPackPtr)smanager->mainMem_get(0);
	    
		pp->next = next;
	    
		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);
		smanager->dma_wait(PP_LOAD);
		pp->init();
	    }

	    TrianglePack *triangle = &pp->tri[pp->info.size++];
	    
	    xyz1[0] = sg->data[(i+0)*3];
	    xyz1[1] = sg->data[(i+0)*3+1];
	    xyz1[2] = sg->data[(i+0)*3+2]*-1;
	    xyz1[3] = 1;
	    xyz2[0] = sg->data[(i+1)*3];
	    xyz2[1] = sg->data[(i+1)*3+1];
	    xyz2[2] = sg->data[(i+1)*3+2]*-1;
	    xyz2[3] = 1;
	    xyz3[0] = sg->data[(i+2)*3];
	    xyz3[1] = sg->data[(i+2)*3+1];
	    xyz3[2] = sg->data[(i+2)*3+2]*-1;
	    xyz3[3] = 1;

	    rotate(xyz1, sg->matrix);
	    rotate(xyz2, sg->matrix);
	    rotate(xyz3, sg->matrix);
		
	    triangle->ver1.x = xyz1[0];
	    triangle->ver1.y = xyz1[1];
	    triangle->ver1.z = xyz1[2];
	    triangle->ver1.tex_x = sg->data[(i+0)*3 + sg->size*6];
	    triangle->ver1.tex_y = sg->data[(i+0)*3 + sg->size*6+1];
		
	    triangle->ver2.x = xyz2[0];
	    triangle->ver2.y = xyz2[1];
	    triangle->ver2.z = xyz2[2];
	    triangle->ver2.tex_x = sg->data[(i+1)*3 + sg->size*6];
	    triangle->ver2.tex_y = sg->data[(i+1)*3 + sg->size*6+1];
		
	    triangle->ver3.x = xyz3[0];
	    triangle->ver3.y = xyz3[1];
	    triangle->ver3.z = xyz3[2];
	    triangle->ver3.tex_x = sg->data[(i+2)*3 + sg->size*6];
	    triangle->ver3.tex_y = sg->data[(i+2)*3 + sg->size*6+1];
	
	    triangle->tex_addr = (long*)sg->texture_info.pixels;
	    triangle->tex_width = sg->texture_info.t_w;
	    triangle->tex_height = sg->texture_info.t_h;
	}


	if (sg->children != NULL) {
	    sg = sg->children;
	} else if (sg->brother != NULL) {
	    sg = sg->brother;
	} else {
	    while (sg) {
		if (sg->brother != NULL) {
		    sg = sg->brother;
		    break;
		} else {
		    if (sg->parent == NULL) {
			sg = NULL;
			break;
		    } else {
			sg = sg->parent;
		    }
		}
	    }
	}
    }
    
    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);

    return 0;
}