/* Car.c */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "libps2.h"
#include "ps2util.h"
#include "field.h"
#include "car.h"
#include "mytype.h"
#include "quotanion.h"
#include "game.h"
#include "stdbool.h"

#include "gSprite.h"
#include "getStrFromFile.h"

#define DEFAULT_SET (EFFECT_TEXTURE_USE | EFFECT_ALPHABLEND_UNUSE | EFFECT_SHADING_FLAT)
#define BUFSIZE 256

/* 初期位置と初期方向 */
static FVECTOR location  = {0, 0, 0, 1};
static FVECTOR direction = {0, 0, 1, 1};

extern FILE* main_fp;

static CarPtr
car_create(int car_id, char *filename, char *texname, float speed_accel,
	   float speed_max, float rot, float brake)
{
  CarPtr car;
  OBJECT *body;
  //TEXTURE* tex;
  void *free_addr;

  // 今はなにもしてない. 
  body = ps2util_obj_Create_fromXML(filename);
  ps2util_obj_Set_effect(body, DEFAULT_SET);

  // xmlファイルから読み込み
  call_createFromXMLfile(filename);

  body = (OBJECT*)malloc(sizeof(OBJECT));

  const char    *sStr = "surface name=\""; // この文字列の直後の文字列を取得する                                                                                             
  const char    *eStr = "\" size=";        // この文字列の直前までの文字列を抜き取る
  
  // xmlファイルから surface name と 数を取得する
  body->partNum = getStrFromFile( body->partName, filename, sStr, eStr );

  /*
  for( int i = 0; i < body->partNum; i ++ ){
    gSprite_DefSprite( 100, body->partName[i], 919, 666, 1);
  }
  */

  // 初期化 // createScenGraph関数の引数アリと無しの違いは？
  //body = droot->createSceneGraph("car");

  //  car = (CarPtr)malloc(sizeof(CarPtr));

  if (malloc_align16(&free_addr, &car, sizeof(Car)) == -1) {
    fprintf(main_fp, "car.c: malloc_align16 error\n");
    exit(EXIT_FAILURE);	
  }

  car->body           = body;

  car->next           = NULL;
  
  car->speed          = 0.0;
  car->speed_accel    = speed_accel;
  car->speed_max      = speed_max;
  car->brake          = brake;
  car->rotation_angle = rot;
  car->y_angle        = 0.0;
  car->free_addr      = free_addr;

  
  ps2_vu0_copy_vector(car->direction, direction);
  ps2_vu0_copy_vector(car->location, location);
  INIT_VECTOR(car->vertical,    0, -1, 0, 1);
  INIT_VECTOR(car->body->xyz,   0,  0, 0, 1);
  INIT_VECTOR(car->body->angle, 0,  0, 0, 1);

  ps2util_obj_Renew_transMatrix(car->body);
  ps2util_obj_Set_effect(car->body, DEFAULT_SET);

  /*
    if (*texname != 'n') {
    tex = read_png_file(texname);
    ps2util_tex_Set(tex);
    ps2util_obj_Set_texture(car->body, tex);
    }
  */

#ifdef DEBUG
  fprintf(main_fp, "malloc car addr = %x\n", (int)free_addr);
#endif

  return car;
}

// idは 1 か 2 か 3 で引数取ってこないと落ちるよ（登録されていない場合は落ちる）
// car/car.dat ファイル内に３つしか車のデータ無から
static CarPtr
car_new_readCSV(FILE *fp, int id)
{
  CarPtr newCar;
  int   car_id;
  float speed_accel, speed_max, brake, rot;
  char  buff[BUFSIZE], *bufp;
  char  carImg[BUFSIZE], texImg[BUFSIZE];

  bool  flag; // 探したい車が見つかったらture

  flag = false;

  while ((bufp = fgets(buff, BUFSIZE, fp)) != NULL) {

    bufp++;

    switch (buff[0]) {
    case 'n':
      if (flag == false) {
	
	// car_id = car/car.datファイルのnの後ろの数字一文字
	sscanf(bufp, " %d\n", &car_id); 
	if (id == car_id) {
	  flag = true;
	}
      }
      break;
    case 't':
      if (flag == true) {
	sscanf(bufp, " %s %s %f %f %f %f\n",
	       carImg, texImg, &speed_accel, &speed_max, &rot, &brake);
	newCar = car_create(id, carImg, texImg,
			    speed_accel, speed_max, rot, brake);
	return newCar;
      }
      break;
    default:
      break;
    }
  }

  /* ここまで辿り着いたら読み込み失敗 */
  fprintf(main_fp, "error - car_new_readCSV\n");
  exit(EXIT_FAILURE);
}

CarPtr car_init(int id)
{
  CarPtr newCar;
  FILE *fp;
  const char *filename = "car/car.dat";

  if (!(fp = fopen(filename, "r"))) {
    fprintf(main_fp, "error read file %s\n", filename);
    exit(EXIT_FAILURE);
  }

  newCar = car_new_readCSV(fp, id);

  fclose(fp);

  return newCar;
}

/*---------------------------
  carをY軸で回転させる
  flg: 回転方向 1:右, -1:左
  ---------------------------*/
void
car_swerve(CarPtr car, int flg)
{
  FMATRIX rot;
  FVECTOR v;
  
  car->y_angle += (float)flg*car->rotation_angle;
  car->y_angle += (car->y_angle < 0) ? 360.0 : 0;
  car->y_angle += (car->y_angle > 360.0) ? -360.0 : 0;

  // rotの初期化が下２行目でなんか２重に処理してる気がするけど別に良いのか
  ps2_vu0_unit_matrix(rot);
  ps2_vu0_rot_matrix_y( rot, rot, degree2radian((float)flg*car->rotation_angle) );

  ps2_vu0_copy_vector(v, car->direction);
  ps2_vu0_apply_matrix(car->direction, rot, v);
}

/*----------------------------
  carを加速・減速する
  (flg == 1) ? 加速 : 減速
  ---------------------------*/
void
car_accelerate(CarPtr car, int flg)
{

  //加速
  car->speed += (flg == 1) ? car->speed_accel : -car->brake;

  // speedが0以下にならんように
  car->speed = (car->speed < 0) ? 0 : car->speed;

  // speedがMax超えないように 
  car->speed = (car->speed > car->speed_max) ? car->speed_max : car->speed;
  
}


/*---------------------------------
  進行後のcarに対してコース面の内外判定を行い、
  : 現在のコース
  : 現在のコースの次のコース
  : 現在のコースの前のコース
  に存在すればTRUEを返す
  どのコースにも居ない(壁に衝突した)場合は
  FALSEを返す。
  ---------------------------------*/
static bool
car_field_check(Game *game ,CarPtr car)
{
  FieldPtr p;
  FieldPtr f = field_get_actual();

  // 現在のコースで衝突・内外判定
  p = f;
  if (col_detect(&p->colface, car->vertical, car->location) == true) {
    goto FIELD_CHECK_OK;
  }

  // 現在のコースの次に隣接するコースで衝突・内外判定
  p = f->next;
  if (col_detect(&p->colface, car->vertical, car->location) == true) {
    if (field_rap_increment(1)) {
      game->rap++;
    }
    goto FIELD_CHECK_OK;
  }

  // 現在のコースの前に隣接するコースで衝突・内外判定
  p = f->prev;
  if (col_detect(&p->colface, car->vertical, car->location) == true) {
    field_rap_increment(-1);
    goto FIELD_CHECK_OK;
  }

  // どのコース上にも居ない(壁に衝突)
  return false;

 FIELD_CHECK_OK:
  field_set_actual(p);
  return true;
}


/*------------------------------
  速度speedと向きdirectionから
  進行後の位置locationを求める。
  また、進行後の衝突判定も行う。
  ------------------------------*/
static void
car_move(Game *game,CarPtr car)
{
  FVECTOR mov, prev_location;

  printf("car->speed = %f ", car->speed);
  printf("\n");

  printf("car->body->xyz[x] = %f ", car->body->xyz[0]);
  printf("car->body->xyz[y] = %f ", car->body->xyz[1]);
  printf("car->body->xyz[z] = %f ", car->body->xyz[2]);
  printf("car->body->xyz[3] = %f ", car->body->xyz[3]);
  printf("\n");

  printf("car->direction[x] = %f ", car->direction[0]);
  printf("car->direction[y] = %f ", car->direction[1]);
  printf("car->direction[z] = %f ", car->direction[2]);
  printf("car->direction[3] = %f ", car->direction[3]);
  printf("\n");
  
  printf("car->location[x] = %f ", car->location[0]);
  printf("car->location[y] = %f ", car->location[1]);
  printf("car->location[z] = %f ", car->location[2]);
  printf("car->location[3] = %f ", car->location[3]);

  printf("\n");

  // movは車の移動量を計算して入れるのかな。たぶん
  ps2_vu0_scale_vector(mov, car->direction, car->speed);

  printf("mov[x] = %f ", mov[0]);
  printf("mov[y] = %f ", mov[1]);
  printf("mov[z] = %f ", mov[2]);
  printf("mov[3] = %f ", mov[3]);
  printf("\n");

  // 前フレームのcar->locationをprev_locationに保存
  ps2_vu0_copy_vector(prev_location, car->location);

  // 車の現在地 = 車の位置 + 移動量  って事だと思う多分
  ps2_vu0_add_vector(car->location, car->location, mov);


  //思う方向と逆に動いたので、マイナスして見ている。
  car->body->xyz[0] = -car->location[0];
  car->body->xyz[1] = -car->location[1];
  car->body->xyz[2] = -car->location[2];

  // いまちょっとコメントアウト
  
  // 壁にぶつかった時の処理
  if (car_field_check(game,car) == false) {
  // 壁にぶつかったら計算前の前フレームの座標にしてsppedは半分
  ps2_vu0_copy_vector(car->location, prev_location);
  car->speed = car->speed*0.5;
  }
  
}


/* car->body->transfer と car->y_angle はここでなんか関係ある */
static void
car_axis_rotation(CarPtr car)
{
  FMATRIX pose, // 最後にcar->transferにコピーしてる 
    yrot,
    yrotinv;

  ps2_vu0_unit_matrix(yrot);
  ps2_vu0_rot_matrix_y(yrot, yrot, -degree2radian(car->y_angle));
  
  ps2_vu0_unit_matrix(yrotinv);
  ps2_vu0_rot_matrix_y(yrotinv, yrotinv, degree2radian(car->y_angle));

  
  {
    FVECTOR yd = {0, -1, 0, 1};
    FVECTOR rotaxis;
    FVECTOR q;

    ps2_vu0_outer_product(rotaxis, car->vertical, yd);
    {
      float scale=1/NORM(rotaxis); // 単位ベクトル？
      rotaxis[0] *= scale;
      rotaxis[1] *= scale;
      rotaxis[2] *= scale;
    }

    quotanion(q, rotaxis, acos(ps2_vu0_inner_product(yd, car->vertical)/(NORM(yd)*NORM(car->vertical))));
    quotanion_rotmatrix(pose, q);
  }



  ps2_vu0_mul_matrix(pose, yrot, pose);
  ps2_vu0_mul_matrix(pose, pose, yrotinv);
  ps2_vu0_copy_matrix(car->body->transfer, pose);
  //ps2_vu0_copy_matrix( &(car->body->matrix), pose);
  ps2_vu0_rot_matrix_y(car->body->transfer, car->body->transfer, degree2radian(car->y_angle));
}



void car_update( Game *game, CarPtr car )
{
  // 移動 ここで壁との当たり判定もしとる
  car_move(game,car);

  // 摩擦による速度減少
  car->speed -= 0.005;
  car->speed = (car->speed < 0) ? 0 : car->speed;

  // 傾き
  car_axis_rotation(car);
}


void
car_destroy(CarPtr p)
{
  /*
  TEXTURE *t;

  if ((t = p->body->surfaces->texture)) {
    //ps2util_tex_Exclude(t);
    free_texture(t);
  }
  */
  
  ps2util_obj_Free(p->body);
  free(p->free_addr);

#ifdef DEBUG
  fprintf(main_fp, "free car addr = %x\n", (int)p->free_addr);
#endif
}

// 適当に自作 
void car_id_update(Game *game, CarPtr jiki)
{
  printf("car_id_update\n");
}


void setCarTransfer(CarPtr car)
{
  // memcpy(car->transfer, car->body->matrix, sizeof(FMATRIX) );
}
  
void setCarMatrix(CarPtr car)
{
  // memcpy(car->body->matrix, car->transfer, sizeof(FMATRIX) );
}