/*]  Copyright (c) 2009-2011, Charles McGarvey  [*****************************
**]  All rights reserved.
*
* 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_RIGID_BODY_HH_
#define _MOOF_RIGID_BODY_HH_

#include <vector>

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

#include <moof/entity.hh>
#include <moof/math.hh>


/**
 * \file rigid_body.hh
 * Classes and functions for simulating rigid body physics.
 */

namespace moof {


extern scalar global_acceleration;

template <int D>
struct linear_state
{
	typedef moof::vector< scalar, fixed<D> > vector;
	typedef boost::function<const vector& (const linear_state&)>
		force_function;

	// primary
	vector		position;
	vector		momentum;

	// secondary
	vector		velocity;

	// user
	vector		force;
	std::vector<force_function> forces;

	// constant
	scalar		mass;
	scalar		inverse_mass;

	void recalculate()
	{
		velocity = momentum * inverse_mass;
	}

	struct gravity_force
	{
		gravity_force()
		{
			force.zero();
		}

		const vector& operator () (const linear_state& state)
		{
			force[1] = state.mass * global_acceleration;
			return force;
		}

	private:

		vector	force;
	};

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

		velocity.zero();

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

		mass = SCALAR(1.0);
		inverse_mass = SCALAR(1.0);
	}

	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 total_force() const
	{
		vector f(force);

		typename std::vector<force_function>::const_iterator it;
		for (it = forces.begin(); it != forces.end(); ++it)
		{
			f += (*it)(*this);
		}

		return f;
	}

	void calculate_derivative(derivative& derivative, scalar t) const
	{
		derivative.velocity = velocity;
		derivative.force = total_force();
	}

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

struct rotational_state2
{
	// primary
	scalar		orientation;
	scalar		angular_momentum;

	// secondary
	scalar		angular_velocity;

	// constant
	scalar		inertia;
	scalar		inverse_inertia;

	void recalculate()
	{
		angular_velocity = angular_momentum * inertia;
	}

	struct derivative
	{
		scalar	angular_velocity;
		scalar	torque;
	};

	void step(const derivative& derivative, scalar dt)
	{
		orientation += dt * derivative.angular_velocity;
		angular_momentum += dt * derivative.torque;
		recalculate();
	}
};

struct rotational_state3
{
	// primary
	quaternion	orientation;
	vector3		angular_momentum;

	// secondary
	quaternion	spin;
	vector3		angular_velocity;

	// constant
	scalar		inertia;
	scalar		inverse_inertia;

	void recalculate()
	{
		angular_velocity = angular_momentum * inertia;
	}
};


struct state2 : public linear_state<2>, public rotational_state2
{
	void recalculate()
	{
		linear_state<2>::recalculate();
		rotational_state2::recalculate();
	}

	void update(scalar t, scalar dt)
	{
		rk4<linear_state<2>,linear_state<2>::derivative>(*this, t, dt);
	}
};

struct state3 : public linear_state<3>, public rotational_state3
{
	void recalculate()
	{
		linear_state<3>::recalculate();
		rotational_state3::recalculate();
	}

	void update(scalar t, scalar dt)
	{
		rk4<linear_state<3>,linear_state<3>::derivative>(*this, t, dt);
	}
};

template <class T>
inline T interpolate(const T& a, const T& b, scalar alpha)
{
	return 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.angular_momentum = interpolate(a.angular_momentum,
			b.angular_momentum, 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 = slerp(a.orientation, b.orientation, alpha);
	state.angular_momentum = interpolate(a.angular_momentum,
			b.angular_momentum, alpha);
	return state;
}

/**
 * Interface for anything that can move.
 */
template <class T>
class rigid_body : public entity
{
public:

	virtual ~rigid_body() {}

	virtual void update(scalar t, scalar dt)
	{
		prev_state_ = state_;
		state_.update(t, dt);
	}

	const T& state() const
	{
		return state_;
	}

	T state(scalar alpha) const
	{
		return interpolate(prev_state_, state_, alpha);
	}

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

protected:

	T	state_;
	T	prev_state_;
};

typedef rigid_body<state2> rigid_body2;
typedef rigid_body<state3> rigid_body3;


} // namespace moof

#endif // _MOOF_RIGID_BODY_HH_