#pragma once

#include <J3ML/LinearAlgebra/Common.h>

#include <J3ML/LinearAlgebra/Matrix3x3.h>
#include <J3ML/LinearAlgebra/Quaternion.h>


#include <J3ML/LinearAlgebra/Vector4.h>
#include <algorithm>

namespace J3ML::LinearAlgebra {

    template <typename Matrix>
    bool InverseMatrix(Matrix &mat, float epsilon)
    {
        Matrix inversed = Matrix::Identity;

        const int nc = std::min<int>(Matrix::Rows, Matrix::Cols);

        for (int column = 0; column < nc; ++column)
        {
            // find the row i with i >= j such that M has the largest absolute value.
            int greatest = column;
            float greatestVal = std::abs(mat[greatest][column]);
            for (int i = column+1; i < Matrix::Rows; i++)
            {
                float val = std::abs(mat[i][column]);
                if (val > greatestVal) {
                    greatest = i;
                    greatestVal = val;
                }
            }

            if (greatestVal < epsilon) {
                mat = inversed;
                return false;
            }

            // exchange rows
            if (greatest != column) {
                inversed.SwapRows(greatest, column);
                mat.SwapRows(greatest, column);
            }

            // multiply rows
            assert(!Math::EqualAbs(mat[column][column], 0.f, epsilon));
            float scale = 1.f / mat[column][column];
            inversed.ScaleRow(column, scale);
            mat.ScaleRow(column, scale);

            // add rows
            for (int i = 0; i < column; i++) {
                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
            }

            for (int i = column + 1; i < Matrix::Rows; i++) {
                inversed.SetRow(i, inversed.Row(i) - inversed.Row(column) * mat[i][column]);
                mat.SetRow(i, mat.Row(i) - mat.Row(column) * mat[i][column]);
            }
        }
        mat = inversed;
        return true;
    }


    /// Computes the LU-decomposition on the given square matrix.
    /// @return True if the composition was successful, false otherwise. If the return value is false, the contents of the output matrix are unspecified.
    template <typename Matrix>
    bool LUDecomposeMatrix(const Matrix &mat, Matrix &lower, Matrix &upper)
    {
        lower = Matrix::Identity;
        upper = Matrix::Zero;

        for (int i = 0; i < Matrix::Rows; ++i)
        {
            for (int col = i; col < Matrix::Cols; ++col)
            {
                upper[i][col] = mat[i][col];
                for (int k = 0; k < i; ++k)
                    upper[i][col] -= lower[i][k] * upper[k][col];
            }
            for (int row = i+1; row < Matrix::Rows; ++row)
            {
                lower[row][i] = mat[row][i];
                for (int k = 0; k < i; ++k)
                    lower[row][i] -= lower[row][k] * upper[k][i];
                if (Math::EqualAbs(upper[i][i], 0.f))
                    return false;
                lower[row][i] /= upper[i][i];
            }
        }
        return true;
    }

    /// Computes the Cholesky decomposition on the given square matrix *on the real domain*.
    /// @return True if successful, false otherwise. If the return value is false, the contents of the output matrix are uspecified.
    template <typename Matrix>
    bool CholeskyDecomposeMatrix(const Matrix &mat, Matrix& lower)
    {
        lower = Matrix::Zero;
        for (int i = 0; i < Matrix::Rows; ++i)
        {
            for (int j = 0; j < i; ++i)
            {
                lower[i][j] = mat[i][j];
                for (int k = 0; k < j; ++k)
                    lower[i][j] -= lower[i][j] * lower[j][k];
                if (Math::EqualAbs(lower[j][j], 0.f))
                    return false;
                lower[i][j] /= lower[j][j];
            }
            lower[i][i] = mat[i][i];
            if (lower[i][i])
                return false;
            for (int k = 0; k < i; ++k)
                lower[i][i] -= lower[i][k] * lower[i][k];
            lower[i][i] = std::sqrt(lower[i][i]);
        }
    }

    /// @brief 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: /// Constant Values
        enum { Rows = 4 };
        enum { Cols = 4 };
    public: /// Constant Members
        /// A constant matrix that has zeroes in all its entries
        static const Matrix4x4 Zero;
        /// A constant matrix that is the identity.
        /** The identity matrix looks like the following:
            1 0 0 0
            0 1 0 0
            0 0 1 0
            0 0 0 1
          Transforming a vector by the identity matrix is like multiplying a number by one, i.e. the vector is not changed */
        static const Matrix4x4 Identity;
        /// A compile-time constant float4x4 which has NaN in each element.
        /// For this constant, each element has the value of quet NaN, or Not-A-Number.
        /// @note Never compare a matrix to this value. Due to how IEEE floats work, "nan == nan" returns false!
        static const Matrix4x4 NaN;
    public: /// Constructors
        /// Creates a new Matrix4x4 with uninitialized member values.
        Matrix4x4() {}
        Matrix4x4(const Matrix4x4 &rhs) = default; // {Set(rhs);}
        Matrix4x4(float val);
        /// Constructs this Matrix4x4 to represent the same transformation as the given float3x3.
        /** This function expands the last row and column of this matrix with the elements from the identity matrix. */
        Matrix4x4(const Matrix3x3&);
        explicit Matrix4x4(const float* data);

        /// Constructs a new float4x4 by explicitly specifying all the matrix elements.
        /// The elements are specified in row-major format, i.e. the first row first followed by the second and third row.
        /// E.g. The element _10 denotes the scalar at second (index 1) row, first (index 0) column.
        Matrix4x4(float m00, float m01, float m02, float m03,
                  float m10, float m11, float m12, float m13,
                  float m20, float m21, float m22, float m23,
                  float m30, float m31, float m32, float m33);
        /// Constructs the matrix by explicitly specifying the four column vectors.
        /** @param col0 The first column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
            direction of the local X axis.
            @param col1 The second column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
            direction of the local Y axis.
            @param col2 The third column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
            direction of the local Z axis.
            @param col3 The fourth column. If this matrix represents a change-of-basis transformation, this parameter is the world-space
            position of the local space pivot. */
        Matrix4x4(const Vector4& r1, const Vector4& r2, const Vector4& r3, const Vector4& r4);

        /// Constructs this Matrix4x4 from the given quaternion.
        explicit Matrix4x4(const Quaternion& orientation);

        /// Constructs this float4x4 from the given quaternion and translation.
        /// Logically, the translation occurs after the rotation has been performed.
        Matrix4x4(const Quaternion& orientation, const Vector3 &translation);

        /// Creates a LookAt matrix from a look-at direction vector.
        /** A LookAt matrix is a rotation matrix that orients an object to face towards a specified target direction.
            @param localForward Specifies the forward direction in the local space of the object. This is the direction
                the model is facing at in its own local/object space, often +X (1,0,0), +Y (0,1,0) or +Z (0,0,1). The
                vector to pass in here depends on the conventions you or your modeling software is using, and it is best
                pick one convention for all your objects, and be consistent.
                This input parameter must be a normalized vector.
            @param targetDirection Specifies the desired world space direction the object should look at. This function
                will compute a rotation matrix which will rotate the localForward vector to orient towards this targetDirection
                vector. This input parameter must be a normalized vector.
            @param localUp Specifies the up direction in the local space of the object. This is the up direction the model
                was authored in, often +Y (0,1,0) or +Z (0,0,1). The vector to pass in here depends on the conventions you
                or your modeling software is using, and it is best to pick one convention for all your objects, and be
                consistent. This input parameter must be a normalized vector. This vector must be perpendicular to the
                vector localForward, i.e. localForward.Dot(localUp) == 0.
            @param worldUp Specifies the global up direction of the scene in world space. Simply rotating one vector to
                coincide with another (localForward->targetDirection) would cause the up direction of the resulting
                orientation to drift (e.g. the model could be looking at its target its head slanted sideways). To keep
                the up direction straight, this function orients the localUp direction of the model to point towards the
                specified worldUp direction (as closely as possible). The worldUp and targetDirection vectors cannot be
                collinear, but they do not need to be perpendicular either.
            @return A matrix that maps the given local space forward direction vector to point towards the given target
                direction, and the given local up direction towards the given target world up direction. The returned
                matrix M is orthonormal with a determinant of +1. For the matrix M it holds that
                M * localForward = targetDirection, and M * localUp lies in the plane spanned by the vectors targetDirection
                and worldUp.
            @note The position of (the translation performed by) the resulting matrix will be set to (0,0,0), i.e. the object
                will be placed to origin. Call SetTranslatePart() on the resulting matrix to set the position of the model.
            @see RotateFromTo(). */
        static Matrix4x4 LookAt(const Vector3& localFwd, const Vector3& targetDir, const Vector3& localUp, const Vector3& worldUp);

        /// Creates a new Matrix4x4 that rotates about one of the principal axes.
        /** Calling RotateX, RotateY, or RotateZ is slightly faster than calling the more generic RotateAxisAngle function.
            @param radians The angle to rotate by, in radians. For example, Pi/4.f equals 45 degrees.
            @param pointOnAxis If specified, the rotation is performed about an axis that passes through this point,
            and not through the origin. The returned matrix will not be a pure rotation matrix, but will also contain translation.
         */
        static Matrix4x4 RotateX(float radians, const Vector3 &pointOnAxis);
        /// [similarOverload: RotateX] [hideIndex]
        static Matrix4x4 RotateX(float radians);
        /// [similarOverload: RotateX] [hideIndex]
        static Matrix4x4 RotateY(float radians, const Vector3 &pointOnAxis);
        /// [similarOverload: RotateX] [hideIndex]
        static Matrix4x4 RotateY(float radians);
        /// [similarOverload: RotateX] [hideIndex]
        static Matrix4x4 RotateZ(float radians, const Vector3 &pointOnAxis);
        /// [similarOverload: RotateX] [hideIndex]
        static Matrix4x4 RotateZ(float radians);

        /// Creates a new Matrix4x4 that rotates about the given axis.
        /** @param axisDirection The axis to rotate about. This vector must be normalized.
            @param angleRadians The angle to rotate by, in radians.
            @param pointOnAxis If specified, the rotation is performed about an axis that passes through this point,
            and not through the origin. The returned matrix will not be a pure rotation matrix, but will also contain translation. */
        static Matrix4x4 RotateAxisAngle(const Vector3 &axisDirection, float angleRadians, const Vector3& pointOnAxis);
        static Matrix4x4 RotateAxisAngle(const Vector3 &axisDirection, float angleRadians);

        /// Creates a new Matrix4x4 that rotates sourceDirection vector to coincide with the targetDirection vector.
        /** @note There are infinite such rotations - this function returns the rotation that has the shortest angle
            (when decomposed to axis-angle notation)
            @param sourceDirection The 'from' direction vector. This vector must be normalized.
            @param targetDirection The 'to' direction vector. This vector must be normalized.
            @param centerPoint If specified, rotation is performed using this point as the coordinate space origin.
            If omitted, the rotation is performed about the coordinate system origin (0,0,0).
            @return A new rotation matrix R for which R*sourceDirection == targetDirection */
        static Matrix4x4 RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection, const Vector3 &centerPoint);
        static Matrix4x4 RotateFromTo(const Vector3 &sourceDirection, const Vector3 &targetDirection);
        static Matrix4x4 RotateFromTo(const Vector4 &sourceDirection, const Vector4 &targetDirection);

        /// Creates a new Matrix4x4 that rotates one coordinate system to coincide with another.
        /** This function rotates the sourceDirection vector to coincide with the targetDirection vector, and then
            rotates sourceDirection2 (which was transformed by 1.) to targetDirection2, but keeping the constraint that
            sourceDirection must look at targetDirection. */
        /** @param sourceDirection The first 'from' direction. This vector must be normalized.
            @param targetDirection The first 'to' direction. This vector must be normalized.
            @param sourceDirection2 The second 'from' direction. This vector must be normalized.
            @param targetDirection2 The second 'to' direction. This vector must be normalized.
            @param centerPoint If specified, rotation is performed using this point as the coordinate space origin.
            @return The returned matrix maps sourceDirection to targetDirection. Additionally, the returned matrix
            rotates sourceDirection2 to point towards targetDirection2 as closely as possible, under the previous constriant.
            The returned matrix is a rotation matrix, i.e. it is orthonormal with a determinant of +1, and optionally
            has a translation component if the rotation is not performed w.r.t. the coordinate system origin */
        static Matrix4x4 RotateFromTo(const Vector3& sourceDirection, const Vector3 &targetDirection,
                                      const Vector3 &sourceDirection2, const Vector3 &targetDirection2,
                                      const Vector3 &centerPoint);
        static Matrix4x4 RotateFromTo(const Vector3& sourceDirection, const Vector3 &targetDirection,
                                      const Vector3 &sourceDirection2, const Vector3 &targetDirection2);


        /// Returns the translation part.
        /** The translation part is stored in the fourth column of this matrix.
            This is equivalent to decomposing this matrix in the form M = T * M', i.e. this translation is applied last,
            after applying rotation and scale. If this matrix represents a local->world space transformation for an object,
            then this gives the world space position of the object.
            @note This function assumes that this matrix does not contain projection (the fourth row of this matrix is [0 0 0 1]). */
        Vector3 GetTranslatePart() const;
        /// Returns the top-left 3x3 part of this matrix. This stores the rotation part of this matrix (if this matrix represents a rotation).
        Matrix3x3 GetRotatePart() const;

        /// Sets the translation part of this matrix.
        /** This function sets the translation part of this matrix. These are the first three elements of the fourth column. All other entries are left untouched. */
        void SetTranslatePart(float translateX, float translateY, float translateZ);
        void SetTranslatePart(const Vector3& offset);
        /// Sets the 3-by-3 part of this matrix to perform rotation about the given axis and angle (in radians). Leaves all other
        /// entries of this matrix untouched.
        void SetRotatePart(const Quaternion& q);
        void SetRotatePart(const Vector3& axisDirection, float angleRadians);

        /// Sets the 3-by-3 part of this matrix.
        /// @note This is a convenience function which calls Set3x3Part.
        /// @note This function erases the previous top-left 3x3 part of this matrix (any previous rotation, scaling and shearing, etc.) Translation is unaffecte.d
        void SetRotatePart(const Matrix3x3& rotation) { Set3x3Part(rotation); }
        /// Sets the 3-by-3 part of this matrix to perform rotation about the positive X axis which passes through the origin.
        /// Leaves all other entries of this matrix untouched.
        void SetRotatePartX(float angleRadians);
        /// Sets the 3-by-3 part of this matrix to perform the rotation about the positive Y axis.
        /// Leaves all other entries of the matrix untouched.
        void SetRotatePartY(float angleRadians);
        /// Sets the 3-by-3 part of this matrix to perform the rotation about the positive Z axis.
        /// Leaves all other entries of the matrix untouched.
        void SetRotatePartZ(float angleRadians);
        void Set3x3Part(const Matrix3x3& r);

        void SetRow(int row, const Vector3& rowVector, float m_r3);
        void SetRow(int row, const Vector4& rowVector);
        void SetRow(int row, float m_r0, float m_r1, float m_r2, float m_r3);

        void SetCol(int col, const Vector3& colVector, float m_c3);
        void SetCol(int col, const Vector4& colVector);
        void SetCol(int col, float m_c0, float m_c1, float m_c2, float m_c3);
        void SetCol(int col, const float *data);

        Vector4 GetRow(int index) const;
        Vector4 GetColumn(int index) const;
        Vector3 GetRow3(int index) const;
        Vector3 GetColumn3(int index) const;
        Vector4 Col(int i) const;
        Vector4 Row(int i) const;
        Vector4 Col3(int i) const;
        Vector4 Row3(int i) const;

        Vector3 GetScale() const
        {

        }
        Matrix4x4 Scale(const Vector3&);

        float &At(int row, int col);
        float At(int x, int y) const;

        template <typename T>
        void Swap(T &a, T &b)
        {
            T temp = std::move(a);
            a = std::move(b);
            b = std::move(temp);
        }


        void SwapColumns(int col1, int col2);

        /// Swaps two rows.
        void SwapRows(int row1, int row2);
        /// Swapsthe xyz-parts of two rows element-by-element
        void SwapRows3(int row1, int row2);

        void ScaleRow(int row, float scalar);
        void ScaleRow3(int row, float scalar);
        void ScaleColumn(int col, float scalar);
        void ScaleColumn3(int col, float scalar);
        /// Algorithm from Eric Lengyel's Mathematics for 3D Game Programming & Computer Graphics, 2nd Ed.
        void Pivot();

        /// Tests if this matrix does not contain any NaNs or infs.
        /** @return Returns true if the entries of this float4x4 are all finite, and do not contain NaN or infs. */
        bool IsFinite() const;

        /// Tests if this matrix has an inverse.
        /** @return Returns true if this matrix can be inverted, up to the given epsilon. */
        bool IsInvertible(float epsilon = 1e-3f) const;

        Vector4 Diagonal() const;
        Vector3 Diagonal3() const;
        /// Returns the local +X 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;

        /// Accesses this structure as a float array.
        /// @return A pointer to the upper-left element. The data is contiguous in memory.
        /// ptr[0] gives the element [0][0], ptr[1] is [0][1], ptr[2] is [0][2].
        /// ptr[4] == [1][0], ptr[5] == [1][1], ..., and finally, ptr[15] == [3][3].
        float *ptr() { return &elems[0][0]; }
        const float *ptr() const { return &elems[0][0]; }

        float Determinant3x3() const;
        /// Computes the determinant of this matrix.
        // If the determinant is nonzero, this matrix is invertible.
        float Determinant() const;

        #define SKIPNUM(val, skip) (val >= skip ? (val+1) : val)

        float Minor(int i, int j) const;

        Matrix4x4 Inverse() const;

        Matrix4x4 Transpose() const;

        Vector2 Transform(float tx, float ty) const;
        Vector2 Transform(const Vector2& rhs) const;


        Vector3 Transform(float tx, float ty, float tz) const;
        Vector3 Transform(const Vector3& rhs) const;

        Vector4 Transform(float tx, float ty, float tz, float tw) const;
        Vector4 Transform(const Vector4& rhs) const;


        Matrix4x4 Translate(const Vector3& rhs) const;
        static Matrix4x4 FromTranslation(const Vector3& rhs);


        static Matrix4x4 D3DOrthoProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
        static Matrix4x4 D3DOrthoProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
        static Matrix4x4 D3DPerspProjLH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);
        static Matrix4x4 D3DPerspProjRH(float nearPlane, float farPlane, float hViewportSize, float vViewportSize);

        /// Computes a left-handled orthographic projection matrix for OpenGL.
        /// @note Use the M*v multiplication order to project points with this matrix.
        static Matrix4x4 OpenGLOrthoProjLH(float n, float f, float h, float v);
        /// Computes a right-handled orthographic projection matrix for OpenGL.
        /// @note Use the M*v multiplication order to project points with this matrix.
        static Matrix4x4 OpenGLOrthoProjRH(float n, float f, float h, float v);

        /// Computes a left-handed perspective projection matrix for OpenGL.
        /// @note Use the M*v multiplication order to project points with this matrix.
        static Matrix4x4 OpenGLPerspProjLH(float n, float f, float h, float v);
        /// Identical to http://www.opengl.org/sdk/docs/man/xhtml/gluPerspective.xml , except uses viewport sizes instead of FOV to set up the
        /// projection matrix.
        /// @note Use the M*v multiplication order to project points with this matrix.
        static Matrix4x4 OpenGLPerspProjRH(float n, float f, float h, float v);

        Vector4 operator[](int row);

        Matrix4x4 operator-() const;
        Matrix4x4 operator +(const Matrix4x4& rhs) const;
        Matrix4x4 operator - (const Matrix4x4& rhs) const;

        Matrix4x4 operator *(float scalar) const;
        Matrix4x4 operator /(float scalar) const;

        Vector4 operator[](int row) const;

        Vector2 operator * (const Vector2& rhs) const;
        Vector3 operator * (const Vector3& rhs) const;
        Vector4 operator * (const Vector4& rhs) const;

        Vector2 Mul(const Vector2& rhs) const;
        Vector3 Mul(const Vector3& rhs) const;
        Vector4 Mul(const Vector4& rhs) const;

        Matrix4x4 operator * (const Matrix3x3 &rhs) const;

        Matrix4x4 operator +() const;

        Matrix4x4 operator * (const Matrix4x4& rhs) const;

        Matrix4x4 &operator = (const Matrix3x3& rhs);
        Matrix4x4 &operator = (const Quaternion& rhs);
        Matrix4x4 &operator = (const Matrix4x4& rhs) = default;


        Vector3 ExtractScale() const;

        bool HasUniformScale(float epsilon = 1e-3f) const;
        bool IsColOrthogonal3(float epsilon = 1e-3f) const;
        bool IsRowOrthogonal3(float epsilon = 1e-3f) const;

        bool IsColOrthogonal(float epsilon = 1e-3f) const;
        bool IsRowOrthogonal(float epsilon = 1e-3f) const;
        /// Returns true if this matrix is seen to contain a "projective" part,
        /// i.e. whether the last row of this matrix differs from [0 0 0 1]
        bool ContainsProjection(float epsilon = 1e-3f) const;


        /// Sets all values of this matrix.
        void Set(float _00, float _01, float _02, float _03,
                 float _10, float _11, float _12, float _13,
                 float _20, float _21, float _22, float _23,
                 float _30, float _31, float _32, float _34);

        /// Sets this to be a copy of the matrix rhs.
        void Set(const Matrix4x4 &rhs);

        /// Sets all values of this matrix.
        /** @param values The values in this array will be copied over to this matrix. The source must contain 16 floats in row-major order
            (the same order as the Set() ufnction above has its input parameters in. */
        void Set(const float *values);

        /// Sets this matrix to equal the identity.
        void SetIdentity();
        /// Returns the adjugate of this matrix.
        Matrix4x4 Adjugate() const;

        /// Computes the Cholesky decomposition of this matrix.
        /// The returned matrix L satisfies L * transpose(L) = this;
        /// Returns true on success.
        bool ColeskyDecompose(Matrix4x4 &outL) const;

        /// Computes the LU decomposition of this matrix.
        /// This decomposition has the form 'this = L * U'
        /// Returns true on success.
        bool LUDecompose(Matrix4x4& outLower, Matrix4x4& outUpper) const;

        /// Inverts this matrix using the generic Gauss's method.
        /// @return Returns true on success, false otherwise.
        bool Inverse(float epsilon = 1e-6f)
        {
            return InverseMatrix(*this, epsilon);
        }

        /// Returns an inverted copy of this matrix.
        /// If this matrix does not have an inverse, returns the matrix that was the result of running
        /// Gauss's method on the matrix.
        Matrix4x4 Inverted() const;

        /// Inverts a column-orthogonal matrix.
        /// If a matrix is of form M=T*R*S, where T is an affine translation matrix
        /// R is a rotation matrix and S is a diagonal matrix with non-zero but pote ntially non-uniform scaling
        /// factors (possibly mirroring), then the matrix M is column-orthogonal and this function can be used to compute the inverse.
        /// Calling this function is faster than the calling the generic matrix Inverse() function.
        /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
        /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
        /// @note The returned matrix will be row-orthogonal, but not column-orthogonal in general.
        /// The returned matrix will be column-orthogonal if the original matrix M was row-orthogonal as well.
        /// (in which case S had uniform scale, InverseOrthogonalUniformScale() could have been used instead).
        bool InverseColOrthogonal();


        /// Inverts a matrix that is a concatenation of only translate, rotate, and uniform scale operations.
        /// If a matrix is of form M = T*R*S, where T is an affine translation matrix,
        /// R is a rotation matrix and S is a diagonal matrix with non-zero and uniform scaling factors (possibly mirroring),
        /// then the matrix M is both column- and row-orthogonal and this function can be used to compute this inverse.
        /// This function is faster than calling InverseColOrthogonal() or the generic Inverse().
        /// Returns true on success. On failure, the matrix is not modified. This function fails if any of the
        /// elements of this vector are not finite, or if the matrix contains a zero scaling factor on X, Y, or Z.
        /// This function may not be called if this matrix contains any shearing or nonuniform scaling.
        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
        bool InverseOrthogonalUniformScale();

        /// Inverts a matrix that is a concatenation of only translate and rotate operations.
        /// If a matrix is of form M = T*R*S, where T is an affine translation matrix, R is a rotation
        /// matrix and S is either identity or a mirroring matrix, then the matrix M is orthonormal and this function can be used to compute the inverse.
        /// This function is faster than calling InverseOrthogonalUniformScale(), InverseColOrthogonal(), or the generic Inverse().
        /// This function may not be called if this matrix contains any scaling or shearing, but it may contain mirroring.
        /// This function may not be called if this matrix contains any projection (last row differs from (0 0 0 1)).
        void InverseOrthonormal();

        /// Transposes this matrix.
        /// This operation swaps all elements with respect to the diagonal.
        void Transpose();
        /// Returns a transposed copy of this matrix.
        Matrix4x4 Transposed() const;
        /// Computes the inverse transpose of this matrix in-place.
        /// Use the inverse transpose to transform covariant vectors (normal vectors).
        bool InverseTranspose();
        /// Returns the inverse transpose of this matrix.
        /// Use that matrix to transform covariant vectors (normal vectors).
        Matrix4x4 InverseTransposed() const
        {
            Matrix4x4 copy = *this;
            copy.Transpose();
            copy.Inverse();
            return copy;
        }
        /// Returns the sum of the diagonal elements of this matrix.
        float Trace() const;

    protected:
        float elems[4][4];


        Vector3 TransformDir(float tx, float ty, float tz) const;
    };
}