#include <stdio.h>
#include <spu_intrinsics.h>
#include "CreatePolygonPack.h"
#include "polygon_pack.h"
#include "scene_graph_pack.h"
#include "sys.h"

//create_pp(SceneGraphPack *sgp, PolygonPack *pp)
//create_pp(void *read, void *write)
//CreatePolygonPack::run(SceneGraphPack *sgp, PolygonPack *pp)
inline float
CreatePolygonPack::sum_across_float4(vector float v)
{
    vector float c12, c2, c3, c4, c34;
    vector float result;

    c2 = spu_rlqwbyte(v, 4);
    c3 = spu_rlqwbyte(v, 8);
    c4 = spu_rlqwbyte(v, 12);
    c12 = spu_add(v,  c2);
    c34 = spu_add(c3, c4);

    result = spu_add(c12, c34);
    return (spu_extract(result, 0));
}

int
CreatePolygonPack::run(void *rbuf, void *wbuf)
{
    SceneGraphPack *sgp = (SceneGraphPack*)rbuf;
    PolygonPack *pp = (PolygonPack*)wbuf;

    float xyz1[4],xyz2[4],xyz3[4];

    for (int i = 0; i < sgp->info.size; i++) {
	SceneGraphNodePtr node = &sgp->node[i];
    
	int n,nt,pt;
	for(n=0,nt=0,pt=0; n<node->size*3; n+=9,nt+=6,pt++) {
	    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);

	    pp->tri[pt].x1 = xyz1[0];
	    pp->tri[pt].y1 = xyz1[1];
	    pp->tri[pt].z1 = xyz1[2];
	    pp->tri[pt].tex_x1 = node->texture[nt];
	    pp->tri[pt].tex_y1 = node->texture[nt+1];

	    pp->tri[pt].x2 = xyz2[0];
	    pp->tri[pt].y2 = xyz2[1];
	    pp->tri[pt].z2 = xyz2[2];
	    pp->tri[pt].tex_x2 = node->texture[nt+2];
	    pp->tri[pt].tex_y2 = node->texture[nt+2+1];

	    pp->tri[pt].x3 = xyz3[0];
	    pp->tri[pt].y3 = xyz3[1];
	    pp->tri[pt].z3 = xyz3[2];
	    pp->tri[pt].tex_x3 = node->texture[nt+4];
	    pp->tri[pt].tex_y3 = node->texture[nt+4+1];

	}
	pp->info.size = pt;
    }

    return sizeof(PolygonPack);
}


void
CreatePolygonPack::rotate(float *xyz, float *matrix)
{
#if 0
    float abc[4];

    abc[0] = xyz[0];
    abc[1] = xyz[1];
    abc[2] = xyz[2];
    abc[3] = xyz[3];
    
    // SIMD 使えるよね
    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];
    }
#else
    vector float *abc = (vector float *)xyz;
    float tmp[4];
    
    vector float matrixT0 = (vector float){matrix[0], matrix[4], matrix[8], matrix[12]};
    vector float matrixT1 = (vector float){matrix[1], matrix[5], matrix[9], matrix[13]};
    vector float matrixT2 = (vector float){matrix[2], matrix[6], matrix[10], matrix[14]};
    vector float matrixT3 = (vector float){matrix[3], matrix[7], matrix[11], matrix[15]};

#if 1
    vector float *v_tmp = (vector float *)tmp;

    *v_tmp = spu_mul(*abc, matrixT0);
    xyz[0] = tmp[0] + tmp[1] + tmp[2] + tmp[3];
    *v_tmp = spu_mul(*abc, matrixT1);
    xyz[1] = tmp[0] + tmp[1] + tmp[2] + tmp[3];
    *v_tmp = spu_mul(*abc, matrixT2);
    xyz[2] = tmp[0] + tmp[1] + tmp[2] + tmp[3];
    *v_tmp = spu_mul(*abc, matrixT3);
    xyz[3] = tmp[0] + tmp[1] + tmp[2] + tmp[3];
#else
    vector float v_tmp;

    v_tmp = spu_mul(*abc, matrixT0);
    xyz[0] = sum_across_float4(v_tmp);
    v_tmp = spu_mul(*abc, matrixT1);
    xyz[1] = sum_across_float4(v_tmp);
    v_tmp = spu_mul(*abc, matrixT2);
    xyz[2] = sum_across_float4(v_tmp);
    v_tmp = spu_mul(*abc, matrixT3);
    xyz[3] = sum_across_float4(v_tmp);
#endif
#endif

}

SchedTask*
createTask_createPolygonPack(TaskListPtr _taskList, TaskPtr _task,
			     void *rbuff, void *wbuff, DmaManager *dma)
{
    return new CreatePolygonPack(_taskList, _task, rbuff, wbuff, dma);
}