/*
 * $Id$
 */
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <arpa/inet.h>
#include <libps2.h>
#include <ps2util.h>

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

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;
    int len = PKT_HEADER_SIZE;

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

    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)
{
    linda_seq[game.play_id] = psx_in(game.play_id);
    sche_func = func;
    send_packet(game.play_id, mode, game.car_id, game.course_id, NULL);
}

static Bool
linda_sche_wait0(int p)
{
    int i, flg=0;
    int id = game.play_id;
    int connect = 1; // 接続済みユーザ数

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

    if (p == 1 && connect == CLIENT_MAX) {
	linda_set_schedule(linda_sche_wait_ready0, MODE_WAIT_READY);
	return TRUE;
    } else {
	return FALSE;
    }

}

static Bool
linda_sche_wait1(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_wait_ready0(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }

}

static Bool
linda_sche_wait_ready1(int p)
{
    int mode;

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

    if (mode == MODE_OPENING) {
	linda_set_schedule(linda_sche_opening1, MODE_OPENING);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_opening0(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_opening1(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool 
linda_sche_select_car0(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_select_car1(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_select_course0(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_select_course1(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}


static Bool
linda_sche_ready0(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_ready1(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_main_init(int p)
{
    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);
	else
	    linda_set_schedule(linda_sche_main_ready1, MODE_MAIN_READY);
	return TRUE;
    } else {
	return FALSE;
    }
}


static Bool
linda_sche_main_ready0(int p)
{
    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);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_main_ready1(int p)
{
    int mode;

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

    if (mode == MODE_MAIN) {
	linda_set_schedule(linda_sche_main, MODE_MAIN);
	return TRUE;
    } else {
	return FALSE;
    }
}

static Bool
linda_sche_main(int p)
{
    int i, flg, mode;
    int id = game.play_id;
    int connect = 1;
    FMATRIX po;
    CarPtr mycar = game.jiki;
    CarPtr enemy = NULL;
    get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, (char*)po);

    for (i=1; i<=CLIENT_MAX; i++) {
	ps2_vu0_unit_matrix(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];
	    get_packet(i, NULL, &mode, NULL, NULL, (char*)po);	    
	    if (mode == MODE_MAIN) {
		ps2_vu0_copy_matrix(enemy->body->transfer, po);
		enemy->body->transfer[3][0] -= game.jiki->location[0];
		enemy->body->transfer[3][1] -= game.jiki->location[1];
		enemy->body->transfer[3][2] -= game.jiki->location[2];
		enemy->body->transfer[3][3] = 1;
	    }
	}
    }
	return FALSE;
}

static Bool
linda_sche_main0(int p)
{
    FMATRIX data;

    ps2_vu0_unit_matrix(data);
    ps2_vu0_copy_matrix(data, game.jiki->body->transfer);
    ps2_vu0_copy_vector(data[3], game.jiki->location);
	
    linda_seq[game.play_id] = psx_in(game.play_id);
    send_packet(game.play_id, MODE_MAIN, 0, 0, (char*)data);

   return FALSE;
}

static Bool
linda_sche_main1(int p)
{
    int mode;
    FMATRIX po;
    CarPtr enemy = linda_carlist[PLAYER_1P];
    ps2_vu0_unit_matrix(po);
    
    get_packet(PLAYER_1P, NULL, &mode, NULL, NULL, (char*)po);

    if (mode == MODE_MAIN) {
	ps2_vu0_copy_matrix(enemy->body->transfer, po);
	enemy->body->transfer[3][0] -= game.jiki->location[0];
	enemy->body->transfer[3][1] -= game.jiki->location[1];
	enemy->body->transfer[3][2] -= game.jiki->location[2];
	enemy->body->transfer[3][3] = 1;
    }    

    return FALSE;
}

Bool
linda_update(int p)
{
    return sche_func(p);
}

#if 0
static void
linda_enemy_update(int id, CarPtr enemy, CarInfoPtr tpl)
{
    int i,j;

    if (id == 1 || tpl->state == GAME_GOAL) {
	common_state = tpl->state;
	game.course_id = tpl->course_id;
    }

    if (enemy && (common_state == GAME_MAIN || common_state == GAME_GOAL)) {
	for (i=0; i<4; i++) {
	    for (j=0; j<4; j++) {
		enemy->body->transfer[i][j] = tpl->position[i][j];
	    }
	}

	/* 敵は相対位置で */
	enemy->body->transfer[3][0] -= game.jiki->location[0];
	enemy->body->transfer[3][1] -= game.jiki->location[1];
	enemy->body->transfer[3][2] -= game.jiki->location[2];
	enemy->body->transfer[3][3] = 1;
    }
}

/*
 * 位置が同じなら1を返す。
 * もっと良い比較方法があるんでは...
 */
static int
linda_jiki_compareLocation()
{
    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()
{
    int i;

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

}



int
linda_init()
{
    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;
	send_packet(game.play_id, MODE_WAIT, 1, 1, NULL);
    } else {
	sche_func = &linda_sche_wait1;
	send_packet(game.play_id, MODE_WAIT, 1, 1, NULL);
    }
    return my_id;
}