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

// #if defined(AVR)
// #include <Arduino.h>
// #else
#include <math.h>
// #endif

/*
Vector2::Vector2() {
  x = 0;
  y = 0;
}
Vector2::Vector2(float _x, float _y) {
  x = _x;
  y = _y;
}
// Vector2::Vector2(Vec2 v) {
//   x = v.x;
//   y = v.y;
// }
Vector2::Vector2(Vector3 v) {
  x = v.Right();    // x;
  y = v.Forward();  // z;
}
Vector2::Vector2(PolarSingle p) {
  float horizontalRad = p.angle.InDegrees() * Deg2Rad;
  float cosHorizontal = cosf(horizontalRad);
  float sinHorizontal = sinf(horizontalRad);

  x = p.distance * sinHorizontal;
  y = p.distance * cosHorizontal;
}

Vector2::~Vector2() {}

const Vector2 Vector2::zero = Vector2(0, 0);
const Vector2 Vector2::one = Vector2(1, 1);
const Vector2 Vector2::right = Vector2(1, 0);
const Vector2 Vector2::left = Vector2(-1, 0);
const Vector2 Vector2::up = Vector2(0, 1);
const Vector2 Vector2::down = Vector2(0, -1);
const Vector2 Vector2::forward = Vector2(0, 1);
const Vector2 Vector2::back = Vector2(0, -1);

bool Vector2::operator==(const Vector2& v) {
  return (this->x == v.x && this->y == v.y);
}

float Vector2::Magnitude(const Vector2& v) {
  return sqrtf(v.x * v.x + v.y * v.y);
}
float Vector2::magnitude() const {
  return (float)sqrtf(x * x + y * y);
}
float Vector2::SqrMagnitude(const Vector2& v) {
  return v.x * v.x + v.y * v.y;
}
float Vector2::sqrMagnitude() const {
  return (x * x + y * y);
}

Vector2 Vector2::Normalize(const Vector2& v) {
  float num = Vector2::Magnitude(v);
  Vector2 result = Vector2::zero;
  if (num > Float::epsilon) {
    result = v / num;
  }
  return result;
}
Vector2 Vector2::normalized() const {
  float num = this->magnitude();
  Vector2 result = Vector2::zero;
  if (num > Float::epsilon) {
    result = ((Vector2) * this) / num;
  }
  return result;
}

Vector2 Vector2::operator-() {
  return Vector2(-this->x, -this->y);
}

Vector2 Vector2::operator-(const Vector2& v) const {
  return Vector2(this->x - v.x, this->y - v.y);
}
Vector2 Vector2::operator-=(const Vector2& v) {
  this->x -= v.x;
  this->y -= v.y;
  return *this;
}
Vector2 Vector2::operator+(const Vector2& v) const {
  return Vector2(this->x + v.x, this->y + v.y);
}
Vector2 Vector2::operator+=(const Vector2& v) {
  this->x += v.x;
  this->y += v.y;
  return *this;
}

Vector2 Vector2::Scale(const Vector2& v1, const Vector2& v2) {
  return Vector2(v1.x * v2.x, v1.y * v2.y);
}
// Vector2 Passer::LinearAlgebra::operator*(const Vector2 &v, float f) {
//   return Vector2(v.x * f, v.y * f);
// }
// Vector2 Passer::LinearAlgebra::operator*(float f, const Vector2 &v) {
//   return Vector2(v.x * f, v.y * f);
// }
Vector2 Vector2::operator*=(float f) {
  this->x *= f;
  this->y *= f;
  return *this;
}
// Vector2 Passer::LinearAlgebra::operator/(const Vector2 &v, float f) {
//   return Vector2(v.x / f, v.y / f);
// }
// Vector2 Passer::LinearAlgebra::operator/(float f, const Vector2 &v) {
//   return Vector2(v.x / f, v.y / f);
// }
Vector2 Vector2::operator/=(float f) {
  this->x /= f;
  this->y /= f;
  return *this;
}

float Vector2::Dot(const Vector2& v1, const Vector2& v2) {
  return v1.x * v2.x + v1.y * v2.y;
}

float Vector2::Distance(const Vector2& v1, const Vector2& v2) {
  return Magnitude(v1 - v2);
}

float Vector2::Angle(const Vector2& v1, const Vector2& v2) {
  return (float)fabs(SignedAngle(v1, v2));
}
float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
  float sqrMagFrom = v1.sqrMagnitude();
  float sqrMagTo = v2.sqrMagnitude();

  if (sqrMagFrom == 0 || sqrMagTo == 0)
    return 0;
  if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
#if defined(AVR)
    return NAN;
#else
    return nanf("");
#endif

  float angleFrom = atan2f(v1.y, v1.x);
  float angleTo = atan2f(v2.y, v2.x);
  return -(angleTo - angleFrom) * Rad2Deg;
}

Vector2 Vector2::Rotate(const Vector2& v, AngleSingle a) {
  float angleRad = a.InDegrees() * Deg2Rad;
#if defined(AVR)
  float sinValue = sin(angleRad);
  float cosValue = cos(angleRad);  // * Angle::Deg2Rad);
#else
  float sinValue = (float)sinf(angleRad);
  float cosValue = (float)cosf(angleRad);
#endif

  float tx = v.x;
  float ty = v.y;
  Vector2 r = Vector2((cosValue * tx) - (sinValue * ty),
                      (sinValue * tx) + (cosValue * ty));
  return r;
}

Vector2 Vector2::Lerp(const Vector2& v1, const Vector2& v2, float f) {
  Vector2 v = v1 + (v2 - v1) * f;
  return v;
}
  */

#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<T> p) {
  float horizontalRad = p.angle.InDegrees() * Deg2Rad;
  float cosHorizontal = cosf(horizontalRad);
  float sinHorizontal = sinf(horizontalRad);

  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);
  return sqrtf(sqrFloat);
}

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>
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>
float Vector2Of<T>::Distance(const Vector2Of& v1, const Vector2Of& v2) {
  return MagnitudeOf(v1 - v2);
}

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<T> Vector2Of<T>::UnsignedAngle(const Vector2Of& v1,
                                       const Vector2Of& v2) {
  return AngleOf<T>::Abs(SignedAngle(v1, v2));
}

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

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

  AngleOf<T> angleFrom = AngleOf<T>::Atan2(static_cast<float>(v1.vertical),
                                           static_cast<float>(v1.horizontal));
  AngleOf<T> angleTo = AngleOf<T>::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<T> a) {
  float sinValue = AngleOf<T>::Sin(a);
  float cosValue = AngleOf<T>::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> v = v1 + static_cast<Vector2Of<float>>(v2 - v1) * f;
  return v;
}

// float Vector2::Angle(const Vector2& v1, const Vector2& v2) {
//   return (float)fabs(SignedAngle(v1, v2));
// }
// float Vector2::SignedAngle(const Vector2& v1, const Vector2& v2) {
//   float sqrMagFrom = v1.sqrMagnitude();
//   float sqrMagTo = v2.sqrMagnitude();

//   if (sqrMagFrom == 0 || sqrMagTo == 0)
//     return 0;
//   if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
// #if defined(AVR)
//     return NAN;
// #else
//     return nanf("");
// #endif

//   float angleFrom = atan2f(v1.y, v1.x);
//   float angleTo = atan2f(v2.y, v2.x);
//   return -(angleTo - angleFrom) * Rad2Deg;
// }

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<float>;

#pragma endregion Vector2Of