#include <iostream>
#include <SDL_image.h>
#include <libxml/parser.h>
#include "SceneGraphRoot.h"
#include "xml.h"
#include "matrix_calc.h"
#include "TextureHash.h"
#include "texture.h"
#include "Application.h"
#include "polygon.h"

static int cnt = 0;
static const int SGLIST_LENGTH = 138;
static int sg_src_size = SGLIST_LENGTH ;
static int sg_src_id = -1;
static SceneGraphPtr *sg_src;

static TextureHash sgid_hash;


SceneGraphRoot *sgroot;

SceneGraphRoot::SceneGraphRoot(float w, float h, TaskManager *manager)
{

    sgroot = this;
    sgroot->tmanager = manager;

    // SGLIST_LENGTH 決め打ちかぁ、動的生成にする場合上限決めておいた方がいいのかな
    sg_src = (SceneGraphPtr*) malloc(sizeof(SceneGraphPtr)*SGLIST_LENGTH);

    camera = new Camera(w, h, this, sgroot->tmanager);

    iterator = new SceneGraphIterator;
    controller = create_controller();

    sg_exec_tree = NULL;
    sg_draw_tree = NULL;
    sg_available_list = NULL;
    sg_remove_list = NULL;


    screen_w = (int)w;
    screen_h = (int)h;

    light_init();
 
    move_finish_flag = 0;

    gtask_array = NULL;

    // TODO
    //   今はとりあえず camera を Root にしています
    //   今はそれすらもしてません
    //sg_exec_tree = camera;
}


SceneGraphRoot::~SceneGraphRoot()
{
    SceneGraphPtr p = sg_available_list;

    while (p) {
	SceneGraphPtr tmp = p->next;
	delete p;
	p = tmp;
	cnt--;
    }

    p = sg_remove_list;

    while (p) {
	SceneGraphPtr tmp = p->next;
	delete p;
	p = tmp;
	cnt--;
    }

    free(sg_src);
    delete camera;
    int light_num = 4;
    for (int i = 0; i < light_num; i++) {
	delete light[i];
    }
    delete iterator;
    delete controller;

    if (gtask_array != NULL) {
	delete gtask_array;
    }
}

void
SceneGraphRoot::light_init()
{

   int light_num = 4;
    light_sysswitch = 0;

    for (int i = 0; i < light_num; i++) {

        light[i] = new SceneGraph(sgroot->tmanager);
	light[i]->xyz[0] = 0;
	light[i]->xyz[1] = 0;
	light[i]->xyz[2] = 0;
	light[i]->xyz[3] = 1.0f;

	light_switch[i] = 0;

    }

    for (int i = 0; i < 4; i++) {
        light_vector[i*4]   = 0.0f;
        light_vector[i*4+1] = 0.0f;
        light_vector[i*4+2] = 0.0f;
        light_vector[i*4+3] = 1.0f;
    }

}


/**
 * xml ファイルから生成された SceneGraph を sg_src に登録する。
 *
 * @param sg SceneGraph created by xmlfile
 */
void
SceneGraphRoot::registSceneGraph(SceneGraphPtr sg)
{
    int dup;
    if ((dup = getSgid(sg->name))>=0) { // while...
	sg_src[dup]->name = "";
	// we should remove this. but some one may use it...
    }
    if (sg_src_id+1> sg_src_size) {
	sg_src_size *= 2;
	sg_src = (SceneGraphPtr*)realloc(sg_src, sg_src_size*sizeof(SceneGraphPtr));
    }
    sg->sgid = ++sg_src_id;
    sg_src[sg->sgid] = sg;
    sgid_hash.sg_hash_regist((const char*)sg->name, sg->sgid);
}


void
SceneGraphRoot::addNext(SceneGraphPtr sg)
{
    SceneGraphPtr last = sg_available_list;

    if (!last) {
	sg_available_list = sg;
    } else {
	while (last->next) {
	    last = last->next;
	}
	last->next = sg;
	sg->prev = last;
    }

    cnt++;
}

/*文字列の生成*/
void
SceneGraphRoot::createStringFont(TaskManager *manager, SceneGraphPtr root,
				 const char *string,int pixels,int screen_w,
				 int screen_h,Uint32 color)
{
    SceneGraphPtr text;
  float width_shift = 0;
  int i;
  int length = strlen(string);
  for (i = 0; i < length;) {
    int len = 0;
    unsigned char initial = string[i];
    while (1) {
      unsigned char mask = 0x80;
      if (mask & initial) {
	len++;
      } else {
	if (len == 0) len++;
	break;
      }
      initial <<= 1;
    }
    char *obj_name;
    float scale[] = {1,1,1};
    sgroot->createFont(manager,"/Library/Fonts/Osaka.ttf", pixels, color, &string[i], len, &obj_name);
    text = sgroot->createSceneGraph(obj_name);
    text->xyz[0] = screen_w/2 + width_shift -100;
    text->xyz[1] = screen_h/2;
    text->xyz[2] = -100;
    width_shift += text->seq;
    root->addChild(text);
    get_matrix_scale(text->matrix, text->angle, text->xyz, scale, root->matrix);
    i += len;
  }
}

void
SceneGraphRoot::createFont(TaskManager *manager,const char *font, int pixels,
			   Uint32 color,const char *string_name, int len,
			   char **obj_name)
{
  *obj_name = (char *)malloc(sizeof(char) * 12);
  char sname[] = "char:";
  memcpy(*obj_name, sname, 5);
  memcpy(*obj_name + 5, string_name, len);
  (*obj_name)[5+len] = '\0';
  printf("%d",sgid_hash.get_sgid(*obj_name));
  if (sgid_hash.get_sgid(*obj_name) != -1) {
    return;
  }
  SceneGraphPtr tmp = new SceneGraph(manager, font, pixels, color,
				     *obj_name);
  registSceneGraph(tmp);
}

/* XMLファイルからポリゴンを作成  */
void
SceneGraphRoot::createFromXMLfile(TaskManager *manager, const char *xmlfile)
{
    xmlDocPtr doc;
    xmlNodePtr cur;
    SceneGraphPtr tmp;

    /* パース DOM生成
       xmlParseFile:ファイルに含まれるXML文書を分析する
       xmlDocGetRootElement:ドキュメントルートを指定する。
       よって、以下のcurにはドキュメントルートの位置が入っている
     */
    doc = xmlParseFile(xmlfile);
    cur = xmlDocGetRootElement(doc);

    /* XMLのノードを一つずつ解析  */
    for (cur=cur->children; cur; cur=cur->next) {
      /*初期化:curをドキュメントルートの一個下に設定
	継続条件:curが真である
	再初期化:次のノードへ
       */

      /* 扱うのはsurfaceオンリーなので、ノードの名前がsurfaceでないなら
	 以下の処理を行なわずにスキップする
     　*/
      if (xmlStrcmp(cur->name,(xmlChar*)"surface") != 0) {
	  continue;
      }
      
      /* ポリゴン(SceneGraph)生成
	 SceneGraph:SceneGraph.cc、L186
      */
	tmp = new SceneGraph(manager, cur);
	
	//シーングラフを登録
	registSceneGraph(tmp);
    }
    //解放
    xmlFreeDoc(doc);
}

/*static const char*
get_property(const char *name, xmlNodePtr cur){
   xmlAttr *p=cur->properties; 
   if (p==0) return "";
   for ( ;p; p=p->next) {
       if ( xmlStrcmp(p->name, (xmlChar*)name) !=0 ) {
	   xmlNode* n=p->children;
	  if ( n==NULL ) return "";
	  const char * v=(const char*)n->content;
	  if ( v==NULL ) return "";
	  return v;
       }
   }
   return "";
}*/

typedef struct source {
    char *id;
    union {
        float *array;
        char *alias;
    }u;
    int count;
    struct source *next;
} SOURCE;
typedef SOURCE *SOURCE_P;

typedef struct list {
    SOURCE_P first;
    SOURCE_P end;
} LIST;
typedef LIST *LIST_P;

/* add source list */
static void
addSource(LIST_P list, SOURCE_P src) {
    if (list->first == NULL && list->end == NULL) {
        list->first = list->end = src;
        return;
    }
    list->end->next = src;
    list->end = src;
}

/* compare a with b. Using to compare id */
/*
static int
strcmp_a(const char *a, const char *b)
{
    while (*a && *a++ == *b++);
    if (*a) return 0;
    return a[-1] > b[-1] ? 1:-1;
}
static float
get_point(char *id, int position, LIST_P list)
{
    SOURCE_P cur = list->first;
    for (;cur ; cur=cur->next) {
        if (!strcmp_a(id, cur->id)) {
            if (cur->count == 0) //alias 
                return get_point(cur->u.alias, position, list);
            float *a = cur->u.array;
            if (position <= cur->count) {
                return a[position];
            }
       }
    }
}
*/
/**
 * co
 */
static SOURCE_P
most_match(const char *id , LIST_P list)
{
    SOURCE_P src = NULL;
    SOURCE_P cur = NULL;;
    int tmplength = 0;
    int strlength;
    for (cur=list->first ;cur!=list->end ;cur=cur->next) {
        for (strlength=0;id[strlength]==cur->id[strlength];strlength++); 
        if (tmplength < strlength) {
            tmplength = strlength;
            src = cur;
        }
    }
    
    if (src == NULL){
      fprintf(stderr,"not match");
    }
    return src;
}

struct collada_state {
    collada_state(){
        polylist = 0;

        vertex_offset = -1;
        vertex_count = 0;

	normal_offset = -1;
	normal_count = 0;

	texcoord_offset = -1;
	texcoord_count = 0; 
	
	polylist_count = 0;
	vcsum = 0;

    }
    int polylist;
    int library_images;
    xmlChar *pid;

    char *vertex_src;
    int vertex_offset;
    int vertex_count;

    char *normal_src;
    int normal_offset;
    int normal_count;

    char *texcoord_src;
    int texcoord_offset;
    int texcoord_count;
  
    float *vcount;
    float *pcount;
  
    SOURCE_P normal_float;
    SOURCE_P vertex_float;
    SOURCE_P texcoord_float;

    char *vertices_src;
    int polylist_count;
 
    char *name;
    char *tex_picname;
    int vcsum;
    int limit;
}; 

static texture_list list[TABLE_SIZE];
static texture_list *texture_info;

void get_texpic(char *filename, SceneGraphPtr sg,  xmlNodePtr cur, TaskManager *manager)
{
    char image_name[20] = "/tmp/image_XXXXXX";
    if (filename == NULL || filename[0] == 0) {
        return;
    }

    /**
     * image_name を既に Load していれば何もしない
     */
    int tex_id;
    /* ball test */
    if (sgid_hash.sg_hash_regist(/*filename*/"Ball", tex_id) == -1) {

        SDL_Surface *texture_image = sg->load_decode_image(filename, image_name, cur);
	    if (texture_image==0) {
	    printf("Can't load image %s\n",filename);
	    exit(0);
	    }
        texture_info->texture_id = sg->makeTapestries(manager, texture_image, tex_id);
	    tex_id = texture_info->texture_id;

        if (unlink(image_name)) {
            printf("unlink error\n");
        }
    } else {
        /**
         * 以前に Load されている Texture を共用
         */
        texture_info->texture_id = tex_id;
    }

      // こんなことすると list[] のいみあるのかなーと
      // 微妙に思う、自分で書き換えた感想 by gongo
      texture_info->t_w = list[tex_id].t_w;
      texture_info->t_h = list[tex_id].t_h;;
      texture_info->pixels_orig = list[tex_id].pixels_orig;
      texture_info->pixels = list[tex_id].pixels;
      texture_info->scale_max = list[tex_id].scale_max;
      texture_info->texture_image = list[tex_id].texture_image;

}
 
void decode_float_array(xmlNodePtr cur,LIST_P list ){ 
    SOURCE_P src = (SOURCE_P)malloc(sizeof(SOURCE));
    char *id = (char*)xmlGetProp(cur, (xmlChar*)"id");
    src->id = (char*)xmlGetProp(cur, (xmlChar*)"id");
    
    int count = atoi((char*)xmlGetProp(cur, (xmlChar*)"count"));
    src->count = atoi((char*)xmlGetProp(cur, (xmlChar*)"count"));
    src->u.array = new float[src->count];
    char *cont =(char*)xmlNodeGetContent(cur);
    //const char *id = get_property("id", cur);
    //int count = atoi(get_property("count", cur));
    
    /* store float inpoint list */
    for (int i = 0; cont != NULL; i++) {
        cont = pickup_float(cont, src->u.array+i);
    }

    src->next = NULL;
    addSource(list, src);
    printf("id:%s count:%d cont:%s\n", id, count, cont);
}


void 
get_points(xmlNodePtr cur, collada_state *s, TaskManager *manager){
    char *pcont = (char*)xmlNodeGetContent(cur);
    for (int i = 0;i < s->polylist_count;i++){
       s->vcsum += s->vcount[i];
    }
    s->limit = s->vcsum * 2;
    if (s->texcoord_offset == 2){
        s->limit = s->vcsum * 3;
    }
    s->pcount = new float[s->limit];
    for (int i=0;i<s->limit;i++){
        s->pcount[i] = 0;
    }
    for (int i=0; pcont != NULL; i++) {
        pcont = pickup_float(pcont, s->pcount+i);
    }
}

SceneGraph*
decode_points(xmlNodePtr cur, collada_state *s, TaskManager *manager){
    int *vertexp;
    vertexp = new int[s->vcsum];
    for (int i=0;i<s->vcsum;i++){
       vertexp[i]=0;
    }
    float *vertex_table;
    float *normal_table;
    float *texcoord_table; 
    vertex_table = new float[s->vcsum];
    normal_table = new float[s->vcsum];
    texcoord_table = new float[s->vcsum];
    for (int i=0;i < s->vcsum;i++){
        vertex_table[i] = 0;
	normal_table[i] = 0;
	texcoord_table[i] = 0;
    }

    /**
     * s->vcsum と s->vertex_float->countの値が違うので大きい方をとりあえず使っておく
     */

    /* p separate vertex position and nomal position. */
    if (s->texcoord_offset == 2){
        for (int i=0,j=0; i < s->limit; i+=3,j++) {
	    vertexp[j] = (int)s->pcount[i];
	    normal_table[j] = s->normal_float->u.array[(int)s->pcount[i+1]];
	    texcoord_table[j] = s->texcoord_float->u.array[(int)s->pcount[i+2]];
	}
    } else{
        for (int i=0,j=0; i < s->limit; i+=2,j++) {
	    vertexp[j] = (int)s->pcount[i];
	    normal_table[j] = s->normal_float->u.array[(int)s->pcount[i+1]];
	}
    }

    for (int i=0;i<s->vcsum;i++) {
        if (s->vcount[i] == 4) {
	    for (int j=0; j < s->vcount[i]; j++) {
	        vertex_table[i] = s->vertex_float->u.array[vertexp[i]];
		vertex_table[i+3] = s->vertex_float->u.array[vertexp[i+1]];
		i += 2;
	    }
	}else if (s->vcount[i]==3) {
	    vertex_table[i] = s->vertex_float->u.array[vertexp[i]];
	}
    }
    /**
     * (SceneGraph.cc)
     * pickup_normal,pickup_coordinate,pickup_textureの処理
     * vcsumは頂点の数,countは面の数
     */
    int count = s->vcsum / 3; 
    //polygonの作成
    SceneGraphPtr sg = new SceneGraph(manager);
    sg->pp_num = (count + MAX_SIZE_TRIANGLE - 1) / MAX_SIZE_TRIANGLE;
    sg->pp = new PolygonPack[sg->pp_num];
    
    for (int i = 0;i < sg->pp_num; i++ ){
        PolygonPackPtr pp = sg->pp;
	TrianglePackPtr tri =  pp[i].tri;
	// TrianglePack の size のチェック
        int tri_size = (count < MAX_SIZE_TRIANGLE) ? count : MAX_SIZE_TRIANGLE ;
	pp[i].info.size = tri_size;
	int k = 0;
	int m = 0;
	int n = 0;
	for (int j = 0; j < tri_size; j++) {
	    tri[j].normal1.x = normal_table[k++];
	    tri[j].normal1.y = normal_table[k++];
	    tri[j].normal1.z = normal_table[k++];
	    
	    tri[j].normal2.x = normal_table[k++];
	    tri[j].normal2.y = normal_table[k++];
	    tri[j].normal2.z = normal_table[k++];
	    
	    tri[j].normal3.x = normal_table[k++];
	    tri[j].normal3.y = normal_table[k++];
	    tri[j].normal3.z = normal_table[k++];
	    
	    tri[j].ver1.tex_x = texcoord_table[m++];
	    tri[j].ver1.tex_y = texcoord_table[m++];
	    
	    tri[j].ver2.tex_x = texcoord_table[m++];
	    tri[j].ver2.tex_y = texcoord_table[m++];
	    
	    tri[j].ver3.tex_x = texcoord_table[m++];
	    tri[j].ver3.tex_y = texcoord_table[m++];
	    
	    tri[j].ver1.x = vertex_table[n++];
	    tri[j].ver1.y = vertex_table[n++];
	    tri[j].ver1.z = vertex_table[n++];
	    
	    tri[j].ver2.x = vertex_table[n++];
	    tri[j].ver2.y = vertex_table[n++];
	    tri[j].ver2.z = vertex_table[n++];
	    
	    tri[j].ver3.x = vertex_table[n++];
	    tri[j].ver3.y = vertex_table[n++];
	    tri[j].ver3.z = vertex_table[n++];

	}
	
    }
    sg->c_xyz[0] = sg->c_xyz[1] = sg->c_xyz[2] = 0;
    
    int tex_id = 0;
    sgid_hash.sg_hash_regist(s->name, tex_id);
    //get_texpic(s->tex_picname, sg, cur , manager);
 
    delete []vertexp;
    delete []vertex_table;
    delete []normal_table;
    delete []texcoord_table;

    /* got out of polylist */
    s->polylist = 0;
    return sg;
}

static void 
xml_walk(xmlNodePtr cur, struct collada_state *s, LIST_P list,SceneGraphRoot *root)
{
    int in_polylist=0;
    printf("name = %s, child:%s\n", (char *)cur->name, (char *)cur->children);
    printf("s->polylist = %d\n",s->polylist);
    if (!xmlStrcmp(cur->name, (xmlChar*)"polylist")) {
        s->polylist_count = atoi((char*)xmlGetProp(cur, (xmlChar*)"count"));
        s->polylist=1;
        in_polylist=1;
    } else if (!xmlStrcmp(cur->name, (xmlChar*)"vertices")) {
        s->pid = xmlGetProp(cur, (xmlChar*)"id");
    } else if (!xmlStrcmp(cur->name, (xmlChar*)"library_images")) {
        s->library_images=1;// library_images is wrote at texture image name. only use one image file
    } else if (s->library_images && !xmlStrcmp(cur->name, (xmlChar*)"init_from")) { 
        s->tex_picname = (char*)xmlGetProp(cur, (xmlChar*)"init_from");
        printf("------------------%s",s->tex_picname);
        s->library_images=0;
    } else if (!s->polylist && !xmlStrcmp(cur->name, (xmlChar*)"input")) {
        char *semantic = (char*)xmlGetProp(cur, (xmlChar*)"semantic");
        if (!xmlStrcmp((xmlChar*)semantic, (xmlChar*)"POSITION")) {
	    s->vertices_src = (char*)xmlGetProp(cur, (xmlChar*)"source");
        }
    } else if (s->polylist && !xmlStrcmp(cur->name, (xmlChar*)"input")) {
	char *semantic = (char*)xmlGetProp(cur, (xmlChar*)"semantic");
        if (!xmlStrcmp((xmlChar*)semantic, (xmlChar*)"VERTEX")) {
	    s->vertex_src = (char*)xmlGetProp(cur, (xmlChar*)"source");
	    s->vertex_offset = atoi((char*)xmlGetProp(cur, (xmlChar*)"offset"));             
	    s->vertex_float = most_match(s->vertices_src+1, list);
        } else if (!xmlStrcmp((xmlChar*)semantic, (xmlChar*)"NORMAL")) {
            s->normal_src = (char*)xmlGetProp(cur, (xmlChar*)"source");
            s->normal_offset = atoi((char*)xmlGetProp(cur, (xmlChar*)"offset"));
            s->normal_float = most_match(s->normal_src+1, list);
        } else if (!xmlStrcmp((xmlChar*)semantic, (xmlChar*)"TEXCOORD")) {
            s->texcoord_src = (char*)xmlGetProp(cur, (xmlChar*)"source");
            s->texcoord_offset = atoi((char*)xmlGetProp(cur, (xmlChar*)"offset"));
            s->texcoord_float = most_match(s->texcoord_src+1, list);
	}
    } else if (!xmlStrcmp(cur->name, (xmlChar*)"vcount")) {
        char *vcont = (char*)xmlNodeGetContent(cur);
	s->vcount = new float[s->polylist_count];
        for (int i=0; vcont!=NULL; i++) {
	    /* store vcount list */
	    vcont = pickup_float(vcont, s->vcount+i);
        }
    } else if (!xmlStrcmp(cur->name, (xmlChar*)"p")) {
        get_points(cur,s,root->tmanager);
	in_polylist = 0;
    } else if (!xmlStrcmp(cur->name, (xmlChar*)"float_array")) {
        decode_float_array(cur,list);
    } else if (!xmlStrcmp(cur->name, (xmlChar*)"node" )) {
        s->name = (char*)xmlGetProp(cur, (xmlChar*)"id");
    }
    for (cur=cur->children; cur; cur=cur->next){
        xml_walk(cur,s,list,root);
    }
}

void
init_list(LIST_P list) {
      list->first = NULL;
        list->end = NULL;
}

void
SceneGraphRoot::createFromCOLLADAfile(TaskManager *manager, const char *xmlColladafile)
{
    /*make parse dom*/
    xmlDocPtr doc;
    xmlNodePtr cur;
    //,cur_images,cur_effects,cur_geometries,cur_visual_scenes;
    //SceneGraphPtr tmp;
    
    doc = xmlParseFile(xmlColladafile);
    cur = xmlDocGetRootElement(doc);
    
    if (xmlStrcmp(cur->name, (xmlChar*)"COLLADA")){
        return ;
    }

    /* node analyze */
    struct collada_state s;
    for (cur=cur->children; cur; cur=cur->next){
        LIST list;
        init_list(&list);
        xml_walk(cur,&s,&list,this);
    }
    registSceneGraph(decode_points(cur,&s,manager));
    xmlFreeDoc(doc);
}

void
SceneGraphRoot::createFromXMLmemory(TaskManager *manager, SceneGraph *node, char *data, int len)
{
    xmlDocPtr doc;
    xmlNodePtr cur;
    // size は取れるはず、テスト用に mmap したデータを使う
    /* パース DOM生成 */

    doc = xmlParseMemory(data, len);
    cur = xmlDocGetRootElement(doc);

    /* XMLのノードを一つずつ解析  */
    for (cur=cur->children; cur; cur=cur->next) {
	/* 扱うのはsurfaceオンリー  */
	if (xmlStrcmp(cur->name,(xmlChar*)"surface") != 0) {
	    continue;
	}
	/* ポリゴン(SceneGraph)生成  */
	SceneGraphPtr original = new SceneGraph(manager, cur);
	registSceneGraph(original);
	SceneGraphPtr clone = createSceneGraph(original->sgid);
	node->addChild(clone);
    }
    xmlFreeDoc(doc);
}

SceneGraphPtr
SceneGraphRoot::createSceneGraph(int id)
{
    SceneGraphPtr src;
    SceneGraphPtr p;

    if (id < 0 || id > sg_src_size) {
	printf("error: createSceneGraph(int id): id not found.\n");
	return NULL;
    }

    /* オリジナルの SceneGraph */
    src = sg_src[id];

    /* ユーザーにはオリジナルの clone を返す */
    p = src->clone(this->tmanager);

    /* move, collision に sgroot を渡したいのでここで sgroot を渡しておく*/
    p->sgroot = (void *)this;

    addNext(p);

    return p;
}




SceneGraphPtr
SceneGraphRoot::createSceneGraph(const char *name)
{
    SceneGraphPtr src;
    SceneGraphPtr p;

    int id = getSgid(name);
    if (id < 0) {
	printf("error: createSceneGraph(name): name object not found.\n");
	return NULL;
    }

    /* オリジナルの SceneGraph */
    src = sg_src[id];

    /* ユーザーにはオリジナルの clone を返す */
    p = src->clone(this->tmanager);

    /* move, collision に sgroot を渡したいのでここで sgroot を渡しておく*/
    p->sgroot = (void *)this;

    addNext(p);

    return p;
}

int
SceneGraphRoot::getSgid(const char *name)
{
  return sgid_hash.get_sgid(name);
}

int
SceneGraphRoot::getLast()
{
    if (sg_src_id>=0)
	return sg_src[sg_src_id]->sgid;
    return -1;
}

/**
 * 何も表示しない、move,collision もしない SceneGraph を生成
 * いずれ、Transform3D 的なものに回す予定
 */
SceneGraphPtr
SceneGraphRoot::createSceneGraph()
{
    SceneGraphPtr p = new SceneGraph(sgroot->tmanager);

    /* move, collision に sgroot を渡したいのでここで sgroot を渡しておく*/
    p->sgroot = (void *)this;

    addNext(p);
    p->flag_drawable = 0;

    return p;
}


// light Object も SceneGraph の一部としてしまえば、別個に計算しなくていい
void
SceneGraphRoot::lightCalc(SceneGraphPtr cur_parent)
{
    int light_num = 4;
    float light_vector_tmp[16];
    
    for (int i = 0; i < 4; i++) {
        light_vector_tmp[i*4]   = 0.0f;
        light_vector_tmp[i*4+1] = 0.0f;
        light_vector_tmp[i*4+2] = 0.0f;
        light_vector_tmp[i*4+3] = 1.0f;
    }


    for (int i = 0; i < light_num; i++) {

        get_matrix(light[i]->matrix, light[i]->angle, light[i]->xyz, cur_parent->matrix);
      	ApplyMatrix(&light_vector_tmp[i*4], light[i]->matrix);

        light_vector_tmp[i*4] /= light_vector_tmp[i*4+3];
        light_vector_tmp[i*4+1] /= light_vector_tmp[i*4+3];
        light_vector_tmp[i*4+2] /= light_vector_tmp[i*4+3];

	/*SIMD演算のため*/
	light_vector_tmp[i*4+2] *= -1;
	light_vector_tmp[i*4+3] *= -1;

    for (int i = 0; i < 4; i++) {
        light_vector[i*4] = light_vector_tmp[i*4];
        light_vector[i*4+1] = light_vector_tmp[i*4+1];
        light_vector[i*4+2] = light_vector_tmp[i*4+2];
        light_vector[i*4+3] = light_vector_tmp[i*4+3];
    }

    }
}

void
SceneGraphRoot::flip()
{
    // 前フレームで描画した SceneGraph は削除
    allRemove(sg_remove_list);

    // 前フレームに作られた SceneGraph は描画用に移行
    // 現フレームでの操作は以下の tree,list には適用されない
    sg_draw_tree = sg_exec_tree;
    sg_remove_list = sg_available_list;

    // 現フレームで新しく SceneGraph がコピーされるので初期化
    sg_exec_tree = NULL;
    sg_available_list = NULL;
}

void
SceneGraphRoot::allExecute(int screen_w, int screen_h)
{
    SceneGraphPtr list = sg_available_list;

    flip();

    camera->move_execute(screen_w, screen_h);
    camera->update(screen_w, screen_h);

    camera->children = NULL;
    camera->lastChild = NULL;

    /*まずは全部動作させる*/
    while (list) {

        list->move_execute(screen_w, screen_h);
        list->collision_check(screen_w, screen_h, list);

	list->frame++;
        list = list->next;
    }

    if(sg_exec_tree != NULL) {
	return;
    }

    /* ここで、scenegraph node の matrix に演算する座標変換は、
     * world->view->perspective まで。 
     *
     * CreatePolygonFromSceneGraph で perspective の座標系で lighting の演算を行い、その後
     * screen 変換をするので。
     *
     * その際に、camera がもつ screen matrix を Task に渡す必要がある
     * Task に screen matrix を渡す部分は viewer.cc にある
     *
     * world 変換は node が持つ matrix で行う
     * view->perspective 変換は camera が持つ matrix で行う
     *

     *
     * (w) = world matrix
     * (v) = view matrix
     * (p) = perspective matrix
     * (s) = screen matrix
     *
     * --- copyTree ---
     *
     * node->(wvp) = node->(w) * node->parent->(w) * ..... camera->(v) * camera->(p)
     *
     * --- CreatePolygonFromSceneGraph ---
     * 
     * (polygon_vertex) * node->(wvp)
     * (light_position) * node->(wvp)
     * (normal_vector) * normal_matrix
     *
     * lighting(polygon_vertex, light_position, normal_vector)
     *
     * (polygon_vertex) * camera->(s)
     */

    matrix4x4(camera->out_matrix, camera->m_view, camera->m_pers);
    lightCalc(camera);
    copyTree(sg_draw_tree, camera);

    // 現在、allExecute が終わった時点では
    // camera->children が User SceneGraph の root になる

    /**
     * NULL じゃなかったら、setSceneData が呼ばれてるから
     * そっちを次の Scene にする
     */

    sg_exec_tree = camera->children;
}

struct st_matrix {
    float *m;
    st_matrix *next;
    st_matrix *prev;
};

typedef struct matrix_list {
    int length;
    st_matrix *first;
    st_matrix *end;
}matrix_list, *matrix_listp;

static void
initList(matrix_list *list) {
    list->first = NULL;
    list->end = NULL;
    list->length = 0;
}


static void
addMatrix(matrix_list *list, float *matrix) {

    ++list->length;

    st_matrix *sm = new st_matrix;
    sm->m = matrix;
    sm->next = NULL;
    sm->prev = NULL;
    
    if (list->first == NULL && list->end == NULL) {
        list->first = list->end = sm;
        return;
    }
    list->end->next = sm;
    sm->prev = list->end;
    list->end = sm;
}

static void
popMatrix(matrix_list *list) {

    st_matrix *end = list->end;
    --list->length;

    if (end != list->first) {
        st_matrix *new_end = end->prev;
        new_end->next = NULL;
        list->end = new_end;
    } else {
        list->first = NULL;
        list->end = NULL;
    }

    delete end;
}

static void
removeMatrix(matrix_list *list) {
    st_matrix *p = list->first;
    while (p) {
	st_matrix *p1 = p->next;
	delete p;
	p = p1;
    }
}


void
SceneGraphRoot::copyTree(SceneGraphPtr t, SceneGraphPtr cur_parent)
{
    // SceneGraphPtr t = sg_draw_tree;

    // 
    //int matrix_size = 16; // 4x4の行列の大きさ
    matrix_list *mlist = new matrix_list;
    initList(mlist);
    addMatrix(mlist, cur_parent->matrix);

    /*removeのflagをもとにtreeを形成*/
    while (t) {
	SceneGraphPtr c = NULL;
	if (!t->isRemoved()) {
	    c = t->clone(this->tmanager);
	    addNext(c);
	    cur_parent->addChild(c);
	    c->frame = t->frame;
	    /*親の回転、座標から、子の回転、座標を算出*/
	    //get_matrix(c->matrix, c->angle, c->xyz, cur_parent->matrix);
            
            /***** 
               // うーん、SceneGraphRoot内で task create しちゃっていいのか？

               HTaskPtr cmat = create_task_array(CALC_MATRIX, 1, 1, mlist->length+1, 1); // +1は自分の分

               int index = 0;
               for (st_matrixp m = list->fisrt; m != NULL; m = m->next) {
                 cmat->set_inData(index, m->matrix, sizeof(float)*matrix_size);
                 index++;
               }
               cmat->set_inData(index, t->matrix, sizeof(float)*matrix_size);

               cmat->set_outData(0, c->out_matrix, sizeof(float)*matrix_size);
               cmat->set_cpu(SPE_ANY);
               task_next->wait_for(cmat);
               cmat->spawn_task_array(cmat->next());
               cmat->spawn();

               // きちんと matrix を list にできるかテストしなきゃ
             ******/

            // どこかで、オブジェクトの状態は保持してないといけない。
            // なので、c->matrix でもつ。 
            matrix4x4(c->out_matrix, c->matrix, cur_parent->out_matrix);
	}


	if (t->children != NULL && c != NULL) {
            addMatrix(mlist, t->matrix);
	    cur_parent = c;
	    t = t->children;
	} else if (t->brother != NULL) {
	    t = t->brother;
	} else {
	    while (t) {
		if (t->brother != NULL) {
		    t = t->brother;
		    break;
		} else {
		    if (t->parent == NULL) {
			t = NULL;
			break;
		    } else {
                        popMatrix(mlist);
                        cur_parent = cur_parent->parent;
                        t = t->parent;
		    }
		}
	    }
	}
    }

    removeMatrix(mlist);
    delete mlist;

}


void
SceneGraphRoot::treeApply(int screen_w, int screen_h)
{
    // don't calcurate sg_draw_tree's brother
    transTree(sg_draw_tree->children, camera);
}


/**
 *     破壊的に変換行列の親子関係を計算する
 */
void
SceneGraphRoot::transTree(SceneGraphPtr t, SceneGraphPtr cur_parent)
{
    // SceneGraphPtr t = sg_draw_tree;

    /*removeのflagをもとにtreeを形成*/
    while (t) {
	SceneGraphPtr c = NULL;
	if (!t->isRemoved()) {
	    /*親の回転、座標から、子の回転、座標を算出*/
	    matrix4x4(t->matrix,t->matrix,cur_parent->matrix);
	} 
	if (t->children != NULL && c != NULL) {
	    cur_parent = t;
	    t = t->children;
	} else if (t->brother != NULL) {
	    t = t->brother;
	} else {
	    while (t) {
		if (t->brother != NULL) {
		    t = t->brother;
		    break;
		} else {
		    if (t->parent == NULL) {
			t = NULL;
			break;
		    } else {
                        cur_parent = cur_parent->parent;
                        t = t->parent;
		    }
		}
	    }
	}
    }

}

/*
  ExecMove task の post func として呼んでやる
*/
void
SceneGraphRoot::move_finish()
{
    list->collision_check(screen_w, screen_h, list);

    list->frame++;
    //list = list->next;

    int light_num = 4;
    for (int i = 0; i < light_num; i++) {

	get_matrix(light[i]->matrix, light[i]->angle, light[i]->xyz, camera->matrix);
	ApplyMatrix(&light_vector[i*4], light[i]->matrix);

	light_vector[i*4] /= light_vector[i*4+3];
	light_vector[i*4+1] /= light_vector[i*4+3];
	light_vector[i*4+2] /= light_vector[i*4+3];


	light_vector[i*4+2] *= -1;
	light_vector[i*4+3] *= -1;
    }

    //sgchange->viewer->light_xyz_stock = getLightVector();
}


void
SceneGraphRoot::appTaskRegist(regist_func new_register)
{
    this->regist = new_register;
}

void
SceneGraphRoot::regist_execute()
{
    (*regist)(this);
}

void
SceneGraphRoot::allRemove(SceneGraphPtr list)
{
    SceneGraphPtr p = list;

    while (p) {
	SceneGraphPtr p1 = p->next;
	delete p;
	p = p1;
	cnt--;
    }
}

// 呼ばれてない
void
SceneGraphRoot::checkRemove()
{
    SceneGraphPtr p = sg_available_list;
    SceneGraphPtr p1;

    while (p) {
	p1 = p->next;
	if (p->isRemoved()) {
	    sg_exec_tree = p->realRemoveFromTree(sg_exec_tree);
	    sg_available_list = p->realRemoveFromList(sg_available_list);
	}
	delete p;
	p = p1;
    }
}

SceneGraphPtr
SceneGraphRoot::getExecuteSceneGraph()
{
    return sg_exec_tree;
}


void
printSceneGraph(SceneGraphPtr t)
{
    while (t) {
	if (!t->isRemoved()) {
	    if (t->name) printf("name: %s ",t->name);
	    printf("x=%g y=%g z=%g\n",t->xyz[0],t->xyz[1],t->xyz[2]);
	}
	if (t->children != NULL) {
	    t = t->children;
	} else if (t->brother != NULL) {
	    t = t->brother;
	} else {
	    while (t) {
		if (t->brother != NULL) {
		    t = t->brother;
		    break;
		} else {
		    if (t->parent == NULL) {
			t = NULL;
			break;
		    } else {
                        t = t->parent;
		    }
		}
	    }
	}
    }
}

SceneGraphPtr
SceneGraphRoot::getDrawSceneGraph()
{
    return sg_draw_tree;
}

void
SceneGraphRoot::updateControllerState()
{
    controller->check();
}

void
SceneGraphRoot::setSceneData(SceneGraphPtr sg)
{
    sg_exec_tree = sg;
}

Pad*
SceneGraphRoot::getController()
{
    return controller;
}

SceneGraphIteratorPtr
SceneGraphRoot::getIterator()
{
    iterator->set(sg_remove_list);
    return iterator;
}

SceneGraphIteratorPtr
SceneGraphRoot::getIterator(SceneGraphPtr list)
{
    iterator->set(list);
    return iterator;
}

CameraPtr
SceneGraphRoot::getCamera()
{
    return camera;
}


SceneGraphPtr
SceneGraphRoot::getLight(int id)
{

    return light[id];

}


float*
SceneGraphRoot::getLightVector()
{
    return light_vector;
}

int*
SceneGraphRoot::getLightSwitch()
{
    return light_switch;
}

int
SceneGraphRoot::getLightSysSwitch()
{
    return light_sysswitch;
}

void
SceneGraphRoot::OnLightSwitch(int id)
{
    light_switch[id] = 1;
}

void
SceneGraphRoot::OffLightSwitch(int id)
{
    light_switch[id] = 0;
}

void
SceneGraphRoot::OnLightSysSwitch()
{

    light_sysswitch = 1;

}

void
SceneGraphRoot::OffLightSysSwitch()
{

    light_sysswitch = 0;

}

void
SceneGraphRoot::task_array_init(int id, int task_num, int param, int inData_num, int outData_num)
{
    gtask_array = new GTaskArray;
    gtask_array->init(id, task_num, param, inData_num, outData_num);
}

void
SceneGraphRoot::create_task_array()
{
    gtask_array->create_task_array(this->tmanager);
}

void
SceneGraphRoot::task_array_finish()
{
    gtask_array->finish();
}

void
SceneGraphRoot::set_gtask_array(int id, void *property, int size, PostFunction post_func)
{
    gtask_array->next_task_array(id);

    gtask_array->game_task->set_inData(0, property, size);
    gtask_array->game_task->set_outData(0, property, size);

}

void
SceneGraphRoot::set_gtask_array(int id, void *property, void *pad, int size, PostFunction post_func)
{
    gtask_array->next_task_array(id);

    gtask_array->game_task->set_inData(0, property, size);
    gtask_array->game_task->set_inData(1, pad, sizeof(Pad));
    gtask_array->game_task->set_outData(0, property, size);

}

void
SceneGraphRoot::set_game_task(int id, void *property, int size)
{
    HTask *task = sgroot->tmanager->create_task(id);
    task->set_cpu(SPE_ANY);
    task->add_inData(property, size);
    task->add_outData(property, size);
    task->spawn();
}

void
SceneGraphRoot::set_game_task(int id, void *property, void *pad, int size)
{
    HTask *task = sgroot->tmanager->create_task(id);
    task->set_cpu(SPE_ANY);
    task->add_inData(property, size);
    task->add_inData(pad, sizeof(Pad));
    task->add_outData(property, size);
    task->spawn();
}


void
main_task_move(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h)
{
    int size = node->property_size;
    void *e  = node->propertyptr;
    int move = node->move_id;
    SceneGraphRoot *sgroottmp = (SceneGraphRoot*)sgroot_;
/*
  ObjPropertyPtr property = (ObjPropertyPtr)node->propertyptr;
  node->xyz[0] = property->x;
  node->xyz[1] = property->y;
*/
    sgroottmp->set_game_task(move, (void*)e, size);
}

void
pad_task_move(SceneGraphPtr node, void *sgroot_, int screen_w, int screen_h)
{
    int size = node->property_size;
    void *e  = node->propertyptr;
    int move = node->move_id;
    SceneGraphRoot *sgroottmp = (SceneGraphRoot*)sgroot_;
    void *pad = (void*)sgroottmp->getController();

/*
    ObjPropertyPtr property = (ObjPropertyPtr)node->propertyptr;
    property->root = node;
    node->xyz[0] = property->x;
    node->xyz[1] = property->y;
*/

    sgroottmp->set_game_task(move, (void*)e, pad, size);
}

void
SceneGraphRoot::set_move_task(SceneGraphPtr node, int move, void *property, int size)
{
    node->move = main_task_move;
    node->move_id = move;
    node->propertyptr = property;
    node->property_size = size;
}

void
SceneGraphRoot::set_pad_task(SceneGraphPtr node, int move, void *property, int size)
{
    node->move = pad_task_move;
    node->move_id = move;
    node->propertyptr = property;
    node->property_size = size;
}

/* end */