
#include <iostream>
#include <Eigen/Geometry>
#include <bench/BenchTimer.h>
using namespace Eigen;
using namespace std;


template<typename Q>
EIGEN_DONT_INLINE Q nlerp(const Q& a, const Q& b, typename Q::Scalar t)
{
  return Q((a.coeffs() * (1.0-t) + b.coeffs() * t).normalized());
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_eigen(const Q& a, const Q& b, typename Q::Scalar t)
{
  return a.slerp(t,b);
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_legacy(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;
  static const Scalar one = Scalar(1) - dummy_precision<Scalar>();
  Scalar d = a.dot(b);
  Scalar absD = internal::abs(d);
  if (absD>=one)
    return a;

  // theta is the angle between the 2 quaternions
  Scalar theta = std::acos(absD);
  Scalar sinTheta = internal::sin(theta);

  Scalar scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
  Scalar scale1 = internal::sin( ( t * theta) ) / sinTheta;
  if (d<0)
    scale1 = -scale1;

  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_legacy_nlerp(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;
  static const Scalar one = Scalar(1) - epsilon<Scalar>();
  Scalar d = a.dot(b);
  Scalar absD = internal::abs(d);
  
  Scalar scale0;
  Scalar scale1;
  
  if (absD>=one)
  {
    scale0 = Scalar(1) - t;
    scale1 = t;
  }
  else
  {
    // theta is the angle between the 2 quaternions
    Scalar theta = std::acos(absD);
    Scalar sinTheta = internal::sin(theta);

    scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
    scale1 = internal::sin( ( t * theta) ) / sinTheta;
    if (d<0)
      scale1 = -scale1;
  }

  return Q(scale0 * a.coeffs() + scale1 * b.coeffs());
}

template<typename T>
inline T sin_over_x(T x)
{
  if (T(1) + x*x == T(1))
    return T(1);
  else
    return std::sin(x)/x;
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_rw(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;
  
  Scalar d = a.dot(b);
  Scalar theta;
  if (d<0.0)
    theta = /*M_PI -*/ Scalar(2)*std::asin( (a.coeffs()+b.coeffs()).norm()/2 );
  else
    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
  
  // theta is the angle between the 2 quaternions
//   Scalar theta = std::acos(absD);
  Scalar sinOverTheta = sin_over_x(theta);

  Scalar scale0 = (Scalar(1)-t)*sin_over_x( ( Scalar(1) - t ) * theta) / sinOverTheta;
  Scalar scale1 = t * sin_over_x( ( t * theta) ) / sinOverTheta;
  if (d<0)
    scale1 = -scale1;

  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
}

template<typename Q>
EIGEN_DONT_INLINE Q slerp_gael(const Q& a, const Q& b, typename Q::Scalar t)
{
  typedef typename Q::Scalar Scalar;
  
  Scalar d = a.dot(b);
  Scalar theta;
//   theta = Scalar(2) * atan2((a.coeffs()-b.coeffs()).norm(),(a.coeffs()+b.coeffs()).norm());
//   if (d<0.0)
//     theta = M_PI-theta;
  
  if (d<0.0)
    theta = /*M_PI -*/ Scalar(2)*std::asin( (-a.coeffs()-b.coeffs()).norm()/2 );
  else
    theta = Scalar(2)*std::asin( (a.coeffs()-b.coeffs()).norm()/2 );
  
  
  Scalar scale0;
  Scalar scale1;
  if(theta*theta-Scalar(6)==-Scalar(6))
  {
    scale0 = Scalar(1) - t;
    scale1 = t;
  }
  else
  {
    Scalar sinTheta = std::sin(theta);
    scale0 = internal::sin( ( Scalar(1) - t ) * theta) / sinTheta;
    scale1 = internal::sin( ( t * theta) ) / sinTheta;
    if (d<0)
      scale1 = -scale1;
  }

  return Quaternion<Scalar>(scale0 * a.coeffs() + scale1 * b.coeffs());
}

int main()
{
  typedef double RefScalar;
  typedef float TestScalar;
  
  typedef Quaternion<RefScalar>  Qd;
  typedef Quaternion<TestScalar> Qf;
  
  unsigned int g_seed = (unsigned int) time(NULL);
  std::cout << g_seed << "\n";
//   g_seed = 1259932496;
  srand(g_seed);
  
  Matrix<RefScalar,Dynamic,1> maxerr(7);
  maxerr.setZero();
  
  Matrix<RefScalar,Dynamic,1> avgerr(7);
  avgerr.setZero();
  
  cout << "double=>float=>double       nlerp        eigen        legacy(snap)         legacy(nlerp)        rightway         gael's criteria\n";
  
  int rep = 100;
  int iters = 40;
  for (int w=0; w<rep; ++w)
  {
    Qf a, b;
    a.coeffs().setRandom();
    a.normalize();
    b.coeffs().setRandom();
    b.normalize();
    
    Qf c[6];
    
    Qd ar(a.cast<RefScalar>());
    Qd br(b.cast<RefScalar>());
    Qd cr;
    
    
    cout.precision(8);
    cout << std::scientific;
    for (int i=0; i<iters; ++i)
    {
      RefScalar t = 0.65;
      cr = slerp_rw(ar,br,t);
      
      Qf refc = cr.cast<TestScalar>();
      c[0] = nlerp(a,b,t);
      c[1] = slerp_eigen(a,b,t);
      c[2] = slerp_legacy(a,b,t);
      c[3] = slerp_legacy_nlerp(a,b,t);
      c[4] = slerp_rw(a,b,t);
      c[5] = slerp_gael(a,b,t);
      
      VectorXd err(7);
      err[0] = (cr.coeffs()-refc.cast<RefScalar>().coeffs()).norm();
//       std::cout << err[0] << "    ";
      for (int k=0; k<6; ++k)
      {
        err[k+1] = (c[k].coeffs()-refc.coeffs()).norm();
//         std::cout << err[k+1] << "    ";
      }
      maxerr = maxerr.cwise().max(err);
      avgerr += err;
//       std::cout << "\n";
      b = cr.cast<TestScalar>();
      br = cr;
    }
//     std::cout << "\n";
  }
  avgerr /= RefScalar(rep*iters);
  cout << "\n\nAccuracy:\n"
       << "  max: " << maxerr.transpose() << "\n";
  cout << "  avg: " << avgerr.transpose() << "\n";
  
  // perf bench
  Quaternionf a,b;
  a.coeffs().setRandom();
  a.normalize();
  b.coeffs().setRandom();
  b.normalize();
  //b = a;
  float s = 0.65;
    
  #define BENCH(FUNC) {\
    BenchTimer t; \
    for(int k=0; k<2; ++k) {\
      t.start(); \
      for(int i=0; i<1000000; ++i) \
        FUNC(a,b,s); \
      t.stop(); \
    } \
    cout << "  " << #FUNC << " => \t " << t.value() << "s\n"; \
  }
  
  cout << "\nSpeed:\n" << std::fixed;
  BENCH(nlerp);
  BENCH(slerp_eigen);
  BENCH(slerp_legacy);
  BENCH(slerp_legacy_nlerp);
  BENCH(slerp_rw);
  BENCH(slerp_gael);
}