#include "Vector2.hpp"
#include "Vector3.hpp"
#include "Vector4.hpp"
#include "MathF.hpp"
#include <cmath>

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

TSE::Vector2::Vector2() { }

TSE::Vector2::Vector2(float _x, float _y)
{
    x = _x;
    y = _y;
}

TSE::Vector2::Vector2(const Vector2 &other)
{
    x = other.x;
    y = other.y;
}

TSE::Vector2::Vector2(const Vector3 &other)
{
    x = other.x;
    y = other.y;
}

TSE::Vector2::Vector2(const Vector4 &other)
{
    x = other.x;
    y = other.y;
}

float TSE::Vector2::Length() const
{
    return std::sqrt(x*x + y*y);
}

float TSE::Vector2::LengthSqr() const
{
    return x*x + y*y;
}

bool TSE::Vector2::IsValid() const
{
    return !std::isnan(x) && !std::isnan(y);
}

void TSE::Vector2::Normalize()
{
    float lensq = LengthSqr();
    if(lensq <= TSE_EPSILON) return;
    const float inv = 1.0f / std::sqrt(lensq);
    x *= inv; y *= inv;
}

void TSE::Vector2::NormalizeSafe(const Vector2 &fallback)
{
    float lensq = LengthSqr();
    if(lensq <= TSE_EPSILON)
    {
        *this = fallback;
        return;
    }
    const float inv = 1.0f / std::sqrt(lensq);
    x *= inv; y *= inv;
    if(!IsValid())
    {
        *this = fallback;
    }
}

TSE::string TSE::Vector2::ToString() const
{
    return "(" + std::to_string(x) + "|" + std::to_string(y) + ")";
}

TSE::Vector3 TSE::Vector2::ToVector3() const
{
    return Vector3(x,y,0);
}

TSE::Vector4 TSE::Vector2::ToVector4() const
{
    return Vector4(x,y,0,0);
}

float TSE::Vector2::Distance(const Vector2 &a, const Vector2 &b)
{
    return (a - b).Length();
}

float TSE::Vector2::DistanceSqr(const Vector2 &a, const Vector2 &b)
{
    return (a - b).LengthSqr();
}

TSE::Vector2 TSE::Vector2::Normalize(const Vector2 &v)
{
    Vector2 result = v;
    result.Normalize();
    return result;
}

TSE::Vector2 TSE::Vector2::NormalizeSafe(const Vector2 &v, const Vector2 &fallback)
{
    Vector2 result = v;
    result.NormalizeSafe(fallback);
    return result;
}

TSE::Vector2 TSE::Vector2::Lerp(const Vector2 &a, const Vector2 &b, float t)
{
    return a + (b - a) * t;
}

TSE::Vector2 TSE::Vector2::Berp(const Vector2 &a, const Vector2 &b, const Vector2 &c, float t)
{
    return Lerp(Lerp(a, b, t), Lerp(b, c, t), t);
}

TSE::Vector2 TSE::Vector2::Midpoint(const Vector2 &a, const Vector2 &b)
{
    return (a + b) / 2.0f;
}

float TSE::Vector2::Dot(const Vector2 &a, const Vector2 &b)
{
    return a.x * b.x + a.y * b.y;
}

float TSE::Vector2::Cross(const Vector2 &a, const Vector2 &b)
{
    return a.x * b.y - a.y * b.x;
}

TSE::Vector2 TSE::Vector2::operator+(const Vector2 &other) const
{
    return Vector2(x + other.x, y + other.y);
}

TSE::Vector2 TSE::Vector2::operator+=(const Vector2 &other)
{
    *this = *this + other;
    return *this;
}

TSE::Vector2 TSE::Vector2::operator-(const Vector2 &other) const
{
    return Vector2(x - other.x, y - other.y);
}

TSE::Vector2 TSE::Vector2::operator-=(const Vector2 &other)
{
    *this = *this - other;
    return *this;
}

TSE::Vector2 TSE::Vector2::operator*(const Vector2 &other) const
{
    return Vector2(x * other.x, y * other.y);
}

TSE::Vector2 TSE::Vector2::operator*=(const Vector2 &other)
{
    *this = *this * other;
    return *this;
}

TSE::Vector2 TSE::Vector2::operator/(const Vector2 &other) const
{
    return Vector2(x / other.x, y / other.y);
}

TSE::Vector2 TSE::Vector2::operator/=(const Vector2 &other)
{
    *this = *this / other;
    return *this;
}

TSE::Vector2 TSE::Vector2::operator*(const float other) const
{
    return Vector2(x * other, y * other);
}

TSE::Vector2 TSE::Vector2::operator*=(const float other)
{
    *this = *this * other;
    return *this;
}

TSE::Vector2 TSE::Vector2::operator/(const float other) const
{
    return Vector2(x / other, y / other);
}

TSE::Vector2 TSE::Vector2::operator/=(const float other)
{
    *this = *this / other;
    return *this;
}

bool TSE::Vector2::operator==(const Vector2 &other) const
{
    return x == other.x && y == other.y;
}

bool TSE::Vector2::operator!=(const Vector2 &other) const
{
    return !(*this == other);
}