/*
* Copyright 2014 Google Inc. All rights reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef MATHFU_QUATERNION_H_
#define MATHFU_QUATERNION_H_

#ifdef _WIN32
#if !defined(_USE_MATH_DEFINES)
#define _USE_MATH_DEFINES  // For M_PI.
#endif                     // !defined(_USE_MATH_DEFINES)
#endif                     // _WIN32

#include "mathfu/matrix.h"
#include "mathfu/vector.h"

#include <math.h>

/// @file mathfu/quaternion.h
/// @brief Quaternion class and functions.
/// @addtogroup mathfu_quaternion
///
/// MathFu provides a Quaternion class that utilizes SIMD optimized
/// Matrix and Vector classes.

namespace mathfu {

/// @addtogroup mathfu_quaternion
/// @{
/// @class Quaternion
///
/// @brief Stores a Quaternion of type T and provides a set of utility
/// operations on each Quaternion.
/// @tparam T Type of each element in the Quaternion.
template <class T>
class Quaternion {
 public:
  /// @brief Construct an uninitialized Quaternion.
  inline Quaternion() {}

  /// @brief Construct a Quaternion from a copy.
  /// @param q Quaternion to copy.
  inline Quaternion(const Quaternion<T>& q) {
    s_ = q.s_;
    v_ = q.v_;
  }

  /// @brief Construct a Quaternion using scalar values to initialize each
  /// element.
  ///
  /// @param s1 Scalar component.
  /// @param s2 First element of the Vector component.
  /// @param s3 Second element of the Vector component.
  /// @param s4 Third element of the Vector component.
  inline Quaternion(const T& s1, const T& s2, const T& s3, const T& s4) {
    s_ = s1;
    v_ = Vector<T, 3>(s2, s3, s4);
  }

  /// @brief Construct a quaternion from a scalar and 3-dimensional Vector.
  ///
  /// @param s1 Scalar component.
  /// @param v1 Vector component.
  inline Quaternion(const T& s1, const Vector<T, 3>& v1) {
    s_ = s1;
    v_ = v1;
  }

  /// @brief Return the scalar component of the quaternion.
  ///
  /// @return The scalar component
  inline T& scalar() { return s_; }

  /// @brief Return the scalar component of the quaternion.
  ///
  /// @return The scalar component
  inline const T& scalar() const { return s_; }

  /// @brief Set the scalar component of the quaternion.
  ///
  /// @param s Scalar component.
  inline void set_scalar(const T& s) { s_ = s; }

  /// @brief Return the vector component of the quaternion.
  ///
  /// @return The scalar component
  inline Vector<T, 3>& vector() { return v_; }

  /// @brief Return the vector component of the quaternion.
  ///
  /// @return The scalar component
  inline const Vector<T, 3>& vector() const { return v_; }

  /// @brief Set the vector component of the quaternion.
  ///
  /// @param v Vector component.
  inline void set_vector(const Vector<T, 3>& v) { v_ = v; }

  /// @brief Calculate the inverse Quaternion.
  ///
  /// This calculates the inverse such that <code>(q * q).Inverse()</code>
  /// is the identity.
  ///
  /// @return Quaternion containing the result.
  inline Quaternion<T> Inverse() const { return Quaternion<T>(s_, -v_); }

  /// @brief Multiply this Quaternion with another Quaternion.
  ///
  /// @note This is equivalent to
  /// <code>FromMatrix(ToMatrix() * q.ToMatrix()).</code>
  /// @param q Quaternion to multiply with.
  /// @return Quaternion containing the result.
  inline Quaternion<T> operator*(const Quaternion<T>& q) const {
    return Quaternion<T>(
        s_ * q.s_ - Vector<T, 3>::DotProduct(v_, q.v_),
        s_ * q.v_ + q.s_ * v_ + Vector<T, 3>::CrossProduct(v_, q.v_));
  }

  /// @brief Multiply this Quaternion by a scalar.
  ///
  /// This multiplies the angle of the rotation by a scalar factor.
  /// @param s1 Scalar to multiply with.
  /// @return Quaternion containing the result.
  inline Quaternion<T> operator*(const T& s1) const {
    T angle;
    Vector<T, 3> axis;
    ToAngleAxis(&angle, &axis);
    angle *= s1;
    return Quaternion<T>(cos(0.5f * angle),
                         axis.Normalized() * static_cast<T>(sin(0.5f * angle)));
  }

  /// @brief Multiply a Vector by this Quaternion.
  ///
  /// This will rotate the specified vector by the rotation specified by this
  /// Quaternion.
  ///
  /// @param v1 Vector to multiply by this Quaternion.
  /// @return Rotated Vector.
  inline Vector<T, 3> operator*(const Vector<T, 3>& v1) const {
    T ss = s_ + s_;
    return ss * Vector<T, 3>::CrossProduct(v_, v1) + (ss * s_ - 1) * v1 +
           2 * Vector<T, 3>::DotProduct(v_, v1) * v_;
  }

  /// @brief Normalize this quaterion (in-place).
  ///
  /// @return Length of the quaternion.
  inline T Normalize() {
    T length = sqrt(s_ * s_ + Vector<T, 3>::DotProduct(v_, v_));
    T scale = (1 / length);
    s_ *= scale;
    v_ *= scale;
    return length;
  }

  /// @brief Calculate the normalized version of this quaternion.
  ///
  /// @return The normalized quaternion.
  inline Quaternion<T> Normalized() const {
    Quaternion<T> q(*this);
    q.Normalize();
    return q;
  }

  /// @brief Convert this Quaternion to an Angle and axis.
  ///
  /// The returned  angle is the size of the rotation in radians about the
  /// axis represented by this Quaternion.
  ///
  /// @param angle Receives the angle.
  /// @param axis Receives the normalized axis.
  inline void ToAngleAxis(T* angle, Vector<T, 3>* axis) const {
    *axis = s_ > 0 ? v_ : -v_;
    *angle = 2 * atan2(axis->Normalize(), s_ > 0 ? s_ : -s_);
  }

  /// @brief Convert this Quaternion to 3 Euler Angles.
  ///
  /// @return 3-dimensional Vector where each element is a angle of rotation
  /// (in radians) around the x, y, and z axes.
  inline Vector<T, 3> ToEulerAngles() const {
    Matrix<T, 3> m(ToMatrix());
    T cos2 = m[0] * m[0] + m[1] * m[1];
    if (cos2 < 1e-6f) {
      return Vector<T, 3>(
          0,
          m[2] < 0 ? static_cast<T>(0.5 * M_PI) : static_cast<T>(-0.5 * M_PI),
          -std::atan2(m[3], m[4]));
    } else {
      return Vector<T, 3>(std::atan2(m[5], m[8]),
                          std::atan2(-m[2], std::sqrt(cos2)),
                          std::atan2(m[1], m[0]));
    }
  }

  /// @brief Convert to a 3x3 Matrix.
  ///
  /// @return 3x3 rotation Matrix.
  inline Matrix<T, 3> ToMatrix() const {
    const T x2 = v_[0] * v_[0], y2 = v_[1] * v_[1], z2 = v_[2] * v_[2];
    const T sx = s_ * v_[0], sy = s_ * v_[1], sz = s_ * v_[2];
    const T xz = v_[0] * v_[2], yz = v_[1] * v_[2], xy = v_[0] * v_[1];
    return Matrix<T, 3>(1 - 2 * (y2 + z2), 2 * (xy + sz), 2 * (xz - sy),
                        2 * (xy - sz), 1 - 2 * (x2 + z2), 2 * (sx + yz),
                        2 * (sy + xz), 2 * (yz - sx), 1 - 2 * (x2 + y2));
  }

  /// @brief Convert to a 4x4 Matrix.
  ///
  /// @return 4x4 transform Matrix.
  inline Matrix<T, 4> ToMatrix4() const {
    const T x2 = v_[0] * v_[0], y2 = v_[1] * v_[1], z2 = v_[2] * v_[2];
    const T sx = s_ * v_[0], sy = s_ * v_[1], sz = s_ * v_[2];
    const T xz = v_[0] * v_[2], yz = v_[1] * v_[2], xy = v_[0] * v_[1];
    return Matrix<T, 4>(1 - 2 * (y2 + z2), 2 * (xy + sz), 2 * (xz - sy), 0.0f,
                        2 * (xy - sz), 1 - 2 * (x2 + z2), 2 * (sx + yz), 0.0f,
                        2 * (sy + xz), 2 * (yz - sx), 1 - 2 * (x2 + y2), 0.0f,
                        0.0f, 0.0f, 0.0f, 1.0f);
  }

  /// @brief Create a Quaternion from an angle and axis.
  ///
  /// @param angle Angle in radians to rotate by.
  /// @param axis Axis in 3D space to rotate around.
  /// @return Quaternion containing the result.
  static Quaternion<T> FromAngleAxis(const T& angle, const Vector<T, 3>& axis) {
    const T halfAngle = static_cast<T>(0.5) * angle;
    Vector<T, 3> localAxis(axis);
    return Quaternion<T>(
        cos(halfAngle),
        localAxis.Normalized() * static_cast<T>(sin(halfAngle)));
  }

  /// @brief Create a quaternion from 3 euler angles.
  ///
  /// @param angles 3-dimensional Vector where each element contains an
  /// angle in radius to rotate by about the x, y and z axes.
  /// @return Quaternion containing the result.
  static Quaternion<T> FromEulerAngles(const Vector<T, 3>& angles) {
    const Vector<T, 3> halfAngles(static_cast<T>(0.5) * angles[0],
                                  static_cast<T>(0.5) * angles[1],
                                  static_cast<T>(0.5) * angles[2]);
    const T sinx = std::sin(halfAngles[0]);
    const T cosx = std::cos(halfAngles[0]);
    const T siny = std::sin(halfAngles[1]);
    const T cosy = std::cos(halfAngles[1]);
    const T sinz = std::sin(halfAngles[2]);
    const T cosz = std::cos(halfAngles[2]);
    return Quaternion<T>(cosx * cosy * cosz + sinx * siny * sinz,
                         sinx * cosy * cosz - cosx * siny * sinz,
                         cosx * siny * cosz + sinx * cosy * sinz,
                         cosx * cosy * sinz - sinx * siny * cosz);
  }

  /// @brief Create a quaternion from a rotation Matrix.
  ///
  /// @param m 3x3 rotation Matrix.
  /// @return Quaternion containing the result.
  static Quaternion<T> FromMatrix(const Matrix<T, 3>& m) {
    const T trace = m(0, 0) + m(1, 1) + m(2, 2);
    if (trace > 0) {
      const T s = sqrt(trace + 1) * 2;
      const T oneOverS = 1 / s;
      return Quaternion<T>(static_cast<T>(0.25) * s, (m[5] - m[7]) * oneOverS,
                           (m[6] - m[2]) * oneOverS, (m[1] - m[3]) * oneOverS);
    } else if (m[0] > m[4] && m[0] > m[8]) {
      const T s = sqrt(m[0] - m[4] - m[8] + 1) * 2;
      const T oneOverS = 1 / s;
      return Quaternion<T>((m[5] - m[7]) * oneOverS, static_cast<T>(0.25) * s,
                           (m[3] + m[1]) * oneOverS, (m[6] + m[2]) * oneOverS);
    } else if (m[4] > m[8]) {
      const T s = sqrt(m[4] - m[0] - m[8] + 1) * 2;
      const T oneOverS = 1 / s;
      return Quaternion<T>((m[6] - m[2]) * oneOverS, (m[3] + m[1]) * oneOverS,
                           static_cast<T>(0.25) * s, (m[5] + m[7]) * oneOverS);
    } else {
      const T s = sqrt(m[8] - m[0] - m[4] + 1) * 2;
      const T oneOverS = 1 / s;
      return Quaternion<T>((m[1] - m[3]) * oneOverS, (m[6] + m[2]) * oneOverS,
                           (m[5] + m[7]) * oneOverS, static_cast<T>(0.25) * s);
    }
  }

  /// @brief Calculate the dot product of two Quaternions.
  ///
  /// @param q1 First quaternion.
  /// @param q2 Second quaternion
  /// @return The scalar dot product of both Quaternions.
  static inline T DotProduct(const Quaternion<T>& q1, const Quaternion<T>& q2) {
    return q1.s_ * q2.s_ + Vector<T, 3>::DotProduct(q1.v_, q2.v_);
  }

  /// @brief Calculate the spherical linear interpolation between two
  /// Quaternions.
  ///
  /// @param q1 Start Quaternion.
  /// @param q2 End Quaternion.
  /// @param s1 The scalar value determining how far from q1 and q2 the
  /// resulting quaternion should be.  A value of 0 corresponds to q1 and a
  /// value of 1 corresponds to q2.
  /// @result Quaternion containing the result.
  static inline Quaternion<T> Slerp(const Quaternion<T>& q1,
                                    const Quaternion<T>& q2, const T& s1) {
    if (q1.s_ * q2.s_ + Vector<T, 3>::DotProduct(q1.v_, q2.v_) > 0.999999f)
      return Quaternion<T>(q1.s_ * (1 - s1) + q2.s_ * s1,
                           q1.v_ * (1 - s1) + q2.v_ * s1);
    return q1 * ((q1.Inverse() * q2) * s1);
  }

  /// @brief Access an element of the quaternion.
  ///
  /// @param i Index of the element to access.
  /// @return A reference to the accessed data that can be modified by the
  /// caller.
  inline T& operator[](const int i) {
    if (i == 0) return s_;
    return v_[i - 1];
  }

  /// @brief Access an element of the quaternion.
  ///
  /// @param i Index of the element to access.
  /// @return A const reference to the accessed.
  inline const T& operator[](const int i) const {
    return const_cast<Quaternion<T>*>(this)->operator[](i);
  }

  /// @brief Returns a vector that is perpendicular to the supplied vector.
  ///
  /// @param v1 An arbitrary vector
  /// @return A vector perpendicular to v1.  Normally this will just be
  /// the cross product of v1, v2.  If they are parallel or opposite though,
  /// the routine will attempt to pick a vector.
  static inline Vector<T, 3> PerpendicularVector(const Vector<T, 3>& v) {
    // We start out by taking the cross product of the vector and the x-axis to
    // find something parallel to the input vectors.  If that cross product
    // turns out to be length 0 (i. e. the vectors already lie along the x axis)
    // then we use the y-axis instead.
    Vector<T, 3> axis = Vector<T, 3>::CrossProduct(
        Vector<T, 3>(static_cast<T>(1), static_cast<T>(0), static_cast<T>(0)),
        v);
    // We use a fairly high epsilon here because we know that if this number
    // is too small, the axis we'll get from a cross product with the y axis
    // will be much better and more numerically stable.
    if (axis.LengthSquared() < static_cast<T>(0.05)) {
      axis = Vector<T, 3>::CrossProduct(
          Vector<T, 3>(static_cast<T>(0), static_cast<T>(1), static_cast<T>(0)),
          v);
    }
    return axis;
  }

  /// @brief Returns the a Quaternion that rotates from start to end.
  ///
  /// @param v1 The starting vector
  /// @param v2 The vector to rotate to
  /// @param preferred_axis the axis to use, if v1 and v2 are parallel.
  /// @return A Quaternion describing the rotation from v1 to v2
  /// See the comment on RotateFromToWithAxis for an explanation of the math.
  static inline Quaternion<T> RotateFromToWithAxis(
      const Vector<T, 3>& v1, const Vector<T, 3>& v2,
      const Vector<T, 3>& preferred_axis) {
    Vector<T, 3> start = v1.Normalized();
    Vector<T, 3> end = v2.Normalized();

    T dot_product = Vector<T, 3>::DotProduct(start, end);
    // Any rotation < 0.1 degrees is treated as no rotation
    // in order to avoid division by zero errors.
    // So we early-out in cases where it's less then 0.1 degrees.
    // cos( 0.1 degrees) = 0.99999847691
    if (dot_product >= static_cast<T>(0.99999847691)) {
      return Quaternion<T>::identity;
    }
    // If the vectors point in opposite directions, return a 180 degree
    // rotation, on the axis that they asked for.
    if (dot_product <= static_cast<T>(-0.99999847691)) {
      return Quaternion<T>(static_cast<T>(0), preferred_axis);
    }
    // Degenerate cases have been handled, so if we're here, we have to
    // actually compute the angle we want:
    Vector<T, 3> cross_product = Vector<T, 3>::CrossProduct(start, end);

    return Quaternion<T>(static_cast<T>(1.0) + dot_product, cross_product)
        .Normalized();
  }

  /// @brief Returns the a Quaternion that rotates from start to end.
  ///
  /// @param v1 The starting vector
  /// @param v2 The vector to rotate to
  /// @return A Quaternion describing the rotation from v1 to v2.  In the case
  /// where the vectors are parallel, it returns the identity.  In the case
  /// where
  /// they point in opposite directions, it picks an arbitrary axis.  (Since
  /// there
  /// are technically infinite possible quaternions to represent a 180 degree
  /// rotation.)
  ///
  /// The final equation used here is fairly elegant, but its derivation is
  /// not obvious:  We want to find the quaternion that represents the angle
  /// between Start and End.
  ///
  /// The angle can be expressed as a quaternion with the values:
  ///  angle: ArcCos(dotproduct(start, end) / (|start|*|end|)
  ///  axis: crossproduct(start, end).normalized * sin(angle/2)
  ///
  /// or written as:
  ///  quaternion(cos(angle/2), axis * sin(angle/2))
  ///
  /// Using the trig identity:
  ///  sin(angle * 2) = 2 * sin(angle) * cos*angle)
  /// Via substitution, we can turn this into:
  ///  sin(angle/2) = 0.5 * sin(angle)/cos(angle/2)
  ///
  /// Using this substitution, we get:
  ///  quaternion( cos(angle/2),
  ///             0.5 * crossproduct(start, end).normalized
  ///                 * sin(angle) / cos(angle/2))
  ///
  /// If we scale the whole thing up by 2 * cos(angle/2) then we get:
  ///  quaternion(2 * cos(angle/2) * cos(angle/2),
  ///             crossproduct(start, end).normalized * sin(angle))
  ///
  /// (Note that the quaternion is no longer normalized after this scaling)
  ///
  /// Another trig identity:
  ///   cos(angle/2) = sqrt((1 + cos(angle) / 2)
  ///
  /// Substituting this in, we can simplify the quaternion scalar:
  ///
  ///  quaternion(1 + cos(angle),
  ///             crossproduct(start, end).normalized * sin(angle))
  ///
  /// Because cross(start, end) has a magnitude of |start|*|end|*sin(angle),
  ///  crossproduct(start,end).normalized
  /// is equivalent to
  ///  crossproduct(start,end) / |start| * |end| * sin(angle)
  /// So after that substitution:
  ///
  ///  quaternion(1 + cos(angle),
  ///             crossproduct(start, end) / (|start| * |end|))
  ///
  /// dotproduct(start, end) has the value of |start| * |end| * cos(angle),
  /// so by algebra,
  ///  cos(angle) = dotproduct(start, end) / (|start| * |end|)
  /// we can replace our quaternion scalar here also:
  ///
  ///  quaternion(1 + dotproduct(start, end) / (|start| * |end|),
  ///             crossproduct(start, end) / (|start| * |end|))
  ///
  /// If start and end are normalized, then |start| * |end| = 1, giving us a
  /// final quaternion of:
  ///
  /// quaternion(1 + dotproduct(start, end), crossproduct(start, end))
  static inline Quaternion<T> RotateFromTo(const Vector<T, 3>& v1,
                                           const Vector<T, 3>& v2) {
    Vector<T, 3> start = v1.Normalized();
    Vector<T, 3> end = v2.Normalized();

    T dot_product = Vector<T, 3>::DotProduct(start, end);
    // Any rotation < 0.1 degrees is treated as no rotation
    // in order to avoid division by zero errors.
    // So we early-out in cases where it's less then 0.1 degrees.
    // cos( 0.1 degrees) = 0.99999847691
    if (dot_product >= static_cast<T>(0.99999847691)) {
      return Quaternion<T>::identity;
    }
    // If the vectors point in opposite directions, return a 180 degree
    // rotation, on an arbitrary axis.
    if (dot_product <= static_cast<T>(-0.99999847691)) {
      return Quaternion<T>(0, PerpendicularVector(start));
    }
    // Degenerate cases have been handled, so if we're here, we have to
    // actually compute the angle we want:
    Vector<T, 3> cross_product = Vector<T, 3>::CrossProduct(start, end);

    return Quaternion<T>(static_cast<T>(1.0) + dot_product, cross_product)
        .Normalized();
  }

  /// @brief Contains a quaternion doing the identity transform.
  static Quaternion<T> identity;

  MATHFU_DEFINE_CLASS_SIMD_AWARE_NEW_DELETE

 private:
  T s_;
  Vector<T, 3> v_;
};

template <typename T>
Quaternion<T> Quaternion<T>::identity = Quaternion<T>(1, 0, 0, 0);
/// @}

/// @addtogroup mathfu_quaternion
/// @{

/// @brief Multiply a Quaternion by a scalar.
///
/// This multiplies the angle of the rotation of the specified Quaternion
/// by a scalar factor.
/// @param s Scalar to multiply with.
/// @param q Quaternion to scale.
/// @return Quaternion containing the result.
///
/// @related Quaternion
template <class T>
inline Quaternion<T> operator*(const T& s, const Quaternion<T>& q) {
  return q * s;
}
/// @}

}  // namespace mathfu
#endif  // MATHFU_QUATERNION_H_