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

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

TSE::Vector3::Vector3() { }

TSE::Vector3::Vector3(float _x, float _y)
{
    x = _x;
    y = _y;
    z = 0;
}

TSE::Vector3::Vector3(float _x, float _y, float _z)
{
    x = _x;
    y = _y;
    z = _z;
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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