//SgoexCerium.ccより
#include "matrix_calc.h"
#include "TaskManager.h"
#include "SceneGraph.h"
#include "Application.h"
#include <iostream>

#include <stdio.h>
#include <stdlib.h>
#include "libps2.h"
#include "ps2util.h"
#include "gSprite.h"
#include <stdbool.h>

#include "camera.h"

#define DEFOBJ		100
#define SPRITE_COL	0x4f
#define SCREEN_HALF_W	320
#define SCREEN_HALF_H	240

static SpriteTable	sptable[DEFOBJ];
static ps2utilSprite	sp[DEFOBJ];	// 表示スプライト用スタック
static unsigned int	sp_counter;
static bool		flag_spriteInited = false;	// スプライトロードフラグ

//static TEXTURE *sprite_tex;

extern Viewer	*droot;

/*static*/SceneGraphPtr root;

static float	my_scale = 5;


static FVECTOR cameraMan_pos = { 0, 0, 0, 1 };
static FVECTOR cameraMan_angle = { 0, 0, 0, 1 };

FVECTOR def_cameraMan = { 0, 0, 0, 1 };

void set_cameraMan( void );
CameraPtr camera;// = droot->sgroot->getCamera();

// SgoexCerium.ccより
// カメラの更新はここでやれば良いんだろうね
void flip()
{
  

  droot->sgroot->flip(); // 前フレームで描画した SceneGraph は削除
                         // 前フレームに作られた SceneGraph は描画用に移行
                         // 現フレームで新しく SceneGraph がコピーされるので初期化
  
  camera = droot->sgroot->getCamera();//
  droot->sgroot->lightCalc();
  
  // rootって変数にカメラの位置設定してるんかな？
  // じゃあ camera ってなによ
  root		 = droot->createSceneGraph();
  
  //root->xyz[0] = 100;
  //root->xyz[1] = 200;
  
  set_cameraMan( );
  
  /*
  // カメラの設定は後回しにします意味わからん
  // magoroku_camera
  CAMERA mc =  get_MagorokuCamera();
  for( int i = 0; i < 4; i ++ ){
    root->xyz[i] = mc.xyz[i];
    root->angle[i] = mc.angle[i];

    //    camera->zd[i] = mc.zd[i];
    //camera->yd[i] = mc.yd[i];
  }
  */
  //droot->createStringFont(root,"A",80,200,100,0x00ff00ff);
  /*親の回転、座標から、子の回転、座標を算出*/
  get_matrix(root->matrix, root->angle, root->xyz, camera->matrix);
  
  /*法線用の行列。Cameraの行列を抜いている(Cameraのコンストラクタで、単位行列にしている)*/
  get_matrix(root->real_matrix, root->angle, root->xyz, camera->real_matrix);
  
  //出力して、次のものを出力するための準備
  droot->setSceneData(root);
}

// rootのポインターを返す
SceneGraphPtr getRootPtr( void )
{
  return root;
}

/*
  n==0 retrun x
  n==1 return y 
  n==2 return z
  n==3 ?
*/
float get_cameraPos( int n )
{
  float num;
  
  if( n >= 0 && n <= 3 )    num =  /*root->xyz[n]*/camera->xyz[n];
  else num = 0;

  return num;
}

// カメラの位置設定
void set_cameraMan( void )
{
  // window_posの略
  FVECTOR wp = {0,0, 1000, 1};

  for( int i = 0; i < 4; i ++ ){
    //    root->xyz[i] = -cameraMan[i] + wp[i];
    /*root*/camera->xyz[i] = cameraMan_pos[i];
    /*root*/camera->angle[i] = cameraMan_angle[i];
  }
  
}

// カメラの位置を引数のヤツに追従する
// ここではcameraManって変数に数値をコピーさせてるだけ
void set_cameraPos( FVECTOR newCameraMan_pos, FVECTOR newCameraMan_angle )
{
  for( int i = 0; i < 4; i ++ ){
    cameraMan_pos[i] = newCameraMan_pos[i];
    cameraMan_angle[i] = newCameraMan_angle[i];
  }
}

void
gSprite_DefSprite(int number, const char *name, float w, float h, int color)//, OBJECT *obj)
{
  SpriteTable	*m = &sptable[number];
  m->w		   = w;
  m->h		   = h;
  m->color	   = (color & 32);
  m->mx		   = w / 2;	//画像の中心を座標としたいらしい。
  m->my		   = h / 2;
  m->tex_w	   = power_of_two(m->w);
  m->tex_h	   = power_of_two(m->h);
  m->texture	   = (int *)name;
}

void gSprite_PutSprite(bool ScreenPos, int number, int x, int y, int z, int test)
{
  SpriteTable *m = &sptable[number];

  char *name = (char *) m->texture;
  if (!name) {
    printf("PutSprite %d unknown\n",number);
    return;
  }

  SceneGraphPtr object = droot->createSceneGraph(name);

  // ズレ直し?
  /*
    object->c_xyz[0] = m->mx;
    object->c_xyz[1] = m->my;

    object->xyz[0] -= object->c_xyz[0]*my_scale;
    object->xyz[1] -= object->c_xyz[1]*my_scale;
    object->xyz[2] -= object->c_xyz[2];

    object->xyz[0] += x+m->mx;
    object->xyz[1] += y+m->my;

    // object->xyz[2] += zorder * 0.01;
    */

  object->xyz[0] = x;
  object->xyz[1] = y;
  object->xyz[2] = z;

  // 画面からの座標で表示したい場合
  if( ScreenPos == true ){
    for( int i = 0; i  < 3; i ++ ){
      object->xyz[i] -= get_cameraPos(i);
    }
  }
  root->addChild(object);
  //droot->createStringFont(root,"As",80,640,480,0x00ff00ff);
  float scale[] = {my_scale,my_scale,1};
  
  if(test == 1){
    SceneGraphPtr wheel1 = droot->createSceneGraph("Cylinder.006");
    for( int i = 0; i < 3; i ++ ){
      wheel1->xyz[i] = object->xyz[i];
    }
    root->addChild(wheel1);
    //object->addChild(wheel1);

    /*親の回転、座標から、子の回転、座標を算出*/
    get_matrix_scale(wheel1->matrix, wheel1->angle, wheel1->xyz, scale, root->matrix);

    /*法線用の行列。Cameraの行列を抜いている(Cameraのコンストラクタで、単位行列にしている)*/
    get_matrix(wheel1->real_matrix, wheel1->angle, wheel1->xyz, root->real_matrix);
  }


  //  float scale[] = {my_scale,my_scale,1};
  //float c_xyz[] = {0,0,0};

  //float c_xyz[] = {0,0,0};
  //scale_matrix(object->matrix, scale, c_xyz);

  /*親の回転、座標から、子の回転、座標を算出*/
  get_matrix_scale(object->matrix, object->angle, object->xyz, scale, root->matrix);

  /*法線用の行列。Cameraの行列を抜いている(Cameraのコンストラクタで、単位行列にしている)*/
  get_matrix(object->real_matrix, object->angle, object->xyz, root->real_matrix);

}

void gSprite_PutObject( int number, OBJECT *obj )
{
  /*
  SpriteTable *m = &sptable[number];

  char *name = (char *) m->texture;
  if (!name) {
    printf("PutSprite %d unknown\n",100);
    return;
  }
  */

  // どっちにするべき？
  //SceneGraphPtr sgObj = droot->createSceneGraph();
  SceneGraphPtr sgObj = droot->createSceneGraph(obj->partName[0]);

  for( int i = 0; i < 4; i ++ ){
    sgObj->xyz[i] = obj->xyz[i];

    for( int j = 0; j < 4; j ++ ){
      //      sgObj->matrix[(i*4)+j] = obj->transfer[i][j];
    }
    
    sgObj->stack_angle[i] = obj->angle[i];
  }

  // とりあえず画面の真ん中にしたい
  //sgObj->xyz[0] += 100;
  //sgObj->xyz[1] += 100;
  //sgObj->xyz[2] += 0;
  
  memcpy( sgObj->matrix, obj->transfer, sizeof(FMATRIX) );

  root->addChild(sgObj);

  float scale[] = {my_scale,my_scale,1};

  // パーツ生成 
  for( int i = 1; i < obj->partNum; i ++ ){
    SceneGraphPtr part = droot->createSceneGraph(obj->partName[i]);
      for(int t=0; t < 3; t++){
	part->xyz[t] = sgObj->xyz[t];
	part->angle[t] = sgObj->angle[t];
      }
    printf("obj_name:%s\n",obj->partName[i]);
    // addChildの意味？
    
    /*親の回転、座標から、子の回転、座標を算出*/
    get_matrix_scale(part->matrix, part->angle, part->xyz, scale, root->matrix);

    /*法線用の行列。Cameraの行列を抜いている(Cameraのコンストラクタで、単位行列にしている)*/
    get_matrix(part->real_matrix, part->angle, part->xyz, root->real_matrix);
    //root->addChild(part);
    sgObj->addChild(part);
  }


  // 親の回転、座標から、子の回転、座標を算出
  get_matrix_scale(sgObj->matrix, sgObj->angle, sgObj->xyz, scale, root->matrix);

  // 法線用の行列。Cameraの行列を抜いている(Cameraのコンストラクタで、単位行列にしている)
  get_matrix(sgObj->real_matrix, sgObj->angle, sgObj->xyz, root->real_matrix);
  //
 
}


void
gSprite_PutSpriteEx(bool ScreenPos,int number, int x, int y, int z, float scalex, float scaley, float angle)
{
  if (1) {
    gSprite_PutSprite(ScreenPos,number, x, y, z, 0);
    return;
  }

  SpriteTable *m = &sptable[number];
  char *name = (char *) m->texture;
  if (!name) {
    printf("PutSpriteEx %d unknown\n",number);
    return;
  }
  SceneGraphPtr object = droot->createSceneGraph(name);
  root->addChild(object);

  float scale[] = {my_scale*scalex*4,my_scale*scaley*4,1};

  /*親の回転、座標から、子の回転、座標を算出*/
  object->xyz[0] = x - object->c_xyz[0] * my_scale - m->w*my_scale;
  object->xyz[1] = y - object->c_xyz[1] * my_scale - m->h*my_scale;
  object->xyz[2] = number * 0.01;
  //object->c_xyz[0] = m->mx;
  //object->c_xyz[1] = m->my;
  object->angle[3] = angle * (3.1415926*2/4096);
  get_matrix(object->matrix, object->angle, object->xyz, root->matrix);
  float c_xyz[] = {0,0,0};
  scale_matrix(object->matrix, scale, c_xyz);
  /*法線用の行列。Cameraの行列を抜いている(Cameraのコンストラクタで、単位行列にしている)*/
  // get_matrix(object->real_matrix, object->angle, object->xyz, root->real_matrix);
}


  //ここいらはどうやら、スプライトを取得する部分。
  /*
    static void
    gSprite_Init_Png(const char *texname)
    {
    if (!(sprite_tex = read_png_file(texname))) {
    fprintf(stderr,
    "cannot creat texture - init_png().\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
    }
  
  
    if (ps2util_tex_Set(sprite_tex) == -1) {
    fprintf(stderr,
    "cannot set texture - init_png().\n");
    fflush(stderr);
    exit(EXIT_FAILURE);
    }

  
    }
  */

  /*
    static void
    gSprite_Create()
    {
    int i;

    for (i=0; i<DEFOBJ; i++) {
    if (ps2util_sprite_Create(&sp[i], sprite_tex) == -1) {
    fprintf(stderr,
    "cannot create sprite No.%d - create_sprite()\n", i);
    fflush(stderr);
    exit(EXIT_FAILURE);
    }
    ps2util_sprite_Unuse_alpha(&sp[i]);
    }
 
    flag_spriteInited = false;

    }
  */
  

void
gSprite_Init()
{
  droot->createFromXMLfile("xml_file/title.xml");

  droot->createFromXMLfile("xml_file/select.xml");
  droot->createFromXMLfile("xml_file/sp_meter.xml");
  droot->createFromXMLfile("xml_file/r_arrow.xml");
  droot->createFromXMLfile("xml_file/l_arrow.xml");
  droot->createFromXMLfile("xml_file/km.xml");
  droot->createFromXMLfile("xml_file/rap.xml");
  droot->createFromXMLfile("xml_file/1p.xml");
  droot->createFromXMLfile("xml_file/2p.xml");
  droot->createFromXMLfile("xml_file/select_car_1.xml");
  droot->createFromXMLfile("xml_file/select_car_2.xml");
  droot->createFromXMLfile("xml_file/select_car_3.xml");
  droot->createFromXMLfile("xml_file/select_corse_1.xml");
  droot->createFromXMLfile("xml_file/select_corse_2.xml");

  ps2util_graphic_Init();
  //gSprite_Init_Png("img/main2.png");
  //gSprite_Create(); // スプライト生成

  // testCoeler
  int tc = 1;

  gSprite_DefSprite( TITLE, "title", 512, 666, tc); // タイトル
  gSprite_DefSprite( SELECT, "select", 33,  33, tc); // 選択マーク
  gSprite_DefSprite( SP_METER, "sp_meter", 106,  34, tc); // スピードメーター
  gSprite_DefSprite( R_ARROW,  "r_arrow", 18,  20, tc); // 右矢印
  gSprite_DefSprite( L_ARROW,  "l_arrow", 17,  20, tc); // 左矢印
  gSprite_DefSprite( KM,  "km",  55,  26, tc); // km/h
  gSprite_DefSprite( RAP,  "rap", 39,  26, tc); // Rap
  gSprite_DefSprite( PLAYER_1, "1P", 53, 46, tc); // 1p
  gSprite_DefSprite( PLAYER_2, "2P", 53,  46, tc); // 2P 
  gSprite_DefSprite( SELECT_CAR_1, "select_car_1", 165,  140, tc); // 選択画面の車 
  gSprite_DefSprite( SELECT_CAR_2, "select_car_2", 165,  140, tc);
  gSprite_DefSprite( SELECT_CAR_3,  "select_car_3", 165,  140, tc);
  gSprite_DefSprite( SELECT_CORSE_1, "select_corse_1", 148, 114, tc); // 選択画面のコース 
  // 本当は (82, 272,...)です。二つ目のコース絵がないので 
  gSprite_DefSprite( SELECT_CORSE_2, "select_corse_2", 148, 114 ,tc);

}

static void
gSprite_Clear()
{
  int i;

  for (i=0; i<DEFOBJ; i++) {
    sprite_disappear(&sp[i].attribute);
  }
  sp_counter = 0;
}



void
gSprite_Draw_Reset()
{
  if (flag_spriteInited == true) {
    gSprite_Clear();
  }
}



void
gSprite_PutSprite_Pause()
{
}

// xmlファイルから読み込み
void call_createFromXMLfile( char *filename )
{
  droot->createFromXMLfile(filename);
}


// createScenGraphe()する
/*
OBJECT* call_createSceneGraph( char *filename )
{
  OBJECT *obj;
  obj = droot->createSceneGraph(filename);

  return obj;
}


// createScenGraphe()する                                                                                                                                                    
OBJECT* call_createSceneGraph( void )
{
  OBJECT *obj;
  obj = droot->createSceneGraph();

  return obj;
}
*/

#include "controler.h"
// controler.cより
extern SGO_PAD pad;

// テスト用関数
void cameraControl( void )
{
  FVECTOR xyz;
  FVECTOR angle;

  for( int i = 0; i < 4; i ++ ){
    xyz[i] = cameraMan_pos[i];
    angle[i] = cameraMan_angle[i];
  }

  float speed = 1.0f;

  bool esc = false;
  
  if( pad.se > 0 ){                               
    esc = true;
    printf("press selectButton\n");
  }

  if( pad.l1 > 0 ){                               
    speed = 10.0f;
    printf("press W_key\n");
  }                                                                 
                                                                  
  if( pad.left > 0 ){
    if ( esc == true ) angle[0] -= speed;                   
    else               xyz[0]   -= speed;                   
  }                                                                 
  if( pad.right > 0 ){                 
    if ( esc == true ) angle[0] += speed;                   
    else               xyz[0]   += speed;                   
  }                                                                 
                                                                  
  if( pad.up > 0 ){                 
    if ( esc == true ) angle[1] -= speed;                   
    else               xyz[1]   -= speed;                   
  }                                                                 
  if( pad.down > 0  ){                 
    if ( esc == true ) angle[1] += speed;                   
    else               xyz[1]   += speed;                   
  }                                                                 
                                                                  
  if( pad.circle > 0 ){                               
    if ( esc == true ) angle[2] += speed;                   
    else               xyz[2]   += speed;                               
  }                                                                 
  if( pad.cross > 0 ){                                
    if ( esc == true ) angle[2] -= speed;                   
    else               xyz[2]   -= speed;                               
  }                                                                 
                                                                  
  for( int i = 0; i < 3; i ++ ){                                    
    if (angle[i] > 360.0f || angle[i] < -360.0f ) { 
      angle[i] = 0.0f;                                      
    }                                                               
  }

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

    cameraMan_pos[i] = xyz[i];
    cameraMan_angle[i] = angle[i];
  }

  for( int i = 0; i < 4; i ++ ){
    printf("cameraPos[%d] = %f ", i, cameraMan_pos[i] );
  }
  printf("\n");
  
  for( int i = 0; i < 4; i ++ ){
    printf("cameraAngle[%d] = %f ", i, cameraMan_angle[i] );
  }
  printf("\n");
  
  
}