#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include <arpa/inet.h>
#include "libps2.h"
#include "ps2util.h"

#include "car.h"
#include "game.h"
#include "Linda/lindaapi.h"
#include "linda.h"

extern void psx_free(unsigned char *);
extern void carNode_append(CarPtr);
extern FILE *main_fp;
static int my_id;
static CarPtr linda_carlist[CLIENT_MAX+1];
static int linda_seq[CLIENT_MAX+1];

static bool (*sche_func)(int);

static void
set_header(unsigned int data, char *pkt, int offset)
{
    int *ipkt;
    int ndata = htonl(data);

    ipkt = (int*)pkt;
    ipkt[offset/4] = ndata;
}

static unsigned int
get_header(char *pkt, int offset)
{
    int *ipkt;
    int data;
    unsigned int header;

    ipkt   = (int *)pkt;
    data   = ipkt[offset/4];
    header = (unsigned int)ntohl(data);

    return header;
}

static char*
make_packet(unsigned int mode,     unsigned int carid,
	    unsigned int courseid, char *data)
{
    char *packet;
    unsigned int len = 0;

    if (data) len += sizeof(FMATRIX);

    packet = (char*)malloc(PKT_HEADER_SIZE+len);

    set_header(mode,     packet, PKT_MODE_OFFSET);
    set_header(carid,    packet, PKT_CARID_OFFSET);
    set_header(courseid, packet, PKT_COURSEID_OFFSET);

    if (data)
      	memcpy(packet+PKT_DATA_OFFSET, data, len);

    return packet;
}


static void
send_packet(unsigned int dest, unsigned int mode, unsigned int car,
	    unsigned int course,  char *data)
{
  char *pkt;
  char *reply;
  int len = PKT_HEADER_SIZE;

  if (data) len += sizeof(FMATRIX);

  reply = psx_reply(linda_seq[dest]);
    
  if (linda_seq[dest] == 0) {
    pkt = make_packet(mode, car, course, data);
    psx_out(dest, pkt, len);
    free(pkt);
  } else if (reply = psx_reply(linda_seq[dest])) {
    pkt = make_packet(mode, car, course, data);
    psx_out(dest, pkt, len);
    free(pkt);
    psx_free(reply);
  }
    
  pkt = make_packet(mode, car, course, data);
  psx_out(dest, pkt, len);
  free(pkt);
}

static void
get_packet(int id, int *flg, int *mode, int *car, int *course, char *data)
{
  char *reply = NULL;
  char *pkt = NULL;

  reply = psx_reply(linda_seq[id]);
  if (reply) {
    pkt    = reply+LINDA_HEADER_SIZE;

    if (flg)    *flg    = 1;
    if (mode)   *mode   = get_header(pkt, PKT_MODE_OFFSET);
    if (car)    *car    = get_header(pkt, PKT_CARID_OFFSET);
    if (course) *course = get_header(pkt, PKT_COURSEID_OFFSET);
    if (data)   memcpy(data, pkt+PKT_DATA_OFFSET, sizeof(FMATRIX));
    psx_free(reply);
    linda_seq[id] = psx_rd(id);
  }    
}

static void
linda_set_schedule(void *func, int mode, Game *game)
{
  int id = game->play_id;

  linda_seq[id] = psx_in(id);
  sche_func = func;
  send_packet(id, mode, game->car_id, game->course_id, NULL);
}

static bool linda_sche_wait0(int p, Game *game)
{
  int i, flg=0;
  int mode = 0;
  int id = game->play_id;
  int connect = 1; // 接続済みユーザ数

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue;
    get_packet(i, &flg, &mode, NULL, NULL, NULL);
    if (flg == 1 && mode == MODE_WAIT) {
      connect++;
      flg = 0;
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    linda_set_schedule(linda_sche_wait_ready0, MODE_WAIT_READY,game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_wait1(int p, Game *game)
{
  int mode;

  get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, NULL);

  if (mode == MODE_WAIT_READY) {
    linda_set_schedule(linda_sche_wait_ready1, MODE_WAIT_READY, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_wait_ready0(int p, Game *game)
{
  int i, flg, mode;
  int id = game->play_id;
  int connect = 1;

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue;
    get_packet(i, &flg, &mode, NULL, NULL, NULL);
    if (i == id) {
      flg = 0;
      continue;
    }
    if (flg == 1 && mode == MODE_WAIT_READY) {
      connect++;
      flg = 0;
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    linda_set_schedule(linda_sche_opening0, MODE_OPENING,game);
    return true;
  } else {
    return false;
  }

}

static bool
linda_sche_wait_ready1(int p, Game *game)
{
  int mode;

  get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, NULL);

  if (mode == MODE_OPENING) {
    linda_set_schedule(linda_sche_opening1, MODE_OPENING,game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_opening0(int p, Game *game)
{
  int i, flg, mode;
  int id = game->play_id;
  int connect = 1;

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue;
    get_packet(i, &flg, &mode, NULL, NULL, NULL);
    if (flg == 1 && mode == MODE_OPENING) {
      connect++;
      flg = 0;
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    linda_set_schedule(linda_sche_select_car0, MODE_SELECT_CAR, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_opening1(int p, Game *game)
{
  int mode;

  get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, NULL);

  if (mode == MODE_SELECT_CAR) {
    linda_set_schedule(linda_sche_select_car1, MODE_SELECT_CAR, game);
    return true;
  } else {
    return false;
  }
}

static bool 
linda_sche_select_car0(int p, Game *game)
{
  int i, flg, mode;
  int id = game->play_id;
  int connect = 1;

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue;
    get_packet(i, &flg, &mode, NULL, NULL, NULL);
    if (flg == 1 && mode == MODE_SELECT_CAR) {
      connect++;
      flg = 0;
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    linda_set_schedule(linda_sche_select_course0, MODE_SELECT_COURSE, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_select_car1(int p, Game *game)
{
  int mode;

  get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, NULL);

  if (mode == MODE_SELECT_COURSE) {
    linda_set_schedule(linda_sche_select_course1, MODE_SELECT_COURSE, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_select_course0(int p, Game *game)
{
  int i;
  int flg, mode;
  int connect = 1;
  int id = game->play_id;
  static int course_id = 1;

  if (course_id != game->course_id) {
    course_id = game->course_id;
    linda_seq[id] = psx_in(id);
    send_packet(id, MODE_SELECT_COURSE, 0, course_id, NULL);
    return false;
  }

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue; 
    get_packet(i, &flg, &mode, NULL, NULL, NULL);
    if (flg == 1 && mode == MODE_SELECT_COURSE) {
      connect++;
      flg = 0;
    }

  }

  if (p == 1 && connect == CLIENT_MAX) {
    linda_set_schedule(linda_sche_ready0, MODE_READY, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_select_course1(int p, Game *game)
{
  int mode, course_id;

  get_packet(PLAYER_1P, NULL, &mode, NULL, &course_id, NULL);

  if (mode == MODE_SELECT_COURSE) {
    game->course_id = course_id;
    return false;
  } else if (mode == MODE_READY) {
    linda_set_schedule(linda_sche_ready1, MODE_READY, game);
    return true;
  } else {
    return false;
  }
}


static bool
linda_sche_ready0(int p, Game *game)
{
  int i, flg, mode;
  int id = game->play_id;
  int connect = 1;

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue;
    get_packet(i, &flg, &mode, NULL, NULL, NULL);
    if (flg == 1 && mode == MODE_READY) {
      connect++;
      flg = 0;
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    linda_set_schedule(linda_sche_main_init, MODE_MAIN_INIT, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_ready1(int p, Game *game)
{
  int mode;

  get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, NULL);

  if (mode == MODE_MAIN_INIT) {
    linda_set_schedule(linda_sche_main_init, MODE_MAIN_INIT, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_main_init(int p, Game *game)
{
  int i, flg, mode, car_id;
  int id = game->play_id;
  int connect = 1;
  CarPtr car = NULL;

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue;
    get_packet(i, &flg, &mode, &car_id, NULL, NULL);

    if (flg == 1 && mode == MODE_MAIN_INIT) {
      if (linda_carlist[i] == NULL) {
	car = car_init(car_id);
	carNode_append(car);
	linda_carlist[i] = car;
      }
      connect++;
      flg = 0;
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    if (id == PLAYER_1P)
      linda_set_schedule(linda_sche_main_ready0, MODE_MAIN_READY, game);
    else
      linda_set_schedule(linda_sche_main_ready1, MODE_MAIN_READY, game);
    return true;
  } else {
    return false;
  }
}


static bool
linda_sche_main_ready0(int p, Game *game)
{
  int i, flg, mode;
  int id = game->play_id;
  int connect = 1;

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == id) continue;
    get_packet(i, &flg, &mode, NULL, NULL, NULL);
    if (flg == 1 && mode == MODE_MAIN_READY) {
      connect++;
      flg = 0;
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    linda_set_schedule(linda_sche_main, MODE_MAIN, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_main_ready1(int p, Game *game)
{
  int mode;

  get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, NULL);

  if (mode == MODE_MAIN) {
    linda_set_schedule(linda_sche_main, MODE_MAIN, game);
    return true;
  } else {
    return false;
  }
}

static bool
linda_sche_main(int p, Game *game)
{
  int i, flg, mode;
  int id = game->play_id;
  int connect = 1;
  FMATRIX po;
  CarPtr mycar = game->jiki;
  CarPtr enemy = NULL;

  for (i=1; i<=CLIENT_MAX; i++) {
    ps2_vu0_unit_matrix(po);
    get_packet(i, &flg, &mode, NULL, NULL, (char*)po);

    if (i == id) {
      ps2_vu0_copy_matrix(po, mycar->body->transfer);
      ps2_vu0_copy_vector(po[3], mycar->location);
	    
      linda_seq[i] = psx_in(i);
      send_packet(i, MODE_MAIN, 0, 0, (char*)po);
    } else {
      enemy = linda_carlist[i];
      if (flg == 1 && mode == MODE_MAIN) {
	ps2_vu0_copy_matrix(enemy->body->transfer, po);
	enemy->body->transfer[3][0] -= mycar->location[0];
	enemy->body->transfer[3][1] -= mycar->location[1];
	enemy->body->transfer[3][2] -= mycar->location[2];
	enemy->body->transfer[3][3] = 1;
      }
    }
  }

  if (p == 1 && connect == CLIENT_MAX) {
    if (id == PLAYER_1P)
      linda_set_schedule(linda_sche_main_ready0, MODE_MAIN_READY, game);
    else
      linda_set_schedule(linda_sche_main_ready1, MODE_MAIN_READY, game);
    return true;
  } else {
    return false;
  }


  return false;
}

bool
linda_update(int p, Game *game)
{
  return sche_func(p);
}

#if 0

/*
 * 位置が同じなら1を返す。
 * もっと良い比較方法があるんでは...
 */
static int
linda_jiki_compareLocation(Game *game)
{
  float x,y,z;
  double d;

  if (common_state == GAME_MAIN || common_state == GAME_GOAL) {
    x = jiki.position[3][0] - game.jiki->location[0];
    y = jiki.position[3][1] - game.jiki->location[1];
    z = jiki.position[3][2] - game.jiki->location[2];
    d = sqrt(x*x+y*y+z*z);

    if (d < 1.0) {
      return 1;
    } else {
      return 0;
    }
  } else {
    return 1;
  }
}


/*
 * 自機情報が更新されていなければ
 * 0を返す(psx_outしない)
 */
static int
linda_jiki_compare()
{
  if ((jiki.car_id == game.car_id) &&
      (jiki.course_id == game.course_id) &&
      (jiki.create_flg == ((game.jiki) ? 1:0)) &&
      (jiki.state == common_state) &&
      (linda_jiki_compareLocation())) {
    return 0;
  } else {
    return 1;
  }
}

static void
linda_jiki_update()
{
  int i,j;
    

  jiki.car_id     = game.car_id;
  jiki.course_id  = game.course_id;
  jiki.create_flg = (game.jiki) ? 1 : 0;
  jiki.state      = common_state;

  if (common_state == GAME_MAIN || common_state == GAME_GOAL) {
    for (i=0; i<3; i++) {
      for (j=0; j<4; j++) {
	jiki.position[i][j]
	  = game.jiki->body->transfer[i][j];
      }
    }
    jiki.position[3][0] = game.jiki->location[0];
    jiki.position[3][1] = game.jiki->location[1];
    jiki.position[3][2] = game.jiki->location[2];
    jiki.position[3][3] = 1;
  }
}

void
linda_update()
{
  int i;
  int barrier = 1; // バリア同期(?)用
  int connect = 1;
  int mode, id, flg;
  char* reply;
  char* pkt;
  CarPtr car = NULL;

  for (i=1; i<=CLIENT_MAX; i++) {
    reply = psx_reply(linda_seq[i]);
    if (reply) {
      pkt  = reply+LINDA_HEADER_SIZE;
      mode = get_header(pkt, PKT_MODE_OFFSET);
      id   = get_header(pkt, PKT_ID_OFFSET);
      flg  = get_header(pkt, PKT_FLG_OFFSET);
	    
      if (i != game.play_id) {
	connect++;
		
	car = linda_carlist[i];
	if (common_state == GAME_MAIN && !car && p->create_flg == 1) {
	  car = car_init(p->car_id);
	  carNode_append(car);
	  linda_carlist[i] = car;
	}
	linda_enemy_update(i, car, p);
	if (common_state == p->state) {
	  barrier++;
	}
      }
      psx_free(reply);
      linda_seq[i] = psx_rd(i);
    }
  }

  if (connect == CLIENT_MAX) {
    if (game->state == GAME_WAIT) {
      gFont_SetString("CONNECT OK!!", 170, 300);
      if (game->play_id == 1)
	gFont_SetString(" PUSH START ", 170, 400);
    }

    // ごちゃごちゃしてる...
    // なんか無駄な処理がありそうだ
    if (game->play_id == 1) {
      // 全員のフラグ成立
      if (common_state == GAME_GOAL) {
	if (game->state == GAME_FINISH) {
	  common_state = game->state;
	} else {
	  game->state = common_state;
	}
      } else if (barrier == CLIENT_MAX) {
	common_state = game->state;
      } else {
	game->state = common_state;
      }
    } else {
      if (game->state == GAME_GOAL) {
	if (common_state != GAME_FINISH) {
	  common_state = game->state;
	} else {
	  game->state = common_state;
	}
      } else {
	game->state = common_state;
      }
    }
  } else {
    if (game->state == GAME_WAIT) {
      gFont_SetString("WAITING...", 200, 100);
    }
    game->state = common_state;
  }

  if (linda_jiki_compare()) {
#ifdef DEBUG
    // どのタイミングでoutされてるか見る
    // 必要なときだけoutしないと重いですね当然だけど
    fprintf(main_fp, "psx_out() jiki\n");
#endif
    linda_jiki_update();
    // 無理矢理
    // 古いものを消すだけなんだけど
    // 正しいやり方が他に有るのかな？
    // 実行し続けてると最終的に激重になるので
    // ここら辺に問題が有るのだろうか。
    psx_free(psx_reply(psx_in(game->play_id)));
    linda_seq[my_id]=psx_in(game->play_id);
    psx_out(game->play_id, (char*)&jiki, sizeof(CarInfo));
  }
}
#endif

static int
get_id()
{
  unsigned char * reply;
  int seq;
  int id;

  //ユーザIDが格納されているTUPLE SpaceのIDへアクセス
  seq = psx_in(LINDA_ASK_ID);

  // IDが取得できるまでループ
  while((reply = psx_reply(seq)) == 0) psx_sync_n();

  id = atoi(reply+LINDA_HEADER_SIZE);
  psx_free(reply);
  return id;
}

void linda_env_init( void )
{
  int i;
  
  for (i=0; i<CLIENT_MAX+1; i++) {
    linda_carlist[i] = NULL;
  }
}



int
linda_init(Game *game)
{
  int i;

  start_linda(LINDA_HOST);
  my_id = get_id();

  for (i=1; i<=CLIENT_MAX; i++) {
    if (i == my_id) {
      send_packet(i, MODE_WAIT, i, 0, NULL);
    }
    linda_seq[i] = psx_rd(i);
  }
  psx_sync_n();

  if (my_id == PLAYER_1P) {
    sche_func = &linda_sche_wait0;
  } else {
    sche_func = &linda_sche_wait1;
  }
  send_packet(game->play_id, MODE_WAIT, 1, 1, NULL);

  return my_id;
}