// 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>(0.0f);
  this->vertical = AngleOf<T>(0.0f);
}

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 =
      atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg;
  this->vertical = 90 - acosf(v.Up()) * Passer::LinearAlgebra::Rad2Deg;
  Normalize();
}

template <typename T>
const DirectionOf<T> DirectionOf<T>::forward = DirectionOf<T>(0.0f, 0.0f);
template <typename T>
const DirectionOf<T> DirectionOf<T>::back = DirectionOf<T>(180.0f, 0.0f);
template <typename T>
const DirectionOf<T> DirectionOf<T>::up = DirectionOf<T>(0.0f, 90.0f);
template <typename T>
const DirectionOf<T> DirectionOf<T>::down = DirectionOf<T>(0.0f, -90.0f);
template <typename T>
const DirectionOf<T> DirectionOf<T>::left = DirectionOf<T>(-90.0f, 0.0f);
template <typename T>
const DirectionOf<T> DirectionOf<T>::right = DirectionOf<T>(90.0f, 0.0f);

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>::Degrees(180),
                                    -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>::Degrees(90) ||
      this->vertical < AngleOf<T>::Degrees(-90)) {
    this->horizontal += AngleOf<T>::Degrees(180);
    this->vertical = AngleOf<T>::Degrees(180) - this->vertical;
  }
}

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