/*
   base/physics.cc
   This file is part of the Osirion project and is distributed under
   the terms of the GNU General Public License version 2.
*/

#include "base/physics.h"
#include "base/collision.h"
#include "base/game.h"
#include "core/zone.h"
#include "math/functions.h"
#include "math/vector3f.h"
#include "model/vertexarray.h"
#include "sys/sys.h"

#ifdef HAVE_BULLET
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"
#endif

namespace game
{

/* ---- class ColliderShape ---------------------------------------- */

CollisionShape::Registry CollisionShape::shape_registry;

CollisionShape::CollisionShape(model::Model *model, const bool moving)
{
	shape_label.assign(model->name());
	shape_moving = moving;

#ifdef HAVE_BULLET
	btGImpactMeshShape *moving_shape = 0;
	btBvhTriangleMeshShape *static_shape = 0;

	shape_mesh = new btTriangleMesh();

	size_t count = 0;

	model::VertexArray *vertexarray = model::VertexArray::instance();

	// iterate model FragmentGroups
	for (model::Model::Groups::iterator git = model->groups().begin(); git != model->groups().end(); git++) {

		model::FragmentGroup *group = (*git);

		// FIXME ignore moving parts
		if (group->type() == model::FragmentGroup::None) {

			// for each fragment
			for (model::FragmentGroup::iterator fit = group->begin(); fit != group->end(); fit++) {	
				model::Fragment *fragment = (*fit);

				const size_t index = fragment->index();		
				const size_t triangle_count = (fragment->structural_size() + fragment->detail_size()) / 3; 
				
				// load vertices from the global VertexArray into the bullet shape
				for (size_t i = 0; i < triangle_count; i++) {
					float *f = vertexarray->vertex() + index + (i  * 9);

					btVector3 v0(f[0], f[1], f[2]);
					btVector3 v1(f[3], f[4], f[5]);
					btVector3 v2(f[6], f[7], f[8]);

					shape_mesh->addTriangle(v0, v1, v2);
				}

				count += triangle_count;
			}
		}
	}

	if (moving) {
		moving_shape = new btGImpactMeshShape(shape_mesh);
		moving_shape->updateBound();
		moving_shape->postUpdate();
		shape_bulletshape = moving_shape;

	} else {
		static_shape = new btBvhTriangleMeshShape(shape_mesh, true, false);
		shape_bulletshape = static_shape;
	}
	
	
	con_debug << "  collision data for " << model->name() << ": " << count << " tris " << std::endl;
#endif
}

CollisionShape::~CollisionShape()
{
	shape_label.clear();
#ifdef HAVE_BULLET
	delete shape_bulletshape;
#endif
}

void CollisionShape::clear()
{
	for (Registry::iterator it = shape_registry.begin(); it != shape_registry.end(); it++) {
		CollisionShape *shape = (*it);
		delete shape;
	}
	shape_registry.clear();
}

CollisionShape *CollisionShape::add(model::Model *model, const bool moving)
{
	CollisionShape *shape = new CollisionShape(model, moving);

	shape_registry.push_back(shape);
	return shape;
}

CollisionShape *CollisionShape::find(model::Model *model, const bool moving)
{
	for (Registry::iterator it = shape_registry.begin(); it != shape_registry.end(); it++) {
		CollisionShape *shape = (*it);

		if ((shape->moving() == moving) && (shape->label().compare(model->name()) == 0)) {
			return shape;
		}
	}
	return 0;
}

/* ---- class Physics ---------------------------------------------- */

#ifdef HAVE_BULLET

btDefaultCollisionConfiguration *Physics::configuration;
btCollisionDispatcher *Physics::dispatcher;
btSequentialImpulseConstraintSolver *Physics::solver;

void Physics::init()
{
	con_print << "^BInitializing bullet physics..." << std::endl;

	configuration = new btDefaultCollisionConfiguration();	
	dispatcher = new btCollisionDispatcher(configuration);
	solver = new btSequentialImpulseConstraintSolver;
	
}

void Physics::frame(const float seconds)
{
	for (core::Zone::Registry::iterator it = core::Zone::registry().begin(); it != core::Zone::registry().end(); it++) {
		PhysicsZone *bz = static_cast<PhysicsZone *>((*it).second);
		bz->dynamics_world()->stepSimulation(seconds);
	}
}

void Physics::shutdown()
{
	con_print << "^BShutting down bullet physics..." << std::endl;

	CollisionShape::clear();

	delete solver;
	delete dispatcher;
	delete configuration;
}

#else

void Physics::init()
{
}

void Physics::frame(const float seconds)
{
	Collision::frame(seconds);
}

void Physics::shutdown()
{
}

#endif

/* ---- class PhysicsZone ------------------------------------------ */

PhysicsZone::PhysicsZone(const std::string &label) : core::Zone(label)
{
#ifdef HAVE_BULLET
	btVector3 worldAabbMin(-10000,-10000,-10000);
	btVector3 worldAabbMax(10000,10000,10000);
	const int maxProxies = 1024;
	
	zone_cache = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
	zone_dynamics_world = new btDiscreteDynamicsWorld(Physics::dispatcher, zone_cache, Physics::solver,Physics::configuration);

	// disable gravity
	zone_dynamics_world->setGravity(btVector3(0.0f, 0.0f, 0.0f));
#endif
}

PhysicsZone::~PhysicsZone()
{
#ifdef HAVE_BULLET
	if (zone_cache) {
		delete zone_cache;
		zone_cache = 0;
	}

	if (zone_dynamics_world) {
		delete zone_dynamics_world;
		zone_dynamics_world = 0;
	}
#endif
}

/* ---- class PhysicsBody ------------------------------------------ */

PhysicsBody::PhysicsBody(core::Entity *entity)
{
	collider_entity = entity;
	collider_shape = 0;
	collider_mass = 0;
#ifdef HAVE_BULLET
	collider_body = 0;
#endif
}

PhysicsBody::~PhysicsBody() 
{
#ifdef HAVE_BULLET
	if (collider_body) {
		delete collider_body;
		collider_body = 0;
	}
#endif
}

void PhysicsBody::init_physics(const float mass)
{
	collider_mass = mass;

#ifdef HAVE_BULLET
	bool moving = (mass > 0.0f);

	if (!entity()->model())
		return;

	if (!collider_shape) {
		collider_shape = CollisionShape::find(entity()->model(), moving);
		if (!collider_shape) {
			collider_shape = CollisionShape::add(entity()->model(), moving);

		}
	}

	if (!collider_body) {
		btVector3 inertia(0,0,0);

		btTransform t;
		t.setIdentity();
		t.setOrigin(to_btVector3(entity()->location()));
		t.setBasis(to_btMatrix3x3(entity()->axis()));
		
		
		if (moving) {
			collider_shape->bullet_shape()->calculateLocalInertia(mass,inertia);

			btMotionState *motionstate = new btDefaultMotionState(t);
			btRigidBody::btRigidBodyConstructionInfo body_info(mass, motionstate, collider_shape->bullet_shape(), inertia);
    			collider_body = new btRigidBody(body_info);
			collider_body->setUserPointer((void *)entity());
			collider_body->setLinearFactor(btVector3(1,1,1));
			collider_body->setCcdMotionThreshold(0.0001f);
		} else {
			collider_body = new btRigidBody(mass, 0, collider_shape->bullet_shape(), inertia);
			collider_body->setWorldTransform(t);
		}
	}

	if (entity()->zone()) {
		PhysicsZone *zone = static_cast<PhysicsZone *>(entity()->zone());
		zone->dynamics_world()->addRigidBody(collider_body);			
		if (moving) {
			//collider_body->setActivationState(ISLAND_SLEEPING);
			//collider_body->setActivationState(ACTIVE_TAG);
			//collider_body->activate(true);
		}
	}
#endif
}

void PhysicsBody::shutdown_physics()
{
#ifdef HAVE_BULLET
	if (collider_body && entity()->zone()) {
		PhysicsZone *zone = static_cast<PhysicsZone *>(entity()->zone());

		if(collider_body->getMotionState())
		{
			delete collider_body->getMotionState();
		}
		zone->dynamics_world()->removeCollisionObject(collider_body);

		delete collider_body;
		collider_body = 0;
	}
#endif
	collider_mass = 0;
}

}