#ifndef __FASTMATH_H__
#define __FASTMATH_H__

#include <cassert>
#include <cmath>
using namespace std;

//Si vous definissez la macro NFASTMATH, le code "fast" n'est pas appelle, remplace
//par un code normal (cos, sin...)
//If you define the macro NFASTMATH, the fast code is not called, replaced by
//normal code (cos, sin...)

//You can also define NDEBUG to remove assert() if the code works

#define FASTMATH_VERSION 1

namespace FastMath
{

/*useful prototypes

template <typename T> T sqr(T x);
template <typename T> T abs(T x);
float fastInvSqrt(float);
float fastSqrt(float);
float fastSin(float);
float fastCos(float);
float fastTan(float);
float fastASin(float);
float fastACos(float);
float fastATan(float);
float fastExp(float);
float fastCosh(float);
float fastSinh(float);
float fastTanh(float);

double fastInvSqrt(double);
double fastSqrt(double);
double fastSin(double);
double fastCos(double);
double fastTan(double);
double fastASin(double);
double fastACos(double);
double fastATan(double);
double fastExp(double);
double fastCosh(double);
double fastSinh(double);
double fastTanh(double);
  
*/

const float M_LN2_f              = 0.69314718055994530941723212145818f;

const float TWO_PI_f             = 6.283185307179586476925286766559f;
const float PI_f                 = 3.1415926535897932384626433832795f;
const float HALF_PI_f            = 1.5707963267948966192313216916398f;
const float THIRD_PI_f           = 1.0471975511965977461542144610932f;
const float QUARTER_PI_f         = 0.78539816339744830961566084581988f;
const float QUARTER_QUARTER_PI_f = 0.19634954084936207740391521145497f;

const float INV_TWO_PI_f         = 0.15915494309189533576888376337251f;
const float INV_PI_f             = 0.31830988618379067153776752674503f;
const float INV_HALF_PI_f        = 0.63661977236758134307553505349006f;
const float INV_THIRD_PI_f       = 0.95492965855137201461330258023509f;
const float INV_QUARTER_PI_f     = 1.2732395447351626861510701069801f;

const float DEG_TO_RAD_f         = 0.017453292519943295769236907684886f;
const float RAD_TO_DEG_f         = 57.295779513082320876798154814105f;

const double M_LN2_d              = 0.69314718055994530941723212145818;

const double TWO_PI_d             = 6.283185307179586476925286766559;
const double PI_d                 = 3.1415926535897932384626433832795;
const double HALF_PI_d            = 1.5707963267948966192313216916398;
const double THIRD_PI_d           = 1.0471975511965977461542144610932;
const double QUARTER_PI_d         = 0.78539816339744830961566084581988;
const double QUARTER_QUARTER_PI_d = 0.19634954084936207740391521145497;

const double INV_TWO_PI_d         = 0.15915494309189533576888376337251;
const double INV_PI_d             = 0.31830988618379067153776752674503;
const double INV_HALF_PI_d        = 0.63661977236758134307553505349006;
const double INV_THIRD_PI_d       = 0.95492965855137201461330258023509;
const double INV_QUARTER_PI_d     = 1.2732395447351626861510701069801;

const double DEG_TO_RAD_d         = 0.017453292519943295769236907684886;
const double RAD_TO_DEG_d         = 57.295779513082320876798154814105;

struct EndianInitializer
{
  bool littleEndian;
  EndianInitializer(void)
  {
    #if defined(__LITTLE_ENDIAN__)
      littleEndian = true;
    #elif defined(__BIG_ENDIAN__)
      littleEndian = false;
    #else
      const int i = 1;
      littleEndian = ((reinterpret_cast<const char*>(&i))[0] == 1);
    #endif
  }
};

inline float ConvertHalfPiSin(const float fAngle, bool& bNeg)
{
  int nb = static_cast<int>(fAngle*FastMath::INV_PI_f);
  float fA = fAngle-nb*FastMath::PI_f;
  bNeg = (fA < 0.0f);
  if (bNeg)
    fA = -fA;
  if (nb & 1)//impair
    bNeg = !bNeg;

  return (fA > FastMath::HALF_PI_f) ? (FastMath::PI_f - fA) : fA;
}

inline double ConvertHalfPiSin(const double fAngle, bool& bNeg)
{
  int nb = static_cast<int>(fAngle*FastMath::INV_PI_d);
  double fA = fAngle-nb*FastMath::PI_d;
  bNeg = (fA < 0.0);
  if (bNeg)
    fA = -fA;
  if (nb & 1)//impair
    bNeg = !bNeg;
  
  return (fA > FastMath::HALF_PI_d) ? (FastMath::PI_d - fA) : fA;
}

inline float ConvertHalfPiCos(const float fAngle, bool& bNeg)
{
  float fA = fAngle - (static_cast<int>(fAngle*FastMath::INV_TWO_PI_f) * FastMath::TWO_PI_f);
	
  if (fA < 0.0f)
    fA = -fA;
  if (fA > FastMath::PI_d)
    fA = FastMath::TWO_PI_f-fA;
  return (bNeg = (fA > FastMath::HALF_PI_f)) ? (FastMath::PI_f - fA) : fA;
}

inline double ConvertHalfPiCos(const double fAngle, bool& bNeg)
{
  double fA = fAngle - (static_cast<int>(fAngle*FastMath::INV_TWO_PI_d) * FastMath::TWO_PI_d);
	
  if (fA < 0.0)
    fA = -fA;
  if (fA > FastMath::PI_d)
    fA = FastMath::TWO_PI_d-fA;
  return (bNeg = (fA > FastMath::HALF_PI_d)) ? (FastMath::PI_d - fA) : fA;
}

//x^2
template <typename T> T sqr(T x) {return x*x;}

// 1/sqrt()
inline float fastInvSqrt(float fX)
{
  #ifdef NFASTMATH
  return 1.f/sqrtf(fX);
  #else
  float fHalf = 0.5f * fX;
  int i = *reinterpret_cast<int*>(&fX);
  i = 0x5f3759df - (i >> 1); // This line hides a LOT of math!
  fX = *reinterpret_cast<float*>(&i);

  // repeat this statement for a better approximation
  fX = fX*(1.5f - fHalf*fX*fX); 
  fX = fX*(1.5f - fHalf*fX*fX);

  return fX;
  #endif
}

inline double fastInvSqrt(double fX)
{
  #ifdef NFASTMATH
  return 1./sqrt(fX);
  #else
  double fHalf = 0.5 * fX;
  float fXf = static_cast<float>(fX);
  int i = *reinterpret_cast<int*>(&fXf);
  i = 0x5f3759df - (i >> 1); // This line hides a LOT of math!
  fX = *reinterpret_cast<float*>(&i);
  
  // repeat this statement for a better approximation
  fX = fX*(1.5 - fHalf*fX*fX); 
  fX = fX*(1.5 - fHalf*fX*fX);
  
  return fX;
  #endif
}

//sqrt(fX)
inline float fastSqrt(float fX)
{
  #ifdef NFASTMATH
  return sqrtf(fX);
  #else
  float tmp = fX;
  float fHalf = 0.5f*fX;
  int i = *reinterpret_cast<int*>(&fX);
  i = 0x5f3759df - (i >> 1); // This line hides a LOT of math!
  fX = *reinterpret_cast<float*>(&i);
  
  // repeat this statement for a better approximation
  fX = fX*(1.5f - fHalf*fX*fX); 
  fX = fX*(1.5f - fHalf*fX*fX);
  return tmp * fX;
  #endif
}

inline double fastSqrt(double fX)
{
  #ifdef NFASTMATH
  return sqrt(fX);
  #else
  double tmp = fX;
  double fHalf = 0.5*fX;
  float  fXf = static_cast<float>(fX);
  int i = *reinterpret_cast<int*>(&fXf);
  i = 0x5f3759df - (i >> 1); // This line hides a LOT of math!
  fX = *reinterpret_cast<float*>(&i);

  // repeat this statement for a better approximation
  fX = fX*(1.5 - fHalf*fX*fX); 
  fX = fX*(1.5 - fHalf*fX*fX);
  return tmp * fX;
  #endif
}

//sin(fAngle)
inline float fastSin(float fAngle)
{
  #ifdef NFASTMATH
  return sin(fAngle);
  #else
  bool bNeg      = false;
  float fA       = ( (fAngle>FastMath::HALF_PI_f) || (fAngle<0.0f) ) ? FastMath::ConvertHalfPiSin(fAngle, bNeg)
                                                                     : fAngle;
  float fASqr    = fA*fA;
  float fResult  = 7.61e-03f;
  fResult       *= fASqr;
  fResult       -= 1.6605e-01f;
  fResult       *= fASqr;
  fResult       += 1.0f;
  fResult       *= fA;

  return bNeg ? -fResult : fResult;
  #endif
}

inline double fastSin(double fAngle)
{
  #ifdef NFASTMATH
  return sin(fAngle);
  #else
  bool bNeg       = false;
  double fA       = ( (fAngle>FastMath::HALF_PI_d) || (fAngle<0.0) ) ? FastMath::ConvertHalfPiSin(fAngle, bNeg)
                                                                     : fAngle;
  double fASqr    = fA*fA;
  double fResult  = 7.61e-03;
  fResult        *= fASqr;
  fResult        -= 1.6605e-01;
  fResult        *= fASqr;
  fResult        += 1.0;
  fResult        *= fA;
  
  return bNeg ? -fResult : fResult;
  #endif
}

//cos(fAngle)
inline float fastCos(float fAngle)
{
  #ifdef NFASTMATH
  return cosf(fAngle);
  #else
  bool bNeg      = false;
  float fA       = ( (fAngle>FastMath::HALF_PI_f) || (fAngle<0.0f) ) ? FastMath::ConvertHalfPiCos(fAngle, bNeg)
                                                                     : fAngle;
  float fASqr    = fA*fA;
  float fResult  = 3.705e-02f;
  fResult       *= fASqr;
  fResult       -= 4.967e-01f;
  fResult       *= fASqr;
  fResult       += 1.0f;
	
  return bNeg ? -fResult : fResult;
  #endif
}

inline double fastCos(double fAngle)
{
  #ifdef NFASTMATH
  return cos(fAngle);
  #else
  bool bNeg       = false;
  double fA       = ( (fAngle>FastMath::HALF_PI_d) || (fAngle<0.0) ) ? FastMath::ConvertHalfPiCos(fAngle, bNeg)
                                                                     : fAngle;
  double fASqr    = fA*fA;
  double fResult  = 3.705e-02;
  fResult        *= fASqr;
  fResult        -= 4.967e-01;
  fResult        *= fASqr;
  fResult        += 1.0;
	
  return bNeg ? -fResult : fResult;
  #endif
}

//tan(fAngle)
inline float fastTan(float fAngle)
{
  #ifdef NFASTMATH
  return tanf(fAngle);
  #else
  if ( (fAngle>FastMath::QUARTER_PI_f) || (fAngle<0.0f) )
    return FastMath::fastSin(fAngle)/FastMath::fastCos(fAngle);

  float fA       = fAngle;
  float fASqr    = fA*fA;
  float fResult  = 2.033e-01f;
  fResult       *= fASqr;
  fResult       += 3.1755e-01f;
  fResult       *= fASqr;
  fResult       += 1.0f;
  fResult       *= fA;
  return fResult;
  #endif
}

inline double fastTan(double fAngle)
{
  #ifdef NFASTMATH
  return tan(fAngle);
  #else
  if ( (fAngle>FastMath::QUARTER_PI_d) || (fAngle<0.0) )
    return FastMath::fastSin(fAngle)/FastMath::fastCos(fAngle);

  double fA       = fAngle;
  double fASqr    = fA*fA;
  double fResult  = 2.033e-01;
  fResult        *= fASqr;
  fResult        += 3.1755e-01;
  fResult        *= fASqr;
  fResult        += 1.0;
  fResult        *= fA;
  return fResult;
  #endif
}

//asin(fValue)
inline float fastASin(float fValue)
{
  #ifdef NFASTMATH
  return asinf(fValue);
  #else
  float fV      = (fValue<0.0f) ? -fValue : fValue;
  float fRoot   = FastMath::fastSqrt(1.0f - fV);
  float fResult = -0.0187293f;

  fResult *= fV;
  fResult += 0.0742610f;
  fResult *= fV;
  fResult -= 0.2121144f;
  fResult *= fV;
  //fResult += 1.5707288f;
  fResult += FastMath::HALF_PI_f;
  fResult  = FastMath::HALF_PI_f - fRoot*fResult;

  return (fValue<0.0f) ? -fResult : fResult;
  #endif
}

inline double fastASin(double fValue)
{
  #ifdef NFASTMATH
  return asin(fValue);
  #else
  double fV      = (fValue<0.0) ? -fValue : fValue;
  double fRoot   = FastMath::fastSqrt(1.0 - fV);
  double fResult = -0.0187293;
  
  fResult *= fV;
  fResult += 0.0742610;
  fResult *= fV;
  fResult -= 0.2121144;
  fResult *= fV;
  //fResult += 1.5707288;
  fResult += FastMath::HALF_PI_d;
  fResult  = FastMath::HALF_PI_d - fRoot*fResult;
  
  return (fValue<0.0) ? -fResult : fResult;
  #endif
}

//acos(fValue)
inline float fastACos(float fValue)
{
  #ifdef NFASTMATH
  return acosf(fValue);
  #else
  float fV      = (fValue<0.0f) ? -fValue : fValue;
  float fRoot   = FastMath::fastSqrt((1.0f)-fV);
  float fResult = -0.0187293f;
  
  fResult *= fV;
  fResult += 0.0742610f;
  fResult *= fV;
  fResult -= 0.2121144f;
  fResult *= fV;
  //fResult += 1.5707288f;
  fResult += FastMath::HALF_PI_f;
  fResult *= fRoot;

  return (fValue<0.0f) ? PI_f-fResult : fResult;
  #endif
}

inline double fastACos(double fValue)
{
  #ifdef NFASTMATH
  return acos(fValue);
  #else
  double fV      = (fValue<0.0) ? -fValue : fValue;
  double fRoot   = FastMath::fastSqrt((1.0)-fV);
  double fResult = -0.0187293;
  
  fResult *= fV;
  fResult += 0.0742610;
  fResult *= fV;
  fResult -= 0.2121144;
  fResult *= fV;
  //fResult += 1.5707288;
  fResult += FastMath::HALF_PI_d;
  fResult *= fRoot;
  
  return (fValue<0.0) ? PI_d-fResult : fResult;
  #endif
}

//atan(fValue)
inline float fastATan(float fValue)
{
  #ifdef NFASTMATH
  return atanf(fValue);
  #else
  if (fValue == 0.0f) return 0.0f;
  
  float fV       = ( (fValue>1.0f) || (fValue<-1.0f) ) ? 1.0f/fValue : fValue;
  float fVSqr    = fV*fV;
  float fResult  = 0.0208351f;
  fResult       *= fVSqr;
  fResult       -= 0.085133f;
  fResult       *= fVSqr;
  fResult       += 0.180141f;
  fResult       *= fVSqr;
  fResult       -= 0.3302995f;
  fResult       *= fVSqr;
  fResult       += 0.999866f;
  fResult       *= fV;
	
  return (fValue>1.0f) ? (FastMath::HALF_PI_f - fResult)
                       : (fValue<-1.0f) ? -(fResult + FastMath::HALF_PI_f)
                                        : fResult;
  #endif
}

inline double fastATan(double fValue)
{
  #ifdef NFASTMATH
  return atan(fValue);
  #else
  if (fValue == 0.0) return 0.0;
  
  double fV       = ( (fValue>1.0) || (fValue<-1.0) ) ? 1.0/fValue : fValue;
  double fVSqr    = fV*fV;
  double fResult  = 0.0208351;
  fResult        *= fVSqr;
  fResult        -= 0.085133;
  fResult        *= fVSqr;
  fResult        += 0.180141;
  fResult        *= fVSqr;
  fResult        -= 0.3302995;
  fResult        *= fVSqr;
  fResult        += 0.999866;
  fResult        *= fV;
	
  return (fValue>1.0) ? (FastMath::HALF_PI_d - fResult)
                      : (fValue<-1.0) ? -(fResult + FastMath::HALF_PI_d)
                                      : fResult;
  #endif
}

inline bool is_little_endian(void)
{
  #if defined(__LITTLE_ENDIAN__)
    return true;
  #elif defined(__BIG_ENDIAN__)
    return false;
  #else
    static EndianInitializer initializer;
    return initializer.littleEndian;
  #endif
}

inline float fastExp(float x)
{
  #ifdef NFASTMATH
  return expf(x);
  #else
  //il faut que sizeof(int) = sizeof(double)/2 = 4
  union
  {
    int i[2];
    double d;
  } n;
  n.d = 0;

  if (!is_little_endian())
    n.i[0] = static_cast<int>((1048576.0f/FastMath::M_LN2_f)*x+(1072693248.0f - 60801.0f));
  else
    n.i[1] = static_cast<int>((1048576.0f/FastMath::M_LN2_f)*x+(1072693248.0f - 60801.0f));

  return n.d;
  #endif
}

inline double fastExp(double x)
{
  #ifdef NFASTMATH
  return exp(x);
  #else
  //il faut que sizeof(int) = sizeof(double)/2 = 4
  assert(sizeof(int) == 4);
  assert(sizeof(int) == sizeof(double)/2);
  union
  {
    int i[2];
    double d;
  } n;
  n.d = 0;

  if (!is_little_endian())
    n.i[0] = static_cast<int>((1048576.0/FastMath::M_LN2_d)*x+(1072693248.0 - 60801.0));
  else
    n.i[1] = static_cast<int>((1048576.0/FastMath::M_LN2_d)*x+(1072693248.0 - 60801.0));

  return n.d;
  #endif
}

inline float fastCosh(float x)
{
  #ifdef NFASTMATH
  return coshf(x);
  #else
  return (fastExp(x)+fastExp(-x))/2.0f;
  #endif
}

inline double fastCosh(double x)
{
  #ifdef NFASTMATH
  return cosh(x);
  #else
  return (fastExp(x)+fastExp(-x))/2.0;
  #endif
}

inline float fastSinh(float x)
{
  #ifdef NFASTMATH
  return sinhf(x);
  #else
  return (fastExp(x)+fastExp(-x))/2.0f;
  #endif
}

inline double fastSinh(double x)
{
  #ifdef NFASTMATH
  return sinh(x);
  #else
  return (fastExp(x)-fastExp(-x))/2.0;
  #endif
}

inline float fastTanh(float x)
{
  #ifdef NFASTMATH
  return tanhf(x);
  #else
  const float ex = fastExp(x);
  const float emx = fastExp(-x);
  return (ex-emx)/(ex+emx);
  #endif
}

inline double fastTanh(double x)
{
  #ifdef NFASTMATH
  return tanh(x);
  #else
  const double ex = fastExp(x);
  const double emx = fastExp(-x);
  return (ex-emx)/(ex+emx);
  #endif
}

}//end namespace FastMath

#endif