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

// 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] = screen_w/2;
  //root->xyz[1] = screen_h/2;
  
  //  set_cameraMan( );
  //camera->xyz[0] = 100000;
  //camera->xyz[1] = 100000;
  //camera->xyz[2] = 100000;

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

  /*親の回転、座標から、子の回転、座標を算出*/
  get_matrix(root->matrix, root->angle, root->xyz, camera->matrix);
  
  /*法線用の行列。Cameraの行列を抜いている(Cameraのコンストラクタで、単位行列にしている)*/
  get_matrix(root->real_matrix, root->angle, root->xyz, camera->real_matrix);

  
  root->addChild(camera);

  //出力して、次のものを出力するための準備
  droot->setSceneData(root);
}

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



/*void
gSprite_DefSprite(int number, int tx, int ty, int tw, int th)
*/

/*
  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];
  else num = 0;

  return num;
}

// カメラの位置設定
void set_cameraMan( void )
{
  // window_posの略
  //FVECTOR wp = {310,150, -800, 1};
  FVECTOR wp = {310 ,400 , 500, 1};

  for( int i = 0; i < 4; i ++ ){
    root->xyz[i] = cameraMan_pos[i] + wp[i];
    root->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(int number, short x, short y, int zorder)
//void gSprite_PutSprite(int number, int x, int y, int z)
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;
  }
  
  //これをするためには、flip内の処理を先にしておかないいけない？
  SceneGraphPtr object = droot->createSceneGraph(name);

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

  // 適当に初期設定:miya //タイトルの位置を設定
  object->xyz[0] = 600;
  object->xyz[1] = 100;
  object->xyz[2] = -500;
    
  root->addChild(object);
  */


  // テスト初期設定:miya
  /*
    object->xyz[0] = 300;
    object->xyz[1] = 100;
    object->xyz[2] = -100;
  */

  /*


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

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


  //  ca->xyz[0] = 20;
  
  printf("camera_[x] = %f ",root->xyz[0]);
  printf("camera_[y] = %f ",root->xyz[1]);
  printf("camera_[z] = %f\n ",root->xyz[2]);


  //これをするためには、flip内の処理を先にしておかないいけない？
  CameraPtr camera = droot->sgroot->getCamera();

  camera->xyz[0] = camera->xyz[1] = 0;
  camera->xyz[2] = -1000;
  camera->xyz[3] = 1;

  printf( "obj_x： %f",   object->xyz[0]);
  printf( "　obj_y：%f",   object->xyz[1]);
  printf( "　obj_z：%f",   object->xyz[2]);
  printf( "　obj_[3]：%f\n",   object->xyz[3]);
  
  printf( "Camera_x： %f", camera->xyz[0]);
  printf( "　Camera_y：%f", camera->xyz[1]);
  printf( "　Camera_z：%f", camera->xyz[2]);
  printf( "　Camera_[3]：%f\n", camera->xyz[3]);
  




  /*親の回転、座標から、子の回転、座標を算出*/
  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_PutObjet(bool ScreenPos, OBJECT *obj)
void gSprite_PutObject( int number, OBJECT *obj )
{
  // どっちにするべき？
  //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->angle[i] = obj->angle[i];
  }

  memcpy( sgObj->matrix, obj->transfer, sizeof(FMATRIX) );

  root->addChild(sgObj);

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

  // パーツ生成 
  for( int i = 0; i < obj->partNum; i ++ ){
    
    SceneGraphPtr part = droot->createSceneGraph(obj->partName[i]);
       
    for( int i = 0; i < 3; i ++ ){
      part->xyz[i] = sgObj->xyz[i];
      part->angle[i] = sgObj->angle[i];
    }
    // addChildの意味？
    //root->addChild(part);
    sgObj->addChild(part);
    //root->addChild(part);
    //sgObj->addChild(part);

    // 親の回転、座標から、子の回転、座標を算出
    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);
  }


  // 親の回転、座標から、子の回転、座標を算出
  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);
}

  

void
gSprite_Init()
{
  //ココを一回だけ宣言する。
  droot->createFromXMLfile("xml_file/title.xml");

  
  //テスト
  //droot->createFromXMLfile("xml_file/kuruma2.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");
  
  
}