/_3rdParty/MathLib/Matrix.h

/*
 * Copyright (C) 2010 Learning Algorithms and Systems Laboratory, EPFL, Switzerland
 * Author: Eric Sauser
 * email:   eric.sauser@a3.epf.ch
 * website: lasa.epfl.ch
 *
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * This program 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 General
 * Public License for more details
 */

#ifndef MATRIX_H
#define MATRIX_H
#include <assert.h>
#include <vector>
using namespace std;

#include "MathLibCommon.h"


#include "Macros.h"
#include "Vector.h"

#ifdef USE_MATHLIB_NAMESPACE
namespace MathLib {
#endif

#ifdef  USE_T_EXTENSIONS
template<unsigned int ROW> class TMatrix;
#endif


/**
 * \class Matrix
 *
 * \ingroup MathLib
 *
 * \brief The basic matrix class
 *
 * This matrix class can be used for doing various matrix manipulation.
 * This shFould be combined with the Vector class for doing almost anything
 * you ever dreamt of. Please understand almost as almost...
 */

class Matrix
{
    friend class Vector;
#ifdef  USE_T_EXTENSIONS
    template<unsigned int ROW> friend class TMatrix;
#endif

protected:
    static int bInverseOk;    ///< Tell if last inverse operation was sucessfull

    unsigned int  row;        ///< Number of rows
    unsigned int  column;     ///< Number of columns
    REALTYPE        *_;          ///< The data array

public:

    /// Empty constructor
    inline Matrix() {
        row    = 0;
        column = 0;
        _      = NULL;
    }

    /// Destructor
    inline virtual ~Matrix(){
        Release();
    }

    /// Copy constructor
    inline Matrix(const Matrix &matrix)
    {
        row    = 0;
        column = 0;
        _      = NULL;
        Set(matrix);
    }

    /**
    * \brief Create a sized matrix
    * \param rowSize  The row size
    * \param colSize  The column size
    * \param clear    Tell if the matrix sould be set to 0
    */
    inline Matrix(unsigned int rowSize, unsigned int colSize, bool clear = true)
    {
        row    = 0;
        column = 0;
        _      = NULL;
        Resize(rowSize,colSize,false);
        if(clear) Zero();
    }

    /**
     * \brief Create a matrix by copying a memory area
     * \param _     The array of REALTYPE to be copied
     * \param rowSize  The row size
     * \param colSize  The column size
     */
    inline Matrix(const REALTYPE *array, unsigned int rowSize, unsigned int colSize)
    {
        row     = 0;
        column  = 0;
        _       = NULL;
        Set(array,rowSize,colSize);
    }

#ifdef  USE_T_EXTENSIONS
    /// Copy Contructor of a template Matrix (see TMatrix)
    template<unsigned int ROW> inline Matrix(const TMatrix<ROW> &matrix)
    {
        row    = 0;
        column = 0;
        _      = NULL;
        Set(matrix._,ROW,ROW);
    }
#endif

    /**
    * \brief Set a matrix by copying a memory area
    * \param _     The array of REALTYPE to be copied
    * \param rowSize  The row size
    * \param colSize  The column size
    */
    inline Matrix& Set(const REALTYPE *array, unsigned int rowSize, unsigned int colSize){
        Resize(rowSize,colSize,false);
        if((row)&&(column)&&(array))
            memcpy(_,array,rowSize*colSize*sizeof(REALTYPE));
        return *this;
    }

    /**
    * \brief Set a matrix by copying a memory area
    * \param _     The array of REALTYPE to be copied
    * \param rowSize  The row size
    * \param colSize  The column size
    */
    inline Matrix& Set(const Matrix & matrix){
        Resize(matrix.row,matrix.column,false);
        if((row)&&(column))
            memcpy(_,matrix._,row*column*sizeof(REALTYPE));
        return *this;
    }


    /// Get the number of rows
    inline unsigned int RowSize() const{
        return row;
    }
    /// Get the number of columns
    inline unsigned int ColumnSize() const{
        return column;
    }
    /// Get the data array
    inline REALTYPE *Array() const{
        return _;
    }

    /// Clear the matrix
    inline Matrix& Zero()
    {
        if((row)&&(column))
            memset(_,0,row*column*sizeof(REALTYPE));
        return *this;
    }
    /// Identity matrix
    inline Matrix& Identity()
    {
        unsigned int k = (row>column?column:row);
        Zero();
        REALTYPE *ptr = _;
        while(k--){
            *ptr = R_ONE;
            ptr += column+1;
        }
        return *this;
    }
    /// Set all elements to 1
    inline Matrix& One()
    {
        REALTYPE *src = _;
        unsigned int len = row*column;
        while(len--)
            *(src++) = R_ONE;
        return *this;
    }

    /// Access to a reference of any element of the matrix
    inline REALTYPE& operator() (const unsigned int row, const unsigned int col){
        if((row<this->row)&&(col<this->column))
            return _[row*column+col];
        return Vector::undef;
    }
    /// Access to a reference of any element of the matrix
    inline REALTYPE& Ref(const unsigned int row, const unsigned int col){
        if((row<this->row)&&(col<this->column))
            return _[row*column+col];
        return Vector::undef;
    }
    /// Access to an element of the matrix
    inline REALTYPE At(const unsigned int row, const unsigned int col) const{
        if((row<this->row)&&(col<this->column))
            return _[row*column+col];
        return Vector::undef;
    }
    /// Access to a reference of any element of the matrix without checks
    inline REALTYPE& RefNoCheck(const unsigned int row, const unsigned int col){
        return _[row*column+col];
    }
    /// Access to an element of the matrix without checks
    inline REALTYPE AtNoCheck(const unsigned int row, const unsigned int col) const{
        return _[row*column+col];
    }

    /// Get a copy of a row into a vector
    inline Vector GetRow(const unsigned int r) const
    {
        Vector result;
        return GetRow(r,result);
    }
    /// Get a copy of a row into a vector
    inline Vector& GetRow(const unsigned int r, Vector& result) const
    {
        result.Resize(column,false);
        if(r<this->row){
            REALTYPE *src = _ + r*column;
            REALTYPE *dst = result._;
            unsigned int len = column;
            while(len--)
                *(dst++) = *(src++);
        }else{
            result.Zero();
        }
        return result;
    }

    /// Get a copy of a column into a vector
    inline Vector GetColumn(const unsigned int col) const
    {
        Vector result;
        return GetColumn(col,result);
    }
    /// Get a copy of a column into a vector
    inline Vector& GetColumn(const unsigned int col, Vector& result) const
    {
        result.Resize(row,false);
        if(col<column){
            REALTYPE *src = _ + col;
            REALTYPE *dst = result._;
            unsigned int len = row;
            while(len--){
                *(dst++) = *(src);
                src += column;
            }
        }else{
            result.Zero();
        }
        return result;
    }

    inline Matrix GetRows(unsigned int rowStart, unsigned int rowEnd) const{
        Matrix result;
        return GetRows(rowStart,rowEnd,result);
    }
    inline Matrix& GetRows(unsigned int rowStart, unsigned int rowEnd, Matrix& result) const{
        if(rowStart>rowEnd){
            return result.Resize(0,0,false);
        }
        unsigned int rowLength = rowEnd-rowStart+1;
        result.Resize(rowLength,column,false);
        if(rowStart>=row){
            return result.Zero();
        }
        REALTYPE *src = _ + rowStart*column;
        REALTYPE *dst = result._;
        unsigned int len;
        if(rowEnd<row) len = rowLength*column;
        else           len = (row-rowStart)*column;
        while(len--)
            *(dst++) = *(src++);
        if(rowEnd>=row){
            len = (rowEnd-row+1)*column;
            while(len--)
                *(dst++) = R_ZERO;
        }
        return result;
    }

    inline Matrix GetColumns(unsigned int colStart, unsigned int colEnd) const{
        Matrix result;
        return GetColumns(colStart,colEnd,result);
    }
    inline Matrix& GetColumns(unsigned int colStart, unsigned int colEnd, Matrix& result) const{
        if(colStart>colEnd){
            return result.Resize(0,0,false);
        }
        unsigned int colLength = colEnd-colStart+1;
        result.Resize(row,colLength,false);
        if(colStart>=column){
            return result.Zero();
        }
        REALTYPE *src = _ + colStart;
        REALTYPE *dst = result._;
        unsigned int dstOffset = 0;
        if(colEnd>=column){
            result.Zero();
            dstOffset = (colEnd-column+1);
            colEnd    = column-1;
            colLength = colEnd-colStart+1;
        }
        unsigned int rowLen = row;
        while(rowLen--){
            unsigned int colLen = colLength;
            REALTYPE *cSrc = src;
            while(colLen--)
                *(dst++) = *(cSrc++);
            dst += dstOffset;
            src += column;
        }
        return result;
    }

    inline Matrix GetMatrix(unsigned int rowStart, unsigned int rowEnd, unsigned int colStart, unsigned int colEnd) const{
        Matrix result;
        return GetMatrix(rowStart,rowEnd,colStart,colEnd,result);
    }
    inline Matrix& GetMatrix(unsigned int rowStart, unsigned int rowEnd, unsigned int colStart, unsigned int colEnd, Matrix &result) const{
        if((rowStart>rowEnd)||(colStart>colEnd)){
            return result.Resize(0,0,false);
        }
        unsigned int rowLength = rowEnd-rowStart+1;
        unsigned int colLength = colEnd-colStart+1;
        result.Resize(rowLength,colLength,false);
        if((rowStart>=row)||(colStart>=column)){
            return result.Zero();
        }
        unsigned int dstOffset = 0;
        if((rowEnd>=row)||(colEnd>=column)){
            result.Zero();
            if(rowEnd>=row){
                rowEnd = row-1;
                rowLength = rowEnd-rowStart+1;
            }
            if(colEnd>=column){
                dstOffset = (colEnd-column+1);
                colEnd    = column-1;
                colLength = colEnd-colStart+1;
            }
        }
        REALTYPE *src = _ + colStart + rowStart*column;
        REALTYPE *dst = result._;
        unsigned int rowLen = rowLength;
        while(rowLen--){
            REALTYPE *cSrc = src;
            unsigned int colLen = colLength;
            while(colLen--){
                *(dst++) = *(cSrc++);
            }
            src += column;
            dst += dstOffset;
        }
        return result;
    }

    /**
     * \brief Get a matrix spanning several rows of the matrix
     * \param row      The starting row
     * \param rowSize  The number of rows
     * \return         The resulting matrix
     */
    inline Matrix GetRowSpace(const unsigned int row, const unsigned int len) const
    {
        Matrix result;
        return GetRowSpace(row,len,result);
    }
    /**
    * \brief Get a matrix spanning several rows of the matrix
    * \param row      The starting row
    * \param len      The number of rows
    * \param result   The target matrix
    * \return         The resulting matrix
    */
    inline Matrix& GetRowSpace(const unsigned int row, const unsigned int len, Matrix &result) const
    {
        if(len>0){
            return GetRows(row,row+len-1,result);
        }
        return result.Resize(0,0,false);
    }

    /**
     * \brief Get a matrix spanning several columns of the matrix
     * \param row      The starting column
     * \param len      The number of columns
     * \return         The resulting matrix
     */
    inline Matrix GetColumnSpace(const unsigned int col, const unsigned int len) const
    {
        Matrix result;
        return GetColumnSpace(col,len,result);
    }
    /**
     * \brief Get a matrix spanning several columns of the matrix
     * \param row      The starting column
     * \param len      The number of columns
     * \param result   The target matrix
     * \return         The resulting matrix
     */
    inline Matrix& GetColumnSpace(const unsigned int col, const unsigned int len, Matrix &result) const
    {
        if(len>0){
            return GetColumns(col,col+len-1,result);
        }
        return result.Resize(0,0,false);
    }

    inline Matrix GetMatrixSpace(const unsigned int row, const unsigned int rowLen,
                                 const unsigned int col, const unsigned int colLen) const
    {
        Matrix result;
        return GetMatrixSpace(row,rowLen,col,colLen,result);
    }
    inline Matrix GetMatrixSpace(const unsigned int row, const unsigned int rowLen,
                                 const unsigned int col, const unsigned int colLen,
                                 Matrix& result) const
    {
        if((rowLen>0)&&(colLen>0)){
            return GetMatrix(row,row+rowLen-1,col,col+colLen-1,result);
        }
        return result.Resize(0,0,false);
    }


    /**
     * \brief Assign a value to a matrix row
     * \param value    The value
     * \param row      The row
     */
    inline Matrix& SetRow(const REALTYPE value, const unsigned int row)
    {
        if(row<this->row){
            REALTYPE *src = _+ row*column;
            unsigned int len = column;
            while(len--){
                *(src++) = value;
            }
        }
        return *this;
    }

    /**
     * \brief Assign a vector to a matrix row
     * \param vector   The input vector
     * \param row      The row
     */
    inline Matrix& SetRow(const Vector &vector, const unsigned int row)
    {
        if(row<this->row){
            REALTYPE *src = vector._;
            REALTYPE *dst = _+ row*column;
            unsigned int len = (column<=vector.row?column:vector.row);
            while(len--){
                *(dst++) = *(src++);
            }
        }
        return *this;
    }

    /**
     * \brief Assign a value to a matrix column
     * \param value    The value
     * \param row      The row
     */
    inline Matrix& SetColumn(const REALTYPE value, const unsigned int col)
    {
        if(col<this->column){
            REALTYPE *src = _ + col;
            unsigned int len = row;
            while(len--){
                *src = value;
                src += column;
            }
        }
        return *this;
    }
    /**
     * \brief Assign a vector to a matrix column
     * \param vector   The input vector
     * \param col      The column
     */
    inline Matrix& SetColumn(const Vector &vector, const unsigned int col)
    {
        if(col<this->column){
            REALTYPE *src = vector._;
            REALTYPE *dst = _+ col;
            unsigned int len = (row<=vector.row?row:vector.row);
            while(len--){
                *dst = *(src++);
                dst += column;
            }
        }
        return *this;
    }

  /**
   * \brief Assign a matrix to the current matrix rows
   * \param vector   The input matrix
   * \param row      The starting row
   */
  inline Matrix& SetRowSpace(const Matrix &matrix, const unsigned int row)
  {
    if(row<this->row){
      const unsigned int ki = (column<=matrix.column?column:matrix.column);
      const unsigned int kj = (row+matrix.row<=this->row?row+matrix.row:this->row);
      for (unsigned int j = row; j < kj; j++)
        for (unsigned int i = 0; i < ki; i++)
          _[j*column+i] = matrix._[(j-row)*matrix.column+i];
    }
    return *this;
  }

  /**
   * \brief Assign a matrix to the current matrix columns
   * \param vector   The input matrix
   * \param col      The starting column
   */
  inline Matrix& SetColumnSpace(const Matrix &matrix, const unsigned int col)
  {
    if(col<this->column){
      const unsigned int kj = (row<=matrix.row?row:matrix.row);
      const unsigned int ki = (col+matrix.column<=this->column?col+matrix.column:this->column);
      for (unsigned int j = 0; j < kj; j++)
        for (unsigned int i = col; i < ki; i++)
          _[j*column+i] = matrix._[j*matrix.column+(i-col)];
    }
    return *this;
  }


  inline Matrix GetRowSpace(const Vector& ids, Matrix &result) const
  {
    int s = ids.Size();
    IndicesVector id;
    for(int i=0;i<s;i++) id.push_back(int(ROUND(ids.At(i))));
    return GetRowSpace(id,result);
  }

  /**
   * \brief Get a matrix spanning several rows of the matrix
   * \param ids      The indices of the desired rows
   * \return         The resulting matrix
   */
  inline Matrix GetRowSpace(const IndicesVector& ids) const
  {
    Matrix result(ids.size(),column);
    return GetRowSpace(ids,result);
  }

  inline Matrix& GetRowSpace(const IndicesVector& ids, Matrix &result) const
  {
    const unsigned int k=ids.size();
    result.Resize(k,column,false);
    for(unsigned int i=0;i<k;i++){
      const unsigned int g      = ids[i];
      const unsigned int offset = i*column;
      if(g<row){
        for(unsigned int j=0;j<column;j++)
          result._[offset+j] = _[g*column+j];
      }else{
        for(unsigned int j=0;j<column;j++)
          result._[offset+j] = R_ZERO;
      }
    }
    return result;
  }

  inline Matrix GetColumnSpace(const Vector& ids, Matrix &result) const
  {
    int s = ids.Size();
    IndicesVector id;
    for(int i=0;i<s;i++) id.push_back(int(ROUND(ids.At(i))));
    return GetColumnSpace(id,result);
  }
  /**
   * \brief Get a matrix spanning several columns of the matrix
   * \param ids      The indices of the desired columns
   * \return         The resulting matrix
   */
  inline Matrix GetColumnSpace(const IndicesVector& ids) const
  {
    Matrix result(row,ids.size());
    return GetColumnSpace(ids,result);
  }

  inline Matrix& GetColumnSpace(const IndicesVector& ids, Matrix &result) const
  {
    const unsigned int k=ids.size();
    result.Resize(row,k);
    for(unsigned int i=0;i<k;i++){
      const unsigned int g = ids[i];
      if(g<column){
        for(unsigned int j=0;j<row;j++)
          result._[j*k+i] = _[j*column+g];
      }else{
        for(unsigned int j=0;j<row;j++)
          result._[j*k+i] = R_ZERO;
      }
    }
    return result;
  }

  inline Matrix GetMatrixSpace(const Vector& rowIds, const Vector& colIds, Matrix &result) const
  {
    int r = rowIds.Size();
    int c = colIds.Size();
    IndicesVector idr,idc;
    for(int i=0;i<r;i++) idr.push_back(int(ROUND(rowIds.At(i))));
    for(int i=0;i<c;i++) idc.push_back(int(ROUND(colIds.At(i))));
    return GetMatrixSpace(idr,idc,result);
  }
  /**
   * \brief Get a matrix spanning several rows and columns of the matrix
   * \param rowIds      The indices of the desired rows
   * \param colIds      The indices of the desired columns
   * \return            The resulting matrix
   */
  inline Matrix GetMatrixSpace(const IndicesVector& rowIds,const IndicesVector& colIds) const
  {
    Matrix result(rowIds.size(),colIds.size());
    return GetMatrixSpace(rowIds,colIds,result);
  }

  inline Matrix& GetMatrixSpace(const IndicesVector& rowIds,const IndicesVector& colIds, Matrix &result) const
  {
    const unsigned int k1=rowIds.size();
    const unsigned int k2=colIds.size();
    result.Resize(k1,k2);
    for(unsigned int i=0;i<k1;i++){
      const unsigned int g1 = rowIds[i];
      if(g1<row){
        for(unsigned int j=0;j<k2;j++){
          const unsigned int g2 = colIds[j];
          if(g2<column){
            result._[i*k2+j] = _[g1*column+g2];
          }else{
            result._[i*k2+j] = R_ZERO;
          }
        }
      }else{
        for(unsigned int j=0;j<k2;j++)
          result._[i*k2+j] = R_ZERO;
      }
    }
    return result;
  }

  /**
   * \brief Set a specified column set of the matrix with columns of the source matrix
   * \param ids      The indices of the desired columns
   * \param source   The source matrix
   * \return         The resulting matrix
   */
  inline Matrix& SetColumnSpace(const IndicesVector& ids, const Matrix &source)
  {
    const unsigned int k      = MIN(ids.size(),source.column);
    const unsigned int r      = MIN(row,source.row);
    for(unsigned int i=0;i<k;i++){
      const unsigned int g = ids[i];
      if(g<column){
        for(unsigned int j=0;j<r;j++)
          _[j*column+g] = source._[j*source.column+i];
      }
    }
    return *this;
  }


  inline Matrix& InsertSubRow(unsigned int startRow, unsigned int startColumn,
                              const Matrix& matrix,
                              unsigned int matrixRow,
                              unsigned int matrixStartColumn, unsigned int matrixColumnLength){

    // Check submatrix boundaries
    if((matrixRow         >= matrix.row)||
       (matrixStartColumn >= matrix.column)) return *this;

    // Check matrix boundaries
    if((startRow    >= row)||
       (startColumn >= column)) return *this;

    if(matrixStartColumn+matrixColumnLength > matrix.column) matrixColumnLength = matrix.column-matrixStartColumn;

    if(startColumn+matrixColumnLength > column) matrixColumnLength = column-startColumn;

    unsigned int rowOffset       = startRow*column;
    unsigned int matrixRowOffset = matrixRow*matrix.column;
    unsigned int colOffset       = startColumn;
    unsigned int matrixColOffset = matrixStartColumn;
    for(unsigned int j=0;j<matrixColumnLength;j++){
      _[rowOffset+colOffset] = matrix._[matrixRowOffset+matrixColOffset];
      colOffset++;
      matrixColOffset++;
    }
    return *this;
  }

  inline Matrix& InsertSubColumn(unsigned int startRow, unsigned int startColumn,
                                 const Matrix& matrix,
                                 unsigned int matrixStartRow, unsigned int matrixRowLength,
                                 unsigned int matrixColumn){

    if((matrixStartRow >= matrix.row)||
       (matrixColumn   >= matrix.column)) return *this;
    if((startRow    >= row)||
       (startColumn >= column)) return *this;

    if(matrixStartRow+   matrixRowLength    > matrix.row)    matrixRowLength    = matrix.row   -matrixStartRow;

    if(startRow+   matrixRowLength    > row)    matrixRowLength    = row   -startRow;

    unsigned int rowOffset       = startRow*column;
    unsigned int matrixRowOffset = matrixStartRow*matrix.column;
    for(unsigned int i=0;i<matrixRowLength;i++){
      _[rowOffset+startColumn] = matrix._[matrixRowOffset+matrixColumn];
      rowOffset       +=column;
      matrixRowOffset +=matrix.column;
    }
    return *this;
  }

  inline Matrix& InsertSubMatrix(unsigned int startRow, unsigned int startColumn,
                                 const Matrix& matrix,
                                 unsigned int matrixStartRow, unsigned int matrixRowLength,
                                 unsigned int matrixStartColumn, unsigned int matrixColumnLength){

    // Check submatrix boundaries
    if((matrixStartRow    >= matrix.row)||
       (matrixStartColumn >= matrix.column)) return *this;

    // Check matrix boundaries
    if((startRow    >= row)||
       (startColumn >= column)) return *this;

    if(matrixStartRow+   matrixRowLength    > matrix.row)    matrixRowLength    = matrix.row   -matrixStartRow;
    if(matrixStartColumn+matrixColumnLength > matrix.column) matrixColumnLength = matrix.column-matrixStartColumn;

    if(startRow+   matrixRowLength    > row)    matrixRowLength    = row   -startRow;
    if(startColumn+matrixColumnLength > column) matrixColumnLength = column-startColumn;

    unsigned int rowOffset       = startRow*column;
    unsigned int matrixRowOffset = matrixStartRow*matrix.column;
    for(unsigned int i=0;i<matrixRowLength;i++){
      unsigned int colOffset       = startColumn;
      unsigned int matrixColOffset = matrixStartColumn;
      for(unsigned int j=0;j<matrixColumnLength;j++){
        _[rowOffset+colOffset] = matrix._[matrixRowOffset+matrixColOffset];
        colOffset++;
        matrixColOffset++;
      }
      rowOffset       +=column;
      matrixRowOffset +=matrix.column;
    }
    return *this;
  }

    inline Matrix operator - () const
    {
        Matrix result(row,column,false);
        REALTYPE *src = _;
        REALTYPE *dst = result._;
        unsigned int len = row*column;
        while(len--){
            *(dst++) = -(*(src++));
        }
        return result;
    }
    inline Matrix& SMinus()
    {
        REALTYPE *src = _;
        unsigned int len = row*column;
        while(len--){
            *(src) = -(*(src));
            src++;
        }
        return *this;
    }

    inline virtual Matrix& operator = (const Matrix &matrix)
    {
        return Set(matrix);
    }

    inline Matrix& operator += (const Matrix &matrix)
    {
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst = _;
        REALTYPE *src = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) += *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) += *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) += *(src++);
                }
                dst += colDif;
            }
        }
        return *this;
    }

    inline Matrix& operator -= (const Matrix &matrix)
    {
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst = _;
        REALTYPE *src = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) -= *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) -= *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) -= *(src++);
                }
                dst += colDif;
            }
        }
        return *this;
    }

    /// matrix multiplication
    inline Matrix& operator *= (const Matrix &matrix){
        Matrix tmp;
        Mult(matrix,tmp);
        return Swap(tmp);
    }

    /// Element-wise multiplication
    inline Matrix& operator ^= (const Matrix &matrix)
    {
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst = _;
        REALTYPE *src = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) *= *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) *= *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) *= *(src++);
                }
                dst += colDif;
            }
        }
        return *this;
    }

    
    /* Row wise multiplication by a vector */

    inline Matrix& operator ^= (Vector vec)
    {
    
      for (unsigned int j = 0; j < row; j++)
	{
	  REALTYPE scalar = vec(j);
	  for (unsigned int i = 0; i < column; i++)
	    _[j*column+i] *= scalar;
	}
      return *this;
    }


     
    /* Row wise division by a vector */

    inline Matrix& operator /= (Vector vec)
    {
    
      for (unsigned int j = 0; j < row; j++)
	{
	  REALTYPE scalar = R_ONE/vec(j);
	  for (unsigned int i = 0; i < column; i++)
	    _[j*column+i] *= scalar;
	}
      return *this;
    }



    /// Element-wise division
    inline Matrix& operator /= (const Matrix &matrix)
    {
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst = _;
        REALTYPE *src = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) /= *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) /= *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) /= *(src++);
                }
                dst += colDif;
            }
        }
        return *this;
    }

    inline Matrix& operator += (REALTYPE scalar)
    {
        REALTYPE *src = _;
        unsigned int len = row*column;
        while(len--){
            *(src++) += scalar;
        }
        return *this;
    }

    inline Matrix& operator -= (REALTYPE scalar)
    {
        REALTYPE *src = _;
        unsigned int len = row*column;
        while(len--){
            *(src++) -= scalar;
        }
        return *this;
    }

    inline Matrix& operator *= (REALTYPE scalar)
    {
        REALTYPE *src = _;
        unsigned int len = row*column;
        while(len--){
            *(src++) *= scalar;
        }
        return *this;
    }

    inline Matrix& operator /= (REALTYPE scalar)
    {
        scalar = R_ONE/scalar;
        return *(this) *= scalar;
    }

    inline Matrix operator + (const Matrix &matrix) const
    {
        Matrix result(row,column,false);
        return Add(matrix,result);
    }
    inline Matrix operator - (const Matrix &matrix) const
    {
        Matrix result(row,column,false);
        return Sub(matrix,result);
    }
    /// Element-wise multiplication
    inline Matrix operator ^ (const Matrix &matrix) const
    {
        Matrix result(row,column,false);
        return PMult(matrix,result);
    }
    inline Matrix operator / (const Matrix &matrix) const
    {
        Matrix result(row,column,false);
        return PDiv(matrix,result);
    }

    inline Matrix& ScaleAddTo(REALTYPE scale, Matrix &result) const
    {
        const unsigned int kj = (row<=result.row?row:result.row);
        REALTYPE *dst = result._;
        REALTYPE *src = _;
        if(column==result.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) += *(src++) * scale;
            }
        }else if(result.column < column){
            unsigned int colLen = result.column;
            unsigned int colDif = column-result.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) += *(src++) * scale;
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = column;
            unsigned int colDif = result.column - column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) += *(src++) * scale;
                }
                dst += colDif;
            }
        }
        return result;
    }

    inline Matrix& Add(const Matrix &matrix, Matrix &result) const
    {
        result.Resize(row,column,false);
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst   = result._;
        REALTYPE *src0  = _;
        REALTYPE *src   = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) = *(src0++) + *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) + *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) + *(src++);
                }
                rowLen = colDif;
                while(rowLen--){
                    *(dst++) = *(src0++);
                }
            }
        }
        if(kj!=row){
            unsigned int len = column*(kj-matrix.row);
            while(len--){
                *(dst++) = *(src0++);
            }
        }
        return result;
    }
    inline Matrix& Sub(const Matrix &matrix, Matrix &result) const
    {
        result.Resize(row,column,false);
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst   = result._;
        REALTYPE *src0  = _;
        REALTYPE *src   = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) = *(src0++) - *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) - *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) - *(src++);
                }
                rowLen = colDif;
                while(rowLen--){
                    *(dst++) = *(src0++);
                }
            }
        }
        if(kj!=row){
            unsigned int len = column*(kj-matrix.row);
            while(len--){
                *(dst++) = *(src0++);
            }
        }
        return result;
    }
    /// Element-wise multiplication
    inline Matrix& PMult(const Matrix &matrix, Matrix &result) const
    {
        result.Resize(row,column,false);
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst   = result._;
        REALTYPE *src0  = _;
        REALTYPE *src   = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) = *(src0++) * *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) * *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) * *(src++);
                }
                rowLen = colDif;
                while(rowLen--){
                    *(dst++) = *(src0++);
                }
            }
        }
        if(kj!=row){
            unsigned int len = column*(kj-matrix.row);
            while(len--){
                *(dst++) = *(src0++);
            }
        }
        return result;
    }
    /// Element-wise division
    inline Matrix& PDiv(const Matrix &matrix, Matrix &result) const
    {
        result.Resize(row,column,false);
        const unsigned int kj = (row<=matrix.row?row:matrix.row);
        REALTYPE *dst   = result._;
        REALTYPE *src0  = _;
        REALTYPE *src   = matrix._;
        if(column==matrix.column){
            unsigned int len = column*kj;
            while(len--){
                *(dst++) = *(src0++) / *(src++);
            }
        }else if(column < matrix.column){
            unsigned int colLen = column;
            unsigned int colDif = matrix.column-column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) / *(src++);
                }
                src += colDif;
            }
        }else{
            unsigned int colLen = matrix.column;
            unsigned int colDif = column - matrix.column;
            while(colLen--){
                unsigned int rowLen = kj;
                while(rowLen--){
                    *(dst++) = *(src0++) / *(src++);
                }
                rowLen = colDif;
                while(rowLen--){
                    *(dst++) = *(src0++);
                }
            }
        }
        if(kj!=row){
            unsigned int len = column*(kj-matrix.row);
            while(len--){
                *(dst++) = *(src0++);
            }
        }
        return result;
    }


    inline Matrix operator + (REALTYPE scalar) const
    {
        Matrix result(row,column,false);
        return Add(scalar,result);
    }
    inline Matrix operator - (REALTYPE scalar) const
    {
        Matrix result(row,column,false);
        return Sub(scalar,result);
    }
    inline Matrix operator * (REALTYPE scalar) const
    {
        Matrix result(row,column,false);
        return Mult(scalar,result);
    }
    inline Matrix operator / (REALTYPE scalar) const
    {
        Matrix result(row,column,false);
        return Div(scalar,result);
    }

    inline Matrix& Add(REALTYPE scalar, Matrix& result) const
    {
        result.Resize(row,column,false);
        REALTYPE *src = _;
        REALTYPE *dst = result._;
        unsigned int len = row*column;
        while(len--){
            *(dst++) = *(src++) + scalar;
        }
        return result;
    }
    inline Matrix& Sub(REALTYPE scalar, Matrix& result) const
    {
        return Add(-scalar,result);
    }
    inline Matrix& Mult(REALTYPE scalar, Matrix& result) const
    {
        result.Resize(row,column,false);
        REALTYPE *src = _;
        REALTYPE *dst = result._;
        unsigned int len = row*column;
        while(len--){
            *(dst++) = *(src++) * scalar;
        }
        return result;
    }
    inline Matrix& Div(REALTYPE scalar, Matrix& result) const
    {
        return Mult(R_ONE/scalar,result);
    }

    inline bool operator == (const Matrix& matrix) const
    {
        if((row!=matrix.row)||(column!=matrix.column)) return false;
        REALTYPE *src = _;
        REALTYPE *dst = matrix._;
        unsigned int len = row*column;
        while(len--){
            if( *(dst++) != *(src++) ) return false;
        }
        return true;
    }

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

    /// Matrix multiplication against a vector
    inline Vector operator * (const Vector &vector) const
    {
        Vector result(row,false);
        return Mult(vector,result);
    }
    /// Matrix multiplication against a vector
    inline Vector Mult(const Vector &vector) const
    {
        Vector result(row,false);
        return Mult(vector,result);
    }
    /// Matrix multiplication against a vector
    inline Vector& Mult(const Vector &vector, Vector &result) const
    {
        result.Resize(row,false);
        const unsigned int ki = (column<=vector.row?column:vector.row);
        unsigned int rowLen = row;
        REALTYPE *dst = result._;
        REALTYPE *srcM = _;
        while(rowLen--){
            REALTYPE *srcV = vector._;
            REALTYPE sum = R_ZERO;
            unsigned int colLen = ki;
            while(colLen--){
                sum += (*(srcM++)) * (*(srcV++));
            }
            srcM += column-ki;
            *(dst++) = sum;
        }
        return result;
    }

    /// Transpose Matrix multiplication against a vector
    inline Vector TransposeMult(const Vector &vector) const
    {
        Vector result(column,false);
        return TransposeMult(vector,result);
    }

    /// Transpose Matrix multiplication against a vector
    inline Vector& TransposeMult(const Vector &vector, Vector &result) const
    {
        result.Resize(column,false);
        result.Zero();
        const unsigned int ki = (row<vector.row?row:vector.row);

        REALTYPE *srcV = vector._;
        REALTYPE *srcM = _;
        unsigned int rowLen = ki;
        while(rowLen--){
            REALTYPE *dst = result._;
            unsigned int colLen = column;
            while(colLen--){
                *(dst++) += (*(srcM++)) * (*(srcV));
            }
            srcV++;
        }
        return result;
    }


    inline Matrix& SAddToRow(const Vector &vector){
        const unsigned int k = (column<=vector.row?column:vector.row);
        REALTYPE *ptr = _;
        unsigned int rowLen = row;
        while(rowLen--){
            REALTYPE *ptr2 = ptr;
            REALTYPE *ptrv = vector._;
            unsigned int colLen = k;
            while(colLen--){
                *(ptr2++) += *(ptrv++);
            }
            ptr += column;
        }
        return *this;
    }

    inline Matrix& SAddToColumn(const Vector &vector){
        REALTYPE *ptr = _;
        REALTYPE *ptrv = vector._;
        unsigned int rowLen = (row<=vector.row?row:vector.row);
        while(rowLen--){
            unsigned int colLen = column;
            while(colLen--)
                *(ptr++) += *(ptrv);
            ptrv++;
        }
        return *this;
    }


    /// Multiplication of each ith row by the ith element of a vector
    inline Matrix MultRow(const Vector &vector) const {
        Matrix result;
        return MultRow(vector,result);
    }
    inline Matrix& MultRow(const Vector &vector, Matrix& result) const {
        result.Set(*this);
        return result.SMultRow(vector);
    }
    /// Self multiplication of each ith row by the ith element of a vector
    inline Matrix& SMultRow(const Vector &vector)
    {
        REALTYPE *cDst = _;
        REALTYPE *cSrc = vector._;
        unsigned int ki = (row<=vector.row?row:vector.row);
        while(ki--){
            REALTYPE v = *(cSrc++);
            unsigned int len = column;
            while(len--) (*cDst++) *= v;
        }
        if(row>vector.row){
            unsigned int len = column * (row-vector.row);
            while(len--) *(cDst++) = R_ZERO;
        }
        return *this;
    }

    /// Multiplication of each ith column by the ith element of a vector
    inline Matrix MultColumn(const Vector &vector) const {
        Matrix result;
        return MultColumn(vector,result);
    }
    inline Matrix& MultColumn(const Vector &vector, Matrix& result) const {
        result.Set(*this);
        return result.SMultColumn(vector);
    }
    /// Self multiplication of each ith row by the ith element of a vector
    inline Matrix& SMultColumn(const Vector &vector)
    {
        REALTYPE *cDst = _;
        if(column<=vector.row){
            unsigned int len = row;
            while(len--){
                REALTYPE *cSrc = vector._;
                unsigned int colLen = column;
                while(colLen--) *(cDst++) *= *(cSrc++);
            }
        }else{
            unsigned int len = row;
            while(len--){
                REALTYPE *cSrc = vector._;
                unsigned int colLen = vector.row;
                while(colLen--) *(cDst++) *= *(cSrc++);
                colLen = row-vector.row;
                while(colLen--) *(cDst++) = R_ZERO;
            }
        }
        return *this;
    }


    /// Matrix multiplication
    inline Matrix operator * (const Matrix &matrix) const {
        Matrix result;
        return Mult(matrix,result);
    }
    /// Matrix multiplication
    inline Matrix Mult(const Matrix &matrix) const {
        Matrix result;
        return Mult(matrix,result);
    }
    /// Matrix multiplication
    inline Matrix& Mult(const Matrix &matrix, Matrix &result) const
    {
        result.Resize(row,matrix.column,false);
        result.Zero();
        const unsigned int rcol = result.column;
        const unsigned int kk = (column<=matrix.row?column:matrix.row);

        REALTYPE *cP1   = _;
        REALTYPE *eP1   = cP1 + row*column;
        REALTYPE *cD    = result._;

        while(cP1!=eP1){
            REALTYPE *currP1  = cP1;
            REALTYPE *endP1   = currP1 + kk;
            REALTYPE *currP2  = matrix._;
            while(currP1!=endP1){
                REALTYPE *currPD  = cD;
                REALTYPE  curr1   = *currP1;
                REALTYPE *endP2   = currP2 + rcol;
                while(currP2!=endP2){
                    (*currPD++) += curr1 * (*(currP2++));
                }
                currP1++;
            }
            cD  += rcol;
            cP1 += column;
        }
        return result;
    }


    inline Matrix& TransposeMult(const Matrix &matrix) const{
        Matrix result;
        return TransposeMult(matrix,result);
    }
    /// Transpose then Multiply by the matrix :
    // result = this^T * matrix (avoiding a useless call to .Transpose() )
    inline Matrix& TransposeMult(const Matrix &matrix, Matrix &result) const
    {
        //[a0 a1 .. an] [b0 b1 .. bn]
        result.Resize(column,matrix.column,false);
        result.Zero();
        unsigned int kk = (row<=matrix.row?row:matrix.row);
        REALTYPE *cP1   = _;
        REALTYPE *cP2   = matrix._;
        while(kk--){
            REALTYPE *currD  = result._;
            REALTYPE *currP1 = cP1;
            unsigned int len1 = column;
            while(len1--){
                REALTYPE *currP2 = cP2;
                unsigned int len2 = matrix.column;
                while(len2--){
                    *(currD++) += *(currP1) *(*(currP2++));
                }
                currP1++;
            }
            cP2 += matrix.column;
            cP1 += column;
        }
        return result;
    }

    inline Matrix& MultTranspose2(const Matrix &matrix) const{
        Matrix result;
        return MultTranspose2(matrix,result);
    }
    /// Transpose then Multiply by the matrix :
    // result = this^T * matrix (avoiding a useless call to .Transpose() )
    inline Matrix& MultTranspose2(const Matrix &matrix, Matrix &result) const
    {
        result.Resize(row,matrix.row,false);
        //[a0 a1 .. an] [b0 b1 .. bn]
        if(column==matrix.column){
            REALTYPE *cP1   = _;
            REALTYPE *currD = result._;
            unsigned int len1 = row;
            while(len1--){
                REALTYPE *currP2 = matrix._;
                unsigned int len2 = matrix.row;
                while(len2--){
                    REALTYPE *currP1 = cP1;
                    REALTYPE sum=0.0;
                    unsigned int len = column;
                    while(len--)
                        sum += *(currP1++) * (*(currP2++));
                    *(currD++) = sum;
                }
                cP1 += column;
            }
        }else{
            REALTYPE *cP1   = _;
            REALTYPE *currD = result._;
            unsigned int len1 = row;
            const unsigned int kk = (column<=matrix.column?column:matrix.column);
            unsigned int off2 = matrix.column - kk;
            while(len1--){
                REALTYPE *currP2 = matrix._;
                unsigned int len2 = matrix.row;
                while(len2--){
                    REALTYPE *currP1 = cP1;
                    REALTYPE sum=0.0;
                    unsigned int len = kk;
                    while(len--)
                        sum += *(currP1++) * (*(currP2++));
                    *(currD++) = sum;
                    currP2 += off2;
                }
                cP1 += column;
            }
        }
        return result;
    }

    /// Set a diagonal matrix given a vector of diagonal elements
    inline Matrix& Diag(const Vector &vector)
    {
        Resize(vector.row,vector.row,false);
        Zero();
        REALTYPE *src = vector._;
        REALTYPE *dst = _;
        unsigned int len = row;
        while(len--){
            *dst = *(src++);
            dst += column+1;
        }
        return *this;
    }

    /// Set a random matrix with value uniformly distributed between 0 and 1
    inline Matrix& Random(){
        REALTYPE *src = _;
        unsigned int len = row*column;
        while(len--){
            *(src++) = RND(R_ONE);
        }
        return *this;
    }

    /// Return the transpose of a matrix
    inline Matrix Transpose() const
    {
        Matrix result;
        return Transpose(result);
    }
    /// Compute the transpose of a matrix
    inline Matrix& Transpose(Matrix &result) const
    {
        result.Resize(column,row,false);
        REALTYPE *src = _;
        REALTYPE *dst = re