#ifndef _COLL2D_H
#define _COLL2D_H

/** \brief Coll2d - A 2d collision detection library
 * \author Martin Felis <martin@silef.de>
 *
 * This library provides functions to detect collisions between polygons and
 * spheres.
 *
 * Notes:
 * - vertices of polygons are described in local coordinates
 * - return values of check_collision are in global coordinates
 * - all Shapes get copied to a temporary instance which will then get
 *   transferred into global coordinates
 * - Polygons are assumed to be convex and the vertices are stored
 *   counter clockwise.
 * - The transformation of the global position and velocities towards
 *   relative position and velocities happens in the
 *   int check_collision_<type>_<type> functions!
 */

#include <mathlib.h>
#include <iostream>
#include <cstring>

#include <coll2d_errors.h>

namespace coll2d {

/** \brief Contains the information of a collision
 *
 * \param normal  The normal of the reference plane
 * \param point   The actual point where the collision happens in global
 *                cooldinates
 * \param time    If both objects move for this amount of time the contact
 *                will occur.
 * \param reference_shape
 *                If 0, the first shape passed to he check_collision function
 *                is the reference shape, otherwise the second.
*/
struct CollisionInfo {
	vector3d normal;
	vector3d point;
	float time;
	int reference_shape;

	CollisionInfo () : normal (0., 0., 0.), point (0., 0., 0.), time (-1.), reference_shape (-1) {
	}

	CollisionInfo& operator= (const CollisionInfo& info) {
		if (this != &info) {
			normal = info.normal;
			point = info.point;
			time = info.time;
			reference_shape = info.reference_shape;
		}

		return *this;
	}

	void doPrint (const char* msg) {
		std::cout << msg;
		std::cout <<  "Time   = " << time << std::endl;
		normal.print ("Normal = ");
		point.print ( "Point  = ");
		std::cout <<  "Reference = " << reference_shape << std::endl;
	}
};

/** \brief Base class for all shapes
 *
 */
class Shape {
protected:
	vector3d mPosition;
	vector3d mVelocity;
	float mAngle;
	float mAngleVelocity;
	float mBoundingRadius;

	virtual void dummy() {
	}
public:
	Shape():
		mPosition (0., 0., 0.),
		mVelocity (0., 0., 0.),
		mAngle (0.),
		mAngleVelocity (0.),
		mBoundingRadius (0.) {
	}
	Shape (const Shape &shape):
		mPosition (shape.mPosition),
		mVelocity (shape.mVelocity),
		mAngle (shape.mAngle),
		mAngleVelocity (shape.mAngleVelocity),
		mBoundingRadius (shape.mBoundingRadius)
	{ }

	virtual ~Shape () {};

	/** \brief Creates and returns a copy of itself */
	virtual Shape* getCopy () = 0;

	void setPosition(vector3d position) {
		mPosition = position;
	}
	vector3d getPosition() {
		return mPosition;
	}
	void setVelocity(vector3d velocity) {
		mVelocity = velocity;
	}
	vector3d getVelocity() {
		return mVelocity;
	}

	void setAngle (const float &angle) {
		mAngle = angle;
	}
	float getAngle () {
		return mAngle;
	}
	void setAngleVelocity (float angle_velocity) {
		mAngleVelocity = angle_velocity;
	}
	float getAngleVelocity () {
		return mAngleVelocity;
	}
	void setBoundingRadius (float bounding_radius) {
		mBoundingRadius = bounding_radius;
	}
	float getBoundingRadius () {
		return mBoundingRadius;
	}

	virtual void doPrintType() {
		std::cout << "Shape" << std::endl;
	}
	virtual void doPrint (const char* name) {
		std::cout << name << "<unknown shape>" << std::endl;
	}
	friend int check_collision_rel(Shape *shape_a, Shape *shape_b,
			vector3d *velocity_b, CollisionInfo* info);
};

class Polygon: public Shape {
private:
	unsigned int mVerticeCount;
	vector3d *mVertices;
	bool mFreeVertices;
public:
	Polygon() {
		mVerticeCount = 0;
		mVertices = NULL;
		mFreeVertices = false;
	}
	Polygon(unsigned int n) {
		mVerticeCount = n;
		mVertices = new vector3d[n];
		mFreeVertices = true;
	}
	Polygon(unsigned int n, vector3d *vertices) {
		mVerticeCount = n;
		mFreeVertices = false;

		if (vertices == NULL && n > 0) {
			mVertices = new vector3d [n];
			mFreeVertices = true;
		}

		mVertices = vertices;
		updateBoundingRadius();
	}
	Polygon (const Polygon &polygon) : Shape (polygon) {
		mVerticeCount = polygon.mVerticeCount;
		mFreeVertices = true;

		mVertices = new vector3d[mVerticeCount];
		memcpy (mVertices, polygon.mVertices, sizeof (vector3d) * mVerticeCount);

		mBoundingRadius = polygon.mBoundingRadius;
	}
	virtual ~Polygon () {
		if (mFreeVertices == true && mVertices)
			delete[] mVertices;
	}

	virtual Polygon* getCopy () {
		Polygon *copy = new Polygon (*this);

		assert (copy);

		return copy;
	}

	virtual void doPrintType() {
		std::cout << "Polygon" << std::endl;
	}

	virtual void doPrint (const char* name) {
		std::cout << name << " (Polygon)" << std::endl;

		std::cout << "mVerticeCount = " << mVerticeCount << std::endl;
		unsigned int i;
		for (i = 0; i < mVerticeCount; i ++) {
			std::cout << i << " = " << mVertices[i][0] << ", " <<
				mVertices[i][1] << ", " <<
				mVertices[i][2] << std::endl;
		}
		std::cout << "mPosition = " << mPosition[0] <<", " <<
			mPosition[1] << ", " <<
			mPosition[2] << std::endl;

		std::cout << "mVelocity = " << mVelocity[0] <<", " <<
			mVelocity[1] << ", " <<
			mVelocity[2] << std::endl;

		std::cout << "mAngle = " << mAngle << std::endl;
		std::cout << "mAngleVelocity = " << mAngleVelocity << std::endl;
		std::cout << "mBoundingRadius = " << mBoundingRadius << std::endl;
	}

	unsigned int getVerticeCount () {
		return mVerticeCount;
	}

	vector3d& getVertice (unsigned int i) {
		assert (i >= 0 && i < mVerticeCount);
		return mVertices [i];
	}

	void setVertice (unsigned int i, const vector3d &vertice) {
		assert (i >= 0 && i < mVerticeCount);
		mVertices[i] = vertice;

		updateBoundingRadius();
	}

	void updateBoundingRadius () {
		unsigned int i;
		float max_vertice_dist2 = 0.;
		for (i = 0; i < mVerticeCount; i++) {
			float vertice_dist2 = mVertices[i].length2();
			if (vertice_dist2 > max_vertice_dist2)
				max_vertice_dist2 = vertice_dist2;
		}

		mBoundingRadius = sqrt (max_vertice_dist2);
	}

	friend int check_collision_polygon_sphere(Shape *polygon_a,
			Shape *sphere_b, CollisionInfo* info);
};

class Sphere: public Shape {
private:
	float mRadius;
public:
	Sphere (float radius) {
		mRadius = radius;
		mBoundingRadius = radius;
		Shape::setPosition (vector3d (0., 0., 0.));
	}

	Sphere(float radius, const vector3d &position) {
		mRadius = radius;
		mBoundingRadius = radius;
		Shape::setPosition(position);
	}

	Sphere (const Sphere &sphere):
		Shape (sphere),
		mRadius (sphere.mRadius)
		{ mBoundingRadius = mRadius; }

	virtual Sphere* getCopy() {
		Sphere* copy = new Sphere (*this);

		return copy;
	}

	virtual void doPrintType() {
		std::cout << "Sphere" << std::endl;
	}

	virtual void doPrint (const char* name) {
		std::cout << name << " (Sphere)" << std::endl;

		std::cout << "mRadius = " << mRadius << std::endl;
		std::cout << "mPosition = " << mPosition[0] <<", " <<
			mPosition[1] << ", " <<
			mPosition[2] << std::endl;

		std::cout << "mVelocity = " << mVelocity[0] <<", " <<
			mVelocity[1] << ", " <<
			mVelocity[2] << std::endl;

		std::cout << "mAngle = " << mAngle << std::endl;
		std::cout << "mAngleVelocity = " << mAngleVelocity << std::endl;
	}

	void setRadius (float radius) {
		mRadius = radius;
	}

	float getRadius () {
		return mRadius;
	}

	friend int check_collision_polygon_sphere(Shape *polygon_a,
			Shape *sphere_b, CollisionInfo* info);
};

/** \brief The higher level function to call for collision detection
 *
 * \param stepsize   the timestep within we want to check for the collision 
 * \param shape_a    first shape
 * \param shape_b    second shape
 * \param info       information about the collision will be to info
 *
 * \returns 0 - no collision, negative on error, positive on collision
 */
int check_collision(float timestep, Shape *shape_a, Shape *shape_b, CollisionInfo* info);

/** \brief The higher level function to call for collision detection
 *
 * \param shape_a    first shape
 * \param shape_b    second shape
 * \param velocity_b relative velocity of b to a
 * \param info       information about the collision will be to info
 *
 * \returns 0 - no collision, negative on error, positive on collision
 */
int check_collision_rel(float timestep, Shape *shape_a, Shape *shape_b, vector3d *velocity_b,
		CollisionInfo* info);

/** \brief Callback signature for the check functions */
typedef int (*check_cb)(float timestep, Shape *shape_a, Shape *shape_b,	CollisionInfo* info);

/** \brief Returns the check functions which performs the checks depending
 * on their types. */
check_cb get_check(Shape *shape_a, Shape *shape_b);

/** \brief Performs a check between a polygon and a sphere
 */
int check_collision_polygon_sphere(float timestep, Shape *shape_a, Shape *shape_b,
		CollisionInfo* info);

/** \brief Performs a check between a sphere and a sphere
 */
int check_collision_sphere_sphere(float timestep, Shape *shape_a, Shape *shape_b,
		CollisionInfo* info);


/** \brief Calculates the time it takes for a point to touch a plane
 *
 * \param normal   normal vector of the plane
 * \param plane_point point on the vector
 * \param point    point that is moving
 * \param velocity velocity of the point
 *
 * \returns -1 if point is moving away from the plane (or is below and
 *                moves even further below
 *          >= 0 if point is moving along the plane or towards the plane
 *
 * This function depends on the scale of velocity. It is assumed
 * that velocity represents the displacement in one frame. In this
 * case a return value > 1 means there will not be a contact
 * within this frame.
 */
float calculate_contact_plane_point(vector3d &normal, vector3d &plane_point, vector3d &point, vector3d &velocity);

}

#endif /* _COLL2D_H */