#include "scene_graph_update.h"

class SceneGraphUpdate : SpeTask {

    SceneGraphUpdate() {
	rbuf = _rbuf;
	wbuf = _wbuf;
    }

  void
  SceneGraphUpdate::matrix4x4(float *xyz, float *xyz1, float *xyz2) {
    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
  SceneGraphUpdate::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
  SceneGraphUpdate::run() {
    SceneGraphPack *sgp = (SceneGraphPack*)rbuf;
    for (int i = 0; i < sgp->size; i++) {
      //SceneGraphNode *node = sgp->node[i];
      (*my_func[sgp->node[i].move])(node);
      (*my_func[sgp->node[i].interaction])(sgp->node[i], sgp);
      if(sgp->node[i].pn != -1) {
	get_matrix(sgp->node[i].translation, sgp->node[i].angle, sgp->node[i].obj_pos, sgp->node[sgp->node[i].pn].translation);
      }
      else {
	get_matrix(sgp->node[i].translation, sgp->node[i].angle, sgp->node[i].obj_pos, NULL);
      }

      copy(sgp->node, wbuf, sgp->node->size());
    }
  }


  void
  Boss::move(SceneGraphNode *node) {
    x = x+1;
  }
  
  
};