#include "mat_to_quat.h"
#include <cmath>

// This could be replaced by something fast
template <typename Q_type>
static inline Q_type ReciprocalSqrt( const Q_type x )
{
  return 1.0/sqrt(x);
}

//// Converts row major order matrix to quat
//// http://software.intel.com/sites/default/files/m/d/4/1/d/8/293748.pdf
//template <typename Q_type>
//IGL_INLINE void igl::mat4_to_quat(const Q_type * m, Q_type * q)
//{
//  Q_type t = + m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f; 
//  Q_type s = ReciprocalSqrt( t ) * 0.5f;
//  q[3] = s * t;
//  q[2] = ( m[0 * 4 + 1] - m[1 * 4 + 0] ) * s; 
//  q[1] = ( m[2 * 4 + 0] - m[0 * 4 + 2] ) * s; 
//  q[0] = ( m[1 * 4 + 2] - m[2 * 4 + 1] ) * s;
//}

// https://bmgame.googlecode.com/svn/idlib/math/Simd_AltiVec.cpp
template <typename Q_type>
IGL_INLINE void igl::mat4_to_quat(const Q_type * mat, Q_type * q)
{
  Q_type trace;
  Q_type s;
  Q_type t;
  int i;
  int j;
  int k;
  
  static int next[3] = { 1, 2, 0 };

  trace = mat[0 * 4 + 0] + mat[1 * 4 + 1] + mat[2 * 4 + 2];

  if ( trace > 0.0f ) {

    t = trace + 1.0f;
    s = ReciprocalSqrt( t ) * 0.5f;

    q[3] = s * t;
    q[0] = ( mat[1 * 4 + 2] - mat[2 * 4 + 1] ) * s;
    q[1] = ( mat[2 * 4 + 0] - mat[0 * 4 + 2] ) * s;
    q[2] = ( mat[0 * 4 + 1] - mat[1 * 4 + 0] ) * s;

  } else {

    i = 0;
    if ( mat[1 * 4 + 1] > mat[0 * 4 + 0] ) {
      i = 1;
    }
    if ( mat[2 * 4 + 2] > mat[i * 4 + i] ) {
      i = 2;
    }
    j = next[i];
    k = next[j];

    t = ( mat[i * 4 + i] - ( mat[j * 4 + j] + mat[k * 4 + k] ) ) + 1.0f;
    s = ReciprocalSqrt( t ) * 0.5f;

    q[i] = s * t;
    q[3] = ( mat[j * 4 + k] - mat[k * 4 + j] ) * s;
    q[j] = ( mat[i * 4 + j] + mat[j * 4 + i] ) * s;
    q[k] = ( mat[i * 4 + k] + mat[k * 4 + i] ) * s;
  }

  //// Unused translation
  //jq.t[0] = mat[0 * 4 + 3];
  //jq.t[1] = mat[1 * 4 + 3];
  //jq.t[2] = mat[2 * 4 + 3];
}



#ifndef IGL_HEADER_ONLY
// Explicit template specialization
template void igl::mat4_to_quat<double>(double const*, double*);
template void igl::mat4_to_quat<float>(float const*, float*);
#endif