/*]  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_MATH_HH_
#define _MOOF_MATH_HH_

/**
 * \file math.hh
 * General math-related types and functions.
 */

#include "../config.h"

#include <cmath>

#include <cml/cml.h>
#include <SDL/SDL_opengl.h>


#if USE_DOUBLE_PRECISION
typedef GLdouble	GLscalar;
#define GL_SCALAR	GL_DOUBLE
#define SCALAR(D)	(D)
#else
typedef GLfloat		GLscalar;
#define GL_SCALAR	GL_FLOAT
#define SCALAR(F)	(F##f)
#endif


namespace moof {


using namespace cml;


typedef GLscalar scalar;

typedef vector< scalar, fixed<2> > vector2;
typedef vector< scalar, fixed<3> > vector3;
typedef vector< scalar, fixed<4> > vector4;

typedef matrix< scalar, fixed<2,2>, col_basis, col_major > matrix2;
typedef matrix< scalar, fixed<3,3>, col_basis, col_major > matrix3;
typedef matrix< scalar, fixed<4,4>, col_basis, col_major > matrix4;

typedef quaternion< scalar, fixed<>, vector_first, positive_cross >	quaternion;

typedef constants<scalar> constants;


inline vector3 demote(const vector4& vec)
{
	return vector3(vec[0], vec[1], vec[2]);
}

inline vector2 demote(const vector3& vec)
{
	return vector2(vec[0], vec[1]);
}

inline vector4 promote(const vector3& vec, scalar extra = SCALAR(0.0))
{
	return vector4(vec[0], vec[1], vec[2], extra);
}

inline vector3 promote(const vector2& vec, scalar extra = SCALAR(0.0))
{
	return vector3(vec[0], vec[1], extra);
}


const scalar EPSILON = SCALAR(0.000001);

/**
 * Check the equality of scalars with a certain degree of error allowed.
 */

inline bool is_equal(scalar a, scalar b, scalar epsilon = EPSILON)
{
	return std::abs(a - b) < epsilon;
}


// Here are some generic implementations of a few simple integrators.  To
// use, you need one type representing the state and another containing the
// derivatives of the primary state variables.  The state class must
// implement these methods:
//
// void calculate_derivative(Derivative_Type& derivative, scalar absoluteTime);
// void step(const Derivative_Type& derivative, scalar deltaTime);
//
// Additionally, the derivative class must overload a few operators:
//
// Derivative_Type operator + (const Derivative_Type& other) const
// Derivative_Type operator * (const Derivative_Type& other) const

template <class S, class D>
inline D evaluate(const S& state, scalar t)
{
	D derivative;
	state.calculate_derivative(derivative, t);
	return derivative;
}

template <class S, class D>
inline D evaluate(S state,  scalar t, scalar dt, const D& derivative)
{
	state.step(derivative, dt);
	return evaluate<S,D>(state, t + dt);
}


template <class S, class D>
inline void euler(S& state, scalar t, scalar dt)
{
	D a = evaluate<S,D>(state, t);

	state.step(a, dt);
}

template <class S, class D>
inline void rk2(S& state, scalar t, scalar dt)
{
	D a = evaluate<S,D>(state, t);
	D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);

	state.step(b, dt);
}

template <class S, class D>
inline void rk4(S& state, scalar t, scalar dt)
{
	D a = evaluate<S,D>(state, t);
	D b = evaluate<S,D>(state, t, dt * SCALAR(0.5), a);
	D c = evaluate<S,D>(state, t, dt * SCALAR(0.5), b);
	D d = evaluate<S,D>(state, t, dt, c);

	state.step((a + (b + c) * SCALAR(2.0) + d) * SCALAR(1.0/6.0), dt);
}


} // namespace moof

#endif // _MOOF_MATH_HH_