//
// This file is part of MorphoGraphX - http://www.MorphoGraphX.org
// Copyright (C) 2012-2015 Richard S. Smith and collaborators.
//
// If you use MorphoGraphX in your work, please cite:
//   http://dx.doi.org/10.7554/eLife.05864
//
// MorphoGraphX is free software, and is licensed under under the terms of the 
// GNU General (GPL) Public License version 2.0, http://www.gnu.org/licenses.
// 
#include <Quaternion.hpp>
#include <cmath>

#ifdef EPSILON
#  undef EPSILON
#endif
#define EPSILON 1e-12

namespace mgx 
{
  Quaternion::Quaternion(const Point3d &from, const Point3d &to) : Point4d(0, 0, 0, 1)
  {
    const double fromSqNorm = mgx::normsq(from);
    const double toSqNorm = mgx::normsq(to);
    // Identity Quaternion when one vector is null
    if(fromSqNorm < EPSILON or toSqNorm < EPSILON)
      return;
    else {
      Point3d axis = from ^ to;
      const double axisSqNorm = mgx::normsq(axis);
  
      // Aligned vectors, pick any axis, not aligned with from or to
      if(axisSqNorm < EPSILON)
        axis = orthogonal(from);
  
      double angle = std::asin(std::sqrt(axisSqNorm / (fromSqNorm * toSqNorm)));
  
      if(from * to < 0.0)
        angle = M_PI - angle;
  
      setAxisAngle(axis, angle);
    }
  }
  
  Quaternion &Quaternion::operator=(const Quaternion &other)
  {
    for(size_t i = 0; i < 4; ++i)
      elems[i] = other.elems[i];
    return *this;
  }
  
  Quaternion Quaternion::operator*(const Quaternion &other) const
  {
    double w_ = w() * other.w() - x() * other.x() - y() * other.y() - z() * other.z();
    double x_ = w() * other.x() + other.w() * x() + y() * other.z() - z() * other.y();
    double y_ = w() * other.y() + other.w() * y() + z() * other.x() - x() * other.z();
    double z_ = w() * other.z() + other.w() * z() + x() * other.y() - y() * other.x();
    return Quaternion(x_, y_, z_, w_);
  }
  
  Quaternion &Quaternion::operator*=(const Quaternion &other)
  {
    Quaternion res = *this * other;
    *this = res;
    return *this;
  }
  
  Quaternion::Quaternion(const Point3d &axis, double angle)
    : Point4d()
  {
    setAxisAngle(axis, angle);
  }
  
  void Quaternion::setAxisAngle(const Point3d &axis, double angle)
  {
    const double norm = mgx::norm(axis);
    if(norm < EPSILON) {
      x() = 0;
      y() = 0;
      z() = 0;
      w() = 1;
    } else {
      const double sin_half_angle = std::sin(angle / 2.0);
      x() = sin_half_angle * axis.x() / norm;
      y() = sin_half_angle * axis.y() / norm;
      z() = sin_half_angle * axis.z() / norm;
      w() = std::cos(angle / 2.0);
    }
  }
  
  Quaternion::Quaternion(const Matrix3d &m)
  {
    // Compute one plus the trace of the matrix
    const double onePlusTrace = 1.0 + m.trace();
    if(onePlusTrace > EPSILON) {
      const double s = std::sqrt(onePlusTrace) * 2.0;
      x() = (m(2, 1) - m(1, 2)) / s;
      y() = (m(0, 2) - m(2, 0)) / s;
      z() = (m(1, 0) - m(0, 1)) / s;
      w() = 0.25 * s;
    } else {
      // Computation depends on major diagonal term
      if(m(0, 0) > m(1, 1) and m(0, 0) > m(2, 2)) {
        const double s = std::sqrt(1 + m(0, 0) - m(1, 1) - m(2, 2)) * 2.0;
        x() = 0.25 * s;
        y() = (m(1, 0) + m(0, 1)) / s;
        z() = (m(2, 0) + m(0, 2)) / s;
        w() = (m(1, 2) - m(2, 1)) / s;
      } else if(m(1, 1) > m(2, 2)) {
        const double s = std::sqrt(1 + m(1, 1) - m(0, 0) - m(2, 2)) * 2.0;
        x() = (m(0, 1) + m(1, 0)) / s;
        y() = 0.25 * s;
        z() = (m(1, 2) + m(2, 1)) / s;
        w() = (m(0, 2) - m(2, 0)) / s;
      } else {
        const double s = std::sqrt(1 + m(2, 2) - m(0, 0) - m(1, 1)) * 2.0;
        x() = (m(0, 2) + m(0, 0)) / s;
        y() = (m(1, 2) + m(2, 1)) / s;
        z() = 0.25 * s;
        w() = (m(0, 1) - m(1, 0)) / s;
      }
    }
    normalize();
  }
  
  template <class matrix> static void setMatrixFromQuaternion(matrix &m, const Quaternion &q)
  {
    const double xx = q.x() * q.x();
    const double xy = q.x() * q.y();
    const double xz = q.x() * q.z();
    const double yy = q.y() * q.y();
    const double yz = q.y() * q.z();
    const double zz = q.z() * q.z();
  
    const double wx = q.w() * q.x();
    const double wy = q.w() * q.y();
    const double wz = q.w() * q.z();
  
    m(0, 0) = 1 - 2 * (yy + zz);
    m(0, 1) = 2 * (xy - wz);
    m(0, 2) = 2 * (wy + xz);
    m(1, 0) = 2 * (xy + wz);
    m(1, 1) = 1 - 2 * (xx + zz);
    m(1, 2) = 2 * (yz - wx);
    m(2, 0) = 2 * (xz - wy);
    m(2, 1) = 2 * (yz + wx);
    m(2, 2) = 1 - 2 * (xx + yy);
  }
  
  void Quaternion::setMatrix(Matrix3d &m) const {
    setMatrixFromQuaternion(m, *this);
  }
  
  void Quaternion::setMatrix(Matrix4d &m) const
  {
    setMatrixFromQuaternion(m, *this);
    for(size_t i = 0; i < 4; ++i) {
      m(i, 3) = 0.0;
      m(3, i) = 0.0;
    }
    m(3, 3) = 1.0;
  }
  
  Point3d Quaternion::axis() const
  {
    Point3d res(x(), y(), z());
    const double sin = res.norm();
    if(sin > EPSILON)
      res /= sin;
    return (std::acos(w()) < M_PI / 2.0) ? res : -res;
  }
  
  double Quaternion::angle() const
  {
    const double a = 2.0 * std::acos(w());
    return (a <= M_PI) ? a : 2.0 * M_PI - a;
  }
  
  Point3d Quaternion::rotate(const Point3d &v) const
  {
    /*
       Quaternion q(v.x(), v.y(), v.z(), 0);
       Quaternion result = *this * q * conjugate();
       return Point3d(result.x(), result.y(), result.z());
     */
  
    const double xx = 2.0l * x() * x();
    const double yy = 2.0l * y() * y();
    const double zz = 2.0l * z() * z();
    const double xy = 2.0l * x() * y();
    const double xz = 2.0l * x() * z();
    const double yz = 2.0l * y() * z();
  
    const double wx = 2.0l * w() * x();
    const double wy = 2.0l * w() * y();
    const double wz = 2.0l * w() * z();
  
    return Point3d((1.0 - yy - zz) * v.x() + (xy - wz) * v.y() + (xz + wy) * v.z(),
                   (xy + wz) * v.x() + (1.0 - zz - xx) * v.y() + (yz - wx) * v.z(),
                   (xz - wy) * v.x() + (yz + wx) * v.y() + (1.0 - xx - yy) * v.z());
  }
  
  Point3d Quaternion::inverseRotate(const Point3d &v) const {
    return inverse().rotate(v);
  }
  
  Quaternion slerp(const Quaternion &q1, const Quaternion &q2, double t)
  {
    if(q1 == q2)
      return q1;
    else if(t <= 0)
      return q1;
    else if(t >= 1.0)
      return q2;

    double ca = static_cast<const Point4d&>(q1) * static_cast<const Point4d&>(q2);
    double a = std::acos(ca);
    if(a > M_PI / 2)
      a -= M_PI;
    return (q1 * std::sin(a * (1 - t)) + q2 * std::sin(a * t)) / std::sin(a);
  }
  
}

namespace std 
{
  mgx::Quaternion pow(const mgx::Quaternion &q, double p)
  {
    double a = q.angle();
    mgx::Point3d v = q.axis();
    return mgx::Quaternion(v, a * p);
  }
}
