/*]  Copyright (c) 2009-2010, Charles McGarvey  [**************************
**]  All rights reserved.
*
* vi:ts=4 sw=4 tw=75
*
* Distributable under the terms and conditions of the 2-clause BSD license;
* see the file COPYING for a complete text of the license.
*
**************************************************************************/

#ifndef _MOOF_RIGIDBODY_HH_
#define _MOOF_RIGIDBODY_HH_

#include <vector>

#include <boost/bind.hpp>
#include <boost/function.hpp>

#include <Moof/Entity.hh>
#include <Moof/Math.hh>


namespace Mf {


template <int D = 3>
struct LinearState
{
	typedef cml::vector< Scalar, cml::fixed<D> >	Vector;
	typedef boost::function<const Vector& (const LinearState&)>
													ForceFunction;

	// primary
	
	Vector		position;
	Vector		momentum;

	// secondary

	Vector		velocity;

	// user

	Vector		force;
	std::vector<ForceFunction> forces;

	// constant
	
	Scalar		mass;
	Scalar		inverseMass;


	void recalculateLinear()
	{
		velocity = momentum * inverseMass;
	}


	struct GravityForce
	{
		explicit GravityForce(Scalar a = -9.8)
		{
			force.zero();
			acceleration = a;
		}

		const Vector& operator () (const LinearState& state)
		{
			force[1] = state.mass * acceleration;
			return force;
		}

	private:

		Vector force;
		Scalar acceleration;
	};


	void init()
	{
		position.zero();
		momentum.zero();

		velocity.zero();

		force.zero();
		forces.clear();

		mass = SCALAR(1.0);
		inverseMass = 1.0 / mass;
	}


	struct Derivative
	{
		Vector	velocity;
		Vector	force;

		Derivative operator*(Scalar dt) const
		{
			Derivative derivative;
			derivative.velocity = dt * velocity;
			derivative.force = dt * force;
			return derivative;
		}

		Derivative operator+(const Derivative& other) const
		{
			Derivative derivative;
			derivative.velocity = velocity + other.velocity;
			derivative.force = force + other.force;
			return derivative;
		}
	};


	Vector getForce() const
	{
		Vector f(force);

		for (size_t i = 0; i < forces.size(); ++i)
		{
			f += forces[i](*this);
		}

		return f;
	}

	void getDerivative(Derivative& derivative, Scalar t) const
	{
		derivative.velocity = velocity;
		derivative.force = getForce();
	}

	void step(const Derivative& derivative, Scalar dt)
	{
		position += dt * derivative.velocity;
		momentum += dt * derivative.force;
		recalculateLinear();
	}
};


struct RotationalState2
{
	// primary

	Scalar		orientation;
	Scalar		angularMomentum;

	// secondary

	Scalar		angularVelocity;

	// constant

	Scalar		inertia;
	Scalar		inverseInertia;


	void recalculateRotational()
	{
		angularVelocity = angularMomentum * inertia;
	}


	struct Derivative
	{
		Scalar	angularVelocity;
		Scalar	torque;
	};

	void step(const Derivative& derivative, Scalar dt)
	{
		orientation += dt * derivative.angularVelocity;
		angularMomentum += dt * derivative.torque;
		recalculateRotational();
	}
};

struct RotationalState3
{
	// primary

	Quaternion	orientation;
	Vector3		angularMomentum;

	// secondary

	Quaternion	spin;
	Vector3		angularVelocity;

	// constant

	Scalar		inertia;
	Scalar		inverseInertia;


	void recalculateRotational()
	{
		angularVelocity = angularMomentum * inertia;
	}
};


struct State2 : public LinearState<2>, public RotationalState2
{
	void recalculate()
	{
		recalculateLinear();
		recalculateRotational();
	}

	void update(Scalar t, Scalar dt)
	{
		rk4<LinearState<2>,LinearState<2>::Derivative>(*this, t, dt);
	}
};

struct State3 : public LinearState<3>, public RotationalState3
{
	void recalculate()
	{
		recalculateLinear();
		recalculateRotational();
	}

	void update(Scalar t, Scalar dt)
	{
		rk4<LinearState<3>,LinearState<3>::Derivative>(*this, t, dt);
	}
};


template <class T>
inline T interpolate(const T& a, const T& b, Scalar alpha)
{
	return cml::lerp(a, b, alpha);
}

template <>
inline State2 interpolate<State2>(const State2& a, const State2& b,
								  Scalar alpha)
{
	State2 state(b);
	state.position = interpolate(a.position, b.position, alpha);
	state.momentum = interpolate(a.momentum, b.momentum, alpha);
	state.orientation = interpolate(a.orientation, b.orientation, alpha);
	state.angularMomentum = interpolate(a.angularMomentum,
										b.angularMomentum, alpha);
	return state;
}

template <>
inline State3 interpolate<State3>(const State3& a, const State3& b,
								  Scalar alpha)
{
	State3 state(b);
	state.position = interpolate(a.position, b.position, alpha);
	state.momentum = interpolate(a.momentum, b.momentum, alpha);
	state.orientation = cml::slerp(a.orientation, b.orientation, alpha);
	state.angularMomentum = interpolate(a.angularMomentum,
										b.angularMomentum, alpha);
	return state;
}



/**
 * Interface for anything that can move.
 */

template <class T>
class RigidBody : public Entity
{
protected:

	T	mState;
	T	mPrevState;

public:

	virtual ~RigidBody() {}

	virtual void update(Scalar t, Scalar dt)
	{
		mPrevState = mState;
		mState.update(t, dt);
	}

	const T& getState() const
	{
		return mState;
	}

	T getState(Scalar alpha) const
	{
		return interpolate(mPrevState, mState, alpha);
	}

	const T& getLastState() const
	{
		return mPrevState;
	}
};

typedef RigidBody<State2> RigidBody2;
typedef RigidBody<State3> RigidBody3;


} // namespace Mf

#endif // _MOOF_RIGIDBODY_HH_