#include <iostream>
#include <math.h>
#include "scene_graph_pack.h"
#include "sys.h"
using namespace std;

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

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

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