#include "Game.h"

#include <stdlib.h>
#include <math.h>
#include <stdlib.h>

#include <iostream>

#include "btBulletDynamicsCommon.h"

Game::Game() :
	local_player(0), collisionConfiguration(
			new btDefaultCollisionConfiguration()), dispatcher(
			new btCollisionDispatcher(collisionConfiguration.get())),
			overlappingPairCache(new btDbvtBroadphase), solver(
					new btSequentialImpulseConstraintSolver), dynamicsWorld(
					new btDiscreteDynamicsWorld(dispatcher.get(),
							overlappingPairCache.get(), solver.get(),
							collisionConfiguration.get())), levelMesh(0) {
	slaveUpdate.setInterval(1. / 60.);
	slaveUpdate.setExact(true);
}

Game::~Game() {
	tlDeleteTrimesh(levelMesh);
	tlDeleteTrimesh(shipMesh);
}

double rand2() {
	float u = (float) rand() / (float) RAND_MAX;
	return 2.0 * (u - 0.5);
}

void reset_team(Team *team) {
	team->points = 0;
}

void reset_player(player_t *player) {
	if (player->team) {
		player->x = player->team->x;
		player->y = player->team->y;
		player->z = player->team->z;
	} else {
		player->x = 0;
		player->y = 0;
		player->z = 0;
	}
	player->vx = 0.;
	player->vy = 0.;
	player->vz = 0.;
}

void reset_point(point_t *point) {
	point->status = 1;
	point->x = 1000 + 500.0 * rand2();
	point->y = 500.0 * rand2();
	point->z = 500.0 * rand2();
}

void setup_team(Team *team, uint8_t id) {
	team->id = id;
	team->color[0] = .5f + .5f * (id & (1 << 0));
	team->color[1] = .5f + .5f * (id & (1 << 1));
	team->color[2] = .5f + .5f * (id & (1 << 2));
	team->color[3] = 1.0f;

	team->x = 0.0;
	team->y = 0;
	team->z = 0;

	team->points = 0;
	team->wins = 0;
}

void setup_player(player_t *player) {
	player->id = 0;
	player->status = 0;
	player->team = NULL;
	player->x = 0.0;
	player->y = 0.0;
	player->z = 0.0;
	player->vx = 0.0;
	player->vy = 0.0;
	player->vz = 0.0;
}

void setup_point(point_t *point) {
	point->status = 0;
	point->x = 0.0;
	point->y = 0.0;
	point->z = 0.0;
}

void setup_bomb(bomb_t *bomb) {
	bomb->status = 0;
	bomb->x = 0.0;
	bomb->y = 0.0;
	bomb->z = 0.0;
}

void Game::loadLevelShape() {
	levelMesh = tlLoadTrimesh("data/level.obj", TL_FVF_XYZ | TL_FVF_UV
			| TL_FVF_NORMAL);

	btIndexedMesh im;
	im.m_numTriangles = levelMesh->face_count;
	im.m_triangleIndexBase = (unsigned char *) levelMesh->faces;
	im.m_triangleIndexStride = sizeof(unsigned short) * 3;

	im.m_numVertices = levelMesh->vertex_count;
	im.m_vertexBase = (unsigned char *) levelMesh->vertices;
	im.m_vertexStride = levelMesh->vertex_size;
	im.m_vertexType = PHY_FLOAT;

	levelVertexArray.reset(new btTriangleIndexVertexArray);
	levelVertexArray->addIndexedMesh(im, PHY_SHORT);

	btBvhTriangleMeshShape *_levelShape = new btBvhTriangleMeshShape(
			levelVertexArray.get(), true, true);
	btVector3 center;
	btScalar radius;
	_levelShape->getBoundingSphere(center, radius);
	std::cout << center.x() << " " << center.y() << " " << center.y() << ", "
			<< radius << std::endl;
	levelShape.reset(_levelShape);
}

void Game::loadShipShape() {
	shipMesh = tlLoadTrimesh("data/ship.obj", TL_FVF_XYZ | TL_FVF_UV
			| TL_FVF_NORMAL);
	/*
	 btIndexedMesh sim;
	 sim.m_numTriangles = shipMesh->face_count;
	 sim.m_triangleIndexBase = (unsigned char *) shipMesh->faces;
	 sim.m_triangleIndexStride = sizeof(unsigned short) * 3;

	 sim.m_numVertices = shipMesh->vertex_count;
	 sim.m_vertexBase = (unsigned char *) shipMesh->vertices;
	 sim.m_vertexStride = shipMesh->vertex_size;
	 sim.m_vertexType = PHY_FLOAT;

	 shipVertexArray.reset(new btTriangleIndexVertexArray);
	 shipVertexArray->addIndexedMesh(sim, PHY_SHORT);

	 btBvhTriangleMeshShape *_shipShape = new btBvhTriangleMeshShape(
	 shipVertexArray.get(), true, true);

	 btVector3 center;
	 btScalar radius;
	 _shipShape->getBoundingSphere(center, radius);
	 std::cout << center.x() << " " << center.y() << " " << center.y() << ", "
	 << radius << std::endl;

	 shipShape.reset(_shipShape);
	 */
	shipShape.reset(new btBoxShape(btVector3(btScalar(1.5), btScalar(1.5),
			btScalar(1.5))));
}

void Game::setupLevel() {
	dynamicsWorld->setGravity(btVector3(0, 0, 0));

	loadLevelShape();
	loadShipShape();

	levelState.reset(new btDefaultMotionState());
	btRigidBody::btRigidBodyConstructionInfo rbInfo(0, levelState.get(),
			levelShape.get());
	levelBody.reset(new btRigidBody(rbInfo));
	dynamicsWorld->addRigidBody(levelBody.get());
}

void Game::setup() {
	size_t i;

	setupLevel();

	teams.resize(2);
	for (i = 0; i < teams.size(); i++)
		setup_team(&teams[i], i);

	for (i = 0; i < GAME_BOMB_COUNT; i++)
		setup_bomb(&bomb[i]);

	for (i = 0; i < GAME_PLAYER_COUNT; i++)
		setup_player(&player[i]);

	for (i = 0; i < GAME_POINT_COUNT; i++)
		setup_point(&point[i]);

	max_player_id = 0;
	master = 0;
	updateTime = 0.0;
	local_player = 0;
}

void Game::reset() {
	size_t i;

	for (i = 0; i < teams.size(); i++)
		reset_team(&teams[i]);

	for (i = 0; i < GAME_BOMB_COUNT; i++)
		setup_bomb(&bomb[i]);

	for (i = 0; i < GAME_PLAYER_COUNT; i++)
		reset_player(&player[i]);

	for (i = 0; i < GAME_POINT_COUNT; i++)
		reset_point(&point[i]);
}

player_t *Game::getFreePlayer() {
	size_t i;
	for (i = 0; i < GAME_PLAYER_COUNT; i++) {
		if (player[i].status == 0)
			return &player[i];
	}
}

player_t *Game::spawn_player(Team *team) {
	player_t *player = getFreePlayer();
	player->team = team;
	player->status = 1;
	player->id = max_player_id++;
	reset_player(player);

	if (player->body.get() == 0) {
		std::cout << "Game::spawn_player: create body" << std::endl;
		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();
		startTransform.setOrigin(btVector3(player->x, player->y, player->z));
		btScalar mass(1.f);
		btVector3 localInertia(0, 0, 0);
		shipShape->calculateLocalInertia(mass, localInertia);
		player->state.reset(new btDefaultMotionState());
		player->state->setWorldTransform(startTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,
				player->state.get(), shipShape.get(), localInertia);
		player->body.reset(new btRigidBody(rbInfo));
		player->body->setCcdSweptSphereRadius(1.5);
		dynamicsWorld->addRigidBody(player->body.get());
	}
	return player;
}

player_t *Game::spawn_player_id(Team *team, uint16_t id) {
	player_t *player = getFreePlayer();
	player->team = team;
	player->id = id;
	if (max_player_id < id)
		max_player_id = id;
	player->status = 1;
	reset_player(player);

	if (player->body.get() == 0) {
		/// Create Dynamic Objects
		std::cout << "[Game] create body" << std::endl;
		btTransform startTransform;
		startTransform.setIdentity();
		startTransform.setOrigin(btVector3(player->x, player->y, player->z));
		btScalar mass(1.f);
		btVector3 localInertia(0, 0, 0);
		shipShape->calculateLocalInertia(mass, localInertia);
		player->state.reset(new btDefaultMotionState());
		player->state->setWorldTransform(startTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,
				player->state.get(), shipShape.get(), localInertia);
		player->body.reset(new btRigidBody(rbInfo));
		player->body->setCcdSweptSphereRadius(1.5);
		dynamicsWorld->addRigidBody(player->body.get());
	}
	return player;
}

void Game::update_players(double dt) {
	size_t i;
	for (i = 0; i < GAME_PLAYER_COUNT; i++) {
		player_t *p = &player[i];
		if (p->status == 0)
			return;
		if (master || true) {
			btTransform wt;
			p->state->getWorldTransform(wt);
			p->x = wt.getOrigin().x();
			p->y = wt.getOrigin().y();
			p->z = wt.getOrigin().z();
			btVector3 v = p->body->getLinearVelocity();
			p->vx = v.x();
			p->vy = v.z();
			p->vz = v.z();
		} else {
			std::cout << "update player " << p->id << ": " << p->x << " += "
					<< p->vx << " * " << dt << std::endl;

			p->x += p->vx * dt;
			p->y += p->vy * dt;
			p->z += p->vz * dt;
		}

		double distance2 = pow(p->x - p->team->x, 2)
				+ pow(p->y - p->team->y, 2) + pow(p->z - p->team->z, 2);
		if (distance2 < 10000) {
			p->team->points += p->points;
			p->points = 0;
		}

		//		if (p->x < (-1000 + 10)) {
		//			p->x = (-1000 + 10);
		//			p->vx *= -1;
		//		} else if (p->x > (5000 - 10)) {
		//			p->x = (5000 - 10);
		//			p->vx *= -1;
		//		}
		//
		//		if (p->y < (-3000 + 10)) {
		//			p->y = (-3000 + 10);
		//			p->vy *= -1;
		//		} else if (p->y > (3000 - 10)) {
		//			p->y = (3000 - 10);
		//			p->vy *= -1;
		//		}
		//
		//		if (p->z < (-3000 + 10)) {
		//			p->z = (-3000 + 10);
		//			p->vz *= -1;
		//		} else if (p->z > (3000 - 10)) {
		//			p->z = (3000 - 10);
		//			p->vz *= -1;
		//		}
	}
}

void Game::explode_bomb(bomb_t *bomb) {
	size_t i, j;
	for (i = 0; i < GAME_PLAYER_COUNT; i++) {
		player_t *p = &player[i];
		if (p->status == 0)
			continue;
		double distance2 = pow(p->x - bomb->x, 2) + pow(p->y - bomb->y, 2)
				+ pow(p->z - bomb->z, 2);
		if (distance2 < pow(150., 2.)) {
			ExplosionSignal(p->x, p->y, p->z);
			p->x = p->team->x + 100;
			p->y = p->team->y;
			p->z = p->team->z;
			p->vx = 0.;
			p->vy = 0.;
			p->vz = 0.;
			for (j = 0; j < p->points; j++) {
				point_t *point = spawn_point();
				point->x = p->x + 20.0 * rand2();
				point->y = p->x + 20.0 * rand2();
				point->z = p->x + 20.0 * rand2();
			}
			p->points = 0;
		}
	}
}

void Game::update_bombs(double dt) {
	size_t i;
	for (i = 0; i < GAME_BOMB_COUNT; i++) {
		bomb_t *b = &bomb[i];
		if (b->status == 0)
			continue;
		b->x += b->vx * dt;
		b->y += b->vy * dt;
		b->z += b->vz * dt;
		b->ttl -= dt;

		if (b->x < (-1000 + 10)) {
			b->x = (-1000 + 10);
			b->ttl = -0.1;
		} else if (b->x > (5000 - 10)) {
			b->x = (5000 - 10);
			b->ttl = -0.1;
		} else if (b->y < (-3000 + 10)) {
			b->y = (-3000 + 10);
			b->ttl = -0.1;
		} else if (b->y > (3000 - 10)) {
			b->y = (3000 - 10);
			b->ttl = -0.1;
		} else if (b->z < (-3000 + 10)) {
			b->z = (-3000 + 10);
			b->ttl = -0.1;
		} else if (b->z > (3000 - 10)) {
			b->z = (3000 - 10);
			b->ttl = -0.1;
		}

		if (b->ttl < 0) {
			if (b->status == 1) {
				ExplosionSignal(b->x, b->y, b->z);
				b->status = 2;
			} else if (b->ttl < -0.2) {
				if (master) {
					explode_bomb(b);
				}
				b->status = 0;
			}
		}
	}
}

void Game::update_point(point_t *point) {
	size_t i;
	for (i = 0; i < GAME_PLAYER_COUNT; i++) {
		player_t *P = &player[i];
		if (P->status == 0)
			continue;
		double distance2 = pow(P->x - point->x, 2) + pow(P->y - point->y, 2)
				+ pow(P->z - point->z, 2);
		if (distance2 > 1000.0)
			continue;
		point->status = 0;
		P->points += 1;
	}

}

void Game::update_points(double dt) {
	size_t i;
	for (i = 0; i < GAME_POINT_COUNT; i++) {
		point_t *p = &point[i];
		if (p->status == 0)
			continue;
		update_point(p);
	}
}

void Game::update(double time, double dt) {
	if (master || true) {
		int steps = dynamicsWorld->stepSimulation(dt, 10);
		if (steps > 0) {
			double timeStep = dt;
			Game::update_players(timeStep);
			Game::update_bombs(timeStep);
			Game::update_points(timeStep);
		}
	} else if (slaveUpdate.next(time)) {
		Game::update_players(slaveUpdate.getInterval());
		Game::update_bombs(slaveUpdate.getInterval());
		Game::update_points(slaveUpdate.getInterval());
	}

}

size_t Game::active_team_players(Team *team) {
	size_t i, count = 0;
	for (i = 0; i < GAME_PLAYER_COUNT; i++) {
		if (player[i].status == 0)
			continue;
		if (player[i].team == team)
			count++;
	}
	return count;
}

Team *Game::team_with_least_players() {
	size_t i, count = -1;
	Team *team = NULL;
	for (i = 0; i < teams.size(); i++) {
		size_t players = active_team_players(&teams[i]);
		if (players < count) {
			count = players;
			team = &teams[i];
		}
	}
	return team;
}

Team *Game::getTeam(uint16_t id) {
	size_t i;
	for (i = 0; i < teams.size(); i++) {
		if (teams[i].id == id) {
			return &teams[i];
		}
	}
	return NULL;
}

player_t *Game::getPlayer(uint16_t id) {
	size_t i;
	for (i = 0; i < GAME_PLAYER_COUNT; i++) {
		if (player[i].id == id) {
			return &player[i];
		}
	}
	return NULL;
}

bomb_t *Game::spawn_bomb() {
	size_t i;
	for (i = 0; i < GAME_BOMB_COUNT; i++) {
		if (bomb[i].status == 0) {
			bomb[i].status = 1;
			return &bomb[i];
		}
	}
	return NULL;
}

point_t *Game::spawn_point() {
	size_t i;
	for (i = 0; i < GAME_POINT_COUNT; i++) {
		if (point[i].status == 0) {
			point[i].status = 1;
			return &point[i];
		}
	}
	return NULL;
}

void Game::set_master(int master) {
	this->master = master;
}