#pragma once

/// Template Parameterized (Generic) Matrix Functions.

#include <J3ML/LinearAlgebra/Quaternion.hpp>
#include "J3ML/J3ML.hpp"


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

        return false;
    }


    template<typename Matrix>
    void SetMatrixRotatePart(Matrix &m, const Quaternion &q) {
        // See https://www.geometrictools.com/Documentation/LinearAlgebraicQuaternions.pdf .

        assert(q.IsNormalized(1e-3f));
        const float x = q.x;
        const float y = q.y;
        const float z = q.z;
        const float w = q.w;
        m[0][0] = 1 - 2 * (y * y + z * z);
        m[0][1] = 2 * (x * y - z * w);
        m[0][2] = 2 * (x * y + y * w);
        m[1][0] = 2 * (x * y + z * w);
        m[1][1] = 1 - 2 * (x * x + z * z);
        m[1][2] = 2 * (y * z - x * w);
        m[2][0] = 2 * (x * z - y * w);
        m[2][1] = 2 * (y * z + x * w);
        m[2][2] = 1 - 2 * (x * x + y * y);
    }

    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the X-axis. Elements
        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
        in the order M*v, and clockwise if rotated in the order v*M.
        @param m The matrix to store the result.
        @param angle the rotation angle in radians. */
    template<typename Matrix>
    void Set3x3PartRotateX(Matrix &m, float angle) {
        float sinz, cosz;
        sinz = Math::Sin(angle);
        cosz = Math::Cos(angle);

        m[0][0] = 1.f;
        m[0][1] = 0.f;
        m[0][2] = 0.f;
        m[1][0] = 0.f;
        m[1][1] = cosz;
        m[1][2] = -sinz;
        m[2][0] = 0.f;
        m[2][1] = sinz;
        m[2][2] = cosz;
    }

    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Y-axis. Elements
        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
        in the order M*v, and clockwise if rotated in the order v*M.
        @param m The matrix to store the result
        @param angle The rotation angle in radians. */
    template<typename Matrix>
    void Set3x3PartRotateY(Matrix &m, float angle) {
        float sinz, cosz;
        sinz = Math::Sin(angle);
        cosz = Math::Cos(angle);

        m[0][0] = cosz;
        m[0][1] = 0.f;
        m[0][2] = sinz;
        m[1][0] = 0.f;
        m[1][1] = 1.f;
        m[1][2] = 0.f;
        m[2][0] = -sinz;
        m[2][1] = 0.f;
        m[2][2] = cosz;
    }

    /** Sets the top-left 3x3 area of the matrix to the rotation matrix about the Z-axis. Elements
        outside the top-left 3x3 area are ignored. This matrix rotates counterclockwise if multiplied
        in the order of M*v, and clockwise if rotated in the order v*M.
        @param m The matrix to store the result.
        @param angle The rotation angle in radians. */
    template<typename Matrix>
    void Set3x3PartRotateZ(Matrix &m, float angle) {
        float sinz, cosz;
        sinz = std::sin(angle);
        cosz = std::cos(angle);

        m[0][0] = cosz;
        m[0][1] = -sinz;
        m[0][2] = 0.f;
        m[1][0] = sinz;
        m[1][1] = cosz;
        m[1][2] = 0.f;
        m[2][0] = 0.f;
        m[2][1] = 0.f;
        m[2][2] = 1.f;
    }


    /** Computes the matrix M = R_x * R_y * R_z, where R_d is the cardinal rotation matrix
        about the axis +d, rotating counterclockwise.
        This function was adapted from https://www.geometrictools.com/Documentation/EulerAngles.pdf .
        Parameters x y and z are the angles of rotation, in radians. */
    template<typename Matrix>
    void Set3x3PartRotateEulerXYZ(Matrix &m, float x, float y, float z) {
        // TODO: vectorize to compute 4 sines + cosines at one time;
        float cx = std::cos(x);
        float sx = std::sin(x);
        float cy = std::cos(y);
        float sy = std::sin(y);
        float cz = std::cos(z);
        float sz = std::sin(z);

        m[0][0] = cy * cz;
        m[0][1] = -cy * sz;
        m[0][2] = sy;
        m[1][0] = cz * sx * sy + cx * sz;
        m[1][1] = cx * cz - sx * sy * sz;
        m[1][2] = -cy * sx;
        m[2][0] = -cx * cz * sy + sx * sz;
        m[2][1] = cz * sx + cx * sy * sz;
        m[2][2] = cx * cy;
    }



}