band_matrix.h

Go to the documentation of this file.

// Last change:  98/11/03 KG
/* $Id: band_matrix.h,v 1.64.2.119 2022/11/03 17:28:10 garloff Exp $ */

#ifndef TBCI_BAND_MATRIX_H
#define TBCI_BAND_MATRIX_H  

//#define BDMAT_NEW_ALLOC

#include "tbci/basics.h"
#include "tbci/vector.h"
#include "tbci/matrix.h"

// Important: Values smaller than this will be considered ZERO
// The default is reasonable for doubles which are of order 1
#ifndef BD_MINVAL
# define BD_MINVAL 1e-16
#endif

#ifndef BDMAT_NEW_ALLOC
# define BDMAT_ONE_BLOCK
#endif

#ifdef BDMAT_MEM_TRACE
# define BDMATDBG(x) x
#else
# define BDMATDBG(x)
#endif

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

NAMESPACE_TBCI

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

class BdMatrixErr : public NumErr
{
    public:
        BdMatrixErr() 
        : NumErr("Error in var conf Band_Matrix class") {}
        BdMatrixErr(const char* t, const long i = 0) 
        : NumErr(t, i) {}
        BdMatrixErr(const BdMatrixErr& be)
        : NumErr(be) {}
        virtual ~BdMatrixErr() throw() {}
};
#endif  /* TBCI_DISABLE_EXCEPT */

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

// Forward - declaration
template <typename T> class ILU0_BdMatrixPreconditioner;
template <typename T> class DILU_BdMatrixPreconditioner;

template <typename T> 
class BdMatrix : public Matrix_Sig<T>
{
        friend class ILU0_BdMatrixPreconditioner<T>;
        friend class DILU_BdMatrixPreconditioner<T>;
    protected:

        unsigned int  dim, maxoff;      
        unsigned long arsz;             
        BVector <unsigned> diagconf;    
        BVector <T*> adiag, bdiag;      
        T* diag;
        //BVector<unsigned int> bestrow;

        //protected constructor
        void constructor(const unsigned int, const bool = true);
        T* check(const unsigned r, const unsigned c) const HOT;
        T* check_internal(const unsigned r, const unsigned c) const HOT;
        inline void free_diags(const unsigned);
        inline void do_copy(const BdMatrix<T>&);

    private:
        mutable T dummy ALIGN(MIN_ALIGN);
        mutable int outw, outp, outf;
        mutable bool outset;

        inline bool search2(unsigned int&, const unsigned int, const unsigned int) HOT;

    public:

        typedef T value_type; 
        typedef T element_type; 
        typedef T aligned_value_type TALIGN(MIN_ALIGN2); 
        // constructors and destructors

        explicit BdMatrix(const unsigned int = 0);
        explicit BdMatrix(const unsigned int, const unsigned int);
        explicit BdMatrix(const unsigned int, const BVector<unsigned int>&);
        BdMatrix(const BdMatrix<T>&);
        BdMatrix(const T&, const unsigned int);
        BdMatrix(const T&, const unsigned int, const BVector<unsigned int>&);
        BdMatrix(const T&, const unsigned int, unsigned int);
        BdMatrix(const Matrix<T>&);
        ~BdMatrix() THROWEXCEPT; 


        void destroy()  
        { 
                free_diags(diagconf.size());
                dim = 0; arsz = 0;
                adiag.resize(0); bdiag.resize(0); diagconf.resize(0);
        }

        // allow instantiation (Matrix_Sig)
        /*virtual*/ static const char* mat_info()
        { return "BdMatrix"; }

#ifndef HAVE_PROMOTION_BUG
        // Promotion (only explicit)
# ifndef HAVE_GCC295_TMPLFRNDCLS_BUG
        template <typename U> friend class BdMatrix;
        template <typename U> explicit BdMatrix(const BdMatrix<U>& bm)
        : diagconf(bm.diagconf), adiag(bm.dim), bdiag(bm.dim)
        { 
                constructor(bm.dim);
                for (unsigned k=0; k<dim; ++k)
                        diag[k] = bm.diag[k];
                for (unsigned i=0; i<diagconf.size(); ++i) {
                        const unsigned di = diagconf.get(i);
                        for (unsigned j=0; j<dim-di; ++j) {
                                adiag.get(di)[j] = bm.adiag.get(di)[j];
                                bdiag.get(di)[j] = bm.bdiag.get(di)[j];
                        }
                }
                if (bm.outset) { 
                        outw = bm.outw; outp = bm.outp; 
                        outf = bm.outf; outset = true; 
                }
        }
# endif
#endif

        operator TMatrix<T>() const;
        //operator  Matrix<T>() const;
   
        T& operator() (const unsigned int, const unsigned int) HOT;
        typename tbci_traits<T>::const_refval_type
           operator() (const unsigned int, const unsigned int) const HOT;
        typename tbci_traits<T>::const_refval_type
           get(const unsigned int, const unsigned int) const HOT;

        BdMatrix<T>& setval(const T& v, const unsigned int r,
                            const unsigned int c)
        { return autoinsert(v,r,c); }
        T& setval(const unsigned int r, const unsigned int c);
        //const T& getcref(const unsigned r, const unsigned c) const
        //{ return (*this)(r,c); }

        //T val(const unsigned int i, const unsigned int j) const
        //  { return this->operator() (i, j); }

        // Basic operations
        BdMatrix<T>& transpose();

        BdMatrix<T>& resize(const unsigned int);
        BdMatrix<T>& resize(const T&, const unsigned int);
        BdMatrix<T>& resize(const unsigned int, const BVector<unsigned int>&);
        BdMatrix<T>& resize(const T&, const unsigned int, const BVector<unsigned int>&);
        BdMatrix<T>& resize(const BdMatrix<T>& bdm)
        { resize(bdm.dim, bdm.diagconf); return *this = bdm; }

        BdMatrix<T>& reconfig(const BVector<unsigned int>&) COLD;
        BdMatrix<T>& setunit(const T& = 1);  //unity Matrix
        BdMatrix<T>& fill(const T& = 0);
        BdMatrix<T>& clear();
        BdMatrix<T>& adddiag(const unsigned);
        BdMatrix<T>& removediag(const unsigned);
        BdMatrix<T>& autoinsert(const T&, const unsigned, const unsigned);
        BdMatrix<T>& expand(unsigned = 0);
        BdMatrix<T>& set_row(const Vector<T>& val, const unsigned, const unsigned = 0);
        TVector<T> get_row(const unsigned int) const;
        TVector<T> get_col(const unsigned int) const;

        inline unsigned int size() const  { return dim; }
        inline unsigned int noelem()  const  { return arsz; }
        inline unsigned int rows() const  { return dim; }
        inline unsigned int columns() const  { return dim; }

        const BVector<unsigned int>& getcfg() const  { return diagconf; }
        unsigned int getmaxoff() const  { return maxoff; }

        T trace() const 
        { 
                T r ALIGN(MIN_ALIGN) = 0; 
                for (REGISTER unsigned i = 0; i < dim; ++i) 
                        r += diag[i]; 
                return r; 
        }
        //T sdet(unsigned int, BVector<unsigned int>&, BVector<unsigned int>&);
        //T subdet(BVector<unsigned int>, BVector<unsigned int>);
        //T det()  { return subdet(BVector<unsigned>(0), BVector<unsigned>(0)); }

        BdMatrix<T>& operator = (const BdMatrix<T>&);
        BdMatrix<T>& operator = (const T&);
        BdMatrix<T>& operator += (const BdMatrix<T>&);
        BdMatrix<T>& operator -= (const BdMatrix<T>&);
        //BdMatrix<T>& operator *= (BdMatrix<T>&);
        BdMatrix<T>& operator *= (const T&);
        BdMatrix<T>& operator /= (const T&);

        // Don't need friendship here. Good!
#ifdef BDMAT_TRANS_FRIENDS
        friend NOINST BdMatrix<T> transpose FGD (BdMatrix<T>&);
#endif

        bool operator == (const BdMatrix<T>&) const;
        bool operator != (const BdMatrix<T>& ma) const { return !(*this == ma); }

        BdMatrix<T> operator + (const BdMatrix<T>&) const;
        BdMatrix<T> operator - (const BdMatrix<T>&) const;
        BdMatrix<T> operator * (const BdMatrix<T>&) const;
        BdMatrix<T> operator * (const T&) const;
        // Friendship also not required
#ifdef BDMAT_OP_FRIENDS
        friend NOINST BdMatrix<T> operator * FGD (const T&, const BdMatrix<T>&);
#endif

        BdMatrix<T> operator / (const T&) const;
        BdMatrix<T> operator - () const;

        /*
         * friend Vector<T> rvec FGD (const BdMatrix<T>&);
         * friend Vector<T> cvec FGD (const BdMatrix<T>&);
         */
        
        TVector<T> operator * (const Vector<T>&) const HOT;
        TVector<T> transMult(const Vector<T>&) const HOT;
        //TVector<T> transMult(const TVector<T>&) const;
        //TVector<T> transMult(const TSVector<T>&) const;
        TVector<T> dotMult(const Vector<T>&) const;

        TMatrix<T> operator * (const Matrix<T>&) const;
        
        BdMatrix<T>& mult_rows(const Vector<T>&);
        BdMatrix<T>& div_rows(const Vector<T>&);
        BdMatrix<T>& mult_row(const T&, const unsigned);
        BdMatrix<T>& div_row(const T&, const unsigned);

        // I/O operations

        friend STD__ istream& operator >> FGD (STD__ istream&, BdMatrix<T>&);
        friend STD__ ostream& operator << FGD (STD__ ostream&, const BdMatrix<T>&);
        const BdMatrix<T>& setoutopts(int w = 5, int p = 4, char f= ' ') const
        { outw = w; outp = p; outf = f; outset = true; return *this; }

        // Solver
        friend NOINST void /* FRIEND_TBCI2__ */ gaussj FGDT (const BdMatrix<T>&,
                                                             const Matrix<T>&);
        friend NOINST int  /* FRIEND_TBCI2__ */ lu_decomp FGDT (BdMatrix<T>&) HOT;

        // Inversion by determinants
        BdMatrix<T> inverse() const;

        // Friends for bdmat-vec mult
        friend void FRIEND_TBCI2__ do_bdmat_vec_mult_lnw FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *, const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_mult_lnw_opt FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *, const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_mult_diagw_exact FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *, const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_mult FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *, const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_transmult_lnw FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *,  const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_transmult_lnw_opt FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *,  const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_transmult_diagw_exact FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *,  const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_transmult FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *,  const Vector<T> *);
        friend void FRIEND_TBCI2__ do_bdmat_vec_dotmult FGDT (const unsigned, const unsigned,
                        TVector<T> *, const BdMatrix<T> *,  const Vector<T> *);
};


#ifdef BDMAT_ONE_BLOCK

template <typename T> 
inline void BdMatrix<T>::free_diags(const unsigned dummy)
{
        BDMATDBG(STD__ cerr << "BdMat free " << diag << " (" << dim << ")" << STD__ endl);
        if (dim)
                TBCIDELETE(T, diag, arsz);
}

template <typename T> 
inline void BdMatrix<T>::do_copy(const BdMatrix<T>& bdm)
{
        TBCICOPY(diag, bdm.diag, T, arsz);
        // In case standard setup has been changed, e.g. by transpose
        for (unsigned i = 0; i < diagconf.size(); ++i) {
                const unsigned di = diagconf.get(i);
                adiag.set(di) = (T*)((long)diag + (long)bdm.adiag.get(di) - (long)bdm.diag);
                bdiag.set(di) = (T*)((long)diag + (long)bdm.bdiag.get(di) - (long)bdm.diag);
        }
}

template <typename T>
void BdMatrix<T>::constructor(const unsigned int d, const bool delout)
{
        dim = d; maxoff = 0;
        //maxoff = findmax(diagconf);
        BCHKNR(dim > 0 && maxoff >= dim, BdMatrixErr, Wrong config Vector, maxoff);
        adiag.resize((T*)0, d); bdiag.resize((T*)0, d); //initialization with NULL Vectors
        unsigned long sz = dim; int t;
        const unsigned long dcs = diagconf.size();
        for (REGISTER unsigned j = 0; j < dcs; ++j)
                if ((t = ((signed)dim - (signed)diagconf.get(j))) > 0) 
                        sz += 2 * t;
        if (UNLIKELY(sz)) {
                diag = NEW(T, sz);
                if (LIKELY(diag)) {
                        adiag.set(0) = diag; bdiag.set(0) = diag; sz = dim;
                        for (unsigned i = 0; i < dcs; ++i) {
                                const unsigned int di = diagconf.get(i);
                                int s = (signed)dim - (signed)di;
                                if (UNLIKELY(s <= 0 || s >= (signed)dim)) {
                                        STD__ cerr << "Wrong config. Vector supplied!" << STD__ endl;
                                        s = 0;
                                } else {
                                        adiag.set(di) = diag+sz; sz += s;
                                        bdiag.set(di) = diag+sz; sz += s;
                                }
                                if (LIKELY(di > maxoff))
                                        maxoff = di;
                        }
                } else { // alloc failure
                        adiag.resize(0); bdiag.resize(0); dim = 0;
                        STD__ cerr << "BdMat ctor: malloc failed to get " << sz << " bytes!" << STD__ endl;
                        BCHKNR(!diag, BdMatrixErr, memory alloc failure, sz);
                        sz = 0;
                }
        }
        else 
                diag = (T*)0;
        
        arsz = sz; dummy = (T)0;
        BDMATDBG(STD__ cerr << "BdMat alloc " << diag << " (" << sz << ")" << STD__ endl);
        if (delout) 
                outset = false;
}

#else /* ! BDMAT_ONE_BLOCK */

/* Free diags until diagconf(max)-1 */
template <typename T> 
inline void BdMatrix<T>::free_diags(const unsigned u)
{
        for (int i = u-1; i >= 0; --i) {
                // The diags could have been swapped by transpose ()
                REGISTER const unsigned di = diagconf.get(i);
                REGISTER T* ptr = MIN(adiag.get(di), bdiag.get(di));
                BDMATDBG(STD__ cerr << "BdMat free " << ptr << STD__ endl);
                TBCIDELETE(T, ptr, 2*(dim-di));
        }
        BDMATDBG(STD__ cerr << "BdMat free " << diag << " (" << dim << ")" << STD__ endl);
        if (LIKELY(dim))
                TBCIDELETE(T, diag, dim);
}

template <typename T> 
inline void BdMatrix<T>::do_copy(const BdMatrix<T>& bdm)
{
        TBCICOPY(diag, bdm.diag, T, dim);
        for (unsigned i = 0; i < diagconf.size(); ++i) {
                REGISTER const unsigned di = diagconf.get(i);
                TBCICOPY(adiag.get(di), bdm.adiag.get(di), T, (dim-di));
                TBCICOPY(bdiag.get(di), bdm.bdiag.get(di), T, (dim-di));
        }
}

template <typename T>
void BdMatrix<T>::constructor(const unsigned int d, const bool delout)
{ 
        unsigned long sz = 0; unsigned i = 0;
        dim = d; maxoff = 0;
        //maxoff = findmax(diagconf);
        BCHKNR (dim > 0 && maxoff >= dim, BdMatrixErr, Wrong config Vector, maxoff);
        //initialization with NULL Vectors
        adiag.resize((T*)0, d); 
        bdiag.resize((T*)0, d);
        
        if (LIKELY(!dim)) 
                diag = (T*)0;
        else {
                diag = NEW(T,dim);
                if (UNLIKELY(!diag))
                        goto memallocfail;
                sz += dim;
                adiag.set(0) = diag; bdiag.set(0) = diag; // paranoia
        }
        
        while (i < diagconf.size()) {
                const unsigned di = diagconf.get(i);
                int s = (signed)dim - (signed)di;
                if (UNLIKELY(s <= 0 || s >= (signed)dim)) {
                        BCHKNR(true, BdMatrixErr, Illegal Config BVector, di);
                        // Try to salvage
                        diagconf.remove(i);
                } else {
                        T* ptr = NEW(T, 2*s);
                        if (UNLIKELY(!ptr))
                                goto memallocfail;
                        adiag.set(di) = ptr;
                        bdiag.set(di) = ptr+s;
                        if (LIKELY(di > maxoff))
                                maxoff = di;
                        sz += 2*s; ++i;
                }
        }
        arsz = sz; dummy = (T)0;
        if (delout) 
                outset = false;
        BDMATDBG(STD__ cerr << "BdMat alloc " << diag << " (" << sz << ")" << STD__ endl);
        return;
    memallocfail:
        free_diags(i);
        adiag.resize(0); bdiag.resize(0); dim = 0; //sz = 0;
        STD__ cerr << "BdMat ctor: mem alloc (" << sz << ") failed!" << STD__ endl;
        BCHKNR(1, BdMatrixErr, mem alloc failure, sz);
        return;
}

#endif /* BDMAT_ONE_BLOCK */

template <typename T>
inline BdMatrix<T>::BdMatrix(const unsigned int d)
: diagconf(1, 1), adiag(d), bdiag(d)
{
        if (UNLIKELY(d <= 1))
                diagconf.resize(0);             // default: Tridiagonal shape
        constructor(d); clear();
}

template <typename T>
inline BdMatrix<T>::BdMatrix(const unsigned int d, const unsigned int e) 
: diagconf(1, 1), adiag(d), bdiag(d)
{ 
        BCHKNR(e != d, BdMatrixErr, "Non square BdMatrix shape impossible!", e);
        if (UNLIKELY(d <= 1))
                diagconf.resize(0);             // default: Tridiagonal shape
        constructor(d); clear();
}

template <typename T>
inline BdMatrix<T>::BdMatrix(const unsigned int d, const BVector<unsigned int>& conf)
: diagconf(conf), adiag(d), bdiag(d)
{ 
        constructor(d); clear();
}


template <typename T>
inline BdMatrix<T>::BdMatrix(const BdMatrix<T>& mat)
: diagconf(mat.diagconf), adiag(mat.dim), bdiag(mat.dim)
{
        constructor(mat.dim); do_copy(mat);

        if (UNLIKELY(mat.outset)) {
                outw = mat.outw; outp = mat.outp;
                outf = mat.outf; outset = true;
        }
}

template <typename T>
inline BdMatrix<T>::BdMatrix(const T& val, const unsigned int d)
: diagconf(1, 1), adiag(d), bdiag(d) 
{
        if (UNLIKELY(d <= 1)) 
                diagconf.resize(0);             // default: Tridiagonal shape
        constructor(d);
        setunit(val);
}

template <typename T>
inline BdMatrix<T>::BdMatrix(const T& val, const unsigned int d, unsigned int mo)
: diagconf(mo), adiag(d), bdiag(d)
{
        if (mo >= d)
                mo = d - 1; 
        diagconf.resize(mo);
        for (REGISTER unsigned int t = 0; t < mo; ++t)
                diagconf.set(t) = t+1;

        constructor(d);
        setunit(val);
}


template <typename T>
inline BdMatrix<T>::BdMatrix(const T& val, const unsigned int d, 
                             const BVector<unsigned int>& conf)
: diagconf(conf), adiag(d), bdiag(d)
{
        constructor(d);
        setunit(val);
}


template <typename T>
BdMatrix<T>::BdMatrix(const Matrix<T>& mat) 
: diagconf(0, 0), adiag(mat.rows()), bdiag(mat.columns())
{
        BCHKNR(mat.columns() != mat.rows(), BdMatrixErr, Only quadratic matrices, 0);

        //set up conf Vector
        unsigned int ds = 0;
        for (unsigned int l = 1; l < mat.columns(); ++l) {
                for (unsigned REGISTER j = 0; 
                     (signed)j < ((signed)(mat.rows() - l)); ++j) {
                        if (mat(j, j+l) != T(0) || mat(j+l, j) != T(0)) {
                                diagconf.resize(++ds);
                                diagconf.set(ds - 1) = l;
                                break;
                        }
                }
        }
        
        //std::cout << diagconf << std::endl;
        constructor(mat.columns());
        
        //copy diagonal
        for (REGISTER unsigned int k = 0; k < dim; ++k)
                diag[k] = mat(k, k);
        //copy configured offdiags
        for (unsigned int i = 0; i < diagconf.size(); ++i) {
                const unsigned int dci = diagconf.get(i);
                for (REGISTER unsigned int j = 0; j < dim - dci; ++j) {
                        adiag.get(dci)[j] = mat(j, j + dci);
                        bdiag.get(dci)[j] = mat(dci + j, j);
                }
        }
}

template <typename T>
inline BdMatrix<T>::~BdMatrix() THROWEXCEPT
{ 
        BCHKNR(dummy!=(T)0, BdMatrixErr, dummy not zero, 0);
        free_diags(diagconf.size());
}


template <typename T>
inline bool BdMatrix<T>::search2(unsigned int& cfg, const unsigned int i,
                                 const unsigned int j)
{
        for (REGISTER int t = 0; (unsigned)t < diagconf.size(); ++t) {
                if (UNLIKELY(CSTD__ abs((int)(j - i)) == (int)diagconf.get(t))) {
                        cfg = t; 
                        return true; 
                }
        }
        return false;
}

template <typename T>
inline T* BdMatrix<T>::check_internal(const unsigned i, const unsigned j) const
{
        const int df = i-j;
        if (LIKELY(!df)) 
                return diag+i;
        if (CSTD__ abs(df) > (int)maxoff)
                return (T*)0;

        T* ptr;
        if (df > 0) {
                if (LIKELY(ptr = bdiag.get( df))) 
                        return ptr+j;
                else 
                        return (T*)0;
        } else {
                if (LIKELY(ptr = adiag.get(-df)))
                        return ptr+i;
                else 
                        return (T*)0;
        }
}

template <typename T>
inline T* BdMatrix<T>::check(const unsigned i, const unsigned j) const
{
        EXPCHK(i>=dim, BdMatrixErr, Invalid row index, i, (T*)0);
        EXPCHK(j>=dim, BdMatrixErr, Invalid col index, j, (T*)0);
        return check_internal(i, j);
}


template <typename T>
inline T& BdMatrix<T>::operator() (const unsigned int r, const unsigned int c)
{
        REGISTER T* pt = check(r, c);
        if (LIKELY(pt))
                return *pt;
        else {
                //EXPCHK(dummy != (T)0, BdMatrixErr, dummy not zero, 0, (dummy=(T)0));
                return (dummy = (T)0);
        }
}

template <typename T>
inline typename tbci_traits<T>::const_refval_type 
BdMatrix<T>::operator() (const unsigned int r, const unsigned int c) const
{
        REGISTER T* pt = check(r, c);
        if (LIKELY(pt))
                return *pt;
        else {
                //EXPCHK(dummy != (T)0, BdMatrixErr, dummy not zero, 0, (dummy=(T)0));
                return (dummy = (T)0);
        }
}

template <typename T>
inline typename tbci_traits<T>::const_refval_type 
BdMatrix<T>::get(const unsigned int r, const unsigned int c) const
{
        REGISTER T* pt = check_internal(r, c);
        if (LIKELY(pt))
                return *pt;
        else {
                //EXPCHK(dummy != (T)0, BdMatrixErr, dummy not zero, 0, (dummy=(T)0));
                return (dummy = (T)0);
        }
}


template <typename T>
T& BdMatrix<T>::setval(const unsigned int r, const unsigned int c)
{
        //Auto-insert
        REGISTER T* pt = check(r, c);
        if (LIKELY(pt))
                return *pt;
        else {
                adddiag(CSTD__ abs((int)(r-c))); 
                pt = check(r, c);
                BCHK(!pt, BdMatrixErr, Internal autoinsert error, 0, dummy=(T)0);
                //*pt = (T)0;
        }
        return *pt;
}


#ifdef BDMAT_ONE_BLOCK
template <typename T>
inline BdMatrix<T>& BdMatrix<T>::setunit(const T& val)
{
        TBCIFILL(diag, val, T, dim);
        if (arsz > dim) 
                TBCIFILL((diag+dim), (T)0, T, (arsz-dim));
        dummy = (T)0;
        return *this;
}
#else
template <typename T>
inline BdMatrix<T>& BdMatrix<T>::setunit(const T& val)
{
        TBCIFILL(diag, val, T, dim);
        for (unsigned i = 0; i < diagconf.size(); ++i) {
                const REGISTER unsigned di = diagconf.get(i);
                TBCIFILL(adiag.get(di), (T)0, T, (dim-di));
                TBCIFILL(bdiag.get(di), (T)0, T, (dim-di));
        }
        dummy = (T)0;
        return *this;
}
#endif

template <typename T>
BdMatrix<T>& BdMatrix<T>::resize(const unsigned int nd)
{
        if (nd == dim) 
                return *this;

        T* oldd(diag); unsigned int od = dim; 
#ifdef BDMAT_ONE_BLOCK  
        unsigned int oldar = arsz;
#endif  
        BVector<T*> oldad(adiag), oldbd(bdiag);
        BVector<unsigned> oldconf(diagconf);

        //compatibility with constructor
        if (od <= 1 && nd > 1) 
                diagconf.resize(1, 1);
        constructor(nd, false); //dim = nd;
        int min = MIN(nd, od);

        if (min) {
                //copy diagonal
                TBCICOPY(diag, oldd, T, min);
                if (nd > od) 
                        TBCIFILL((diag+min), (T)0, T, (nd-dim));
                //copy offdiags
                for (unsigned int t = 0; t < diagconf.size(); ++t) {
                        const REGISTER int s = diagconf.get(t);
                        if (min - s > 0) {
                                TBCICOPY(adiag.get(s), oldad.get(s), T, (min-s));
                                TBCICOPY(bdiag.get(s), oldbd.get(s), T, (min-s));
                                if (nd > od) {
                                        TBCIFILL((adiag.get(s)+(min-s)), (T)0, T, (nd-dim));
                                        TBCIFILL((bdiag.get(s)+(min-s)), (T)0, T, (nd-dim));
                                }
                        }
                }
        }

#ifndef BDMAT_ONE_BLOCK
        for (unsigned i = 0; i < oldconf.size(); ++i)
                TBCIDELETE(T, MIN(oldad.get(oldconf.get(i)), oldbd.get(oldconf.get(i))), 
                           2*(od-oldconf.get(i)));
        if (od) 
                TBCIDELETE(T, oldd, od);
#else
        if (oldar) 
                TBCIDELETE(T, oldd, oldar);
#endif
        return *this;
}


template <typename T>
BdMatrix<T>& BdMatrix<T>::resize(const T& val, const unsigned int nd)
{
        if (nd != dim) {
                if (dim) 
                        free_diags(diagconf.size());
                //compatibility with constructor
                if (dim <= 1 && nd > 1) 
                        diagconf.resize(1, 1);
                constructor(nd, false);
        }
        setunit(val);
        return *this;
}


//Warning: This resize() will NOT preserve data
template <typename T>
BdMatrix<T>& BdMatrix<T>::resize(const unsigned int nd, const BVector<unsigned int>& cfg)
{
        if (nd == dim && cfg == diagconf)
                return *this;
        if (dim) 
                free_diags(diagconf.size());
        diagconf.copy(cfg);
        constructor(nd, false);
        clear();
        return *this;
}


template <typename T>
BdMatrix<T>& BdMatrix<T>::resize(const T& val, const unsigned int nd,
                                 const BVector<unsigned int>& cfg)
{
        resize(nd, cfg);
        //setunit(val);
        //clear(); is called by resize(nd, cfg);
        for (unsigned i = 0; i < dim; ++i)
                diag[i] = val;
        return *this;
}


//reconfigures a BdMatrix and preserves its contents
template <typename T>
BdMatrix<T>& BdMatrix<T>::reconfig(const BVector<unsigned int>& cfg)
{
        if (dim == 0 || cfg == diagconf) 
                return *this;
        T* oldd = diag; 
#ifdef BDMAT_ONE_BLOCK
        unsigned int oldar = arsz;
#else   
        unsigned int olddim = dim;
#endif
        BVector<T*> oldad(adiag), oldbd(bdiag);
        BVector<unsigned> oldconf(diagconf);

        diagconf.resize(cfg); //diagconf = cfg;
        constructor(dim, false);
        //delete junk
        clear();
        //copy elements: diagonal
        TBCICOPY(diag, oldd, T, dim);
        // offdiag
        for (REGISTER unsigned t = 0; t < diagconf.size(); ++t) {
                const unsigned int dct = diagconf.get(t);
                if (oldad(dct)) {
                        TBCICOPY(adiag.get(dct), oldad.get(dct), T, (dim-dct));
                        TBCICOPY(bdiag.get(dct), oldbd.get(dct), T, (dim-dct));
                }
        }
#ifndef BDMAT_ONE_BLOCK
        for (unsigned i = 0; i < oldconf.size(); ++i)
                TBCIDELETE(T, MIN(oldad.get(oldconf.get(i)), oldbd.get(oldconf.get(i))), 
                           2*(olddim-oldconf.get(i)));
        if (oldd)
                TBCIDELETE(T, oldd, olddim);
#else
        if (oldar) 
                TBCIDELETE(T, oldd, oldar);
#endif
        return *this;
}

#ifdef BDMAT_ONE_BLOCK
template <typename T>
BdMatrix<T>& BdMatrix<T>::adddiag(const unsigned ndg)
{
        const int newsz = dim - ndg; /* no of elements in new diagonal */
        BCHK(newsz <= 0, BdMatrixErr, Diag out of range, ndg, *this);
        // Diag already stored ?
        if (LIKELY(diagconf.contains(ndg)))
                return *this;
        // Save old position (for diff)
        T* olddiag = diag;
        REALLOC(diag, arsz, T, (arsz + 2 * newsz));
        if (UNLIKELY(!diag)) {
                diag = olddiag;
                STD__ cerr << "Allocation error" << STD__ endl;
                return *this;
        }
        BDMATDBG(STD__ cerr << "BdMat realloc " << diag << " (" 
                            << arsz+2*newsz << ")" << STD__ endl);
        // Delete new elements
        // Calculate new pointers
        /* difference in units of sizeof(T) could fail and actually did, as NO
         * alignment is guaranteed by new ! Ughh ! Hard to find bug ! KG, 98/09/04
         * (Note: Stroustrup (3rd ed.) does not say anything about alignment for
         *  operator new, so I can't tell whether this is a bug or not.)
         * maybe overloading of the new operator would be a good idea to ensure 
         * alignment */
        long diff = (long)diag - (long)olddiag; //long is better for 64 bit platforms ..
        if (LIKELY(diff)) {
                //STD__ cout << "DEBUG: diag " << olddiag << " -> " << diag << " , ad0 " << adiag(0) << " -> " << adiag(0) + diff << STD__ endl;
                /* ANSI C++ is really stupid about forbidding pointer arithmetic,
                 * never produced such ugly code before! KG, 98/09/04 */
                adiag.set(0) = (T*)((long)adiag.get(0) + diff); 
                bdiag.set(0) = (T*)((long)bdiag.get(0) + diff);
                const unsigned dcs = diagconf.size();
                for (unsigned i = 0; i < dcs; ++i) {
                        const unsigned di = diagconf.get(i);
                        adiag.set(di) = (T*)((long)adiag.get(di) + diff);
                        bdiag.set(di) = (T*)((long)bdiag.get(di) + diff);
                }
                //STD__ cout << " -> " << adiag(0) << STD__ endl;
        }
        // Paranoia
        BCHKNR(adiag.get(0) != diag || bdiag.get(0) != diag, BdMatrixErr, adddiag: mismatch while doing pointer arithmetic, diag - adiag.get(0));
         // Set pointers for new diagonals
        diagconf.append(ndg);
        adiag.set(ndg) = diag+arsz; bdiag.set(ndg) = diag+arsz+newsz;
        TBCIFILL((diag+arsz), (T)0, T, (unsigned)(2*newsz));
        // New sizes ...
        if (ndg > maxoff) 
                maxoff = ndg;
        arsz += 2 * newsz;
        //STD__ cout << "DEBUG: diagconf, diag, arsz: " << diagconf << " - " << diag << " - "<< arsz << "\n adiag: " << adiag << "\n bdiag: " << bdiag << STD__ endl;
        return *this;
}

#else /* BDMAT_ONE_BLOCK */

template <typename T>
BdMatrix<T>& BdMatrix<T>::adddiag(const unsigned ndg)
{
        const int newsz = dim - ndg; /* no of elements in new diagonal */
        BCHK(newsz <= 0, BdMatrixErr, Diag out of range, ndg, *this);
        // Diag already stored ?
        if (LIKELY(diagconf.contains(ndg)))
                return *this;
        T* ptr = NEW(T, 2*newsz);
        if (UNLIKELY(!ptr)) {
                BCHK(1, BdMatrixErr, Allocation failed in adddiag, 2*newsz*sizeof(T), *this);
                STD__ cerr << "BdMat adddiag allocation failed!" << STD__ endl;
                return *this;
        }
        adiag.set(ndg) = ptr; bdiag.set(ndg) = ptr+newsz;
        TBCIFILL(ptr, (T)0, T, (2*newsz));
        diagconf.append(ndg);
        // New sizes ...
        if (ndg > maxoff) 
                maxoff = ndg;
        arsz += 2 * newsz;
        BDMATDBG(STD__ cerr << "BdMat adddiag " << ptr << " (" << arsz << ")" << STD__ endl);
        return *this;
}

#endif /* BDMAT_ONE_BLOCK */

// Only really functional for new (not ONE_BLOCK) mem alloc strategy
template <typename T>
BdMatrix<T>& BdMatrix<T>::removediag(const unsigned dg)
{
        if (dg > maxoff)
                return *this;
#ifdef BDMAT_ONE_BLOCK
        TBCIFILL(adiag.get(dg), (T)0, T, (dim-dg));
        TBCIFILL(bdiag.get(dg), (T)0, T, (dim-dg));
#else   
        if (adiag.get(dg)) {
                BDMATDBG(STD__ cerr << "BdMat rmvdiag " 
                                << MIN(adiag.get(dg), bdiag.get(dg)) << STD__ endl);
                TBCIDELETE(T, MIN(adiag.get(dg),bdiag.get(dg)), 2*(dim-dg));
                adiag.set(dg) = (T*)0; bdiag.set(dg) = (T*)0;
                unsigned ix; diagconf.contains(dg, &ix);
                diagconf.remove(ix); arsz -= 2*(dim-dg);
                if (maxoff == dg) 
                        maxoff = diagconf.max();
        }
#endif
        return *this;
}

template <typename T>
BdMatrix<T>& BdMatrix<T>::autoinsert(const T& val, const unsigned r, const unsigned c)
{
        REGISTER T* pt = check(r, c);
        if (LIKELY(pt)) {
                *pt = val;
                return *this;
        } else {
                if (LIKELY(MATH__ fabs(val) >= BD_MINVAL)) {
                        adddiag(CSTD__ abs((int)(r-c)));
                        pt = check(r, c);
                        BCHK(!pt, BdMatrixErr, Internal autoinsert error, 0, *this);
                        *pt = val;
                }
        }
        return *this;
}

template <typename T>
BdMatrix<T>& BdMatrix<T>::set_row(const Vector<T>& val, const unsigned r, const unsigned s)
{
        for (unsigned i=0; i<val.size(); ++i)
                autoinsert(val(i), r, i+s);
        return *this;
}

template <typename T>
TVector<T> BdMatrix<T>::get_row(const unsigned int r) const
{
        TVector<T> ret(0, dim);
        BCHK(r >= dim, BdMatrixErr, get_row idx out of bounds, r, ret);
        ret.set(diag[r], r);
        for (unsigned i = 0; i < diagconf.size(); ++i) {
                const REGISTER unsigned di = diagconf.get(i);
                if (LIKELY(di < dim-r))
                        ret.set(adiag.get(di)[r]   , r+di);
                if (LIKELY(r >= di))
                        ret.set(bdiag.get(di)[r-di], r-di);
        }
        return ret;
}

template <typename T>
TVector<T> BdMatrix<T>::get_col(const unsigned int c) const
{
        TVector<T> ret(0, dim);
        BCHK(c >= dim, BdMatrixErr, get_col idx out of bounds, c, ret);
        ret.set(diag[c], c);
        for (unsigned i = 0; i < diagconf.size(); i++) {
                const REGISTER unsigned di = diagconf.get(i);
                if (LIKELY(di < dim-c))
                        ret.set(bdiag.get(di)[c]   , c+di);
                if (LIKELY(c >= di))
                        ret.set(adiag.get(di)[c-di], c-di);
        }
        return ret;
}

#ifdef BDMAT_ONE_BLOCK
template <typename T>
inline BdMatrix<T>& BdMatrix<T>::fill(const T& val)
{
        TBCIFILL(diag, val, T, arsz);
        return *this;
}

template <typename T>
inline BdMatrix<T>& BdMatrix<T>::clear()
{
        TBCIFILL(diag, (T)0, T, arsz);
        return *this;
}

#else

template <typename T>
inline BdMatrix<T>& BdMatrix<T>::fill(const T& val)
{
        TBCIFILL(diag, val, T, dim);
        for (unsigned i = 0; i < diagconf.size(); ++i) {
                const REGISTER unsigned di = diagconf.get(i);
                TBCIFILL(adiag.get(di), val, T, (dim-di));
                TBCIFILL(bdiag.get(di), val, T, (dim-di));
        }
        return *this;
}

template <typename T>
inline BdMatrix<T>& BdMatrix<T>::clear()
{
        TBCIFILL(diag, (T)0, T, dim);
        for (unsigned i = 0; i < diagconf.size(); ++i) {
                const REGISTER unsigned di = diagconf.get(i);
                TBCIFILL(adiag.get(di), (T)0, T, (dim-di));
                TBCIFILL(bdiag.get(di), (T)0, T, (dim-di));
        }
        return *this;
}
#endif

// expand to outermost diagonal (or m, if given)
template <typename T>
BdMatrix<T>& BdMatrix<T>::expand(unsigned m)
{
        if (UNLIKELY(m == 0))
                m = maxoff;
        BCHK(m>dim, BdMatrixErr, Wrong expansion size, m, *this);
        if (LIKELY(maxoff == m && arsz == (dim + 2*dim*m - m*(m+1))))
                return *this;
#ifdef BDMAT_ONE_BLOCK
        BVector<unsigned> newconf(m);
        for (unsigned i = 0; i < m; ++i)
                newconf.set(i) = i+1;
        return reconfig(newconf);
#else
        for (unsigned i=1; i<=m; ++i)
                adddiag(i);
        return *this;
#endif
}


//#warning "Calculation of (sub)determinants not yet implemented!"
#if 0
template <typename T>
T BdMatrix<T>::sdet(unsigned int level, BVector<unsigned int>& arow, 
                    BVector<unsigned int>& acol)
{
        T el;
        int sz = arow.size();
        //if (sz < 1) 
        //      STD__ cerr << "FEHLER!" << STD__ endl;
        if (sz == 1) 
                return this->operator() (arow(0), acol(0));
        if (sz == 2) {
                el = (this->operator() (arow(0), acol(0)) \
                    * this->operator() (arow(1), acol(1)) \
                    - this->operator() (arow(0), acol(1)) \
                    * this->operator() (arow(1), acol(0)));
                return el;
        }
        T tmp = 0;
        //if (sz>=20) STD__ cout << sz << " ";
        BVector<unsigned int> ar(sz-1); BVector<unsigned int> ac(sz-1);
        int r = bestrow(level++); //row to expand
        int rw = 0; int mul;
        //setup rows
        for (REGISTER int i = 0; i < sz; i++) {
                if (arow(i) == r) { 
                        mul = (i%2? -1: 1); rw--; 
                } else 
                        ar(rw) = arow(i);
                rw++;
        }
        for (REGISTER int j = 0; j < sz; j++) {
                el = this->operator() (r, acol(j));
                if (MATH__ fabs(el) < BD_MINVAL) 
                        continue;
                //setup cols if necessary
#if 0
                int cl = 0;
                for (REGISTER int t = 0; t < sz; t++) {
                        if (t == j) 
                                cl--;
                        else 
                                ac(cl) = acol(t);
                        cl++;
                }
#else
                TBCICOPY(&(ac(0)), &(acol(0)), int, j);
                TBCICOPY(&(ac(j)), &(acol(j+1)), int, (sz-j-1));
#endif

                tmp += (j%2? -1: 1) * sdet(level, ar, ac) * el; 
        }
        return tmp * mul;
}

template <typename T>
T BdMatrix<T>::subdet(BVector<unsigned int> ri, BVector<unsigned int> ci)
{
        if (ri.size() != ci.size()) {
#ifndef TBCI_DISABLE_EXCEPT
                throw (BdMatrixErr("Unequal # of lines and rows to ignore!"));
#else
                STD__ cerr << "Unequal # of lines and rows to ignore!" << STD__ endl;
                return 0;
#endif
        }

        BVector<unsigned int> actrow(dim - ri.size());
        BVector<unsigned int> actcol(dim - ci.size());
        BVector<unsigned int> rowfill(dim); 
        bestrow.resize(dim);

        unsigned int rw = 0; unsigned int cl = 0;
        for (REGISTER int j = 0; j < dim; j++) {
                if (ri.contains (j)) 
                        rw--;
                else 
                        actrow(rw) = j;
                if (ci.contains(j)) 
                        cl--;
                else 
                        actcol(cl) = j;
                rw++; cl++;
        }

        //STD__ cout << actrow << STD__ endl; STD__ cout << actcol << STD__ endl;

        // (pseudo-) count # of non-zero elements of rows
        matmem<T>* el = offdiag.setfirst(); int elemi = (el? el->row: -1);

        for (int i = 0; i < dim; i++) {
                if (!(actrow.contains(i))) { 
                        rowfill(i) = dim + 1; continue; 
                }

                int fill = 0;
                if (actcol.contains(i))
                        fill++;
                if (actcol.contains(i+1) && i+1 < dim)
                        fill++;
                if (actcol.contains(i-1) && i-1 >= 0)
                        fill++;

                for (REGISTER int s = 0; s < diagconf.size(); s++) {
                        if (actcol.contains(i+diagconf.get(s)) && i+diagconf.get(s) < dim)
                                fill++;
                        if (actcol.contains(i-diagconf.get(s)) && i-diagconf.get(s) >= 0)
                                fill++;
                }

                while (el && elemi == i) { 
                        if (actcol.contains(el->col))
                                fill++; 
                        el = offdiag.setnext(); elemi = (el? el->row: -1);
                }

                rowfill(i) = fill;
        }
        //STD__ cout << "rowfill:" << rowfill << STD__ endl;
        //sort
        for (int k = 0; k < dim; k++) {
                int min = dim + 2; int minr = -1;
                for (int s = 0; s < dim; s++)
                        if (rowfill(s) < min) { 
                                min = rowfill(s); minr = s; 
                        }
                bestrow(k) = minr;
                if (minr >= 0) 
                        rowfill(minr) = dim + 1;
        }
        return sdet(0, actrow, actcol);
}
#endif /* 0 */


template <typename T>
BdMatrix<T>& BdMatrix<T>::operator = (const BdMatrix<T>& mat)
{
        BCHK(mat.dim != dim, BdMatrixErr, Assignment from wrong size Matrix, mat.dim, *this);
        if (diagconf != mat.diagconf) {
#ifdef BOUNDCHECK
                STD__ cout << "old:" << mat.diagconf << "\nnew:" << diagconf << STD__ endl;
                STD__ cout << "Warning: Automatic reconfiguring BdMatrix in assignment!" << STD__ endl;
#endif
                free_diags(diagconf.size());
                diagconf.resize(mat.diagconf.size());
                diagconf = mat.diagconf;
                constructor(dim);
        }

        dummy = mat.dummy;
        do_copy(mat);
        return *this;
}


//writes value to diagonal and deletes off-diags 
template <typename T>
inline BdMatrix<T>& BdMatrix<T>::operator = (const T& val)
{
        setunit(val); return *this;
}


// Fully flexible now!
#define SFORALL_S(op)                                                                   \
template <typename T>                                                                   \
BdMatrix<T>& BdMatrix<T>::operator op (const BdMatrix<T>& mat)                          \
{                                                                                       \
        BCHK(mat.dim != dim, BdMatrixErr, Adding wrong size BdMatrix, mat.dim, *this);  \
        unsigned i, j;                                                                  \
        for (i = 0; i < dim; ++i)                                                       \
                diag[i] op mat.diag[i];                                                 \
        for (j = 0; j < diagconf.size(); ++j) {                                         \
                const REGISTER unsigned di = diagconf.get(j);                           \
                if (LIKELY(mat.adiag.get(di)))                                          \
                        for (i = 0; i < (dim-di); ++i) {                                \
                                adiag.set(di)[i] op mat.adiag.get(di)[i];               \
                                bdiag.set(di)[i] op mat.bdiag.get(di)[i];               \
                        }                                                               \
        }                                                                               \
        if (diagconf == mat.diagconf)                                                   \
                return *this;                                                           \
        for (j = 0; j < mat.diagconf.size(); ++j) {                                     \
                const REGISTER unsigned di = mat.diagconf.get(j);                       \
                if (!diagconf.contains(di))  {                                          \
                        adddiag(di);                                                    \
                        for (i = 0; i < (dim-di); ++i) {                                \
                                adiag.set(di)[i] op mat.adiag.get(di)[i];               \
                                bdiag.set(di)[i] op mat.bdiag.get(di)[i];               \
                        }                                                               \
                }                                                                       \
        }                                                                               \
        return *this;                                                                   \
}

SFORALL_S(+=)
SFORALL_S(-=)


#define SFORALL_T(op)                                                   \
template <typename T>                                                   \
BdMatrix<T>& BdMatrix<T>::operator op (const T& mult)                   \
{                                                                       \
        for (REGISTER unsigned int i = 0; i < dim; ++i)                 \
                diag[i] op mult;                                        \
        for (unsigned j = 0; j < diagconf.size(); ++j) {                \
                const REGISTER unsigned di = diagconf.get(j);           \
                for (unsigned k = 0; k < (dim-di); ++k) {               \
                        adiag.set(di)[k] op mult;                       \
                        bdiag.set(di)[k] op mult;                       \
                }                                                       \
        }                                                               \
        return *this;                                                   \
}

SFORALL_T(*=)
//SFORALL_T(+=)
//SFORALL_T(-=)

//The inline is necessary to prevent linker errors
template <typename T>
inline BdMatrix<T>& BdMatrix<T>::operator /= (const T& div)
{
        BCHK((div==(T)0), BdMatrixErr, "Divide BdMatrix by zero", 0, *this);
        REGISTER T mult ALIGN(MIN_ALIGN) = (T)1 / div;
        for (REGISTER unsigned int i = 0; i < dim; ++i) 
                diag[i] *= mult;
        for (unsigned j = 0; j < diagconf.size(); ++j) {
                const REGISTER unsigned di = diagconf.get(j);
                for (unsigned k = 0; k < (dim-di); ++k) { 
                        adiag.set(di)[k] *= mult; 
                        bdiag.set(di)[k] *= mult;
                }
        }
        return *this;
}

#define INTDIVEQ(T)                                                             \
template <>                                                                     \
inline BdMatrix < T >& BdMatrix< T >::operator /= (const T & div)               \
{                                                                               \
        BCHK((div==( T )0), BdMatrixErr, "Divide BdMatrix by zero", 0, *this);  \
        for (REGISTER unsigned i = 0; i < dim; ++i)                             \
                diag[i] /= div;                                                 \
        for (unsigned j = 0; j < diagconf.size(); ++j) {                        \
                const REGISTER unsigned di = diagconf.get(j);                   \
                for (unsigned k = 0; k < (dim-di); ++k) {                       \
                        adiag.set(di)[k] /= div;                                \
                        bdiag.set(di)[k] /= div;                                \
                }                                                               \
        }                                                                       \
        return *this;                                                           \
}

INTDIVEQ(int)
INTDIVEQ(unsigned int)

template <typename T>
inline BdMatrix<T>& BdMatrix<T>::transpose()
{
#if 0 //def BDMAT_ONE_BLOCK
        // A swap_vectors implementation would be faster!
        for (REGISTER unsigned int t = 0; t < diagconf.size(); ++t) {
                const unsigned int s = diagconf.get(t);
                T* tmp = adiag.get(s); adiag.set(s) = bdiag.get(s); bdiag.set(s) = tmp;
        }
#else
        adiag.swap(bdiag);
#endif
        return *this;
}

INST(template <typename T> class BdMatrix friend BdMatrix<T> transpose(BdMatrix<T>&);)
template <typename T>
inline BdMatrix<T> transpose(BdMatrix<T>& mat)
{
        return (BdMatrix<T>(mat)).transpose();
}

#define STDDEF_ST(op)                                           \
template <typename T>                                           \
inline BdMatrix<T> BdMatrix<T>::operator op (const T& val) const \
{                                                               \
        return (BdMatrix<T> (*this)) op##= val;                 \
}

STDDEF_ST(/)
STDDEF_ST(*)
//STDDEF_ST(+)
//STDDEF_ST(-)

#define STDDEF_SS(op)                                           \
template <typename T>                                           \
BdMatrix<T> BdMatrix<T>::operator op (const BdMatrix<T>& m2) const      \
{                                                               \
        return (BdMatrix<T> (*this)) op##= m2;                  \
}

STDDEF_SS(+)
STDDEF_SS(-)


template <typename T>
BdMatrix<T> BdMatrix<T>::operator * (const BdMatrix<T>& mat2) const
{
        const unsigned int newoff = maxoff + mat2.maxoff;
        //const unsigned int moff = MAX(maxoff, mat2.maxoff);
        BdMatrix<T> prd(0, dim, newoff);
        BCHK(dim != mat2.dim, BdMatrixErr, Multiplying diff size matrices, 0, prd);

        for (int i = 0; i < (signed)dim; ++i) {
                /* A really good compiler would do this for us */
                const int jend = MIN(dim, i+newoff+1);
                const int kstart = MAX(0, i-(signed)maxoff);
                const int kend = MIN(dim, i+maxoff+1);
                /* TODO: Exact summing */
                for (int j = MAX(0,i-(signed)newoff); j < jend; ++j) {
                        REGISTER T el ALIGN(MIN_ALIGN) = T(0);
                        for (REGISTER int k = kstart; k < kend; ++k)
                                el += this->get(i, k) * mat2.get(k, j);
                        //prd.setval(i, j, el);
                        if (el != T(0))
                                prd(i, j) = el; /* autoinsert ? */
                }
        }
        return prd;
}

INST(template <typename T> class BdMatrix friend BdMatrix<T> operator * (const T&, const BdMatrix<T>&);)
template <typename T>
inline BdMatrix<T> operator * (const T& v, const BdMatrix<T>& m)
{ return m*v; }
// Assuming commutativity here!

#if 0
template <typename T>
TMatrix<T> BdMatrix<T>::operator * (const Matrix<T>& mat) const
{
        TMatrix<T> tm (mat.rows(), mat.columns());
        if (UNLIKELY(tm.rows() != dim)) {
#ifndef TBCI_DISABLE_EXCEPT
                throw (BdMatrixErr("Multiplying Matrix with Matrix: wrong sizes!"));
#else
                STD__ cerr << "Multiplying Matrix with Matrix: wrong sizes!" << STD__ endl;
                return tm;
#endif
        }

        T val ALIGN(MIN_ALIGN);
        for (int c = 0; c < tm.columns(); c++)
                for (int i = 0; i < mat.rows(); i++) {
                        val = 0;
                        for (REGISTER int j=0; j < dim; j++)
                                val += this->operator()(i, j) * mat(j, c);
                        tm(i, c) = val;
                }
        return tm
}
#endif /* 0 */

#ifdef USE_PREFETCH
# define MAY_PREFETCH_R(adr,loc) PREFETCH_R(adr,loc)
# define MAY_PREFETCH_W(adr,loc) PREFETCH_W(adr,loc)
#else
# define MAY_PREFETCH_R(adr,loc) do {} while (0) 
# define MAY_PREFETCH_W(adr,loc) do {} while (0)
#endif


template <typename T>
BdMatrix<T> BdMatrix<T>::operator - () const
{
        BdMatrix<T> neg(dim, diagconf);
        //diagonal
        for (REGISTER unsigned int j = 0; j < dim; ++j)
                neg.diag[j] = -diag[j];
        //offdiag
        for (unsigned int t = 0; t < diagconf.size(); ++t) {
                const unsigned int dc = diagconf.get(t);
                for (REGISTER unsigned int i = 0; i < (dim - dc); ++i) {
                        neg.adiag.set(dc)[i] = -adiag.get(dc)[i];
                        neg.bdiag.set(dc)[i] = -bdiag.get(dc)[i];
                }
        }
        return neg;
}

/* Default if optimized linewise multiply */
#if !defined(BDMATVEC_LNWISE) && !defined(BDMATVEC_LNWISE_OPT) && !defined(BDMATVEC_DIAGWISE)
# define BDMATVEC_LNWISE_OPT  
#endif

/* Cost of these ops w/ dim dxd, o offdiags and a maxoffset of m */
#define COST_BDMATVEC_LN(d,o,m) (d*(COST_MULT+2*COST_UNIT_LOAD+COST_UNIT_STORE+COST_LOOP\
 +o*(COST_LOOP+2*COST_BRANCH+COST_UNIT_LOAD))+(2*d-m)*o*(3*COST_NU_LOAD+COST_ADD+COST_MULT))
#define COST_BDMATVEC_LNO(d,o,m) (d*(COST_MULT+2*COST_UNIT_LOAD+COST_UNIT_STORE+COST_LOOP\
 +o*(COST_LOOP+COST_UNIT_LOAD))+(2*d-m)*o*(3*COST_NU_LOAD+COST_ADD+COST_MULT+COST_BRANCH/2))
#define COST_BDMATVEC_DIAG(d,o,m) (d*(2*COST_UNIT_LOAD+COST_MULT+COST_UNIT_STORE+COST_LOOP)\
 +o*(6*COST_NU_LOAD+COST_LOOP+3*COST_UNIT_LOAD+2*COST_NU_STORE\
  +(2*d-m)*(3*COST_UNIT_LOAD+COST_LOOP+COST_MULT+COST_ADD+COST_UNIT_STORE)))

template <typename T>
inline void do_bdmat_vec_mult_lnw(const unsigned start, const unsigned end,
                                  TVector<T> *res, const BdMatrix<T> *mat,
                                  const Vector<T> *vec)
                               
{
        const unsigned int od = mat->diagconf.size();
        const unsigned int dim = mat->dim;
        // Zeilenweise, stur
        // TODO: Exact summation
        if (do_exactsum2()) {
            for (unsigned i = start; i < end; ++i) {
                // diag
                REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                REGISTER T comp(0.0);
                // offdiag
                for (unsigned j = 0; j < od; ++j) {
                        REGISTER const unsigned s  = mat->diagconf.get(j);
                        if (LIKELY(s <= i)) {
                                const REGISTER T y = mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                const REGISTER T t = val+y;
                                comp += (t-val) - y;
                                val = t;
                        }
                        if (LIKELY(i+s < dim)) {
                                const REGISTER T y = mat->adiag.get(s)[i] * vec->get(i+s);
                                const REGISTER T t = val+y;
                                comp += (t-val) - y;
                                val = t;
                        }
                }
                res->set(val-comp, i);
            }
        } else {
            for (unsigned i = start; i < end; ++i) {
                // diag
                REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                // offdiag
                for (unsigned j = 0; j < od; ++j) {
                        REGISTER const unsigned s  = mat->diagconf.get(j);
                        if (LIKELY(s <= i))
                                val += mat->bdiag.get(s)[i-s] * vec->get(i-s);
                        if (LIKELY(i+s < dim))
                                val += mat->adiag.get(s)[i] * vec->get(i+s);
                }
                res->set(val, i);
            }
        }
}

template <typename T>
inline void do_bdmat_vec_mult_lnw_opt(const unsigned start, const unsigned end,
                                      TVector<T> *res, const BdMatrix<T> *mat,
                                      const Vector<T> *vec)
                               
{
        const unsigned int od = mat->diagconf.size();
        const unsigned int dim = mat->dim;
        // Zeilenweise, optimiert
        unsigned int i;
        //const unsigned dconf0 = mat->diagconf.get(0);
        const unsigned int maxoff = mat->maxoff;
        /* A very good compiler would do this for us */
        unsigned iend = MIN(maxoff, dim-maxoff);
        iend = MIN(iend, end);
        MAY_PREFETCH_R(&mat->diagconf.getcref(0), 2);
        //MAY_PREFETCH_R(&mat->adiag.get(0), 3);
        //MAY_PREFETCH_R(&mat->bdiag.get(0), 3);
#ifdef USE_PREFETCH
        for (unsigned int _j = 0; _j < od; ++_j) {
                const unsigned s = mat->diagconf.get(_j);
                PREFETCH_R(&mat->adiag.getcref(s), 2);
                PREFETCH_R(&mat->bdiag.getcref(s), 2);
                PREFETCH_R(mat->adiag.get(s), 2);
                PREFETCH_R(mat->bdiag.get(s), 2);
        }
#endif
        MAY_PREFETCH_R(mat->diag, 2);
        MAY_PREFETCH_R(&vec->getcref(0), 2);
        MAY_PREFETCH_W(&res->setval(0), 3);
        if (do_exactsum2()) {
                /* First loop: Need to check bdiag, as i-s = i-(i-j) = j could be negative
                 * adiag safe, as i < iend <=> i < N-maxoff and s <= maxoff => i+s < N
                 */
                for (i = start; i < iend; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                        REGISTER T comp(0.0);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                if (s <= i) {
                                        const T y = mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                        const T t = val + y;
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                }
                                // s <= maxoff; i < N-maxoff => s+i < N
                                /*if (s+i < dim)*/ 
                                const T y = mat->adiag.get(s)[i] * vec->get(i+s);
                                const T t = val + y;
                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                comp += (t-val) - y;
                                val = t;
                        }
                        res->set(val-comp, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
                iend = MIN(end, dim - maxoff);
                if (i < iend) {
                        /* Second loop: No checks needed; only executed if 2*maxoff <= mat->dim */
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                                REGISTER T comp(0.0);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        T y = mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                        T t = val + y;
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                        y = mat->adiag.get(s)[i] * vec->get(i+s);
                                        t = val + y;
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                }
                                res->set(val-comp, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                } else {
                        iend = MIN(maxoff, end);
                        /* Second loop: Both checks needed; only executed if 2*maxoff > mat->dim */
                        // TODO: Exact summation
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                                REGISTER T comp(0.0);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        if (s <= i) {
                                                const T y = mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                                const T t = val + y;
                                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                                comp += (t-val) - y;
                                                val = t;
                                        }
                                        if (s < (signed)dim - i) {
                                                const T y = mat->adiag.get(s)[i] * vec->get(i+s);
                                                const T t = val + y;
                                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                                comp += (t-val) - y;
                                                val = t;
                                        }
                                }
                                res->set(val-comp, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                }
                /* Third loop: Need to check to adiag, as i+s = i+(j-i) = j could be >= N 
                 * bdiag safe, as s <= maxoff and i >= maxoff => i-s >= 0
                 */
                for (; i < end; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                        REGISTER T comp(0.0);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                /* if (s <= i) */
                                T y = mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                T t = val + y;
                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                comp += (t-val) - y;
                                val = t;
                                if (s < (signed)dim - i) {
                                        y = mat->adiag.get(s)[i] * vec->get(i+s);
                                        t = val + y;
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                }
                        }
                        res->set(val-comp, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
        } else {        /* Not exactsum2 */
                /* First loop: Need to check bdiag, as i-s = i-(i-j) = j could be negative
                 * adiag safe, as i < iend <=> i < N-maxoff and s <= maxoff => i+s < N
                 */
                for (i = start; i < iend; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                if (s <= i) {
                                        val += mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                }
                                // s <= maxoff; i < N-maxoff => s+i < N
                                /*if (s+i < dim)*/ 
                                val += mat->adiag.get(s)[i] * vec->get(i+s);
                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                        }
                        res->set(val, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
                iend = MIN(end, dim - maxoff);
                if (i < iend) {
                        /* Second loop: No checks needed; only executed if 2*maxoff <= mat->dim */
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        val += mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        val += mat->adiag.get(s)[i] * vec->get(i+s);
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                }
                                res->set(val, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                } else {
                        iend = MIN(maxoff, end);
                        /* Second loop: Both checks needed; only executed if 2*maxoff > mat->dim */
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        if (s <= i) {
                                                val += mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        }
                                        if (s < (signed)dim - i) {
                                                val += mat->adiag.get(s)[i] * vec->get(i+s);
                                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                        }
                                }
                                res->set(val, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                }
                /* Third loop: Need to check to adiag, as i+s = i+(j-i) = j could be >= N 
                 * bdiag safe, as s <= maxoff and i >= maxoff => i-s >= 0
                 */
                for (; i < end; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN2) = mat->diag[i] * vec->get(i);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                /* if (s <= i) */ 
                                val += mat->bdiag.get(s)[i-s] * vec->get(i-s);
                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                if (s < (signed)dim - i) {
                                        val += mat->adiag.get(s)[i] * vec->get(i+s);
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                }
                        }
                        res->set(val, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
        }
}

template <typename T>
inline void do_bdmat_vec_mult_diagw_exact(const unsigned start, const unsigned end,
                                         TVector<T> *res, const BdMatrix<T> *mat,
                                         const Vector<T> *vec)
                               
{
        const unsigned int od = mat->diagconf.size();
        const unsigned int dim = mat->dim;
        //Diagonal wise
        //        We need to store a complete vector full of compensation values
        //        and this is actually FASTER than the linewise algo
        //        that we could employ alternatively.
        // TODO: Find heuristics to determine what's better
        // (current suspicion: for rather sparse matrices, the lnw is better,
        //  for rather full matrices, the compensation vector will work best)
        // diag
        Vector<T> corr((T)0, end-start);
        do_vec_vec_mul(end-start, &res->setval(start),
                        &mat->diag[start], &vec->getcref(start));
        // offdiag
        for (unsigned j = 0; j < od; ++j) {
                // s ist die Nummer der Diagonalen (1<=s<=maxoff)
                const unsigned s  = mat->diagconf.get(j);
                const unsigned iend = MIN(end,dim-s); 
                const unsigned iend2 = MAX(0,(int)(end-s));
                const unsigned istart2 = MAX((int)(start-s),0);
                        //do_add_vec_vec_mul(iend-start, &res->setval(start),
                        //      mat->adiag.get(s)+start, &vec->getcref(start+s));
                for (unsigned off = start; off < iend; ++off) {
                        const REGISTER T y = *(mat->adiag.get(s)+off) * vec->get(s+off);
                        const REGISTER T t = res->get(off) + y;
                        corr.setval(off-start) += (t - res->get(off)) - y;
                        res->setval(off) = t;
                }
                        //do_add_vec_vec_mul(iend2-istart2, &res->setval(istart2+s),
                        //      mat->bdiag.get(s)+istart2, &vec->getcref(istart2));
                for (unsigned off = istart2; off < iend2; ++off) {
                        const REGISTER T y = *(mat->bdiag.get(s)+off) * vec->get(off);
                        const REGISTER T t = res->get(s+off) + y;
                        corr.setval(s+off-start) += (t - res->get(s+off)) - y;
                        res->setval(s+off) = t;
                }
        }
        /* Apply correction */
        for (unsigned off = start; off < end; ++off)
                res->setval(off) -= corr.get(off-start);
}

template <typename T>
inline void do_bdmat_vec_mult(const unsigned start, const unsigned end,
                              TVector<T> *res, const BdMatrix<T> *mat,
                              const Vector<T> *vec)
                               
{
        const unsigned int dim = mat->dim;
        const unsigned int od = mat->diagconf.size();
        // Diagonal wise
        // TODO: Find heuristics to determine what's better
        // (current suspicion: for rather sparse matrices, the lnw is better,
        //  for rather full matrices, the compensation vector will work best)
        if (do_exactsum2()) {
                if (od < 8)
                        do_bdmat_vec_mult_lnw(start, end, res, mat, vec);
                else
                        do_bdmat_vec_mult_diagw_exact(start, end, res, mat, vec);
                return;
        }
        // diag
        do_vec_vec_mul(end-start, &res->setval(start),
                        &mat->diag[start], &vec->getcref(start));
        // offdiag
        for (unsigned j = 0; j < od; ++j) {
                // s ist die Nummer der Diagonalen (1<=s<=maxoff)
                const unsigned s  = mat->diagconf.get(j);
                const unsigned iend = MIN(end,dim-s); 
                const unsigned iend2 = MAX(0,(int)(end-s));
                const unsigned istart2 = MAX((int)(start-s),0);
                if (LIKELY(iend > start))
                        do_add_vec_vec_mul(iend-start, &res->setval(start),
                                mat->adiag.get(s)+start, &vec->getcref(start+s));
                if (LIKELY(iend2 > istart2))
                        do_add_vec_vec_mul(iend2-istart2, &res->setval(istart2+s),
                                mat->bdiag.get(s)+istart2, &vec->getcref(istart2));
        }
}

#if defined(BDMATVEC_LNWISE)
# define COST_BDMATVEC(d,o,m) COST_BDMATVEC_LN(d,o,m)
#elif defined(BDMATVEC_LNWISE_OPT)
# define COST_BDMATVEC(d,o,m) COST_BDMATVEC_LNO(d,o,m)
#elif defined(BDMATVEC_DIAGWISE)
# define COST_BDMATVEC(d,o,m) COST_BDMATVEC_DIAG(d,o,m)
#endif

#define COST_BDMATVEC_TRANS(d,o,m) COST_BDMATVEC(d,o,m)
template <typename T>
inline void do_bdmat_vec_transmult_lnw(const unsigned start, const unsigned end,
                                       TVector<T> *res, const BdMatrix<T> *mat,
                                       const Vector<T> *vec)
{
        const unsigned od = mat->diagconf.size();
        const unsigned int dim = mat->dim;
        if (do_exactsum2()) {
                for (unsigned i = start; i < end; ++i) {
                        //diagonal
                        REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                        REGISTER T comp(0.0);
                        //off-diagonals
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned s  = mat->diagconf.get(j);
                                if (LIKELY(s <= i)) {
                                        const T y = mat->adiag.get(s)[i-s] * vec->get(i-s);
                                        const T t = val + y;
                                        comp += (t-val) - y;
                                        val = t;
                                }
                                if (LIKELY(i+s < dim)) {
                                        const T y = mat->bdiag.get(s)[i] * vec->get(i+s);
                                        const T t = val + y;
                                        comp += (t-val) - y;
                                        val = t;
                                }
                        }
                        res->set(val-comp, i);
                }
        } else {
                for (unsigned i = start; i < end; ++i) {
                        //diagonal
                        REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                        //off-diagonals
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned s  = mat->diagconf.get(j);
                                if (LIKELY(s <= i))
                                        val += mat->adiag.get(s)[i-s] * vec->get(i-s);
                                if (LIKELY(i+s < dim))
                                        val += mat->bdiag.get(s)[i] * vec->get(i+s);
                        }
                        res->set(val, i);
                }
        }
}

template <typename T>
inline void do_bdmat_vec_transmult_lnw_opt(const unsigned start, const unsigned end,
                                           TVector<T> *res, const BdMatrix<T> *mat,
                                           const Vector<T> *vec)
{
        const unsigned od = mat->diagconf.size();
        const unsigned int dim = mat->dim;
        // Zeilenweise, optimiert
        //const unsigned dconf0 = mat->diagconf.get(0);
        unsigned int i;
        const unsigned int maxoff = mat->maxoff;
        /* A very good compiler would do this for us */
        unsigned iend = MIN(maxoff, mat->dim-maxoff);
        iend = MIN(end, iend);
        MAY_PREFETCH_R(&mat->diagconf.getcref(0), 2);
        //MAY_PREFETCH_R(&mat->adiag.getcref(0), 2);
        //MAY_PREFETCH_R(&mat->bdiag.getcref(0), 2);
#ifdef USE_PREFETCH
        for (unsigned int _j = 0; _j < od; ++_j) {
                const unsigned s = mat->diagconf.get(_j);
                PREFETCH_R(&mat->adiag.getcref(s), 2);
                PREFETCH_R(&mat->bdiag.getcref(s), 2);
                PREFETCH_R(mat->adiag.get(s), 2);
                PREFETCH_R(mat->bdiag.get(s), 2);
        }
#endif
        MAY_PREFETCH_R(mat->diag, 2);
        MAY_PREFETCH_R(&vec->getcref(0), 2);
        MAY_PREFETCH_W(&res->setval(0), 3);
        /* First loop: Need to check adiag, as i-s could be smaller than 0
         * bdiag safe, as i < N-maxoff and s <= maxoff => i+s < N
         */     
        if (do_exactsum2()) {
                for (i = start; i < iend; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                        REGISTER T comp(0.0);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                if (s <= i) {
                                        const T y = mat->adiag.get(s)[i-s] * vec->get(i-s);
                                        const T t = val + y;
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                }
                                /*if (s < mat->dim - i)*/
                                const T y = mat->bdiag.get(s)[i] * vec->get(i+s);
                                const T t = val + y;
                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                comp += (t-val) - y;
                                val = t;
                        }
                        res->set(val-comp, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
                iend = MIN(end, dim - maxoff);
                if (i < iend) {
                        /* Second loop, no checks necessary, 2*maxoff < N */
                        // TODO: Exact summation
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                                REGISTER T comp(0.0);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        T y = mat->adiag.get(s)[i-s] * vec->get(i-s);
                                        T t = val + y;
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                        y = mat->bdiag.get(s)[i] * vec->get(i+s);
                                        t = val + y;
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                }
                                res->set(val-comp, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                } else {
                        iend = MIN(end, maxoff);
                        /* Second loop, both checks necessary */
                        // TODO: Exact summation
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                                REGISTER T comp(0.0);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        if (s <= i) {
                                                const T y = mat->adiag.get(s)[i-s] * vec->get(i-s);
                                                const T t = val + y;
                                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                                comp += (t-val) - y;
                                                val = t;
                                        }
                                        if (s < (signed)dim - i) {
                                                const T y = mat->bdiag.get(s)[i] * vec->get(i+s);
                                                const T t = val + y;
                                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                                comp += (t-val) - y;
                                                val = t;
                                        }
                                }
                                res->set(val-comp, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                }
                /* Third loop, adiag safe */
                // TODO: Exact summation
                for (; i < end; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                        REGISTER T comp(0.0);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                /* if (s <= i) */ 
                                T y = mat->adiag.get(s)[i-s] * vec->get(i-s);
                                T t = val + y;
                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                comp += (t-val) - y;
                                val = t;
                                if (s < (signed)dim - i) {
                                        y = mat->bdiag.get(s)[i] * vec->get(i+s);
                                        t = val + y;
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                        comp += (t-val) - y;
                                        val = t;
                                }
                        }
                        res->set(val-comp, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
        } else {
                for (i = start; i < iend; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                if (s <= i) {
                                        val += mat->adiag.get(s)[i-s] * vec->get(i-s);
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                }
                                /*if (s < mat->dim - i)*/
                                val += mat->bdiag.get(s)[i] * vec->get(i+s);
                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                        }
                        res->set(val, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
                iend = MIN(end, dim - maxoff);
                if (i < iend) {
                        /* Second loop, no checks necessary, 2*maxoff < N */
                        // TODO: Exact summation
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        val += mat->adiag.get(s)[i-s] * vec->get(i-s);
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        val += mat->bdiag.get(s)[i] * vec->get(i+s);
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                }
                                res->set(val, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                } else {
                        iend = MIN(end, maxoff);
                        /* Second loop, both checks necessary */
                        // TODO: Exact summation
                        for (; i < iend; ++i) {
                                // diag
                                REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                                MAY_PREFETCH_R(mat->diag+i+1, 2);
                                // offdiag, no checking necessary
                                for (unsigned int j = 0; j < od; ++j) {
                                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                                        if (s <= i) {
                                                val += mat->adiag.get(s)[i-s] * vec->get(i-s);
                                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                        }
                                        if (s < (signed)dim - i) {
                                                val += mat->bdiag.get(s)[i] * vec->get(i+s);
                                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                        }
                                }
                                res->set(val, i);
                                MAY_PREFETCH_W(&res->setval(i+1), 3);
                        }
                }
                /* Third loop, adiag safe */
                // TODO: Exact summation
                for (; i < end; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN) = mat->diag[i] * vec->get(i);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                /* if (s <= i) */ 
                                val += mat->adiag.get(s)[i-s] * vec->get(i-s);
                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1-s, 2);
                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                if (s < (signed)dim - i) {
                                        val += mat->bdiag.get(s)[i] * vec->get(i+s);
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1, 2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                }
                        }
                        res->set(val, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
        }
}

template <typename T>
inline void do_bdmat_vec_transmult_diagw_exact(const unsigned start, const unsigned end,
                                                TVector<T> *res, const BdMatrix<T> *mat,
                                                const Vector<T> *vec)
{
        const unsigned int dim = mat->dim;
        const unsigned od = mat->diagconf.size();
        // diagonal wise
        // We need to store a vector of correction values ...
        Vector<T> corr((T)0, end-start);
        // diag
        do_vec_vec_mul(end-start, &res->setval(start),
                        &mat->diag[start], &vec->getcref(start));
        // offdiag
        for (unsigned j = 0; j < od; ++j) {
                // s ist die Nummer der Diagonalen (1<=s<=maxoff)
                const int s  = mat->diagconf.get(j);
                const int iend = MIN(end,dim-s); 
                const int iend2 = end-s; 
                const int istart2 = MAX((int)(start-s),0);
                //do_add_vec_vec_mul(iend2-istart2, &res->setval(istart2+s),
                //              mat->adiag.get(s)+istart2, &vec->getcref(istart2));
                for (int off = istart2; off < iend2; ++off) {
                        const REGISTER T y = *(mat->adiag.get(s)+off) * vec->get(off);
                        const REGISTER T t = res->get(off+s) + y;
                        corr.setval(off+s-start) += (t - res->get(off+s)) - y;
                        res->setval(off+s) = t;
                }
                //do_add_vec_vec_mul(iend-start, &res->setval(start),
                //              mat->bdiag.get(s)+start, &vec->getcref(start+s));
                for (int off = start; off < iend; ++off) {
                        const REGISTER T y = *(mat->bdiag.get(s)+off) * vec->get(off+s);
                        const REGISTER T t = res->get(off) + y;
                        corr.setval(off-start) += (t - res->get(off)) - y;
                        res->setval(off) = t;
                }
        }
        // Apply correction
        for (unsigned off = start; off < end; ++off)
                res->setval(off) -= corr(off-start);
}

template <typename T>
inline void do_bdmat_vec_transmult(const unsigned start, const unsigned end,
                                   TVector<T> *res, const BdMatrix<T> *mat,
                                   const Vector<T> *vec)
{
        const unsigned int dim = mat->dim;
        const unsigned od = mat->diagconf.size();
        if (do_exactsum2()) {
                //do_bdmat_vec_transmult_lnw(start, end, res, mat, vec);
                do_bdmat_vec_transmult_diagw_exact(start, end, res, mat, vec);
                return;
        }
        // diagonal wise
        // diag
        do_vec_vec_mul(end-start, &res->setval(start),
                        &mat->diag[start], &vec->getcref(start));
        // offdiag
        for (unsigned j = 0; j < od; ++j) {
                // s ist die Nummer der Diagonalen (1<=s<=maxoff)
                const int s  = mat->diagconf.get(j);
                const int iend = MIN(end,dim-s); 
                const int iend2 = end-s; 
                const int istart2 = MAX((int)(start-s),0);
                do_add_vec_vec_mul(iend2-istart2, &res->setval(istart2+s),
                                mat->adiag.get(s)+istart2, &vec->getcref(istart2));
                do_add_vec_vec_mul(iend-start, &res->setval(start),
                                mat->bdiag.get(s)+start, &vec->getcref(start+s));
        }
}

#define COST_BDMATVEC_DOT(d,o,m) COST_BDMATVEC(d,o,m)
template <typename T>
inline void do_bdmat_vec_dotmult(const unsigned start, const unsigned end,
                                 TVector<T> *res, const BdMatrix<T> *mat,
                                 const Vector<T> *vec)
{
        const unsigned int od = mat->diagconf.size();
        const unsigned int dim = mat->dim;
#ifdef BDMATVEC_LNWISE
        // Zeilenweise, stur
        if (do_exactsum2()) {
                for (unsigned i = start; i < end; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN) = CPLX__ conj(mat->diag[i]) * vec->get(i);
                        REGISTER T comp(0.0);
                        // offdiag
                        for (unsigned j = 0; j < od; ++j) {
                                REGISTER const unsigned s  = mat->diagconf.get(j);
                                if (LIKELY(s <= i)) {
                                        const T y = CPLX__ conj(mat->bdiag.get(s)[i-s]) * vec->get(i-s);
                                        const T t = val + y;
                                        comp += (t-val) - y;
                                        val = t;
                                }
                                if (LIKELY(i+s < dim)) {
                                        const T y = CPLX__ conj(mat->adiag.get(s)[i]) * vec->get(i+s);
                                        const T t = val + y;
                                        comp += (t-val) - y;
                                        val = t;
                                }
                        }
                        res->set(val-comp, i);
                }
        } else {
                for (unsigned i = start; i < end; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN) = CPLX__ conj(mat->diag[i]) * vec->get(i);
                        // offdiag
                        for (unsigned j = 0; j < od; ++j) {
                                REGISTER const unsigned s  = mat->diagconf.get(j);
                                if (LIKELY(s <= i))
                                        val += CPLX__ conj(mat->bdiag.get(s)[i-s]) * vec->get(i-s);
                                if (LIKELY(i+s < dim))
                                        val += CPLX__ conj(mat->adiag.get(s)[i]) * vec->get(i+s);
                        }
                        res->set(val, i);
                }
        }
#elif defined(BDMATVEC_LNWISE_OPT)
        // Zeilenweise, optimiert
        unsigned int i;
        //const unsigned dconf0 = mat->diagconf.get(0);
        const unsigned int maxoff = mat->maxoff;
        /* A very good compiler would do this for us */
        unsigned iend = MIN(maxoff, dim-maxoff);
        iend = MIN(iend, end);
        MAY_PREFETCH_R(&mat->diagconf.getcref(0), 2);
        //MAY_PREFETCH_R(&mat->adiag(0), 3);
        //MAY_PREFETCH_R(&mat->bdiag(0), 3);
#ifdef USE_PREFETCH
        for (unsigned int _j = 0; _j < od; ++_j) {
                const unsigned s = mat->diagconf.get(_j);
                PREFETCH_R(&mat->adiag.getcref(s), 2);
                PREFETCH_R(&mat->bdiag.getcref(s), 2);
                PREFETCH_R(mat->adiag.get(s), 2);
                PREFETCH_R(mat->bdiag.get(s), 2);
        }
#endif
        MAY_PREFETCH_R(mat->diag, 2);
        MAY_PREFETCH_R(&vec->getcref(0), 2);
        MAY_PREFETCH_W(&res->setval(0), 3);
        /* First loop: Need to check bdiag, as i-s = i-(i-j) = j could be negative
         * adiag safe, as i < iend <=> i < N-maxoff and s <= maxoff => i+s < N
         */
        // TODO: Exact summation
        // NOTE: Not implemented here, as dotMult is note even used by the solvers ...
        for (i = start; i < iend; ++i) {
                // diag
                REGISTER T val ALIGN(MIN_ALIGN2) = CPLX__ conj(mat->diag[i]) * vec->get(i);
                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                MAY_PREFETCH_R(mat->diag+i+1, 2);
                // offdiag
                for (unsigned int j = 0; j < od; ++j) {
                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                        if (s <= i) {
                                val += CPLX__ conj(mat->bdiag.get(s)[i-s]) * vec->get(i-s);
                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                        }
                        // s <= maxoff; i < N-maxoff => s+i < N
                        /*if (s+i < dim)*/ 
                        val += CPLX__ conj(mat->adiag.get(s)[i]) * vec->get(i+s);
                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                }
                res->set(val, i);
                MAY_PREFETCH_W(&res->setval(i+1), 3);
        }
        iend = MIN(end, dim - maxoff);
        /* i = MAX(start,MIN(maxoff,dim-maxoff,end)) */
        if (i < iend) {
                /* Second loop: No checks needed; only executed if 2*maxoff <= mat->dim */
                // TODO: Exact summation
                for (; i < iend; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN2) = CPLX__ conj(mat->diag[i]) * vec->get(i);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag, no checking necessary
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                val += CPLX__ conj(mat->bdiag.get(s)[i-s]) * vec->get(i-s);
                                MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                val += CPLX__ conj(mat->adiag.get(s)[i]) * vec->get(i+s);
                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                        }
                        res->set(val, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
        } else {
                iend = MIN(maxoff, end);
                /* Second loop: Both checks needed; only executed if 2*maxoff > mat->dim */
                // TODO: Exact summation
                for (; i < iend; ++i) {
                        // diag
                        REGISTER T val ALIGN(MIN_ALIGN2) = CPLX__ conj(mat->diag[i]) * vec->get(i);
                        MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                        MAY_PREFETCH_R(mat->diag+i+1, 2);
                        // offdiag, no checking necessary
                        for (unsigned int j = 0; j < od; ++j) {
                                REGISTER const unsigned int s  = mat->diagconf.get(j);
                                if (s <= i) {
                                        val += CPLX__ conj(mat->bdiag.get(s)[i-s]) * vec->get(i-s);
                                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                                }
                                if (s < (signed)dim - i) {
                                        val += CPLX__ conj(mat->adiag.get(s)[i]) * vec->get(i+s);
                                        MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                        MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                                }
                        }
                        res->set(val, i);
                        MAY_PREFETCH_W(&res->setval(i+1), 3);
                }
        }
        /* Third loop: Need to check to adiag, as i+s = i+(j-i) = j could be >= N 
         * bdiag safe, as s <= maxoff and i >= maxoff => i-s >= 0
         */
        // TODO: Exact summation
        for (; i < end; ++i) {
                // diag
                REGISTER T val ALIGN(MIN_ALIGN2) = CPLX__ conj(mat->diag[i]) * vec->get(i);
                MAY_PREFETCH_R(&vec->getcref(i+1), 2);
                MAY_PREFETCH_R(mat->diag+i+1, 2);
                // offdiag
                for (unsigned int j = 0; j < od; ++j) {
                        REGISTER const unsigned int s  = mat->diagconf.get(j);
                        /* if (s <= i) */ 
                        val += CPLX__ conj(mat->bdiag.get(s)[i-s]) * vec->get(i-s);
                        MAY_PREFETCH_R(mat->bdiag.get(s)+i+1-s,2);
                        MAY_PREFETCH_R(&vec->getcref(i+1-s), 2);
                        if (s < (signed)dim - i) {
                                val += CPLX__ conj(mat->adiag.get(s)[i]) * vec->get(i+s);
                                MAY_PREFETCH_R(mat->adiag.get(s)+i+1,2);
                                MAY_PREFETCH_R(&vec->getcref(i+1+s), 2);
                        }
                }
                res->set(val, i);
                MAY_PREFETCH_W(&res->setval(i+1), 3);
        }
#elif defined(BDMATVEC_DIAGWISE)
        //Diagonalweise
        // FIXME: I see no way we could implement exact summation here ...
        // diag
        do_vec_vec_cmul(end-start, &res->setval(start),
                        &mat->diag[start], &vec->getcref(start));
        // offdiag
        for (unsigned j = 0; j < od; j++) {
                // s ist die Nummer der Diagonalen (1<=s<=maxoff)
                const int s  = mat->diagconf.get(j);
                const int iend = MIN(end,dim-s); 
                const int iend2 = end-s; 
                const int istart2 = MAX((int)(start-s),0);
                do_add_vec_vec_cmul(iend-start, &res->setval(start),
                                mat->adiag.get(s)+start, &vec->getcref(start+s));
                do_add_vec_vec_cmul(iend2-istart2, &res->setval(istart2+s),
                                mat->bdiag.get(s)+istart2, &vec->getcref(istart2));
        }
#endif
}

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

INST(template <typename T> class BdMatrix friend void job_bdmat_vec_mult(struct thr_ctrl*);)
template <typename T>
void job_bdmat_vec_mult(struct thr_ctrl* tc)
{
        do_bdmat_vec_mult(tc->t_off, tc->t_size,
                          (TVector<T>*)(tc->t_par[0]), (const BdMatrix<T>*)(tc->t_par[1]),
                          (const Vector<T>*)(tc->t_par[2]));
}

INST(template <typename T> class BdMatrix friend void job_bdmat_vec_transmult(struct thr_ctrl*);)
template <typename T>
void job_bdmat_vec_transmult (struct thr_ctrl* tc)
{
        do_bdmat_vec_transmult(tc->t_off, tc->t_size,
                               (TVector<T>*)(tc->t_par[0]), (const BdMatrix<T>*)(tc->t_par[1]), 
                               (const Vector<T>*)(tc->t_par[2]));
}


 /*  TODO: Adjust dynamically (or at least depending on OPT target). */
# if !defined(SMP_BDMATSLICE)
#  define SMP_BDMATSLICE 32768
# endif
# define SMP_BDMATSLICE2 (SMP_BDMATSLICE/sizeof(T))

template <typename T>
TVector<T> BdMatrix<T>::operator * (const Vector<T>& vec) const
{
        TVector<T> tv(dim);
        const unsigned n_thr = threads_avail(dim / SMP_BDMATSLICE2);
        update_n_thr(n_thr);
        BCHK(vec.size() != dim, BdMatrixErr, Multiplication BdMatrix with Vector: wrong sizes, 0, tv);
        if (LIKELY(n_thr < 2))
                do_bdmat_vec_mult<T>(0UL, dim, &tv, this, &vec);
        else {
                PREFETCH_R_MANY(this->diag, 2);
                PREFETCH_R_MANY(vec.vec, 2); 
                PREFETCH_W_MANY(tv.vec, 3);
                const unsigned long first = slice_offset(1, n_thr, dim, (T*)0);
                unsigned long st, en = first;
                for (unsigned t = 0; t < n_thr-1; ++t) {
                        st = en; en = slice_offset(t+2, n_thr, dim, (T*)0);
                        thread_start_off(t, (thr_job_t)job_bdmat_vec_mult<T>,
                                         st, en, &tv, this, &vec, (void*)0);
                }
                do_bdmat_vec_mult<T>(0UL, first, &tv, this, &vec);
                for (unsigned s = 0; s < n_thr-1; ++s)
                        thread_wait(s);
        }
        return tv;
}

// transpose-vector multiplication
template <typename T>
TVector<T> BdMatrix<T>::transMult (const Vector<T>& vec) const
{
        TVector<T> tv(dim);
        const unsigned n_thr = threads_avail(dim / SMP_BDMATSLICE2);
        update_n_thr(n_thr);
        BCHK(vec.size() != dim, BdMatrixErr, Multiplication BdMatrix with Vector: wrong sizes, 0, tv);
        if (LIKELY(n_thr < 2 ))
                do_bdmat_vec_transmult<T> (0UL, dim, &tv, this, &vec);
        else {
                PREFETCH_R_MANY(this->diag, 2);
                PREFETCH_R_MANY(vec.vec, 2);
                PREFETCH_W_MANY(tv.vec, 3);
                const unsigned long first = slice_offset(1, n_thr, dim, (T*)0);
                unsigned long st, en = first;
                for (unsigned t = 0; t < n_thr-1; ++t) {
                        st = en; en = slice_offset(t+2, n_thr, dim, (T*)0);
                        thread_start_off(t, (thr_job_t)job_bdmat_vec_transmult<T>, 
                                         st, en, &tv, this, &vec, (void*)0);
                }
                do_bdmat_vec_transmult<T>(0UL, first, &tv, this, &vec);
                for (unsigned s = 0; s < n_thr-1; ++s)
                        thread_wait(s);
        }
        return tv;
}

#else /* SMP */

template <typename T>
TVector<T> BdMatrix<T>::operator * (const Vector<T>& vec) const
{
        TVector<T> tv(dim);
        BCHK(vec.size() != dim, BdMatrixErr, Multiplication BdMatrix with Vector: wrong sizes, 0, tv);
        do_bdmat_vec_mult<T>(0, dim, &tv, this, &vec);
        return tv;
}

/* bicg and qmr solvers use this */
template <typename T>
TVector<T> BdMatrix<T>::transMult (const Vector<T>& vec) const
{
        TVector<T> tv(dim);
        BCHK(vec.size() != dim, BdMatrixErr, Multiplication BdMatrix with Vector: wrong sizes, 0, tv);
        do_bdmat_vec_transmult<T>(0, dim, &tv, this, &vec);
        return tv;
}

#endif /* SMP */

/* This is not currently used by anyone ... */
template <typename T>
TVector<T> BdMatrix<T>::dotMult(const Vector<T>& vec) const
{
        TVector<T> tv(dim);
        BCHK(vec.size() != dim, BdMatrixErr, Dot Multiplication BdMatrix with Vector: wrong sizes, 0, tv);
        do_bdmat_vec_dotmult<T>(0, dim, &tv, this, &vec);
        return tv;
}



//template <typename T>
//TVector<T> BdMatrix<T>::transMult (const TVector<T>& tvec) const;
//template <typename T>
//TVector<T> BdMatrix<T>::transMult (const TSVector<T>& tsvec) const;


template <typename T>
bool BdMatrix<T>::operator == (const BdMatrix<T>& ma) const
{
        if (LIKELY(ma.dim != dim))
                return false;
        if (LIKELY(dim == 0 || ((diag == ma.diag) && (adiag == ma.adiag) && (bdiag == ma.bdiag))))
                return true;
        if (TBCICOMP(ma.diag, diag, T, dim)) 
                return false;
        unsigned i; // MSC++ annoyances
        for (i = 0; i < diagconf.size(); ++i) {
                const unsigned di = diagconf.get(i);
                if (ma.adiag.get(di)) {
                        if (TBCICOMP(ma.adiag.get(di), adiag.get(di), T, (dim-di))) return false;
                        if (TBCICOMP(ma.bdiag.get(di), bdiag.get(di), T, (dim-di))) return false;
                } else {
                        for (unsigned j = 0; j < dim-di; ++j)
                                if (MATH__ fabs(adiag.get(di)[j]) >= BD_MINVAL || MATH__ fabs(bdiag(di)[j]) >= BD_MINVAL)
                                        return false;
                }
        }
        if (LIKELY(diagconf == ma.diagconf))
                return true;
        for (i = 0; i < ma.diagconf.size(); ++i) {
                const unsigned di = ma.diagconf.get(i);
                if (!diagconf.contains (di)) {
                        for (unsigned j = 0; j < dim-di; ++j)
                                if (UNLIKELY(MATH__ fabs(ma.adiag.get(di)[j]) >= BD_MINVAL || MATH__ fabs(ma.bdiag.get(di)[j]) >= BD_MINVAL))
                                        return false;
                }
        }
        return true;
}

template <typename T>
BdMatrix<T>& BdMatrix<T>::mult_rows(const Vector<T>& v)
{
        BCHK(dim != v.size(), BdMatrixErr, multiply rowwise with wrong size, v.size(), *this);
        // diag
        for (unsigned j = 0; j < dim; ++j)
                diag[j] *= v.get(j);
        // offdiags
        for (unsigned od = 0; od < diagconf.size(); ++od) {
                const unsigned off = diagconf.get(od);
                for (unsigned i = 0; i < (dim-off); ++i)
                        adiag.get(off)[i] *= v.get(i);
                for (unsigned k = 0; k < (dim-off); ++k) 
                        bdiag.get(off)[k] *= v.get(k+off);
        }
        return *this;   
}

template <typename T>
BdMatrix<T>& BdMatrix<T>::div_rows(const Vector<T>& v)
{
        BCHK(dim != v.size(), BdMatrixErr, divide rowwise with wrong size, v.size(), *this);
        // diag
        for (unsigned j = 0; j < dim; ++j)
                diag[j] /= v.get(j);
        // offdiags
        for (unsigned od = 0; od < diagconf.size(); ++od) {
                const unsigned off = diagconf.get(od);
                for (unsigned i = 0; i < (dim-off); ++i) 
                        adiag.get(off)[i] /= v.get(i);
                for (unsigned k = 0; k < (dim-off); ++k) 
                        bdiag.get(off)[k] /= v.get(k+off);
        }
        return *this;   
}

template <typename T>
BdMatrix<T>& BdMatrix<T>::mult_row(const T& f, const unsigned i)
{
        BCHK(i>dim, BdMatrixErr, mult_row index out of range, i, *this);
        diag[i] *= f;
        for (unsigned od = 0; od < diagconf.size(); ++od) {
                const unsigned off = diagconf.get(od);
                if (i < dim-off) 
                        adiag.get(off)[i] *= f;
                if (i >= off) 
                        bdiag.get(off)[i-off] *= f;
        }
        return *this;
}

// Don't do it with integer types
template <typename T>
BdMatrix<T>& BdMatrix<T>::div_row(const T& d, const unsigned i)
{
        BCHK(i>dim, BdMatrixErr, div_row index out of range, i, *this);
        BCHK(d == (T)0, BdMatrixErr, div_row by zero, i, *this); 
        T f = (T)1.0/d;
        diag[i] *= f;
        for (unsigned od = 0; od < diagconf.size(); ++od) {
                const unsigned off = diagconf.get(od);
                if (i < dim-off) 
                        adiag.get(off)[i] *= f;
                if (i >= off) 
                        bdiag.get(off)[i-off] *= f;
        }
        return *this;
}

template <typename T>
STD__ ostream& operator << (STD__ ostream& ostr, const BdMatrix<T>& mat)
{
        //if (!mat.outset) 
        //      mat.setoutopts();
        //ostr << '(';
        for (unsigned i = 0; i < mat.dim; ++i) {
                for (unsigned j = 0; j < mat.dim; ++j) {
                        if (UNLIKELY(mat.outset)) {
                                ostr.width(mat.outw); 
                                ostr.precision(mat.outp); 
                                ostr.fill(mat.outf);
                        }
                        if (i == j || mat.diagconf.contains(CSTD__ abs((int)i-(int)j)))
                                ostr << mat.get(i, j) << ' ';
                        else
                                ostr << (T)0 << ' ';
                }
                //if (i == (mat.dim - 1)) 
                //      ostr << ')' << STD__ endl;
                //else 
                        ostr << "\n";
        }
        EXPCHK(mat.dummy!=(T)0, BdMatrixErr, dummy not zero, 0, ostr);
        return ostr;
}

template <typename T>
STD__ istream& operator >> (STD__ istream& istr, BdMatrix<T>& mat)
{
        T tmp;
        for (unsigned i = 0; i < mat.dim; ++i) {
                for (unsigned j = 0; j < mat.dim; ++j) {
                        istr >> tmp;
                        if (i == j || mat.diagconf.contains(CSTD__ abs((int)i-(int)j)))
                                mat.setval(tmp, i, j);
                        else 
                                if (tmp != (T)0) 
                                        STD__ cerr << "Element (" << i << "," << j 
                                                   << ") should be zero!" << STD__ endl;
                }
        }
        return istr;
}

template <typename T>
BdMatrix<T>::operator TMatrix<T> () const
{
        const unsigned _DIM = dim;
        TMatrix<T> tm ((T)0, _DIM, _DIM); 
        // diag
        for (unsigned i = 0; i < _DIM; ++i)
                tm.setval(diag[i], i, i);
        // offdiags
        for (unsigned j = 0; j < diagconf.size(); ++j) {
                const unsigned di = diagconf.get(j);
                for (unsigned k = 0; k < _DIM-di; ++k) {
                        tm.setval(adiag.get(di)[k], k, k+di);
                        tm.setval(bdiag.get(di)[k], k+di, k);
                }
        }
        return tm;
}

#if 0
template <typename T>
BdMatrix<T>::operator Matrix<T>() const
{
        Matrix<T> tm ((T)0, dim, dim); unsigned i;
        // diag
        for (i = 0; i < dim; ++i) 
                tm.setval(diag[i], i, i);
        // offdiags
        for (unsigned j = 0; j < diagconf.size(); ++j)
                for (unsigned k = 0; k < dim-(i=diagconf.get(j)); ++k) {
                        tm.setval(adiag.get(i)[k], k, k+i);
                        tm.setval(bdiag.get(i)[k], k+i, k);
                }
        return tm;
}
#endif

#if 0
template <typename T>
BdMatrix<T> BdMatrix<T>::inverse()
{
        BdMatrix<T> inv(dim);
        BVector<unsigned int> ri(1); BVector<unsigned int> ci(1);
        T d = det();
        //STD__ cout << "Det: " << d << STD__ endl;
        if (MATH__ fabs(d) < BD_MINVAL) {
#ifndef TBCI_DISABLE_EXCEPT
                throw (BdMatrixErr("Trying to invert singular Matrix!"));
#else
                STD__ cerr << "Trying to invert singular Matrix!" << STD__ endl;
                return inv;
#endif
        }
        d = 1/d;
        for (unsigned i = 0; i < dim; i++)
                for (unsigned j = 0; j < dim; j++) {
                        ri(0) = i; ci(0) = j;
                        inv.setval(subdet(ri, ci) * ((i+j)%2? -d: d), j, i);
                }
        return inv;
}
#endif /* 0 */

#if defined(SMP) && defined(HAVE_LIBNUMA)
template <typename T>
int numa_opt_bdmat_helper(const BdMatrix<T>& bm, const BVector<unsigned>& cfg,
                          const int node, bool fault_in,
                          const unsigned first, const unsigned last)
{
        int moved = 0;
        const unsigned int sz = bm.rows();
        /* Diag */
        const T* diag = &(((BdMatrix<T>&)bm).setval(0,0));
        moved = do_numa_move_pages(node, fault_in, 
                        (unsigned long)(diag+first), 
                        (unsigned long)(diag+last));
        /* Now do off-diags */
        for (unsigned i = 0; i < cfg.size(); ++i) {
                const unsigned off = cfg.get(i);
                const unsigned dln = sz - off;
                const T* adiag = &(((BdMatrix<T>&)bm).setval(0,off));
                const T* bdiag = &(((BdMatrix<T>&)bm).setval(off,0));
                if (first < dln)
                        moved += do_numa_move_pages(node, fault_in,
                                        (unsigned long)(adiag+first),
                                        (unsigned long)(adiag+MIN(dln,last)));
                if (last > off)
                        moved += do_numa_move_pages(node, fault_in,
                                        (unsigned long)(bdiag+MAX(0,(signed)(first-off))),
                                        (unsigned long)(bdiag+last-off));
        }
        return moved;
}

template <typename T>
void numa_opt_bdmat_job(struct thr_ctrl *tc)
{
        const BdMatrix<T>& bm(*(const BdMatrix<T>*)tc->t_par[2]);
        const BVector<unsigned>& bmcfg(*(const BVector<unsigned>*)tc->t_par[3]);
        /* Stick with convention that size, off are node, fault_in resp. */
        const int node = tc->t_size;
        const bool fault_in = tc->t_off;
        /* start and end are params 0 and 1 */
        const unsigned st = (unsigned long)tc->t_par[0];
        const unsigned en = (unsigned long)tc->t_par[1];
        tc->t_res_l = numa_opt_bdmat_helper(bm, bmcfg, node, fault_in, st, en);
}

template <typename T>
int numa_optimize(const BdMatrix<T>& bm, bool fault_in)
{
        if (!numa_avail && !fault_in)
                return 0;
        /* use some heuristic to decide for the num of threads */
        const unsigned sz = bm.rows();
        const unsigned n_thr = threads_avail (sz / SMP_BDMATSLICE2);
        //update_n_thr(n_thr);

        BVector<unsigned> bmcfg(bm.getcfg());
        /* OK, so how do we distribute a BandMatrix to threads?
         * 1. We do it by row, reflecting that this is how we later
         *    use it in BdMat - Vector multiplication
         * 2. A BdMat consists of several vectors, one for the diagonal
         *    and a variable number of others for the off-diagonals.
         *    All these vectors need to be split to threads.
         * 3. Problem: Can we even hope to come close to page boundaries?
         */
        if (LIKELY(n_thr < 2)) {
                if (fault_in)
                        numa_opt_bdmat_helper(bm, bmcfg, 
                                        main_numa_node, fault_in,
                                        0, sz);
                return 0;
        } else {
                const unsigned first = slice_offset(1, n_thr, sz, (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, sz, (T*)0);
                        thread_start_off (t, (thr_job_t)numa_opt_bdmat_job<T>, 
                                          threads[t].numa_node, fault_in,
                                          st, en, &bm, &bmcfg, (void*)0);
                }
                /* The first slice is handled by the main thread */
                res = numa_opt_bdmat_helper(bm, bmcfg, 
                                main_numa_node, fault_in, 0, first);
                /* 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 BdMatrix<T>& bm, bool fault_in)
{
        /* FIXME: We should emulate fault_in here, no? */
        return 0;
}
#endif



NAMESPACE_END

#endif  /* TBCI_BAND_MATRIX_H */