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

#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() {}

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

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

float Vector2::SqrMagnitude(const Vector2& a) {
  return a.x * a.x + a.y * a.y;
}
float Vector2::sqrMagnitude() const {
  return (x * x + y * y);
}

Vector2 Vector2::Normalize(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-(const Vector2& v2) const {
  return Vector2(this->x - v2.x, this->y - v2.y);
}

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

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

Vector2 Vector2::Scale(const Vector2& p1, const Vector2& p2) {
  return Vector2(p1.x * p2.x, p1.y * p2.y);
}

Vector2 Vector2::operator*(float f) const {
  return Vector2(this->x * f, this->y * f);
}

Vector2 Vector2::operator/(const float& d) {
  return Vector2(this->x / d, this->y / d);
}

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

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

float Vector2::Distance(const Vector2& p1, const Vector2& p2) {
  return Magnitude(p1 - p2);
}

float Vector2::Angle(Vector2 from, Vector2 to) {
  return (float)fabs(SignedAngle(from, to));
}

float Vector2::SignedAngle(Vector2 from, Vector2 to) {
  float sqrMagFrom = from.sqrMagnitude();
  float sqrMagTo = to.sqrMagnitude();

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

  float angleFrom = atan2(from.y, from.x);
  float angleTo = atan2(to.y, to.x);
  return (angleTo - angleFrom) * Angle::Rad2Deg;
}

Vector2 Vector2::Rotate(Vector2 v, float angle) {
#if defined(AVR)
  float sinValue = sin(angle * Angle::Deg2Rad);
  float cosValue = cos(angle * Angle::Deg2Rad);
#else
  float sinValue = (float)sinf(angle * Angle::Deg2Rad);
  float cosValue = (float)cosf(angle * Angle::Deg2Rad);
#endif

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

Vector2 Vector2::Lerp(Vector2 from, Vector2 to, float f) {
  Vector2 v = from + (to - from) * f;
  return v;
}

float Vector2::ToFactor(Vector2 a, Vector2 b) {
  return (1 - Vector2::Dot(a, b)) / 2;
}