#include <UnitTest++.h>

#include <coll2d.h>

using namespace coll2d;
using namespace std;

TEST ( CheckErrorInvalidTypes ) {
	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_polygon_sphere (1., sphere, sphere, &info);
	CHECK (result == CHECK_ERROR_INVALID_TYPES);

	result = check_collision_polygon_sphere (1., polygon, polygon, &info);
	CHECK (result == CHECK_ERROR_INVALID_TYPES);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereNoVelocity ) {
	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);

	vector3d sphere_position (2.1, 0., 0.);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (0., 0., 0.);

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

	CHECK (result >= 0);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereDiverging ) {
	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);

	vector3d sphere_position (2.1, 0., 0.);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (1., 0., 0.);

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

	CHECK (result >= 0);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereContact ) {
	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);

	vector3d sphere_position (2., 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 (result > 0);
	CHECK_EQUAL (0.0, info.time);
	CHECK_EQUAL (1., info.normal[0]);
	CHECK_EQUAL (0., info.normal[1]);
	CHECK_EQUAL (0., info.normal[2]);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereColliding ) {
	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);

	vector3d sphere_position (2.5, 0., 0.1);
	Shape* sphere = new Sphere (1, sphere_position);

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

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

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereCollidingTop ) {
	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);

	vector3d sphere_position (0., 0., 2.5);
	Shape* sphere = new Sphere (1, sphere_position);

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

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

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereCollidingNonPerpendicular ) {
	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);

	vector3d sphere_position (0., 0., 2.5);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (0.01, 0., -1.);

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

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereCollidingCorner) {
	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);

	vector3d sphere_position ( 1.5, 0., 2.5);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (0.0, 0., -1.);

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

	CHECK_EQUAL (1, result);

	delete polygon;
	delete sphere;
}

// In this test we make sure, that the z value of both the polygon and the
// sphere get ignored:
TEST ( CheckCollisionPolygonSphereCollidingCornerIgnoreHeight) {
	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);

	vector3d sphere_position ( 1.5, 10., 2.5);
	Shape* sphere = new Sphere (1, sphere_position);
	sphere->setVelocity (vector3d (0., 0., -1.));

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

	CHECK_EQUAL (1, result);

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

	sphere->setPosition (vector3d (1.5, 0., 2.5));
	result = check_collision (1., polygon, sphere, &info);

	CHECK_EQUAL (1, result);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereCollidingCornerTop) {
	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);

	vector3d sphere_position (1.5, 0., - 2.5);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (0., 0., 1.);

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

	CHECK_EQUAL (1, result);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereCollidingCornerNeighbour) {
	vector3d vertices[4];

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

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

	vector3d sphere_position (1.5 , 0., - 2.5);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (0., 0., 1.);

	CollisionInfo info;
	check_collision_rel (1., polygon, sphere, &velocity, &info);

	CHECK (info.point == vertices[2]);

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereNonCollidingSetPosition ) {
	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);

	vector3d sphere_position (2.5, 0., 0.1);
	Shape* sphere = new Sphere (1, sphere_position);
	sphere->setPosition (vector3d (35, 0, 0));
	vector3d velocity (-1., 0., 0.);

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

	CHECK_EQUAL (0, result);

	delete polygon;
	delete sphere;
}

// In this test we set the velocity of the polygon instead of the
// sphere.
TEST ( CheckCollisionPolygonSphereCollidingPolygonSetVelocity ) {
	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);

	vector3d sphere_position (2.5, 0., 0.);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (1., 0., 0.);
	polygon->setVelocity (velocity);

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

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);

	delete polygon;
	delete sphere;
}

TEST ( CheckSetGetAngle ) {
	Shape* polygon = new Polygon (0, NULL);

	polygon->setAngle (0.1234567);

	CHECK (fabs (0.1234567 - polygon->getAngle ()) < 1.0e-7);

	delete polygon;
}

TEST ( CheckCollisionPolygonSphereDiamond ) {
	vector3d vertices[4];
	float sqrt2 = sqrt (2);

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

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

	Shape* sphere = new Sphere (1., vector3d (sqrt2 + 1.5, 0., 0.));
	sphere->setVelocity (vector3d (-1., 0., 0.));

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

	CHECK_EQUAL (1, result);
	CHECK (fabs (0.5 - info.time) <= 1.0e-6);
	CHECK_EQUAL (0, info.reference_shape);
	
	delete polygon;
	delete sphere;
}

TEST ( CheckPolygonSpherePassingBy ) {
	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);

	vector3d sphere_position (1., 0., 2.5);
	Shape* sphere = new Sphere (1, sphere_position);
	sphere->setVelocity (vector3d (-2., 0., 0.));

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

	CHECK_EQUAL (0, result);

	delete sphere;
	delete polygon;
}

TEST ( CheckCollisionPolygonSphereReturnValuePoint ) {
	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);
	polygon->setPosition (vector3d (sqrt (2), 0., - sqrt (2)));
	polygon->setAngle (M_PI * 0.25);

	Shape* sphere = new Sphere (1, vector3d (-sqrt(2) * 0.5, 0., sqrt (2) * 0.5));
	sphere->setVelocity (vector3d (sqrt (2), 0., -sqrt(2)));

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

	CHECK_EQUAL (1, result);
	CHECK ( fabs (0.5 - info.time) < 1.0e-6);
	CHECK ( (info.point - vector3d (sqrt (2) * 0.5, 0., -sqrt(2) * 0.5)).length() < 1.0e-6 );

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereReturnValuePointTranslated ) {
	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);
	polygon->setPosition (vector3d (sqrt (2) + 1., 0., - sqrt (2) - 1.));
	polygon->setAngle (M_PI * 0.25);

	Shape* sphere = new Sphere (1, vector3d (-sqrt(2) * 0.5 + 1., 0., sqrt (2) * 0.5 -1.));
	sphere->setVelocity (vector3d (sqrt (2), 0., -sqrt(2)));

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

	CHECK_EQUAL (1, result);
	CHECK ( fabs (0.5 - info.time) < 1.0e-6);
	CHECK ( (info.point - vector3d (sqrt (2) * 0.5 + 1, 0., -sqrt(2) * 0.5 - 1.)).length() < 1.0e-6 );

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereReturnValuePointVerticeTranslated ) {
	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);
	polygon->setPosition (vector3d (0. + 2., 0., - sqrt (2) - 1.5));
	polygon->setAngle (M_PI * 0.25);

	Shape* sphere = new Sphere (1, vector3d (0. + 2., 0., 0.));
	sphere->setVelocity (vector3d (0. , 0., -1.));

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

	CHECK_EQUAL (1, result);
	CHECK ( fabs (0.5 - info.time) < 1.0e-6);
	CHECK ( (info.point - vector3d (0. + 2., 0., -1.5)).length() < 1.0e-6 );

	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereReferenceShapePolygon ) {
	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);
	polygon->setPosition (vector3d (0., 0., 0.));
	polygon->setVelocity (vector3d (1., 0., 0.));

	Shape* sphere = new Sphere (1, vector3d (2.5, 0., 0.));
	sphere->setVelocity (vector3d (0., 0., 0.));

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

	CHECK_EQUAL (1, result);
	CHECK ( fabs (0.5 - info.time) < 1.0e-6);
	CHECK_EQUAL ( 0, info.reference_shape );
	
	polygon->setPosition (vector3d (0., 0., 0.));
	polygon->setVelocity (vector3d (1., 0., 0.));
	sphere->setPosition (vector3d (2.5, 0., 0.));
	sphere->setVelocity (vector3d (0., 0., 0.));

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

	CHECK_EQUAL (1, result);
	CHECK ( fabs (0.5 - info.time) < 1.0e-6);
	CHECK_EQUAL ( 1, info.reference_shape );


	delete polygon;
	delete sphere;
}

TEST ( CheckCollisionPolygonSphereTimestep ) {
	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);

	vector3d sphere_position (2.5, 0., 0.1);
	Shape* sphere = new Sphere (1, sphere_position);

	vector3d velocity (-1., 0., 0.);
	sphere->setVelocity (velocity);

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

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);

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

	CHECK_EQUAL (1, result);
	CHECK_EQUAL (0.5, info.time);

	result = check_collision (0.499, polygon, sphere, &info);
	CHECK_EQUAL (0, result);

	delete polygon;
	delete sphere;
}