#ifndef SSC_PHYSICS_H
#define SSC_PHYSICS_H

#include "coord.h"
#include "ode/ode.h"

// The ERP specifies what proportion of the joint error will be fixed during the next
// simulation step. If ERP=0 then no correcting force is applied and the bodies will
// eventually drift apart as the simulation proceeds. If ERP=1 then the simulation will
// attempt to fix all joint error during the next time step. However, setting ERP=1 is
// not recommended, as the joint error will not be completely fixed due to various
// internal approximations. A value of ERP=0.1 to 0.8 is recommended (0.2 is the default).

const dReal ERP = 0.8;
const dReal CFM = 0.000001;

extern void NearCallback(void *data, dGeomID g1, dGeomID g2);

class Environ
{
private:
	friend void NearCallback(void*, dGeomID, dGeomID);
	dWorldID mWorld;
	dSpaceID mSpace;
	dJointGroupID mContactGroup;

	static Environ *mInstance;

	Environ(bool useQuadTree=false)
		: mWorld(0), mSpace(0), mContactGroup(0)
	{
		mWorld = dWorldCreate();
		if (useQuadTree)
		{
			dVector3 center = { 1500, 1500, 0 },
				 extents = {10, 0, 10, 0};
			mSpace = dQuadTreeSpaceCreate(0, center, extents, 7);
		}
		else mSpace = dHashSpaceCreate(0);
		mContactGroup = dJointGroupCreate(0);

		dWorldSetGravity(mWorld, 0, 0, 0);
		dWorldSetERP(mWorld, ERP);
		dWorldSetCFM(mWorld, CFM);
	}

public:
	static Environ *getInstance()
	{
		if (!mInstance)
			mInstance = new Environ;
		return mInstance;
	}


	inline dBodyID newBody() { return dBodyCreate(mWorld); }
	inline dGeomID newSphere(double r) { return dCreateSphere(0, r); }
	inline dGeomID newPlane(double a, double b, double c, double d)
	{
		return dCreatePlane(0, a, b, c, d);
	}
	inline dGeomID newRay(double len) { return dCreateRay(0, len); }

	inline void setSpace(dGeomID g) { dSpaceAdd(mSpace, g); }


	// -------------------------------------------------------------------
	//
	// collision detection
	//
	// -------------------------------------------------------------------

	inline void update(double dt)
	{
		dSpaceCollide(mSpace, 0, NearCallback);
		dWorldStepFast1(mWorld, dt, 5);
		dJointGroupEmpty(mContactGroup);
	}
};

enum CollisionObjectType
{
	COLLISION_WALL,
	COLLISION_SCREENOBJECT,
	COLLISION_RAY
};

struct CollisionData
{
	CollisionData() : type(COLLISION_WALL), ptr(0) {}
	CollisionObjectType type;
	void *ptr;
};

class Collidable
{
protected:
	dGeomID mGeometry;
	CollisionData mCollData;
public:
	Coord3<double> mPosition;
	Collidable() : mGeometry(0) {}

	virtual ~Collidable() { dGeomDestroy(mGeometry); }

	inline const CollisionData* collisionData() { return &mCollData; }

	inline virtual void avoid(Collidable *) {}
	void setData(CollisionObjectType t, void *data)
	{
		mCollData.type = t;
		mCollData.ptr = data;
		dGeomSetData(mGeometry, &mCollData);
	}

	inline virtual bool shouldCollide(Collidable *other) { return true; }
};

class Wall : public Collidable
{
private:
	Environ *mEnviron;
public:
	Wall(double a, double b, double c, double d)
	{
		mEnviron = Environ::getInstance();

		mGeometry = mEnviron->newPlane(a, b, c, d);
		mEnviron->setSpace(mGeometry);
		dGeomSetData(mGeometry, 0);
	}

	virtual ~Wall() {}
};

class Ray : public Collidable
{
private:
	Environ *mEnviron;
	Collidable *mParent;
public:
	Ray(Collidable *p, double length=100)
	{
		mEnviron = Environ::getInstance();
		mGeometry = mEnviron->newRay(length);
		setData(COLLISION_RAY, (void *) p);
		mEnviron->setSpace(mGeometry);
		mParent = p;
	}
	virtual ~Ray() { mParent = 0; }


	inline virtual bool shouldCollide(Collidable *other)
	{
		mParent->avoid(other);
		return false;
	}

	inline void setLength(double l) { dGeomRaySetLength(mGeometry, l); }
	inline double getLength() { return dGeomRayGetLength(mGeometry); }

	inline
	void set(double x, double y, double z,
		 double dx, double dy, double dz)
	{
		dGeomRaySet(mGeometry, x, y, z, dx, dy, dz);
	}
};


class PhysicsObject : public Collidable
{
private:
	dBodyID mBody;
	Environ *mEnviron;

public:
	Coord3<double> mVelocity;

	double mass, radius;
	double speed;
	double direction2D;

	PhysicsObject(double r, double _mass,
		      double x, double y, double z,
		      double fx, double fy, double fz,
		      void *data)
		: mVelocity(fx, fy, fz),
		  mass(_mass), radius(r),
		  speed(hypot(fx, fy)),
		  direction2D(atan2(fx, -fy))
	{
		mPosition.set(x, y, z);
		dMass m;

		mEnviron = Environ::getInstance();

		mBody = mEnviron->newBody();
		dBodySetForce(mBody, fx, fy, fz);
		dBodySetLinearVel(mBody, fx, fy, fz);
		dMassSetSphereTotal(&m, mass, radius);
		dBodySetMass(mBody, &m);

		mGeometry = mEnviron->newSphere(radius);
		mEnviron->setSpace(mGeometry);
		dGeomSetBody(mGeometry, mBody);
		setData(COLLISION_SCREENOBJECT, data);

		setPosition(x, y, z);
	}

	inline void setPosition(double x, double y, double z)
	{
		mPosition.set(x, y, z);
		dBodySetPosition(mBody, x, y, z);
		dGeomSetPosition(mGeometry, x, y, z);
	}

	inline void setVelocity(double x, double y, double z)
	{
		mVelocity.set(x, y, z);
		dBodySetLinearVel(mBody, x, y, z);
	}

	virtual ~PhysicsObject()
	{
		dBodyDestroy(mBody);
	}


	inline void disable() { dBodyDisable(mBody); }
	inline void enable() { dBodyEnable(mBody); }

	virtual inline void sync()
	{
		const dReal *pos = dGeomGetPosition(mGeometry),
		            *vel = dBodyGetLinearVel(mBody);

		mPosition.set(pos[0], pos[1], pos[2]);
		mVelocity.set(vel[0], vel[1], vel[2]);
		speed = mVelocity.length();
		direction2D = atan2(vel[0], -vel[1]);
	}

	inline virtual void accelerate(double fx, double fy, double fz)
	{
		dBodyAddForce(mBody, fx, fy, fz);
	}
};

#endif // SSC_PHYSICS_H