#include <UnitTest++.h>

#include "Logging.h"
#include "PhysicsBase.h"
#include "ModelBase.h"
#include "EntityBase.h"
#include "coll2d.h"

using namespace std;
using namespace Engine;

struct PhysicsFixture {
	PhysicsFixture () {
		LoggingModule.Init (0, NULL);
		LoggingModule.SetLogPrintLevel (LogLevelMessage);
		PhysicsModule.Init (0, NULL);

		dummy_entity = CreateEntityPhysicState (EntityBaseTypeNone, 0);
		particle_entity = CreateEntityPhysicState (EntityBaseTypeParticle, 1);
		block_entity = CreateEntityPhysicState (EntityBaseTypeBlock, 2);
		actor_entity = CreateEntityPhysicState (EntityBaseTypeActor, 3);

		PhysicsModule.RegisterEntity (dummy_entity);
		PhysicsModule.RegisterEntity (particle_entity);
		PhysicsModule.RegisterEntity (block_entity);
		PhysicsModule.RegisterEntity (actor_entity);
	}

	~PhysicsFixture () {
		PhysicsModule.UnregisterEntity (0);
		PhysicsModule.UnregisterEntity (1);
		PhysicsModule.UnregisterEntity (2);
		PhysicsModule.UnregisterEntity (3);

		PhysicsModule.Destroy ();
		LoggingModule.Destroy ();
	}

	Logging LoggingModule;
	PhysicsBase PhysicsModule;

	EntityPhysicState* dummy_entity;
	EntityPhysicState* particle_entity;
	EntityPhysicState* block_entity;
	EntityPhysicState* actor_entity;
};

TEST_FIXTURE ( PhysicsFixture, PhysicsModuleActorActorCollision ) {
	int result = -1;

	EntityPhysicState* actor_2_entity = CreateEntityPhysicState (EntityBaseTypeActor, 4);
	assert (actor_2_entity->mShape);
	assert (actor_entity->mShape);

	PhysicsModule.RegisterEntity (actor_2_entity);

	actor_entity->mPosition = vector3d (0., 0., 0.);
	actor_entity->mVelocity = vector3d (1., 0., 0.);

	actor_2_entity->mPosition = vector3d (1.5, 0., 0.1);
	actor_2_entity->mVelocity = vector3d (-1., 0., 0.);

	result = PhysicsModule.Simulate (1000.);
	CHECK_EQUAL (0, result);

	PhysicsModule.UnregisterEntity (actor_2_entity->mId);
}

TEST_FIXTURE (PhysicsFixture, PhysicsTestWarpCollisionX ) {
	float world_size = 10;
	PhysicsModule.SetWorldBounds (
			vector3d (-world_size, -world_size, -world_size),
			vector3d (world_size, world_size, world_size)
			);
	
	EntityPhysicState* actor_2_entity = CreateEntityPhysicState (EntityBaseTypeActor, 4);
	assert (actor_2_entity->mShape);
	assert (actor_entity->mShape);

	PhysicsModule.RegisterEntity (actor_2_entity);

	float actor_radius = dynamic_cast<coll2d::Sphere*>(actor_entity->mShape)->getRadius();
//	actor_entity->mPosition = vector3d (-world_size + actor_radius + 0.01 + 2 * world_size, 0., world_size - actor_radius);
	actor_entity->mPosition = vector3d (-world_size + actor_radius + 0.01, 0., 0.);
	actor_entity->mVelocity = vector3d (-1., 0., 0.);

	actor_2_entity->mPosition = vector3d (world_size - actor_radius - 0.01, 0., 0.);
	actor_2_entity->mVelocity = vector3d (0., 0., 0.);

	bool coll_res = false;
	coll2d::CollisionInfo info;
	unsigned int reference_id, incidence_id;

	// with disabled world warp, there should not be a collision ... 
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeX);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (!coll_res);

	// ... however with WorldWarp enabled, there should be!
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeX);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (coll_res);

	// Now we change the velocities and let actor_2 move out of the maximum x
	// direction
	actor_entity->mVelocity = vector3d (0., 0., 0.);
	actor_2_entity->mVelocity = vector3d (1., 0., 0.);

	// with disabled world warp, there should not be a collision ... 
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeX);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (!coll_res);

	// ... however with WorldWarp enabled, there should be!
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeX);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (coll_res);
}

TEST_FIXTURE (PhysicsFixture, PhysicsTestWarpCollisionZ ) {
	float world_size = 10;
	PhysicsModule.SetWorldBounds (
			vector3d (-world_size, -world_size, -world_size),
			vector3d (world_size, world_size, world_size)
			);
	
	EntityPhysicState* actor_2_entity = CreateEntityPhysicState (EntityBaseTypeActor, 4);
	assert (actor_2_entity->mShape);
	assert (actor_entity->mShape);

	PhysicsModule.RegisterEntity (actor_2_entity);

	float actor_radius = dynamic_cast<coll2d::Sphere*>(actor_entity->mShape)->getRadius();
//	actor_entity->mPosition = vector3d (-world_size + actor_radius + 0.01 + 2 * world_size, 0., world_size - actor_radius);
	actor_entity->mPosition = vector3d (0, 0, -world_size + actor_radius + 0.01);
	actor_entity->mVelocity = vector3d (0., 0., -1.);

	actor_2_entity->mPosition = vector3d (0, 0, world_size - actor_radius - 0.01);
	actor_2_entity->mVelocity = vector3d (0., 0., 1.);

	bool coll_res = false;
	coll2d::CollisionInfo info;
	unsigned int reference_id, incidence_id;

	// with disabled world warp, there should not be a collision ... 
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (!coll_res);

	// ... however with WorldWarp enabled, there should be!
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (coll_res);

	// Now we change the velocities and let actor_2 move out of the maximum x
	// direction
	actor_entity->mVelocity = vector3d (0., 0., -1.);
	actor_2_entity->mVelocity = vector3d (0., 0., 0.);

	// with disabled world warp, there should not be a collision ... 
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (!coll_res);

	// ... however with WorldWarp enabled, there should be!
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (coll_res);
}

TEST_FIXTURE (PhysicsFixture, PhysicsTestWarpCollisionXZ ) {
	float world_size = 10;
	PhysicsModule.SetWorldBounds (
			vector3d (-world_size, -world_size, -world_size),
			vector3d (world_size, world_size, world_size)
			);

	EntityPhysicState* actor_2_entity = CreateEntityPhysicState (EntityBaseTypeActor, 4);
	assert (actor_2_entity->mShape);
	assert (actor_entity->mShape);

	PhysicsModule.RegisterEntity (actor_2_entity);

	float actor_radius = dynamic_cast<coll2d::Sphere*>(actor_entity->mShape)->getRadius();
	actor_entity->mPosition = vector3d (- world_size + actor_radius + 0.01, 0, -world_size + actor_radius + 0.01);
	actor_entity->mVelocity = vector3d (-1., 0., -1.);

	actor_2_entity->mPosition = vector3d (world_size - actor_radius - 0.01, 0, world_size - actor_radius - 0.01);
	actor_2_entity->mVelocity = vector3d (0., 0., 0.);

	bool coll_res = false;
	coll2d::CollisionInfo info;
	unsigned int reference_id, incidence_id;

	// with disabled world warp, there should not be a collision ... 
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeX);
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (!coll_res);

	// ... however with WorldWarp enabled, there should be!
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeX);
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (coll_res);

	// Now we change the velocities and let actor_2 move out of the maximum x
	// direction
	actor_entity->mVelocity = vector3d (0., 0., 0.);
	actor_2_entity->mVelocity = vector3d (1., 0., 1.);

	// with disabled world warp, there should not be a collision ... 
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeX);
	PhysicsModule.DisableWorldWarp(PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (!coll_res);

	// ... however with WorldWarp enabled, there should be!
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeX);
	PhysicsModule.EnableWorldWarp (PhysicsBase::WorldWarpModeZ);
	coll_res = PhysicsModule.CalcNextCollision (1., reference_id, incidence_id, info);
	CHECK (coll_res);
}