text:
/**************************************************************************************************
 * Class for MPC with constraint
 *
 *  The plant to be controlled is a Linear Time-Invariant System:
 *          x(k+1)  = A*x(k) + B*u(k)   ; x = Nx1, u = Mx1
 *          z(k)    = C*x(k)            ; z = Zx1
 *
 *
 ** Calculate prediction of z(k+1..k+Hp) constants ************************************************
 *
 *      Prediction of state variable of the system:
 *        z(k+1..k+Hp) = (CPSI)*x(k) + (COMEGA)*u(k-1) + (CTHETA)*dU(k..k+Hu-1)         ...{MPC_1}
 *
 *        Constants:
 *          CPSI   = [CA C(A^2) ... C(A^Hp)]'                                       : (Hp*N)xN
 *          COMEGA = [CB C(B+A*B) ... C*Sigma(i=0->Hp-1)A^i*B]'                     : (Hp*N)xM
 *          CTHETA = [         CB                0  ....           0              ]
 *                   [       C(B+A*B)           CB   .             0              ]
 *                   [           .               .    .           CB              ] : (Hp*N)x(Hu*M)
 *                   [           .               .     .           .              ]
 *                   [C*Sigma(i=0->Hp-1)(A^i*B)  .  ....  C*Sigma(i=0->Hp-Hu)A^i*B]
 *
 *
 ** Calculate offline optimization variable H *****************************************************
 *
 *      H = CTHETA'*Q*CTHETA + R                                                        ...{MPC_2}
 *
 ** MPC update algorithm **************************************************************************
 *
 *      Formulation of plant error prediction
 *          E(k) = SP(k) - CPSI*x(k) - COMEGA*u(k-1)                                    ...{MPC_3}
 *
 *      Calculate online optimization variable G:
 *          G = 2*CTHETA'*Q*E(k)                                                        ...{MPC_4}
 *
 *      Create the inequality constraints                                               ...{MPC_5}
 *          dUmin <= dU(k) <= dUmax                                                     ...{MPC_5a}
 *          Umin <= U(kj <= Umax                                                        ...{MPC_5b}
 *          Zmin <= Z(kj <= Zmax                                                        ...{MPC_5c}
 *
 *      MPC solution:
 *          (a) For unconstrained MPC:
 *              d [dU(k)'*H*dU(k) - G'*dU(k)]
 *              ----------------------------- = 0   -->   2*H*dU(k)-G = 0
 *                      d[dU(k)]
 *
 *              --> dU(k)_optimal = 1/2 * H^-1 * G
 *              --> https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library
 *
 *          (b) For constrained MPC (quadrating programming):
 *                  min   dU(k)'*H*dU(k) - G'*dU(k)     ; subject to inequality constraints
 *                 dU(k)
 *
 *              Reconditioning H & G matrices into a standard QP form:
 *                x_opt = arg Min  1/2*x'*Q*x + c'*x    ; subject to ineqLHS*x <= ineqRHS
 *                           x
 *
 *              Then solve by Active Set:
 *                  dU_opt(k) = ActiveSet(2H, -G, ineqLHS, ineqRHS)                     ...{MPC_6}
 *
 *      Integrate the du(k) to get u(k):
 *              u(k) = u(k-1) + du(k)                                                   ...{MPC_7}
 *
 *
 *
 * See https://github.com/pronenewbits for more!
 *************************************************************************************************/
#include "mpc.h"

MPC::MPC(Matrix &A, Matrix &B, Matrix &C, float_prec _bobotQ, float_prec _bobotR, void (*vCreateConstraintsLHS)(Matrix &, Matrix &), void (*vCreateConstraintsRHS)(Matrix &, Matrix &, Matrix &, Matrix &, Matrix &))
{
    vReInit(A, B, C, _bobotQ, _bobotR, vCreateConstraintsLHS, vCreateConstraintsRHS);
}

void MPC::vReInit(Matrix &A, Matrix &B, Matrix &C, float_prec _bobotQ, float_prec _bobotR, void (*vCreateConstraintsLHS)(Matrix &, Matrix &), void (*vCreateConstraintsRHS)(Matrix &, Matrix &, Matrix &, Matrix &, Matrix &))
{
    this->A = A;
    this->B = B;
    this->C = C;
    this->vCreateConstraintsLHS = vCreateConstraintsLHS;
    this->vCreateConstraintsRHS = vCreateConstraintsRHS;
    CnstLHS = Matrix(0,0);
    CnstRHS = Matrix(0,0);
    Q.vSetDiag(_bobotQ);
    R.vSetDiag(_bobotR);

    /*  Calculate prediction of z(k+1..k+Hp) constants
     *
     *      Prediction of state variable of the system:
     *        z(k+1..k+Hp) = (CPSI)*x(k) + (COMEGA)*u(k-1) + (CTHETA)*dU(k..k+Hu-1)         ...{MPC_1}
     *
     *        Constants:
     *          CPSI   = [CA C(A^2) ... C(A^Hp)]'                                       : (Hp*N)xN
     *          COMEGA = [CB C(B+A*B) ... C*Sigma(i=0->Hp-1)A^i*B]'                     : (Hp*N)xM
     *          CTHETA = [         CB                0  ....           0              ]
     *                   [       C(B+A*B)           CB   .             0              ]
     *                   [           .               .    .           CB              ] : (Hp*N)x(Hu*M)
     *                   [           .               .     .           .              ]
     *                   [C*Sigma(i=0->Hp-1)(A^i*B)  .  ....  C*Sigma(i=0->Hp-Hu)A^i*B]
     *
     */
    Matrix _Apow(SS_X_LEN, SS_X_LEN);
    /* CPSI     : [ C *   A  ]
     *            [ C *  A^2 ]
     *            [     .    ]                                                   : (Hp*N) x N
     *            [     .    ]
     *            [ C * A^Hp ]
     */
    _Apow = A;
    for (int16_t _i = 0; _i < MPC_HP_LEN; _i++) {
        CPSI = CPSI.InsertSubMatrix((C*_Apow), _i*SS_Z_LEN, 0);
        _Apow = _Apow * A;
    }

    /* COMEGA   : [          C * (B)         ]
     *            [        C * (B+A*B)       ]
     *            [             .            ]                                   : (Hp*N) x M
     *            [             .            ]
     *            [ C * Sigma(i=0->Hp-1)A^i*B]
     */
    Matrix _tempSigma(SS_X_LEN, SS_U_LEN);
    _Apow.vSetIdentity();
    _tempSigma = B;
    for (int16_t _i = 0; _i < MPC_HP_LEN; _i++) {
        COMEGA = COMEGA.InsertSubMatrix((C*_tempSigma), _i*SS_Z_LEN, 0);
        _Apow = _Apow * A;
        _tempSigma = _tempSigma + (_Apow*B);
    }

    /* CTHETA   : [          C * (B)              0         ....              0             ]
     *            [       C * (B+A*B)           C * (B)      .                0             ]
     *            [            .                  .           .             C * (B)         ]: (Hp*N)x(Hu*M)
     *            [            .                  .            .              .             ]
     *            [C * Sigma(i=0->Hp-1)A^i*B      .         ....  C * Sigma(i=0->Hp-Hu)A^i*B]
     *
     *          : [COMEGA   [0 COMEGA(0:(len(COMEGA)-len(B)),:)]'  ....  [0..0 COMEGA(0:(len(COMEGA)-((Hp-Hu)*len(B))),:)]']
     */
    for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) {
        CTHETA = CTHETA.InsertSubMatrix(COMEGA, _i*SS_Z_LEN, _i*SS_U_LEN, (MPC_HP_LEN*SS_Z_LEN)-(_i*SS_Z_LEN), SS_U_LEN);
    }

    /* Calculate offline optimization variable H:
     *  H = CTHETA'*Q*CTHETA + R                                                            ...{MPC_2}
     */
    H = ((CTHETA.Transpose()) * Q * CTHETA) + R;
}

bool MPC::bUpdate(Matrix &SP, Matrix &x, Matrix &u)
{
    Matrix Err((MPC_HP_LEN*SS_Z_LEN), 1, Matrix::NoInitMatZero);
    Matrix G((MPC_HU_LEN*SS_U_LEN), 1, Matrix::NoInitMatZero);

    /* Formulation of plant error prediction:
     *  E(k) = SP(k) - CPSI*x(k) - COMEGA*u(k-1)                                            ...{MPC_3}
     */
    Err = SP - CPSI*x - COMEGA*u;

    /* Calculate online optimization variable G:
     *  G = 2*CTHETA'*Q*E(k)                                                                ...{MPC_4}
     */
    G = 2.0 * (CTHETA.Transpose()) * Q * Err;

    /*  Create the inequality constraints                                                   ...{MPC_5}
     *          dUmin <= dU(k) <= dUmax                                                     ...{MPC_5a}
     *          Umin <= U(kj <= Umax                                                        ...{MPC_5b}
     *          Zmin <= Z(kj <= Zmax                                                        ...{MPC_5c}
     */
    vCreateConstraintsLHS(CnstLHS, CTHETA);
    vCreateConstraintsRHS(CnstRHS, COMEGA, CPSI, u, x);

    #if (0)
        /* Experiment: disable constraints, the MPC will behave like unconstrained MPC */
        #warning("Contstraints bypassed (no constraints)");
        CnstLHS = Matrix(0,0);
        CnstRHS = Matrix(0,0);
    #endif

    /*  Formulation of the optimal control problem:
     *
     *      For constrained MPC (quadrating programming):
     *          min   dU(k)'*H*dU(k) - G'*dU(k)     ; subject to inequality constraints
     *             dU(k)
     *
     *      Reconditioning H & G matrices into a standard QP form:
     *          x_opt = arg Min  1/2*x'*Q*x + c'*x  ; subject to ineqLHS*x <= ineqRHS
     *                     x
     *
     *      Then solve by Active Set:
     *          dU_opt(k) = ActiveSet(2H, -G, ineqLHS, ineqRHS)                             ...{MPC_6}
     *
     *
     * 1/2*Q = H    --> Q = 2*H
     * c' = -G'     --> c = -G
     */
    if (!bActiveSet(DU, 2*H, -G, CnstLHS, CnstRHS, cntIterActiveSet)) {
        /* return false; */
        DU.vSetToZero();
        return false;
    }

    /* Integrate the du(k) to get u(k):
     *  u(k) = u(k-1) + du(k)                                                               ...{MPC_7}
     */
    Matrix DU_Out(SS_U_LEN, 1);
    for (int16_t _i = 0; _i < SS_U_LEN; _i++) {
        DU_Out[_i][0] = DU[_i][0];
    }
    u = u + DU_Out;

    return true;
}

/* Active Set solver for Quadratic Programming problem in the form:
 *
 *      x_opt = arg Min  1/2*x'*Q*x + c'*x    ; subject to ineqLHS*x <= ineqRHS
 *                 x
 *
 *
 * The Active Set: search x, by solving this minimization problem:
 *
 *  min. 1/2*dx'*Q*dx + (Q*x+c)'*dx   , subject to ineqLHS_dx*x = 0
 *   dx
 *
 *  Integrate x(iter+1) = x(iter) + dx(iter)
 *
 *  Until KKT conditions is satisfied:
 *      1. Q*dx + ineqLHS_dx*(dLambda) = -(Q*x+c)   (dLambda = Lagrange multiplier of above minimization solution)
 *      2. -ineqLHS_dx*dx = 0
 *
 */
bool MPC::bActiveSet(Matrix &x, const Matrix &Q, const Matrix &c, const Matrix &ineqLHS, const Matrix &ineqRHS, int16_t &_i16iterActiveSet)
{
    bool _flagConstActive[ineqRHS.i16getRow()];    /* Contains information about which inequality is active (if true, then that inequality is active). */
    bool _dlambdaPos, _dxNol;
    Matrix dx(x.i16getRow(), 1, Matrix::NoInitMatZero);

    /* TODO: Make sure x initial value is inside feasible region (e.g. using Linear Programming).
     *       For now we set it to zero --> no mathematical guarantee!
     */
    x.vSetToZero();
    /* In the beginning, every constraints is non-active */
    for (int16_t _i = 0; _i < ineqRHS.i16getRow(); _i++) {
        _flagConstActive[_i] = false;
    }

    _i16iterActiveSet = 0;  /* Reset counter iteration Active Set */
    do {
        /* Construct active set matrix Aw (_ineqActiveLHS) */
        uint16_t _u16cntConstActive = 0;
        for (int16_t _i = 0; _i < ineqLHS.i16getRow(); _i++) {
            if (_flagConstActive[_i] == true) {
                _u16cntConstActive++;
            }
        }

        /* Construct Active Set solution matrix (the KKT matrix):
         *
         *  _KKT_LHS(k) * [  dx   ] = _KKT_RHS(k)
         *                [dLambda]
         *
         *  _KKT_LHS(k) = [        Q           _ineqActiveLHS(k)']
         *                [_ineqActiveLHS(k)           0         ]
         *
         *  _KKT_RHS(k) = [-(Q*x+c)]
         *                [    0   ]
         */
        int16_t _indexConstActive[_u16cntConstActive];                                          /* Contains information about index of the active inequality constraints in ineqLHS */
        Matrix _ineqActiveLHS(_u16cntConstActive, ineqLHS.i16getCol(), Matrix::NoInitMatZero);  /* Aggregation of the row vectors of ineqLHS that is 'active', i.e. the Aw matrix */

        for (int16_t _i = 0, _iterConstActive = 0; _i < ineqLHS.i16getRow(); _i++) {
            if (_flagConstActive[_i] == true) {
                _ineqActiveLHS = _ineqActiveLHS.InsertSubMatrix(ineqLHS, _iterConstActive, 0, _i, 0, 1, ineqLHS.i16getCol());
                _indexConstActive[_iterConstActive] = _i;

                _iterConstActive++;
                ASSERT((_iterConstActive <= _u16cntConstActive), "Bug on the active set: Create _ineqActiveLHS");
            }
        }
        Matrix _KKT_LHS((Q.i16getRow()+_u16cntConstActive), (Q.i16getCol()+_u16cntConstActive));
        Matrix _KKT_RHS((Q.i16getRow()+_u16cntConstActive), 1);

        _KKT_LHS = _KKT_LHS.InsertSubMatrix(Q, 0, 0);
        _KKT_LHS = _KKT_LHS.InsertSubMatrix(_ineqActiveLHS, Q.i16getRow(), 0);
        _KKT_LHS = _KKT_LHS.InsertSubMatrix(_ineqActiveLHS.Transpose(), 0, Q.i16getCol());
        _KKT_RHS = _KKT_RHS.InsertSubMatrix(-((Q*x)+c), 0, 0);

        /* [   dx  ] = [        Q           _ineqActiveLHS(k)']^-1 * [-(Q*x+c)]
         * [dLambda]   [_ineqActiveLHS(k)           0         ]      [    0   ]
         *
         */
        Matrix _KKTvector(_KKT_LHS.Invers()*_KKT_RHS);
        if (!_KKTvector.bMatrixIsValid()) {
            return false;
        }
        dx = dx.InsertSubMatrix(_KKTvector, 0, 0, x.i16getRow(), 1);

        Matrix dLambda(_u16cntConstActive, 1, Matrix::NoInitMatZero);
        dLambda = dLambda.InsertSubMatrix(_KKTvector, 0, 0, x.i16getRow(), 0, _u16cntConstActive, 1);

        /* Check for Karush–Kuhn–Tucker conditions ------------------------------------------------------------------------------------------------ */
        /* Karush–Kuhn–Tucker conditions:
         *  1. dx == 0
         *  2. dLambda >= 0
         */
        /* Search for dx == 0 ----------------- */
        _dxNol = true;
        for (int16_t _i = 0; _i < Q.i16getRow(); _i++) {
            if (fabs(dx[_i][0]) > float_prec(float_prec_ZERO_ECO)) {
                _dxNol = false;
                break;
            }
        }
        /* Search for dLambda >= 0 ------------ */
        float_prec _lowestLambda = 0.0;
        int16_t _idxIneqLowestLambda = -1;
        _dlambdaPos = true;
        for (int16_t _i = 0; _i < _u16cntConstActive; _i++) {
            if (dLambda[_i][0] < _lowestLambda) {
                /* Some constraints become inactive, search constraint with lowest Lagrange multiplier so we can remove it */
                _lowestLambda = dLambda[_i][0];
                _idxIneqLowestLambda = _indexConstActive[_i];
                _dlambdaPos = false;
            }
        }
        /* Check it! -------------------------- */
        if (_dxNol && _dlambdaPos) {
            break;
        }

        if (!_dlambdaPos) {
            /* There is a constraint that become inactive ----------------------------------------------------------------------------------------- */

            /* remove the constraint that have lowest Lagrange multiplier */
            ASSERT((_idxIneqLowestLambda != -1), "Bug on the active set: remove ineq active");
            _flagConstActive[_idxIneqLowestLambda] = false;
        }

        if (!_dxNol) {
            /* It means the x(k+1) will move ------------------------------------------------------------------------------------------------------ */

            /* Search for violated constraints. If any, then get the lowest scalar value _alpha that makes
             *  the searching direction feasible again
             */
            float_prec _alphaMin = 1.0;
            int16_t _idxAlphaMin = -1;

            Matrix _ineqNonActiveLHS_dx(ineqLHS * dx);
            Matrix _ineqNonActiveLHS_x(ineqLHS * x);

            for (int16_t _i = 0; _i < ineqLHS.i16getRow(); _i++) {
                if (_flagConstActive[_i] == false) {
                    /* For all constraints that is not active, we check if that constraint is violated */
                    if (_ineqNonActiveLHS_dx[_i][0] > float_prec_ZERO_ECO) {
                        /* We have a violated constraint. Make that constraint a candidate of active constraint */
                        float_prec _alphaCandidate = - (_ineqNonActiveLHS_x[_i][0] - ineqRHS[_i][0]) / _ineqNonActiveLHS_dx[_i][0];

                        if (_alphaCandidate < _alphaMin) {
                            _alphaMin = _alphaCandidate;
                            _idxAlphaMin = _i;
                        }
                    }
                }
            }
            if (_idxAlphaMin != -1) {
                _flagConstActive[_idxAlphaMin] = true;
            }

            /* Should we do ASSERT(_alphaMin > 0.0) here? */
            if (_alphaMin > 0) {
                x = x + (dx * _alphaMin);
            }
        }

        /* Check for maximum iteration conditions ------------------------------------------------------------------------------------------------- */
        _i16iterActiveSet++;
        if (_i16iterActiveSet > MPC_MAXIMUM_ACTIVE_SET_ITERATION) {
            /* Maximum number of iteration reached, terminate and use last solution */
            break;
        }
    } while (1);

    return true;
}


text:
/**************************************************************************************************
 * Class for MPC with constraint.
 *
 *
 * See https://github.com/pronenewbits for more!
 *************************************************************************************************/
#ifndef MPC_H
#define MPC_H

#include "konfig.h"
#include "matrix.h"

#if (MPC_HP_LEN < MPC_HU_LEN)
    #error("The MPC_HP_LEN must be more than or equal MPC_HU_LEN!");
#endif
#if (((MPC_HP_LEN*SS_Z_LEN) > MATRIX_MAXIMUM_SIZE) || ((MPC_HU_LEN*SS_U_LEN) > MATRIX_MAXIMUM_SIZE))
    #error("The MATRIX_MAXIMUM_SIZE is too small to do MPC calculation!");
#endif

#define MPC_MAXIMUM_ACTIVE_SET_ITERATION    (50)

#if defined(MPC_USE_CONSTRAINT_DU)
    #define INEQ_LEN_DU     (2*(MPC_HU_LEN*SS_U_LEN))       /* size DU = (MPC_HU_LEN*SS_U_LEN), times 2 for DU_MIN & DU_MAX */
#else
    #define INEQ_LEN_DU     (0)
#endif
#if defined(MPC_USE_CONSTRAINT_U)
    #define INEQ_LEN_U      (2*(MPC_HU_LEN*SS_U_LEN))       /* size U  = (MPC_HU_LEN*SS_U_LEN), times 2 for U_MIN & U_MAX */
#else
    #define INEQ_LEN_U      (0)
#endif
#if defined(MPC_USE_CONSTRAINT_Z)
    #define INEQ_LEN_Z      (2*(MPC_HP_LEN*SS_Z_LEN))       /* size Z  = (MPC_HP_LEN*SS_Z_LEN), times 2 for Z_MIN & Z_MAX */
#else
    #define INEQ_LEN_Z      (0)
#endif
#if (((MPC_HP_LEN*SS_Z_LEN) + INEQ_LEN_DU + INEQ_LEN_U + INEQ_LEN_Z) > MATRIX_MAXIMUM_SIZE)
    /* Worst case for KKT Matrix size: Q + Cnst_DU + Cnst_U + Cnst_Z */
//     #error("The MATRIX_MAXIMUM_SIZE is too small to do MPC calculation!");
// WARNING! We disable this because Teensy has too little RAM, you __HAVE__ to enable this for mission critical implementation.
#endif
#if ((INEQ_LEN_DU + INEQ_LEN_U + INEQ_LEN_Z) > MATRIX_MAXIMUM_SIZE)
    /* Worst case for KKT Matrix size exploiting Schur complement: Cnst_DU + Cnst_U + Cnst_Z */
    #error("The MATRIX_MAXIMUM_SIZE is too small to do MPC calculation!");
#endif

class MPC
{
public:
    MPC(Matrix &A, Matrix &B, Matrix &C, float_prec _bobotQ, float_prec _bobotR,
        void (*vCreateConstraintsLHS)(Matrix &, Matrix &),
        void (*vCreateConstraintsRHS)(Matrix &, Matrix &, Matrix &, Matrix &, Matrix &));
    void vReInit(Matrix &A, Matrix &B, Matrix &C, float_prec _bobotQ, float_prec _bobotR,
                 void (*vCreateConstraintsLHS)(Matrix &, Matrix &),
                 void (*vCreateConstraintsRHS)(Matrix &, Matrix &, Matrix &, Matrix &, Matrix &));
    bool bUpdate(Matrix &SP, Matrix &x, Matrix &u);

    /* for analyzing purpose. If cntIterActiveSet = MPC_MAXIMUM_ACTIVE_SET_ITERATION, the solution might not be optimal (although feasible) */
    int16_t cntIterActiveSet;

protected:
    bool bActiveSet(Matrix &x, const Matrix &Q, const Matrix &c, const Matrix &ineqLHS, const Matrix &ineqRHS, int16_t &_i16iterActiveSet);
    void (*vCreateConstraintsLHS)(Matrix &CnstLHS, Matrix &CTHETA);
    void (*vCreateConstraintsRHS)(Matrix &CnstRHS, Matrix &COMEGA, Matrix &CPSI, Matrix &u_prev, Matrix &x);

private:
    Matrix CPSI     {(MPC_HP_LEN*SS_Z_LEN), SS_X_LEN};
    Matrix COMEGA   {(MPC_HP_LEN*SS_Z_LEN), SS_U_LEN};
    Matrix CTHETA   {(MPC_HP_LEN*SS_Z_LEN), (MPC_HU_LEN*SS_U_LEN)};

    Matrix DU       {(MPC_HU_LEN*SS_U_LEN), 1};

    Matrix A        {SS_X_LEN, SS_X_LEN};
    Matrix B        {SS_X_LEN, SS_U_LEN};
    Matrix C        {SS_Z_LEN, SS_X_LEN};

    Matrix Q        {(MPC_HP_LEN*SS_Z_LEN), (MPC_HP_LEN*SS_Z_LEN)};
    Matrix R        {(MPC_HU_LEN*SS_U_LEN), (MPC_HU_LEN*SS_U_LEN)};

    Matrix H        {(MPC_HU_LEN*SS_U_LEN), (MPC_HU_LEN*SS_U_LEN)};

    Matrix CnstLHS	{0, 0};
    Matrix CnstRHS	{0, 0};
};

#endif // MPC_H

mime_type: text/x-tex


text:
/************************************************************************************
 * Class Matrix
 *  See matrix.h for description
 *
 * See https://github.com/pronenewbits for more!
 *************************************************************************************/

#include "matrix.h"

/* There's nothing here yet! */


text:
/*************************************************************************************************************
 * Matrix Class
 *  Contain the matrix class definition and operation.
 *
 *  Notes:
 *    a. The matrix data is a 2 dimensional array, with structure:
 *      ->  0 <= i16row <= MATRIX_MAXIMUM_SIZE
 *      ->  0 <= i16col <= MATRIX_MAXIMUM_SIZE
 *      ->  floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory
 *           representation of the matrix. We only use the first i16row-th
 *           and first i16col-th memory for the matrix data. The rest is unused.
 *
 *    b. Indexing start from 0, there are 3 ways to access the data:
 *          1. A(idxRow,idxCol)       <-- The preferred way. With bounds checking.
 *          2. A[idxRow][idxCol]      <-- Slow, not recommended, but makes a cute code. With bounds checking.
 *          3. A._at(idxRow,idxCol)   <-- Just for internal function usage. Without bounds checking.
 *
 *       Accessing index start from 0 until i16row/i16col, that is:
 *          (0 <= idxRow < i16row)     and     (0 <= idxRow < i16col).
 *
 *    See below at "Data structure of Matrix class" at private member class definition for more information!
 *
 *
 * Class Matrix Versioning:
 *    v0.10 (2020-04-29), {PNb}:
 *      - Add NoInitMatZero for _outp matrix initialization in ForwardSubtitution.
 *      - Fixing bug in HouseholderTransformQR function where _outp & _vectTemp need to be initialized
 *          with zero.
 *      - Change comparison with zero in these functions using float_prec_ZERO_ECO:
 *          -> operator ==
 *          -> operator /
 *          -> vRoundingElementToZero
 *          -> RoundingMatrixToZero
 *      - No need to check fabs(_normM) as non-zero in bNormVector().
 *      - Add operator != (checking with float_prec_ZERO_ECO).
 *      - Add MatIdentity() function.
 *
 *    v0.9 (2020-04-28), {PNb}:
 *      - Set all function as inline function (insert oprah meme here).
 *      - Rework matrix library to increase readability.
 *      - Make every parameters and function as reference as much as possible.
 *      - Make every member function as a const function as possible.
 *      - Add () operator overload to access the matrix data, so we can do both:
 *         -> A[0][3] = access 1st row, 4th column of matrix data.
 *         -> A(2,0)  = access 3rd row, 1st column of matrix data. < this is the way.
 *       - Remove (bug) a remnant of ancient code where we haven't implemented NoInitMatZero:
 *          float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] = {{0}};
 *         Into:
 *          float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE];
 *         We can get very nice speed up (MPC benchmark @2020-04-27) from 698 us to 414 us!
 *       - Implement copy constructor               (more sweet speed up, 414 us -> 382 us!).
 *       - Implement assignment operator       (more more sweet speed up, 382 us -> 327 us!).
 *       - Remove Copy() function, use assignment operator instead.
 *       - Change MATRIX_USE_BOUND_CHECKING -> MATRIX_USE_BOUNDS_CHECKING (see ...BOUND*S*_CH...).
 *
 *    v0.8 (2020-03-26), {PNb}:
 *      - Change indexing from int32_t to int16_t.
 *      - Add way to initialize matrix with existing float_prec array.
 *      - Add enum InitZero.
 *      - Make temporary matrix initialization inside almost all method with
 *          NoInitMatZero argument.
 *      - Remove the 1 index buffer reserve in bMatrixIsValid function.
 *      - Add bMatrixIsPositiveDefinite method to check the positive
 *          (semi)definiteness of a matrix.
 *      - Add GetDiagonalEntries method.
 *      - Change SYSTEM_IMPLEMENTATION_EMBEDDED_NO_PRINT into
 *          SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM, and make vPrint and
 *          vPrintFull as function declaration (the user must define that
 *          function somewhere).
 *
 *    v0.7 (2020-02-23), {PNb}:
 *      - Make the matrix class interface in English (at long last, yay?).
 *
 *
 *** Documentation below is for tracking purpose *************************************
 *
 *    v0.6 (2020-01-16), {PNb}:
 *      - Tambahkan sanity check saat pengecekan MATRIX_PAKAI_BOUND_CHECKING
 *          dengan membandingkan baris & kolom dengan MATRIX_MAXIMUM_SIZE.
 *      - Menambahkan pengecekan matrix untuk operasi dasar antar matrix (*,+,-).
 *
 *    v0.5 (2020-01-14), {PNb}:
 *      - Buat file matrix.cpp (akhirnya!) untuk definisi fungsi di luar class.
 *      - Tambahkan operator overloading untuk operasi negatif matrix (mis. a = -b).
 *      - Tambahkan operator overloading untuk operasi penjumlahan & pengurangan
 *          dengan scalar.
 *      - Ubah evaluasi MATRIX_PAKAI_BOUND_CHECKING menggunakan ASSERT.
 *      - Tambahkan pengecekan index selalu positif di MATRIX_PAKAI_BOUND_CHECKING.
 *
 *    v0.4 (2020-01-10), {PNb}:
 *      - Tambahkan rounding to zero sebelum operasi sqrt(x) untuk menghindari
 *          kasus x = 0-
 *      - Fungsi QRDec mengembalikan Q' dan R (user perlu melakukan transpose
 *          lagi setelah memanggil QRDec untuk mendapatkan Q).
 *      - Menambahkan pengecekan hasil HouseholderTransformQR di dalam QRDec.
 *      - Tambah warning jika MATRIX_PAKAI_BOUND_CHECKING dinonaktifkan.
 *
 *    v0.3_engl (2019-12-31), {PNb}:
 *      - Modifikasi dokumentasi kode buat orang asing.
 *
 *    v0.3 (2019-12-25), {PNb}:
 *      - Menambahkan fungsi back subtitution untuk menyelesaikan permasalahan
 *          persamaan linear Ax = B. Dengan A matrix segitiga atas & B vektor.
 *      - Memperbaiki bug pengecekan MATRIX_PAKAI_BOUND_CHECKING pada indexing kolom.
 *      - Menambahkan fungsi QR Decomposition (via Householder Transformation).
 *      - Menambahkan fungsi Householder Transformation.
 *      - Menghilangkan warning 'implicit conversion' untuk operasi pembandingan
 *          dengan float_prec_ZERO.
 *      - Menambahkan function overloading operasi InsertSubMatrix, untuk
 *          operasi insert dari SubMatrix ke SubMatrix.
 *      - Saat inisialisasi, matrix diisi nol (melalui vIsiHomogen(0.0)).
 *      - Menambahkan function overloading operator '/' dengan scalar.
 *
 *    v0.2 (2019-11-30), {PNb}:
 *      - Fungsi yang disupport:
 *          - Operator ==
 *          - Normalisasi matrix
 *          - Cholesky Decomposition
 *          - InsertSubMatrix
 *          - InsertVector
 *
 *    v0.1 (2019-11-29), {PNb}:
 *      - Fungsi yang disupport:
 *          - Operasi matrix dasar
 *          - Invers
 *          - Cetak
 *
 * See https://github.com/pronenewbits for more!
 ************************************************************************************************************/
#ifndef MATRIX_H
#define MATRIX_H

#include "konfig.h"

#if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
    #include <iostream>
    #include <iomanip>      // std::setprecision

    using namespace std;
#elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
    #include <Wire.h>
#endif

class Matrix
{
public:
    typedef enum {
        InitMatWithZero,    /* Initialize matrix with zero data */
        NoInitMatZero
    } InitZero;

    /* --------------------------------------------- Basic Matrix Class functions --------------------------------------------- */
    /* Init empty matrix size _i16row x _i16col */
    Matrix(const int16_t _i16row, const int16_t _i16col, const InitZero _init = InitMatWithZero);
    /* Init matrix size _i16row x _i16col with entries initData */
    Matrix(const int16_t _i16row, const int16_t _i16col, const float_prec* initData, const InitZero _init = InitMatWithZero);
    /* Copy constructor (for this operation --> A(B)) (copy B into A) */
    Matrix(const Matrix& old_obj);
    /* Assignment operator (for this operation --> A = B) (copy B into A) */
    Matrix& operator = (const Matrix& obj);
    /* Destructor */
    ~Matrix(void);
    /* Get internal state */
    inline int16_t i16getRow(void) const { return this->i16row; }
    inline int16_t i16getCol(void) const { return this->i16col; }

    /* ------------------------------------------- Matrix entry accessing functions ------------------------------------------- */
    /* For example: A(1,2) access the 1st row and 2nd column data of matrix A <--- The preferred way to access the matrix */
    float_prec& operator () (const int16_t _row, const int16_t _col);
    float_prec operator () (const int16_t _row, const int16_t _col) const;

    /* For example: A[1][2] access the 1st row and 2nd column data of matrix A <-- The awesome way */
    class Proxy {
        public:
            Proxy(float_prec* _inpArr, const int16_t _maxCol) { _array.ptr = _inpArr; this->_maxCol = _maxCol; }
            Proxy(const float_prec* _inpArr, const int16_t _maxCol) { _array.cptr = _inpArr; this->_maxCol = _maxCol; }
            float_prec & operator [] (const int16_t _col);
            float_prec operator [] (const int16_t _col) const;
        private:
            union { /* teehee xp */
                const float_prec* cptr;
                float_prec* ptr;
            } _array;
            int16_t _maxCol;
    };
    Proxy operator [] (const int16_t _row);
    const Proxy operator [] (const int16_t _row) const;

    /* ----------------------------------------- Matrix checking function declaration ----------------------------------------- */
    bool bMatrixIsValid(void);
    void vSetMatrixInvalid(void);
    bool bMatrixIsSquare();
    /* --------------------------------------------- Matrix elementary operations --------------------------------------------- */
    bool operator == (const Matrix& _compare) const;
    bool operator != (const Matrix& _compare) const;
    Matrix operator - (void) const;
    Matrix operator + (const float_prec _scalar) const;
    Matrix operator - (const float_prec _scalar) const;
    Matrix operator * (const float_prec _scalar) const;
    Matrix operator / (const float_prec _scalar) const;
    Matrix operator + (const Matrix& _matAdd) const;
    Matrix operator - (const Matrix& _matSub) const;
    Matrix operator * (const Matrix& _matMul) const;
    /* Declared outside class below */
    /* inline Matrix operator + (const float_prec _scalar, Matrix _mat); */
    /* inline Matrix operator - (const float_prec _scalar, Matrix _mat); */
    /* inline Matrix operator * (const float_prec _scalar, Matrix _mat); */
    /* ----------------------------------------------- Simple Matrix operations ----------------------------------------------- */
    void vRoundingElementToZero(const int16_t _i, const int16_t _j);
    Matrix RoundingMatrixToZero(void);
    void vSetHomogen(const float_prec _val);
    void vSetToZero(void);
    void vSetRandom(const int32_t _maxRand, const int32_t _minRand);
    void vSetDiag(const float_prec _val);
    void vSetIdentity(void);
    Matrix Transpose(void);
    bool bNormVector(void);
    /* ------------------------------------------ Matrix/Vector insertion operations ------------------------------------------ */
    Matrix InsertVector(const Matrix& _Vector, const int16_t _posCol);
    Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol);
    Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                           const int16_t _lenRow, const int16_t _lenColumn);
    Matrix InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                           const int16_t _posRowSub, const int16_t _posColSub,
                           const int16_t _lenRow, const int16_t _lenColumn);
    /* ---------------------------------------------------- Big operations ---------------------------------------------------- */
    /* Matrix invertion using Gauss-Jordan algorithm */
    Matrix Invers(void) const;
    /* Check the definiteness of a matrix */
    bool bMatrixIsPositiveDefinite(const bool checkPosSemidefinite = false) const;
    /* Return the vector (Mx1 matrix) correspond with the diagonal entries of 'this' */
    Matrix GetDiagonalEntries(void) const;
    /* Do the Cholesky Decomposition using Cholesky-Crout algorithm, return 'L' matrix */
    Matrix CholeskyDec(void) const;
    /* Do Householder Transformation for QR Decomposition operation */
    Matrix HouseholderTransformQR(const int16_t _rowTransform, const int16_t _colTransform);
    /* Do QR Decomposition for matrix using Householder Transformation */
    bool QRDec(Matrix& Qt, Matrix& R) const;
    /* Do back-subtitution for upper triangular matrix A & column matrix B:
     * x = BackSubtitution(&A, &B)          ; for Ax = B
     */
    Matrix BackSubtitution(const Matrix& A, const Matrix& B) const;
    /* Do forward-subtitution for lower triangular matrix A & column matrix B:
     * x = ForwardSubtitution(&A, &B)       ; for Ax = B
     */
    Matrix ForwardSubtitution(const Matrix& A, const Matrix& B) const;
    /* ----------------------------------------------- Matrix printing function ----------------------------------------------- */
    void vPrint(void);
    void vPrintFull(void);

private:
    /* Data structure of Matrix class:
     *  0 <= i16row <= MATRIX_MAXIMUM_SIZE      ; i16row is the row of the matrix. i16row is invalid if (i16row == -1)
     *  0 <= i16col <= MATRIX_MAXIMUM_SIZE      ; i16col is the column of the matrix. i16col is invalid if (i16col == -1)
     *
     * Accessing index start from 0 until i16row/i16col, that is:
     *  (0 <= idxRow < i16row)     and     (0 <= idxCol < i16col).
     * There are 3 ways to access the data:
     *  1. A[idxRow][idxCol]          <-- Slow, not recommended, but make a cute code. With bounds checking.
     *  2. A(idxRow, idxCol)          <-- The preferred way. With bounds checking.
     *  3. A._at(idxRow, idxCol)      <-- Just for internal function usage. Without bounds checking.
     *
     * floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE] is the memory representation of the matrix. We only use the
     *  first i16row-th and first i16col-th memory for the matrix data. The rest is unused.
     *
     * This configuration might seems wasteful (yes it is). But with this, we can make the matrix library code as cleanly
     *  as possible (like I said in the github page, I've made decision to sacrifice speed & performance to get best code
     *  readability I could get).
     *
     * You could change the data structure of floatData if you want to make the implementation more memory efficient.
     */
    int16_t i16row;
    int16_t i16col;
    float_prec floatData[MATRIX_MAXIMUM_SIZE][MATRIX_MAXIMUM_SIZE];

    /* Private way to access floatData without bound checking.
     *  TODO: For Matrix member function we could do the bound checking once at the beginning of the function, and use this
     *          to access the floatData instead of (i,j) operator. From preliminary experiment doing this only on elementary
     *          operation (experiment @2020-04-27), we can get up to 45% computation boost!!! (MPC benchmark 414 us -> 226 us)!
     */
    float_prec& _at(const int16_t _row, const int16_t _col) { return this->floatData[_row][_col]; }
    float_prec _at(const int16_t _row, const int16_t _col) const { return this->floatData[_row][_col]; }
};

inline Matrix operator + (const float_prec _scalar, const Matrix& _mat);
inline Matrix operator - (const float_prec _scalar, const Matrix& _mat);
inline Matrix operator * (const float_prec _scalar, const Matrix& _mat);
inline Matrix MatIdentity(const int16_t _i16size);

/* ================================================= inline definition below: ================================================= */
/* ================================================= inline definition below: ================================================= */

/* ------------------------------------------ Basic Matrix Class functions ------------------------------------------ */
/* ------------------------------------------ Basic Matrix Class functions ------------------------------------------ */

inline Matrix::Matrix(const int16_t _i16row, const int16_t _i16col, const InitZero _init) {
    this->i16row = _i16row;
    this->i16col = _i16col;

    if (_init == InitMatWithZero) {
        this->vSetHomogen(0.0);
    }
}

inline Matrix::Matrix(const int16_t _i16row, const int16_t _i16col, const float_prec* initData, const InitZero _init) {
    this->i16row = _i16row;
    this->i16col = _i16col;

    if (_init == InitMatWithZero) {
        this->vSetHomogen(0.0);
    }
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) = *initData;
            initData++;
        }
    }
}

inline Matrix::Matrix(const Matrix& old_obj) {
    /* For copy contructor, we only need to copy (_i16row x _i16col) submatrix, there's no need to copy all data */
    this->i16row = old_obj.i16row;
    this->i16col = old_obj.i16col;

    const float_prec *sourc = old_obj.floatData[0];
    float_prec *desti = this->floatData[0];

    for (int16_t _i = 0; _i < i16row; _i++) {
        /* Still valid with invalid matrix ((i16row == -1) or (i16col == -1)) */
        memcpy(desti, sourc, sizeof(float_prec)*size_t((this->i16col)));
        sourc += (MATRIX_MAXIMUM_SIZE);
        desti += (MATRIX_MAXIMUM_SIZE);
    }
}

inline Matrix& Matrix::operator = (const Matrix& obj) {
    /* For assignment operator, we only need to copy (_i16row x _i16col) submatrix, there's no need to copy all data */
    this->i16row = obj.i16row;
    this->i16col = obj.i16col;

    const float_prec *sourc = obj.floatData[0];
    float_prec *desti = this->floatData[0];

    for (int16_t _i = 0; _i < i16row; _i++) {
        /* Still valid with invalid matrix ((i16row == -1) or (i16col == -1)) */
        memcpy(desti, sourc, sizeof(float_prec)*size_t((this->i16col)));
        sourc += (MATRIX_MAXIMUM_SIZE);
        desti += (MATRIX_MAXIMUM_SIZE);
    }

    return *this;
}

inline Matrix::~Matrix(void) {
    /* haven't implemented anything here, definition needed for the rule of three */
    /* (´・ω・`) */
}

/* ---------------------------------------- Matrix entry accessing functions ---------------------------------------- */
/* ---------------------------------------- Matrix entry accessing functions ---------------------------------------- */

/* The preferred method to access the matrix data (boring code) */
inline float_prec& Matrix::operator () (const int16_t _row, const int16_t _col) {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
        ASSERT((_col >= 0) && (_col < this->i16col) && (_col < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at column _column)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return this->floatData[_row][_col];
}
inline float_prec Matrix::operator () (const int16_t _row, const int16_t _col) const {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
        ASSERT((_col >= 0) && (_col < this->i16col) && (_col < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at column _column)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return this->floatData[_row][_col];
}

/* Ref: https://stackoverflow.com/questions/6969881/operator-overload
 * Modified to be lvalue modifiable (I know this is so dirty, but it makes the code so FABULOUS XD)
 */
inline float_prec & Matrix::Proxy::operator [] (const int16_t _col) {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_col >= 0) && (_col < this->_maxCol) && (_col < MATRIX_MAXIMUM_SIZE),
                "Matrix index out-of-bounds (at column evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return _array.ptr[_col];
}
inline float_prec Matrix::Proxy::operator [] (const int16_t _col) const {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_col >= 0) && (_col < this->_maxCol) && (_col < MATRIX_MAXIMUM_SIZE),
                "Matrix index out-of-bounds (at column evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return _array.cptr[_col];
}
inline Matrix::Proxy Matrix::operator [] (const int16_t _row) {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return Proxy(floatData[_row], this->i16col);  /* Parsing column index for bound checking */
}
inline const Matrix::Proxy Matrix::operator [] (const int16_t _row) const {
    #if (defined(MATRIX_USE_BOUNDS_CHECKING))
        ASSERT((_row >= 0) && (_row < this->i16row) && (_row < MATRIX_MAXIMUM_SIZE),
               "Matrix index out-of-bounds (at row evaluation)");
    #else
        #warning("Matrix bounds checking is disabled... good luck >:3");
    #endif
    return Proxy(floatData[_row], this->i16col);  /* Parsing column index for bound checking */
}

/* -------------------------------------- Matrix checking function declaration -------------------------------------- */
/* -------------------------------------- Matrix checking function declaration -------------------------------------- */

inline bool Matrix::bMatrixIsValid(void) {
    /* Check whether the matrix is valid or not */
    if ((this->i16row > 0) && (this->i16row <= MATRIX_MAXIMUM_SIZE) &&
        (this->i16col > 0) && (this->i16col <= MATRIX_MAXIMUM_SIZE))
    {
        return true;
    } else {
        return false;
    }
}

inline void Matrix::vSetMatrixInvalid(void) {
    this->i16row = -1;
    this->i16col = -1;
}

inline bool Matrix::bMatrixIsSquare(void) {
    return (this->i16row == this->i16col);
}

/* ------------------------------------------ Matrix elementary operations ------------------------------------------ */
/* ------------------------------------------ Matrix elementary operations ------------------------------------------ */
/* TODO: We could do loop unrolling here for elementary, simple, and matrix insertion operations. It *might* speed up
 *        the computation time up to 20-30% for processor with FMAC and cached CPU (I still mull on this because
 *        the code will be awful).
 */

inline bool Matrix::operator == (const Matrix& _compare) const {
    if ((this->i16row != _compare.i16row) || (this->i16col != _compare.i16col)) {
        return false;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            if (fabs((*this)(_i,_j) - _compare(_i,_j)) > float_prec(float_prec_ZERO_ECO)) {
                return false;
            }
        }
    }
    return true;
}

inline bool Matrix::operator != (const Matrix& _compare) const {
    return (!(*this == _compare));
}

inline Matrix Matrix::operator - (void) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = -(*this)(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator + (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) + _scalar;
        }
    }
    return _outp;
}

inline Matrix Matrix::operator - (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) - _scalar;
        }
    }
    return _outp;
}

inline Matrix Matrix::operator * (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) * _scalar;
        }
    }
    return _outp;
}

inline Matrix Matrix::operator / (const float_prec _scalar) const {
    Matrix _outp(this->i16row, this->i16col, Matrix::NoInitMatZero);

    if (fabs(_scalar) < float_prec(float_prec_ZERO_ECO)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) / _scalar;
        }
    }
    return _outp;
}

inline Matrix operator + (const float_prec _scalar, const Matrix& _mat) {
    Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
            _outp(_i,_j) = _scalar + _mat(_i,_j);
        }
    }
    return _outp;
}

inline Matrix operator - (const float_prec _scalar, const Matrix& _mat) {
    Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
            _outp(_i,_j) = _scalar - _mat(_i,_j);
        }
    }
    return _outp;
}

inline Matrix operator * (const float_prec _scalar, const Matrix& _mat) {
    Matrix _outp(_mat.i16getRow(), _mat.i16getCol(), Matrix::NoInitMatZero);

    for (int16_t _i = 0; _i < _mat.i16getRow(); _i++) {
        for (int16_t _j = 0; _j < _mat.i16getCol(); _j++) {
            _outp(_i,_j) = _scalar * _mat(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator + (const Matrix& _matAdd) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
    if ((this->i16row != _matAdd.i16row) || (this->i16col != _matAdd.i16col)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) + _matAdd(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator - (const Matrix& _matSub) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
    if ((this->i16row != _matSub.i16row) || (this->i16col != _matSub.i16col)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_i,_j) = (*this)(_i,_j) - _matSub(_i,_j);
        }
    }
    return _outp;
}

inline Matrix Matrix::operator * (const Matrix& _matMul) const {
    Matrix _outp(this->i16row, _matMul.i16col, NoInitMatZero);
    if ((this->i16col != _matMul.i16row)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < _matMul.i16col; _j++) {
            _outp(_i,_j) = 0.0;
            for (int16_t _k = 0; _k < this->i16col; _k++) {
                _outp(_i,_j) += ((*this)(_i,_k) * _matMul(_k,_j));
            }
        }
    }
    return _outp;
}

/* -------------------------------------------- Simple Matrix operations -------------------------------------------- */
/* -------------------------------------------- Simple Matrix operations -------------------------------------------- */

inline void Matrix::vRoundingElementToZero(const int16_t _i, const int16_t _j) {
    if (fabs((*this)(_i,_j)) < float_prec(float_prec_ZERO_ECO)) {
        (*this)(_i,_j) = 0.0;
    }
}

inline Matrix Matrix::RoundingMatrixToZero(void) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            if (fabs((*this)(_i,_j)) < float_prec(float_prec_ZERO_ECO)) {
                (*this)(_i,_j) = 0.0;
            }
        }
    }
    return (*this);
}

inline void Matrix::vSetHomogen(const float_prec _val) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) = _val;
        }
    }
}

inline void Matrix::vSetToZero(void) {
    this->vSetHomogen(0.0);
}

inline void Matrix::vSetRandom(const int32_t _maxRand, const int32_t _minRand) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) = float_prec((rand() % (_maxRand - _minRand + 1)) + _minRand);
        }
    }
}

inline void Matrix::vSetDiag(const float_prec _val) {
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            if (_i == _j) {
                (*this)(_i,_j) = _val;
            } else {
                (*this)(_i,_j) = 0.0;
            }
        }
    }
}

inline void Matrix::vSetIdentity(void) {
    this->vSetDiag(1.0);
}

inline Matrix MatIdentity(const int16_t _i16size) {
    Matrix _outp(_i16size, _i16size, Matrix::NoInitMatZero);
    _outp.vSetDiag(1.0);
    return _outp;
}

/* Return the transpose of the matrix */
inline Matrix Matrix::Transpose(void) {
    Matrix _outp(this->i16col, this->i16row, NoInitMatZero);
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _outp(_j,_i) = (*this)(_i,_j);
        }
    }
    return _outp;
}

/* Normalize the vector */
inline bool Matrix::bNormVector(void) {
    float_prec _normM = 0.0;
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            _normM = _normM + ((*this)(_i,_j) * (*this)(_i,_j));
        }
    }

    /* Rounding to zero to avoid case where sqrt(0-), and _normM always positive */
    if (_normM < float_prec(float_prec_ZERO)) {
        return false;
    }
    _normM = sqrt(_normM);
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        for (int16_t _j = 0; _j < this->i16col; _j++) {
            (*this)(_i,_j) /= _normM;
        }
    }
    return true;
}

/* --------------------------------------- Matrix/Vector insertion operations --------------------------------------- */
/* --------------------------------------- Matrix/Vector insertion operations --------------------------------------- */

/* Insert vector into matrix at _posCol position
 * Example: A = Matrix 3x3, B = Vector 3x1
 *
 *  C = A.InsertVector(B, 1);
 *
 *  A = [A00  A01  A02]     B = [B00]
 *      [A10  A11  A12]         [B10]
 *      [A20  A21  A22]         [B20]
 *
 *  C = [A00  B00  A02]
 *      [A10  B10  A12]
 *      [A20  B20  A22]
 */
inline Matrix Matrix::InsertVector(const Matrix& _Vector, const int16_t _posCol) {
    Matrix _outp(*this);
    if ((_Vector.i16row > this->i16row) || (_Vector.i16col+_posCol > this->i16col)) {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _Vector.i16row; _i++) {
        _outp(_i,_posCol) = _Vector(_i,0);
    }
    return _outp;
}

/* Insert submatrix into matrix at _posRow & _posCol position
 * Example: A = Matrix 4x4, B = Matrix 2x3
 *
 *  C = A.InsertSubMatrix(B, 1, 1);
 *
 *  A = [A00  A01  A02  A03]    B = [B00  B01  B02]
 *      [A10  A11  A12  A13]        [B10  B11  B12]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 *
 *
 *  C = [A00  A01  A02  A03]
 *      [A10  B00  B01  B02]
 *      [A20  B10  B11  B12]
 *      [A30  A31  A32  A33]
 */
inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol) {
    Matrix _outp(*this);
    if (((_subMatrix.i16row+_posRow) > this->i16row) || ((_subMatrix.i16col+_posCol) > this->i16col)) {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _subMatrix.i16row; _i++) {
        for (int16_t _j = 0; _j < _subMatrix.i16col; _j++) {
            _outp(_i + _posRow,_j + _posCol) = _subMatrix(_i,_j);
        }
    }
    return _outp;
}

/* Insert the first _lenRow-th and first _lenColumn-th submatrix into matrix;
 *  at the matrix's _posRow and _posCol position.
 *
 * Example: A = Matrix 4x4, B = Matrix 2x3
 *
 *  C = A.InsertSubMatrix(B, 1, 1, 2, 2);
 *
 *  A = [A00  A01  A02  A03]    B = [B00  B01  B02]
 *      [A10  A11  A12  A13]        [B10  B11  B12]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 *
 *
 *  C = [A00  A01  A02  A03]
 *      [A10  B00  B01  A13]
 *      [A20  B10  B11  A23]
 *      [A30  A31  A32  A33]
 */
inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                                      const int16_t _lenRow, const int16_t _lenColumn)
{
    Matrix _outp(*this);
    if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posCol) > this->i16col) ||
        (_lenRow > _subMatrix.i16row) || (_lenColumn > _subMatrix.i16col))
    {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _lenRow; _i++) {
        for (int16_t _j = 0; _j < _lenColumn; _j++) {
            _outp(_i + _posRow,_j + _posCol) = _subMatrix(_i,_j);
        }
    }
    return _outp;
}

/* Insert the _lenRow & _lenColumn submatrix, start from _posRowSub & _posColSub submatrix;
 *  into matrix at the matrix's _posRow and _posCol position.
 *
 * Example: A = Matrix 4x4, B = Matrix 2x3
 *
 *  C = A.InsertSubMatrix(B, 1, 1, 0, 1, 1, 2);
 *
 *  A = [A00  A01  A02  A03]    B = [B00  B01  B02]
 *      [A10  A11  A12  A13]        [B10  B11  B12]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 *
 *  C = [A00  A01  A02  A03]
 *      [A10  B01  B02  A13]
 *      [A20  A21  A22  A23]
 *      [A30  A31  A32  A33]
 */
inline Matrix Matrix::InsertSubMatrix(const Matrix& _subMatrix, const int16_t _posRow, const int16_t _posCol,
                        const int16_t _posRowSub, const int16_t _posColSub,
                        const int16_t _lenRow, const int16_t _lenColumn)
{
    Matrix _outp(*this);
    if (((_lenRow+_posRow) > this->i16row) || ((_lenColumn+_posCol) > this->i16col) ||
        ((_posRowSub+_lenRow) > _subMatrix.i16row) || ((_posColSub+_lenColumn) > _subMatrix.i16col))
    {
        /* Return false */
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _i = 0; _i < _lenRow; _i++) {
        for (int16_t _j = 0; _j < _lenColumn; _j++) {
            _outp(_i + _posRow,_j + _posCol) = _subMatrix(_posRowSub+_i,_posColSub+_j);
        }
    }
    return _outp;
}

/* ------------------------------------------------- Big operations ------------------------------------------------- */
/* ------------------------------------------------- Big operations ------------------------------------------------- */

/* Invers operation using Gauss-Jordan algorithm */
inline Matrix Matrix::Invers(void) const {
    Matrix _outp(this->i16row, this->i16col, NoInitMatZero);
    Matrix _temp(*this);
    _outp.vSetIdentity();

    /* Gauss Elimination... */
    for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) {
        for (int16_t _i = _j+1; _i < _temp.i16row; _i++) {
            if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) {
                /* Matrix is non-invertible */
                _outp.vSetMatrixInvalid();
                return _outp;
            }

            float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j);

            for (int16_t _k = 0; _k < _temp.i16col; _k++) {
                _temp(_i,_k) -= (_temp(_j,_k) * _tempfloat);
                _outp(_i,_k) -= (_outp(_j,_k) * _tempfloat);

                _temp.vRoundingElementToZero(_i, _k);
                _outp.vRoundingElementToZero(_i, _k);
            }

        }
    }

    #if (1)
        /* Here, the _temp matrix should be an upper triangular matrix.
         * But because of rounding error, it might not.
         */
        for (int16_t _i = 1; _i < _temp.i16row; _i++) {
            for (int16_t _j = 0; _j < _i; _j++) {
                _temp(_i,_j) = 0.0;
            }
        }
    #endif

    /* Jordan... */
    for (int16_t _j = (_temp.i16row)-1; _j > 0; _j--) {
        for (int16_t _i = _j-1; _i >= 0; _i--) {
            if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) {
                /* Matrix is non-invertible */
                _outp.vSetMatrixInvalid();
                return _outp;
            }

            float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j);
            _temp(_i,_j) -= (_temp(_j,_j) * _tempfloat);
            _temp.vRoundingElementToZero(_i, _j);

            for (int16_t _k = (_temp.i16row - 1); _k >= 0; _k--) {
                _outp(_i,_k) -= (_outp(_j,_k) * _tempfloat);
                _outp.vRoundingElementToZero(_i, _k);
            }
        }
    }

    /* Normalization */
    for (int16_t _i = 0; _i < _temp.i16row; _i++) {
        if (fabs(_temp(_i,_i)) < float_prec(float_prec_ZERO)) {
            /* Matrix is non-invertible */
            _outp.vSetMatrixInvalid();
            return _outp;
        }

        float_prec _tempfloat = _temp(_i,_i);
        _temp(_i,_i) = 1.0;

        for (int16_t _j = 0; _j < _temp.i16row; _j++) {
            _outp(_i,_j) /= _tempfloat;
        }
    }
    return _outp;
}

/* Use elemetary row operation to reduce the matrix into upper triangular form
 *  (like in the first phase of gauss-jordan algorithm).
 *
 * Useful if we want to check whether the matrix is positive definite or not
 *  (useful before calling CholeskyDec function).
 */
inline bool Matrix::bMatrixIsPositiveDefinite(const bool checkPosSemidefinite) const {
    bool _posDef, _posSemiDef;
    Matrix _temp(*this);

    /* Gauss Elimination... */
    for (int16_t _j = 0; _j < (_temp.i16row)-1; _j++) {
        for (int16_t _i = _j+1; _i < _temp.i16row; _i++) {
            if (fabs(_temp(_j,_j)) < float_prec(float_prec_ZERO)) {
                /* Q: Do we still need to check this?
                 * A: idk, it's 3 AM. I need sleep :<
                 *
                 * NOTE TO FUTURE SELF: Confirm it!
                 */
                return false;
            }

            float_prec _tempfloat = _temp(_i,_j) / _temp(_j,_j);

            for (int16_t _k = 0; _k < _temp.i16col; _k++) {
                _temp(_i,_k) -= (_temp(_j,_k) * _tempfloat);
                _temp.vRoundingElementToZero(_i, _k);
            }

        }
    }

    _posDef = true;
    _posSemiDef = true;
    for (int16_t _i = 0; _i < _temp.i16row; _i++) {
        if (_temp(_i,_i) < float_prec(float_prec_ZERO)) {
            /* false if less than 0+ (zero included) */
            _posDef = false;
        }
        if (_temp(_i,_i) < -float_prec(float_prec_ZERO)) {
            /* false if less than 0- (zero is not included) */
            _posSemiDef = false;
        }
    }

    if (checkPosSemidefinite) {
        return _posSemiDef;
    } else {
        return _posDef;
    }
}

/* For square matrix 'this' with size MxM, return vector Mx1 with entries
 * correspond with diagonal entries of 'this'.
 *
 *   Example:    this = [a11 a12 a13]
 *                      [a21 a22 a23]
 *                      [a31 a32 a33]
 *
 * out = this.GetDiagonalEntries() = [a11]
 *                                   [a22]
 *                                   [a33]
 */
inline Matrix Matrix::GetDiagonalEntries(void) const {
    Matrix _temp(this->i16row, 1, NoInitMatZero);

    if (this->i16row != this->i16col) {
        _temp.vSetMatrixInvalid();
        return _temp;
    }
    for (int16_t _i = 0; _i < this->i16row; _i++) {
        _temp(_i,0) = (*this)(_i,_i);
    }
    return _temp;
}

/* Do the Cholesky Decomposition using Cholesky-Crout algorithm.
 *
 *      A = L*L'     ; A = real, positive definite, and symmetry MxM matrix
 *
 *      L = A.CholeskyDec();
 *
 *      CATATAN! NOTE! The symmetry property is not checked at the beginning to lower
 *          the computation cost. The processing is being done on the lower triangular
 *          component of _A. Then it is assumed the upper triangular is inherently
 *          equal to the lower end.
 *          (as a side note, Scilab & MATLAB is using Lapack routines DPOTRF that process
 *           the upper triangular of _A. The result should be equal mathematically if A
 *           is symmetry).
 */
inline Matrix Matrix::CholeskyDec(void) const {
    float_prec _tempFloat;

    /* Note that _outp need to be initialized as zero matrix */
    Matrix _outp(this->i16row, this->i16col, InitMatWithZero);

    if (this->i16row != this->i16col) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }
    for (int16_t _j = 0; _j < this->i16col; _j++) {
        for (int16_t _i = _j; _i < this->i16row; _i++) {
            _tempFloat = (*this)(_i,_j);
            if (_i == _j) {
                for (int16_t _k = 0; _k < _j; _k++) {
                    _tempFloat = _tempFloat - (_outp(_i,_k) * _outp(_i,_k));
                }
                if (_tempFloat < -float_prec(float_prec_ZERO)) {
                    /* Matrix is not positif (semi)definit */
                    _outp.vSetMatrixInvalid();
                    return _outp;
                }
                /* Rounding to zero to avoid case where sqrt(0-) */
                if (fabs(_tempFloat) < float_prec(float_prec_ZERO)) {
                    _tempFloat = 0.0;
                }
                _outp(_i,_i) = sqrt(_tempFloat);
            } else {
                for (int16_t _k = 0; _k < _j; _k++) {
                    _tempFloat = _tempFloat - (_outp(_i,_k) * _outp(_j,_k));
                }
                if (fabs(_outp(_j,_j)) < float_prec(float_prec_ZERO)) {
                    /* Matrix is not positif definit */
                    _outp.vSetMatrixInvalid();
                    return _outp;
                }
                _outp(_i,_j) = _tempFloat / _outp(_j,_j);
            }
        }
    }
    return _outp;
}

/* Do the Householder Transformation for QR Decomposition operation.
 *              out = HouseholderTransformQR(A, i, j)
 */
inline Matrix Matrix::HouseholderTransformQR(const int16_t _rowTransform, const int16_t _colTransform) {
    float_prec _tempFloat;
    float_prec _xLen;
    float_prec _x1;
    float_prec _u1;
    float_prec _vLen2;

    /* Note that _outp & _vectTemp need to be initialized as zero matrix */
    Matrix _outp(this->i16row, this->i16row, InitMatWithZero);
    Matrix _vectTemp(this->i16row, 1, InitMatWithZero);

    if ((_rowTransform >= this->i16row) || (_colTransform >= this->i16col)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    /* Until here:
     *
     * _xLen    = ||x||            = sqrt(x1^2 + x2^2 + .. + xn^2)
     * _vLen2   = ||u||^2 - [u1^2] = x2^2 + .. + xn^2
     * _vectTemp= [0 0 0 .. x1=0 x2 x3 .. xn]'
     */
    _x1 = (*this)(_rowTransform,_colTransform);
    _xLen = _x1*_x1;
    _vLen2 = 0.0;
    for (int16_t _i = _rowTransform+1; _i < this->i16row; _i++) {
        _vectTemp(_i,0) = (*this)(_i,_colTransform);

        _tempFloat = _vectTemp(_i,0) * _vectTemp(_i,0);
        _xLen  += _tempFloat;
        _vLen2 += _tempFloat;
    }
    _xLen = sqrt(_xLen);

    /* u1    = x1+(-sign(x1))*xLen */
    if (_x1 < 0.0) {
        _u1 = _x1+_xLen;
    } else {
        _u1 = _x1-_xLen;
    }

    /* Solve vlen2 & tempHH */
    _vLen2 += (_u1*_u1);
    _vectTemp(_rowTransform,0) = _u1;

    if (fabs(_vLen2) < float_prec(float_prec_ZERO_ECO)) {
        /* x vector is collinear with basis vector e, return result = I */
        _outp.vSetIdentity();
    } else {
        /* P = -2*(u1*u1')/v_len2 + I */
        /* PR TODO: We need to investigate more on this */
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            _tempFloat = _vectTemp(_i,0);
            if (fabs(_tempFloat) > float_prec(float_prec_ZERO)) {
                for (int16_t _j = 0; _j < this->i16row; _j++) {
                    if (fabs(_vectTemp(_j,0)) > float_prec(float_prec_ZERO)) {
                        _outp(_i,_j) = _vectTemp(_j,0);
                        _outp(_i,_j) = _outp(_i,_j) * _tempFloat;
                        _outp(_i,_j) = _outp(_i,_j) * (-2.0/_vLen2);
                    }
                }
            }
            _outp(_i,_i) = _outp(_i,_i) + 1.0;
        }
    }
    return _outp;
}

/* Do the QR Decomposition for matrix using Householder Transformation.
 *                      A = Q*R
 *
 * PERHATIAN! CAUTION! The matrix calculated by this function are Q' and R (Q transpose and R).
 *  Because QR Decomposition usually used to calculate solution for least-squares equation
 *  (that need Q'), we don't do the transpose of Q inside this routine to lower the
 *  computation cost (user need to transpose outside if they want Q).
 *
 * Example of using QRDec to solve least-squares:
 *                      Ax = b
 *                   (QR)x = b
 *                      Rx = Q'b    --> Afterward use back-subtitution to solve x
 */
inline bool Matrix::QRDec(Matrix& Qt, Matrix& R) const {
    Matrix Qn(Qt.i16row, Qt.i16col, NoInitMatZero);
    if ((this->i16row < this->i16col) || (!Qt.bMatrixIsSquare()) || (Qt.i16row != this->i16row) ||
        (R.i16row != this->i16row) || (R.i16col != this->i16col))
    {
        Qt.vSetMatrixInvalid();
        R.vSetMatrixInvalid();
        return false;
    }
    R = (*this);
    Qt.vSetIdentity();
    for (int16_t _i = 0; (_i < (this->i16row - 1)) && (_i < this->i16col-1); _i++) {
        Qn  = R.HouseholderTransformQR(_i, _i);
        if (!Qn.bMatrixIsValid()) {
            Qt.vSetMatrixInvalid();
            R.vSetMatrixInvalid();
            return false;
        }
        Qt = Qn * Qt;
        R  = Qn * R;
    }
#if (0)
    Qt.RoundingMatrixToZero();
    R.RoundingMatrixToZero();
#else
    Qt.RoundingMatrixToZero();
    for (int16_t _i = 1; ((_i < R.i16row) && (_i < R.i16col)); _i++) {
        for (int16_t _j = 0; _j < _i; _j++) {
            R(_i, _j) = 0.0;
        }
    }
#endif

    /* Q = Qt.Transpose */
    return true;
}

/* Do the back-subtitution operation for upper triangular matrix A & column matrix B to solve x:
 *                      Ax = B
 *
 * x = BackSubtitution(&A, &B);
 *
 * CATATAN! NOTE! To lower the computation cost, we don't check that A is a upper triangular
 *  matrix (it's assumed that user already make sure of that before calling this routine).
 */
inline Matrix Matrix::BackSubtitution(const Matrix& A, const Matrix& B) const {
    Matrix _outp(A.i16row, 1, NoInitMatZero);
    if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = A.i16col-1; _i >= 0; _i--) {
        _outp(_i,0) = B(_i,0);
        for (int16_t _j = _i + 1; _j < A.i16col; _j++) {
            _outp(_i,0) = _outp(_i,0) - A(_i,_j)*_outp(_j,0);
        }
        if (fabs(A(_i,_i)) < float_prec(float_prec_ZERO)) {
            _outp.vSetMatrixInvalid();
            return _outp;
        }
        _outp(_i,0) = _outp(_i,0) / A(_i,_i);
    }

    return _outp;
}

/* Do the forward-subtitution operation for lower triangular matrix A & column matrix B to solve x:
 *                      Ax = B
 *
 * x = ForwardSubtitution(&A, &B);
 *
 * CATATAN! NOTE! To lower the computation cost, we don't check that A is a lower triangular
 *  matrix (it's assumed that user already make sure of that before calling this routine).
 */
inline Matrix Matrix::ForwardSubtitution(const Matrix& A, const Matrix& B) const {
    Matrix _outp(A.i16row, 1, NoInitMatZero);
    if ((A.i16row != A.i16col) || (A.i16row != B.i16row)) {
        _outp.vSetMatrixInvalid();
        return _outp;
    }

    for (int16_t _i = 0; _i < A.i16row; _i++) {
        _outp(_i,0) = B(_i,0);
        for (int16_t _j = 0; _j < _i; _j++) {
            _outp(_i,0) = _outp(_i,0) - A(_i,_j)*_outp(_j,0);
        }
        if (fabs(A(_i,_i)) < float_prec(float_prec_ZERO)) {
            _outp.vSetMatrixInvalid();
            return _outp;
        }
        _outp(_i,0) = _outp(_i,0) / A(_i,_i);
    }
    return _outp;
}

/* -------------------------------------------- Matrix printing function -------------------------------------------- */
/* -------------------------------------------- Matrix printing function -------------------------------------------- */
#if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
    inline void Matrix::vPrint(void) {
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            cout << "[ ";
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                cout << std::fixed << std::setprecision(3) << (*this)(_i,_j) << " ";
            }
            cout << "]";
            cout << endl;
        }
        cout << endl;
    }
    inline void Matrix::vPrintFull(void) {
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            cout << "[ ";
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                cout << resetiosflags( ios::fixed | ios::showpoint ) << (*this)(_i,_j) << " ";
            }
            cout << "]";
            cout << endl;
        }
        cout << endl;
    }
#elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
    inline void Matrix::vPrint(void) {
        char _bufSer[10];
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            Serial.print("[ ");
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                snprintf(_bufSer, sizeof(_bufSer)-1, "%2.2f ", (*this)(_i,_j));
                Serial.print(_bufSer);
            }
            Serial.println("]");
        }
        Serial.println("");
    }
    inline void Matrix::vPrintFull(void) {
        char _bufSer[32];
        for (int16_t _i = 0; _i < this->i16row; _i++) {
            Serial.print("[ ");
            for (int16_t _j = 0; _j < this->i16col; _j++) {
                snprintf(_bufSer, sizeof(_bufSer)-1, "%e ", (*this)(_i,_j));
                Serial.print(_bufSer);
            }
            Serial.println("]");
        }
        Serial.println("");
    }
#else
    /* User must define the print function somewhere */
    /* void vPrint(); */
    /* void vPrintFull(); */
#endif

#endif // MATRIX_H


text:
/**************************************************************************************************
 * This file contains configuration parameters
 *
 *
 * See https://github.com/pronenewbits for more!
 *************************************************************************************************/
#ifndef KONFIG_H
#define KONFIG_H

#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <math.h>

/* State Space dimension */
#define SS_X_LEN    (4)
#define SS_Z_LEN    (2)
#define SS_U_LEN    (2)
#define SS_DT_MILIS (10)                            /* 10 ms */
#define SS_DT       float_prec(SS_DT_MILIS/1000.)   /* Sampling time */

/* MPC Parameters */
#define MPC_HP_LEN      (4)
#define MPC_HU_LEN      (3)

#define MPC_USE_CONSTRAINT_DU
// #define MPC_USE_CONSTRAINT_U
#define MPC_USE_CONSTRAINT_Z

#define MPC_MIN_DU      (-5)
#define MPC_MAX_DU      (5)
#define MPC_MIN_U       (-150)
#define MPC_MAX_U       (100)
#define MPC_MIN_Z       (-4)
#define MPC_MAX_Z       (3.5)

/* Change this size based on the biggest matrix you will use */
#define MATRIX_MAXIMUM_SIZE     (28)

/* Define this to enable matrix bounds checking */
#define MATRIX_USE_BOUNDS_CHECKING

/* Set this define to choose math precision of the system */
#define PRECISION_SINGLE    1
#define PRECISION_DOUBLE    2
#define FPU_PRECISION       (PRECISION_SINGLE)

#if (FPU_PRECISION == PRECISION_SINGLE)
    #define float_prec          float
    #define float_prec_ZERO     (1e-7)
    #define float_prec_ZERO_ECO (1e-5)      /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
#elif (FPU_PRECISION == PRECISION_DOUBLE)
    #define float_prec          double
    #define float_prec_ZERO     (1e-13)
    #define float_prec_ZERO_ECO (1e-8)      /* 'Economical' zero, for noisy calculation where 'somewhat zero' is good enough */
#else
    #error("FPU_PRECISION has not been defined!");
#endif

/* Set this define to choose system implementation (mainly used to define how you print the matrix via the Matrix::vPrint() & Matrix::vPrintFull() function) */
#define SYSTEM_IMPLEMENTATION_PC                    1
#define SYSTEM_IMPLEMENTATION_EMBEDDED_CUSTOM       2
#define SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO      3

#define SYSTEM_IMPLEMENTATION                       (SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)

/* ASSERT is evaluated locally (without function call) to lower the computation cost */
void SPEW_THE_ERROR(char const * str);
#define ASSERT(truth, str) { if (!(truth)) SPEW_THE_ERROR(str); }

#endif // KONFIG_H


text:
#include <Wire.h>
#include <elapsedMillis.h>
#include "konfig.h"
#include "matrix.h"
#include "mpc.h"

void vCreateConstraintsLHS(Matrix &CnstLHS, const Matrix &CTHETA);
void vCreateConstraintsRHS(Matrix &CnstRHS, const Matrix &COMEGA, const Matrix &CPSI, const Matrix &u_prev, const Matrix &x);

elapsedMillis timerMPC;     /* Timer for sampling time */
uint64_t u64compuTime;      /* For benchmark */
char bufferTxSer[100];      /* For serial printing */

/* Plant system */
Matrix A(SS_X_LEN, SS_X_LEN);
Matrix B(SS_X_LEN, SS_U_LEN);
Matrix C(SS_Z_LEN, SS_X_LEN);
Matrix SP((MPC_HP_LEN*SS_Z_LEN), 1);
Matrix x(SS_X_LEN, 1);
Matrix u(SS_U_LEN, 1);
Matrix z(SS_Z_LEN, 1);

int16_t i16iterSP = 0;
MPC MPC_HIL(A, B, C, 1, 0.01, vCreateConstraintsLHS, vCreateConstraintsRHS);
uint64_t _maxu64compuTime = 0;
void setup() {
    /* serial to display data */
    Serial.begin(115200);
    while(!Serial) {}
    pinMode(LED_BUILTIN, OUTPUT);

    /* Ref: https://www.mathworks.com/help/control/ug/mimo-state-space-models.html#buv3tp8-1
     *
     * State-Space Model of Jet Transport Aircraft
     *  This example shows how to build a MIMO model of a jet transport. Because the development of a physical model
     *  for a jet aircraft is lengthy, only the state-space equations are presented here. See any standard text in
     *  aviation for a more complete discussion of the physics behind aircraft flight.
     * The jet model during cruise flight at MACH = 0.8 and H = 40,000 ft. is
     *
     * (The model has two inputs and two outputs. The units are radians for beta (sideslip angle) and phi (bank angle) and
     * radians/sec for yaw (yaw rate) and roll (roll rate). The rudder and aileron deflections are in degrees.)
     */
    A[0][0] = -0.0558;      A[0][1] = -0.9968;      A[0][2] =  0.0802;      A[0][3] = 0.0415;
    A[1][0] =  0.5980;      A[1][1] = -0.1150;      A[1][2] = -0.0318;      A[1][3] = 0.0000;
    A[2][0] = -3.0500;      A[2][1] =  0.3880;      A[2][2] = -0.4650;      A[2][3] = 0.0000;
    A[3][0] =  0.0000;      A[3][1] =  0.0805;      A[3][2] =  1.0000;      A[3][3] = 0.0000;

    B[0][0] =  0.0073;      B[0][1] =  0.0000;
    B[1][0] = -0.4750;      B[1][1] =  0.0077;
    B[2][0] =  0.1530;      B[2][1] =  0.1430;
    B[3][0] =  0.0000;      B[3][1] =  0.0000;

    C[0][0] =  0.0000;      C[0][1] =  1.0000;      C[0][2] =  0.0000;      C[0][3] =  0.0000;
    C[1][0] =  0.0000;      C[1][1] =  0.0000;      C[1][2] =  0.0000;      C[1][3] =  1.0000;

    MPC_HIL.vReInit(A, B, C, 10, 0.05, vCreateConstraintsLHS, vCreateConstraintsRHS);
}

void loop() {
    if (timerMPC > SS_DT_MILIS) {
        /* ================================ Updating Set Point ================================= */
        Matrix SP_NEXT(SS_Z_LEN, 1);
        if (i16iterSP < 100-MPC_HP_LEN+1) {
            SP_NEXT[0][0] = 3.14/2.;
            SP_NEXT[1][0] = 1;
        } else if (i16iterSP < 200-MPC_HP_LEN+1) {
            SP_NEXT[0][0] = 3.14/2.;
            SP_NEXT[1][0] = -3;
        } else {
            SP_NEXT[0][0] = 3.14;
            SP_NEXT[1][0] = -3;
        }
        if (i16iterSP < 300-MPC_HP_LEN+1) {
            i16iterSP++;
        } else {
            i16iterSP = 0;
        }
        for (int32_t _i = 0; _i < (MPC_HP_LEN-1); _i++) {
            SP = SP.InsertSubMatrix(SP, (_i*SS_Z_LEN), 0, ((_i+1)*SS_Z_LEN), 0, SS_Z_LEN, 1);
        }
        SP = SP.InsertSubMatrix(SP_NEXT, ((MPC_HP_LEN-1)*SS_Z_LEN), 0, 0, 0, SS_Z_LEN, 1);
        /* -------------------------------- Updating Set Point --------------------------------- */

        digitalWrite(LED_BUILTIN, HIGH);
        /* ===================================== MPC Update ==================================== */
        u64compuTime = micros();

        MPC_HIL.bUpdate(SP, x, u);

        u64compuTime = (micros() - u64compuTime);

        _maxu64compuTime < u64compuTime? _maxu64compuTime = u64compuTime: 0;
        /* ------------------------------------- MPC Update ------------------------------------ */
        digitalWrite(LED_BUILTIN, LOW);

        /* ================================= Plant Simulation ================================== */
        x = A*x + B*u;
        z = C*x;
        /* --------------------------------- Plant Simulation ---------------------------------- */

        /* =========================== Print to serial (for plotting) ========================== */
        #if (1)
            /* Print: Computation time, number of active set iteration, Set-Point, z */
            snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %i %.3f %.3f %.3f %.3f", ((float)_maxu64compuTime)/1000., MPC_HIL.cntIterActiveSet, SP[0][0], SP[1][0], z[0][0], z[1][0]);
        #else
            /* Print: Computation time, Set-Point, z, u */
            snprintf(bufferTxSer, sizeof(bufferTxSer)-1, "%.3f %i %.3f %.3f %.3f %.3f %.3f %.3f", ((float)_maxu64compuTime)/1000., MPC_HIL.cntIterActiveSet, SP[0][0], SP[1][0], z[0][0], z[1][0], u[0][0], u[1][0]);
        #endif
        Serial.print(bufferTxSer);
        Serial.print('\n');
        /* --------------------------- Print to serial (for plotting) -------------------------- */

        timerMPC = 0;
    }
}

void vCreateConstraintsLHS(Matrix &CnstLHS, const Matrix &CTHETA)
{
    #if (defined(MPC_USE_CONSTRAINT_DU) || defined(MPC_USE_CONSTRAINT_U) || defined(MPC_USE_CONSTRAINT_Z))
        int16_t _i16offsetCnstr = 0;
        CnstLHS = Matrix((INEQ_LEN_DU + INEQ_LEN_U + INEQ_LEN_Z), (MPC_HU_LEN*SS_U_LEN));
    #endif
    #if defined(MPC_USE_CONSTRAINT_DU)
        /* for dUmin <= dU(kj <= dUmax                                                      ...{MPC_5a} */
        {
            Matrix CnstDu_LHS   {(2*(MPC_HU_LEN*SS_U_LEN)), (MPC_HU_LEN*SS_U_LEN)};
            Matrix CnstDu_Identity((MPC_HU_LEN*SS_U_LEN), (MPC_HU_LEN*SS_U_LEN));
            CnstDu_Identity.vSetIdentity();
            CnstDu_LHS = CnstDu_LHS.InsertSubMatrix(-CnstDu_Identity, 0, 0);
            CnstDu_LHS = CnstDu_LHS.InsertSubMatrix(CnstDu_Identity, (MPC_HU_LEN*SS_U_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstLHS = CnstLHS.InsertSubMatrix(CnstDu_LHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstDu_LHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_U)
        /* for Umin <= U(kj <= Umax                                                         ...{MPC_5b} */
        {
            Matrix CnstU_LHS   {(2*(MPC_HU_LEN*SS_U_LEN)), (MPC_HU_LEN*SS_U_LEN)};
            Matrix U_Identity(SS_U_LEN, SS_U_LEN);
            U_Identity.vSetIdentity();
            for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) {
                for (int16_t _j = 0; _j < MPC_HU_LEN; _j++) {
                    if (_j <= _i) {
                        /* for Umin <= U(k) */
                        CnstU_LHS = CnstU_LHS.InsertSubMatrix(-U_Identity, (_i*SS_U_LEN), (_j*SS_U_LEN));
                        /* for U(k) <= Umax */
                        CnstU_LHS = CnstU_LHS.InsertSubMatrix(U_Identity,  (MPC_HU_LEN*SS_U_LEN)+(_i*SS_U_LEN), (_j*SS_U_LEN));
                    }
                }
            }

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstLHS = CnstLHS.InsertSubMatrix(CnstU_LHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstU_LHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_Z)
        /* for Zmin <= Z(kj <= Zmax                                                         ...{MPC_5c} */
        {
            Matrix CnstZ_LHS   {(2*(MPC_HP_LEN*SS_Z_LEN)), (MPC_HU_LEN*SS_U_LEN)};
            CnstZ_LHS = CnstZ_LHS.InsertSubMatrix(-CTHETA, 0, 0);
            CnstZ_LHS = CnstZ_LHS.InsertSubMatrix(CTHETA, (MPC_HP_LEN*SS_Z_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstLHS = CnstLHS.InsertSubMatrix(CnstZ_LHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstZ_LHS.i16getRow();
        }
    #endif
}

#if defined(MPC_USE_CONSTRAINT_DU)
    Matrix CnstDu_RHS   {(2*(MPC_HU_LEN*SS_U_LEN)), 1};
    Matrix CnstDu_MaxMin((MPC_HU_LEN*SS_U_LEN), 1);
#endif
#if defined(MPC_USE_CONSTRAINT_U)
    Matrix CnstU_RHS   {(2*(MPC_HU_LEN*SS_U_LEN)), 1};
    Matrix CnstU_MaxMin((MPC_HU_LEN*SS_U_LEN), 1);
#endif
#if defined(MPC_USE_CONSTRAINT_Z)
    Matrix CnstZ_RHS   {(2*(MPC_HP_LEN*SS_Z_LEN)), 1};
    Matrix CnstZ_Min((MPC_HP_LEN*SS_Z_LEN), 1);
    Matrix CnstZ_Max((MPC_HP_LEN*SS_Z_LEN), 1);
#endif

void vCreateConstraintsRHS(Matrix &CnstRHS, const Matrix &COMEGA, const Matrix &CPSI, const Matrix &u_prev, const Matrix &x)
{
	#if (defined(MPC_USE_CONSTRAINT_DU) || defined(MPC_USE_CONSTRAINT_U) || defined(MPC_USE_CONSTRAINT_Z))
        int16_t _i16offsetCnstr = 0;
        CnstRHS = Matrix((INEQ_LEN_DU + INEQ_LEN_U + INEQ_LEN_Z), 1);
    #endif
    #if defined(MPC_USE_CONSTRAINT_DU)
        /* for dUmin <= dU(kj <= dUmax                                                      ...{MPC_5a} */
        {
            CnstDu_MaxMin.vSetHomogen(MPC_MIN_DU);
            CnstDu_RHS = CnstDu_RHS.InsertSubMatrix(-CnstDu_MaxMin, 0, 0);
            CnstDu_MaxMin.vSetHomogen(MPC_MAX_DU);
            CnstDu_RHS = CnstDu_RHS.InsertSubMatrix(CnstDu_MaxMin, (MPC_HU_LEN*SS_U_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstRHS = CnstRHS.InsertSubMatrix(CnstDu_RHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstDu_RHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_U)
        /* for Umin <= U(kj <= Umax                                                         ...{MPC_5b} */
        {
            for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) {
                for (int16_t _j = 0; _j < SS_U_LEN; _j++) {
                    CnstU_MaxMin[(_i*SS_U_LEN)+_j][0] = -MPC_MIN_U + u_prev[_j][0];
                }
            }
            CnstU_RHS = CnstU_RHS.InsertSubMatrix(CnstU_MaxMin, 0, 0);
            for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) {
                for (int16_t _j = 0; _j < SS_U_LEN; _j++) {
                    CnstU_MaxMin[(_i*SS_U_LEN)+_j][0] = MPC_MAX_U - u_prev[_j][0];
                }
            }
            CnstU_RHS = CnstU_RHS.InsertSubMatrix(CnstU_MaxMin, (MPC_HU_LEN*SS_U_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstRHS = CnstRHS.InsertSubMatrix(CnstU_RHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstU_RHS.i16getRow();
        }
    #endif
    #if defined(MPC_USE_CONSTRAINT_Z)
        /* for Zmin <= Z(kj <= Zmax                                                         ...{MPC_5c} */
        {
            CnstZ_Min = CPSI*x + COMEGA*u_prev - MPC_MIN_Z;
            CnstZ_RHS = CnstZ_RHS.InsertSubMatrix(CnstZ_Min, 0, 0);

            CnstZ_Max = -CPSI*x + -COMEGA*u_prev + MPC_MAX_Z;
            CnstZ_RHS = CnstZ_RHS.InsertSubMatrix(CnstZ_Max, (MPC_HP_LEN*SS_Z_LEN), 0);

            /* Append to Constraint_LHS & Constraint_RHS */
            CnstRHS = CnstRHS.InsertSubMatrix(CnstZ_RHS, _i16offsetCnstr, 0);
            _i16offsetCnstr += CnstZ_RHS.i16getRow();
        }
    #endif
}

void SPEW_THE_ERROR(char const * str)
{
    #if (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_PC)
        cout << (str) << endl;
    #elif (SYSTEM_IMPLEMENTATION == SYSTEM_IMPLEMENTATION_EMBEDDED_ARDUINO)
        Serial.println(str);
    #else
        /* Silent function */
    #endif
    while(1);
}