matrix.h

Go to the documentation of this file.

//-------------------------------------------------------------
// Attila Michael Bilgic, 1/97
// last update 97/10/04 AMB
//             98/11/03 KG
// $Id: matrix.h,v 1.57.2.142 2022/11/05 21:31:35 garloff Exp $
//-------------------------------------------------------------

#ifndef TBCI_MATRIX_H
#define TBCI_MATRIX_H  

#include "tbci/basics.h"
#include "tbci/vector.h"
//#include "fs_vector.h"
#include "tbci/matrix_sig.h"

#ifdef DEBUG_THREAD
# include <cstdio>
#endif

// Avoid -fguiding-decls
#if !defined(NO_GD) && !defined(AUTO_DECL)
# include "tbci/matrix_gd.h"
#endif

NAMESPACE_TBCI


#ifdef HAVE_GCC295_FRIEND_BUG
# define _VEC getvec()
# define _ENDVEC getendvec()
# define _DIM size()
# define _ROW rows()
# define _COL columns()
# define _FAC getfac()
#else
# define _VEC vec
# define _ENDVEC endvec
# define _DIM dim
# define _ROW row
# define _COL col
# define _FAC fac
#endif


#ifndef TBCI_DISABLE_EXCEPT
//# include "except.h" in basics.h

class MatErr : public NumErr
{
    public:
        MatErr()
        : NumErr(/*errtext =*/ "Error in Matrix library") {}
        MatErr(const char* t, const long i = 0) 
        : NumErr(t, i) {}
        MatErr(const MatErr& me)
        : NumErr(me) {}
        virtual ~MatErr() throw() {}
};
#endif

#ifdef PRAGMA_I
# pragma interface "matrix.h"
#endif

enum rowcolvec { rowvec = 0, colvec = 1 };

template <typename T> class   Matrix;
template <typename T> class  TMatrix;
template <typename T> class TSMatrix;
template <typename T> class Mat_Brack;
template <typename T> class BdMatrix;
template <typename T> class Tensor;

template <typename T>
void do_mat_vec_mult (const unsigned start, const unsigned end,
                      TVector<T> *res, const Matrix<T> *mat,
                      const Vector<T> *vec);

template <typename T>
void do_mat_tsv_mult (const unsigned start, const unsigned end,
                        TVector<T> *res, const Matrix<T> *mat,
                        const TSVector<T> *vec);

template <typename T>
void do_mat_vec_transmult (const unsigned start, const unsigned end,
                           TVector<T> *res, const Matrix<T> *mat,
                           const Vector<T> *vec);

 /*  TODO: Adjust dynamically (or at least depending on OPT target). */
#if defined(SMP) && !defined(SMP_MATSLICE)
# define SMP_MATSLICE 4096
#endif
#ifdef SMP
# define SMP_MATSLICE2 (SMP_MATSLICE/sizeof(T))
#endif

//-----------------------------------------------------
//-----------------------------------------------------
template <typename T>
class TMatrix : public Matrix_Sig<T>
{
    protected:
        typedef T mat_t /*TALIGN(sizeof(T))*/;
        typedef T* Tptr /*TALIGN(sizeof(T))*/;
        
        unsigned long dim;
        unsigned int row, col:31;
        mutable unsigned int freeable:1;
        T **mat, *vec, *endvec;
        
        int set_ptrs ();

    public:
#if 1 //defined(HAVE_GCC295_FRIEND_BUG) || defined(HAVE_PROMOTION_BUG)
        // allow access to vec and endvec
        T* getvec () const     { return vec; }
        T* getendvec () const  { return endvec; }
#endif
        typedef T value_type;
        typedef T element_type;
        typedef T aligned_value_type TALIGN(MIN_ALIGN2);
        
        friend class Matrix<T>;
        friend class TSMatrix<T>;
        friend class Tensor<T>;
        friend class Mat_Brack<T>;
        friend class Vector<T>;
        friend class TVector<T>;
        friend class BdMatrix<T>;
        
        friend NOINST TMatrix<T> /*FRIEND_TBCI2__*/ LU_solve FGD (const BdMatrix<T>&, const Matrix<T>&); 
        friend NOINST TMatrix<T> /*FRIEND_TBCI2__*/ lu_solve FGD (BdMatrix<T>&, const Matrix<T>&);
        friend NOINST TMatrix<T> /*FRIEND_TBCI2__*/ LU_invert FGD (const BdMatrix<T>&);
        friend NOINST TMatrix<T> /*FRIEND_TBCI2__*/ lu_invert FGD (BdMatrix<T>&);
        
        friend void FRIEND_TBCI2__ do_mat_vec_mult FGD (const unsigned start, const unsigned end,
                                                TVector<T> *res, const Matrix<T> *mat,
                                                const Vector<T> *vec) HOT;
        friend void FRIEND_TBCI2__ do_mat_tsv_mult FGD (const unsigned start, const unsigned end,
                                                TVector<T> *res, const Matrix<T> *mat,
                                                const TSVector<T> *tsv) HOT;
        friend void FRIEND_TBCI2__ do_mat_vec_transmult_exact FGD (const unsigned start, const unsigned end,
                                                     TVector<T> *res, const Matrix<T> *mat,
                                                     const Vector<T> *vec) HOT;
        friend void FRIEND_TBCI2__ do_mat_vec_transmult FGD (const unsigned start, const unsigned end,
                                                     TVector<T> *res, const Matrix<T> *mat,
                                                     const Vector<T> *vec) HOT;

    //protected:
    public:
        explicit TMatrix (const unsigned = 0);
        TMatrix (const unsigned, const unsigned);
        TMatrix (const T&, const unsigned, const unsigned);
        explicit TMatrix (const Vector<T>&, const enum rowcolvec = colvec);
        TMatrix (const TMatrix<T>&) HOT;
        TMatrix (TSMatrix<T>);
        TMatrix (const Matrix<T>&) HOT;
        void real_destroy ();
        void mark_destroy() const;
        
        // allow instantiation (Matrix_Sig)
        /*virtual*/ static const char* mat_info ()   { return ("TMatrix"); }
        
    //public:
        // empty destructor: TMatrix will never be destroyed but explicitly !!!
        ~TMatrix ()  
        { if (freeable) real_destroy(); }

#ifndef HAVE_PROMOTION_BUG 
# ifndef HAVE_GCC295_TMPLFRNDCLS_BUG
        // Promotion (only explicit)
        template <typename U> friend class Matrix;
        template <typename U> friend class TMatrix;
# endif
        template <typename U> explicit TMatrix (const Matrix<U>& m)
          : dim (m._DIM), row (m._ROW), col (m._COL),
            freeable (0),
            mat (dim>0?NEW(Tptr,row):0), vec (dim>0?NEW(T,dim):0)
         { set_ptrs (); T *t = vec; U *u =m._VEC; 
           while (t < endvec) *t++ = *u++; }

        template <typename U> explicit TMatrix (const TMatrix<U>& tm)
          : dim (tm._DIM), row (tm._ROW), col (tm._COL),
            freeable (0),
            mat (dim>0?NEW(Tptr,row):0), vec (dim>0?NEW(T,dim):0)
         { set_ptrs (); T *t = vec; U *u =tm._VEC; 
           while (t < endvec) *t++ = *u++; }
#endif
    //public:
        TMatrix<T>& operator = (const Matrix<T>&) HOT;
        TMatrix<T>& operator = (const TMatrix<T>&) HOT;
        TMatrix<T>& operator = (TSMatrix<T>);
        TMatrix<T>& operator = (const T&);
        TMatrix<T>& alias (const TMatrix<T>& m);

        TMatrix<T>& operator += (TMatrix<T>);           // arg will be destroyed
        TMatrix<T>& operator += (const Matrix<T>&);     // arg wont be destroyed
        TMatrix<T>& operator += (const T&);
        TMatrix<T>& operator += (TSMatrix<T>);          // arg will be destroyed
        TMatrix<T>& operator -= (TMatrix<T>);
        TMatrix<T>& operator -= (const Matrix<T>&);
        TMatrix<T>& operator -= (const T&);
        TMatrix<T>& operator -= (TSMatrix<T>);
        
        TSMatrix<T> operator *= (const T&);
        TSMatrix<T> operator /= (const T&);
        
        // operations
        TMatrix<T>& operator - ();
        
        TMatrix<T>& operator + (TMatrix<T>);            // arg will be destroyed
        TMatrix<T>& operator + (TSMatrix<T>);           // arg will be destroyed
        TMatrix<T>& operator + (const Matrix<T>&);      // arg wont be destroyed
        TMatrix<T>& operator + (const T&);
        TMatrix<T>& operator - (TMatrix<T>);
        TMatrix<T>& operator - (TSMatrix<T>);
        TMatrix<T>& operator - (const Matrix<T>&);
        TMatrix<T>& operator - (const T&);
        TSMatrix<T> operator * (const T&);
        TSMatrix<T> operator / (const T&);
        
        TMatrix<T> operator * (const Matrix<T>&);
        TMatrix<T> operator * (TMatrix<T>);
        TMatrix<T> operator * (TSMatrix<T>);
        
        TVector<T> operator * (const Vector<T>& v)
          { Matrix<T> m (*this); return (m * v); }
        TVector<T> operator * (TVector <T>& tv)
          { Matrix<T> m (*this); return (m * tv); }
        TVector<T> operator * (const TSVector <T>& tsv)
          { Matrix<T> m (*this); return (m * tsv); }

        // friend operations
#ifndef HAVE_GCC295_FRIEND_BUG
        friend NOINST  TMatrix<T> FRIEND_TBCI__ operator + FGD (const T&, TMatrix<T>);
        friend NOINST  TMatrix<T> FRIEND_TBCI__ operator - FGD (const T&, TMatrix<T>);
        friend NOINST  TMatrix<T> FRIEND_TBCI__ operator + FGD (const T&, const Matrix<T>&);
        friend NOINST  TMatrix<T> FRIEND_TBCI__ operator - FGD (const T&, const Matrix<T>&);
        friend NOINST TSMatrix<T> FRIEND_TBCI__ operator * FGD (const T&, TMatrix<T>);
        friend NOINST TSMatrix<T> FRIEND_TBCI__ operator * FGD (const T&, const Matrix<T>&);
#endif

        TMatrix<T>& swap (TMatrix<T>&);
        TMatrix<T>  transposed_copy() const;
        TMatrix<T>& transpose();

        // Access
        
    //public:
        T operator () (const unsigned int, const unsigned int) const HOT;
        //T& operator () (const unsigned, const unsigned) const;
#ifndef TBCI_NEW_BRACKET
        Mat_Brack<T> operator [] (const unsigned int i) const;
#else   
        T* operator [] (const unsigned int i) const;
#endif
        //      TVector<T> operator [] (const unsigned int i);

        T& setval(const T& val, const unsigned int r, const unsigned int c)
         { return mat[r][c] = val; }
        T& setval (const unsigned r, const unsigned c)
         { return mat[r][c]; }
        
        typename tbci_traits<T>::const_refval_type
           get (const unsigned r, const unsigned c) const  
        { return mat[r][c]; }
        T& set (const T& val, const unsigned r, const unsigned c)
        { return mat[r][c] = val; }
        const T& getcref(const unsigned r, const unsigned c) const
        { return mat[r][c]; }
   
        const T* getrowptr (const unsigned r) const { return mat[r]; }
              T* getrowptr (const unsigned r)       { return mat[r]; }
        
        bool operator == (const Matrix<T>& m);
        bool operator != (const Matrix<T>& m)
          { return !(*this == m); }
        
        bool operator == (TMatrix<T> tm)
          { Matrix<T> m (tm); return (*this == m); }
        bool operator != (TMatrix<T> tm)
          { return !(*this == tm); }
        
        bool operator == (TSMatrix<T>);
        bool operator != (TSMatrix<T> ts)
          { return !(*this == ts); }
        
        //public:
        unsigned int  columns () const { return col; }
        unsigned int  rows ()    const { return row; }
        unsigned long size ()    const { return dim; }

        TVector<T> operator () (const unsigned int) const;
        TVector<T> get_row (const unsigned int) const;
        TVector<T> get_col (const unsigned int) const;

        void set_row (const Vector<T>&, const unsigned int);
        void set_col (const Vector<T>&, const unsigned int);
        // On request of R. Prescott ...
        void set_row_partial (const Vector<T>&, const unsigned int, 
                              const unsigned int);
        void set_col_partial (const Vector<T>&, const unsigned int, 
                              const unsigned int);
#if 0
        void set_row_indexed (const Vector<T>&, const unsigned int, 
                              const BVector<unsigned int>&);
        void set_row_indexed (const Vector<T>&, const unsigned int, 
                              const BVector<unsigned int>&);
        void set_submatrix (const Matrix<T>&,
                            const unsigned int, const unsigned int);
#endif

#if 0
        template <unsigned long dim>
        void set_row (const FS_Vector<dim,T>& v, const unsigned int i)
        {
          BCHKNR(i >= row, MatErr, Illegal row index in set_row, i);
          BCHKNR(dim != col, MatErr, Different sizes in set_row, dim);
          TBCICOPY (mat[i], &v(0), T, dim);
        }
#endif

        TMatrix<T>& resize (const unsigned int, const unsigned int);
        TMatrix<T>& resize (const unsigned int d)  { return this->resize (d, d); }
        TMatrix<T>& resize (const T&, const unsigned int, const unsigned int);
        TMatrix<T>& resize (const TMatrix<T>&);
        TMatrix<T>& cheapdownsizerow (const unsigned);
        TMatrix<T>& fill (const T& = (T)0);
        TMatrix<T>& clear ();
        TMatrix<T>& fill (const Vector<T>&);
        TMatrix<T>& setunit (const T& = (T)1);
        TMatrix<T>& row_expand (const unsigned int r);
        TMatrix<T>& row_expand (const TMatrix<T>& m);
        
        T trace () const;
        
        double fabssqr () const;
        double fabs () const  { return GLBL__ MATH__ sqrt (fabssqr()); }
}; 


template <typename T>
inline void TMatrix<T>::real_destroy ()
{
        if (UNLIKELY(dim)) { 
                TBCIDELETE(T, vec, dim); TBCIDELETE(Tptr, mat, row);
        }
}

template <typename T>
inline void TMatrix<T>::mark_destroy () const
{
        this->freeable = 1;
}


template <typename T>
inline int TMatrix<T>::set_ptrs ()
{
        if (LIKELY(mat && vec)) {
                //for (unsigned long i=0; i<dim; i++) vec[i] = 0;
#if 1
                T* ptr = vec;
                for (REGISTER unsigned i = 0; i < row; ++i) {
                        mat[i] = ptr; ptr += col;
                        //vec[i + i*d] = 1;
                }
                endvec = ptr;
#else
                // Avoid this access in loop, so gcc4 could vectorize it
                // Unfortunately, it still fails, as the loop counter i 
                // is used in the computation
                const unsigned int ROW = row;
                const unsigned int COL = col;
                T* RESTRICT const VEC = vec;
                T** RESTRICT const MAT = mat;
                for (REGISTER unsigned i = 0; i < ROW; ++i)
                        MAT[i] = VEC + i*COL;
                endvec = VEC + ROW*COL;
#endif
                return 0;
        } else {
                if (UNLIKELY(vec)) 
                        TBCIDELETE(T, vec, dim);
                if (UNLIKELY(mat)) 
                        TBCIDELETE(Tptr, mat, row);
                dim = 0; row = 0; col = 0;
                mat = (T**)0; vec = (T*)0; endvec = (T*)0;
                return -1;
        }
}

template <typename T>
inline TMatrix<T>::TMatrix (const unsigned d) 
: dim (((unsigned long)d)*d), row(d), col(d), freeable (0),
  mat (d>0?NEW(Tptr,d):0), vec (d>0?NEW(T,d*d):0)
{
        set_ptrs ();
}


template <typename T>
inline TMatrix<T>::TMatrix (const unsigned r, const unsigned c)
: dim(((unsigned long)r)*c), row (r), col (c), freeable (0),
  mat (r*c>0?NEW(Tptr,r):0), vec (r*c>0?NEW(T,r*c):0)
{
        set_ptrs ();
}

template <typename T>
inline TMatrix<T>::TMatrix (const T& val, const unsigned r, const unsigned c)
: dim(((unsigned long)r)*c), row (r), col (c), freeable (0),
  mat (r*c>0?NEW(Tptr,r):0), vec (r*c>0?NEW(T,r*c):0)
{
        if (LIKELY(!set_ptrs ()))
                TBCIFILL (vec, val, T, dim);
}


template <typename T>
inline TMatrix<T>::TMatrix (const Vector<T>& v, const enum rowcolvec r)
: dim(v.dim), row(r==rowvec?v.dim:1), col(r==rowvec?1:v.dim), 
  freeable(0),
  mat (v.dim?(r==rowvec?NEW(Tptr,1):NEW(Tptr,v.dim)):0),
  vec (v.dim>0?NEW(T,v.dim):0)
{
        if (LIKELY(!set_ptrs ()))
                TBCICOPY (vec, v.vec, T, dim);
}

// data is duplicated
template <typename T>
inline TMatrix<T>::TMatrix (const Matrix<T>& m)
: dim (m.dim), row (m.row), col (m.col), 
  freeable (0),
  mat (m.dim>0?NEW(Tptr,m.row):0), vec (m.dim>0?NEW(T,m.dim):0) 
{
        if (LIKELY(!set_ptrs ()))
                TBCICOPY (vec, m.vec, T, dim);
}


// copy constructor only copies pointers !
template <typename T>
inline TMatrix<T>::TMatrix (const TMatrix<T>& tm)
: dim (tm.dim), row (tm.row), col (tm.col), freeable (0),
  mat (tm.mat), vec (tm.vec), endvec (tm.endvec)
{
        BCHKNR(tm.freeable, MatErr, alias freeable object, (long)&tm);
}

template <typename T>
inline TMatrix<T>& TMatrix<T>::alias (const TMatrix<T>& tm)
{
        BCHKNR(tm.freeable, MatErr, alias freeable object, (long)&tm);
        real_destroy();
        dim = tm.dim; row = tm.row; col = tm.col; freeable = 0; 
        mat = tm.mat; vec = tm.vec; endvec = tm.endvec;
        return *this; 
}

template <typename T>
inline TMatrix<T>::TMatrix (TSMatrix<T> ts)
: dim (ts.dim), row (ts.row), col (ts.col), freeable (0)
{
        ts.eval (); vec = ts.vec; mat = ts.mat; 
        endvec = ts.endvec; ts.mut = false;
}


template <typename T>
inline T TMatrix<T>::operator () (const unsigned int r, const unsigned int c) const
{
        EXPCHK(r>=row, MatErr, Illegal row index, r, mat[0][0]);
        EXPCHK(c>=col, MatErr, illegal col index, c, mat[0][0]);
        mark_destroy ();
        return mat[r][c];
}

//copy
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator = (const Matrix<T>& a)
{
        BCHK(dim != a.dim, MatErr, Different sizes in assignment, a.dim, *this );
        TBCICOPY(vec, a.vec, T, dim);
        return *this;
}

//alias (do we need it?)
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator = (const TMatrix<T>& a)
{
        BCHK(row != a.row || col != a.col, MatErr, Different sizes in assignment, a.dim, *this );
        this->alias(a);
        //freeable = a.freeable;
        return *this;
}

template <typename T>
inline TMatrix<T>& TMatrix<T>::operator = (TSMatrix<T> a)
{
        BCHK(col != a.col || row != a.row, MatErr, Different sizes in assignment, a.dim, *this );
        a.eval (this);
        return *this;
}

//fill
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator = (const T& val)
{
        return this->fill (val);
}


template <typename T>
inline TMatrix<T>& TMatrix<T>::setunit (const T& fac)
{
        // This does only make sense for numbers, so the memset () is OK
        if (LIKELY(dim)) 
                TBCICLEAR (vec, T, dim);
        BCHK(col!=row, MatErr, setunit makes only sense on quadratic matrices, col, *this);
        unsigned iend = MIN(row,col);
        for (REGISTER unsigned i = 0; i < iend; i++) 
                mat[i][i] = fac;
        return *this;
}

template <typename T>
inline TMatrix<T>& TMatrix<T>::clear ()
{
        // Assume 0 is uninitialized or zeroed state
        if (UNLIKELY(dim)) 
                TBCICLEAR (vec, T, dim);
        return *this;
}

template <typename T>
inline void TMatrix<T>::set_row (const Vector<T>& v, const unsigned int r)
{
        BCHKNR(r >= row, MatErr, Illegal row index in set_row, r);
        BCHKNR(v.dim != (unsigned long)col, MatErr, Wrong size vector, v.dim);
        TBCICOPY(mat[r], v.vec, T, v.dim);
}

template <typename T>
inline void TMatrix<T>::set_row_partial (const Vector<T>& v, 
                        const unsigned int r, const unsigned off)
{
        BCHKNR(r >= row, MatErr, Illegal row index in set_row_partial, r);
        BCHKNR(v.dim > (unsigned long)col+off, MatErr, Too large vector, v.dim);
        TBCICOPY(mat[r]+off, v.vec, T, v.dim);
}

template <typename T>
inline void TMatrix<T>::set_col (const Vector<T>& v, const unsigned int c)
{
        BCHKNR(c >= col, MatErr, Illegal col index in set_col, c);
        BCHKNR(v.dim != (unsigned long)row, MatErr, Wrong size vector, v.dim);
        const unsigned ROW = row;
        for (REGISTER unsigned r = 0; r < ROW; ++r) 
                mat[r][c] = v.vec[r];
}

template <typename T>
inline void TMatrix<T>::set_col_partial (const Vector<T>& v, 
                        const unsigned int c, const unsigned off)
{
        BCHKNR(c >= col, MatErr, Illegal col index in set_col, c);
        BCHKNR(v.dim > (unsigned long)row+off, MatErr, Too large vector, v.dim);
        const unsigned ROW = v.dim;
        for (REGISTER unsigned r = 0; r < ROW; ++r) 
                mat[r+off][c] = v.vec[r];
}

template <typename T>
inline TVector<T> TMatrix<T>::get_col (const unsigned int c) const
{
        const unsigned ROW = row;
        TVector<T> v(ROW);
        BCHK(c >= col, MatErr, Illegal col index in get_col, c, v);
        for (REGISTER unsigned j = 0; j < ROW; ++j)
                v.vec[j] = mat[j][c];
        mark_destroy();
        return v;
}


template <typename T>
inline TVector<T> TMatrix<T>::get_row (const unsigned int r) const  
{
        TVector<T> v(col);
        TBCICOPY (v.vec, mat[r], T, col);
        mark_destroy();
        return v; 
}

template <typename T>
inline TVector<T> TMatrix<T>::operator () (const unsigned int i) const
{
        return this->get_row(i);
}

template <typename T>
TMatrix<T>& TMatrix<T>::resize (const unsigned int r, const unsigned int c)
{
        if (LIKELY(r == row && c == col)) 
                return *this;
        if (UNLIKELY(dim)) 
                TBCIDELETE(Tptr, mat, row);
        T* vv = vec; 
        unsigned long dd = dim;
        dim = ((unsigned long)r)*c;
        if (LIKELY(dim)) { 
                mat = NEW(Tptr,r); vec = NEW(T,dim); 
                row = r; col = c;
        } else { 
                mat = (T**)0; vec = (T*)0; endvec = (T*)0; 
        }
        if (!set_ptrs())
                TBCICOPY(vec, vv, T, MIN(dim,dd));
        if (UNLIKELY(dd))
                TBCIDELETE(T, vv, dd);
        return *this;
}

template <typename T>
TMatrix<T>& TMatrix<T>::row_expand (const unsigned int r)
{
        if (LIKELY(r == row))
                return *this;
        dim = ((unsigned long)r)*col;
        if (LIKELY(dim)) { 
                REALLOC (vec, ((unsigned long)row)*col, T, dim);
                TBCIDELETE (Tptr, mat, row);
                mat = NEW (Tptr,r);
        } else { 
                if (UNLIKELY(row*col)) {
                        TBCIDELETE (T, vec, (row*col)); TBCIDELETE (Tptr, mat, row);
                }
                mat = (T**)0; vec = (T*)0; endvec = (T*)0;
        }
        if (LIKELY(mat && vec)) {
                T* ptr = vec;
                for (REGISTER unsigned i=0; i<r; i++) { 
                        mat[i] = ptr; ptr += col; 
                }
                endvec = ptr; row = r;
        } else { // Allocation failure
                dim = 0; row = 0; col = 0;
                mat = (T**)0; vec = (T*)0; endvec = (T*)0;
        }
        return *this;
}

template <typename T>
TMatrix<T>& TMatrix<T>::row_expand (const TMatrix<T>& m)
{
        if (LIKELY(m.dim == 0))
                return *this;
        unsigned long olddim = dim;
        dim += m.dim;
        row += m.row;
        if (LIKELY(dim)) {
                REALLOC (vec, olddim, T, dim);
                TBCIDELETE (Tptr, mat, (row-m.row));
                mat = NEW (Tptr, row);
                if (LIKELY(mat && vec)) {
                        T* ptr = vec;
                        for (REGISTER unsigned i=0; i<row; i++) { 
                                mat[i] = ptr; ptr += col; 
                        }
                        endvec = ptr;
                        for (REGISTER unsigned long l=0; l<m.dim; l++)
                                vec[dim-m.dim+l] = m.vec[l];
                        return *this;
                } else { // Allocation failure
                        if (UNLIKELY(vec)) 
                                TBCIDELETE(T, vec, dim);
                        if (UNLIKELY(mat)) 
                                TBCIDELETE(Tptr, mat, row);
                }
        } else if (olddim) { 
                TBCIDELETE (T, vec, olddim); 
                TBCIDELETE (Tptr, mat, row-m.row); 
        }
        mat = (T**)0; vec = (T*)0; endvec = (T*)0;
        dim = 0; row = 0; col = 0;
        return *this;
}


template <typename T>
TMatrix<T>& TMatrix<T>::resize (const T& v, const unsigned int r, const unsigned int c)
{
        if (LIKELY(r == row && c == col)) {
                TBCIFILL (vec, v, T, dim);
                return *this;
        }
        if (UNLIKELY(dim)) { 
                TBCIDELETE(T, vec, dim); 
                TBCIDELETE(Tptr, mat, row);
        }
        dim = ((unsigned long)r)*c;
        row = r; col = c;
        if (LIKELY(dim)) {
                mat = NEW(Tptr,r); vec = NEW(T,dim); 
                if (!set_ptrs())
                        TBCIFILL(vec, v, T, dim);
        } else {
                mat = (T**)0; vec=(T*)0; endvec =(T*)0;
        }
        return *this;
}

template <typename T>
TMatrix<T>& TMatrix<T>::resize (const TMatrix<T>& m)
{
        if (LIKELY(m.row == row && m.col == col)) {
                TBCICOPY (vec, m.vec, T, dim);
                return *this;
        }
        if (UNLIKELY(dim)) { 
                TBCIDELETE(T, vec, dim); 
                TBCIDELETE(Tptr, mat, row);
        }
        dim = ((unsigned long)m.row)*m.col;
        row = m.row; col = m.col;
        if (LIKELY(dim)) {
                mat = NEW(Tptr,m.row); vec = NEW(T,dim);
                if (!set_ptrs())
                        TBCICOPY(vec, m.vec, T, dim);
        } else {        
                mat = (T**)0; vec = (T*)0; endvec = (T*)0;
        }
        return *this;
}

/* We don't really free space unless it's 0 */
template <typename T>
TMatrix<T>& TMatrix<T>::cheapdownsizerow (const unsigned nr)
{
        if (LIKELY(nr == row))
                return *this;
        BCHK (nr > row, MatErr, cheapdownsize does not upsize, nr, *this);
        if (UNLIKELY(!nr))
                return this->resize (nr, col);
        dim = ((unsigned long)nr) * col; 
        row = nr; endvec = vec + dim;
        return *this;
}

template <typename T>
inline TMatrix<T>& TMatrix<T>::fill (const T& val)
{
        TBCIFILL(vec, val, T, dim);
        return *this;
}

template <typename T>
inline T TMatrix<T>::trace () const
{
        ALIGN3(REGISTER T tr, vec[0], MIN_ALIGN2);
        BCHK (row != col, MatErr, Trace over non quadratic matrix, 0, tr=0);
        for (REGISTER unsigned i = 1; i < row; i++) 
                tr += mat[i][i];
        return tr;
}

template <typename T>
inline TMatrix<T>& TMatrix<T>::fill (const Vector<T>& v)
{
        BCHK (dim != v.dim, MatErr, Vector size does not match matrix size, v.dim, *this);
        TBCICOPY (vec, v.vec, T, v.dim);
        return *this;
}

/************* Arithmetics *****************/ 

// unary -
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator - ()
{
        do_vec_neg<T> (dim, vec);
        return *this;
}

template <typename T>
bool TMatrix<T>::operator == (const Matrix<T>& m)
{
        Matrix<T> th (*this);
        if (LIKELY(row != m.row || col != m.col))
                return false;
        if (LIKELY(vec == m.vec || dim == 0))
                return true;
        if (TBCICOMP (vec, m.vec, T, dim)) 
                return false;
        /* else */ 
        return true;
}

template <typename T>
bool TMatrix<T>::operator == (TSMatrix<T> ts)
{
        Matrix<T> th (*this);
        if (LIKELY(row != ts.row || col != ts.col))
                return (ts.real_destroy(), false);
        if (LIKELY(dim == 0)) { 
                /*ts.real_destroy();*/
                return true; 
        }
        if (LIKELY(ts.fac == (T)1)) {
                if (LIKELY(vec == ts.vec))
                        return true;
                if (TBCICOMP (vec, ts.vec, T, dim)) { 
                        ts.real_destroy (); return false; 
                }
        } else {
                if (LIKELY(vec == ts.vec))
                        return false;
                for (REGISTER T *p1 = vec, *p2 = ts.vec; p1 < endvec; p1++, p2++)
                        if (UNLIKELY(*p1 != *p2 * ts.fac)) {
                                ts.real_destroy (); return false; 
                        }
        }
        ts.real_destroy (); return true;
}


template <typename T>
inline TMatrix<T>& TMatrix<T>::operator += (const Matrix<T>& a)
{
        BCHK(row != a.row, MatErr, number of rows differ in op, a.row, *this );
        BCHK(col != a.col, MatErr, number of cols differ in op, a.col, *this );
        STD_SMP_TEMPLATE2V(vec_add_vec, dim, vec, a.vec);
        return *this;
}
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator -= (const Matrix<T>& a)
{
        BCHK(row != a.row, MatErr, number of rows differ in op, a.row, *this );
        BCHK(col != a.col, MatErr, number of cols differ in op, a.col, *this );
        STD_SMP_TEMPLATE2V(vec_sub_vec, dim, vec, a.vec);
        return *this;
}

template <typename T>
inline TMatrix<T>& TMatrix<T>::operator += (TSMatrix<T> a)
{
        BCHK(row != a.row, MatErr, number of rows differ in op, a.row, *this );
        BCHK(col != a.col, MatErr, number of cols differ in op, a.col, *this );
        STD_SMP_TEMPLATE3VC(vec_add_svc, dim, vec, a.vec, a.fac);
        a.real_destroy(); return *this;
}
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator -= (TSMatrix<T> a)
{
        BCHK(row != a.row, MatErr, number of rows differ in op, a.row, *this );
        BCHK(col != a.col, MatErr, number of cols differ in op, a.col, *this );
        STD_SMP_TEMPLATE3VC(vec_sub_svc, dim, vec, a.vec, a.fac);
        a.real_destroy(); return *this;
}

// Changing a TMatrix to a Matrix destroys it ...
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator += (TMatrix<T> a)
{ return this->operator += (Matrix<T>(a)); }
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator -= (TMatrix<T> a)
{ return this->operator -= (Matrix<T>(a)); }

template <typename T>
inline TMatrix<T>& TMatrix<T>::operator + (TSMatrix<T> ts)
{
        BCHK(row != ts.row, MatErr, number of rows differ in op, ts.row, *this );
        BCHK(col != ts.col, MatErr, number of cols differ in op, ts.col, *this );
        STD_SMP_TEMPLATE3VC(vec_add_svc, dim, vec, ts.vec, ts.fac);
        ts.real_destroy (); return *this;
}
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator - (TSMatrix<T> ts)
{
        BCHK(row != ts.row, MatErr, number of rows differ in op, ts.row, *this );
        BCHK(col != ts.col, MatErr, number of cols differ in op, ts.col, *this );
        STD_SMP_TEMPLATE3VC(vec_sub_svc, dim, vec, ts.vec, ts.fac);
        ts.real_destroy (); return *this;
}

//Change these to operate on diag only ?
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator += (const T& a)
{
        STD_SMP_TEMPLATE2C(vec_add_val, dim, vec, a);
        return *this;
}
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator -= (const T& a)
{
        STD_SMP_TEMPLATE2C(vec_add_val, dim, vec, a);
        return *this;
}

template <typename T>
inline TSMatrix<T> TMatrix<T>::operator *= (const T& a)
{ return TSMatrix<T> (*this, a); }

template <typename T>
inline TSMatrix<T> TMatrix<T>::operator /= (const T& a)
{ 
        BCHK(a==(T)0, MatErr, Matrix division by zero, 0, TSMatrix<T> (*this));
        return TSMatrix<T> (*this, (T)1/a); 
}

// temporaries can be changed, because we can't refer them
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator + (const Matrix<T>& a)
{ return this->operator += (a); }
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator - (const Matrix<T>& a)
{ return this->operator -= (a); }

template <typename T>
inline TMatrix<T>& TMatrix<T>::operator + (TMatrix<T> a)
{ return this->operator += (Matrix<T>(a)); }
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator - (TMatrix<T> a)
{ return this->operator -= (Matrix<T>(a)); }

template <typename T>
inline TMatrix<T>& TMatrix<T>::operator + (const T& a)
{ return this->operator += (a); }
template <typename T>
inline TMatrix<T>& TMatrix<T>::operator - (const T& a)
{ return this->operator -= (a); }

template <typename T>
inline TSMatrix<T> TMatrix<T>::operator * (const T& a)
{ return TSMatrix<T> (*this, a); }

template <typename T>
inline TSMatrix<T> TMatrix<T>::operator / (const T& b)
{
        BCHK((b == (T)0), MatErr, Matrix division by 0, 0, TSMatrix<T> (*this)); 
        return TSMatrix<T> (*this, (T)1/b);
}

//friends

//Change to operate on diag only ?
INST(template <typename T> class TMatrix friend TMatrix<T> operator + (const T&, TMatrix<T>);)
template <typename T>
inline TMatrix<T> operator + (const T& a, TMatrix<T> b)
{
        STD_SMP_TEMPLATE2C(val_add_vec, b._DIM, b._VEC, a);
        return b;
}
INST(template <typename T> class TMatrix friend TMatrix<T> operator - (const T&, TMatrix<T>);)
template <typename T>
inline TMatrix<T> operator - (const T& a, TMatrix<T> b)
{
        STD_SMP_TEMPLATE2C(val_sub_vec, b._DIM, b._VEC, a);
        return b;
}

INST(template <typename T> class TMatrix friend TSMatrix<T> operator * (const T&, TMatrix<T>);)
template <typename T>
inline TSMatrix<T> operator * (const T& a, TMatrix<T> b)
{ return TSMatrix<T> (b, a); } 
// Commutativity is assumed here !!!


//Change to operate on diag only ?
INST(template <typename T> class TMatrix friend TMatrix<T> operator + (const T&, const Matrix<T>&);)
template <typename T>                                           
inline TMatrix<T> operator + (const T& a, const Matrix<T>& b)
{
        TMatrix<T> c (b._ROW, b._COL);
        STD_SMP_TEMPLATE3VC(val_vec_add, b._DIM, c._VEC, b._VEC, a);
        return c;
}
INST(template <typename T> class TMatrix friend TMatrix<T> operator - (const T&, const Matrix<T>&);)
template <typename T>                                           
inline TMatrix<T> operator - (const T& a, const Matrix<T>& b)
{
        TMatrix<T> c (b._ROW, b._COL);
        STD_SMP_TEMPLATE3VC(val_vec_sub, b._DIM, c._VEC, b._VEC, a);
        return c;
}

INST(template <typename T> class TMatrix friend TSMatrix<T> operator * (const T&, const Matrix<T>&);)
template <typename T>
inline TSMatrix<T> operator * (const T& a, const Matrix<T>& b)
{ return TSMatrix<T> (b, a); }
// Commutativity is assumed here !



template <typename T>
inline double TMatrix<T>::fabssqr () const
{ Matrix<T> m (*this); return m.fabssqr (); }

template <typename T>
inline double fabssqr (const TMatrix<T>& tm)
{       
  return tm.fabssqr ();
}


template <typename T>
TMatrix<T> TMatrix<T>::transposed_copy() const
{
        TMatrix<T> trm(this->col, this->row);
        for (unsigned r = 0; r < this->row; ++r)
                for (unsigned c = 0; c < this->col; ++c)
                        trm.mat[c][r] = this->mat[r][c];
        this->mark_destroy();
        return trm;
}

INST(template <typename T> class TMatrix friend TMatrix<T> transpose(const TMatrix<T>& tm);)
template <typename T>
inline TMatrix<T> transpose(const TMatrix<T>& tm)
{
        return tm.transposed_copy();
}

template <typename T>
TMatrix<T>& TMatrix<T>::transpose()
{
        if (this->col != this->row) {
                // for non quadratic shape, doing inplace swap ops is too hard
                Matrix<T> transmat(this->transposed_copy());
                return this->swap(transmat);
        } else {
                // do in place swap ops
                for (unsigned r = 1; r < this->row; ++r)
                        for (unsigned c = 0; c < r; ++c)
                                SWAP(this->mat[r][c], this->mat[c][r]);
                return *this;
        }
}

NAMESPACE_END

NAMESPACE_CSTD

INST(template <typename T> class TBCI__ Matrix friend double fabs (const TBCI__ TMatrix<T>&);)
template<typename T>
inline double fabs (const TBCI__ TMatrix<T>& tm)
{ return tm.fabs (); }

INST(template <typename T> class TBCI__ Matrix friend double fabs (const TBCI__ Matrix<T>&);)
template<typename T>
inline double fabs (const TBCI__ Matrix<T>& m)
{ return m.fabs (); }

NAMESPACE_CSTD_END

NAMESPACE_TBCI

//-------------------------------------------------------------
//-------------------------------------------------------------
template <typename T>
class TSMatrix : public Matrix_Sig<T> //: public TMatrix
{
 protected:
        T* vec; 
        unsigned long dim;
        unsigned int  row, col;
        T** mat;
        T* endvec;
        T fac ALIGN(MIN_ALIGN2); //factor
        bool mut;  //mutability: does vec point to changeable (non-refered) data ?

        void real_destroy (); 
        void clone (bool = false, TMatrix<T>* = 0);
#if 1 //def HAVE_GCC295_FRIEND_BUG
 public:
#endif
        void detach (TMatrix<T>* = 0);
        
        typedef T* Tptr;
 public:

        friend class TMatrix<T>;
        friend class Matrix<T>; 

        typedef T value_type;
        typedef T element_type;
        typedef T aligned_value_type TALIGN(MIN_ALIGN2);

#if 1 //def HAVE_GCC295_FRIEND_BUG
        T* getvec () const     { return vec; }
        T* getendvec () const  { return endvec; }
        T& getfac ()  { return fac; }
        const T& getfac () const  { return fac; }
#endif

        TSMatrix () : vec((T*)0), dim(0), mat((T**)0), endvec((T*)0), mut(false)  {}
        // empty destructor: TSMatrix will never be destroyed but explicitly !!!
        ~TSMatrix ()  {}
         
        TSMatrix (const TMatrix<T>& tm, const T& f = (T)1) 
                : vec(tm.vec), dim(tm.dim), row(tm.row), col(tm.col),
                  mat(tm.mat), endvec(tm.endvec), fac(f), mut(true)  {}
        TSMatrix (const Matrix<T>& m, const T& f = (T)1)
                : vec(m.vec), dim(m.dim), row(m.row), col(m.col),
                  mat(m.mat), endvec(m.endvec), fac(f), mut(false)  {}
        TSMatrix (const TSMatrix<T>& ts)
                : vec(ts.vec), dim(ts.dim), row(ts.row), col(ts.col),
                  mat(ts.mat), endvec(ts.endvec), fac(ts.fac), mut(ts.mut)  {}
         
        // allow instantiation (Matrix_Sig)
        /*virtual*/ static const char* mat_info ()  { return ("TSMatrix"); }

        T operator () (const unsigned int r, const unsigned int c) HOT
         { T val = mat[r][c] * fac; real_destroy (); return val; }
         
        TSMatrix<T>& eval (TMatrix<T>* = 0);
         
        // These operations are very cheap now
        TSMatrix<T>& operator = (const TSMatrix<T>& ts)
        { real_destroy (); dim = ts.dim; row = ts.row; col = ts.col;
          mat = ts.mat; vec = ts.vec; endvec = ts.endvec;
          fac = ts.fac; mut = ts.mut; return *this; }
        TSMatrix<T>& operator = (const TMatrix<T>& tm)
        { real_destroy (); dim = tm.dim; row = tm.row; col = tm.col;
          mat = tm.mat; vec = tm.vec; endvec = tm.endvec; 
          fac = (T)1; mut = true; return *this; }
         
        TSMatrix<T>& operator *= (const T& f)  { fac *= f; return *this; }
        TSMatrix<T>& operator /= (const T& f)  { fac /= f; return *this; }
        TSMatrix<T>& operator *  (const T& f)  { fac *= f; return *this; }
        TSMatrix<T>& operator /  (const T& f)  { fac /= f; return *this; }
#ifndef HAVE_GCC295_FRIEND_BUG
        friend NOINST TSMatrix<T> FRIEND_TBCI__ operator * FGD (const T&, TSMatrix<T>);
#endif
        TSMatrix<T>& operator - ()  { fac *= (T)(-1); return *this; } 

        // Now for the more complicated
        TMatrix<T> operator + (const Matrix<T>&);
        TMatrix<T> operator + (const TMatrix<T>&);
        TMatrix<T> operator + (TSMatrix<T>);
        TMatrix<T> operator + (const T&);
        TMatrix<T> operator - (const Matrix<T>&);
        TMatrix<T> operator - (const TMatrix<T>&);
        TMatrix<T> operator - (TSMatrix<T>);
        TMatrix<T> operator - (const T&);

#ifndef HAVE_GCC295_FRIEND_BUG
        // functions to work around the need for friends (gcc-2.95 hack)
        friend NOINST TMatrix<T> FRIEND_TBCI__ operator + FGD (const T&, const TSMatrix<T>&);
        friend NOINST TMatrix<T> FRIEND_TBCI__ operator - FGD (const T&, const TSMatrix<T>&);
#endif

        TMatrix<T> operator * (const Matrix<T>& m);
        TMatrix<T> operator * (TMatrix<T> tm);
        TVector<T> operator * (const Vector<T>& v);
        TVector<T> operator * (TVector<T> tv);
         
        bool operator == (const Matrix<T>&);
        bool operator != (const Matrix<T>& m) { return !(*this == m); }
        
        bool operator == (TSMatrix<T>&);
        bool operator != (TSMatrix<T>& ts) { return !(*this == ts); }
        
        bool operator == (TMatrix<T>& tm) { Matrix<T> m(tm); return (*this == m); }
        bool operator != (TMatrix<T>& tm) { return !(*this == tm); }
        
        double fabssqr ();
        double fabs ()  { return GLBL__ MATH__ sqrt (fabssqr ()); }

        unsigned long size ()    const { return dim; }
};

template <typename T>
inline void TSMatrix<T>::real_destroy ()
{
        if (LIKELY(!mut || !dim))
                return;
        TBCIDELETE(T, vec, dim); TBCIDELETE(Tptr, mat, row);
}

template <typename T>
inline void TSMatrix<T>::detach (TMatrix<T>* tm)
{
        if (LIKELY((!tm && mut) || !dim))
                return;
        if (!tm) { //mut = false
                vec = NEW(T, dim); if (!vec) { 
                        dim = 0; mat = (T**)0; return; 
                }
                mat = NEW(Tptr,row); if (!mat) { 
                        TBCIDELETE(T, vec, dim); dim = 0; return;
                }
                for (REGISTER unsigned r = 0; r < row; r++) 
                        mat[r] = vec + r*col;
                endvec = vec + dim; 
        } else { //tm = true, mut = ?
                vec = tm->vec; mat = tm->mat; endvec = tm->endvec;
        }
        mut = true;
}

template <typename T>
void TSMatrix<T>::clone (bool evl, TMatrix<T>* tm)
{
        if (LIKELY((mut && !tm)|| !dim))
                return;
        T* oldv = vec; T** oldm = mat; bool omut = mut;
        detach (tm);
        if (evl && fac != (T)1) {
                STD_SMP_TEMPLATE3VC(vec_val_mul, dim, vec, oldv, fac);
                fac = (T)1;
        } else 
                TBCICOPY (vec, oldv, T, dim);
        if (tm && omut) { 
                TBCIDELETE(T, oldv, dim); TBCIDELETE(Tptr, oldm, row);
        }
}



//creates mutable evaluated TSMatrix (can be easily converted to TMatrix)
template <typename T>
inline TSMatrix<T>& TSMatrix<T>::eval (TMatrix<T>* tm)
{
        if (mut && !tm) {
                if (fac != (T)1) {
                        STD_SMP_TEMPLATE2C(vec_mul_val, dim, vec, fac);
                        fac = (T)1;
                }
        } else
                clone (true, tm);
        return (*this);
}


// Arithmetics
template <typename T>
inline TMatrix<T> TSMatrix<T>::operator + (const Matrix<T>& m)
{
        BCHK(row != m.row || col != m.col, MatErr, Op + on diff size Mats, m.row, TMatrix<T> (*this));
        TSMatrix<T> ts (*this); ts.detach ();
        STD_SMP_TEMPLATE4V(svc_vec_add, dim, ts.vec, vec, m.vec, fac);
        ts.fac = (T)1; return TMatrix<T> (ts);
}
template <typename T>
inline TMatrix<T> TSMatrix<T>::operator - (const Matrix<T>& m)
{
        BCHK(row != m.row || col != m.col, MatErr, Op - on diff size Mats, m.row, TMatrix<T> (*this));
        TSMatrix<T> ts (*this); ts.detach ();
        STD_SMP_TEMPLATE4V(svc_vec_sub, dim, ts.vec, vec, m.vec, fac);
        ts.fac = (T)1; return TMatrix<T> (ts);
}

template <typename T>
inline TMatrix<T> TSMatrix<T>::operator + (const TMatrix<T>& tm)
{
        BCHK(row != tm.row || col != tm.col, MatErr, Op + on diff size Mats, tm.row, tm);
        STD_SMP_TEMPLATE3VC(vec_add_svc, dim, tm.vec, vec, fac);
        real_destroy (); return tm;
}
template <typename T>
inline TMatrix<T> TSMatrix<T>::operator - (const TMatrix<T>& tm)
{
        BCHK(row != tm.row || col != tm.col, MatErr, Op - on diff size Mats, tm.row, tm);
        STD_SMP_TEMPLATE3VC(vec_sub_svc_inv, dim, tm.vec, vec, fac);
        real_destroy (); return tm;
}

template <typename T>
inline TMatrix<T> TSMatrix<T>::operator + (TSMatrix<T> ts)
{
        BCHK(row != ts.row || col != ts.col, MatErr, Op op on diff size Mats, ts.row, TMatrix<T> (*this));
        TSMatrix<T> tm;
        if (LIKELY(!mut && ts.mut))
                tm = ts;
        else { 
                tm = *this; tm.detach (); 
        }
        STD_SMP_TEMPLATE5(svc_svc_add, dim, tm.vec, vec, ts.vec, fac, ts.fac);
        if (LIKELY(!mut && ts.mut))
                real_destroy (); 
        else 
                ts.real_destroy ();
        tm.fac = (T)1; 
        return TMatrix<T> (tm);
}

template <typename T>
inline TMatrix<T> TSMatrix<T>::operator - (TSMatrix<T> ts)
{
        BCHK(row != ts.row || col != ts.col, MatErr, Op op on diff size Mats, ts.row, TMatrix<T> (*this));
        TSMatrix<T> tm;
        if (LIKELY(!mut && ts.mut))
                tm = ts;
        else { 
                tm = *this; tm.detach (); 
        }
        STD_SMP_TEMPLATE5(svc_svc_sub, dim, tm.vec, vec, ts.vec, fac, ts.fac);
        if (LIKELY(!mut && ts.mut))
                real_destroy (); 
        else 
                ts.real_destroy ();
        tm.fac = (T)1; 
        return TMatrix<T> (tm);
}

template <typename T>
inline TMatrix<T> TSMatrix<T>::operator + (const T& a)
{
        TSMatrix<T> ts (*this); ts.detach ();
        STD_SMP_TEMPLATE4C(svc_val_add, dim, ts.vec, vec, fac, a);
        ts.fac = (T)1; return TMatrix<T> (ts);
}

template <typename T>
inline TMatrix<T> TSMatrix<T>::operator - (const T& a)
{
        TSMatrix<T> ts (*this); ts.detach ();
        STD_SMP_TEMPLATE4C(svc_val_sub, dim, ts.vec, vec, fac, a);
        ts.fac = (T)1; return TMatrix<T> (ts);
}

// operate on diag only?
INST(template <typename T> class TSMatrix friend TMatrix<T> operator + (const T&, const TSMatrix<T>&);)
template <typename T>
inline TMatrix<T> operator + (const T& a, const TSMatrix<T>& ts)
{
        TSMatrix<T> tm (ts); tm.detach ();
        STD_SMP_TEMPLATE4C(val_svc_add, tm._DIM, tm._VEC, ts._VEC, a, ts._FAC);
        tm._FAC = (T)1; return TMatrix<T> (tm);
}
INST(template <typename T> class TSMatrix friend TMatrix<T> operator - (const T&, const TSMatrix<T>&);)
template <typename T>
inline TMatrix<T> operator - (const T& a, const TSMatrix<T>& ts)
{
        TSMatrix<T> tm (ts); tm.detach ();
        STD_SMP_TEMPLATE4C(val_svc_sub, tm._DIM, tm._VEC, ts._VEC, a, ts._FAC);
        tm._FAC = (T)1; return TMatrix<T> (tm);
}


template <typename T>
TMatrix<T> TSMatrix<T>::operator * (const Matrix<T>& m)
{
        TMatrix<T> c (row, m.col);
        BCHK(col != m.row, MatErr, Illegal sizes in TSMat-Mat multiplication, 0, (real_destroy(), c));
        T tmp ALIGN(MIN_ALIGN2);
        // TODO: Exact summation
        for (unsigned i=0; i<row; i++) {
                for (unsigned j=0; j<m.col; j++) {
                        tmp = mat[i][0] * m.mat[0][j];
                        for (REGISTER unsigned l=1; l<col; l++)
                                tmp += mat[i][l] * m.mat[l][j];
                        c.mat[i][j] = tmp * fac;
                }
        }
        real_destroy (); return c;
}

template <typename T>
TMatrix<T> TSMatrix<T>::operator * (TMatrix<T> tm)
{
        TMatrix<T> c (row, tm.col);
        BCHK(col != tm.row, MatErr, Illegal sizes in TSMat-TMat multiplication, 0, (real_destroy(), c));
        T tmp ALIGN(MIN_ALIGN2);
        // TODO: Exact summation
        for (unsigned i=0; i<row; i++) {
                for (unsigned j=0; j<tm.col; j++) {
                        tmp = mat[i][0] * tm.get(0,j);
                        for (REGISTER unsigned l=1; l<col; l++)
                                tmp += mat[i][l] * tm.get(l,j);
                        c.mat[i][j] = tmp * fac;
                }
        }
        real_destroy (); tm.mark_destroy(); return c;
}

template <typename T>
TVector<T> TSMatrix<T>::operator * (const Vector<T>& v)
{
        TVector<T> tv (row);
        BCHK (v.size() != col, MatErr, multiply TSMatrix w/ Vector: wrong config, v.size(), (real_destroy(), tv));
        // TODO: Exact summation
        for (unsigned r=0; r<row; r++) {
                ALIGN3(REGISTER T el, mat[r][0] * v(0), MIN_ALIGN2);
                for (REGISTER unsigned c=1; c<col; c++) 
                        el += mat[r][c] * v(c);
                tv.set (el * fac, r);
        }
        real_destroy (); return tv;
}

template <typename T>
TVector<T> TSMatrix<T>::operator * (TVector<T> v)
{
        TVector<T> tv (row);
        BCHK (v.size() != col, MatErr, multiply TSMatrix w/ TVector: wrong config, v.size(), (real_destroy(), tv));
        // TODO: Exact summation
        for (unsigned r=0; r<row; r++) {
                ALIGN3(REGISTER T el, mat[r][0] * v.get(0), MIN_ALIGN2);
                for (REGISTER unsigned c=1; c<col; c++) 
                        el += mat[r][c] * v.get(c);
                tv.set (el * fac, r);
        }
        real_destroy(); v.destroy(); return tv;
}


template <typename T>
bool TSMatrix<T>::operator == (const Matrix<T>& m)
{
        if (LIKELY(row != m.row || col != m.col)) { 
                real_destroy (); return false; 
        }
        if (LIKELY(dim == 0)) {
                /*real_destroy();*/ return true; 
        }
        if (LIKELY(fac == (T)1)) {
                if (LIKELY(vec == m.vec))
                        return true;
                if (TBCICOMP (vec, m.vec, T, dim)) { 
                        real_destroy (); return false; 
                }
        } else {
                if (LIKELY(vec == m.vec))
                        return false;
                for (REGISTER T *p1 = vec, *p2 = m.vec; p1 < endvec; p1++, p2++)
                        if (UNLIKELY(*p1 * fac != *p2)) {
                                real_destroy (); return false; 
                        }
        }
        real_destroy (); return true;
}

template <typename T>
bool TSMatrix<T>::operator == (TSMatrix<T>& ts)
{
        if (LIKELY(row != ts.row || col != ts.col)) {
                real_destroy (); ts.real_destroy (); return false; 
        }
        if (LIKELY(dim == 0)) {
                /*real_destroy(); ts.real_destroy();*/ 
                return true; 
        }
        if (LIKELY(fac == ts.fac)) {
                if (LIKELY(vec == ts.vec)) {
                        real_destroy (); return true; 
                }
                if (TBCICOMP (vec, ts.vec, T, dim)) { 
                        real_destroy (); ts.real_destroy (); return false; 
                }
        } else {
                if (LIKELY(vec == ts.vec)) {
                        real_destroy (); return false; 
                }
                for (REGISTER T *p1 = vec, *p2 = ts.vec; p1 < endvec; p1++, p2++)
                        if (UNLIKELY(*p1 * fac != *p2)) {
                                real_destroy (); ts.real_destroy (); return false; 
                        }
        }
        real_destroy (); ts.real_destroy (); 
        return true;
}


INST(template <typename T> class TSMatrix friend TSMatrix<T> operator * (const T&, TSMatrix<T>);)
template <typename T>
inline TSMatrix<T> operator * (const T& f, TSMatrix<T> ts)
{ ts._FAC = f * ts._FAC; return ts; }


template <typename T>
double TSMatrix<T>::fabssqr ()
{
        double res = 0.0;
        if (do_exactsum())
                do_vec_fabssqr_exact (dim, vec, res);
        else
                do_vec_fabssqr_quick (dim, vec, res);
        res *= GLBL2__ TBCI__ fabssqr (fac);
        real_destroy ();
        return res;
}

template <typename T>
inline double fabssqr (TSMatrix<T>& tsm)
{       
        return tsm.fabssqr ();
}

NAMESPACE_END
        
NAMESPACE_CSTD

INST(template <typename T> class TBCI__ TSMatrix friend double fabs (TBCI__ TSMatrix<T>&);)
template <typename T>
double fabs (TBCI__ TSMatrix<T>& ts)
{ return ts.fabs (); }

NAMESPACE_CSTD_END

NAMESPACE_TBCI          
//---------------------------------------------------------------
//---------------------------------------------------------------
template <typename T>
class Matrix : public TMatrix<T> 
{
        
 public:
         
        friend class TSMatrix<T>;
        
        typedef T value_type;
        typedef T element_type;
        typedef T aligned_value_type TALIGN(MIN_ALIGN2);

        // constructor, destructor
         
        explicit Matrix (const unsigned d=0) 
        : TMatrix<T> (d) 
        { /* this->freeable = 1; */ }
        Matrix (const unsigned r, const unsigned c)
        : TMatrix<T> (r, c) 
        { /* this->freeable = 1; */ }
        Matrix (const T& v, const unsigned r, const unsigned c) 
        : TMatrix<T> (v, r, c) 
        { /* this->freeable = 1; */ }
        Matrix (const Vector<T>& v, const enum rowcolvec r = colvec) 
        : TMatrix<T> (v, r) 
        { /* this->freeable = 1; */ }
        
        Matrix (const Matrix<T>& m) 
        : TMatrix<T> (m) 
        { /* this->freeable = 1; */ }
        Matrix (const TMatrix<T>& tm) HOT
        : TMatrix<T> (tm) 
        { /* this->freeable = 1; */ }
        Matrix (const TSMatrix<T>& ts) 
        : TMatrix<T> (ts) 
        { /* this->freeable = 1; */ }
        
        ~Matrix () { this->freeable = 1; /* Let ~TMatrix do the job */ }

        // allow instantiation (Matrix_Sig)
        /*virtual*/ static const char* mat_info ()  { return ("Matrix"); }
#ifndef HAVE_PROMOTION_BUG
        // Promotion (only explicit)
        template <typename U> 
        explicit Matrix (const Matrix<U>& m) 
        : TMatrix<T> (m)  
        { /* this->freeable = 1; */ }
        template <typename U> 
        explicit Matrix (const TMatrix<U>& tm)
        : TMatrix<T> (tm)  
        { /* this->freeable = 1; */ }
        // basics
#endif
         
        typename tbci_traits<T>::const_refval_type
           operator () (const unsigned int, const unsigned int) const HOT;
        T& operator () (const unsigned int, const unsigned int) HOT;
        TVector<T> operator () (const unsigned int) const;
        
        Matrix<T>& operator = (const Matrix<T>& m)
         { this->TMatrix<T>::operator = (m); return *this; }
        Matrix<T>& operator = (const TMatrix<T>& tm)
         { this->TMatrix<T>::operator = (tm); return *this; }
        Matrix<T>& operator = (TSMatrix<T> ts)
         { TMatrix<T>(this->TMatrix<T>::operator = (ts)); return *this; }
        Matrix<T>& operator = (const T& v)
         { this->TMatrix<T>::operator = (v); return *this; }
         
        Matrix<T>& resize (const unsigned r, const unsigned c)
         { this->TMatrix<T>::resize (r, c); return *this; }
        Matrix<T>& resize (const unsigned d)  
         { return this->resize (d, d); }
        Matrix<T>& resize (const T& v, const unsigned r, const unsigned c)
         { this->TMatrix<T>::resize (v, r, c); return *this; }
        Matrix<T>& resize (const Matrix<T>& m)
         { this->TMatrix<T>::resize (m); return *this; }
        Matrix<T>& fill (const T& v = (T) 0)
         { this->TMatrix<T>::fill (v); return *this; }
        Matrix<T>& fill (const Vector<T>& v)
         { this->TMatrix<T>::fill (v); return *this; }
        Matrix<T>& setunit (const T& f = (T) 1)
         { this->TMatrix<T>::setunit (f); return *this; }
        Matrix<T>& clear ()
         { this->TMatrix<T>::clear (); return *this; }

        // How to make a Vector from a Matrix ...
        friend /*inline*/ BVector<T>& FRIEND_TBCI2__ bvfillm FGD (BVector<T>&, const Matrix<T>& m);
        
        
         // assignment ops (do we really need em ?)
         // Seems we have to make sure, that a Matrix and no TMatrix
         // is returned; otherwise there's danger to be destroyed
        Matrix<T>& operator += (const Matrix<T>& a) 
         { this->TMatrix<T>::operator += (a); return *this; } 
        Matrix<T>& operator -= (const Matrix<T>& a)
         { this->TMatrix<T>::operator -= (a); return *this; } 
        Matrix<T>& operator += (TMatrix<T> a) 
         { this->TMatrix<T>::operator += (a); return *this; } 
        Matrix<T>& operator -= (TMatrix<T> a)
         { this->TMatrix<T>::operator -= (a); return *this; } 
        Matrix<T>& operator += (TSMatrix<T> a) 
         { this->TMatrix<T>::operator += (a); return *this; } 
        Matrix<T>& operator -= (TSMatrix<T> a)
         { this->TMatrix<T>::operator -= (a); return *this; } 
        Matrix<T>& operator += (const T& a)
         { this->TMatrix<T>::operator += (a); return *this; } 
        Matrix<T>& operator -= (const T& a)
         { this->TMatrix<T>::operator -= (a); return *this; } 
        Matrix<T>& operator *= (const T& a)
         /* { TMatrix<T>(this->TMatrix<T>::operator *= (a)); return *this; } */;
        Matrix<T>& operator /= (const T& a)
         /* { TMatrix<T>(this->TMatrix<T>::operator /= (a)); return *this; } */;
        
        
         // basic operations
        TMatrix<T> operator - () const { return -(TMatrix<T> (*this)); }
        
        TMatrix<T> operator + (const Matrix<T>&) const;
        TMatrix<T> operator - (const Matrix<T>&) const;
        TMatrix<T> operator * (const Matrix<T>&) const;
        
        TMatrix<T>& operator + (TMatrix<T>&) const;
        TMatrix<T>& operator - (TMatrix<T>&) const;
        TMatrix<T>  operator * (TMatrix<T>&) const;
        
        TMatrix<T> operator + (TSMatrix<T>&) const;
        TMatrix<T> operator - (TSMatrix<T>&) const;
        TMatrix<T> operator * (TSMatrix<T>&) const;
        
        TMatrix<T>  operator + (const T&) const;
        TMatrix<T>  operator - (const T&) const;
        TSMatrix<T> operator * (const T&) const;
        TSMatrix<T> operator / (const T&) const;
        
        TVector<T> operator * (const Vector<T>&) const HOT;
        TVector<T> operator * (TVector<T>) const HOT;
        TVector<T> operator * (const TSVector <T>& tsv) const HOT;
        TVector<T> transMult  (const Vector<T>&) const HOT;
        //friend TMatrix<T> operator + FGD (const T&, const Matrix<T>&);
        //friend TMatrix<T> operator - FGD (const T&, const Matrix<T>&);
        //friend TMatrix<T> operator * FGD (const T&, const Matrix<T>&);
        
        bool operator == (const Matrix<T>& m) const;
        bool operator != (const Matrix<T>& m) const
         { return !(*this == m); }
        
        bool operator == (TMatrix<T> tm) const
         { Matrix<T> m (tm); return (*this == m); }
        bool operator != (TMatrix<T> tm) const
         { return !(*this == tm); }
        
        bool operator == (TSMatrix<T>) const;
        bool operator != (TSMatrix<T> ts) const 
         { return !(*this == ts); } 
        
        Matrix<T>& mult_rows (const Vector<T>&);
        Matrix<T>& div_rows  (const Vector<T>&);
        Matrix<T>& mult_row  (const T&, const unsigned);
        Matrix<T>& div_row   (const T&, const unsigned);

         // io-streams
         
        friend STD__ ostream& operator << FGDT (STD__ ostream&, const Matrix<T>&);
        friend STD__ istream& operator >> FGDT (STD__ istream&, Matrix<T>&);
        
        // solvers
        friend NOINST char FRIEND_TBCI2__ gaussj FGD (Matrix<T>&, Matrix<T>&);
        
        //friend int lu_solver(..);
        
        //misc
        double fabssqr () const;
        double fabs () const  { return GLBL__ MATH__ sqrt (fabssqr()); }
};


// definitions

template <typename T>
inline TMatrix<T>& TMatrix<T>::swap (TMatrix<T>& m)
{
        //BCHK(this->row != m.row || this->col != m.col, MatErr, Swapping Matrices w/ different layout, m.row, *this);
        SWAP(this->dim, m.dim);
        SWAP(this->row, m.row); 
        //SWAP(this->col, m.col);
        unsigned int tcol = this->col; this->col = m.col; m.col = tcol;
        SWAP(this->mat, m.mat);
        SWAP(this->vec, m.vec); SWAP(this->endvec, m.endvec);
        // Hmmm, swap freeability???
        //SWAP(this->freeable, m.freeable);
        return *this;
}

template <typename T>
inline typename tbci_traits<T>::const_refval_type
Matrix<T>::operator () (const unsigned int i, const unsigned int j) const
{
        EXPCHK(i>=this->row, MatErr, Illegal row index, i, this->mat[0][0]);
        EXPCHK(j>=this->col, MatErr, illegal col index, j, this->mat[0][0]);
        return this->mat[i][j];
}

template <typename T>
inline T& Matrix<T>::operator () (const unsigned int i, const unsigned int j)
{
        EXPCHK(i>=this->row, MatErr, Illegal row index, i, this->mat[0][0]);
        EXPCHK(j>=this->col, MatErr, illegal col index, j, this->mat[0][0]);
        return this->mat[i][j];
}

template <typename T>
inline TVector<T> Matrix<T>::operator () (const unsigned int i) const
{
        TVector<T> v(this->col);
        BCHK(i>= this->row, MatErr, Illegal row index, i, v);
        TBCICOPY (v.vec, this->mat[i], T, this->col);
        return v;
}


template <typename T>
STD__ ostream& operator << (STD__ ostream& os, const Matrix<T>& m)
{
        for (unsigned r=0; r<m.row; r++)
#if 0
                os << m(i) << "\n";
#else
        {
                for (unsigned c=0; c<m.col; c++)
                        os << m(r,c) << " ";
                os << "\n";
        }
#endif
        return os;
}

template <typename T>
STD__ istream& operator >> (STD__ istream& in, Matrix<T>& m)
{
        Vector<T> buf(m.col);
        
        for (unsigned i=0; i<m.row; i++)
    {
                in >> buf;
                m.set_row (buf, i);
    }
        return in;
}

INST(template <typename T> class TMatrix friend STD__ ostream& operator << (STD__ ostream& os, TMatrix<T> tm);)
template <typename T>
STD__ ostream& operator << (STD__ ostream& os, TMatrix<T> tm)
{
        Matrix<T> m(tm);
        return os << m;
}

INST(template <typename T> class TSMatrix friend STD__ ostream& operator << (STD__ ostream& os, const TSMatrix<T>& ts);)
template <typename T>
STD__ ostream& operator << (STD__ ostream& os, const TSMatrix<T>& ts)
{
        return os << Matrix<T>(ts);
}


/************** Arithmetics ***************/

//Change these to operate on diag only ?
template <typename T>
inline Matrix<T>& Matrix<T>::operator *= (const T& a)
{
        STD_SMP_TEMPLATE2C(vec_mul_val, this->dim, this->vec, a);
        return *this;
}
template <typename T>
inline Matrix<T>& Matrix<T>::operator /= (const T& a)
{
        STD_SMP_TEMPLATE2C(vec_div_val, this->dim, this->vec, a);
        return *this;
}


//change these to operate on diag only ?
template <typename T>
inline TMatrix<T> Matrix<T>::operator + (const T& a) const
{
        TMatrix<T> t (this->row, this->col);
        STD_SMP_TEMPLATE3VC(vec_val_add, this->dim, t.vec, this->vec, a);
        return t;
}
template <typename T>
inline TMatrix<T> Matrix<T>::operator - (const T& a) const
{
        TMatrix<T> t (this->row, this->col);
        STD_SMP_TEMPLATE3VC(vec_val_sub, this->dim, t.vec, this->vec, a);
        return t;
}

template <typename T>
inline TSMatrix<T> Matrix<T>::operator * (const T& a) const
{ return TSMatrix<T> (*this, a); }

template <typename T>
inline TSMatrix<T> Matrix<T>::operator / (const T& a) const
{ 
        BCHK (a==(T)0, MatErr, Divide Mat by 0, 0, *this);
        return TSMatrix<T> (*this, (T)1/a); 
}

template <typename T>
inline TMatrix<T> Matrix<T>::operator + (const Matrix<T>& a) const
{
        TMatrix<T> t (this->row, this->col);
        BCHK(this->dim != a.dim, MatErr, Operator + on diff size matrices, a.dim, t);
        STD_SMP_TEMPLATE3VV(vec_vec_add, this->dim, t.vec, this->vec, a.vec);
        return t;
}
template <typename T>
inline TMatrix<T> Matrix<T>::operator - (const Matrix<T>& a) const
{
        TMatrix<T> t (this->row, this->col);
        BCHK(this->dim != a.dim, MatErr, Operator - on diff size matrices, a.dim, t);
        STD_SMP_TEMPLATE3VV(vec_vec_sub, this->dim, t.vec, this->vec, a.vec);
        return t;
}

template <typename T>
inline TMatrix<T>& Matrix<T>::operator + (TMatrix<T>& a) const
{
        BCHK(this->dim != a.dim, MatErr, Applying + on diff. size matrices, a.dim, a);
        STD_SMP_TEMPLATE2V(vec_add_vec, this->dim, a.vec, this->vec);
        return a;
}
template <typename T>
inline TMatrix<T>& Matrix<T>::operator - (TMatrix<T>& a) const
{
        BCHK(this->dim != a.dim, MatErr, Applying - on diff. size matrices, a.dim, a);
        STD_SMP_TEMPLATE2V(vec_sub_vec_inv, this->dim, a.vec, this->vec);
        return a;
}

template <typename T>
inline TMatrix<T> Matrix<T>::operator + (TSMatrix<T>& ts) const
{
        BCHK(this->dim != ts.dim, MatErr, Applying + on diff. size matrices, ts.dim, (ts.real_destroy(),TMatrix<T> (*this)) );
        TSMatrix<T> tm (ts); tm.detach ();
        STD_SMP_TEMPLATE4V(vec_svc_add, this->dim, tm.vec, this->vec, ts.vec, ts.fac);
        tm.fac = (T)1; return TMatrix<T> (tm);
}
template <typename T>
inline TMatrix<T> Matrix<T>::operator - (TSMatrix<T>& ts) const
{
        BCHK(this->dim != ts.dim, MatErr, Applying - on diff. size matrices, ts.dim, (ts.real_destroy(),TMatrix<T> (*this)) );
        TSMatrix<T> tm (ts); tm.detach ();
        STD_SMP_TEMPLATE4V(vec_svc_sub, this->dim, tm.vec, this->vec, ts.vec, ts.fac);
        tm.fac = (T)1; return TMatrix<T> (tm);
}

/* costs of mat vec mult; ra = a.row, ca = a.col, cb = b.col */
#define COST_MATMAT_OLD(ra,ca,cb) (ra*cb*(COST_UNIT_STORE+COST_LOOP \
  +ca*(3*COST_UNIT_LOAD/*+2*COST_CALL*/+COST_NU_LOAD+COST_MULT+COST_ADD+COST_LOOP)))
#define COST_MATMAT_NEW(ra,ca,cb) (ra*cb*COST_MEMSET+ra*ca*(2*COST_UNIT_LOAD+COST_LOOP \
  /*+COST_CALL*/+cb*(3*COST_UNIT_LOAD/*+COST_CALL*/+COST_UNIT_STORE+COST_ADD+COST_MULT+COST_LOOP)))
#ifdef OLD_MAT_MAT_MULT
# define COST_MATMAT(ra,ca,cb) COST_MATMAT_OLD(ra,ca,cb)
#else
# define COST_MATMAT(ra,ca,cb) COST_MATMAT_NEW(ra,ca,cb)
#endif

// Matrix multiplication
INST(template <typename T> class TMatrix friend void do_mat_mat_mult (const unsigned, const unsigned, \
                                TMatrix<T> *, const Matrix<T> *, const Matrix<T> *);)

#include "matrix_kernels.h"
        
#if defined(SMP) && !defined(NOSMP_MATVEC)

template <typename T>
inline void job_mat_mat_mult (struct thr_ctrl *tc)
{
        do_mat_mat_mult (tc->t_off, tc->t_size, 
                         (TMatrix<T>*)(tc->t_par[0]), (const Matrix<T>*)(tc->t_par[1]), 
                         (const Matrix<T>*)(tc->t_par[2]));
}

template <typename T>
TMatrix<T> Matrix<T>::operator * (const Matrix<T>& b) const
{
        TMatrix<T> c(this->row, b.col); 
        /* FIXME: Should this be SMP_VECSLICE2? */
        const unsigned n_thr = threads_avail(this->col*this->row/SMP_MATSLICE2);
        update_n_thr(n_thr);
#ifndef OLD_MAT_MAT_MULT
        c.clear();
#endif
        BCHK(this->col != b.row, MatErr, Illegal sizes in Matrix multiplication, 0, c);
        if (LIKELY(n_thr < 2))
                do_mat_mat_mult<T> (0UL, this->row, &c, this, &b);
        else {
                PREFETCH_R(this->vec, 2);
                PREFETCH_R(b.vec, 2);
                PREFETCH_W(c.vec, 3);
                const unsigned long first = slice_offset(1, n_thr, this->row, (T*)0);
                unsigned long st, en = first;
                /* Start threads */
                for (unsigned t = 0; t < n_thr-1; ++t) {
                        st = en; en = slice_offset(t+2, n_thr, this->row, (T*)0);
                        thread_start_off (t, (thr_job_t)job_mat_mat_mult<T>,
                                          st, en, &c, this, &b, (void*)0);
                }
                /* The first slice is handled by the main thread */
                do_mat_mat_mult<T> (0UL, first, &c, this, &b);
                //sched_yield ();
                /* Wait for the end */
                for (unsigned s = 0; s < n_thr-1; ++s)
                        thread_wait (s);
        }
        return c;
}


#else /* SMP */
template <typename T>
TMatrix<T> Matrix<T>::operator * (const Matrix<T>& b) const
{
        TMatrix<T> c(this->row, b.col); 
#ifndef OLD_MAT_MAT_MULT
        c.clear();
#endif
        BCHK(this->col != b.row, MatErr, Illegal sizes in Matrix multiplication, 0, c);
        do_mat_mat_mult <T> (0, this->row, &c, this, &b);
        return c;
}
#endif /* SMP */

template <typename T>
TMatrix<T> Matrix<T>::operator * (TSMatrix<T>& a) const
{
        TMatrix<T> c(this->row, a.col);
        BCHK(this->col != a.row, MatErr, Illegal sizes in Matrix multiplication, 0, c);
        T tmp ALIGN(MIN_ALIGN2);
        for (unsigned i=0; i<this->row; i++) {
                for (unsigned j=0; j<a.col; j++) {
                        tmp = this->mat[i][0] * a.mat[0][j];
                        for (REGISTER unsigned l=1; l<this->col; l++) 
                                tmp += this->mat[i][l] * a.mat[l][j];
                        c.mat[i][j] = tmp * a.fac;
                }
        }
        a.real_destroy (); return c;
}


template <typename T>
inline TMatrix<T> Matrix<T>::operator * (TMatrix<T>& a) const
{
        TMatrix<T> c (this->operator * (Matrix<T> (a)));
        return c;
}

template <typename T>
inline TMatrix<T> TMatrix<T>::operator * (const Matrix<T>& a)
{
        Matrix<T> m (*this);
        return (m * a);
}


template <typename T>
inline TMatrix<T> TMatrix<T>::operator * (TMatrix<T> a)
{
        Matrix<T> m (*this);
        return (m * (Matrix<T> (a)));
}

template <typename T>
inline TMatrix<T> TMatrix<T>::operator * (TSMatrix<T> a)
{
        Matrix<T> m (*this);
        return (m * a);
}


template <typename T>
bool Matrix<T>::operator == (const Matrix<T>& m) const
{
        if (LIKELY(this->row != m.row || this->col != m.col))
                return false;
        if (LIKELY(this->vec == m.vec || this->dim == 0))
                return true;
        if (TBCICOMP (this->vec, m.vec, T, this->dim)) 
                return false;
        else 
                return true;
}

template <typename T>
bool Matrix<T>::operator == (TSMatrix<T> ts) const
{
        if (LIKELY(this->row != ts.row || this->col != ts.col))
                return (ts.real_destroy(), false);
        if (LIKELY(this->dim == 0)) {
                /*ts.real_destroy();*/ return true; 
        }
        if (LIKELY(ts.fac == (T)1)) {
                if (LIKELY(this->vec == ts.vec))
                        return true;
                if (TBCICOMP (this->vec, ts.vec, T, this->dim)) { 
                        ts.real_destroy (); return false; 
                }
        } else {
                if (LIKELY(this->vec == ts.vec))
                        return false;
                for (REGISTER T *p1 = this->vec, *p2 = ts.vec; p1 < this->endvec; p1++, p2++)
                        if (UNLIKELY(*p1 != *p2 * ts.fac)) {
                                ts.real_destroy (); return false; 
                        }
        }
        ts.real_destroy (); return true;
}

template <typename T>
double Matrix<T>::fabssqr () const
{
        double res = 0.0;
        if (do_exactsum())
                do_vec_fabssqr_exact (this->dim, this->vec, res);
        else
                do_vec_fabssqr_quick (this->dim, this->vec, res);
        return (res);
}

template <typename T>
inline double fabssqr (const Matrix<T>& m)
{
        return m.fabssqr ();
}

template<typename T>
inline TVector<T> Matrix<T>::operator * (TVector<T> c) const
{
        Vector<T> cn (c);
        return (*this * cn);
}


#if defined(SMP) && !defined(NOSMP_MATVEC)

template <typename T>
inline void job_mat_vec_mult (struct thr_ctrl *tc)
{
  do_mat_vec_mult (tc->t_off, tc->t_size,
                   (TVector<T>*)(tc->t_par[0]), (const Matrix<T>*)(tc->t_par[1]),
                   (const Vector<T>*)(tc->t_par[2]));
}
  
template <typename T>
inline void job_mat_vec_transmult (struct thr_ctrl *tc)
{
  do_mat_vec_transmult (tc->t_off, tc->t_size,
                        (TVector<T>*)(tc->t_par[0]), (const Matrix<T>*)(tc->t_par[1]), 
                        (const Vector<T>*)(tc->t_par[2]));
}


template <typename T>
TVector<T> Matrix<T>::operator * (const Vector<T>& v) const
{
  TVector<T> tv (this->row);
  /* use some heuristic to decide for the num of threads */
  const unsigned n_thr = threads_avail (this->row / SMP_MATSLICE2);
  update_n_thr(n_thr);
  //std::cout << "Mat*Vec: " << n_thr << " threads" << std::endl;
  BCHK (v.size() != this->col, MatErr, multiply Matrix w/ Vector: wrong config, v.size(), tv);
  if (LIKELY(n_thr < 2))
      do_mat_vec_mult<T> (0, this->row, &tv, this, &v);
  else {
      PREFETCH_R(this->vec, 2);
      PREFETCH_R(v.vec, 2);
      PREFETCH_W(tv.vec, 3);
      const unsigned long first = slice_offset(1, n_thr, this->row, (T*)0);
      unsigned long st, en = first;           
      /* Start threads */
      for (unsigned t = 0; t < n_thr-1; ++t) {
                st = en; en = slice_offset(t+2, n_thr, this->row, (T*)0);
                thread_start_off (t, (thr_job_t)job_mat_vec_mult<T>,
                                  st, en, &tv, this, &v, (void*)0);
          }
          /* The first slice is handled by the main thread */
          do_mat_vec_mult<T> (0, first, &tv, this, &v);
          //sched_yield ();
          /* Wait for the end */
          for (unsigned s = 0; s < n_thr-1; ++s)
                thread_wait (s);
  }
  return tv;
}

template<typename T>
TVector<T> Matrix<T>::transMult (const Vector<T>& v) const
{
   TVector<T> tv (this->col);
   const unsigned n_thr = threads_avail (this->col / SMP_MATSLICE2);
   update_n_thr(n_thr);
   BCHK (v.size() != this->row, MatErr, transMult Matrix w/ Vector: wrong config, v.size(), tv);
   if (LIKELY(n_thr < 2))
       do_mat_vec_transmult<T> (0, this->col, &tv, this, &v);
   else {
       /* FIXME: This does not use slice_offset yet! */
       const unsigned long first = slice_offset(1, n_thr, this->col, (T*)0);
       //const unsigned long first = this->col/n_thr - (this->col/n_thr)%4;
       unsigned long st, en = first;          
       for (unsigned t = 0; t < n_thr-1; ++t) {
          //st = en; en = (t+2)*this->col/n_thr;
          //if (t < n_thr-2)
          //     en -= en%4;
          st = en; en = slice_offset(t+2, n_thr, this->col, (T*)0);
          thread_start_off (t, (thr_job_t)job_mat_vec_transmult<T>,
                            st, en, &tv, this, &v, (void*)0);
       }
       do_mat_vec_transmult<T> (0, first, &tv, this, &v);
       //sched_yield();
       for (unsigned s = 0; s < n_thr-1; ++s)
          thread_wait (s);
     }
   return tv;
}

#else /* SMP */

template <typename T>
TVector<T> Matrix<T>::operator * (const Vector<T>& v) const
{
        TVector<T> tv (this->row);
        BCHK (v.size() != this->col, MatErr, multiply Matrix w/ Vector: wrong config, v.size(), tv);
        do_mat_vec_mult (0, this->row, &tv, this, &v);
        return tv;
}

template<typename T>
TVector<T> Matrix<T>::transMult (const Vector<T>& v) const
{
        TVector<T> tv (this->col);
        BCHK (v.size() != this->row, MatErr, transMult Matrix w/ Vector: wrong config, v.size(), tv);
        do_mat_vec_transmult<T> (0, this->col, &tv, this, &v);
        return tv;
}


#endif /* SMP */

/* No SMP version for Mat * TSV for now */
template <typename T>
TVector<T> Matrix<T>::operator * (const TSVector<T>& tsv) const
{
        TVector<T> tv (this->row);
        BCHK (tsv.size() != this->col, MatErr, multiply Matrix w/ Vector: wrong config, tsv.size(), tv);
        do_mat_tsv_mult (0, this->row, &tv, this, &tsv);
        return tv;
}


template <typename T>
inline BVector<T>& bvfillm (BVector<T>& bv, const Matrix<T>& m)
{
  BCHK (bv.dim != m.dim, VecErr, Matrix size does not fit Vector, bv.dim, bv);
  TBCICOPY (bv.vec, m.vec, T, bv.dim);
  return bv;
}




/* Should be added for cplx, too */

template <typename T>
Matrix<T>& Matrix<T>::mult_rows (const Vector<T>& v)
{
   BCHK (this->row != v.size(), MatErr, multiply rows: wrong sizes, v.size(), *this);
   for (unsigned r = 0; r < this->row; r++)
     {
        T fac = v (r);
        for (unsigned c = 0; c < this->col; c++)
          this->operator() (r, c) *= fac;
     }
   return *this;
}

template <typename T>
Matrix<T>& Matrix<T>::div_rows (const Vector<T>& v)
{
   BCHK (this->row != v.size(), MatErr, multiply rows: wrong sizes, v.size(), *this);
   for (unsigned r = 0; r < this->row; r++)
     {
        T fac = (T)1.0 / v (r);
        for (unsigned c = 0; c < this->col; c++)
          (*this) (r, c) *= fac;
     }
   return *this;
}

template <typename T>
Matrix<T>& Matrix<T>::mult_row (const T& f, const unsigned r)
{
   BCHK (r>this->row, MatErr, mult_row: wrong index, r, *this);
   for (unsigned c = 0; c < this->col; c++)
     (*this) (r, c) *= f;
   return *this;
}
   
template <typename T>
Matrix<T>& Matrix<T>::div_row (const T& d, const unsigned r)
{
   BCHK (r>this->row, MatErr, mult_row: wrong index, r, *this);
   BCHK (d == (T)0, MatErr, div_row by zero, r, *this);
   T f = (T)1.0 / d;
   for (unsigned c = 0; c < this->col; c++)
     (*this) (r, c) *= f;
   return *this;
}


/* New KG, 98/06/29 */
/* Helper class for [] operator */
template <typename T>
class Mat_Brack
{
 private:
  const TMatrix<T> & tmat;
  unsigned int idx;
 public:
  friend class TVector<T>;
  friend class  Vector<T>;
  friend class TMatrix<T>;
 private:
  Mat_Brack (const TMatrix<T> &tm, unsigned int ix) : tmat(tm), idx (ix) {}
  Mat_Brack (const Mat_Brack<T>& m) : tmat(m.tmat), idx(m.idx) {}
 public:
  ~Mat_Brack() { tmat.mark_destroy(); }
  inline const T& operator [] (unsigned int j) const
    { EXPCHK(j >= tmat.col, MatErr, Idx2 out of range, j, tmat.mat[idx][0]); 
      return tmat.mat[idx][j]; }
  inline T& operator [] (unsigned int j)
    { EXPCHK(j >= tmat.col, MatErr, Idx2 out of range, j, tmat.mat[idx][0]); 
      return tmat.mat[idx][j]; }
};

template <typename T>
inline TVector<T>::TVector (const Mat_Brack<T>& mb)
{
        this->vec = mb.tmat.mat[mb.idx];
        this->dim = (unsigned long)mb.tmat.col; this->keep = true;
}

template <typename T>
inline Vector<T>::Vector (const Mat_Brack<T>& mb)
{
        this->dim = (unsigned long)mb.tmat.col;
        this->keep = false; 
        if (LIKELY(this->dim)) {
                this->vec = NEW (T, this->dim);
                if (LIKELY(this->vec)) {
                        TBCICOPY (this->vec, mb.tmat.mat[mb.idx], T, this->dim);
                        return;
                }
        }
        this->dim = 0; this->vec = (T*)0;
}
#ifndef TBCI_NEW_BRACKET
template <typename T>
inline Mat_Brack<T> TMatrix<T>::operator [] (const unsigned int ix) const
{
  EXPCHK(ix >= row, MatErr, Idx1 out of range, ix, Mat_Brack<T> (*this, 0));
  return Mat_Brack<T> (*this, ix);
}
#else
template <typename T>
inline T* TMatrix<T>::operator [] (const unsigned int ix) const
{
        EXPCHK(ix >= row, MatErr, Idx1 out of range, ix, (T*)0);
        return mat[ix];
}
#endif


#undef _DIM
#undef _VEC
#undef _ENDVEC
#undef _ROW
#undef _COL
#undef _FAC

/* GCC-4 ... 7 use FMA in ways that hurt perf. for lu_decomp */
#if defined(__GNUC__) && __GNUC__ >= 5 && __GNUC__ < 8 && (defined(__x86_64__) || defined(__i386__))
# define NOFMA __attribute__((target("no-fma")))
#else
# define NOFMA
#endif

template <typename T> int lu_decomp (Matrix<T>&) HOT NOFMA;


template <typename T>
class mat_fill_fn {
    public:
        T (*fn)(const unsigned i1, const unsigned i2, void* par);
        mat_fill_fn(T (*f)(const unsigned, const unsigned, void*))
        : fn(f) {};
};

template <typename T>
void do_fill_mat(const unsigned firstrow, const unsigned lastrow, Matrix<T>* mat, mat_fill_fn<T> fn, void* par)

{
        const unsigned cols = mat->columns();
        //printf("do_fill_mat(%i->%i)\n", firstrow, lastrow);
        for (unsigned r = firstrow; r < lastrow; ++r)
                for (REGISTER unsigned c = 0; c < cols; ++c)
                        (*mat)(r,c) = fn.fn(r, c, par);
}

#ifdef SMP
template <typename T>
inline void job_fill_mat(struct thr_ctrl *tc)
{
  do_fill_mat<T> (tc->t_off, tc->t_size,
                  (Matrix<T>*)(tc->t_par[0]), 
                  *(mat_fill_fn<T>*)(tc->t_par[1]),
                  tc->t_par[2]);
}

template <typename T>
void par_fill(Matrix<T>& mat, mat_fill_fn<T> fn, void* par)
{
  /* use some heuristic to decide for the num of threads */
  const unsigned rows = mat.rows();
  const unsigned n_thr = threads_avail (rows / SMP_MATSLICE2);
  update_n_thr(n_thr);
  //std::cout << "Mat*Vec: " << n_thr << " threads" << std::endl;
  if (LIKELY(n_thr < 2))
      do_fill_mat<T> (0, rows, &mat, fn, par);
  else {
      PREFETCH_W(&mat.setval(0,0), 3);
      const unsigned long first = slice_offset(1, n_thr, rows, (T*)0);
      unsigned long st, en = first;           
      /* Start threads */
      for (unsigned t = 0; t < n_thr-1; ++t) {
                st = en; en = slice_offset(t+2, n_thr, rows, (T*)0);
                thread_start_off (t, (thr_job_t)job_fill_mat<T>,
                                  st, en, &mat, (void*)&fn, par, (void*)0);
          }
          /* The first slice is handled by the main thread */
          do_fill_mat<T> (0, first, &mat, fn, par);
          //sched_yield ();
          /* Wait for the end */
          for (unsigned s = 0; s < n_thr-1; ++s)
                  thread_wait (s);
  }
}

#else
template <typename T>
void par_fill(Matrix<T>& mat, mat_fill_fn<T> fn, void* par)
{
        do_fill_mat<T> (0, mat.rows(), &mat, fn, par);
}
#endif

#if defined(SMP) && defined(HAVE_LIBNUMA)
template <typename T>
int numa_optimize(const Matrix<T>& m, bool fault_in)
{
        if (!numa_avail && !fault_in)
                return 0;
        /* use some heuristic to decide for the num of threads */
        const unsigned rows = m.rows();
        const unsigned n_thr = threads_avail (rows / SMP_MATSLICE2);
        //update_n_thr(n_thr);
        //smp_barrier();
        
        if (LIKELY(n_thr < 2)) {
                if (fault_in)
                        do_numa_move_pages(main_numa_node, 1,
                                (unsigned long)m.getrowptr(0), 
                                (unsigned long)(m.getrowptr(rows-1)+m.columns()));      
                return 0;
        } else {
                const unsigned first = slice_offset(1, n_thr, rows, (T*)0);
                unsigned long st, en = first;
                /* Start threads */
                unsigned long res = 0;
                for (unsigned t = 0; t < n_thr-1; ++t) {
                        st = en; en = slice_offset(t+2, n_thr, rows, (T*)0);
                        const T* endptr = (en == rows? m.getrowptr(en-1)+m.columns(): m.getrowptr(en));
                        thread_start_off (t, (thr_job_t)numa_move_pages_job, 
                                          threads[t].numa_node, fault_in,
                                          m.getrowptr(st), endptr, (void*)0);
                }
                /* The first slice is handled by the main thread */
                res = do_numa_move_pages(main_numa_node, fault_in,
                                (unsigned long)(m.getrowptr(0)), 
                                (unsigned long)(m.getrowptr(first)));
                //sched_yield ();
                /* Wait for the end */
                for (unsigned t = 0; t < n_thr-1; ++t) {
                        job_output out; 
                        thread_wait (t, &out);
                        res += out.t_res_l;
                }
                //fprintf(stderr, "NUMA Optimize Matrix %p: %li pages moved\n", m.getrowptr(0), res); 
                return res;
        }
}
#else
template <typename T>
int numa_optimize(const Matrix<T>& m, bool fault_in)
{
        /* FIXME: We should emulate fault_in here, no? */
        return 0;
}
#endif


NAMESPACE_END

#endif /* TBCI_MATRIX_H */