/*
 * Copyright (c) ICG. All rights reserved.
 * See copyright.txt for more information.
 *
 * Institute for Computer Graphics and Vision
 * Graz, University of Technology / Austria
 *
 *
 * This software is distributed WITHOUT ANY WARRANTY; without even
 * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  See the above copyright notices for more information.
 *
 *
 * Project     : MIPItkProjects
 * Module      : Evaluation
 * Class       : $RCSfile:  $
 * Language    : C++
 * Description : 
 *
 * Author     : Martin Urschler
 * EMail      : urschler@icg.tu-graz.ac.at
 * Date       : $Date:  $
 * Version    : $Revision:  $
 * Full Id    : $Id:  $
 *
 */
 

#ifndef __MLMATH_HPP__
#define __MLMATH_HPP__

#ifdef WIN32
#pragma warning(disable:4786)
#endif // WIN32

#include <math.h>

#ifdef PI
#undef PI
#endif

/// Definition of Pi and fractions of PI
const double PI = 4.0*atan(1.0);
const double INV_PI = 1.0 / PI;

const double PI_HALF = 0.5 * PI;
const double THREE_PI_HALF = 1.5 * PI ;
const double TWO_PI = 2.0 * PI;

const double PI_FOURTH = 0.25 * PI;
const double THREE_PI_FOURTH = 0.75 * PI;

const double PI_SIXTH = PI / 6.0;


/// Some often used square roots.
const double SQRT_2 = sqrt(2.0);
const double INV_SQRT_2 = 1.0 / SQRT_2;
const double SQRT_3 = sqrt(3.0); 
const double SQRT_2PI = sqrt(TWO_PI);

const double BIG_DBL = 2.e+38;

/// Precisions of testing double's for equality
const double SMALL_DBL_EPSILON = 1.e-15;
const double DOUBLE_EPSILON = 1.e-12;
const double LARGE_DBL_EPSILON = 1.e-6;



#ifdef SQR
#undef SQR
#endif


/// Squaring template suited for squaring function calls [e.g. SQR(cos(x))]
// Be careful when using it with integer values (especially 16 bit integers), 
// there won't be any warnings if there is an overflow
template<typename Num>
inline Num SQR( Num a )
{
   return a*a;
}

/// Cubing macro suited for cubing function calls [e.g. TRI(cos(x))]
// Be careful when using it with integer values(especially 16 bit integers), 
// there won't be any warnings if there is an overflow
template<typename Num>
inline Num TRI( Num a )
{
   return a*a*a;
}



/// Convert radians angle to degree.
inline double RAD_TO_DEG( double rad_angle )
{
   return (rad_angle * 180.0 * INV_PI); 
}

/// Convert degree angle to radians.
inline double DEG_TO_RAD( double deg_angle )
{
   return ((deg_angle * PI) / 180.0); 
}


/// Returns the sign value (+1 or -1) of \c a
template<class C>
inline int GSGN(C a)
{
   return (a < 0) ? -1 : 1;
}

// Returns +/- a depending on the sign of b 
template<class C>
inline C GSGN(C a, C b)
{
   return (b >= 0 ? fabs(a) : -fabs(a));
}


inline double PYTHAG(double a, double b)
{
   double absa = fabs(a);
   double absb = fabs(b);

   if (absa > absb) 
      return absa*sqrt(1.0+SQR<double>(absb/absa));
   else 
      return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR<double>(absa/absb)));
}

/// Returns the absolute value of \c a
template<class C>
inline C GABS(C a)
{  
   return (a < 0) ? -a : a;
}

/// Returns the larger value of \c a and \c b
template<class C>
inline C GMAX(C a, C b)
{
   return (a > b) ? a : b;
}

/// Returns the smaller value of \c a and \c b
template<class C>
inline C GMIN(C a, C b)
{
   return (a < b) ? a : b;
}

/// Returns if an unsigned integer is a power of two
inline bool isPowerOfTwo( unsigned long n )
{
   return ( (n&(n-1)) == 0 );
}

#ifdef SWAP
#undef SWAP
#endif

/// Swap two elements.
template<class C> 
inline void SWAP(C& a, C& b) 
{
   C temp = a; 
   a = b; 
   b = temp;
}


// our rounding strategy is "round-half-up", we round half values x.5 always
// to the larger value independent of its sign (ex: 1.5 -> 2; -1.5 -> 1)

/// Round a double number to specified datatype
template<class C>
inline C ROUND_DBL( double d )
{
   // complicated expression is to make sure that -x.5 is rounded to -x
   // not to -(x+1) like is the case with 'static_cast<C>(d-0.5)'
   
   return (d >= 0.0) ? static_cast<C>(d+0.5) :
      static_cast<C>( d - (static_cast<C>(d)-1) + 0.5 ) + (static_cast<C>(d)-1);
}

/// Round a float number to specified datatype
template<class C>
inline C ROUND_FLT( float d )
{
   // complicated expression is to make sure that -x.5 is rounded to -x
   // not to -(x+1) like is the case with 'static_cast<C>(d-0.5)'
   
   return (d >= 0.0) ? static_cast<C>(d+0.5f) :
      static_cast<C>( d - (static_cast<C>(d)-1) + 0.5f ) + (static_cast<C>(d)-1);
}



inline double NON_AMBIGUOUS_ATAN(double y, double x)
{
   double result = atan2(y, x);   // angle 'result' lies between [-PI...PI]

   // correct ambiguous angles (angle 'result' now lies between [-PI/2...PI/2])
   if (result > PI_HALF) 
      result -= PI;
   else if (result < -PI_HALF)
      result += PI;

   return result;
}


/** Compares two {\tt double} numbers for equality using {\tt DBL_EPSILON}, 
*  while taking into account the order of magnitude of the numbers.
*/
inline bool IS_EQUAL(double a, double b) 
{
   double max = fabs(b);
   if (fabs(a) > max) 
      max = fabs(a);
   if (max > DOUBLE_EPSILON)
      return (fabs(a-b)/max) < DOUBLE_EPSILON;
   else
      return true;
}

/// Checks a {\tt double} number on zero using {\tt DBL_EPSILON}.
inline bool IS_ZERO(double a) 
{
   return(fabs(a) < DOUBLE_EPSILON);
}



/// Functions to calculate values of the gaussian distribution and its first
/// and second derivations.
inline double GAUSSIAN_1D( double x, double sigma )
{
   return ( exp( -(x*x)/(2.0*sigma*sigma) ) / ( SQRT_2PI*sigma ) );
}

inline double GAUSSIAN_1D_1ST_DERIV( double x, double sigma )
{
   return ( -x*exp( -(x*x)/(2.0*sigma*sigma) ) / ( SQRT_2PI*sigma*sigma*sigma ) );
}

inline double GAUSSIAN_1D_2ND_DERIV( double x, double sigma )
{
   return( ((x*x)-(sigma*sigma))*exp( -(x*x)/(2.0*sigma*sigma) ) / ( SQRT_2PI*pow(sigma,5) ) );
}


inline void
CALCULATE_ARBITRARY_3D_NORMAL_VECTOR(const float v[3], float n[3])
{
   unsigned int argmax = 0;
   if (fabsf(v[1]) > fabsf(v[0]))      argmax = 1;
   if (fabsf(v[2]) > fabsf(v[argmax])) argmax = 2;

   switch (argmax)
   {
      case 0:
         n[1] = n[2] = 1.0f;
         n[0] = (-v[1] - v[2]) / v[0];
         break;
      case 1:
         n[0] = n[2] = 1.0f;
         n[1] = (-v[0] - v[2]) / v[1];
         break;
      case 2:
         n[0] = n[1] = 1.0f;
         n[2] = (-v[0] - v[1]) / v[2];
         break;
   }
}

inline void
CALCULATE_ARBITRARY_3D_NORMAL_VECTOR(const double v[3], double n[3])
{
   unsigned int argmax = 0;
   if (fabs(v[1]) > fabs(v[0]))      argmax = 1;
   if (fabs(v[2]) > fabs(v[argmax])) argmax = 2;

   switch (argmax)
   {
      case 0:
         n[1] = n[2] = 1.0;
         n[0] = (-v[1] - v[2]) / v[0];
         break;
      case 1:
         n[0] = n[2] = 1.0;
         n[1] = (-v[0] - v[2]) / v[1];
         break;
      case 2:
         n[0] = n[1] = 1.0;
         n[2] = (-v[0] - v[1]) / v[2];
         break;
   }
}



template <typename Num>
inline void
CROSS_PRODUCT(const Num v[3], const Num w[3], Num result[3])
{
   result[0] = v[1]*w[2] - v[2]*w[1];
   result[1] = v[2]*w[0] - v[0]*w[2];
   result[2] = v[0]*w[1] - v[1]*w[0];
}

template< typename Num >
inline Num
LENGTH_3D( const Num v[3] )
{
   return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}

template< typename Num >
inline Num
SQUARED_LENGTH_3D( const Num v[3] )
{
   return (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}


#endif // __MLMATH_HPP__