matrices.hpp

#pragma once

#include <cstdio>
#include <algorithm>
#include <numeric>
#include <limits>
#include <memory>
#include <deque>
#include <stdexcept>

#include "my_timer.hpp"
#include "my_common.hpp"





template<typename T, typename I, typename A = my_stdallocator_wrapper>
class MatrixCSR
{
public:
    using float_type = T;
    using int_type = I;
    using allocator_type = A;
public:
    T * vals = nullptr;
    I * colidxs = nullptr;
    I * rowptrs = nullptr;
    A ator;
    I nrows;
    I ncols;
    I nvals;
    bool should_i_free;
public:
    MatrixCSR() : MatrixCSR(0, 0, 0, false) { }
    MatrixCSR(const A & ator_) : MatrixCSR(0, 0, 0, false, ator_) { }
    MatrixCSR(I nrows_, I ncols_, I nvals_, bool do_allocate_, const A & ator_ = A())
        : ator{ator_}, nrows{nrows_}, ncols{ncols_}, nvals{nvals_}, should_i_free{do_allocate_}
    {
        if(do_allocate_)
        {
            allocate();
        }
    }
    MatrixCSR(const MatrixCSR<T,I,A> & other) = delete;
    MatrixCSR(MatrixCSR<T,I,A> && other)
        : vals(other.vals), colidxs(other.colidxs), rowptrs(other.rowptrs), ator(std::move(other.ator)), nrows(other.nrows), ncols(other.ncols), nvals(other.nvals), should_i_free(other.should_i_free)
    {
        other.should_i_free = false;
    }
    ~MatrixCSR()
    {
        deallocate();
    }
    MatrixCSR<T,I,A> & operator=(const MatrixCSR<T,I,A> & other) = delete;
    MatrixCSR<T,I,A> & operator=(MatrixCSR<T,I,A> && other)
    {
        if(&other != *this)
        {
            deallocate();
            vals = other.vals;
            colidxs = other.colidxs;
            rowptrs = other.rowptrs;
            ator = std::move(other.ator);
            nrows = other.nrows;
            ncols = other.ncols;
            nvals = other.nvals;
            should_i_free = other.should_i_free;
            other.should_i_free = false;
        }
        return *this;
    }
    MatrixCSR<T,I,A> get_view() const
    {
        MatrixCSR<T,I,A> ret(nrows, ncols, nvals, false, ator);
        ret.rowptrs = rowptrs;
        ret.colidxs = colidxs;
        ret.vals = vals;
        return ret;
    }
    void resize(I nrows_, I ncols_, I nvals_, bool do_allocate_)
    {
        deallocate();
        nrows = nrows_;
        ncols = ncols_;
        nvals = nvals_;
        should_i_free = do_allocate_;
        if(do_allocate_)
        {
            allocate();
        }
    }
    void set_size(I nrows_, I ncols_, I nvals_)
    {
        resize(nrows_, ncols_, nvals_, false);
    }
    void allocate()
    {
        deallocate();
        if(nvals > 0) vals = ator.template allocate<T>(nvals);
        if(nvals > 0) colidxs = ator.template allocate<I>(nvals);
        rowptrs = ator.template allocate<I>(nrows + 1);
        should_i_free = true;
    }
    void deallocate()
    {
        if(should_i_free)
        {
            ator.deallocate(vals);
            ator.deallocate(colidxs);
            ator.deallocate(rowptrs);
            vals = nullptr;
            colidxs = nullptr;
            rowptrs = nullptr;
        }
        should_i_free = false;
    }
    static size_t approx_memory_needed(I nrows, I /*ncols*/, I nvals)
    {
        return (nrows+1) * sizeof(I) + nvals * sizeof(I) + nvals * sizeof(T);
    }
};





template<typename T, typename I, typename A = my_stdallocator_wrapper>
class MatrixDense
{
public:
    using float_type = T;
    using int_type = I;
    using allocator_type = A;
public:
    T * vals = nullptr;
    A ator;
    I nrows;
    I ncols;
    I ld;
    bool should_i_free;
public:
    MatrixDense() : MatrixDense(0, 0, -1, false) { }
    MatrixDense(const A & ator_) : MatrixDense(0, 0, -1, false, ator_) { }
    MatrixDense(I nrows_, I ncols_, I ld_, bool do_allocate_, const A & ator_ = A())
        : ator{ator_}, nrows{nrows_}, ncols{ncols_}, ld{ld_}, should_i_free{do_allocate_}
    {
        if(ld < 0) ld = ncols;
        if(do_allocate_) allocate();
    }
    MatrixDense(const MatrixDense<T,I,A> & other) = delete;
    MatrixDense(MatrixDense<T,I,A> && other)
        : vals(other.vals), ator(std::move(other.ator)), nrows(other.nrows), ncols(other.ncols), ld(other.ld), should_i_free(other.should_i_free)
    {
        other.should_i_free = false;
    }
    ~MatrixDense()
    {
        deallocate();
    }
    MatrixDense<T,I,A> & operator=(const MatrixDense<T,I,A> & other) = delete;
    MatrixDense<T,I,A> & operator=(MatrixDense<T,I,A> && other)
    {
        if(&other != this)
        {
            deallocate();
            ator = other.ator;
            vals = other.vals;
            nrows = other.nrows;
            ncols = other.ncols;
            ld = other.ld;
            should_i_free = other.should_i_free;
            other.should_i_free = false;
        }
        return *this;
    }
    MatrixDense<T,I,A> get_view() const
    {
        MatrixDense<T,I,A> ret(nrows, ncols, ld, false, ator);
        ret.vals = vals;
        return ret;
    }
    void resize(I nrows_, I ncols_, I ld_, bool do_allocate_)
    {
        if(ld_ < 0) ld_ = ncols_;

        deallocate();
        nrows = nrows_;
        ncols = ncols_;
        ld = ld_;
        should_i_free = do_allocate_;
        if(do_allocate_) allocate();
    }
    void set_size(I nrows_, I ncols_, I ld_)
    {
        resize(nrows_, ncols_, ld_, false);
    }
    void allocate()
    {
        deallocate();
        if(static_cast<size_t>(size_alloc()) != (static_cast<size_t>(nrows) * ld)) MY_ABORT("MatrixDense::allocate: matrix too large for the used index_type");
        if(size_alloc() > 0) vals = ator.template allocate<T>(size_alloc());
        should_i_free = true;
    }
    void deallocate()
    {
        if(should_i_free)
        {
            ator.deallocate(vals);
            vals = nullptr;
        }
        should_i_free = false;
    }
    I size_alloc() const
    {
        return nrows * ld;
    }
    I size_matrix() const
    {
        return nrows * ncols;
    }
    static size_t approx_memory_needed(I nrows, I ncols, I ld)
    {
        if(ld < 0) ld = ncols;
        return nrows * ld * sizeof(T);
    }
};





template<typename I, typename A = my_stdallocator_wrapper>
class Permutation
{
public:
    using int_type = I;
    using allocator_type = A;
public:
    I * map_dst_to_src = nullptr; // idx_src = map_dst_to_src[idx_dst];   dst[i] = src[map_dst_to_src[i]];
    I * map_src_to_dst = nullptr; // idx_dst = map_src_to_dst[idx_src];   dst[map_src_to_dst[i]] = src[i];
    A ator;
    I size;
    bool should_i_free;
public:
    Permutation() : Permutation(0, false) { }
    Permutation(const A & ator_) : Permutation(0, false, ator_) { }
    Permutation(I size_, bool do_allocate_, const A & ator_ =  A())
        : ator{ator_}, size{size_}, should_i_free{do_allocate_}
    {
        if(do_allocate_)
        {
            allocate();
        }
    }
    Permutation(const Permutation<I,A> & other) = delete;
    Permutation(Permutation<I,A> && other)
        : map_dst_to_src(other.map_dst_to_src), map_src_to_dst(other.map_src_to_dst), ator(std::move(other.ator)), size(other.size), should_i_free(other.should_i_free)
    {
        other.should_i_free = false;
    }
    ~Permutation()
    {
        deallocate();
    }
    Permutation<I,A> & operator=(const Permutation<I,A> & other) = delete;
    Permutation<I,A> & operator=(Permutation<I,A> && other)
    {
        if(&other != this)
        {
            deallocate();
            ator = other.ator;
            map_dst_to_src = other.map_dst_to_src;
            map_src_to_dst = other.map_src_to_dst;
            size = other.size;
            should_i_free = other.should_i_free;
            other.should_i_free = false;
        }
        return *this;
    }
    Permutation<I,A> get_view() const
    {
        Permutation<I,A> ret(size, false, ator);
        ret.map_dst_to_src = map_dst_to_src;
        ret.map_src_to_dst = map_src_to_dst;
        return ret;
    }
    void resize(I size_, bool do_allocate_)
    {
        deallocate();
        size = size_;
        should_i_free = do_allocate_;
        if(do_allocate_)
        {
            allocate();
        }
    }
    void set_size(I size_)
    {
        resize(size_, false);
    }
    void allocate()
    {
        deallocate();
        if(size > 0) map_dst_to_src = ator.template allocate<I>(size);
        if(size > 0) map_src_to_dst = ator.template allocate<I>(size);
        should_i_free = true;
    }
    void deallocate()
    {
        if(should_i_free)
        {
            ator.deallocate(map_dst_to_src);
            ator.deallocate(map_src_to_dst);
            map_dst_to_src = nullptr;
            map_src_to_dst = nullptr;
        }
        should_i_free = false;
    }
    void invert()
    {
        std::swap(map_dst_to_src, map_src_to_dst);
    }
    static size_t approx_memory_needed(I size)
    {
        return 2 * size * sizeof(I);
    }
};





struct putimers
{
    my_timer read, sort, write;
};

struct permtimers
{
    my_timer upper2full, vecinit, traverse, invperm, doperm;
    putimers pu;
};

struct sctimers
{
    my_timer perm, dosc, copy;
    my_timer subS, boundcalc, subX, mkl, trf, trs, syrk;
    permtimers pt;
};






























template<typename T, typename I, typename A>
static bool save_matrix(const MatrixDense<T,I,A> & M, const char * filepath)
{
    FILE * f = fopen(filepath, "w");
    if(f == nullptr)
    {
        fprintf(stderr, "Could not open file '%s'\n", filepath);
        return false;
    }

    fprintf(f, "%lld %lld\n", (long long)M.nrows, (long long)M.ncols);
    for(I r = 0; r < M.nrows; r++)
    {
        for(I c = 0; c < M.ncols; c++)
        {
            fprintf(f, "%+.15e ", (double)M.vals[r * M.ncols + c]);
        }
        fprintf(f, "\n");
    }

    fclose(f);

    return true;
}



template<typename T, typename I, typename A>
static bool save_matrix(const MatrixCSR<T,I,A> & M, const char * filepath)
{
    FILE * f = fopen(filepath, "w");
    if(f == nullptr)
    {
        fprintf(stderr, "Could not open file '%s'\n", filepath);
        return false;
    }

    fprintf(f, "%lld %lld %lld\n", (long long)M.nrows, (long long)M.ncols, (long long)M.nvals);
    for(I r = 0; r < M.nrows; r++)
    {
        I start = M.rowptrs[r];
        I end = M.rowptrs[r+1];
        for(I i = start; i < end; i++)
        {
            I c = M.colidxs[i];
            double v = static_cast<double>(M.vals[i]);
            fprintf(f, "%6lld %6lld %+25.15e\n", (long long)r, (long long)c, (double)v);
        }
    }

    fclose(f);

    return true;
}



template<typename T, typename I, typename A>
static bool load_matrix_csr(MatrixCSR<T,I,A> * output, const char * filepath)
{
    FILE * f = fopen(filepath, "r");
    if(f == nullptr)
    {
        fprintf(stderr, "Could not open file '%s'\n", filepath);
        return false;
    }

    I nrows, ncols, nvals;
    fscanf(f, std::is_same_v<I,int32_t> ? "%d%d%d" : "%ld%ld%ld", &nrows, &ncols, &nvals);
    output->resize(nrows, ncols, nvals, true);
    I lastrow = 0;
    output->rowptrs[0] = 0;
    for(I i = 0; i < output->nvals; i++)
    {
        I row, col;
        T val;
        fscanf(f, std::is_same_v<I,int32_t> ? "%d%d" : "%ld%ld", &row, &col);
        fscanf(f, std::is_same_v<T,float> ? "%f" : "%lf",  &val);
        while(row > lastrow)
        {
            lastrow++;
            output->rowptrs[lastrow] = i;
        }
        output->vals[i] = val;
        output->colidxs[i] = col;
    }
    while(nrows > lastrow)
    {
        lastrow++;
        output->rowptrs[lastrow] = nvals;
    }
    output->rowptrs[output->nrows] = output->nvals;

    fclose(f);

    return true;
}



template<typename T, typename I, typename A>
static bool load_matrix_dense(MatrixDense<T,I,A> * output, const char * filepath)
{
    FILE * f = fopen(filepath, "r");
    if(f == nullptr)
    {
        fprintf(stderr, "Could not open file '%s'\n", filepath);
        return false;
    }

    I nrows, ncols;
    fscanf(f, std::is_same_v<I,int32_t> ? "%d%d" : "%ld%ld", &nrows, &ncols);
    output->resize(nrows, ncols, -1, true);
    for(I r = 0; r < nrows; r++)
    {
        for(I c = 0; c < ncols; c++)
        {
            fscanf(f, std::is_same_v<T,float> ? "%f" : "%lf", output->vals + r * output->ld + c);
        }
    }

    fclose(f);

    return true;
}



template<typename T, typename I, typename A>
static bool load_vector_dense(MatrixDense<T,I,A> * output, const char * filepath)
{
    FILE * f = fopen(filepath, "r");
    if(f == nullptr)
    {
        fprintf(stderr, "Could not open file '%s'\n", filepath);
        return false;
    }

    std::vector<T> vals;
    while(true)
    {
        int nscanned;
        T currval;        
        if constexpr(std::is_same_v<T,int>)       nscanned = fscanf(f, "%d",   &currval);
        if constexpr(std::is_same_v<T,long>)      nscanned = fscanf(f, "%ld",  &currval);
        if constexpr(std::is_same_v<T,long long>) nscanned = fscanf(f, "%lld", &currval);
        if constexpr(std::is_same_v<T,float>)     nscanned = fscanf(f, "%f",   &currval);
        if constexpr(std::is_same_v<T,double>)    nscanned = fscanf(f, "%f",   &currval);
        if(nscanned <= 0)
            break;
        vals.push_back(currval);
    }

    fclose(f);

    output->resize(vals.size(), 1, true);
    std::copy(vals.begin(), vals.end(), output->vals);

    return true;
}



template<typename T, typename I, typename A>
static bool contains_nan(MatrixDense<T,I,A> & M)
{
    static_assert(A::is_data_host_accessible, "Matrix data has to be host accessible");
    for(size_t i = 0; i < M.size_alloc(); i++) if(my_isnan(M.vals[i])) return true;
    return false;
}



template<typename T, typename I, typename Ao, typename Ai>
static void sparse_to_dense(MatrixDense<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols) MY_ABORT("sparse_to_dense: output matrix has wrong dimensions");

    std::fill(output.vals, output.vals + output.size_alloc(), T{0});
    for(I r = 0; r < input.nrows; r++)
    {
        I i_start = input.rowptrs[r];
        I i_end = input.rowptrs[r+1];
        for(I i = i_start; i < i_end; i++)
        {
            I c = input.colidxs[i];
            output.vals[r * output.ld + c] = input.vals[i];
        }
    }
}



template<typename T, typename I, typename Ao, typename Ai>
static void sparse_to_dense_transpose(MatrixDense<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input)
{
    if(output.nrows != input.ncols || output.ncols != input.nrows) MY_ABORT("sparse_to_dense_transpose: output matrix has wrong dimensions");

    std::fill(output.vals, output.vals + output.size_alloc(), T{0});
    for(I r = 0; r < input.nrows; r++)
    {
        I i_start = input.rowptrs[r];
        I i_end = input.rowptrs[r+1];
        for(I i = i_start; i < i_end; i++)
        {
            I c = input.colidxs[i];
            output.vals[c * output.ld + r] = input.vals[i];
        }
    }
}



template<typename T, typename I, typename Ao, typename Ai, typename Ap>
static void sparse_to_dense_permcol(MatrixDense<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input, const Permutation<I,Ap> & perm)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols) MY_ABORT("sparse_to_dense_permcol: output matrix has wrong dimensions");
    if(perm.size != input.ncols) MY_ABORT("sparse_to_dense_permcol: permutation has wrong size");

    std::fill(output.vals, output.vals + output.nrows * output.ncols, 0);

    for(I r = 0; r < input.nrows; r++)
    {
        I row = r;
        I i_start = input.rowptrs[r];
        I i_end = input.rowptrs[r+1];
        for(I i_in = i_start; i_in < i_end; i_in++)
        {
            I col_in = input.colidxs[i_in];
            I col_out = perm.map_src_to_dst[col_in];
            I i_out = row * output.ncols + col_out;
            output.vals[i_out] = input.vals[i_in];
        }
    }
}



template<typename T, typename I, typename Ao, typename Ai>
static void sparse_to_dense_select_rows(MatrixDense<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input, const std::vector<I> & rows)
{
    if(output.nrows != rows.size() || output.ncols != input.ncols) MY_ABORT("sparse_to_dense_select_rows: output matrix has wrong dimensions");

    std::fill(output.vals, output.vals + output.get_nvals(), I{0});

    for(I r_out = 0; r_out < output.nrows; r_out++)
    {
        I r_in = rows[r_out];
        I start = input.rowptrs[r_in];
        I end = input.rowptrs[r_in+1];
        for(I i = start; i < end; i++)
        {
            I c = input.colidxs[i];
            output.vals[r_out * output.ncols + c] = input.vals[i];
        }
    }
}



template<typename T, typename I, typename Ao, typename Ai, typename Ap>
static void permute_matrix_cols(MatrixCSR<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input, const Permutation<I,Ap> & perm)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols || output.nvals != input.nvals) MY_ABORT("permute_matrix_cols: output matrix has wrong dimensions");
    if(perm.size != input.ncols) MY_ABORT("permute_matrix_cols: permutation has wrong size");

    struct colval{ I col; T val; colval(I c, T v){ col = c; val = v;} };
    
    std::copy(input.rowptrs, input.rowptrs + input.nrows + 1, output.rowptrs);

    for(I r = 0; r < input.nrows; r++)
    {
        I start = input.rowptrs[r];
        I end = input.rowptrs[r+1];
        std::vector<colval> colvals_out;
        colvals_out.reserve(end - start);
        for(I i = start; i < end; i++)
        {
            T val = input.vals[i];
            I col_in = input.colidxs[i];
            I col_out = perm.map_src_to_dst[col_in];
            colvals_out.emplace_back(col_out, val);
        }
        std::sort(colvals_out.begin(), colvals_out.end(), [&](const colval & l, const colval & r){ return l.col < r.col; });
        for(size_t j = 0; j < colvals_out.size(); j++) { output.colidxs[start + j] = colvals_out[j].col; output.vals[start + j] = colvals_out[j].val; }
    }
}



template<typename T, typename I, typename Ao, typename Ai, typename Ap>
static void permute_matrix_rows(MatrixCSR<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input, const Permutation<I,Ap> & perm_orig, bool invperm = false)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols || output.nvals != input.nvals) MY_ABORT("permute_matrix_rows: output matrix has wrong dimensions");
    if(perm_orig.size != input.ncols) MY_ABORT("permute_matrix_rows: permutation has wrong size");

    Permutation<I,Ap> perm = perm_orig.get_view();
    if(invperm) perm.invert();

    std::vector<I> out_row_nnz(output.nrows+1);
    for(I r_in = 0; r_in < input.nrows; r_in++)
    {
        I nnz = input.rowptrs[r_in+1] - input.rowptrs[r_in];
        I r_out = perm.map_src_to_dst[r_in];
        out_row_nnz[r_out] = nnz;
    }
    output.rowptrs[0] = 0;
    for(I r = 1; r <= output.nrows; r++) output.rowptrs[r] = output.rowptrs[r-1] + out_row_nnz[r-1];

    for(I r_in = 0; r_in < input.nrows; r_in++)
    {
        I r_out = perm.map_src_to_dst[r_in];
        I instart = input.rowptrs[r_in];
        I inend = input.rowptrs[r_in+1];
        I outstart = output.rowptrs[r_out];
        std::copy(input.colidxs + instart, input.colidxs + inend, output.colidxs + outstart);
        std::copy(input.vals + instart, input.vals + inend, output.vals + outstart);
    }
}



template<typename T, typename I, typename Ao, typename Ai, typename Ap>
static void permute_matrix(MatrixCSR<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input, const Permutation<I,Ap> & perm)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols || output.nvals != input.nvals) MY_ABORT("permute_matrix: output matrix has wrong dimensions");
    if(perm.size != input.ncols) MY_ABORT("permute_matrix: permutation has wrong size");

    struct colval{ I col; T val; colval(I c, T v){ col = c; val = v;} };

    I curr_idx = 0;
    for(I r_out = 0; r_out < output.nrows; r_out++)
    {
        output.rowptrs[r_out] = curr_idx;
        I r_in = perm.map_dst_to_src[r_out];
        I in_start = input.rowptrs[r_in];
        I in_end = input.rowptrs[r_in+1];
        std::vector<colval> colvals_out;
        colvals_out.reserve(in_end - in_start);
        for(I i = in_start; i < in_end; i++)
        {
            T val = input.vals[i];
            I col_in = input.colidxs[i];
            I col_out = perm.map_src_to_dst[col_in];
            colvals_out.emplace_back(col_out, val);
        }
        std::sort(colvals_out.begin(), colvals_out.end(), [](const colval & l, const colval & r) { return l.col < r.col; });
        for(size_t j = 0; j < colvals_out.size(); j++)
        {
            output.colidxs[curr_idx] = colvals_out[j].col;
            output.vals[curr_idx] = colvals_out[j].val;
            curr_idx++;
        }
    }
    output.rowptrs[output.nrows] = curr_idx;
}



template<typename T, typename I, typename Ao, typename Ai, typename Ap>
static void permute_matrix_upper(MatrixCSR<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input, const Permutation<I,Ap> & perm, putimers & tm)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols || output.nvals != input.nvals) MY_ABORT("permute_matrix_upper: output matrix has wrong dimensions");
    if(perm.size != input.ncols) MY_ABORT("permute_matrix_upper: permutation has wrong size");
    struct colval{ I col; T val; colval(I c, T v){ col = c; val = v;} };

    tm.read.start();
    std::vector<std::vector<colval>> out_data(output.nrows);
    for(I r_in = 0; r_in < input.nrows; r_in++)
    {
        I r_out = perm.map_src_to_dst[r_in];
        I start = input.rowptrs[r_in];
        I end = input.rowptrs[r_in+1];
        for(I i = start; i < end; i++)
        {
            T val = input.vals[i];
            I c_in = input.colidxs[i];
            I c_out = perm.map_src_to_dst[c_in];
            if(r_out < c_out) out_data[r_out].emplace_back(c_out, val);
            else out_data[c_out].emplace_back(r_out, val);
        }
    }
    tm.read.stop();

    tm.sort.start();
    for(I r_out = 0; r_out < output.nrows; r_out++)
    {
        std::sort(out_data[r_out].begin(), out_data[r_out].end(), [&](const colval & l, const colval & r){ return l.col < r.col; });
    }
    tm.sort.stop();

    tm.write.start();
    I curr_idx = 0;
    for(I r_out = 0; r_out < output.nrows; r_out++)
    {
        output.rowptrs[r_out] = curr_idx;
        for(size_t j = 0; j < out_data[r_out].size(); j++)
        {
            output.colidxs[curr_idx] = out_data[r_out][j].col;
            output.vals[curr_idx] = out_data[r_out][j].val;
            curr_idx++;
        }
    }
    output.rowptrs[output.nrows] = curr_idx;
    tm.write.stop();
}



template<typename T, typename I, typename Ao, typename Ai, typename Ap>
static void permute_matrix_upper(MatrixCSR<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input, const Permutation<I,Ap> & perm)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols || output.nvals != input.nvals) MY_ABORT("permute_matrix_upper: output matrix has wrong dimensions");
    if(perm.size != input.ncols) MY_ABORT("permute_matrix_upper: permutation has wrong size");
    struct colval{ I col; T val; colval(I c, T v){ col = c; val = v;} };

    std::vector<std::vector<colval>> out_data(output.nrows);
    for(I r_in = 0; r_in < input.nrows; r_in++)
    {
        I r_out = perm.map_src_to_dst[r_in];
        I start = input.rowptrs[r_in];
        I end = input.rowptrs[r_in+1];
        for(I i = start; i < end; i++)
        {
            T val = input.vals[i];
            I c_in = input.colidxs[i];
            I c_out = perm.map_src_to_dst[c_in];
            if(r_out < c_out) out_data[r_out].emplace_back(c_out, val);
            else out_data[c_out].emplace_back(r_out, val);
        }
    }

    for(I r_out = 0; r_out < output.nrows; r_out++)
    {
        std::sort(out_data[r_out].begin(), out_data[r_out].end(), [&](const colval & l, const colval & r){ return l.col < r.col; });
    }

    I curr_idx = 0;
    for(I r_out = 0; r_out < output.nrows; r_out++)
    {
        output.rowptrs[r_out] = curr_idx;
        for(size_t j = 0; j < out_data[r_out].size(); j++)
        {
            output.colidxs[curr_idx] = out_data[r_out][j].col;
            output.vals[curr_idx] = out_data[r_out][j].val;
            curr_idx++;
        }
    }
    output.rowptrs[output.nrows] = curr_idx;
}



template<typename T, typename I, typename Ao, typename Ai, typename Ap>
static void permute_matrix_rows(MatrixDense<T,I,Ao> & output, const MatrixDense<T,I,Ai> & input, const Permutation<I,Ap> & perm_orig, bool invperm = false)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols || output.ld != input.ld) MY_ABORT("permute_matrix_rows: output matrix has wrong dimensions");
    if(perm_orig.size != input.ncols) MY_ABORT("permute_matrix_rows: permutation has wrong size");

    Permutation<I,Ap> perm = perm_orig.get_view();
    if(invperm) perm.invert();

    for(I r_in = 0; r_in < input.nrows; r_in++)
    {
        I r_out = perm.map_src_to_dst[r_in];
        std::copy(input.vals + r_in * input.ld, input.vals + r_in * input.ld + input.ncols, output.vals + r_out * output.ld);
    }
}



template<typename T, typename I, typename Ao, typename Aa, typename Ab>
static void matrix_add(MatrixCSR<T,I,Ao> & output, const MatrixCSR<T,I,Aa> & in1, const MatrixCSR<T,I,Ab> & in2)
{
    // add in2 to in1 and stores result in output, assumung all nonzero entries from in2 already exist in in1

    if(in1.nrows != in2.nrows || in1.ncols != in2.ncols) MY_ABORT("matrix_add: input matrices have non-matching dimensions");
    if(output.nrows != in1.nrows || output.ncols != in1.ncols || output.nvals != in1.nvals) MY_ABORT("matrix_add: output matrix has wrong dimensions");
    std::copy(in1.vals, in1.vals + in1.nvals, output.vals);
    std::copy(in1.colidxs, in1.colidxs + in1.nvals, output.colidxs);
    std::copy(in1.rowptrs, in1.rowptrs + in1.nrows + 1, output.rowptrs);

    for(I r = 0; r < in2.nrows; r++)
    {
        I i_start = in2.rowptrs[r];
        I i_end = in2.rowptrs[r+1];
        for(I i = i_start; i < i_end; i++)
        {
            I c = in2.colidxs[i];
            I * begin = output.colidxs + output.rowptrs[r];
            I * end = output.colidxs + output.rowptrs[r+1];
            I * res = std::find(begin, end, c);
            I idx = res - output.colidxs;
            if(res == end) MY_ABORT("matrix_add: in2 contains nz entry not present in in1");
            output.vals[idx] += in2.vals[i];
        }
    }
}



template<typename T, typename I, typename A>
static void matrix_scale_inplace(MatrixDense<T,I,A> & matrix, T scalar)
{
    I nvals = matrix.size_alloc();
    for(I i = 0; i < nvals; i++)
    {
        matrix.vals[i] *= scalar;
    }
}



template<typename T, typename I, typename Ai, typename Ao>
static void matrix_scale_outofplace(MatrixDense<T,I,Ao> & output, const MatrixDense<T,I,Ai> & input, T scalar)
{
    if(output.nrows != input.nrows || output.ncols != input.ncols) MY_ABORT("matrix_scale_outofplace: output matrix has wrong dimensions");

    if(output.ld == input.ld)
    {
        I nvals = input.size_alloc();
        for(I i = 0; i < nvals; i++)
        {
            output.vals[i] = scalar * input.vals[i];
        }
    }
    else
    {
        for(I r = 0; r < input.nrows; r++)
        {
            T * row_in = input.vals + r * input.ld;
            T * row_out = output.vals + r * output.ld;
            for(I c = 0; c < input.ncols; c++)
            {
                row_out[c] = scalar * row_in[c];
            }
        }
    }
}



template<typename I, typename Ao, typename Ai>
static void copy_permutation(Permutation<I,Ao> & output, const Permutation<I,Ai> & input)
{
    if(output.size != input.size) MY_ABORT("copy_permutation: output permutation has wrong dimension");
    std::copy_n(input.map_dst_to_src, input.size, output.map_dst_to_src);
    std::copy_n(input.map_src_to_dst, input.size, output.map_src_to_dst);
}



template<typename T, typename I, typename Ao, typename Ai>
static void matrix_transpose(MatrixDense<T,I,Ao> & output, const MatrixDense<T,I,Ai> & input)
{
    if(output.nrows != input.ncols || output.ncols != input.nrows) MY_ABORT("matrix_transpose: output matrix has wrong dimensions");

    for(I r = 0; r < input.nrows; r++)
    {
        for(I c = 0; c < input.ncols; c++)
        {
            output.vals[c * output.ld + r] = input.vals[r * input.ld + c];
        }
    }
}



template<typename T, typename I, typename Ao, typename Ai>
static void matrix_transpose(MatrixCSR<T,I,Ao> & output, const MatrixCSR<T,I,Ai> & input)
{
    if(output.nrows != input.ncols || output.ncols != input.nrows || output.nvals != input.nvals) MY_ABORT("matrix_transpose: output matrix has wrong dimensions");

    struct colval{ I col; T val; colval(I c, T v){ col = c; val = v;} };

    std::vector<std::vector<colval>> out_rows(output.nrows);

    for(I r = 0; r < input.nrows; r++)
    {
        I start = input.rowptrs[r];
        I end = input.rowptrs[r+1];
        for(I i = start; i < end; i++)
        {
            I c = input.colidxs[i];
            T v = input.vals[i];
            out_rows[c].emplace_back(r, v);
        }
    }

    I curr_idx = 0;
    for(I row_out = 0; row_out < output.nrows; row_out++)
    {
        output.rowptrs[row_out] = curr_idx;
        std::vector<colval> & data = out_rows[row_out];
        for(size_t i = 0; i < data.size(); i++)
        {
            output.colidxs[curr_idx] = data[i].col;
            output.vals[curr_idx] = data[i].val;
            curr_idx++;
        }
    }
    output.rowptrs[output.nrows] = curr_idx;
}



template<typename T, typename I, typename Ao, typename Am, typename Ai>
static void matrix_transpose_and_get_map(MatrixCSR<T,I,Ao> & output, MatrixDense<I,I,Am> & map, const MatrixCSR<T,I,Ai> & input)
{
    if(output.nrows != input.ncols || output.ncols != input.nrows || output.nvals != input.nvals) MY_ABORT("matrix_transpose_and_get_map: output matrix has wrong dimensions");
    if(input.nvals != map.nrows) MY_ABORT("matrix_transpose_and_get_map: input map has wrong dimensions");

    struct colvalidx{ T val; I col; I idx; colvalidx(I c, T v, I i){ col = c; val = v; idx = i;} };

    std::vector<std::vector<colvalidx>> out_rows(output.nrows);

    for(I r = 0; r < input.nrows; r++)
    {
        I start = input.rowptrs[r];
        I end = input.rowptrs[r+1];