#include <cstring>
#include<coll2d.h>

#include <iostream>

using namespace std;

namespace coll2d {

int check_collision (float timestep, Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
	check_cb check = NULL;

	check = get_check (shape_a, shape_b);

	if ( check == NULL ) {
		return CHECK_ERROR_NOT_IMPLEMENTED;
	}

	return check (timestep, shape_a, shape_b, info);
};

check_cb get_check (Shape *shape_a, Shape *shape_b) {
	if ( (dynamic_cast<Polygon*> (shape_a) != NULL)
			&& (dynamic_cast<Sphere*> (shape_b) != NULL) ) {
		return check_collision_polygon_sphere;
	}
	else if ( (dynamic_cast<Polygon*> (shape_b) != NULL)
			&& (dynamic_cast<Sphere*> (shape_a) != NULL) ) {
		return check_collision_polygon_sphere;
	}	else if ( (dynamic_cast<Sphere*> (shape_b) != NULL)
			&& (dynamic_cast<Sphere*> (shape_a) != NULL) ) {
		return check_collision_sphere_sphere;
	}

	return NULL;
}

int check_collision_rel (float timestep, Shape *shape_a, Shape *shape_b, vector3d *velocity_b, CollisionInfo *info) {
	check_cb check = NULL;

	shape_b->setVelocity (*velocity_b);
	check = get_check (shape_a, shape_b);

	if ( check == NULL ) {
		return CHECK_ERROR_NOT_IMPLEMENTED;
	}

	return check (timestep, shape_a, shape_b, info);
};

/** \brief calculates the time for a moving point to touch a plane
 *
 * \param normal      The normal of the plane
 * \param plane_point a point on the plane
 * \param point       the moving point
 * \param velocity    the velocity of the point
 *
 * \returns If the return value is negative, the point is moving away from the
 * plane or is below the plane and thus does not touch it. Otherwise 
 *   point + (return value) * velocity
 * is on the plane.
 */
float calculate_contact_plane_point (vector3d &normal, vector3d &plane_point, vector3d &point, vector3d &velocity) {
	vector3d temp_vector (point);
	temp_vector -= plane_point;

	// If the following is < 0 then the point is "below" the plane
	if (normal * temp_vector < 0) {
		// If the following is < 0 then we move even deeper
		if ( normal * velocity < 0 )
			return -999;
		// Or towards the upper side of the plane.
		else
			return -111;
	}

	int i,imax = -1;
	float vmax = 0.;
	for (i = 0; i < 3; i++) {
		if ( normal[i] * velocity[i] < vmax) {
			vmax = normal[i] * velocity[i];
			imax = i;
		}
	}

	if (imax == -1) {
		return -1.;
	}

	return (normal[imax] * plane_point[imax] - normal[imax] * point[imax]) / vmax;
}

// #define VERBOSE_PHASE

/** \brief Checks whether a sphere penetrates one of the polygones edges
 *
 * This check only finds intersections of the sphere with one of the sides in
 * other words it will not find intersections with the vertices. As for convex
 * polygons a sphere will either touch one of the edges or one of the
 * vertices.
 *
 * First thing we do is calculate the normals pointing out of the polygon.
 * Then for each edge e of the polygon (is being done by the function
 * calculate_contatc_plane_point (...):
 * - Calculate the point that would be the first to touch edge e if a contact
 *   occurs
 * - Calculate the time it would take for this point to touch the edge with
 *   the relative velocity of the sphere
 *
 * Next we calculatefor each edge the point on the sphere which would be the first to touch
 * the polygon, if it was moving towards the current edge.
 */
int check_collision_polygon_sphere_sides (Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
	Polygon* polygon = dynamic_cast<Polygon*> (shape_a);
	Sphere* sphere = dynamic_cast<Sphere*> (shape_b);

	if ( polygon == NULL && sphere == NULL) {
		polygon = dynamic_cast<Polygon*> (shape_b);
		sphere = dynamic_cast<Sphere*> (shape_a);
	}

	if (!polygon || !sphere) {
		return CHECK_ERROR_INVALID_TYPES;
	}

	vector3d velocity_sphere = sphere->getVelocity();
	
	vector3d temp_vector (velocity_sphere);
	if (temp_vector.length2() == 0.)
		return 0;

	// Calculate the normals pointing OUT of the polygon
	float *t = NULL, t_min = 10000.;
	unsigned int i,j,vertices,count = 0;
	vertices = polygon->getVerticeCount();
	vector3d *normals = NULL;
	vector3d side (0., 0., 0.);
	vector3d up (0., 1., 0.);
/*
	if (polygon->getPosition().length() > 0.) {
		for (i = 0; i < vertices; i++) {
			polygon->getVertice(i) += polygon->getPosition();
		}
	}
*/
	// normals contains all the normal vectors out of
	// the polygon
	normals = new vector3d[vertices];
	// The array t contains the time it takes until the sphere
	// would touch the plane with the given velocity
	t = new float[vertices];

	for (i = 0; i < vertices; i++) {
		j = (i + 1) % vertices;
		side = polygon->getVertice(j);
		side -= polygon->getVertice(i);
		normals[i] = cross_product (side, up);
		normals[i] /= normals[i].length ();

		// temp_vector is the point on the sphere that would touch the plane first
		// if it was directly moving towards it
		temp_vector = normals[i];
		temp_vector *= - sphere->getRadius();
		temp_vector += sphere->getPosition();

		t[i] = calculate_contact_plane_point (normals[i], polygon->getVertice(i), temp_vector, velocity_sphere);

#ifdef VERBOSE_PHASE
		cout << "= next =" << endl;
		cout << "t[" << i << "] = " << t[i] << " normal = "; normals[i].print ();
		cout << "point       = "; temp_vector.print();
		cout << "plane point = "; polygon->getVertice(i).print();
#endif
		if (t[i] < 0)
			normals[i].setValues (0., 0., 0.);
		else {
			if (t[i] < t_min)
				t_min = t[i];
			count ++;
		}
	}

	if (count == 0) {
		delete[] t;
		delete[] normals;
		return 0;
	}

	vector3d contact_point, cp_near;

	/// \ToDo: Instead of checking all planes, remember which ones to check
	for (i = 0; i < vertices; i++) {
		if ( t[i] >= 0) {
			// So there seems to occur an collision. Now we have to check,
			// whether this is actually between the points that define the
			// plane.
			unsigned int min_distance_vertex_index = i, other_vertice = (i + 1) % vertices;
			float min_distance;
			j = (i + 1) % vertices;
			// First we calculate the actual contact point:
			temp_vector = normals[i];
			temp_vector *= - sphere->getRadius();
			temp_vector *= t[i];
			temp_vector += sphere->getPosition();

			contact_point = normals[i];
			contact_point *= -sphere->getRadius();
			contact_point += temp_vector;

			// Now we calculate to which vertice this point is closer:
			temp_vector = contact_point;
			temp_vector -= polygon->getVertice(i);
			min_distance = temp_vector.length2 ();

			temp_vector = contact_point;
			temp_vector -= polygon->getVertice(j);

			if (min_distance > temp_vector.length2 ()) {
				min_distance_vertex_index = j;
				other_vertice = i;
			}

			// Then we want to see, whether the contact point actually lies
			// on the vector that goes from one vertice to the other. For
			// this we calculate (cp - near) * (far - near). The value
			// cannot be greater than 1. (otherwise we would have picked
			// the other vector as near. If it is < 0 then it is outside
			// of the polygon.
			temp_vector = polygon->getVertice(other_vertice);
			temp_vector -= polygon->getVertice(min_distance_vertex_index);

			temp_vector /= temp_vector.length();

			cp_near = contact_point;
			cp_near -= polygon->getVertice(min_distance_vertex_index);

			float val = cp_near * temp_vector;
			if ( (val >= 0 && val <= 1) && (t[i] <= 1.)) {
				info->time = t[i];
				info->normal = normals[i];

				info->point = sphere->getPosition();
				info->point += sphere->getVelocity() * t[i];
				info->point -= normals[i] * sphere->getRadius ();

				delete[] t;
				delete[] normals;
				return 1;
			}
		}
	}

	/*
	if ( (t_min <= 1.) && (t_min >= 0.)) {
		return 1;
	}
	*/

	delete[] t;
	delete[] normals;

	if (t_min > 1.)
		return 0;

	return 0;
//	return CHECK_ERROR_UNKNOWN;
};

int check_collision_polygon_sphere_vertices (Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
	Polygon* polygon = dynamic_cast<Polygon*> (shape_a);
	Sphere* sphere = dynamic_cast<Sphere*> (shape_b);

	if ( polygon == NULL && sphere == NULL) {
		polygon = dynamic_cast<Polygon*> (shape_b);
		sphere = dynamic_cast<Sphere*> (shape_a);
	}

	if (!polygon || !sphere) {
		return CHECK_ERROR_INVALID_TYPES;
	}

	// Transform global velocities to relative velocities:
	/*
	sphere->setVelocity (sphere->getVelocity() - polygon->getVelocity());
*/
	vector3d velocity_sphere = sphere->getVelocity();
	
	vector3d temp_vector (velocity_sphere);
	if (temp_vector.length2() == 0.) {
#ifdef VERBOSE_PHASE
		cout << "sphere has no velocity!" << endl;
#endif
		return 0;
	}

	vector3d contact_point, cp_near;

	// Okay if we happen to be here, there still might be one of
	// the corners colliding with the sphere.
	float a, b, c, t1, t2, t_min = 10000;
	unsigned int i, j, vertices;
	vector3d position (sphere->getPosition());
	vector3d velocity (sphere->getVelocity());
	float radius = sphere->getRadius();
	vertices = polygon->getVerticeCount();

	for (i = 0; i < vertices; i++) {
		vector3d vertice (polygon->getVertice(i));
		a = b = c = 0;
		
		// We must ensure that all happens in the x-z-plane (so y == 0.)
		vertice[1] = 0.;
		position[1] = 0.;

		vector3d distance_sphere_vertice;
		for (j = 0; j < 3; j++) {
			distance_sphere_vertice[j] = position[j] - vertice[j];
		}
#ifdef VERBOSE_PHASE
		cout << "  ===== " << endl;
		cout << "vertice = ";
		vertice.print ();
		velocity.print ();
		position.print ();
		cout << "radius = " << radius << endl;
		cout << "distance = " << distance_sphere_vertice.length () << endl;;
#endif
		for (j = 0; j < 3; j++) {
			a += velocity[j]*velocity[j];
			b += 2 * velocity[j] * (position[j] - vertice[j]);
			c += position[j] * position[j] - 2 * position[j] * vertice[j] + vertice[j] * vertice[j]; 
		}
		c -= radius * radius;

		if (solve_quadratic (a, b, c, &t1, &t2)) {
//			cout << "solve_quadratic = " << t1 << "\t" << t2 << endl;
			float t_temp_min = t2;
			if (t1 < t2)
				t_temp_min = t1;

			if (t_temp_min < t_min) {
				if ((t_temp_min <= 1.) && (t_temp_min >= 0.)) {
					t_min = t_temp_min;
					temp_vector = position;
					temp_vector -= vertice;
					temp_vector /= temp_vector.length ();
#ifdef VERBOSE_PHASE
					cout << "new closest point t = " << t_min << endl;
#endif				
					info->time = t_min;
					info->normal = temp_vector;
					info->point = vertice;
				}
			}
		}
	}

	if ( (t_min <= 1.) && (t_min >= 0.)) {
		return 1;
	}

	if (t_min > 1.)
		return 0;

	return CHECK_ERROR_UNKNOWN;
};

int check_collision_polygon_sphere (float timestep, Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
	/* If the first shape given is a sphere and the second one a shape, we have
	 * to remember it to set the right value to *info.
	 */
	bool swapped = false;
	Polygon* polygon_cast_test = dynamic_cast<Polygon*> (shape_a);
	Sphere* sphere_cast_test = dynamic_cast<Sphere*> (shape_b);

	if ( polygon_cast_test == NULL && sphere_cast_test == NULL) {
		polygon_cast_test = dynamic_cast<Polygon*> (shape_b);
		sphere_cast_test = dynamic_cast<Sphere*> (shape_a);

		swapped = true;
	}
	
	if (!polygon_cast_test || !sphere_cast_test) {
		return CHECK_ERROR_INVALID_TYPES;
	}

	Polygon polygon_copy (*polygon_cast_test);
	Sphere sphere_copy (*sphere_cast_test);
	
	unsigned int vertices;
	vertices = polygon_copy.getVerticeCount();

#ifdef VERBOSE_PHASE
	cout << "======== New Polygon Sphere Test: Phase 1" << endl;
	if (swapped)
		cout << "Swapped!" << endl;
	cout << "Polygon position: ";
	polygon_copy.getPosition().print ();
	cout << "Polygon velocity: ";
	polygon_copy.getVelocity().print ();
	cout << "Sphere position: ";
	sphere_copy.getPosition().print ();
	cout << "Sphere velocity: ";
	sphere_copy.getVelocity().print ();
	cout << "Timestep       : " << timestep << endl;

	cout << "Vertices before transformation = " << endl;
	int i;
	for (i = 0; i < vertices; i++) {
		polygon_copy.getVertice(i).print();
	}
#endif

	// Here we translate the polygon and its velocity into the reference frame
	// of the polygon.
	sphere_copy.setPosition (sphere_copy.getPosition() - polygon_copy.getPosition());
	sphere_copy.setPosition (sphere_copy.getPosition().rotate_y (-polygon_copy.getAngle()));
	// Here scale the velocity so that our time horizon lies in [0., 1.]
	sphere_copy.setVelocity ((sphere_copy.getVelocity() - polygon_copy.getVelocity() ) * timestep );
	sphere_copy.setVelocity (sphere_copy.getVelocity().rotate_y (-polygon_copy.getAngle()));

#ifdef VERBOSE_PHASE
	cout << "Vertices after transformation = " << endl;
	for (i = 0; i < vertices; i++) {
		polygon_copy.getVertice(i).print();
	}
#endif

#ifdef VERBOSE_PHASE
	cout << "After transformation:" << endl;
	cout << "Polygon position: ";
	polygon_copy.getPosition().print ();
	cout << "Polygon velocity: ";
	polygon_copy.getVelocity().print ();
	cout << "Sphere position: ";
	sphere_copy.getPosition().print ();
	cout << "Sphere velocity: ";
	sphere_copy.getVelocity().print ();
#endif

	CollisionInfo sides_info;
	CollisionInfo vertices_info;
	int sides_result = 0, vertices_result = 0;

	/* Tricky part: Since polygons are assumed to be convex and we calculated
	 * with both methods a collision, we take the sides result. Since they are
	 * convex the first event to happen is the collision with the side.
	 * Otherwise it would first touch the vertice and then the side, which is
	 * not possible. (\Todo true?)
	 */

	sides_result = check_collision_polygon_sphere_sides (&polygon_copy, &sphere_copy, &sides_info);
	// We have to transform the time back to [0., timestep]
	sides_info.time *= timestep;
#ifdef VERBOSE_PHASE
	cout << "sides_result = " << sides_result << " t = " << sides_info.time << endl;
#endif	
	if (sides_result > 0) {
		sides_info.point.rotate_y (polygon_copy.getAngle());
		sides_info.normal.rotate_y (polygon_copy.getAngle());
		sides_info.point += polygon_copy.getPosition();
		memcpy (info, &sides_info, sizeof (CollisionInfo));
		sides_info.time *= timestep;
		if (swapped == true) {
			info->reference_shape = 1;
		} else {
			info->reference_shape = 0;
		}
		return sides_result;
	} 

	vertices_result = check_collision_polygon_sphere_vertices (&polygon_copy, &sphere_copy, &vertices_info);
	// We have to transform the time back to [0., timestep]
	vertices_info.time *= timestep;
#ifdef VERBOSE_PHASE
	cout << "vertices_res = " << vertices_result << " t = " << vertices_info.time << endl;
	cout << "vertices_point = ";
	vertices_info.point.print();
#endif
	if (vertices_result > 0) {
		vertices_info.point.rotate_y (polygon_copy.getAngle());
		vertices_info.normal.rotate_y (polygon_copy.getAngle());
		vertices_info.point += polygon_copy.getPosition();
		memcpy (info, &vertices_info, sizeof (CollisionInfo));
		if (swapped == true) {
			info->reference_shape = 1;
		} else {
			info->reference_shape = 0;
		}
		return vertices_result;
	}

	if ((sides_result == 0) && (vertices_result == 0))
		return 0;

	return CHECK_ERROR_UNKNOWN;
};

int check_collision_sphere_sphere (float timestep, Shape *shape_a, Shape *shape_b, CollisionInfo *info) {
	/* If the first shape given is a sphere and the second one a shape, we have
	 * to remember it to set the right value to *info.
	 */
	Sphere* sphere_test_a = dynamic_cast<Sphere*> (shape_a);
	Sphere* sphere_test_b = dynamic_cast<Sphere*> (shape_b);

	if (!sphere_test_a || !sphere_test_b) {
		return CHECK_ERROR_INVALID_TYPES;
	}

	Sphere sphere_a (*sphere_test_a);
	Sphere sphere_b (*sphere_test_b);

	// First we check whether there is actually a relative velocity towards each
	// other:
	vector3d rel_velocity = sphere_b.getVelocity ();
	rel_velocity -= sphere_a.getVelocity ();
	rel_velocity *= timestep;
	if (rel_velocity.length2() == 0.)
		return 0;

	vector3d rel_position = sphere_b.getPosition ();
	rel_position -= sphere_a.getPosition ();

	// We need to ignore height differences
	rel_position[1] = 0.;
	rel_velocity[1] = 0.;

	vector3d rel_position_norm = rel_position;
	rel_position_norm.normalize ();

	float velocity_projection = rel_position_norm * rel_velocity;
	float distance = rel_position.length();

	if (velocity_projection >= 0.) 
		return 0;

	float t = (- distance +  sphere_a.getRadius () + sphere_b.getRadius ()) / velocity_projection;

#ifdef VERBOSE_PHASE
	cout << "==== New Sphere Sphere Test ====" << endl;
	cout << "Relative Position = ";
	rel_position.print ();
	cout << "Relative Velocity = ";
	rel_velocity.print ();
	cout << "velocity_projection = " << velocity_projection << endl;
	cout << "distance = " << distance << endl;
	cout << "- distance + Ra + Rb = " << - distance + sphere_a.getRadius () + sphere_b.getRadius () << endl;
	cout << "t = " << t << endl;
#endif

	// if t < 0 this means we would have to move back in time
	// to get to the point where the two spheres touched. In other words: they
	// are overlapping, hence it is an invalid state!
	if (t < 0)
		return CHECK_ERROR_OVERLAP;
	if (t > 1)
		return 0;

	info->point = sphere_a.getPosition() + rel_position_norm * t;
	info->time = t * timestep;

	if (sphere_a.getVelocity().length2() == 0.) {
		info->normal = rel_position_norm;
		info->reference_shape = 0;
	} else {
		info->normal = rel_position_norm * -1.;
		info->reference_shape = 1;
	}
	return 1;
};

}