#include <types/vector.h>

namespace LinearAlgebra {



#pragma region vector2

    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)
    {
        assert(index < 2);
        if (index == 0) return x;
        if (index == 1) return y;
        return 0;
    }

    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};
#pragma endregion

#pragma region vector3

    const Vector3 Vector3::Zero     = {0,0,0};
    const Vector3 Vector3::Up       = {0, -1, 0};
    const Vector3 Vector3::Down     = {0, 1, 0};
    const Vector3 Vector3::Left     = {-1, 0, 0};
    const Vector3 Vector3::Right    = {1, 0, 0};
    const Vector3 Vector3::Forward  = {0, 0, -1};
    const Vector3 Vector3::Backward = {0, 0, 1};

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

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

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


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

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

    Vector4::Vector4(): x(0), y(0), z(0), w(0)
    {}

    Vector4::Vector4(float X, float Y, float Z, float W): x(X),y(Y),z(Z),w(W)
    { }


    Vector3::Vector3(): x(0), y(0), z(0)
    {}

    Vector3::Vector3(float X, float Y, float Z): x(X), y(Y), z(Z)
    {}

    Vector3::Vector3(const Vector3& rhs)
    {
        this->x = rhs.x;
        this->y = rhs.y;
        this->z = rhs.z;
    }

    Vector3& Vector3::operator=(const Vector3& rhs)
    {
        this->x = rhs.x;
        this->y = rhs.y;
        this->z = rhs.z;
        return *this;
    }

    float Vector3::operator[](std::size_t index) const
    {
        assert(index < 3);
        if (index==0) return x;
        if (index==1) return y;
        if (index==2) return z;
        return 0;
    }

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

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

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

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

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

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

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

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

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

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

    float Vector3::Dot(const Vector3& rhs) const
    {
        auto a = this->Normalize();
        auto b = rhs.Normalize();
        return a.x * b.x +
               a.y * b.y +
               a.z * b.z;
    }

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

    Vector3 Vector3::Cross(const Vector3& rhs) const
    {
        return {
                this->y * rhs.z - this->z * rhs.y,
                this->z * rhs.x - this->x * rhs.z,
                this->x * rhs.y - this->y * rhs.x
        };
    }

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

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

#pragma endregion

#pragma region vector4


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

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

    Vector4 Vector4::min(const Vector4& min) const
    {
        return {
                std::min(this->x, min.x),
                std::min(this->y, min.y),
                std::min(this->z, min.z),
                std::min(this->w, min.w)
        };
    }

    Vector4 Vector4::max(const Vector4& max) const
    {
        return {
                std::max(this->x, max.x),
                std::max(this->y, max.y),
                std::max(this->z, max.z),
                std::max(this->w, max.w)
        };
    }

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

    float Vector4::distance(const Vector4& to) const
    {
        return ( (*this) - to ).magnitude();
    }

    float Vector4::length() const
    {
        return std::sqrt(lengthSquared());
    }

    float Vector4::lengthSquared() const
    {
        return (x*x + y*y + z*z + w*w);
    }

    float Vector4::magnitude() const
    {
        return std::sqrt(x*x + y*y + z*z + w*w);
    }

    float Vector4::dot(const Vector4& rhs) const
    {
        auto a = this->normalize();
        auto b = rhs.normalize();

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

    Vector4 Vector4::project(const Vector4& rhs) const
    {
        float scalar = this->dot(rhs) / (rhs.magnitude()* rhs.magnitude());
        return rhs * scalar;
    }

    Vector4 Vector4::normalize() const
    {
        if (length() > 0)
            return {
                    x / length(),
                    y / length(),
                    z / length(),
                    w / length()
            };
        else
            return {0,0,0,0};
    }

    Vector4 Vector4::lerp(const Vector4& goal, float alpha) const
    {
        return this->operator*(1.0f - alpha) + (goal * alpha);
    }

    Vector4 Vector4::operator+(const Vector4& rhs) const
    {
        return {x+rhs.x, y+rhs.y, z+rhs.z, w+rhs.w};
    }

    Vector4 Vector4::operator-(const Vector4& rhs) const
    {
        return {x-rhs.x, y-rhs.y, z-rhs.z, w-rhs.w};
    }



#pragma endregion



#pragma region EulerAngle

    EulerAngle::EulerAngle(float pitch, float yaw, float roll): pitch(pitch), yaw(yaw), roll(roll)
    {}

    float EulerAngle::GetPitch(float pitch_limit) const
    { return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }

    float EulerAngle::GetYaw(float yaw_limit) const
    { return std::clamp(std::remainderf(yaw, 360.f), -yaw_limit, yaw_limit);   }

    float EulerAngle::GetRoll(float pitch_limit) const
    { return std::clamp( std::remainderf(pitch,360.f), -pitch_limit, pitch_limit); }

    bool EulerAngle::operator==(const EulerAngle& a) const
    {
        return (pitch == a.pitch) && (yaw == a.yaw) && (roll == a.roll);
    }

    void EulerAngle::clamp()
    {
        if (this->pitch > 89.0f)
            this->pitch = 89.0f;
        if (this->pitch <= -89.0f)
            this->pitch = -89.0f;
        //TODO: Make this entirely seamless by getting the amount they rotated passed -180 and +180 by.
        if (this->yaw <= -180.0f)
            this->yaw = 180.0f;
        if (this->yaw >= 180.01f)
            this->yaw = -179.9f;
        if (this->roll >= 360.0f)
            this->roll = 0.0;
        if (this->roll <= -360.0f)
            this->roll = 0.0;
    }

    EulerAngle EulerAngle::movementAngle() const
    {
        EulerAngle a;
        a.pitch = (cos(glm::radians(yaw)) * cos(glm::radians(pitch)));
        a.yaw = -sin(glm::radians(pitch));
        a.roll = (sin(glm::radians(yaw)) * cos(glm::radians(pitch)));
        return a;
    }


    const Matrix3x3 Matrix3x3::Zero = Matrix3x3(0,0,0, 0,0,0, 0,0,0);
    const Matrix3x3 Matrix3x3::Identity = Matrix3x3(1,0,0, 0,1,0, 0,0,1);
    const Matrix3x3 Matrix3x3::NaN = Matrix3x3(NAN);

    Matrix3x3::Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21,
                         float m22)
    {
        this->elems[0][0] = m00;
        this->elems[0][1] = m01;
        this->elems[0][2] = m02;

        this->elems[1][0] = m10;
        this->elems[1][1] = m11;
        this->elems[1][2] = m12;

        this->elems[2][0] = m20;
        this->elems[2][1] = m21;
        this->elems[2][2] = m22;
    }

    Vector3 Matrix3x3::GetRow(int index) const
    {
        float x = this->elems[index][0];
        float y = this->elems[index][1];
        float z = this->elems[index][2];
        return {x,y,z};
    }

    Vector3 Matrix3x3::GetColumn(int index) const
    {
        float x = this->elems[0][index];
        float y = this->elems[1][index];
        float z = this->elems[2][index];

        return {x,y,z};
    }

    float Matrix3x3::At(int x, int y) const
    {
        return this->elems[x][y];

    }

    Vector3 Matrix3x3::operator*(const Vector3& rhs) const
    {
        return {
                At(0,0) * rhs.x + At(0, 1) * rhs.y + At(0, 2) * rhs.z,
                At(1, 0) * rhs.x + At(1, 1) * rhs.y + At(1, 2) * rhs.z,
                At(2, 0) * rhs.x + At(2, 1) * rhs.y + At(2,2) * rhs.z
        };
    }

    Matrix3x3 Matrix3x3::operator*(const Matrix3x3& rhs) const
    {
        //Matrix3x3 r;
        auto m00 = At(0, 0) * rhs.At(0, 0) + At(0, 1) * rhs.At(1, 0) + At(0, 2) * rhs.At(2, 0);
        auto m01 = At(0, 0) * rhs.At(0, 1) + At(0, 1) * rhs.At(1, 1) + At(0, 2) * rhs.At(2, 1);
        auto m02 = At(0, 0) * rhs.At(0, 2) + At(0, 1) * rhs.At(1, 2) + At(0, 2) * rhs.At(2, 2);

        auto m10 = At(1, 0) * rhs.At(0, 0) + At(1, 1) * rhs.At(1, 0) + At(1, 2) * rhs.At(2, 0);
        auto m11 = At(1, 0) * rhs.At(0, 1) + At(1, 1) * rhs.At(1, 1) + At(1, 2) * rhs.At(2, 1);
        auto m12 = At(1, 0) * rhs.At(0, 2) + At(1, 1) * rhs.At(1, 2) + At(1, 2) * rhs.At(2,2);

        auto m20 = At(2, 0) * rhs.At(0, 0) + At(2, 1) * rhs.At(1, 0) + At(2,2) * rhs.At(2, 0);
        auto m21 = At(2, 0) * rhs.At(0, 1) + At(2, 1) * rhs.At(1, 1) + At(2,2) * rhs.At(2, 1);
        auto m22 = At(2, 0) * rhs.At(0, 2) + At(2, 1) * rhs.At(1, 2) + At(2,2) * rhs.At(2,2);

        return Matrix3x3({m00, m01, m02}, {m10, m11, m12}, {m20, m21, m22});
    }

    Quaternion Quaternion::operator-() const
    {
        return {-x, -y, -z, -w};
    }


#pragma endregion



#pragma region Matrix2x2

#pragma endregion



#pragma region Matrix3x3

#pragma endregion


#pragma region Matrix4x4


#pragma endregion


#pragma region Quaternion

#pragma endregion
}