#include <UnitTest++.h>

#include <coll2d.h>

using namespace coll2d;
using namespace std;

TEST ( CheckSphereSphereInvalidTypes ) {
	Shape* polygon = new Polygon (0, NULL);
	vector3d velocity (0., 0., 0.);
	vector3d sphere_position (2.1, 0., 0.);
	Shape* sphere = new Sphere (1, sphere_position);
	sphere->setVelocity (vector3d(0., 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., polygon, sphere, &info);
	CHECK (result == CHECK_ERROR_INVALID_TYPES);
	
	delete polygon;
	delete sphere;
}

TEST ( CheckSphereSphereNoVelocity ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (4., 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (0, result);
	
	delete sphere_a;
	delete sphere_b;
}

/* This tests whether we report a collision time of 0.0 when two spheres are
 * in contact with each other
 */
TEST ( CheckCollisionSphereSphereContact ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (2., 0., 0.));

	sphere_b->setVelocity (vector3d(-1., 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);

	CHECK (result >= 0);
	CHECK_EQUAL (0.0, info.time);

	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereDiverging ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (4., 0., 0.));

	sphere_b->setVelocity (vector3d (1., 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (0, result);
	
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereCollision ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (3., 0., 0.));

	sphere_b->setVelocity (vector3d (-1., 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1, result);
	
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereCollisionCheckOrder ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (3., 0., 0.));

	sphere_a->setVelocity (vector3d (1., 0., 0.));
	sphere_b->setVelocity (vector3d (-1.5, 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1, result);

	vector3d normal = info.normal;
	result = check_collision_sphere_sphere (1., sphere_b, sphere_a, &info);
	CHECK_EQUAL (1, result);

	CHECK ( normal[0] == -info.normal[0]);
	CHECK ( normal[1] == -info.normal[1]);
	CHECK ( normal[2] == -info.normal[2]);
	
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereNoCollision ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (3., 0., 0.));

	sphere_b->setVelocity (vector3d (-0.9, 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (0, result);
	
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereCollisionResult ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (2.5, 0., 0.));

	sphere_b->setVelocity (vector3d (-1, 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);
	
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereMovingNonHorizontal ) {
	Shape* sphere_a = new Sphere (3., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1.5, vector3d (3., 0., 4.));

	sphere_a->setVelocity (vector3d (3./5., 0., 4./5.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	
	vector3d result_normal = vector3d (-3., 0., -4.).normalize();
	vector3d collision_point = result_normal * -0.5;
	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);
	CHECK_EQUAL (1, info.reference_shape);
	CHECK (result_normal == info.normal);
	CHECK (collision_point == info.point);

	sphere_a->setVelocity (vector3d (0., 0., 0.));
	
	sphere_b->setVelocity (vector3d (-3./5., 0., -4./5.));
	result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);
	
	delete sphere_a;
	delete sphere_b;
}

/* If there is one of the sphere non moving we must ensure that we
 * set the moving one as the incidence shape.
 */
TEST ( CheckSphereSphereReferenceShape ) {
	Shape* sphere_a = new Sphere (3., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1.5, vector3d (3., 0., 4.));

	sphere_a->setVelocity (vector3d (3./5., 0., 4./5.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	
	vector3d result_normal = vector3d (-3., 0., -4.).normalize();
	vector3d collision_point = result_normal * 0.5;
	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);
	CHECK (result_normal == info.normal);
	CHECK_EQUAL (1, info.reference_shape);

	sphere_a->setVelocity (vector3d (0., 0., 0.));
	
	sphere_b->setVelocity (vector3d (-3./5., 0., -4./5.));
	result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1, result);
	CHECK (result_normal * -1. == info.normal);
	CHECK_EQUAL (0.5, info.time);
	CHECK_EQUAL (0, info.reference_shape);
	
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereIgnoreHeight ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (3., 0., 0.));

	sphere_b->setVelocity (vector3d (-2., 0., 0.));

	sphere_a->setPosition (vector3d (0., 10., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1, result);
	
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckSphereSphereCollisionTimestep ) {
	Shape* sphere_a = new Sphere (1., vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (1, vector3d (3., 0., 0.));

	sphere_b->setVelocity (vector3d (-1., 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1, result);
	CHECK_EQUAL (1, info.time);
	
	result = check_collision_sphere_sphere (2., sphere_a, sphere_b, &info);
	CHECK_EQUAL (1., result);
	CHECK_EQUAL (1., info.time);
	
	result = check_collision_sphere_sphere (0.5, sphere_a, sphere_b, &info);
	CHECK_EQUAL (0, result);
		
	delete sphere_a;
	delete sphere_b;
}

TEST ( CheckPerpendicularMovement ) {
	Shape* sphere_a = new Sphere (0.4, vector3d (0., 0., 0.));
	Shape* sphere_b = new Sphere (0.4, vector3d (0., 0., -1.));

	sphere_a->setVelocity (vector3d (0., 0., 0.));
	sphere_b->setVelocity (vector3d (5., 0., 0.));

	CollisionInfo info;
	int result = check_collision_sphere_sphere (1., sphere_a, sphere_b, &info);
	CHECK_EQUAL (0, result);

	delete sphere_a;
	delete sphere_b;
}