Stupid Matrix class

//  Copyright Nicolas Janin 2004 - 2013.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy
//  at http://www.boost.org/LICENSE_1_0.txt)

#ifndef _SMatrix_HPP_
#define _SMatrix_HPP_

#include<vector>
#include<cassert>
#include<iostream>


/// Simple/Small/Stupid Matrix - A lightweight Matrix class
///
/// This library is designed around a few simple ideas:
/// 1. the API is straightforward and easily extensible.
/// 2. although it performs okay in my tests, it's not designed for HPC
///    or matrices with more than say a million elements.
///    Use a dedicated library (Eigen, Armadillo...) for this kind
///    of work.
///    SMatrix is well suited for embedded devices or when you need
///    quick and dirty matrices in your code.
/// 3. it tries to avoid allocations behind your back to preserve performance
///    and suit tight resource constraints.
///    This means that most operations are performed in place, including arithmetic
///    operations. You have to arrange your calculations in order to allocate
///    your matrices beforehand. There are exceptions that are documented in the API.
///    Moreover, matrices are *not* resized automatically.
///    This prevents spurious allocations while matrix calculations are performed
///    in your loops.
/// 4. size verifications are performed using assertions. Please test your code
///    before releasing it to production !
///
/// A word of caution:
/// Indices are zero based.
/// Also, because most operations are performed in place,
/// SMatrix is *NOT* a direct replacement for Matlab.
/// Speed and memory are of concern more than convenience.
/// For instance, in Matlab
/// A = B + C will allocate matrix A and A will store the result
/// Here, the same effect can be achieved thus:
/// B + C   // equivalent to B = B + C
/// A = B   // copy B into A
///


template <typename T> class SMatrix
{
public:
    /// constructors/destructor

    SMatrix(const size_t l, const size_t c):
        _iL(l), _iC(c), _v(new std::vector<T>(l * c)) {
    }

    SMatrix(const size_t l, const size_t c, const T tab[]):
        _iL(l), _iC(c), _v(new std::vector<T>(l * c)) {
        for( size_t l = 0; l < _iL; l++ )
            for( size_t c = 0; c < _iC; c++ )
                (*_v)[l*_iC + c] = tab[l*_iC + c];
    }

    SMatrix(const SMatrix &rhs):
        _iL(rhs.rows()), _iC(rhs.cols()), _v(new std::vector<T>(_iL * _iC)) {
        (*_v) = *(rhs.getv());
    }

    ~SMatrix(){ delete _v; }

    /// @brief accessors
    inline T operator()(const size_t l, const size_t c) const {
        assert(l < _iL && c < _iC);
        return (*_v)[l * _iC + c];
    }

    inline T& operator()(const size_t l, const size_t c) {
        assert(l < _iL && c < _iC);
        return  (*_v)[l * _iC + c];
    }

    /// @brief dimensions
    inline size_t rows() const { return _iL; }
    inline size_t cols() const { return _iC; }

    ///  class operations

    /// @brief change the dimensions of the matrix
    /// Warning: reallocates !
    void resize(const size_t rows, const size_t cols) {
        _iL = rows;
        _iC = cols;
        _v->resize(rows, cols);
    }

    /// @brief  assignment operator. Copies the matrix.
    T& operator=(const T& rhs) {
        if (&rhs != this) {
            assert(rhs.rows() == _iL && rhs.cols() == _iC);
            //if(rhs.rows() != _iL || rhs.cols() != _iC)
            //  resize(rhs.rows(), rhs.cols());
            _v = rhs.getv();
        }
        return *this;
    }

    void swap_rows(const size_t l1, const size_t l2){
        // Todo
    }

    /// @brief fill with the scalar s
    inline size_t fill(const T s) {
        size_t nbelem(_iL * _iC);
        _v->assign(nbelem + 1, s);
        return nbelem;
    }

    /// @brief submatrix
    size_t submatrix(SMatrix &sub, const size_t l1, const size_t c1,
            const size_t l2, const size_t c2) const {
        assert(l2 < _iL && l1 <= l2);
        assert(c2 < _iC && c1 <= c2);
        const size_t dim1 = l2 - l1 + 1;
        const size_t dim2 = c2 - c1 + 1;
        for(size_t l = l1, lsub = 0; l <= l2; ++l, ++lsub)
            for(size_t c = c1, csub = 0; c <= c2; ++c, ++csub)
                sub(lsub, csub) = (*_v)[l * _iC + c];
        return dim1 * dim2;
    }

    /// @brief return a submatrix(1,0) with the line l
    inline size_t row(SMatrix &row, const size_t l) const {
        assert(l < _iL);
        return submatrix(row, l, 0, l, _iC - 1);
    }

    /// @brief return a submatrix(0,1) with the column c
    inline size_t column(SMatrix &col, const size_t c) const {
        assert(c < _iC);
        return submatrix(col, 0, c, _iL - 1, c);
    }

    /// @brief transpose the matrix in place
    /// warning: temporary allocation for square matrices > 512
    SMatrix& tr() {
        /*if (_iL == _iC && _iL > 512){
            std::vector<T> *v2 = new std::vector<T>(*_v);
            this->blockTranspose(*v2, 0, _iL, 0, _iC);
            delete v2;
        }
        else
           */
            this->transpose();
        return *this;
    }

    ///  scalar operations
    SMatrix& operator+(const T s){
        for (size_t ij = 0; ij < _iC * _iL; ++ij) (*_v)[ij] += s;
        return *this;
    }

    SMatrix& operator-(const T s){
        for (size_t ij = 0; ij < _iC * _iL; ++ij) (*_v)[ij] -= s;
        return *this;
    }

    SMatrix& operator*(const T s){
        for (size_t ij = 0; ij < _iC * _iL; ++ij) (*_v)[ij] *= s;
        return *this;
    }

    SMatrix& operator/(const T s){
        for (size_t ij = 0; ij < _iC * _iL; ++ij) (*_v)[ij] /= s;
        return *this;
    }

    // matrix to matrix operations
    SMatrix& operator+(const SMatrix &rhs){
        assert(_iL == rhs.rows() && _iC == rhs.cols());
        std::vector<T>* rhs_v = rhs.getv();
        for (size_t ij = 0; ij < _iC * _iL; ++ij) (*_v)[ij] += (*rhs_v)[ij];
        return *this;
    }

    SMatrix& operator-(const SMatrix &rhs){
        assert(_iL == rhs.rows() && _iC == rhs.cols());
        std::vector<T>* rhs_v = rhs.getv();
        for (size_t ij = 0; ij < _iC * _iL; ++ij) (*_v)[ij] -= (*rhs_v)[ij];
        return *this;
    }

    /// @brief in place dot/scalar product, naive O(n^3) algorithm
    /// warning: temporary stack allocation
    // Could be accelerated with OpenMP
    SMatrix& dot(const SMatrix &b){
        assert(b.rows() == _iC);
        size_t bcols = b.cols();
        std::vector<T> prod(_iL * bcols);
        for (size_t i = 0; i < _iL; ++i){
            // Swap j and k loops for cache friendliness
            for (size_t k = 0; k < _iC; ++k){
                const double aik = (*_v)[i * _iC + k];
                for (size_t j = 0; j < bcols; ++j)
                    prod[i * bcols + j] += aik * b(k, j);
            }
        }
        resize(_iL, b.cols());
        *_v = prod;
        return *this;
    }


    /// @brief apply the function f(arg) to each matrix element
    template<typename ret>
    void apply(ret (*f)(T)) {
        typename std::vector<T>::iterator it = _v->begin();
        while(it != _v->end()) {
            *it = (*f)(*it);
            ++it;
        }
    }

    /// @brief apply the function f(arg1, arg2) to each matrix element
    /// arg2 must have the exact argument type, no implicit conversion is possible
    template<typename ret, typename A>
    void apply(ret (*f)(T, A), A arg2) {
        typename std::vector<T>::iterator it = _v->begin();
        while(it != _v->end()) {
            *it = (*f)(*it, arg2);
            ++it;
        }
    }


protected:
    size_t _iL, _iC;            // number of lines, columns
    std::vector<T>  *_v;        // heap based matrix

    SMatrix() {}

    inline std::vector<T>* getv() const { return _v; }

    // transpose a MxN matrix
    SMatrix& transpose() {
        std::vector<T> v2((*_v));
        (*_v).clear();
        for (size_t l = 0; l < _iC; ++l) {
            for (size_t c = 0; c < _iL; ++c) {
                (*_v).push_back(v2[c * _iC + l]);
            }
        }
        std::swap(_iL, _iC);
        return *this;
    }

    // fast block algorithm for square matrices
    SMatrix& blockTranspose(const std::vector<T> &v2,
            const size_t i0, const size_t i1, const size_t j0, const size_t j1) {
        const size_t di = i1 - i0;
        const size_t dj = j1 - j0;
        const size_t LEAFSIZE = 32;
        // the idea here is to recursively subdivide the matrix into blocks
        // that easily fit into cache
        if (di >= dj && di > LEAFSIZE) {
            const size_t im = (i0 + i1) / 2;
            blockTranspose(v2, i0, im, j0, j1); // first half
            blockTranspose(v2, im, i1, j0, j1); // second half
        }
        else if (dj > LEAFSIZE) {
            const size_t jm = (j0 + j1) / 2;
            blockTranspose(v2, i0, i1, j0, jm);
            blockTranspose(v2, i0, i1, jm, j1);
        }
        else {
            for (size_t i = i0; i < i1; i++)
                for (size_t j = j0; j < j1; j++){
                    (*_v)[j * i1 + i] = v2[i * j1 + j];
                }
        }

        return *this;
    }


};

#endif