#pragma once

/// Template Parameterized (Generic) Matrix Functions.

#include <J3ML/LinearAlgebra/Quaternion.h>



namespace J3ML::LinearAlgebra {

    /** 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 = std::sin(angle);
        cosz = std::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 = std::sin(angle);
        cosz = std::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;
    }



}