#pragma once

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

namespace J3ML::LinearAlgebra {

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

    enum {Dimensions = 3};

    // 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
    explicit Vector3(const float* data);

    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 const Vector3 Infinity;
    static const Vector3 NegativeInfinity;

    float* ptr();

    static void Orthonormalize(Vector3& a, Vector3& b);

    Vector3 Abs() const;
    static Vector3 Abs(const Vector3& rhs);

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

    static void Orthonormalize(Vector3& a, Vector3& b, Vector3& c);

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

    Vector3 ProjectToNorm(const Vector3& direction) const;

    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;
    float &operator[](std::size_t index);
    bool operator == (const Vector3& rhs) const;
    bool operator != (const Vector3& rhs) const;

    bool IsFinite() const;
    float MinElement() const;
    static float MinElement(const Vector3& of);

    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 DistanceSquared(const Vector3& to) const;
    static float DistanceSquared(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);

    /// Multiplies this vector by a vector, element-wise
    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
    ///	 but this function is provided here for syntactical convenience.
    Vector3 Mul(const Vector3& rhs) const;

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


    /// Divides this vector by a vector, element-wise
    /// @note Mathematically, the multiplication of two vectors is not defined in linear space structures,
    /// but this function is provided here for syntactical convenience
    Vector3 Div(const Vector3& v) const;

    /// Unary + operator
    Vector3 operator+() const; // TODO: Implement
    /// Unary - operator (Negation)
    Vector3 operator-() const;

    bool Equals(const Vector3& rhs, float epsilon = 1e-3f) const;
    bool Equals(float _x, float _y, float _z, float epsilon = 1e-3f) const;


    Vector3 &operator =(const Vector3& rhs);
    Vector3& operator+=(const Vector3& rhs);
    Vector3& operator-=(const Vector3& rhs);
    Vector3& operator*=(float scalar);
    Vector3& operator/=(float scalar);
public:
     float x = 0;
     float y = 0;
     float z = 0;
};

    static Vector3 operator*(float lhs, const Vector3& rhs)
    {
        return rhs * lhs;
    }
}