/**
 * @file Mat4.h
 * 4x4 matrix class.
 */
//
// Copyright 2020 Arvind Singh
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
//version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; If not, see <http://www.gnu.org/licenses/>.


#ifndef _TGX_MAT4_H_
#define _TGX_MAT4_H_

// only C++, no plain C
#ifdef __cplusplus


#include <stdint.h>
#include <type_traits>

#include "Misc.h"
#include "Vec2.h"
#include "Vec3.h"
#include "Vec4.h"


namespace tgx
{

    // forward declaration

    template<typename T> struct Vec2;

    template<typename T> struct Vec3;

    template<typename T> struct Vec4;

    template<typename T> struct Mat4;


    // Specializations (only floating types T make sense for the matrix class).

    typedef Mat4<float>   fMat4;  ///< 4x4 matrix with single (float) precision

    typedef Mat4<double>  dMat4;  ///< 4x4 matrix with double precision


    // forward declaration of matrix multiplication
    template<typename T> Mat4<T> operator*(const Mat4<T> & A, const Mat4<T> & B);



    /**
     * Generic 4x4 matrix [specializations #fMat4, #dMat4]
     *
     * The class encapsulate a 4x4 matrix with element of type `T` which must be a floating point
     * type (either `float` or `double`). Such a matrix is used in 3D grpahics to represent a
     * transformation (translation, rotation, dilatation...).
     *
     * The matrix is internally represented by an public array `M[16]` in column major ordering:
     *
     * ```
     * +-----------------------------+
     * | M[0] | M[4] | M[8]  | M[12] |
     * |------+------+-------+-------|
     * | M[1] | M[5] | M[9]  | M[13] |
     * |------+------+-------+-------|
     * | M[2] | M[6] | M[10] | M[14] |
     * |------+------+-------+-------|
     * | M[3] | M[7] | M[11] | M[15] |
     * +-----------------------------+
     * ```
     *
     * @sa Vec2, Vec3, Vec4
     */
    template<typename T> struct Mat4
        {

        static_assert(std::is_floating_point<T>::value, "The template parameter T of class Mat4<T> must be a floating point number.");


        // mtools extension (if available).
        #if (MTOOLS_TGX_EXTENSIONS)
        #include <mtools/extensions/tgx/tgx_ext_Mat4.inl>
        #endif


        /**
         * The matrix array in column major ordering:
         *
         * ```
         * +-----------------------------+
         * | M[0] | M[4] | M[8]  | M[12] |
         * |------+------+-------+-------|
         * | M[1] | M[5] | M[9]  | M[13] |
         * |------+------+-------+-------|
         * | M[2] | M[6] | M[10] | M[14] |
         * |------+------+-------+-------|
         * | M[3] | M[7] | M[11] | M[15] |
         * +-----------------------------+
         * ```
        **/
        T M[16];


        /**
         * Default constructor. **the matrix content is undefined**.
         */
        Mat4() {}


        /**
         * Constructor from explicit values.
         */
        constexpr Mat4(T x1, T y1, T z1, T t1,
                       T x2, T y2, T z2, T t2,
                       T x3, T y3, T z3, T t3,
                       T x4, T y4, T z4, T t4) : M{ x1,x2,x3,x4, y1,y2,y3,y4, z1,z2,z3,z4, t1,t2,t3,t4 }
            {
            }


        /**
         * Constructor from an array (with column major ordering same as `M`).
         */
        Mat4(const T* mat) { memcpy(M, mat, 16 * sizeof(T)); }


        /**
         * Copy constructor.
         */
        Mat4(const Mat4 & mat) = default;


        /**
         * Assignment operator.
         */
        Mat4& operator=(const Mat4 & mat) = default;


        /**
         * Implicit conversion to an array T[16].
         */
        operator T*() { return M; }

        /**
         * Implicit conversion to a const array T[16].
         */
        operator const T*() const { return M; }


        /**
         * Set as the null matrix (all coefficients equal 0).
         */
        void setZero()
            {
            memset(M, 0, 16 * sizeof(T));
            }


        /**
         * Set as the identity matrix.
         */
        void setIdentity()
            {
            memset(M, 0, 16 * sizeof(T));
            M[0] = M[5] = M[10] = M[15] = ((T)1);
            }


        /**
         * Set as an orthographic projection matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glOrtho.xml
         *
         * @param left, right   coordinates for the left and right vertical clipping planes
         * @param bottom, top   coordinates for the bottom and top horizontal clipping planes.
         * @param zNear, zFar   distances to the nearer and farther depth clipping planes.
         */
        void setOrtho(T left, T right, T bottom, T top, T zNear, T zFar)
            {
            memset(M, 0, 16 * sizeof(T));
            M[0] = ((T)2) / (right - left);
            M[5] = ((T)2) / (top - bottom);
            M[10] = -((T)2) / (zFar - zNear);
            M[12] = (right + left) / (left - right);
            M[13] = (top + bottom) / (bottom - top);
            M[14] = (zFar + zNear) / (zNear - zFar);
            M[15] = (T)1;
            }


        /**
         * Set as a perspective projection matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glFrustum.xml
         *
         * @param left, right   coordinates for the left and right vertical clipping planes
         * @param bottom, top   coordinates for the bottom and top horizontal clipping planes.
         * @param zNear, zFar   distances to the nearer and farther depth clipping planes.
         */
        void setFrustum(T left, T right, T bottom, T top, T zNear, T zFar)
            {
            memset(M, 0, 16 * sizeof(T));
            M[0] = (((T)2) * zNear) / (right - left);
            M[5] = (((T)2) * zNear) / (top - bottom);
            M[8] = (right + left) / (right - left);
            M[9] = (top + bottom) / (top - bottom);
            M[10] = (zFar + zNear) / (zNear - zFar);
            M[11] = -((T)1);
            M[14] = (((T)2) * zFar * zNear) / (zNear - zFar);
            }


        /**
         * Set as a perspective projection matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/gluPerspective.xml
         *
         * @param   fovy   field of view angle, in degrees, in the y direction.
         * @param   aspect aspect ratio that determines the field of view in the x direction. The aspect ratio is the ratio of x (width) to y (height).
         * @param   zNear  distance from the viewer to the near clipping plane.
         * @param   zFar   distance from the viewer to the far clipping plane.
         */
        void setPerspective(T fovy, T aspect, T zNear, T zFar)
            {
            static const T deg2rad = (T)(M_PI / 180);
            const T angle = (fovy / ((T)2)) * deg2rad;
            T aux;
            if constexpr (std::is_same<T, float>::value)
                aux = (T)tanf((float)angle);
            else
                aux = (T)tan((double)angle);
            const T top = zNear * aux;
            const T bottom = -top;
            const T right = zNear * aspect * aux;
            const T left = -right;
            setFrustum(left, right, bottom, top, zNear, zFar);
            }


        /**
         * Set as a rotation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glRotate.xml
         *
         * @param   angle rotation angle in degrees.
         * @param   x,y,z coordinates of the direction vector for the rotation.
         */
        void setRotate(T angle, T x, T y, T z)
            {
            static const T deg2rad = (T)(M_PI / 180);
            const T norm = tgx::precise_sqrt(x*x + y*y + z*z);
            if (norm == 0) { setIdentity(); return; }
            const T nx = x / norm;
            const T ny = y / norm;
            const T nz = z / norm;
            const T angle_rad = deg2rad * angle;
            T c, s;
            if constexpr (std::is_same<T, float>::value)
                {
                c = (T)cosf((float)angle_rad);
                s = (T)sinf((float)angle_rad);
                }
            else
                {
                c = (T)cos((double)angle_rad);
                s = (T)sin((double)angle_rad);
                }
            const T oneminusc = ((T)1) - c;

            memset(M, 0, 16 * sizeof(T));
            M[0] = nx * nx * oneminusc + c;
            M[1] = ny * nx * oneminusc + nz * s;
            M[2] = nx * nz * oneminusc - ny * s;
            M[4] = nx * ny * oneminusc - nz * s;
            M[5] = ny * ny * oneminusc + c;
            M[6] = ny * nz * oneminusc + nx * s;
            M[8] = nx * nz * oneminusc + ny * s;
            M[9] = ny * nz * oneminusc - nx * s;
            M[10] = nz * nz * oneminusc + c;
            M[15] = (T)1;
            }


        /**
         * Set as a rotation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glRotate.xml
         *
         * @param   angle rotation angle in degrees.
         * @param   v     direction vector for the rotation.
         */
        void setRotate(T angle, const Vec3<T> v)
            {
            setRotate(angle, v.x, v.y, v.z);
            }


        /**
         * Pre-multiply this matrix by a rotation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glRotate.xml
         *
         * @param   angle rotation angle in degrees.
         * @param   x,y,z coordinates of the direction vector for the rotation.
         */
        void multRotate(T angle, T x, T y, T z)
            {
            Mat4 mat;
            mat.setRotate(angle, x, y, z);
            *this = (mat * (*this));
            }



        /**
         * Pre-multiply this matrix by a rotation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glRotate.xml
         *
         * @param   angle rotation angle in degrees.
         * @param   v     direction vector for the rotation.
         */
        void multRotate(T angle, const Vec3<T> v)
            {
            multRotate(angle, v.x, v.y, v.z);
            }


        /**
         * Set as a translation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glTranslate.xml
         *
         * @param   x,y,z   coordinates of the translation vector.
         */
        void setTranslate(T x, T y, T z)
            {
            memset(M, 0, 16 * sizeof(T));
            M[0] = (T)1;
            M[5] = (T)1;
            M[10] = (T)1;
            M[12] = x;
            M[13] = y;
            M[14] = z;
            M[15] = (T)1;
            }


        /**
         * Set as a translation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glTranslate.xml
         *
         * @param   v   the translation vector.
         */
        void setTranslate(const Vec3<T> v)
            {
            setTranslate(v.x, v.y, v.z);
            }


        /**
         * Pre-multiply this matrix by a translation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glTranslate.xml
         *
         * @param   x,y,z   coordinates of the translation vector.
        **/
        void multTranslate(T x, T y, T z)
            {
            Mat4 mat;
            mat.setTranslate(x, y, z);
            *this = (mat * (*this));
            }


        /**
         * Pre-multiply this matrix by a translation matrix.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glTranslate.xml
         *
         * @param   v   the translation vector.
        **/
        void multTranslate(const Vec3<T> v)
            {
            multTranslate(v.x, v.y, v.z);
            }


        /**
         * Set as a dilatation matrix
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glScale.xml
         *
         * @param   x,y,z   scale factors along the x, y, and z axes respectively.
        **/
        void setScale(T x, T y, T z)
            {
            memset(M, 0, 16 * sizeof(T));
            M[0] = x;
            M[5] = y;
            M[10] = z;
            M[15] = (T)1;
            }


        /**
         * Set as a dilatation matrix
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glScale.xml
         *
         * @param   v   vector representing the scale factors along each axis.
        **/
        void setScale(const Vec3<T> v)
            {
            setScale(v.x, v.y, v.z);
            }


        /**
        * Pre-multiply this matrix by a dilatation matrix.
        *
        * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glScale.xml
         *
         * @param   x,y,z   scale factors along the x, y, and z axes respectively.
        **/
        void multScale(T x, T y, T z)
            {
            Mat4 mat;
            mat.setScale(x, y, z);
            *this = (mat * (*this));
            }


        /**
        * Pre-multiply this matrix by a dilatation matrix
        *
        * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/glScale.xml
         *
         * @param   v   vector representing the scale factors along each axis.
        **/
        void multScale(const Vec3<T> v)
            {
            multScale(v.x, v.y, v.z);
            }


        /**
        * invert the y axis, same as multScale({1,-1,1})
        **/
        void invertYaxis()
            {
            M[1] = -M[1];
            M[5] = -M[5];
            M[9] = -M[9];
            M[13] = -M[13];
            }


        /**
         * Set the matrix for a camera looking at a given direction.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/gluLookAt.xml
         *
         * The formula given by khronos is wrong ! The cross-product of two unit vectors is not
         * normalized.
         *
         * See https://stackoverflow.com/questions/30409318/lookat-matrix-distorts-when-looking-up-or-down
         *
         * @param   eyeX, eyeY, eyeZ            position of the eye point.
         * @param   centerX, centerY, centerZ   position of the reference point (the camera points toward this point).
         * @param   upX, upY, upZ               direction of the up vector.
         */
        void setLookAt(T eyeX, T eyeY, T eyeZ, T centerX, T centerY, T centerZ, T upX, T upY, T upZ)
            {
             const Vec4<T> f = normalize(Vec4<T>{ centerX - eyeX, centerY - eyeY, centerZ - eyeZ, (T)0 });
            Vec4<T> up = normalize(Vec4<T>{ upX, upY, upZ, (T)0 });
            up = normalize(up - (dotProduct(up, f) * f));
            const Vec4<T> s = crossProduct(f, up);
            const Vec4<T> u = crossProduct(s, f);
            M[0] = s.x;    M[4] = s.y;    M[8] = s.z;     M[12] = -s.x * eyeX - s.y * eyeY - s.z * eyeZ;
            M[1] = u.x;    M[5] = u.y;    M[9] = u.z;     M[13] = -u.x * eyeX - u.y * eyeY - u.z * eyeZ;
            M[2] = -f.x;   M[6] = -f.y;   M[10] = -f.z;   M[14] = f.x * eyeX + f.y * eyeY + f.z * eyeZ;
            M[3] = 0;      M[7] = 0;      M[11] = 0;      M[15] = (T)1;
            }


        /**
         * Set the matrix for a camera looking at a given direction.
         *
         * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/gluLookAt.xml
         *
         * The formula given by khronos is wrong ! The cross-product of two unit vectors is not
         * normalized.
         *
         * See https://stackoverflow.com/questions/30409318/lookat-matrix-distorts-when-looking-up-or-down
         *
         * @param   eye     eye position.
         * @param   center  reference point (the camera points toward this point).
         * @param   up      up vector.
         */
        void setLookAt(const Vec3<T> eye, const Vec3<T> center, const Vec3<T> up)
            {
            setLookAt(eye.x, eye.y, eye.z, center.x, center.y, center.z, up.x, up.y, up.z);
            }


        /**
        * Matrix-vector multiplication.
        */
         TGX_INLINE inline Vec4<T> mult(const Vec4<T> V) const
            {
            return Vec4<T>{ M[0] * V.x + M[4] * V.y + M[8] * V.z + M[12] * V.w,
                M[1] * V.x + M[5] * V.y + M[9] * V.z + M[13] * V.w,
                M[2] * V.x + M[6] * V.y + M[10] * V.z + M[14] * V.w,
                M[3] * V.x + M[7] * V.y + M[11] * V.z + M[15] * V.w };
            }


        /**
        * Matrix-vector multiplication.
        */
        TGX_INLINE inline Vec4<T> mult(const Vec3<T> & V, T w) const
            {
#if TGX_USE_FMA_MATH
            if constexpr (std::is_same<T, float>::value)
                {
                return Vec4<T>{ fmaf(M[0], V.x, fmaf(M[4], V.y, fmaf(M[8], V.z, M[12] * w))),
                    fmaf(M[1], V.x, fmaf(M[5], V.y, fmaf(M[9], V.z, M[13] * w))),
                    fmaf(M[2], V.x, fmaf(M[6], V.y, fmaf(M[10], V.z, M[14] * w))),
                    fmaf(M[3], V.x, fmaf(M[7], V.y, fmaf(M[11], V.z, M[15] * w))) };
                }
#endif
            return Vec4<T>{ M[0] * V.x + M[4] * V.y + M[8] * V.z + M[12] * w,
                M[1] * V.x + M[5] * V.y + M[9] * V.z + M[13] * w,
                M[2] * V.x + M[6] * V.y + M[10] * V.z + M[14] * w,
                M[3] * V.x + M[7] * V.y + M[11] * V.z + M[15] * w };
            }


        /**
        * Matrix-vector multiplication (last component of vector set to w = 0).
        */
        TGX_INLINE inline Vec4<T> mult0(const Vec3<T> & V) const
            {
#if TGX_USE_FMA_MATH
            if constexpr (std::is_same<T, float>::value)
                {
                return Vec4<T>{ fmaf(M[0], V.x, fmaf(M[4], V.y, M[8] * V.z)),
                    fmaf(M[1], V.x, fmaf(M[5], V.y, M[9] * V.z)),
                    fmaf(M[2], V.x, fmaf(M[6], V.y, M[10] * V.z)),
                    fmaf(M[3], V.x, fmaf(M[7], V.y, M[11] * V.z)) };
                }
#endif
            return Vec4<T>{ M[0] * V.x + M[4] * V.y + M[8] * V.z,
                M[1] * V.x + M[5] * V.y + M[9] * V.z,
                M[2] * V.x + M[6] * V.y + M[10] * V.z,
                M[3] * V.x + M[7] * V.y + M[11] * V.z };
            }


        /**
        * Matrix-vector multiplication (last component of vector set to w = 1)
        */
        TGX_INLINE inline Vec4<T> mult1(const Vec3<T> & V) const
            {
#if TGX_USE_FMA_MATH
            if constexpr (std::is_same<T, float>::value)
                {
                return Vec4<T>{ fmaf(M[0], V.x, fmaf(M[4], V.y, fmaf(M[8], V.z, M[12]))),
                    fmaf(M[1], V.x, fmaf(M[5], V.y, fmaf(M[9], V.z, M[13]))),
                    fmaf(M[2], V.x, fmaf(M[6], V.y, fmaf(M[10], V.z, M[14]))),
                    fmaf(M[3], V.x, fmaf(M[7], V.y, fmaf(M[11], V.z, M[15]))) };
                }
#endif
            return Vec4<T>{ M[0] * V.x + M[4] * V.y + M[8] * V.z + M[12],
                M[1] * V.x + M[5] * V.y + M[9] * V.z + M[13],
                M[2] * V.x + M[6] * V.y + M[10] * V.z + M[14],
                M[3] * V.x + M[7] * V.y + M[11] * V.z + M[15]};
            }


        //
        // DO NOT DEFINE TO PREVENT ANBIGUITY !
        // Matrix multiplication : (*this ) = M * (*this)
        //
        //
        //void operator*=(const Mat4 & M)
        //    {
        //    *this = (M * (*this));
        //    }


        /**
        * Scalar  multiplication.
        **/
        inline void operator*=(T a)
            {
            for (int i = 0; i < 16; i++) { M[i] *= a; }
            }



        /**
        * Transpose the matrix (in place)
        **/
        inline void transpose()
            {
            tgx::swap(M[4],M[1]);
            tgx::swap(M[8],M[2]);
            tgx::swap(M[12],M[3]);
            tgx::swap(M[13],M[7]);
            tgx::swap(M[14],M[11]);
            tgx::swap(M[9],M[6]);
            }













#ifdef TGX_ON_ARDUINO


        /**
        * Output a representation of the matrix using a given Stream.
        *
        * @attention Defined only in the Arduino environment.
        */
        inline void print(Stream & outputStream = Serial) const
            {
            outputStream.printf("%.3f  \t %.3f  \t %.3f \t  %.3f\n", M[0],M[4],M[8],M[12]);
            outputStream.printf("%.3f  \t %.3f  \t %.3f \t  %.3f\n", M[1], M[5], M[9], M[13]);
            outputStream.printf("%.3f  \t %.3f  \t %.3f \t  %.3f\n", M[2], M[6], M[10], M[14]);
            outputStream.printf("%.3f  \t %.3f  \t %.3f \t  %.3f\n\n", M[3], M[7], M[11], M[15]);
            }

#endif

        };



    /**
    * Matrix-vector multiplication
    */
    template<typename T> TGX_INLINE inline Vec4<T> operator*(const Mat4<T> & M, const Vec4<T> V)
        {
#if TGX_USE_FMA_MATH
        if constexpr (std::is_same<T, float>::value)
            {
            return Vec4<T>{ fmaf(M.M[0], V.x, fmaf(M.M[4], V.y, fmaf(M.M[8], V.z, M.M[12] * V.w))),
                            fmaf(M.M[1], V.x, fmaf(M.M[5], V.y, fmaf(M.M[9], V.z, M.M[13] * V.w))),
                            fmaf(M.M[2], V.x, fmaf(M.M[6], V.y, fmaf(M.M[10], V.z, M.M[14] * V.w))),
                            fmaf(M.M[3], V.x, fmaf(M.M[7], V.y, fmaf(M.M[11], V.z, M.M[15] * V.w))) };
            }
#endif
        return Vec4<T>{ M.M[0] * V.x + M.M[4] * V.y + M.M[8] * V.z + M.M[12] * V.w,
                        M.M[1] * V.x + M.M[5] * V.y + M.M[9] * V.z + M.M[13] * V.w,
                        M.M[2] * V.x + M.M[6] * V.y + M.M[10] * V.z + M.M[14] * V.w,
                        M.M[3] * V.x + M.M[7] * V.y + M.M[11] * V.z + M.M[15] * V.w  };
        }



    /**
    * Matrix-matrix multiplication
    */
    template<typename T> inline Mat4<T> operator*(const Mat4<T> & A, const Mat4<T> & B)
        {
        Mat4<T> R;
        for (int i = 0; i < 4; i++)
            {
            for (int j = 0; j < 4; j++)
                {
                R.M[i + j*4] = (T)0;
                for (int k = 0; k < 4; k++) { R.M[i + j * 4] += A.M[i + k * 4] * B.M[k + j * 4]; }
                }
            }
        return R;
        }


    /**
     * Scalar-matrix multiplication.
     */
    template<typename T> inline Mat4<T> operator*(T a, const Mat4<T> & m)
        {
        Mat4<T> R(m);
        for (int i = 0; i < 16; i++) { R.M[i] *= a; }
        return R;
        }






}

#endif

#endif

/** end of file */


