/**
 * 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

/**
 *  ベクトルに行列を乗算する
 * @param[out] v vector (float[4])
 * @param[in] m matrix (float[16])
 */
static void
ApplyMatrix(float *v, float *m)
{
    float t[4];

    t[0] = v[0];
    t[1] = v[1];
    t[2] = v[2];
    t[3] = v[3];

    for (int i = 0; i < 4; i++) {
	v[i] = t[0]*m[i] + t[1]*m[i+4] + t[2]*m[i+8] + t[3]*m[i+12];
    }
}

static void
ApplyNormalMatrix(float *v, float *m)
{
    float t[4];

    t[0] = v[0];
    t[1] = v[1];
    t[2] = v[2];

    for (int i = 0; i < 3; i++) {
        v[i] = t[0]*m[i] + t[1]*m[i+4] + t[2]*m[i+8];
    }
}


/**
 * 行列の積
 *
 * @param[out] m0 output matrix
 * @param[in] m1 input left matrix
 * @param[in] m2 input right matrix
 */
/*
static void
MulMatrix(float *m0, float *m1, float *m2) //xyz[16]
{
    for(int t = 0; t < 16; t += 4) {
	for (int i = 0; i < 4; i++) {
	    m0[t+i] =
		m1[t+0]*m2[ i ] + m1[t+1]*m2[i+ 4] +
		m1[t+2]*m2[i+8] + m1[t+3]*m2[i+12];
	}
    }
}
*/
static int 
run(SchedTask *smanager, void *rbuf, void *wbuf)
{


    float xyz1[4], xyz2[4], xyz3[4];
    float normal1[4],normal2[4],normal3[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) {
	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, (memaddr)pp_addr,
					sizeof(PolygonPack), PP_STORE);
	    
		    pp_addr = next;
	    
		    smanager->dma_wait(PP_LOAD);
		    smanager->dma_load(pp, (memaddr)pp_addr,
				       sizeof(PolygonPack), PP_LOAD);
		    smanager->dma_wait(PP_LOAD);
		    pp->init();
		}


		xyz1[0] = sg->coord_xyz[(i+0)*3];
		xyz1[1] = sg->coord_xyz[(i+0)*3+1];
		xyz1[2] = sg->coord_xyz[(i+0)*3+2]*-1.0f;
		xyz1[3] = 1.0f;

		xyz2[0] = sg->coord_xyz[(i+1)*3];
		xyz2[1] = sg->coord_xyz[(i+1)*3+1];
		xyz2[2] = sg->coord_xyz[(i+1)*3+2]*-1.0f;
		xyz2[3] = 1.0f;

		xyz3[0] = sg->coord_xyz[(i+2)*3];
		xyz3[1] = sg->coord_xyz[(i+2)*3+1];
		xyz3[2] = sg->coord_xyz[(i+2)*3+2]*-1.0f;
		xyz3[3] = 1.0f;

		// sg->matrix = 回転行列*透視変換行列
		ApplyMatrix(xyz1, sg->matrix);
		ApplyMatrix(xyz2, sg->matrix);
		ApplyMatrix(xyz3, sg->matrix);

		xyz1[0] /= xyz1[2];
		xyz1[1] /= xyz1[2];
		xyz2[0] /= xyz2[2];
		xyz2[1] /= xyz2[2];
		xyz3[0] /= xyz3[2];
		xyz3[1] /= xyz3[2];

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

		triangle->ver1.x = xyz1[0];
		triangle->ver1.y = xyz1[1];
		triangle->ver1.z = xyz1[2];
		triangle->ver1.tex_x = sg->coord_tex[(i+0)*3];
		triangle->ver1.tex_y = sg->coord_tex[(i+0)*3+1];
		
		triangle->ver2.x = xyz2[0];
		triangle->ver2.y = xyz2[1];
		triangle->ver2.z = xyz2[2];
		triangle->ver2.tex_x = sg->coord_tex[(i+1)*3];
		triangle->ver2.tex_y = sg->coord_tex[(i+1)*3+1];
		
		triangle->ver3.x = xyz3[0];
		triangle->ver3.y = xyz3[1];
		triangle->ver3.z = xyz3[2];
		triangle->ver3.tex_x = sg->coord_tex[(i+2)*3];
		triangle->ver3.tex_y = sg->coord_tex[(i+2)*3+1];

                normal1[0] = sg->normal[(i+0)*3];
                normal1[1] = sg->normal[(i+0)*3+1];
                normal1[2] = sg->normal[(i+0)*3+2]*-1.0f;
                //normal1[3] = 1.0f;
		normal1[3] = 0.0f;

                normal2[0] = sg->normal[(i+1)*3];
                normal2[1] = sg->normal[(i+1)*3+1];
                normal2[2] = sg->normal[(i+1)*3+2]*-1.0f;
                //normal2[3] = 1.0f;
                normal2[3] = 0.0f;

                normal3[0] = sg->normal[(i+2)*3];
                normal3[1] = sg->normal[(i+2)*3+1];
                normal3[2] = sg->normal[(i+2)*3+2]*-1.0f;
		//normal3[3] = 1.0f;
                normal3[3] = 0.0f;

                //ApplyNormalMatrix(normal1,sg->real_matrix);
                //ApplyNormalMatrix(normal2,sg->real_matrix);
                //ApplyNormalMatrix(normal3,sg->real_matrix);

		ApplyMatrix(normal1,sg->real_matrix);
		ApplyMatrix(normal2,sg->real_matrix);
		ApplyMatrix(normal3,sg->real_matrix);

                normal1[0] /= normal1[2];
                normal1[1] /= normal1[2];

                normal2[0] /= normal2[2];
                normal2[1] /= normal2[2];

                normal3[0] /= normal3[2];
                normal3[1] /= normal3[2];

                triangle->normal1.x = normal1[0];
                triangle->normal1.y = normal1[1];
                triangle->normal1.z = normal1[2];

                triangle->normal2.x = normal2[0];
                triangle->normal2.y = normal2[1];
                triangle->normal2.z = normal2[2];

                triangle->normal3.x = normal3[0];
                triangle->normal3.y = normal3[1];
                triangle->normal3.z = normal3[2];

		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, (memaddr)pp_addr,
			sizeof(PolygonPack), PP_STORE);
    smanager->dma_wait(PP_STORE);

    free(pp);
    free(send_pp);

    return 0;

}