#include <J3ML/LinearAlgebra/Vector3.h>
#include <algorithm>
#include <cassert>
#include <cmath>

namespace LinearAlgebra {

#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};
    const Vector3 Vector3::NaN      = {NAN, NAN, NAN};

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




    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) : x(rhs.x), y(rhs.y), 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);
    }

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

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

    float Vector3::GetZ() const { return z;}

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

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

    void Vector3::SetZ(float newZ) { z = newZ;}

    Vector3 Vector3::Min(const Vector3 &lhs, const Vector3 &rhs) {
        return lhs.Min(rhs);
    }

    Vector3 Vector3::Max(const Vector3 &lhs, const Vector3 &rhs) {
        return lhs.Max(rhs);
    }

    Vector3 Vector3::Clamp(const Vector3 &min, const Vector3 &input, const Vector3 &max) {
        return input.Clamp(min, max);
    }

    float Vector3::Distance(const Vector3 &from, const Vector3 &to) {
        return from.Distance(to);
    }

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

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

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

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

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

    Vector3 Vector3::Cross(const Vector3 &lhs, const Vector3 &rhs) {
        return lhs.Cross(rhs);
    }

    Vector3 Vector3::Normalize(const Vector3 &targ) {
        return targ.Normalize();
    }

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

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

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

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

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

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

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

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

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

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

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

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

    Angle2D Vector3::AngleBetween(const Vector3 &rhs) const {
        const auto Pi_x_180 = 180.f / M_PI;
        auto dist = this->Distance(rhs);
        float x = -(asinf((rhs.y - this->y) / dist));
        float y = (atan2f(rhs.x - this->x,rhs.z - this->z));
        return {x, y};
    }

    Angle2D Vector3::AngleBetween(const Vector3 &lhs, const Vector3 &rhs) // TODO: 3D Angle representation?
    {
        return lhs.AngleBetween(rhs);
    }

#pragma endregion
}