// 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 "Vector2.h"
#include "Angle.h"
#include "FloatSingle.h"
#include "Vector3.h"

#include <math.h>

#pragma region Vector2Of

template <typename T>
Vector2Of<T>::Vector2Of() {}

template <typename T>
Vector2Of<T>::Vector2Of(T horizontal, T vertical)
    : horizontal(horizontal), vertical(vertical) {}

template <typename T>
Vector2Of<float> Vector2Of<T>::FromPolar(PolarOf<float> p) {
  float cosHorizontal = AngleOf<float>::Cos(p.angle);
  float sinHorizontal = AngleOf<float>::Sin(p.angle);

  Vector2Of<float> v;
  v.horizontal = p.distance * sinHorizontal;
  v.vertical = p.distance * cosHorizontal;
  return v;
}

template <typename T>
const Vector2Of<T> Vector2Of<T>::zero = Vector2Of(T{}, T{});

template <typename T>
const Vector2Of<T> Vector2Of<T>::forward = Vector2Of(T{}, 1);

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

template <typename T>
float Vector2Of<T>::MagnitudeOf(const Vector2Of& v) {
  T sqr = v.horizontal * v.horizontal + v.vertical * v.vertical;
  float sqrFloat = static_cast<float>(sqr);
  return sqrtf(sqrFloat);
}

template <typename T>
float Vector2Of<T>::Magnitude() const {
  T sqr = this->horizontal * this->horizontal + this->vertical * this->vertical;
  float sqrFloat = static_cast<float>(sqr);
  float r = sqrtf(sqrFloat);
  return r;
}

template <typename T>
float Vector2Of<T>::SqrMagnitudeOf(const Vector2Of& v) {
  T sqr = v.horizontal * v.horizontal + v.vertical * v.vertical;
  return static_cast<float>(sqr);
}

template <typename T>
float Vector2Of<T>::SqrMagnitude() const {
  T sqr = this->horizontal * this->horizontal + this->vertical * this->vertical;
  return static_cast<float>(sqr);
}

template <typename T>
float Vector2Of<T>::Distance(const Vector2Of& v1, const Vector2Of& v2) {
  Vector2Of<float> delta = v1 - v2;
  float r = MagnitudeOf(delta);
  return r;
}

template <typename T>
Vector2Of<T> Vector2Of<T>::operator-() {
  return Vector2Of<T>(-this->horizontal, -this->vertical);
}

template <typename T>
Vector2Of<T> Vector2Of<T>::operator-(const Vector2Of& v) const {
  return Vector2Of(this->horizontal - v.horizontal,
                   this->vertical - v.vertical);
}

template <typename T>
Vector2Of<T> Vector2Of<T>::operator-=(const Vector2Of& v) {
  this->horizontal -= v.horizontal;
  this->vertical -= v.vertical;
  return *this;
}

template <typename T>
Vector2Of<T> Vector2Of<T>::operator+(const Vector2Of& v) const {
  return Vector2Of(this->horizontal + v.horizontal,
                   this->vertical + v.vertical);
}

template <typename T>
Vector2Of<T> Vector2Of<T>::operator+=(const Vector2Of& v) {
  this->horizontal += v.horizontal;
  this->vertical += v.vertical;
  return *this;
}

template <typename T>
Vector2Of<T> Vector2Of<T>::Scale(const Vector2Of& v1, const Vector2Of& v2) {
  return Vector2Of<T>(v1.horizontal * v2.horizontal, v1.vertical * v2.vertical);
}

// template <typename T>
// Vector2Of<float> Vector2Of<T>::operator/=(float f) {
//   this->x /= f;
//   this->y /= f;
//   return *this;
// }

template <typename T>
T Vector2Of<T>::Dot(const Vector2Of& v1, const Vector2Of& v2) {
  return v1.horizontal * v2.horizontal + v1.vertical * v2.vertical;
}

template <typename T>
Vector2Of<float> Vector2Of<T>::Normalize(const Vector2Of<T>& v) {
  float num = Vector2Of<T>::MagnitudeOf(v);
  Vector2Of<float> result = Vector2Of::zero;
  if (num > Float::epsilon) {
    result = static_cast<Vector2Of<float>>(v) / num;
  }
  return result;
}

template <typename T>
AngleOf<float> Vector2Of<T>::UnsignedAngle(const Vector2Of& v1,
                                           const Vector2Of& v2) {
  return AngleOf<float>::Abs(SignedAngle(v1, v2));
}

template <typename T>
AngleOf<float> Vector2Of<T>::SignedAngle(const Vector2Of& v1,
                                         const Vector2Of& v2) {
  float sqrMagFrom = v1.SqrMagnitude();
  float sqrMagTo = v2.SqrMagnitude();

  if (sqrMagFrom == 0 || sqrMagTo == 0)
    return AngleOf<float>::zero;
  if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
    return AngleOf<float>::zero;  // Angle does not support NaN...

  AngleOf<float> angleFrom = AngleOf<float>::Atan2(
      static_cast<float>(v1.vertical), static_cast<float>(v1.horizontal));
  AngleOf<float> angleTo = AngleOf<float>::Atan2(
      static_cast<float>(v2.vertical), static_cast<float>(v2.horizontal));
  return -(angleTo - angleFrom);
}

template <typename T>
Vector2Of<float> Vector2Of<T>::Rotate(const Vector2Of& v, AngleOf<float> a) {
  float sinValue = AngleOf<float>::Sin(a);
  float cosValue = AngleOf<float>::Cos(a);
  float tx = static_cast<float>(v.horizontal);
  float ty = static_cast<float>(v.vertical);
  Vector2Of<float> r = Vector2Of<float>((cosValue * tx) - (sinValue * ty),
                                        (sinValue * tx) + (cosValue * ty));
  return r;
}

template <typename T>
Vector2Of<float> Vector2Of<T>::Lerp(const Vector2Of& v1,
                                    const Vector2Of& v2,
                                    float f) {
  Vector2Of<float> v1f = (Vector2Of<float>)v1;
  Vector2Of<float> delta = v2 - v1;
  Vector2Of<float> r = v1f + delta * f;
  return r;
}

template <typename T>
Vector2Of<float> Vector2Of<T>::Normalized() const {
  float num = Vector2Of<T>::MagnitudeOf(*this);
  Vector2Of<float> result = Vector2Of::zero;
  if (num > Float::epsilon) {
    result = static_cast<Vector2Of<float>>(*this) / num;
  }
  return result;
}

// Explicit instantiation for int
template class Vector2Of<signed short>;
template class Vector2Of<int>;
template class Vector2Of<float>;

#pragma endregion Vector2Of