#pragma once
#include <cstdint>
#include <cmath>
#include <cstdlib>
#include <algorithm>
#include <functional>
// TODO: Make GLM redundant, and remove it thereafter.
#include <glm/glm.hpp>
#include <glm/ext/scalar_constants.hpp>

// TODO: SIMD operations for fast math (parallelization!!)

// Dawsh Linear Algebra Library - Everything you need for 3D math
namespace LinearAlgebra {
    class Vector2; // A type representing a position in a 2-dimensional coordinate space.
    class Vector3; // A type representing a position in a 3-dimensional coordinate space.
    class Vector4; // A type representing a position in a 4-dimensional coordinate space.
    class Angle2D; // Uses x,y components to represent a 2D rotation.
    class EulerAngle; // Uses pitch,yaw,roll components to represent a 3D orientation.
    class AxisAngle; //
    class CoordinateFrame; //
    class Matrix2x2;
    class Matrix3x3;
    class Matrix4x4;
    class Transform2D;
    class Transform3D;
    class Quaternion;
    inline float lerp(float a, float b, float t);
}

// TODO: Enforce Style Consistency (Function Names use MicroSoft Case)

// Note: Josh Linear Algebra Types are designed as Immutable Data Types
// x, y, z, w, etc. values should not and can not be set directly.
// rather, you just construct a new type and assign it.
// This might sound ass-backwards for many object types.
// But mathematically, a vector or matrix is defined by it's size, and values.
// Changing the value of one axis fundamentally changes the definition of the vector/matrix.
// So we enforce this conceptually at code level...
// If you're wondering how it remains performant, it only heap-allocates a tiny space (4*n bytes for vectors) (4*n*m bytes for matrices)
// Just Trust Me Bro - Josjh
#define MUTABLE true // Toggle This For: Ugly math, ugly code, and an ugly genital infection!

#if MUTABLE
#define IMMUTABLE !MUTABLE
#endif

namespace LinearAlgebra
{

    class Vector2 {
    public:
        Vector2();
        // Constructs a new Vector2 with the value (X, Y)
        Vector2(float X, float Y);
        Vector2(const Vector2& rhs);   // Copy Constructor
        Vector2(Vector2&&) = default;   // Move Constructor
        float GetX() const { return x; }
        float GetY() const { return y; }
#if MUTABLE
        void SetX(float newX) { x = newX;}
        void SetY(float newY) { y = newY; }
#endif
        static const Vector2 Zero;
        static const Vector2 Up;
        static const Vector2 Left;
        static const Vector2 Down;
        static const Vector2 Right;

        float operator[](std::size_t index);

        bool IsWithinMarginOfError(const Vector2& rhs, float margin=0.001f) const;

        bool operator == (const Vector2& rhs) const;
        bool operator != (const Vector2& rhs) const;

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

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

        Vector2 Clamp(const Vector2& min, const Vector2& max) const;
        static Vector2 Clamp(const Vector2& min, const Vector2& middle, const Vector2& max);

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

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

        float LengthSquared() const;
        static float LengthSquared(const Vector2& of) { return of.LengthSquared(); }
        // Returns the length of the vector, which is sqrt(x^2 + y^2)
        float Magnitude() const;
        static float Magnitude(const Vector2& of) { return of.Magnitude();}

        // 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 Vector2& rhs) const;
        static float Dot(const Vector2& lhs, const Vector2& rhs) { return lhs.Dot(rhs); }

        // Projects one vector onto another and returns the result. (IDK)
        Vector2 Project(const Vector2& rhs) const;
        // @see Project
        static Vector2 Project(const Vector2& lhs, const Vector2& rhs) { return lhs.Project(rhs); }

        // Returns a copy of this vector, resized to have a magnitude of 1, while preserving "direction"
        Vector2 Normalize() const;
        static Vector2 Normalize(const Vector2& of) { return of.Normalize(); }

        // 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).
        Vector2 Lerp(const Vector2& rhs, float alpha) const;
        // @see Lerp
        static Vector2 Lerp(const Vector2& lhs, const Vector2& rhs, float alpha) { return lhs.Lerp(rhs, alpha); }

        float AngleBetween(const Vector2& rhs) const;
        static float AngleBetween(const Vector2& lhs, const Vector2& rhs);

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

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

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

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


        // Unary operator +
        Vector2 operator +() const; // TODO: Implement
        Vector2 operator -() const;
        // Assigns a vector to another
        Vector2& operator=(const Vector2&v);


        Vector2& operator+=(const Vector2& rhs); // Adds a vector to this vector, in-place.
        Vector2& operator-=(const Vector2& rhs); // Subtracts a vector from this vector, in-place
        Vector2& operator*=(float scalar);
        Vector2& operator/=(float scalar);

    public:
#if MUTABLE
        float x = 0;
        float y = 0;
#else
        const float x = 0;
    const float y = 0;
#endif
    };

    class Vector3 {
    public:
        Vector3();
        Vector3(float X, float Y, float Z);
        Vector3(const Vector3& rhs);
        Vector3(Vector3&&) = default;
        Vector3& operator=(const Vector3& rhs);

        float getX() const;
        float getY() const;
        float getZ() const;
#if MUTABLE
        void setX(float newX);
        void setY(float newY);
        void setZ(float newZ);
#endif
        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;

        float operator[](std::size_t index) const;

        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 IsFinite() const;
        bool IsPerpendicular(const Vector2& other, float epsilonSq=1e-5f) 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);

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

        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;
        Vector3 operator+(const Vector3& rhs) const;
        Vector3 operator-(const Vector3& rhs) const;
        Vector3 operator*(float rhs) const;
        Vector3 operator/(float rhs) const;
        Vector3 operator+() const; // TODO: Implement
        Vector3 operator-() const;
    public:
#if MUTABLE
        float x = 0;
        float y = 0;
        float z = 0;
#else
        const float x = 0;
    const float y = 0;
    const float z = 0;
#endif
    };

    class Vector4 {
    public:
        Vector4();
        Vector4(const Vector3& xyz, float w = 0);
        Vector4(float X, float Y, float Z, float W);
        Vector4(const Vector4& copy) = default;
        Vector4(Vector4&& move) = default;
        float getX() const;
        float getY() const;
        float getZ() const;
        float getW() const;
#if MUTABLE
        void setX(float newX);
        void setY(float newY);
        void setZ(float newZ);
        void setW(float newW);
#endif
        float operator[](int index) const;
        bool IsWithinMarginOfError(const Vector4& rhs, float margin=0.0001f) const;
        bool operator==(const Vector4& rhs) const;
        bool operator!=(const Vector4& rhs) const;
        Vector4 min(const Vector4& min) const;
        Vector4 max(const Vector4& max) const;
        Vector4 clamp(const Vector4& min, const Vector4& max) const;
        float distance(const Vector4& to) const;
        float length() const;
        float lengthSquared() const;
        float magnitude() const;
        float dot(const Vector4& rhs) const;
        Vector4 project(const Vector4& rhs) const;
        // While it is feasable to compute a cross-product in four dimensions
        // the cross product only has the orthogonality property in 3 and 7 dimensions
        // You should consider instead looking at Gram-Schmidt Orthogonalization
        // to find orthonormal vectors.
        Vector4 cross(const Vector4& rhs) const;
        Vector4 normalize() const;
        Vector4 lerp(const Vector4& goal, float alpha) const;
        Vector4 operator+(const Vector4& rhs) const;
        Vector4 operator-(const Vector4& rhs) const;
        Vector4 operator*(float rhs) const;
        Vector4 operator/(float rhs) const;
        Vector4 operator+() const;
        Vector4 operator-() const;
    public:
#if MUTABLE
        float x = 0;
        float y = 0;
        float z = 0;
        float w = 0;
#else
        const float x = 0;
    const float y = 0;
    const float z = 0;
    const float w = 0;
#endif
    };
// Essential Reading:
// http://www.essentialmath.com/GDC2012/GDC2012_JMV_Rotations.pdf
    class EulerAngle {
    public:
        EulerAngle();
        EulerAngle(float pitch, float yaw, float roll);
        EulerAngle(const Vector3& vec) : pitch(vec.x), yaw(vec.y), roll(vec.z) {}
        static EulerAngle FromRadians(float radians);
        static EulerAngle FromDegrees(float degrees);
        /// TODO: Implement separate upper and lower bounds
        /// Preserves internal value of euler angles, normalizes and clamps the output.
        /// This does not solve gimbal lock!!!
        float GetPitch(float pitch_limit) const;
        float GetYaw(float yaw_limit)     const;
        float GetRoll(float roll_limit)   const;

        bool operator==(const EulerAngle& a) const;
        void clamp();

        // TODO: Euler Angles do not represent a vector, length doesn't apply, nor is this information meaningful for this data type.
        // If you need a meaningful representation of length in 3d space, use a vector!!
        [[nodiscard]] float length() const {
            return 0;
        }
        // TODO: Implement
        Vector3 unitVector() const;

        EulerAngle movementAngle() const;
    public:
        float pitch;
        float yaw;
        float roll;
    };

/// Transitional datatype, not useful for internal representation of rotation
/// But has uses for conversion and manipulation.
    class AxisAngle {
        Vector3 axis;
        float angle;
    };

    using Position = Vector3;

/// The CFrame is fundamentally 4 vectors (position, forward, right, up vector)
    class CoordinateFrame
    {
        Vector3 getPosition();
        Vector3 getLookVector();
        Vector3 getRightVector();
        Vector3 getUpVector();
        AxisAngle GetAxisAngle();
        EulerAngle GetEulerAngleXYZ();
        EulerAngle GetWorldAngleYZX();
    };

    class Matrix2x2 {
    public:
        static const Matrix2x2 Zero;
        static const Matrix2x2 Identity;
        static const Matrix2x2 NaN;

        Vector2 GetRow() const;
        Vector2 GetColumn() const;

    protected:
        float elems[2][2];
    };

/// A 3-by-3 matrix for linear transformations of 3D geometry.
    /* This can represent any kind of linear transformations of 3D geometry, which include
     * rotation, scale, shear, mirroring, and orthographic projection.
     * A 3x3 matrix cannot represent translation, which requires a 3x4, or perspective projection (4x4).
     * The elements of this matrix are
     *  m_00, m_01, m_02
     *  m_10, m_11, m_12
     *  m_20, m_21, m_22
     *
     *  The element m_yx is the value on the row y and column x.
     *  You can access m_yx using the double-bracket notation m[y][x], or using the member function At.
     *
     *  @note The member functions in this class use the convention that transforms are applied to
     *  vectors in the form M * v. This means that "Matrix3x3 M, M1, M2; M = M1 * M2;" gives a transformation M
     *  that applies M2 first, followed by M1 second
     */
    class Matrix3x3 {
    public:
        enum { Rows = 3 };
        enum { Cols = 3 };

        static const Matrix3x3 Zero;
        static const Matrix3x3 Identity;
        static const Matrix3x3 NaN;

        Matrix3x3();
        Matrix3x3(float fillValue);
        Matrix3x3(float m00, float m01, float m02, float m10, float m11, float m12, float m20, float m21, float m22);
        Matrix3x3(const Vector3& r1, const Vector3& r2, const Vector3& r3);
        explicit Matrix3x3(const Quaternion& orientation);

        static Matrix3x3 RotateX(float radians);
        static Matrix3x3 RotateY(float radians);
        static Matrix3x3 RotateZ(float radians);

        Vector3 GetRow(int index) const;
        Vector3 GetColumn(int index) const;
        float At(int x, int y) const;

        /// Creates a new M3x3 that rotates about the given axis by the given angle
        static Matrix3x3 RotateAxisAngle(const Vector3& rhs);

        static Matrix3x3 RotateFromTo(const Vector3& source, const Vector3& direction);

        static Matrix3x3 LookAt(const Vector3& forward, const Vector3& target, const Vector3& localUp, const Vector3& worldUp);

        static Matrix3x3 FromQuat(const Quaternion& orientation);

        Quaternion ToQuat() const;

        /// Creates a new Matrix3x3 as a combination of rotation and scale.
        // This function creates a new matrix M in the form M = R * S
        // where R is a rotation matrix and S is a scale matrix.
        // Transforming a vector v using this matrix computes the vector
        // v' == M * v == R*S*v == (R * (S * v)) which means the scale operation
        // is applied to the vector first, followed by rotation, and finally translation
        static Matrix3x3 FromRS(const Quaternion& rotate, const Matrix3x3& scale);
        static Matrix3x3 FromRS(const Matrix3x3 &rotate, const Matrix3x3& scale);


        /// Creates a new transformation matrix that scales by the given factors.
        // This matrix scales with respect to origin.
        static Matrix3x3 Scale(float sx, float sy, float sz);
        static Matrix3x3 Scale(const Matrix3x3& scale);

        /// Returns the main diagonal.
        Vector3 Diagonal() const;
        /// Returns the local +X/+Y/+Z axis in world space.
        /// This is the same as transforming the vector{1,0,0} by this matrix.
        Vector3 WorldX() const;
        /// Returns the local +Y axis in world space.
        /// This is the same as transforming the vector{0,1,0} by this matrix.
        Vector3 WorldY() const;
        /// Returns the local +Z axis in world space.
        /// This is the same as transforming the vector{0,0,1} by this matrix.
        Vector3 WorldZ() const;

        /// Computes the determinant of this matrix.
        // If the determinant is nonzero, this matrix is invertible.
        // If the determinant is negative, this matrix performs reflection about some axis.
        // From http://msdn.microsoft.com/en-us/library/bb204853(VS.85).aspx :
        // "If the determinant is positive, the basis is said to be "positively" oriented (or right-handed).
        // If the determinant is negative, the basis is said to be "negatively" oriented (or left-handed)."
        // @note This function computes 9 LOADs, 9 MULs and 5 ADDs. */
        float Determinant() const;

        // Returns an inverted copy of this matrix. This
        Matrix3x3 Inverse() const;

        // Returns a transposed copy of this matrix.
        Matrix3x3 Transpose() const;

        // Transforms the given vectors by this matrix M, i.e. returns M * (x,y,z)

        Vector2 Transform(const Vector2& vector) const;
        Vector3 Transform(const Vector3& rhs) const;
        Vector2 Transform(float x, float y) const;
        Vector3 Transform(float x, float y, float z) const;



        Vector3 operator[] (float index) const;
        Vector3 operator * (const Vector3& rhs) const;
        Matrix3x3 operator * (const Matrix3x3& rhs) const;

    protected:
        float elems[3][3];
    };

/// A 4-by-4 matrix for affine transformations and perspective projections of 3D geometry.
/* This matrix can represent the most generic form of transformations for 3D objects,
 * including perspective projections, which a 4-by-3 cannot store,
 * and translations, which a 3-by-3 cannot represent.
 * The elements of this matrix are
 *  m_00, m_01, m_02, m_03
 *  m_10, m_11, m_12, m_13
 *  m_20, m_21, m_22, m_23,
 *  m_30, m_31, m_32, m_33
 *
 *  The element m_yx is the value on the row y and column x.
 *  You can access m_yx using the double-bracket notation m[y][x]
 */
    class Matrix4x4 {
    public:
        static const Matrix4x4 Zero;
        static const Matrix4x4 Identity;

        Vector3 GetTranslationComponent() const;
        Matrix3x3 GetRotationComponent() const;

        Vector4 GetRow() const;
        Vector4 GetColumn() const;
    protected:
        float elems[4][4];
    };

    class Transform2D {
    protected:
        Matrix3x3 transformation;
    public:
        Transform2D Translate(const Vector2& offset) const;
        Transform2D Translate(float x, float y) const;
        Transform2D Scale(float scale); // Perform Uniform Scale
        Transform2D Scale(float x, float y); // Perform Nonunform Scale
        Transform2D Scale(const Vector2& scales); // Perform Nonuniform Scale
        Transform2D Rotate();
    };

    class Transform3D {
    protected:
        Matrix4x4 transformation;
    public:
        Transform3D Scale();
        Transform3D Translate();
        Transform3D Rotate();
    };

    struct Movement {
        EulerAngle angle;
        Position position;
    };
    class Quaternion : public Vector4
    {
    public:
        Quaternion() {}
        Quaternion(const Quaternion& rhs) = default;
        explicit Quaternion(Matrix3x3& rotationMtrx) {}
        explicit Quaternion(Matrix4x4& rotationMtrx) {}
        // @note The input data is not normalized after construction, this has to be done manually.
        Quaternion(float x, float y, float z, float w);
        // Constructs this quaternion by specifying a rotation axis and the amount of rotation to be performed about that axis
        // @param rotationAxis The normalized rotation axis to rotate about. If using Vector4 version of the constructor, the w component of this vector must be 0.
        Quaternion(const Vector3& rotationAxis, float rotationAngleBetween) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); }
        Quaternion(const Vector4& rotationAxis, float rotationAngleBetween) { SetFromAxisAngle(rotationAxis, rotationAngleBetween); }
        //void Inverse();
        Quaternion Inverse() const;
        //void Normalize();
        Quaternion Normalize() const;
        Vector3 GetWorldX() const { return Transform(1.f, 0.f, 0.f); }
        Vector3 GetWorldY() const { return Transform(0.f, 1.f, 0.f); }
        Vector3 GetWorldZ() const { return Transform(0.f, 0.f, 1.f); }

        Matrix3x3 ToMatrix3x3() const;

        Vector3 Transform(const Vector3& vec) const
        {
            Matrix3x3 mat = this->ToMatrix3x3();
            return mat * vec;
        }
        Vector3 Transform(float X, float Y, float Z) const
        {
            return Transform(Vector3{X, Y, Z});
        }
        // Note: We only transform the x,y,z components of 4D vectors, w is left untouched
        Vector4 Transform(const Vector4& vec) const
        {
            return Vector4(Transform(vec.x, vec.y, vec.z), vec.w);
        }
        Vector4 Transform(float X, float Y, float Z, float W) const
        {
            return Transform(Vector4(X, Y, Z, W));
        }

        Quaternion GetInverse() const;
        Quaternion Lerp(const Quaternion& b, float t) const
        {
            float angle = this->dot(b);
            if (angle >= 0.f) // Make sure we rotate the shorter arc
                return (*this * (1.f - t) + b * t).Normalize();
            else
                return (*this * (t - 1.f) + b * t).Normalize();
        }
        Quaternion Slerp(const Quaternion& target) const;

        void SetFromAxisAngle(const Vector3& axis, float angle)
        {
            float sinz, cosz;

        }

        void SetFromAxisAngle(const Vector4& axis, float angle)
        {

        }


        static Quaternion LookAt(const Vector3& position, const Vector3& direction, const Vector3& axisUp);

        // Multiplies two quaternions together.
        // The product q1 * q2 returns a quaternion that concatenates the two orientation rotations.
        // The rotation q2 is applied first before q1.
        Quaternion operator *  (const Quaternion& rhs) const;

        Quaternion operator * (float scalar) const
        {
            return Quaternion(x * scalar, y * scalar, z * scalar, w * scalar);
        }

        // Transforms the given vector by this Quaternion.
        Vector3 operator * (const Vector3& rhs) const;
        Vector4 operator * (const Vector4& rhs) const;

        // Divides a quaternion by another. Divison "a / b" results in a quaternion that rotates the orientation b to coincide with orientation of
        Quaternion operator / (const Quaternion& rhs) const;
        Quaternion operator +(const Quaternion& rhs) const;
        Quaternion operator +() const { return *this; }
        Quaternion operator -() const;
    };

    inline namespace VectorMath {
        inline float distance(Position sP, Position eP) {
            return sqrt(pow(eP.x - sP.x, 2) + pow(eP.y - sP.y, 2) + pow(eP.z - sP.z, 2));
        }

        //Basically an aimbot.
        inline Vector3 calcAngle(Position sP, Position eP) {
            const auto pi = glm::pi<float>();
            //returned.x = -(asinf((eP.y - sP.y) / distance(sP, eP)) * 180.0f / M_PI);
            //returned.y = (atan2f(eP.x - sP.x,eP.z - sP.z) / M_PI * 180.0f);
            return {static_cast<float>((-(asinf((eP.y - sP.y) / distance(sP, eP)) * 180.0f / pi ))),static_cast<float>((atan2f(eP.x - sP.x,eP.z - sP.z) / pi * 180.0f)),0};
        }
    }


}