// 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->horizontal = AngleOf<T>(0);
  // this->vertical = AngleOf<T>(0);
  this->swing = DirectionOf<T>(0, 0);
  this->twist = AngleOf<T>(0);
}

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>(AngleOf<T>::Degrees(horizontal),
                                        AngleOf<T>::Degrees(vertical));
  AngleOf<T> twistAngle = AngleOf<T>::Degrees(twist);
  SwingTwistOf<T> orientation = SwingTwistOf(swing, twistAngle);
  return orientation;
}

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

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

  return r;
}

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.horizontal + this->swing.horizontal,
                                    vector.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->swing += rotation.swing;
  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, -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>;