#include <J3ML/LinearAlgebra/Vector2.h>
#include <cassert>
#include <algorithm>
#include <valarray>
#include <iostream>

namespace J3ML::LinearAlgebra {

    Vector2::Vector2(): x(0), y(0)
    {}

    Vector2::Vector2(float X, float Y): x(X), y(Y)
    {}

    Vector2::Vector2(const Vector2& rhs): x(rhs.x), y(rhs.y)
    {}

    float Vector2::operator[](std::size_t index) const
    {
        return At(index);
    }

    float &Vector2::operator[](std::size_t index)
    {
        return At(index);
    }

    bool Vector2::IsWithinMarginOfError(const Vector2& rhs, float margin) const
    {
        return this->Distance(rhs) <= margin;
    }

    bool Vector2::operator==(const Vector2& rhs) const
    {
        return this->IsWithinMarginOfError(rhs);
    }

    bool Vector2::operator!=(const Vector2& rhs) const
    {
        return this->IsWithinMarginOfError(rhs) == false;
    }

    Vector2 Vector2::Min(const Vector2& min) const
    {
        return {
                std::min(this->x, min.x),
                std::min(this->y, min.y)
        };
    }

    Vector2 Vector2::Max(const Vector2& max) const
    {
        return {
                std::max(this->x, max.x),
                std::max(this->y, max.y)
        };
    }

    Vector2 Vector2::Clamp(const Vector2& min, const Vector2& max) const
    {
        return {
                std::clamp(this->x, min.x, max.x),
                std::clamp(this->y, min.y, max.y)
        };
    }

    float Vector2::Distance(const Vector2& to) const
    {
        return ((*this)-to).Magnitude();
    }

    float Vector2::Length() const
    {
        return std::sqrt(LengthSquared());
    }

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

    float Vector2::Magnitude() const
    {
        return std::sqrt(LengthSquared());
    }

    float Vector2::Dot(const Vector2& rhs) const
    {
        auto a = this->Normalize();
        auto b = rhs.Normalize();

        return a.x * b.x + a.y * b.y;
    }

    Vector2 Vector2::Project(const Vector2& rhs) const
    {
        float scalar = this->Dot(rhs) / (rhs.Magnitude()*rhs.Magnitude());
        return rhs * scalar;
    }

    Vector2 Vector2::Normalize() const
    {
        if (Length() > 0)
            return {
                    x / Length(),
                    y / Length()
            };
        else
            return {0,0};
    }

    Vector2 Vector2::Lerp(const Vector2& rhs, float alpha) const
    {
        return this->operator*(1.0f - alpha) + (rhs * alpha);
    }

    float Vector2::AngleBetween(const Vector2& rhs) const
    {
        auto numer = this->Dot(rhs);
        auto denom = this->Magnitude() * rhs.Magnitude();
        return std::acos(numer / denom);
    }

    float Vector2::AngleBetween(const Vector2& lhs, const Vector2& rhs)
    { return lhs.AngleBetween(rhs); }

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

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

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

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

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

    const Vector2 Vector2::Zero  = {0, 0};
    const Vector2 Vector2::Up    = {0, -1};
    const Vector2 Vector2::Down  = {0, 1};
    const Vector2 Vector2::Left  = {-1, 0};
    const Vector2 Vector2::Right = {1, 0};
    const Vector2 Vector2::NaN = {NAN, NAN};

    float Vector2::GetX() const { return x; }

    float Vector2::GetY() const { return y; }

    float Vector2::LengthSquared(const Vector2 &of) { return of.LengthSquared(); }

    Vector2 Vector2::Min(const Vector2 &value, const Vector2 &minimum) { return value.Min(minimum); }

    Vector2 Vector2::Max(const Vector2 &value, const Vector2 &maximum) { return value.Max(maximum);}

    float Vector2::Length(const Vector2 &of) { return of.Length(); }

    void Vector2::SetX(float newX) { x = newX;}

    void Vector2::SetY(float newY) { y = newY; }

    bool Vector2::IsNormalized(float epsilonSq) const {
        return std::abs(LengthSquared() - 1.f) <= epsilonSq;
    }

    bool Vector2::IsZero(float epsilonSq) const {
        return LengthSquared() <= epsilonSq;
    }

    bool Vector2::IsPerpendicular(const Vector2 &other, float epsilonSq) const {
        float dot = Dot(other);
        return dot*dot <= epsilonSq * LengthSquared() * other.LengthSquared();
    }

    Vector2 Vector2::Normalize(const Vector2 &of) { return of.Normalize(); }

    Vector2 Vector2::Project(const Vector2 &lhs, const Vector2 &rhs) { return lhs.Project(rhs); }

    float Vector2::Dot(const Vector2 &lhs, const Vector2 &rhs) { return lhs.Dot(rhs); }

    float Vector2::Magnitude(const Vector2 &of) { return of.Magnitude();}

    Vector2 Vector2::Lerp(const Vector2 &lhs, const Vector2 &rhs, float alpha) { return lhs.Lerp(rhs, alpha); }

    Vector2 Vector2::Div(const Vector2 &lhs, float rhs) {
        return lhs.Div(rhs);
    }

    Vector2 Vector2::Mul(const Vector2 &lhs, float rhs) {
        return lhs.Mul(rhs);
    }

    Vector2 Vector2::Sub(const Vector2 &lhs, const Vector2 &rhs) {
        return lhs.Sub(rhs);
    }

    Vector2 Vector2::Add(const Vector2 &lhs, const Vector2 &rhs) {
        return lhs.Add(rhs);
    }

    Vector2 Vector2::Add(const Vector2 &rhs) const {
        return *this + rhs;
    }

    Vector2 Vector2::Sub(const Vector2 &rhs) const {
        return *this - rhs;
    }

    Vector2 Vector2::Mul(float scalar) const {
        return *this * scalar;
    }

    Vector2 Vector2::Div(float scalar) const {
        return *this / scalar;
    }

    bool Vector2::IsFinite(const Vector2 &v) {
        return v.IsFinite();
    }

    float Vector2::MinElement() const {
        return std::min(x, y);
    }

    float Vector2::MaxElement() const {
        return std::max(x, y);
    }

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

    bool Vector2::IsFinite() const {
        return std::isfinite(x) && std::isfinite(y);
    }

    Vector2 Vector2::Div(const Vector2 &v) const {
        return {this->x/v.x, this->y/v.y};
    }

    Vector2 Vector2::Abs() const { return {std::abs(x), std::abs(y)};}

    float *Vector2::ptr() {
        return &x;
    }

    const float *Vector2::ptr() const { return &x;}

    const float Vector2::At(std::size_t index) const {
        assert(index >= 0);
        assert(index < Dimensions);
        return ptr()[index];
    }

    float &Vector2::At(std::size_t index) {
        assert(index >= 0);
        assert(index < Dimensions);
        return ptr()[index];
    }

    Vector2 &Vector2::operator/=(float scalar) {
        x /= scalar;
        y /= scalar;

        return *this;
    }

    Vector2 &Vector2::operator*=(float scalar) {
        x *= scalar;
        y *= scalar;

        return *this;
    }

    Vector2 &Vector2::operator-=(const Vector2 &rhs) // Subtracts a vector from this vector, in-place
    {
        x -= rhs.x;
        y -= rhs.y;

        return *this;
    }

    Vector2 &Vector2::operator+=(const Vector2 &rhs) // Adds a vector to this vector, in-place.
    {
        x += rhs.x;
        y += rhs.y;

        return *this;
    }

    Vector2 &Vector2::operator=(const Vector2 &rhs) {
        x = rhs.x;
        y = rhs.y;

        return *this;
    }

    Vector2::Vector2(const float *data) {
        assert(data);
        x = data[0];
        y = data[1];
    }

    Vector2::Vector2(float scalar) {
        x = scalar;
        y = scalar;
    }
}