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

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

SchedDefineTask1(CreatePolygonFromSceneGraph, 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];
    }
}

static int 
createPolygonFromSceneGraph(SchedTask *smanager, void *rbuf, void *wbuf)
{
    float xyz1[4], xyz2[4], xyz3[4];
    float normal1[4],normal2[4],normal3[4];

    //coord_xyz, coord_tex, normal, matrix, real_matrix を受け取る
    float *coord_xyz   = (float*)smanager->get_input(rbuf, 0);
    float *coord_tex   = (float*)smanager->get_input(rbuf, 1);
    float *normal      = (float*)smanager->get_input(rbuf, 2);
    float *matrix      = (float*)smanager->get_input(rbuf, 3);
    float *real_matrix = (float*)smanager->get_input(rbuf, 4);
    uint32 *pixels     = (uint32*)smanager->get_input(rbuf, 5);

    int sg_size   = (int)smanager->get_param(0);
    int width     = (int)smanager->get_param(1);
    int height    = (int)smanager->get_param(2);
    int scale_max = (int)smanager->get_param(3);

    // triangle を書き戻す
    //TrianglePackPtr triangle = (TrianglePackPtr)smanager->get_output(wbuf, 0);

    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;


    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] = coord_xyz[(i+0)*3];
      xyz1[1] = coord_xyz[(i+0)*3+1];
      xyz1[2] = coord_xyz[(i+0)*3+2]*-1.0f;
      xyz1[3] = 1.0f;
      
      xyz2[0] = coord_xyz[(i+1)*3];
      xyz2[1] = coord_xyz[(i+1)*3+1];
      xyz2[2] = coord_xyz[(i+1)*3+2]*-1.0f;
      xyz2[3] = 1.0f;
      
      xyz3[0] = coord_xyz[(i+2)*3];
      xyz3[1] = coord_xyz[(i+2)*3+1];
      xyz3[2] = coord_xyz[(i+2)*3+2]*-1.0f;
      xyz3[3] = 1.0f;
      
      // matrix = 回転行列*透視変換行列
      ApplyMatrix(xyz1, matrix);
      ApplyMatrix(xyz2, matrix);
      ApplyMatrix(xyz3, 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 = coord_tex[(i+0)*3];
      triangle->ver1.tex_y = 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 = coord_tex[(i+1)*3];
      triangle->ver2.tex_y = 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 = coord_tex[(i+2)*3];
      triangle->ver3.tex_y = coord_tex[(i+2)*3+1];
      
      normal1[0] = normal[(i+0)*3];
      normal1[1] = normal[(i+0)*3+1];
      normal1[2] = normal[(i+0)*3+2]*-1.0f;
      //normal1[3] = 1.0f;
      normal1[3] = 0.0f;
      
      normal2[0] = normal[(i+1)*3];
      normal2[1] = normal[(i+1)*3+1];
      normal2[2] = normal[(i+1)*3+2]*-1.0f;
      //normal2[3] = 1.0f;
      normal2[3] = 0.0f;
      
      normal3[0] = normal[(i+2)*3];
      normal3[1] = normal[(i+2)*3+1];
      normal3[2] = normal[(i+2)*3+2]*-1.0f;
      //normal3[3] = 1.0f;
      normal3[3] = 0.0f;
      
      ApplyNormalMatrix(normal1,real_matrix);
      ApplyNormalMatrix(normal2,real_matrix);
      ApplyNormalMatrix(normal3,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   = pixels;
      triangle->tex_info.width  = width;
      triangle->tex_info.height = height;
      triangle->tex_info.scale_max = scale_max;
    }
    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;
}