// 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 "Direction.h"

#include "Quaternion.h"
#include "Vector3.h"

#include <math.h>

template <typename T>
DirectionOf<T>::DirectionOf() {
  this->horizontal = AngleOf<T>();
  this->vertical = AngleOf<T>();
}

template <typename T>
DirectionOf<T>::DirectionOf(AngleOf<T> horizontal, AngleOf<T> vertical) {
  this->horizontal = horizontal;
  this->vertical = vertical;
  Normalize();
};

// template <typename T>
// DirectionOf<T>::DirectionOf(Vector3 v) {
//   this->horizontal = AngleOf<T>::Atan2(
//       v.Right(),
//       v.Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
//   this->vertical =
//       -AngleOf<T>::deg90 -
//       AngleOf<T>::Acos(
//           v.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
//   Normalize();
// }

template <typename T>
const DirectionOf<T> DirectionOf<T>::forward =
    DirectionOf<T>(AngleOf<T>(), AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::back =
    DirectionOf<T>(AngleOf<T>::deg180, AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::up =
    DirectionOf<T>(AngleOf<T>(), AngleOf<T>::deg90);
template <typename T>
const DirectionOf<T> DirectionOf<T>::down =
    DirectionOf<T>(AngleOf<T>(), -AngleOf<T>::deg90);
template <typename T>
const DirectionOf<T> DirectionOf<T>::left =
    DirectionOf<T>(-AngleOf<T>::deg90, AngleOf<T>());
template <typename T>
const DirectionOf<T> DirectionOf<T>::right =
    DirectionOf<T>(AngleOf<T>::deg90, AngleOf<T>());

template <typename T>
Vector3 Passer::LinearAlgebra::DirectionOf<T>::ToVector3() const {
  Quaternion q = Quaternion::Euler(-this->vertical.InDegrees(),
                                   this->horizontal.InDegrees(), 0);
  Vector3 v = q * Vector3::forward;
  return v;
}

template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::FromVector3(Vector3 v) {
  DirectionOf<T> d;
  d.horizontal = AngleOf<T>::Atan2(
      v.Right(),
      v.Forward());  // AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
  d.vertical =
      -AngleOf<T>::deg90 -
      AngleOf<T>::Acos(
          v.Up());  // AngleOf<T>::Radians(-(0.5f * pi) - acosf(v.Up()));
  d.Normalize();
  return d;
}

template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Degrees(float horizontal,
                                                              float vertical) {
  return DirectionOf<T>(AngleOf<T>::Degrees(horizontal),
                        AngleOf<T>::Degrees(vertical));
}

template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::Radians(float horizontal,
                                                              float vertical) {
  return DirectionOf<T>(AngleOf<T>::Radians(horizontal),
                        AngleOf<T>::Radians(vertical));
}

template <typename T>
bool Passer::LinearAlgebra::DirectionOf<T>::operator==(
    const DirectionOf<T> d) const {
  return (this->horizontal == d.horizontal) && (this->vertical == d.vertical);
}

template <typename T>
DirectionOf<T> Passer::LinearAlgebra::DirectionOf<T>::operator-() const {
  DirectionOf<T> r =
      DirectionOf<T>(this->horizontal + AngleOf<T>::deg180, -this->vertical);
  return r;
}

template <typename T>
Vector3 DirectionOf<T>::ToVector3() {
  Vector3 v = Quaternion::Euler(-this->vertical.InDegrees(),
                                this->horizontal.InDegrees(), 0) *
              Vector3::forward;
  return v;
}

template <typename T>
void DirectionOf<T>::Normalize() {
  if (this->vertical > AngleOf<T>::deg90 ||
      this->vertical < -AngleOf<T>::deg90) {
    this->horizontal += AngleOf<T>::deg180;
    this->vertical = AngleOf<T>::deg180 - this->vertical;
  }
}

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