/**
 * @file
 * @brief Globally used functions
 * @author Kai Lingemann. Institute of Computer Science, University of Osnabrueck, Germany.
 * @author Andreas Nuechter. Institute of Computer Science, University of Osnabrueck, Germany.
 */

#ifndef __GLOBALS_ICC__
#define __GLOBALS_ICC__

#ifdef _MSC_VER
#include <windows.h>
#define  _USE_MATH_DEFINES
#include <math.h>

inline int gettimeofday (struct timeval* tp, void* tzp) 
{
  unsigned long t;
  t = timeGetTime();
  tp->tv_sec = t / 1000;
  tp->tv_usec = t % 1000;
  return 0;      /* 0 indicates success. */
}
#else
#include <sys/time.h>
#endif

#define  _USE_MATH_DEFINES
#include <math.h>

#if defined(__CYGWIN__)
# ifndef M_PI
#  define M_PI		3.14159265358979323846
#  define M_PI_2	1.57079632679489661923
#  define M_PI_4	0.78539816339744830962
#  define M_1_PI	0.31830988618379067154
#  define M_2_PI	0.63661977236758134308
#  define M_SQRT2	1.41421356237309504880
#  define M_SQRT1_2	0.70710678118654752440
# endif
#endif

#include <algorithm>
using std::min;
using std::max;
#include <cmath>
#include <sstream>
using std::stringstream;
#include <fstream>
using std::ostream;
using std::istream;
#include <iostream>
using std::cout;
using std::endl;
#include <iomanip>
#include <stdexcept>
using std::runtime_error;

/**
 * Set bits count
 *
 * @param unsigned char x 
 *
 * @return char 
 *
 */
inline unsigned char _my_popcount_3(unsigned char x) {
  x -= (x >> 1) & 0x55;                //put count of each 2 bits into those 2 bits
  x = (x & 0x33) + ((x >> 2) & 0x33);  //put count of each 4 bits into those 4 bits 
  x = (x + (x >> 4)) & 0x0f;           //put count of each 8 bits into those 8 bits 
  return x;
}

/**
 * Converts a class T to a string of width with padding 0
 *
 * @param t output
 * @param width length
 *
 * @return string of t
 *
 */
template <class T>
inline std::string to_string(const T& t, int width)
{
  stringstream ss;
  ss << std::setfill('0') << std::setw(width) << t;
  return ss.str();
}

/**
 * Converts a class T to a string of width with padding 0
 *
 * @param t output
 * @return string of t
 *
 */
template <class T>
inline std::string to_string(const T& t)
{
  stringstream ss;
  ss << t;
  return ss.str();
}


/**
 * Overridden "<<" operator for sending a (4x4)-matrix to a stream
 *
 * @param os      stream
 * @param matrix  4x4 matrix sent to stream
 * @return stream
 */
inline ostream& operator<<(ostream& os, const double matrix[16])
{
  for (int i = 0; i < 16; os << matrix[i++] << " ");
  return os;
}

/**
 * Overridden ">>" operator for reading a (4x4)-matrix from a stream.<br>
 * Throws a runtime error if not enough data in the stream.
 *
 * @param is      stream
 * @param matrix  4x4 matrix sent to stream
 * @return stream
*/
inline istream& operator>>(istream& is, double matrix[16])
{
  for (int i = 0; i < 16; i++) {
    if (!is.good()) throw runtime_error("Not enough elements to read for >>(istream&, double[16])");
    is >> matrix[i];
  }
  return is;
}


/**
 * Converts an angle (given in deg) to rad
 *
 * @param deg integer indicating, whether the figure to be drawn to show
 * the clusters should be circles (0) or rectangles(1)
 *
 * @return the clustered image, with the clusters marked by colored figures
 *
 */
template <class T>
inline T rad(const T deg)
{
  return ( (2 * M_PI * deg) / 360 );
}

/**
 * Converts an angle (given in rad) to deg
 *
 * @param rad  angle in rad
 * @return     angle in deg
 */
template <class T>
inline T deg(const T rad)
{
  return ( (rad * 360) / (2 * M_PI) ); 
}


/**
 *  Calculates x^2
 *
 * @param   x  input scalar value
 * @return  squared value
 */
template <class T>
static inline T sqr(const T &x)
{
  return x*x;
}


/**
 * Computes the <i>squared</i> length of a 3-vector
 *
 * @param x   input 3-vector
 * @return    length^2 of vector 
 */
template <class T>
inline T Len2(const T *x)
{
  return sqr(x[0]) + sqr(x[1]) + sqr(x[2]);
}


/**
 * Computes the length of a 3-vector
 *
 * @param x   input 3-vector
 * @return    length of vector
 */
template <class T>
inline T Len(const T *x)
{
  return sqrt(Len2(x));
}


/**
 * Computes the <i>squared</i> Eucledian distance between two points
 * in 3-space
 *
 * @param x1   first input vector
 * @param x2   decond input vecotr
 * @return  Eucledian distance^2 between the two locations
 */
template <class T, class F>
inline T Dist2(const T *x1, const F *x2)
{
  T dx = x2[0] - x1[0];
  T dy = x2[1] - x1[1];
  T dz = x2[2] - x1[2];

  return sqr(dx) + sqr(dy) + sqr(dz);
}

/*
 * Normalization of the input 3-vector
 *
 * @param x  input/output 3-vector
 */
template <class T>
static inline void Normalize3(T *x)
{
  T norm = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
  x[0] /= norm;
  x[1] /= norm;
  x[2] /= norm;
}

/*
 * Normalization of the input 4-vector
 *
 * @param x  input/output 4-vector
 */
template <class T>
static inline void Normalize4(T *x)
{
  T norm = sqrt((x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3]));
  
  x[0] /= norm;
  x[1] /= norm;
  x[2] /= norm;
  x[3] /= norm;
}

/**
 * Sets a 4x4 matrix to identity
 *
 * @param M 4x4 matrix
 */
template <class T>
inline void M4identity( T *M )
{
  M[0] = M[5] = M[10] = M[15] = 1.0;
  M[1] = M[2] = M[3] = M[4] = M[6] = M[7] = M[8] = M[9] = M[11] = M[12] = M[13] = M[14] = 0.0;
}

/**
 *   Multiplies a 4x4 matrices in OpenGL
 *   (column-major) order
 *
 *  @param M1   first input matrix 
 *  @param M2   second input matrix
 *  @param Mout output matrix
 *
 */
template <class T>
inline void MMult(const T *M1,
			   const T *M2,
			   T *Mout)
{
  Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3];
  Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3];
  Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3];
  Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3];
  Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7];
  Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7];
  Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7];
  Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7];
  Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11];
  Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11];
  Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11];
  Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11];
  Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15];
  Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15];
  Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15];
  Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15];
}

template <class T>
inline void MMult(const T *M1,
			   const T *M2,
			   float *Mout)
{
  Mout[ 0] = M1[ 0]*M2[ 0]+M1[ 4]*M2[ 1]+M1[ 8]*M2[ 2]+M1[12]*M2[ 3];
  Mout[ 1] = M1[ 1]*M2[ 0]+M1[ 5]*M2[ 1]+M1[ 9]*M2[ 2]+M1[13]*M2[ 3];
  Mout[ 2] = M1[ 2]*M2[ 0]+M1[ 6]*M2[ 1]+M1[10]*M2[ 2]+M1[14]*M2[ 3];
  Mout[ 3] = M1[ 3]*M2[ 0]+M1[ 7]*M2[ 1]+M1[11]*M2[ 2]+M1[15]*M2[ 3];
  Mout[ 4] = M1[ 0]*M2[ 4]+M1[ 4]*M2[ 5]+M1[ 8]*M2[ 6]+M1[12]*M2[ 7];
  Mout[ 5] = M1[ 1]*M2[ 4]+M1[ 5]*M2[ 5]+M1[ 9]*M2[ 6]+M1[13]*M2[ 7];
  Mout[ 6] = M1[ 2]*M2[ 4]+M1[ 6]*M2[ 5]+M1[10]*M2[ 6]+M1[14]*M2[ 7];
  Mout[ 7] = M1[ 3]*M2[ 4]+M1[ 7]*M2[ 5]+M1[11]*M2[ 6]+M1[15]*M2[ 7];
  Mout[ 8] = M1[ 0]*M2[ 8]+M1[ 4]*M2[ 9]+M1[ 8]*M2[10]+M1[12]*M2[11];
  Mout[ 9] = M1[ 1]*M2[ 8]+M1[ 5]*M2[ 9]+M1[ 9]*M2[10]+M1[13]*M2[11];
  Mout[10] = M1[ 2]*M2[ 8]+M1[ 6]*M2[ 9]+M1[10]*M2[10]+M1[14]*M2[11];
  Mout[11] = M1[ 3]*M2[ 8]+M1[ 7]*M2[ 9]+M1[11]*M2[10]+M1[15]*M2[11];
  Mout[12] = M1[ 0]*M2[12]+M1[ 4]*M2[13]+M1[ 8]*M2[14]+M1[12]*M2[15];
  Mout[13] = M1[ 1]*M2[12]+M1[ 5]*M2[13]+M1[ 9]*M2[14]+M1[13]*M2[15];
  Mout[14] = M1[ 2]*M2[12]+M1[ 6]*M2[13]+M1[10]*M2[14]+M1[14]*M2[15];
  Mout[15] = M1[ 3]*M2[12]+M1[ 7]*M2[13]+M1[11]*M2[14]+M1[15]*M2[15];
}

/**
 * Transforms a vector with a matrix (P = M * V)
 * @param M the transformation matrix
 * @param v the initial vector
 * @param p the transformt vector
 */
template <class T>
inline void VTrans(const T *M, const T *V, T *P)
{
    P[0] = M[0] * V[0] + M[4] * V[1] + M[8] * V[2] + M[12];
    P[1] = M[1] * V[0] + M[5] * V[1] + M[9] * V[2] + M[13];
    P[2] = M[2] * V[0] + M[6] * V[1] + M[10] * V[2] + M[14];
}

/**
 * Converts an Euler angle to a 3x3 matrix
 *
 * @param rPosTheta  vector of Euler angles
 * @param alignxf    3x3 matrix corresponding to the Euler angles
 */
inline void EulerToMatrix3(const double *rPosTheta, double *alignxf)
{
  double sx = sin(rPosTheta[0]);
  double cx = cos(rPosTheta[0]);
  double sy = sin(rPosTheta[1]);
  double cy = cos(rPosTheta[1]);
  double sz = sin(rPosTheta[2]);
  double cz = cos(rPosTheta[2]);

  alignxf[0]  = cy*cz;
  alignxf[1]  = sx*sy*cz + cx*sz;
  alignxf[2]  = -cx*sy*cz + sx*sz;
  alignxf[3]  = -cy*sz;
  alignxf[4]  = -sx*sy*sz + cx*cz;
  alignxf[5]  = cx*sy*sz + sx*cz;
  alignxf[6]  = sy;
  alignxf[7]  = -sx*cy;
  alignxf[8] = cx*cy;
}

/**
 * Calculates the determinant of a 3x3 matrix
 *
 * @param M  input 3x3 matrix
 * @return   determinant of input matrix
 */
template <class T>
inline double M3det( const T *M )
{
  double det;
  det = (double)(M[0] * ( M[4]*M[8] - M[7]*M[5] )
			  - M[1] * ( M[3]*M[8] - M[6]*M[5] )
			  + M[2] * ( M[3]*M[7] - M[6]*M[4] ));
  return ( det );
}

/**
 * Inverts a 3x3 matrix
 *
 * @param Min   input 3x3 matrix
 * @param Mout  output 3x3 matrix
 */
template <class T>
inline void M3inv( const T *Min, T *Mout )
{
  double det = M3det( Min );

  if ( fabs( det ) < 0.0005 ) {
    M3identity( Mout );
    return;
  }
  
  Mout[0] =  (double)( Min[4]*Min[8] - Min[5]*Min[7] ) / det;
  Mout[1] =  (double)(-( Min[1]*Min[8] - Min[7]*Min[2] )) / det;
  Mout[2] =  (double)( Min[1]*Min[5] - Min[4]*Min[2] ) / det;

  Mout[3] = (double)(-( Min[3]*Min[8] - Min[5]*Min[6] )) / det;
  Mout[4] = (double)( Min[0]*Min[8] - Min[6]*Min[2] ) / det;
  Mout[5] = (double)(-( Min[0]*Min[5] - Min[3]*Min[2] )) / det;

  Mout[6] = (double) ( Min[3]*Min[7] - Min[6]*Min[4] ) / det;
  Mout[7] = (double)(-( Min[0]*Min[7] - Min[6]*Min[1] )) / det;
  Mout[8] = (double) ( Min[0]*Min[4] - Min[1]*Min[3] ) / det;
}


/**
 * Converts a pose into a RT matrix
 * @param *rPos Pointer to the position (double[3])
 * @param *rPosTheta Pointer to the angles (double[3])
 * @param *alignxf The calculated matrix
 */
inline void EulerToMatrix4(const double *rPos, const double *rPosTheta, double *alignxf)
{
  double sx = sin(rPosTheta[0]);
  double cx = cos(rPosTheta[0]);
  double sy = sin(rPosTheta[1]);
  double cy = cos(rPosTheta[1]);
  double sz = sin(rPosTheta[2]);
  double cz = cos(rPosTheta[2]);

  alignxf[0]  = cy*cz;
  alignxf[1]  = sx*sy*cz + cx*sz;
  alignxf[2]  = -cx*sy*cz + sx*sz;
  alignxf[3]  = 0.0;
  alignxf[4]  = -cy*sz;
  alignxf[5]  = -sx*sy*sz + cx*cz;
  alignxf[6]  = cx*sy*sz + sx*cz;
  alignxf[7]  = 0.0;
  alignxf[8]  = sy;
  alignxf[9]  = -sx*cy;
  alignxf[10] = cx*cy;
  
  alignxf[11] = 0.0;

  alignxf[12] = rPos[0];
  alignxf[13] = rPos[1];
  alignxf[14] = rPos[2];
  alignxf[15] = 1;
}

/**
 * Converts a 4x4 matrix to Euler angles.
 *
 * @param alignxf    input 4x4 matrix
 * @param rPosTheta  output 3-vector of Euler angles
 * @param rPos       output vector of trnaslation (position) if set
 *
 */
static inline void Matrix4ToEuler(const double *alignxf, double *rPosTheta, double *rPos = 0)
{
  
  double _trX, _trY;
  
  if(alignxf[0] > 0.0) {
    rPosTheta[1] = asin(alignxf[8]);
  } else {
    rPosTheta[1] = M_PI - asin(alignxf[8]);
  }
  // rPosTheta[1] =  asin( alignxf[8]);           // Calculate Y-axis angle 
                  
  double  C    =  cos( rPosTheta[1] );
  if ( fabs( C ) > 0.005 )  {                 // Gimball lock? 
    _trX      =  alignxf[10] / C;             // No, so get X-axis angle 
    _trY      =  -alignxf[9] / C;
    rPosTheta[0]  = atan2( _trY, _trX );
    _trX      =  alignxf[0] / C;              // Get Z-axis angle 
    _trY      = -alignxf[4] / C;
    rPosTheta[2]  = atan2( _trY, _trX );
  } else {                                    // Gimball lock has occurred 
    rPosTheta[0] = 0.0;                       // Set X-axis angle to zero 
    _trX      =  alignxf[5];  //1                // And calculate Z-axis angle 
    _trY      =  alignxf[1];  //2
    rPosTheta[2]  = atan2( _trY, _trX );
  }
  
  rPosTheta[0] = rPosTheta[0];
  rPosTheta[1] = rPosTheta[1];
  rPosTheta[2] = rPosTheta[2];
     
  if (rPos != 0) {
    rPos[0] = alignxf[12];
    rPos[1] = alignxf[13];
    rPos[2] = alignxf[14];
  }
}


/**
 * Sets a 3x3 matrix to the identity matrix
 *
 * @param M  input 3x3 matrix
 */
template <class T>
static inline void M3identity( T *M )
{
  M[0] = M[4] = M[8] = 1.0;
  M[1] = M[2] = M[3] = M[5] = M[6] = M[7] = 0.0;
}

/**
 * Gets the current time (in ms)
 *
 * @return current time (in ms)
 */
static inline unsigned long GetCurrentTimeInMilliSec()
{
  static unsigned long milliseconds;
#ifdef _MSC_VER
  SYSTEMTIME stime;
  GetSystemTime(&stime);
  milliseconds = ((stime.wHour * 60 + stime.wMinute) * 60 +  stime.wSecond) * 1000 + stime.wMilliseconds;
#else
  static struct timeval tv;
  gettimeofday(&tv, NULL);
  milliseconds = tv.tv_sec * 1000 + tv.tv_usec / 1000;
#endif   
  return milliseconds;
}

/**
 * generates random numbers in [0..rnd]
 *
 * @param rnd  maximum number
 * @return random number between 0 and rnd
 */
inline int rand(int rnd)
{
  return (int) ((double)rnd * (double)std::rand() / (RAND_MAX + 1.0));
}

/**
 * generates unsigned character random numbers in [0..rnd]
 *
 * @param rnd  maximum number
 * @return random number between 0 and rnd
 */
inline unsigned char randUC(unsigned char rnd)
{
  return (unsigned char) ((float)rnd * std::rand() / (RAND_MAX + 1.0));
}

/**
 * Computes the angle between 2 points in polar coordinates
 */
inline double polardist(double* p, double *p2) {
  double stheta = sin(p[0]) * sin(p2[0]);
  double myd2 = acos( stheta * cos(p[1]) * cos(p2[1]) 
                    + stheta * sin(p[1]) * sin(p2[1]) 
                    + cos(p[0]) * cos(p2[0])); 
  return myd2;
}

                        

inline void toKartesian(double *polar, double *kart) {
  kart[0] = polar[2] * cos( polar[1] ) * sin( polar[0] );
  kart[1] = polar[2] * sin( polar[1] ) * sin( polar[0] );
  kart[2] = polar[2] * cos( polar[0] );

}

/**
  * Transforms a point in cartesian coordinates into polar 
  * coordinates 
  */

inline void toPolar(double *n, double *polar) {
  double phi, theta, rho;
  rho = Len(n);
  Normalize3(n);

 // if(fabs(1 - fabs(n[1])) < 0.001)  {
 // cout << "Y " << n[0] << " " << n[1] << " " << n[2] << endl;

  phi = acos(n[2]);
  //if ( fabs(phi) < 0.0001) phi = 0.0001;
  //if ( fabs(M_PI - phi) < 0.0001) phi = 0.0001;

  double theta0;

  if(fabs(phi) < 0.0001) {
    theta = 0.0;
  } else if(fabs(M_PI - phi) < 0.0001) {
    theta = 0.0;
  } else {
    if(fabs(n[0]/sin(phi)) > 1.0) {
      if(n[0]/sin(phi) < 0) {
        theta0 = M_PI;
      } else {
        theta0 = 0.0;
      }
    } else {
      theta0 = acos(n[0]/sin(phi));
    
    }
  
    
    double sintheta = n[1]/sin(phi);

    double EPS = 0.0001;

    if(fabs(sin(theta0) - sintheta) < EPS) {
      theta = theta0;
    } else if(fabs( sin( 2*M_PI - theta0 ) - sintheta ) < EPS) {
      theta = 2*M_PI - theta0;
    } else {
      theta = 0;
      cout << "Fehler" << endl;
    }

  }
 /* } else {
    theta = 0.0;
    phi = 0.0;
  }*/
  polar[0] = phi;
  polar[1] = theta;
  polar[2] = rho;

}

/*
 *   Computes the submatrix without
 *   row i and column j
 *
 * @param Min   input 4x4 matrix
 * @param Mout  output 3x3 matrix
 * @param i     row index i 
 * @param j     column index j
 */
template <class T>
static inline void	M4_submat(const T *Min, T *Mout, int i, int j ) {
  int di, dj, si, sj;
  // loop through 3x3 submatrix
  for( di = 0; di < 3; di ++ ) {
    for( dj = 0; dj < 3; dj ++ ) {
	 // map 3x3 element (destination) to 4x4 element (source)
	 si = di + ( ( di >= i ) ? 1 : 0 );
	 sj = dj + ( ( dj >= j ) ? 1 : 0 );
	 // copy element
	 Mout[di * 3 + dj] = Min[si * 4 + sj];
    }
  }
}

/*
 *  Computes the determinant of a 4x4 matrix
 *
 * @param 4x4 matrix
 * @return determinant
 */
template <class T>
static inline double M4det(const T *M )
{
  T det, result = 0, i = 1.0;
  T Msub3[9];
  int    n;
  for ( n = 0; n < 4; n++, i *= -1.0 ) {
    M4_submat( M, Msub3, 0, n );
    det     = M3det( Msub3 );
    result += M[n] * det * i;
  }
  return( result );
}


/* 
 * invert a 4x4 Matrix
 *
 * @param Min input 4x4 matrix
 * @param Mout output matrix
 * @return 1 if successful
 */
template <class T>
static inline int M4inv(const T *Min, T *Mout )
{
  T  mdet = M4det( Min );
  if ( fabs( mdet ) < 0.0005 ) {
    cout << "Error matrix inverting!" << endl;
    M4identity( Mout );
    return( 0 );
  }
  T  mtemp[9];
  int     i, j, sign;
  for ( i = 0; i < 4; i++ ) {
    for ( j = 0; j < 4; j++ ) {
	 sign = 1 - ( (i +j) % 2 ) * 2;
	 M4_submat( Min, mtemp, i, j );
	 Mout[i+j*4] = ( M3det( mtemp ) * sign ) / mdet;
    }
  }
  return( 1 );
}

/*
 * transposes a 4x4 matrix
 * 
 * @param Min  input 4x4 matrix
 * @param Mout output 4x4 matrix
 */
template <class T>
static inline int M4transpose(const T *Min, T *Mout )
{
  Mout[0]  = Min[0]; 
  Mout[4]  = Min[1]; 
  Mout[8]  = Min[2]; 
  Mout[12] = Min[3]; 
  Mout[1]  = Min[4]; 
  Mout[5]  = Min[5]; 
  Mout[9]  = Min[6]; 
  Mout[13] = Min[7]; 
  Mout[2]  = Min[8]; 
  Mout[6]  = Min[9]; 
  Mout[10] = Min[10]; 
  Mout[14] = Min[11]; 
  Mout[3]  = Min[12]; 
  Mout[7]  = Min[13]; 
  Mout[11] = Min[14]; 
  Mout[15] = Min[15]; 
  return( 1 );
}

/* +++++++++-------------++++++++++++
 * NAME
 *   choldc
 * DESCRIPTION
 *   Cholesky Decomposition of a symmetric
 *    positive definite matrix
 *   Overwrites lower triangle of matrix
 *   Numerical Recipes, but has a bit of
 *    the fancy C++ template thing happening
 * +++++++++-------------++++++++++++ */ 
static inline bool choldc(double A[3][3], double diag[3])
{
  for (unsigned int i = 0; i < 3; i++) {
    for (unsigned int j = i; j < 3; j++) {
      double sum = A[i][j];
      for (int k=i-1; k >= 0; k--)
        sum -= A[i][k] * A[j][k];
      if (i == j) {
        if (sum < 1.0e-7)
          return false;
        diag[i] = sqrt(sum);
      } else {
        A[j][i] = sum / diag[i];
      }
    }
  }
  return true;
}


/* +++++++++-------------++++++++++++
 * NAME
 *   choldc
 * DESCRIPTION
 *   Cholesky Decomposition of a symmetric
 *    positive definite matrix
 *   Overwrites lower triangle of matrix
 *   Numerical Recipes, but has a bit of
 *    the fancy C++ template thing happening
 * +++++++++-------------++++++++++++ */
static inline bool choldc(unsigned int n, double **A, double *diag)
{
  for (unsigned int i = 0; i < n; i++) {
    for (unsigned int j = i; j < n; j++) {
      double sum = A[i][j];
      for (int k=i-1; k >= 0; k--)
        sum -= A[i][k] * A[j][k];
      if (i == j) {
        if (sum < 1.0e-7)
          return false;
        diag[i] = sqrt(sum);
      } else {
        A[j][i] = sum / diag[i];
      }
    }
  }
  return true;
}


/* +++++++++-------------++++++++++++
 * NAME
 *   cholsl
 * DESCRIPTION
 *   Solve Ax=B after choldc
 * +++++++++-------------++++++++++++ */  
static inline void cholsl(double A[3][3],
					 double diag[3],
					 double B[3],
					 double x[3])
{
  for (int i=0; i < 3; i++) {
    double sum = B[i];
    for (int k=i-1; k >= 0; k--)
      sum -= A[i][k] * x[k];
    x[i] = sum / diag[i];
  }
  for (int i=2; i >= 0; i--) {
    double sum = x[i];
    for (int k=i+1; k < 3; k++)
      sum -= A[k][i] * x[k];
    x[i] = sum / diag[i];
  }
}


/* +++++++++-------------++++++++++++
 * NAME
 *   cholsl
 * DESCRIPTION
 *   Solve Ax=B after choldc
 * +++++++++-------------++++++++++++ */
static inline void cholsl(unsigned int n,
					 double **A,
					 double *diag,
					 double *B,
					 double *x)
{
  for (unsigned int i=0; i < n; i++) {
    double sum = B[i];
    for (int k=(int)i-1; k >= 0; k--)
      sum -= A[i][k] * x[k];
    x[i] = sum / diag[i];
  }
  for (int i=(int)n-1; i >= 0; i--) {
    double sum = x[i];
    for (unsigned int k=i+1; k < n; k++)
      sum -= A[k][i] * x[k];
    x[i] = sum / diag[i];
  }
}


/**
 * Transforms a a quaternion and a translation vector into a 4x4
 * Matrix
 *
 * @param quat  input quaternion
 * @param t     input translation
 * @param mat   output matrix
 */
static inline void QuatToMatrix4(const double *quat, const double *t, double *mat)
{
  //  double q00 = quat[0]*quat[0];
  double q11 = quat[1]*quat[1];
  double q22 = quat[2]*quat[2];
  double q33 = quat[3]*quat[3];
  double q03 = quat[0]*quat[3];
  double q13 = quat[1]*quat[3];
  double q23 = quat[2]*quat[3];
  double q02 = quat[0]*quat[2];
  double q12 = quat[1]*quat[2];
  double q01 = quat[0]*quat[1];
  mat[0] = 1 - 2 * (q22 + q33);
  mat[5] = 1 - 2 * (q11 + q33);
  mat[10] = 1 - 2 * (q11 + q22);
  mat[4] = 2.0*(q12-q03);
  mat[1] = 2.0*(q12+q03);
  mat[8] = 2.0*(q13+q02);
  mat[2] = 2.0*(q13-q02);
  mat[9] = 2.0*(q23-q01);
  mat[6] = 2.0*(q23+q01);

  mat[3] = mat[7] = mat[11] = 0.0;

  if (t == 0) {
    mat[12] = mat[13] = mat[14] = 0.0;
  } else {
    mat[12] = t[0];
    mat[13] = t[1];
    mat[14] = t[2];
  }
  mat[15] = 1.0;
}


/**
 * Transforms a 4x4 Transformation Matrix into a quaternion
 *
 * @param mat     matrix to be converted
 * @param quat    resulting quaternion
 * @param t       resulting translation
 */
static inline void Matrix4ToQuat(const double *mat, double *quat, double *t = 0)
{
  
  double T, S, X, Y, Z, W;
  T = 1 + mat[0] + mat[5] + mat[10];
  if ( T > 0.00000001 ) { // to avoid large distortions!

    S = sqrt(T) * 2;
    X = ( mat[9] - mat[6] ) / S;
    Y = ( mat[2] - mat[8] ) / S;
    Z = ( mat[4] - mat[1] ) / S;
    W = 0.25 * S;
  } else if ( mat[0] > mat[5] && mat[0] > mat[10] )  { // Column 0: 
    S  = sqrt( 1.0 + mat[0] - mat[5] - mat[10] ) * 2;
    X = 0.25 * S;
    Y = (mat[4] + mat[1] ) / S;
    Z = (mat[2] + mat[8] ) / S;
    W = (mat[9] - mat[6] ) / S;
  } else if ( mat[5] > mat[10] ) {                    // Column 1: 
    S  = sqrt( 1.0 + mat[5] - mat[0] - mat[10] ) * 2;
    X = (mat[4] + mat[1] ) / S;
    Y = 0.25 * S;
    Z = (mat[9] + mat[6] ) / S;
    W = (mat[2] - mat[8] ) / S;
  } else {                                            // Column 2:
    S  = sqrt( 1.0 + mat[10] - mat[0] - mat[5] ) * 2;
    X = (mat[2] + mat[8] ) / S;
    Y = (mat[9] + mat[6] ) / S;
    Z = 0.25 * S;
    W = (mat[4] - mat[1] ) / S;
  }
  quat[0] = W;
  quat[1] = -X;
  quat[2] = -Y;
  quat[3] = -Z;
  
  Normalize4(quat);
  if (t != 0) {
    t[0] = mat[12];
    t[1] = mat[13];
    t[2] = mat[14];
  }
}

/**
 *  Transforms a Quaternion to the corresponding Axis-Angle representation
 *
 * @param quat   4-vector of quaternion
 *               gets overridden by the axis/angle representation
 */
static inline void QuatToAA(double *quat){
  //double x, y, z, w;
  double sum = 0.0;
  
  double cos_a, angle, x, y, z, sin_a;

  for(int i = 0; i < 4; i++){
    sum += quat[i]*quat[i];
  }
  sum = sqrt(sum);

  //quaternion_normalise( |W,X,Y,Z| );
  cos_a = quat[0]/sum;
  angle = acos( cos_a ) * 2;
  sin_a = sqrt( 1.0 - cos_a * cos_a );
  if ( fabs( sin_a ) < 0.0005 ) sin_a = 1;
  x = quat[1] / sin_a;
  y = quat[2] / sin_a;
  z = quat[3] / sin_a;
  
  quat[0] = angle;
  quat[1] = x;
  quat[2] = y;
  quat[3] = z;
}

/**
 * Quaternion Multiplication q1 * q2 = q3
 */
static inline void QMult(const double *q1, const double *q2, double *q3) {
  q3[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
  q3[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
  q3[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
  q3[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
}

/**
 * Quaternion SLERP
 * http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
 */
static inline void slerp(const double *qa, const double *qb, const double t, double *qm) {
  // Calculate angle between them.
  double cosHalfTheta = qa[0] * qb[0] + qa[1] * qb[1] + qa[2] * qb[2] + qa[3] * qb[3];
  // if qa=qb or qa=-qb then theta = 0 and we can return qa
  if (fabs(cosHalfTheta) >= 1.0) {
    qm[0] = qa[0];
    qm[1] = qa[1];
    qm[2] = qa[2];
    qm[3] = qa[3];
    return;
  }
  // Calculate temporary values.
  double halfTheta = acos(cosHalfTheta);
  double sinHalfTheta = sqrt(1.0 - cosHalfTheta * cosHalfTheta);
  // if theta = 180 degrees then result is not fully defined
  // we could rotate around any axis normal to qa or qb
  if (fabs(sinHalfTheta) < 0.001){
    qm[0] = (qa[0] * 0.5 + qb[0] * 0.5);
    qm[1] = (qa[1] * 0.5 + qb[1] * 0.5);
    qm[2] = (qa[2] * 0.5 + qb[2] * 0.5);
    qm[3] = (qa[3] * 0.5 + qb[3] * 0.5);
    Normalize4(qm);
    return;
  }
  double ratioA = sin((1 - t) * halfTheta) / sinHalfTheta;
  double ratioB = sin(t * halfTheta) / sinHalfTheta; 
  //calculate Quaternion.
  qm[0] = (qa[0] * ratioA + qb[0] * ratioB);
  qm[1] = (qa[1] * ratioA + qb[1] * ratioB);
  qm[2] = (qa[2] * ratioA + qb[2] * ratioB);
  qm[3] = (qa[3] * ratioA + qb[3] * ratioB);
  Normalize4(qm);
}

/* taken from ROOT (CERN)
 * as well in:
 * Effective Sampling and Distance Metrics for 3D Rigid Body Path Planning
 * James J. Kuffner
 *
 * Distance between two rotations in Quaternion form
 * Note:  The rotation group is isomorphic to a 3-sphere
 * with diametrically opposite points identified.
 * The (rotation group-invariant) is the smaller
 * of the two possible angles between the images of
 * the two rotations on that sphere.  Thus the distance
 * is never greater than pi/2.
 */
inline double quat_dist(double quat1[4], double quat2[4]) {
  double chordLength = std::fabs(quat1[0]*quat2[0] + quat1[1]*quat2[1] + quat1[2]*quat2[2] + quat1[3]*quat2[3]);
  if (chordLength > 1) chordLength = 1; // in case roundoff fouls us up
  return acos(chordLength) / M_PI * 180.0;
}



/**
 *  Converts a Rotation given by Axis-Angle and a Translation into a
 *  4x4 Transformation matrix
 *
 * @param aa         axis and angle aa[0] is the angle
 * @param trans      vector containing the translation
 * @param matrix     matrix to be computed
 */
inline void AAToMatrix(double *aa, double *trans, double *matrix ){
  double rcos = cos(aa[0]);
  double rsin = sin(aa[0]);
  double u = aa[1];
  double v = aa[2];
  double w = aa[3];

  matrix[0]    =      rcos + u*u*(1-rcos);
  matrix[1]    =  w * rsin + v*u*(1-rcos);
  matrix[2]    = -v * rsin + w*u*(1-rcos);
  matrix[3]    = 0.0;
  matrix[4]    = -w * rsin + u*v*(1-rcos);
  matrix[5]    =      rcos + v*v*(1-rcos);
  matrix[6]    =  u * rsin + w*v*(1-rcos);
  matrix[7]    = 0.0;
  matrix[8]    =  v * rsin + u*w*(1-rcos);
  matrix[9]    = -u * rsin + v*w*(1-rcos);
  matrix[10]    =     rcos + w*w*(1-rcos);
  matrix[11]   = 0.0;
  matrix[12]   = trans[0];
  matrix[13]   = trans[1];
  matrix[14]   = trans[2];
  matrix[15]   = 1.0;

}


/**
 * Factors matrix A into lower and upper triangular matrices 
 *   (L and U respectively) in solving the linear equation Ax=b.  
 *
 * @param A        (input/output) Matrix(1:n, 1:n)  In input, matrix to be
 *                  factored.  On output, overwritten with lower and 
 *                  upper triangular factors.
 *
 * @param indx     (output) Vector(1:n)    Pivot vector. Describes how
 *                  the rows of A were reordered to increase
 *                  numerical stability.
 *
 * @return return int(0 if successful, 1 otherwise)
 */
inline int LU_factor( double A[4][4], int indx[4])
{
  int M = 4;
  int N = 4;

  int  i=0,j=0,k=0;
  int  jp=0;

  double t;

  int minMN = 4;

  for (j = 0; j < minMN; j++)
    {

	 // find pivot in column j and  test for singularity.

	 jp = j;
	 t = fabs(A[j][j]);
	 for (i = j+1; i < M; i++)
	   if ( fabs(A[i][j]) > t)
		{
		  jp = i;
		  t = fabs(A[i][j]);
		}

	 indx[j] = jp;

	 // jp now has the index of maximum element 
	 // of column j, below the diagonal

	 if ( A[jp][j] == 0 )                 
	   return 1;       // factorization failed because of zero pivot


	 if (jp != j)            // swap rows j and jp
	   for (k = 0; k < N; k++)
		{
		  t = A[j][k];
		  A[j][k] = A[jp][k];
		  A[jp][k] =t;
		}

	 if (j < M)                // compute elements j+1:M of jth column
        {
		// note A(j,j), was A(jp,p) previously which was
		// guarranteed not to be zero (Label #1)
		//
		double recp =  1.0 / A[j][j];

		for (k = j+1; k < M; k++)
		  A[k][j] *= recp;
        }

	 if (j < minMN)
        {
		// rank-1 update to trailing submatrix:   E = E - x*y;
		//
		// E is the region A(j+1:M, j+1:N)
		// x is the column vector A(j+1:M,j)
		// y is row vector A(j,j+1:N)

		int ii,jj;

		for (ii = j+1; ii < M; ii++)
		  for (jj = j+1; jj < N; jj++)
		    A[ii][jj] -= A[ii][j]*A[j][jj];
        }
    }

  return 0;
}   

/**
 * Solves a linear system via LU after LU factor
 *
 * @param A 4x4 matrix
 * @param indx indices
 * @param b 4 vectort
 *
 * @return 0
 *
 */
inline int LU_solve(const double A[4][4], const int indx[4], double b[4])
{
  int i,ii=0,ip,j;
  int n = 4;
  double sum = 0.0;

  for (i = 0; i < n; i++) 
    {
	 ip=indx[i];
	 sum=b[ip];
	 b[ip]=b[i];
	 if (ii)
	   for (j = ii;j <= i-1; j++) 
		sum -= A[i][j]*b[j];
	 else if (sum) ii=i;
	 b[i]=sum;
    }
  for (i = n-1; i >= 0; i--) 
    {
	 sum=b[i];
	 for (j = i+1; j < n; j++) 
	   sum -= A[i][j]*b[j];
	 b[i]=sum/A[i][i];
    }

  return 0;
}

/**
 * Calculates the <i>cross</i> product of two 4-vectors
 *
 * @param x input 1
 * @param y input 2
 * @param T output
 *
 */
template <class T>
static inline void Cross(const T *x, const T *y, T *result)
{
  result[0] = x[1] * y[2] - x[2] * y[1];
  result[1] = x[2] * y[0] - x[0] * y[2];
  result[2] = x[0] * y[1] - x[1] * y[0];
  return;
}

/**
 * Computes the <i>dot</i> product of two 3-vector
 *
 * @param x   input 3-vector
 * @param y   input 3-vector
 * @return    dot product of x and y
 */
template <class T>
inline T Dot(const T *x, const T *y)
{
  return x[0] * y[0] + x[1] * y[1] + x[2] * y[2];
}

/**
 * converts a quaternion to Euler angels in the roll pitch yaw system
 */
static inline void QuatRPYEuler(const double *quat, double *euler)
{
	double n = sqrt(quat[0]*quat[0] + quat[1]*quat[1] + quat[2]*quat[2] + quat[3]*quat[3]);
	double s = n > 0?2./(n*n):0.;

	double m00, m10, m20, m21, m22;


	double xs = quat[1]*s;
	double ys = quat[2]*s;
	double zs = quat[3]*s;

	double wx = quat[0]*xs;
	double wy = quat[0]*ys;
	double wz = quat[0]*zs;

	double xx = quat[1]*xs;
	double xy = quat[1]*ys;
	double xz = quat[1]*zs;

	double yy = quat[2]*ys;
	double yz = quat[2]*zs;

	double zz = quat[3]*zs;

	m00 = 1.0 - (yy + zz);
	m22 = 1.0 - (xx + yy);


	m10 = xy + wz;

	m20 = xz - wy;
	m21 = yz + wx;

	euler[0] = atan2(m21,m22);
	euler[1] = atan2(-m20,sqrt(m21*m21 + m22*m22));
	euler[2] = atan2(m10,m00);
}

/**
 * converts from Euler angels in the roll pitch yaw system to a quaternion
 */
static inline void RPYEulerQuat(const double *euler, double *quat)
{
	double sphi   = sin(euler[0]);
	double stheta = sin(euler[1]);
	double spsi   = sin(euler[2]);
	double cphi   = cos(euler[0]);
	double ctheta = cos(euler[1]);
	double cpsi   = cos(euler[2]);

	double _r[3][3] = { //create rotational Matrix
		{cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
		{spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
		{    -stheta,                  ctheta*sphi,                  ctheta*cphi}
	};

#define MY_MAX(a,b) (((a)>(b))?(a):(b))
	double _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0;
	double _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0;
	double _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0;
	double _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0;
	quat[0] = _w;
	quat[1] = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x);
	quat[2] = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y);
	quat[3] = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z);
}


inline void transform3(const double *alignxf, double *point)
{
  double x_neu, y_neu, z_neu;
  x_neu = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8];
  y_neu = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9];
  z_neu = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10];
  point[0] = x_neu + alignxf[12];
  point[1] = y_neu + alignxf[13];
  point[2] = z_neu + alignxf[14];
}

inline void transform3(const double *alignxf, const double *point, double *tpoint)
{
  tpoint[0] = point[0] * alignxf[0] + point[1] * alignxf[4] + point[2] * alignxf[8]  + alignxf[12];
  tpoint[1] = point[0] * alignxf[1] + point[1] * alignxf[5] + point[2] * alignxf[9]  + alignxf[13];
  tpoint[2] = point[0] * alignxf[2] + point[1] * alignxf[6] + point[2] * alignxf[10] + alignxf[14];
}

inline std::string trim(const std::string& source)
{
  unsigned int start = 0, end = source.size() - 1;
  while(source[start] == ' ') start++;
  while(source[end] == ' ') end--;
  return source.substr(start, end - start + 1);
}

#endif