// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.

#include "SwingTwist.h"

template <typename T>
SwingTwistOf<T>::SwingTwistOf() {
  this->swing = DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
  this->twist = AngleOf<T>();
}

template <typename T>
Passer::LinearAlgebra::SwingTwistOf<T>::SwingTwistOf(DirectionOf<T> swing,
                                                     AngleOf<T> twist) {
  this->swing = swing;
  this->twist = twist;
}

template <typename T>
SwingTwistOf<T>::SwingTwistOf(AngleOf<T> horizontal,
                              AngleOf<T> vertical,
                              AngleOf<T> twist) {
  // this->horizontal = horizontal;
  // this->vertical = vertical;
  this->swing = DirectionOf<T>(horizontal, vertical);
  this->twist = twist;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::Degrees(float horizontal,
                                         float vertical,
                                         float twist) {
  DirectionOf<T> swing = DirectionOf<T>::Degrees(horizontal, vertical);
  AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
  SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
  return orientation;
}

template <typename T>
Quaternion SwingTwistOf<T>::ToQuaternion() const {
  Quaternion q = Quaternion::Euler(this->swing.vertical.InDegrees(),
                                   this->swing.horizontal.InDegrees(),
                                   this->twist.InDegrees());
  return q;
}

template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromQuaternion(
    Quaternion q) {
  Vector3 angles = Quaternion::ToAngles(q);
  SwingTwistOf<T> r =
      SwingTwistOf<T>::Degrees(angles.Up(), angles.Right(), angles.Forward());

  return r;
}

template <typename T>
SphericalOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::ToAngleAxis() const {
  Quaternion q = this->ToQuaternion();
  float angle;
  Vector3 axis;
  q.ToAngleAxis(&angle, &axis);
  DirectionOf<T> direction = DirectionOf<T>::FromVector3(axis);

  SphericalOf<T> aa = SphericalOf<T>(angle, direction);
  return aa;
}

template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::FromAngleAxis(
    SphericalOf<T> aa) {
  Vector3 vectorAxis = aa.direction.ToVector3();
  Quaternion q = Quaternion::AngleAxis(aa.distance, vectorAxis);
  return SwingTwistOf<T>();
}

template <typename T>
const SwingTwistOf<T> SwingTwistOf<T>::identity = SwingTwistOf();

template <typename T>
SphericalOf<T> SwingTwistOf<T>::operator*(const SphericalOf<T>& vector) const {
  SphericalOf<T> v = SphericalOf<T>(
      vector.distance, vector.direction.horizontal + this->swing.horizontal,
      vector.direction.vertical + this->swing.vertical);
  return v;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*(
    const SwingTwistOf<T>& rotation) const {
  SwingTwistOf<T> r =
      SwingTwistOf(this->swing.horizontal + rotation.swing.horizontal,
                   this->swing.vertical + rotation.swing.vertical,
                   this->twist + rotation.twist);
  return r;
}

template <typename T>
SwingTwistOf<T> SwingTwistOf<T>::operator*=(const SwingTwistOf<T>& rotation) {
  this->swing.horizontal += rotation.swing.horizontal;
  this->swing.vertical += rotation.swing.vertical;
  this->twist += rotation.twist;
  return *this;
}

template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::Inverse(
    SwingTwistOf<T> rotation) {
  SwingTwistOf<T> r = SwingTwistOf<T>(
      -rotation.swing.horizontal, -rotation.swing.vertical, -rotation.twist);
  return r;
}

template <typename T>
SwingTwistOf<T> Passer::LinearAlgebra::SwingTwistOf<T>::AngleAxis(
    float angle,
    const SphericalOf<T>& axis) {
  Vector3 axis_vector = axis.ToVector3();
  Quaternion q = Quaternion::AngleAxis(angle, axis_vector);
  SwingTwistOf<T> r = SwingTwistOf<T>::FromQuaternion(q);
  return r;
}

template class SwingTwistOf<float>;
template class SwingTwistOf<signed short>;