#pragma once

#include <J3ML/LinearAlgebra/Vector3.h>
#include <cstddef>
#include <cstdlib>
#include <J3ML/LinearAlgebra/Angle2D.h>


namespace LinearAlgebra {

// A 3D (x, y, z) ordered pair.
class Vector3 {
public:

    // Default Constructor - Initializes to zero
    Vector3();
    // Constructs a new Vector3 with the value (X, Y, Z)
    Vector3(float X, float Y, float Z);
    Vector3(const Vector3& rhs); // Copy Constructor
    Vector3(Vector3&&) = default; // Move Constructor
    Vector3& operator=(const Vector3& rhs);


    static const Vector3 Zero;
    static const Vector3 Up;
    static const Vector3 Down;
    static const Vector3 Left;
    static const Vector3 Right;
    static const Vector3 Forward;
    static const Vector3 Backward;
    static const Vector3 NaN;

    static void Orthonormalize(Vector3& a, Vector3& b)
    {
        a = a.Normalize();
        b = b - b.ProjectToNorm(a);
        b = b.Normalize();
    }


    //Returns the DirectionVector for a given angle.
    Vector3 Direction(const Vector3 &rhs) const;


    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c)
    {
        a = a.Normalize();
        b = b - b.ProjectToNorm(a);
        b = b.Normalize();
        c = c - c.ProjectToNorm(a);
        c = c - c.ProjectToNorm(b);
        c = c.Normalize();
    }

    bool AreOrthonormal(const Vector3& a, const Vector3& b, float epsilon)
    {

    }

    Vector3 ProjectToNorm(const Vector3& direction)
    {
        return direction * this->Dot(direction);
    }

    float GetX() const;
    float GetY() const;
    float GetZ() const;
    void SetX(float newX);
    void SetY(float newY);
    void SetZ(float newZ);

    bool IsWithinMarginOfError(const Vector3& rhs, float margin=0.001f) const;
    bool IsNormalized(float epsilonSq = 1e-5f) const;
    bool IsZero(float epsilonSq = 1e-6f) const;
    bool IsPerpendicular(const Vector3& other, float epsilonSq=1e-5f) const;

    float operator[](std::size_t index) const;
    bool operator == (const Vector3& rhs) const;
    bool operator != (const Vector3& rhs) const;

    Vector3 Min(const Vector3& min) const;
    static Vector3 Min(const Vector3& lhs, const Vector3& rhs);

    Vector3 Max(const Vector3& max) const;
    static Vector3 Max(const Vector3& lhs, const Vector3& rhs);

    Vector3 Clamp(const Vector3& min, const Vector3& max) const;
    static Vector3 Clamp(const Vector3& min, const Vector3& input, const Vector3& max);

    // Returns the magnitude between the two vectors.
    float Distance(const Vector3& to) const;
    static float Distance(const Vector3& from, const Vector3& to);

    float Length() const;
    static float Length(const Vector3& of);

    float LengthSquared() const;
    static float LengthSquared(const Vector3& of);

    // Returns the length of the vector, which is sqrt(x^2 + y^2 + z^2)
    float Magnitude() const;
    static float Magnitude(const Vector3& of);

    // Returns a float value equal to the magnitudes of the two vectors multiplied together and then multiplied by the cosine of the angle between them.
    // For normalized vectors, dot returns 1 if they point in exactly the same direction,
    // -1 if they point in completely opposite directions, and 0 if the vectors are perpendicular.
    float Dot(const Vector3& rhs) const;
    static float Dot(const Vector3& lhs, const Vector3& rhs);

    // Projects one vector onto another and returns the result. (IDK)
    Vector3 Project(const Vector3& rhs) const;
    static Vector3 Project(const Vector3& lhs, const Vector3& rhs);

    // The cross product of two vectors results in a third vector which is perpendicular to the two input vectors.
    // The result's magnitude is equal to the magnitudes of the two inputs multiplied together and then multiplied by the sine of the angle between the inputs.
    Vector3 Cross(const Vector3& rhs) const;
    static Vector3 Cross(const Vector3& lhs, const Vector3& rhs);

    // Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
    Vector3 Normalize() const;
    static Vector3 Normalize(const Vector3& targ);

    // Linearly interpolates between two points.
    // Interpolates between the points and b by the interpolant t.
    // The parameter is (TODO: SHOULD BE!) clamped to the range[0, 1].
    // This is most commonly used to find a point some fraction of the wy along a line between two endpoints (eg. to move an object gradually between those points).
    Vector3 Lerp(const Vector3& goal, float alpha) const;
    static Vector3 Lerp(const Vector3& lhs, const Vector3& rhs, float alpha);


    Angle2D AngleBetween(const Vector3& rhs) const;
    static Angle2D AngleBetween(const Vector3& lhs, const Vector3& rhs);

    // Adds two vectors
    Vector3 operator+(const Vector3& rhs) const;
    Vector3 Add(const Vector3& rhs) const;
    static Vector3 Add(const Vector3& lhs, const Vector3& rhs);

    // Subtracts two vectors
    Vector3 operator-(const Vector3& rhs) const;
    Vector3 Sub(const Vector3& rhs) const;
    static Vector3 Sub(const Vector3& lhs, const Vector3& rhs);

    // Multiplies this vector by a scalar value
    Vector3 operator*(float rhs) const;
    Vector3 Mul(float scalar) const;
    static Vector3 Mul(const Vector3& lhs, float rhs);

    // Divides this vector by a scalar
    Vector3 operator/(float rhs) const;
    Vector3 Div(float scalar) const;
    static Vector3 Div(const Vector3& lhs, float rhs);

    // Unary + operator
    Vector3 operator+() const; // TODO: Implement
    // Unary - operator (Negation)
    Vector3 operator-() const;
public:
     float x = 0;
     float y = 0;
     float z = 0;
};
}