/**
 * 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];
    }
}

static void
apply_matrix(float *xyz, float *mat)
{
    float tmp[4];

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

    for (int i = 0; i < 4; i++) {
	xyz[i] = tmp[0]*mat[i*4+0] + tmp[1]*mat[i*4+1]
	    + tmp[2]*mat[i*4+2] + tmp[3]*mat[i*4+3];
    }
}

float fx = 1.0f;
float fy = 1.0f;
float fz = 0.9f;
float fd = 0.05f;

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;

    float n = 10.0f;
    float f = 1000.0f;
    float t = 30.0f;
    float b = -30.0f;
    float r = 30.0f;
    float l = -30.0f;

    float mat[16] = {
	2*n/(r-l),         0,  (r+l)/(r-l),            0,
	        0, 2*n/(t-b),  (t+b)/(t-b),            0,
	        0,         0, -(f+n)/(f-n), -2*n*f/(f-n),
	        0,         0,           -1,            0
    };

    
    pp->init();
    send_pp->init();

    while (sg) {
	if (sg->flag_drawable) { // sg->isDrawable() とかの方がよくね？
	    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);

		//apply_matrix(xyz1, mat);
		//apply_matrix(xyz2, mat);
		//apply_matrix(xyz3, mat);
		
		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_info.addr   = sg->texture_info.pixels;
		triangle->tex_info.width  = sg->texture_info.t_w;
		triangle->tex_info.height = sg->texture_info.t_h;
		triangle->tex_info.scale_max = sg->texture_info.scale_max;
	    }
	}

	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;
}