/*******************************************************************************

 Copyright (c) 2009, Charles McGarvey
 All rights reserved.
 
 Redistribution   and   use  in  source  and  binary  forms,  with  or  without
 modification, are permitted provided that the following conditions are met:
 
   * Redistributions  of  source  code  must retain the above copyright notice,
     this list of conditions and the following disclaimer.
   * Redistributions  in binary form must reproduce the above copyright notice,
     this  list of conditions and the following disclaimer in the documentation
     and/or other materials provided with the distribution.
 
 THIS  SOFTWARE  IS  PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND  ANY  EXPRESS  OR  IMPLIED  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED.  IN  NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 FOR  ANY  DIRECT,  INDIRECT,  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES  (INCLUDING,  BUT  NOT  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 SERVICES;  LOSS  OF  USE,  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 CAUSED  AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 OR  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*******************************************************************************/

#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_

/** vim: set ts=4 sw=4 tw=80: *************************************************/