/*]  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 <cmath>
#include <cml/cml.h>

#include <SDL/SDL_opengl.h>

#if HAVE_CONFIG_H
#include "config.h"
#endif


#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 Mf {


typedef GLscalar								Scalar;

typedef cml::vector< Scalar, cml::fixed<2> >	Vector2;
typedef cml::vector< Scalar, cml::fixed<3> >	Vector3;
typedef cml::vector< Scalar, cml::fixed<4> >	Vector4;

typedef cml::matrix< Scalar, cml::fixed<2,2>,
		cml::col_basis, cml::col_major >		Matrix2;
typedef cml::matrix< Scalar, cml::fixed<3,3>,
		cml::col_basis, cml::col_major >		Matrix3;
typedef cml::matrix< Scalar, cml::fixed<4,4>,
		cml::col_basis, cml::col_major >		Matrix4;

typedef cml::quaternion< Scalar, cml::fixed<>, cml::vector_first,
		cml::positive_cross >					Quaternion;

typedef cml::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 = 0.0)
{
	return Vector4(vec[0], vec[1], vec[2], extra);
}

inline Vector3 promote(const Vector2& vec, Scalar extra = 0.0)
{
	return Vector3(vec[0], vec[1], extra);
}


template <class R, class P>
inline R convert(const P& p)
{
	return R(p);
}

template <>
inline Vector3 convert<Vector3,Vector4>(const Vector4& vec)
{
	return Vector3(vec[0], vec[1], vec[2]);
}

template <>
inline Vector2 convert<Vector2,Vector3>(const Vector3& vec)
{
	return Vector2(vec[0], vec[1]);
}

template <>
inline Vector4 convert<Vector4,Vector3>(const Vector3& vec)
{
	return Vector4(vec[0], vec[1], vec[2], SCALAR(0.0));
}

template <>
inline Vector3 convert<Vector3,Vector2>(const Vector2& vec)
{
	return Vector3(vec[0], vec[1], SCALAR(0.0));
}

template <class P>
struct cast
{
	cast(const P& p) : param(p) {}
	template <class R>
	operator R() { return convert<R,P>(param); }
private:
	const P& param;
};



const Scalar EPSILON = SCALAR(0.000001);

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

inline bool isEqual(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 getDerivative(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.getDerivative(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 Mf

#endif // _MOOF_MATH_HH_