#include "SchedTask.h"

#if 0

#include <stdio.h>
#include <string.h>
#include <math.h>
#include "polygon_pack.h"
#include "scene_graph_pack.h"
#include "sys.h"

int
//create_pp(SceneGraphPack *sgp, PolygonPack *pp)
create_pp(void *read, void *write)
{
    SceneGraphPack *sgp = (SceneGraphPack*)read;
    PolygonPack *pp = (PolygonPack*)write;

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

}

void matrix4x4(float *xyz, float *xyz1, float *xyz2) //xyz[16]
{
  for(int t=0; t<16; t+=4)
    {
	for(int i=0; i<4; i++)
	{
	  xyz[t+i] = xyz1[t]*xyz2[i] + xyz1[t+1]*xyz2[4+i] + xyz1[t+2]*xyz2[8+i] + xyz1[t+3]*xyz2[12+i];
	}
    }
}

void get_matrix( float *matrix, float *rxyz, float *txyz, float *stack)
{
  float radx,rady,radz;
  radx = rxyz[0]*3.14/180;
  rady = rxyz[1]*3.14/180;
  radz = rxyz[2]*3.14/180;

#if 0
  vector float rad = (vector float){radx, rady, radz, 0};
  vector float in_sin = _sinf4(rad);
  vector float in_cos = _sinf4(rad);
  float *sin_d = (float *)&in_sin;
  float *cos_d = (float *)&in_cos;

  float sinx = sin_d[0];
  float cosx = cos_d[0];
  float siny = sin_d[1];
  float cosy = cos_d[1];
  float sinz = sin_d[2];
  float cosz = cos_d[2];

#else
  float sinx = sin(radx);
  float cosx = cos(radx);
  float siny = sin(rady);
  float cosy = cos(rady);
  float sinz = sin(radz);
  float cosz = cos(radz);
#endif

  matrix[0] = cosz*cosy+sinz*sinx*siny;
  matrix[1] =sinz*cosx;
  matrix[2] = -cosz*siny+sinz*sinx*cosy;
  matrix[3] = 0;
  matrix[4] = -sinz*cosy+cosz*sinx*siny;
  matrix[5] = cosz*cosx;
  matrix[6] = sinz*siny+cosz*sinx*cosy;
  matrix[7] = 0;
  matrix[8] = cosx*siny;
  matrix[9] = -sinx;
  matrix[10] = cosx*cosy;
  matrix[11] = 0;
  matrix[12] = txyz[0];
  matrix[13] = txyz[1];
  matrix[14] = txyz[2];
  matrix[15] = 1;

  float m[16];

  for(int i=0; i<16; i++)
    {
      m[i] = matrix[i];
    }

  if(stack)
    {
      matrix4x4(matrix, m, stack);
    }

}


int
//update_sgp(SceneGraphPack *sgp, SceneGraphPack *_sgp)
update_sgp(void *rbuf, void *wbuf)
{

    SceneGraphPack *sgp = (SceneGraphPack*)rbuf;
    SceneGraphPack *_sgp = (SceneGraphPack*)wbuf;

    SceneGraphNodePtr node;
    float y_angle;

    for (int i = 0; i < sgp->info.size; i++) {
	//(*my_func[node->move])(node);
	//(*my_func[node->interaction])(node, sgp);

	node = &sgp->node[i];

	// 本当は、ここは上の my_func で行う
	y_angle = node->angle[1];
	y_angle += 1.0f;
	if (y_angle > 360.0f) y_angle = 0.0f;
	node->angle[1] = y_angle;
	//node->obj_pos[0] += 0.5f;
	//node->obj_pos[1] += 0.5f;
	//node->obj_pos[2] += 0.5f;

	if (node->pn != -1) {
	    get_matrix(node->translation,
		       node->angle, node->obj_pos,
		       sgp->node[node->pn].translation);
	} else {
	    get_matrix(node->translation,
		       node->angle, node->obj_pos,
		       NULL);
	}
    }

    // まあこれは多分駄目なんだけど。
    // in/out と update は分ける必要ある？
    // それはユーザ側で in/out になるように書かせるもの？
    memcpy(_sgp, sgp, sizeof(SceneGraphPack));

    return sizeof(SceneGraphPack);
}
 
int (*func_list[16])(void*, void*) = {0, update_sgp, create_pp, 0};
//int (*func_list[16])(void*, void*) = {0, 0, create_pp, 0};
#endif

extern SchedTask*
createTask_createPolygonPack(TaskListPtr _taskList, TaskPtr _task,
			     void *rbuff, void *wbuff, DmaManager *dma);

SchedTask* (*task_list[16])(TaskListPtr, TaskPtr, void*, void*, DmaManager*) = {0, 0, createTask_createPolygonPack, 0};