// 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/.

#ifndef SWINGTWIST_H
#define SWINGTWIST_H

#include "Angle.h"
#include "Direction.h"
#include "Quaternion.h"
#include "Spherical.h"

namespace LinearAlgebra {

/// @brief An orientation using swing and twist angles in various
/// representations
/// @tparam T The implmentation type used for the representation of the angles
template <typename T>
class SwingTwistOf {
 public:
  DirectionOf<T> swing;
  AngleOf<T> twist;

  SwingTwistOf();
  SwingTwistOf(DirectionOf<T> swing, AngleOf<T> twist);
  SwingTwistOf(AngleOf<T> horizontal, AngleOf<T> vertical, AngleOf<T> twist);

  static SwingTwistOf<T> Degrees(float horizontal,
                                 float vertical = 0,
                                 float twist = 0);

  Quaternion ToQuaternion() const;
  static SwingTwistOf<T> FromQuaternion(Quaternion q);

  SphericalOf<T> ToAngleAxis() const;
  static SwingTwistOf<T> FromAngleAxis(SphericalOf<T> aa);

  const static SwingTwistOf<T> identity;

  bool operator==(const SwingTwistOf<T> d) const;

  /// <summary>
  /// Rotate a vector using this rotation
  /// </summary>
  /// <param name="vector">The vector to rotate</param>
  /// <returns>The rotated vector</returns>
  SphericalOf<T> operator*(const SphericalOf<T>& vector) const;
  /// <summary>
  /// Multiply this rotation with another rotation
  /// </summary>
  /// <param name="rotation">The swing/twist rotation to multiply with</param>
  /// <returns>The resulting swing/twist rotation</returns>
  /// The result will be this rotation rotated according to
  /// the give rotation.
  SwingTwistOf<T> operator*(const SwingTwistOf<T>& rotation) const;
  SwingTwistOf<T> operator*=(const SwingTwistOf<T>& rotation);

  static SwingTwistOf<T> Inverse(SwingTwistOf<T> rotation);

  /// <summary>
  /// Convert an angle/axis representation to a swingt
  /// </summary>
  /// <param name="angle">The angle</param>
  /// <param name="axis">The axis</param>
  /// <returns>The resulting quaternion</returns>
  static SwingTwistOf<T> AngleAxis(float angle, const DirectionOf<T>& axis);

  static AngleOf<T> Angle(const SwingTwistOf<T>& r1, const SwingTwistOf<T>& r2);

  void Normalize();
};

using SwingTwistSingle = SwingTwistOf<float>;
using SwingTwist16 = SwingTwistOf<signed short>;

#if defined(ARDUINO)
using SwingTwist = SwingTwist16;
#else
using SwingTwist = SwingTwistSingle;
#endif

}  // namespace LinearAlgebra
using namespace LinearAlgebra;

#endif