#include "PhysicsBase.h"
#include "EntityBase.h"

// needed for GetEntityGameState() in ProcessCollisionEvent()
#include "ModelBase.h"

#include "coll2d.h"

#define EPSILON 1.0e-4

namespace Engine {

/*
 * Inherited Module functions
 */
int PhysicsBase::OnInit (int argc, char* argv[]) {
	LogDebug ("Physics Init");
	mEntities.clear ();

	return 0;
}

void PhysicsBase::OnDestroy () {
	LogDebug ("Physics Destroy");
}

/*
 * Module specific functions
 */

void PhysicsBase::Move (float delta_msec) {
	EntityPhysicState* entity = NULL;
	std::map<unsigned int, EntityPhysicState*>::iterator entity_iter;
	std::map<unsigned int, EntityPhysicState*>::iterator collision_iter;
	vector3d velocity, orientation;

	for (entity_iter = mEntities.begin (); entity_iter != mEntities.end(); entity_iter++){
		entity = entity_iter->second;
		velocity = entity->GetVelocity ();

		if (velocity.length2() == 0.)
				continue;

		CheckContactCache (entity);

		entity->SetPosition (entity->GetPosition () + velocity * delta_msec);
		entity->SetOrientation (vector3d(0., entity->GetOrientation()[1] + entity->GetAngleVelocity() * delta_msec, 0.));
	}
}

int PhysicsBase::Simulate (float msec, ModelBase* model) {
	vector3d velocity, orientation;

	unsigned int entity_a_id, entity_b_id;
	int resolve_steps = 0;;
	float current_time = 0, stepsize = msec / (float) 10;

	while (current_time < msec) {
		if (msec - current_time < stepsize) 
			stepsize = msec - current_time;

		coll2d::CollisionInfo info;
		bool collision_result = false;

		collision_result = CalcNextCollision (stepsize, entity_a_id, entity_b_id, info);

		if (collision_result == 0) {
			// there is no collision, so we integrate to the end of
			// the timestep
			current_time += stepsize;
			Move (stepsize);
		} else {
			LogDebug ("Collision between %u and %u", entity_a_id, entity_b_id);
			float collision_remaining_step = stepsize;
			resolve_steps = 0;

			// This is the collision loop. It loops until there is no collision any
			// more within the stepsize.
			while (collision_remaining_step > 0.) {
				// with this we count how often we iterate through and can
				// detect certain false behaviour
				resolve_steps ++;

				// info.time tells us in what time the contact will happen and
				// theoretically we can move to that point, however due to round-off
				// errors we might end up in a situation where the Entities overlap
				// which we must prevent to keep the collision detection working.
				// 
				// Therefore we only move to a portion of the time and resolve the
				// collision then. This alpha variable controls how much of the time
				// we skip.
				float alpha = 0.001; 

				// If the timestep is already very small, we try to resolve it
				// immediately.
				if (info.time < EPSILON ) {
					// if the timestep is too small we simply resolve the collision and 
					// do not move beforehang
					LogDebug ("Time step too small ==> Resolving Step immediately");
					ResolveCollision (0., entity_a_id, entity_b_id, info);
					// Send the collision event to the Model (if we have one)
					if (model) {
						model->SendEntityCollisionEvent (entity_a_id, entity_b_id, info.time, info.normal);
					}
					//	collision_remaining_step -= 1.0e-4;
				} else {
					Move (info.time * (1 - alpha));
					LogDebug ("Resolving Step between %u and %u t = %f alpha = %f", entity_a_id, entity_b_id, info.time, alpha);
					ResolveCollision (info.time * (1 - alpha), entity_a_id, entity_b_id, info);	
					// Send the collision event to the Model (if we have one)
					if (model) {
						model->SendEntityCollisionEvent (entity_a_id, entity_b_id, info.time, info.normal);
					}
					collision_remaining_step -= info.time * (1 - alpha);
				}

				// At this point we resolved the collision we had found. Now we need
				// to check whether there is a new one
				collision_result = CalcNextCollision (collision_remaining_step, entity_a_id, entity_b_id, info);

				// If there was none, we happily move on to the end of the frame and
				// set collision_remaining_step to 0 so that we jump out of the
				// collision loop.
				if (!collision_result) {
					Move (collision_remaining_step);
					collision_remaining_step = 0.;
					break;
				}

				assert (resolve_steps <= 50);
			}
		} 
	}
	/* Postcondition: we simulated exactly msec milliseconds, no less, no more! */
	assert (current_time == msec);

	return 0;
}

void PhysicsBase::RegisterEntity (EntityPhysicState* entity) {
	unsigned int id = entity->mId;
	LogDebug ("Registering EntityPhysicState with id '%d'", id);
	if (mEntities.find (id) == mEntities.end ())
		mEntities[id] = entity;
	else {
		LogError ("Physics already has registered an Entity with id '%d'", id);
	}
}

void PhysicsBase::UnregisterEntity (const unsigned int id) {
	std::map<unsigned int, EntityPhysicState*>::iterator iter = mEntities.find(id);
	LogDebug ("Unregistering EntityPhysicState with id '%d'", id);

	if (iter != mEntities.end ()) {
		// Remove all the references of existing contacts to the Entity that is
		// going to be deleted.
		std::map<unsigned int, vector3d>::iterator temp_iter, contact_iter = iter->second->mContactNormals.begin();

		while (contact_iter != iter->second->mContactNormals.end()) {
			temp_iter = contact_iter;
			contact_iter ++;

			ContactCacheRemove (id, temp_iter->first);
		}

		// Remove the Entity from mEntities and erase the Entity
		EntityPhysicState* entity_physic_state = iter->second;
		mEntities.erase (iter);

		// We also have to delete the state!
		if (entity_physic_state->mShape)
			delete entity_physic_state->mShape;

		delete entity_physic_state;
	} else {
		LogError ("Could not unegister EntityPhysicState with id '%d': Entity not found!", id);
	}
}

/**
 * This function also sorts out collisions that might be invalid such as
 * Particle-Particle collision and the like.
 */
inline bool PhysicsBase::CheckPossibleCollisionPair (EntityPhysicState* entity_a, EntityPhysicState* entity_b) {
	// no collision checks against itself
	if (entity_a == entity_b)
		return false;

	// no checks if both are static
	if (entity_a->mStatic && entity_a->mStatic)
		return false;

	// no checks if both are particles
	if (entity_a->mBaseType == EntityBaseTypeParticle 
			&& entity_b->mBaseType == EntityBaseTypeParticle)
		return false;

	// no checks if both are particles
	if (entity_a->mBaseType == EntityBaseTypeBlock
			&& entity_b->mBaseType == EntityBaseTypeBlock)
		return false;

	return true;
}

/**
 * \param stepsize  The timestep for which we want to check whether a
 *                  collision occurs.
 * \param reference_entity_id If a collision occurs the Id of the entity that
 * 	is the reference entity is stored in this variable.
 * \param incidence_entity_id If a collision occurs the Id of the entity that
 * 	crashes into the reference entity is stored in this variable.
 * \param info Contains information about the collision (normal, time, point)
 *
 * This function calls PhysicsBase::CheckPossibleCollisionPair() to sort out
 * invalid collision pairs.
 */
bool PhysicsBase::CalcNextCollision (
		float stepsize,
		unsigned int &reference_entity_id,
		unsigned int &incidence_entity_id,
		coll2d::CollisionInfo &info) {
	std::map<unsigned int, EntityPhysicState*>::iterator collision_iter;
	std::map<unsigned int, EntityPhysicState*>::iterator collision_ref;

	// We have to find the next collision, so we check everything and
	// take the first one that will happen.
	coll2d::CollisionInfo temp_info;
	info.time = 2.;

	for (collision_ref = mEntities.begin (); collision_ref != mEntities.end (); collision_ref ++) {
		if (collision_ref->second->mId != 0) {
			collision_ref->second->UpdateShape();
		}
	}

	for (collision_ref = mEntities.begin (); collision_ref != mEntities.end (); collision_ref ++) {
		if (collision_ref->second->mAlive == false)
			continue;
		for (collision_iter = collision_ref; collision_iter != mEntities.end (); collision_iter ++) {
			if (collision_iter->second->mAlive == false)
				continue;

			if (!CheckPossibleCollisionPair(collision_ref->second, collision_iter->second))
				continue;

			EntityPhysicState *entity_a, *entity_b;
			entity_a = collision_ref->second;
			entity_b = collision_iter->second;

			int coll2d_result = coll2d::check_collision (stepsize, entity_a->mShape, entity_b->mShape, &temp_info);

			if (!HandleColl2dError (coll2d_result, stepsize, entity_a, entity_b, temp_info)) {
				LogError ("Could not handle coll2d error: %d\n", coll2d_result);
			}

			if (coll2d_result > 0 && temp_info.time < info.time ) {
				info = temp_info;

				assert (info.reference_shape >= 0);
				if (info.reference_shape == 0) {
					reference_entity_id = collision_ref->first;
					incidence_entity_id = collision_iter->first;
				} else {
					reference_entity_id = collision_iter->first;
					incidence_entity_id = collision_ref->first;
				}
			}
		}
	}
	if (info.time != 2.)
		return true;

	return false;
}

/**
 * This function updates the velocity of the Entity with id of
 * incidence_entity_id so that it no more is in collision with with Entity
 * with id reference_entity_id. Additionally to that we also cache the
 * contact normals and check against them so that the new velocity does not
 * violate previous contacts.
 *
 * We store the cached contact normals both in the incidence and the reference
 * entity and we have to make sure that these references get cleared once one
 * of the two get deleted! This is done in PhysicsBase::UnregisterEntity().
 */
void PhysicsBase::ResolveCollision (float stepsize, unsigned int reference_entity_id, unsigned int incidence_entity_id, coll2d::CollisionInfo &info) {
	EntityPhysicState* reference_entity = mEntities[reference_entity_id];
	EntityPhysicState* incidence_entity = mEntities[incidence_entity_id];

	if (!reference_entity || !incidence_entity) {
		LogError ("Invalid entity IDs passed to %s: %u and %u", __FUNCTION__, reference_entity_id, incidence_entity_id);
	}
	assert (reference_entity && incidence_entity);

	// So far only resolving of collision of sphere collisions is
	// allowed
	assert (dynamic_cast<coll2d::Sphere*> (incidence_entity->mShape));

	// First we calculate the velocity along the normal and then we calculate
	// the velocity that is tangential to the plane and set the new velocity to
	// it.
	vector3d new_velocity = incidence_entity->mVelocity;
	new_velocity -= reference_entity->mVelocity;

	// It should be greater zero otherwise there was an error in the collision
	// detection.
	assert (new_velocity.length2 ());

	// The scalar proj tells us how far we went along the normal
	float proj = new_velocity * info.normal;
	// As there is a collision, and the incidence_entity is moving towards the
	// plane, this value must be strictly smaller than zero, otherwise we would
	// not penetrate the plane.
	if (proj > 0.) {
		LogError ("Projection invalid: %e", proj);
		info.doPrint ("Collision Info:\n");
	}
	assert (proj < 0.);

	// Collision handling start:
	// This is the code that tells us how we deal with collisions and how to
	// prevent them. To get real dynamics one has to adjust this section:
	vector3d old_velocity = incidence_entity->mVelocity;
	new_velocity = old_velocity + info.normal * proj * (-1.0);
	incidence_entity->SetVelocity (new_velocity);
	// Collision handling end

	// And we also add the normal to the cached contacts for both incidence
	// and reference entity
	ContactCacheAdd (incidence_entity, reference_entity, info.normal);	

	// Now we check whether we are violating any of the cached contacts. For
	// this we loop through all contacts and try to adjust the velocity of
	// incidence_entity until we no more violate the cached contacts.
	std::map<unsigned int, vector3d>::iterator iter = incidence_entity->mContactNormals.begin();
	int readjust = 0;
	while (iter != incidence_entity->mContactNormals.end()) {
		vector3d contact_normal = iter->second;
		float contact_velocity = new_velocity * contact_normal;

		if (contact_velocity < 0.) {
			if (reference_entity_id == iter->first) {
				// In this case, the projection was not good enough. We simultaneously
				// damp the velocity and push a little harder. If it was damped too
				// much or we had to readjust too often, we set the velocity to zero.
				LogDebug ("Resolved collision needs to be readjusted");
				new_velocity = incidence_entity->GetVelocity();
				new_velocity *= 0.5;
				if (new_velocity.length2() < EPSILON)
					new_velocity.setValues (0., 0., 0.);
				else 
					new_velocity += info.normal * 0.001;
				incidence_entity->SetVelocity (new_velocity);
				readjust++;

				// More than 10 readjusts? -> set velocity to zero
				if (readjust > 10)
					new_velocity.setValues (0., 0., 0.);
				
				// and we redo all the checking:
				iter = incidence_entity->mContactNormals.begin();
				continue;
			}

			// In this case the proposed velocity is violating another contact
			// and we set the velocity to zero.
			LogDebug ("Cached collision: %e -> resetting velocity", contact_velocity);
//			contact_normal.print ("contact normal: ");
			new_velocity.setValues (0., 0., 0.);
			incidence_entity->mVelocity = new_velocity;
		} else if (contact_velocity > 0.01) {
			// If we move sufficiently fast away from a contact we remove the
			// contact from the cache.
			
			// * Attention! * As the function ContactCacheRemove() deletes an entry
			// of mContactNormals we have to increase the iterator *before* we
			// remove the cache. Otherwise the iterator might become invalid!
			unsigned int contact_id = iter->first;
			iter++;

			ContactCacheRemove (contact_id, incidence_entity_id);

			continue;
		}
		iter++;
	}
}

/*
 * Contact Cache Functions
 */
void PhysicsBase::ContactCacheAdd (EntityPhysicState* incidence_entity, EntityPhysicState* reference_entity, vector3d normal) {
	incidence_entity->mContactNormals[reference_entity->mId] = normal;
	reference_entity->mContactNormals[incidence_entity->mId] = normal * -1.;

	LogDebug ("Adding normal (%f,%f,%f) id=%d to entity %d",
			normal[0], normal[1], normal[2], 
			reference_entity->mId, incidence_entity->mId);
	LogDebug ("Adding normal (%f,%f,%f) id=%d to entity %d",
			normal[0] * -1., normal[1] * -1., normal[2] * -1., 
			incidence_entity->mId, reference_entity->mId);
}

void PhysicsBase::ContactCacheRemove (unsigned int entity_a_id, unsigned int entity_b_id) {
	assert (entity_a_id != entity_b_id);

	EntityPhysicState *entity_a, *entity_b;

#ifdef WIN32
	entity_a = mEntities[entity_a_id];
	entity_b = mEntities[entity_b_id];
#else
	entity_a = mEntities.at(entity_a_id);
	entity_b = mEntities.at(entity_b_id);
#endif

	// Check the entries exist
	assert (entity_a->mContactNormals.find (entity_b_id) != entity_a->mContactNormals.end());
	assert (entity_b->mContactNormals.find (entity_a_id) != entity_b->mContactNormals.end());

#ifdef WIN32
	vector3d contact_normal = entity_a->mContactNormals[entity_b_id];
#else
	vector3d contact_normal = entity_a->mContactNormals.at(entity_b_id);
#endif

	LogDebug ("Removing normal (%f,%f,%f) id=%d from entity %d",
			contact_normal[0], contact_normal[1], contact_normal[2],
			entity_a_id, entity_b_id);
	entity_a->mContactNormals.erase (entity_a->mContactNormals.find(entity_b_id));

#ifdef WIN32
	contact_normal = entity_b->mContactNormals[entity_a_id];
#else
	contact_normal = entity_b->mContactNormals.at(entity_a_id);
#endif
	
	LogDebug ("Removing normal (%f,%f,%f) id=%d from entity %d",
			contact_normal[0], contact_normal[1], contact_normal[2],
			entity_b_id, entity_a_id);
	entity_b->mContactNormals.erase (entity_b->mContactNormals.find(entity_a_id));
}

/** \brief Checks whether we are still in contact with the entities stored in mContactNormals
 *
 * To check whether we still are in contact, we modify temporarily the
 * velocity of the given Entity that it moves towards the contact point (i.e.
 * we add the negative normal to the velocity) and re-check for a collision.
 * If the collision has a time value of 0.0 and the reported normal stays the
 * same, we know, that the two Entities are still in contact. If not, we lost
 * contact.
 *
 * We can skip the test, if the scalar product of the normal and velocity are
 * positive (in this case we move away from the plane)
 */
void PhysicsBase::CheckContactCache (EntityPhysicState* entity) {
	std::map<unsigned int, vector3d>::iterator contacts_iter, current_iter;

	contacts_iter = entity->mContactNormals.begin();
	while (contacts_iter != entity->mContactNormals.end()) {
		// * Attention! *
		// current_iter can be used throughout this environment and contacts_iter
		// can already now be increased as it *must not* be used! This is due to
		// the nature of ContactCacheRemove() which might make contacts_iter
		// invalid if we were using that.
		current_iter = contacts_iter;
		contacts_iter ++;

		unsigned int contact_entity_id = current_iter->first;

#ifdef WIN32
		EntityPhysicState* contact_entity = mEntities[contact_entity_id];
#else
		EntityPhysicState* contact_entity = mEntities.at (contact_entity_id);
#endif
	
		vector3d normal = current_iter->second;
		vector3d old_velocity = entity->GetVelocity();

		// If we already move away from the normal, we delete the contact.
		if (normal * old_velocity > 0.01) {
			LogDebug ("Lost Contact with entity %d!", current_iter->first);
			ContactCacheRemove (entity->mId, current_iter->first);
			continue;
		} else {
			vector3d new_velocity (old_velocity);
			new_velocity -= normal;
			entity->SetVelocity (new_velocity);

			entity->UpdateShape();
			contact_entity->UpdateShape();

			coll2d::CollisionInfo info;
			int coll2d_result;

			coll2d_result = coll2d::check_collision (1.0, entity->mShape, contact_entity->mShape, &info);

			if (!HandleColl2dError (coll2d_result, 1.0, entity, contact_entity, info)) {
				// error
				LogError ("Error when performing collision check: %s\n", __FUNCTION__);
			}
			
			if (coll2d_result > 0) {
				// no contact, so delete it:
				LogDebug ("Lost Contact with entity %d!", current_iter->first);
				ContactCacheRemove (entity->mId, current_iter->first);
	
				entity->SetVelocity (old_velocity);
				continue;
			} else if ( coll2d_result < 0){
				// contact
				if (info.time != 0. || normal != info.normal) {
					LogError ("Something strange happened when checking for contacts in %s", __FUNCTION__);
				}
			}

			entity->SetVelocity (old_velocity);
		}
	}
}

/**
 * So far we ignore overlapping if one entity is an EntityBaseTypeActor and
 * the other a EntityBaseTypeParticle.
 *
 * If this function returns true everything is ok and we can safely continue
 * otherwise it is recommended to quit the application.
 *
 * \returns true if there was no error at all or we were able to deal with it
 */
bool PhysicsBase::HandleColl2dError (int coll2d_result, float stepsize,
		EntityPhysicState* entity_a, EntityPhysicState* entity_b, coll2d::CollisionInfo &info)
{
	if (coll2d_result < 0) {
		if (coll2d_result == CHECK_ERROR_OVERLAP) {
			// this can happen if an Actor is faster than its thrown Particle,
			// we ignore this for now
			/// \todo Handle overlaps of Actors and Particles better or define clear guidelines. Note this also in Entity.h
			if ( (entity_a->mBaseType == EntityBaseTypeParticle && entity_b->mBaseType == EntityBaseTypeActor) 
					|| (entity_b->mBaseType == EntityBaseTypeParticle && entity_a->mBaseType == EntityBaseTypeActor) )
				LogDebug ("Ignoring CHECK_ERROR_OVERLAP");
			return true;
		} else {
			LogError ("coll2d Error: %d (stepsize = %f, id_a = %d, id_b = %d)", coll2d_result, stepsize, entity_a->mId, entity_b->mId);
			entity_a->mShape->doPrint ("Debug: entity_a");
			entity_b->mShape->doPrint ("Debug: entity_b");
			return false;
		}
	}

	return true;
}

EntityPhysicState* CreateEntityPhysicState (EntityBaseType type, unsigned int id) {
	EntityPhysicState* entity_physics = new EntityPhysicState ();
	if (!entity_physics) {
		LogError ("Could not allocate enough memory for EntityPhysicState of type '%d'", type);
	}
	// default values for all Entities
	entity_physics->mId = id;
	entity_physics->mBaseType = type;

  // specific values for each Entity type
	if (type == EntityBaseTypeNone) {
		entity_physics->mShape = new coll2d::Sphere (0.01);
		assert (entity_physics->mShape);
	} else if (type == EntityBaseTypeActor) {
		entity_physics->mShape = new coll2d::Sphere (0.4);
		assert (entity_physics->mShape);
	} else if (type == EntityBaseTypeBlock) {
		entity_physics->mShape = new coll2d::Polygon (4);
		assert (entity_physics->mShape);

		static_cast<coll2d::Polygon*> (entity_physics->mShape)->setVertice (0, vector3d (-0.5, 0., 0.5));
		static_cast<coll2d::Polygon*> (entity_physics->mShape)->setVertice (1, vector3d (0.5, 0., 0.5));
		static_cast<coll2d::Polygon*> (entity_physics->mShape)->setVertice (2, vector3d (0.5, 0., -0.5));
		static_cast<coll2d::Polygon*> (entity_physics->mShape)->setVertice (3, vector3d (-0.5, 0., -0.5));
	} else if (type == EntityBaseTypeParticle) {
		entity_physics->mShape = new coll2d::Sphere (0.05);
		assert (entity_physics->mShape);
	} else {
		LogError ("No EntityPhysicState defined for Entity type '%d'", type);
	}

	return entity_physics;
}

}