#include <UnitTest++.h>

#include <coll2d.h>

using namespace coll2d;
using namespace std;

TEST ( SphereCopyConstructer ) {
	Sphere sphere_a (123.);
	sphere_a.setVelocity (vector3d(1., 2., 3.));

	Sphere sphere_b (sphere_a);
	CHECK_EQUAL (sphere_a.getRadius (), sphere_b.getRadius ());

	vector3d velocity_a = sphere_a.getVelocity ();
	vector3d velocity_b = sphere_b.getVelocity ();

	CHECK (velocity_a == velocity_b );

	sphere_b.setVelocity (vector3d (0., 0., 0.));

	velocity_a = sphere_a.getVelocity ();
	velocity_b = sphere_b.getVelocity ();

	CHECK (velocity_a != velocity_b );

	CHECK_EQUAL (123., sphere_a.getBoundingRadius());
	CHECK_EQUAL (123., sphere_b.getBoundingRadius());
}

TEST ( PolygonCopyConstructer ) {
	Polygon polygon_a (3);

	vector3d vertice0 (-1., 0., -1.);
	vector3d vertice1 (1., 0., -1.);
	vector3d vertice2 (0., 0., 1.);

	polygon_a.setVertice (0, vertice0);
	polygon_a.setVertice (1, vertice1);
	polygon_a.setVertice (2, vertice2);

	polygon_a.setVelocity (vector3d(1., 2., 3.));

	Polygon polygon_b (polygon_a);

	vector3d velocity_a = polygon_a.getVelocity ();
	vector3d velocity_b = polygon_b.getVelocity ();

	CHECK (vertice0 == polygon_b.getVertice (0));
	CHECK (vertice1 == polygon_b.getVertice (1));
	CHECK (vertice2 == polygon_b.getVertice (2));

	CHECK_CLOSE (sqrt (2.), polygon_a.getBoundingRadius(), 1.0e-7);

	polygon_b.setVelocity (vector3d (0., 0., 0.));
	velocity_a = polygon_a.getVelocity ();
	velocity_b = polygon_b.getVelocity ();
	
	CHECK (velocity_a != velocity_b );
}

/** Checks whether we identify the different shapes
 * correctly.
 */
TEST ( CheckShapeType ) {
	vector3d sphere_position (0., 0., 0.);

	Shape* polygon = new Polygon (0, NULL);
	Shape* sphere = new Sphere (0, sphere_position);

	Shape* cast_test = NULL;

	cast_test = dynamic_cast <Sphere*> ( sphere );
	CHECK_EQUAL (cast_test, sphere);
	cast_test = dynamic_cast <Sphere*> ( polygon );
	CHECK (cast_test ==  NULL );
	cast_test = dynamic_cast <Polygon*> (polygon);
	CHECK_EQUAL (cast_test, polygon);
	cast_test = dynamic_cast <Polygon*> (sphere);
	CHECK (cast_test == NULL );

	delete polygon;
	delete sphere;
}

/** Checks whether the right check function is chosen
 */
TEST ( CheckGetCheck ) {
	vector3d sphere_position (0., 0., 0.);

	Shape* polygon = new Polygon (0, NULL);
	Shape* sphere = new Sphere (0, sphere_position);

	check_cb check = NULL;

	// polygon - sphere -> polygon_sphere
	check = get_check (polygon, sphere);
	CHECK (static_cast<check_cb> (check) == static_cast<check_cb> (check_collision_polygon_sphere));

	// sphere - polygon -> polygon_sphere
	check = get_check (sphere, polygon);
	CHECK (static_cast<check_cb> (check) == static_cast<check_cb> (check_collision_polygon_sphere));

	// and we do not want that the pointers have changed!
	// (as it used to be in previous versions)
	CHECK (dynamic_cast <Polygon*> (sphere) == NULL);
	CHECK (dynamic_cast <Sphere*> (polygon) == NULL);

	check = get_check (sphere, sphere);
	CHECK (check == check_collision_sphere_sphere);

	delete polygon;
	delete sphere;
}

TEST ( CheckErrorNotImplemented ) {
	Shape* polygon = new Polygon (0, NULL);
	vector3d velocity (0., 0., 0.);

	CollisionInfo info;
	int result = check_collision_rel (1., polygon, polygon, &velocity, &info);

	CHECK (result == CHECK_ERROR_NOT_IMPLEMENTED);

	delete polygon;
}

TEST ( CheckCalculateContactPlanePoint ) {
	vector3d normal (1., 0., 0.);
	vector3d plane_point (0., 0., 0.);
	vector3d point (1., 0., 0.);
	vector3d velocity (-1., 0., 0);

	float result;

	// Directly moving towards the plane
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (1, result);

	// Moving with a slight angle onto the plane
	velocity.setValues (-1., 0., 0.2);
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (1, result);

	velocity.setValues (-1., 0., 0.);

	// Point is "below" the plane and moves even deeper
	plane_point.setValues (2., 0., 0);
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (-999, result);

	// Point is "below" but moves towards the plane
	velocity.setValues (1., 0., 0);
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (-111, result);
}

TEST ( CheckCollisionStepsize ) {
	vector3d normal (1., 0., 0.);
	vector3d plane_point (0., 0., 0.);
	vector3d point (1., 0., 0.);
	vector3d velocity (-1., 0., 0);

	float result;

	// Directly moving towards the plane
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (1, result);

	// Moving with a slight angle onto the plane
	velocity.setValues (-1., 0., 0.2);
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (1, result);

	velocity.setValues (-1., 0., 0.);

	// Point is "below" the plane and moves even deeper
	plane_point.setValues (2., 0., 0);
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (-999, result);

	// Point is "below" but moves towards the plane
	velocity.setValues (1., 0., 0);
	result = calculate_contact_plane_point (normal, plane_point, point, velocity);
	CHECK_EQUAL (-111, result);
}

/* Here we test, whether the value of info.reference_shape is correct.
 * If we swap the shapes passed to check_collision, then the value must swap,
 * too.
 */
TEST ( CheckReferenceShapeValue ) {
	vector3d vertices[4];

	vertices[0].setValues (-1., 0., 1.);
	vertices[1].setValues (1., 0., 1.);
	vertices[2].setValues (1., 0., -1.);
	vertices[3].setValues (-1., 0., -1.);

	Shape* polygon = new Polygon (4, vertices);

	// first part of the test, the collision occurs 
	// on an edge
	vector3d sphere_position (2.3, 0., 0.);
	Shape* sphere = new Sphere (1, sphere_position);
	sphere->setVelocity (vector3d (-1., 0., 0.));

	CollisionInfo info;
	int result = check_collision (1., polygon, sphere, &info);

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0, info.reference_shape);

	result = check_collision (1., sphere, polygon, &info);

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (1, info.reference_shape);

	// here the collision occurs on a vertice
	sphere->setPosition (vector3d (1.5, 0., -2.5));
	sphere->setVelocity (vector3d (0., 0., 1.));

	result = check_collision (1., sphere, polygon, &info);

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (1, info.reference_shape);

	result = check_collision (1., polygon, sphere, &info);

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0, info.reference_shape);

	delete sphere;
	delete polygon;
}

/* Test whether Polygon::getCopy() does what it should */
TEST ( CheckPolygonGetCopy ) {
	vector3d vertices[4];

	vertices[0].setValues (-1., 0., 1.);
	vertices[1].setValues (1., 0., 1.);
	vertices[2].setValues (1., 0., -1.);
	vertices[3].setValues (-1., 0., -1.);

	Polygon* polygon = new Polygon (4, vertices);
	Polygon* polygon_copy = polygon->getCopy();

	CHECK (polygon != polygon_copy);
	CHECK_EQUAL (polygon->getVerticeCount(), polygon_copy->getVerticeCount());

	unsigned int i, count;
	count = polygon->getVerticeCount ();

	for (i = 0; i < count; i ++) {
		vector3d vertice = polygon->getVertice (i);
		vector3d vertice_copy = polygon_copy->getVertice (i);

		int j;
		for (j = 0; j < 3; j++) {
			CHECK_EQUAL (vertice[j], vertice_copy[j]);
		}
	}

	delete polygon;
	delete polygon_copy;
}

/* Test whether Sphere::getCopy() does what it should */
TEST ( CheckSphereGetCopy ) {
	Sphere* sphere = new Sphere (1.23);
	Sphere* sphere_copy = sphere->getCopy();

	CHECK_EQUAL (sphere->getRadius(), sphere_copy->getRadius());
	CHECK (sphere != sphere_copy);

	delete sphere;
	delete sphere_copy;
}

TEST ( SphereSetGetRadius ) {
	Sphere sphere (123.);
	CHECK_EQUAL (123, sphere.getRadius());

	sphere.setRadius (456.);
	CHECK_EQUAL (456, sphere.getRadius());
}