Schur.cpp

Go to the documentation of this file.

/**
 * @file Schur.cpp
 * @brief Implementation of Schur Complement
 * @date 2023-02-03
 *
 * @copyright Copyright (c) 2023
 *
 */
 
namespace MoFEM {
 
constexpr bool debug_schur = false;
 
struct OpSchurAssembleBaseImpl : public OpSchurAssembleBase {
  OpSchurAssembleBaseImpl() = delete;
 
protected:
  using OpSchurAssembleBase::OpSchurAssembleBase;
 
  /**
   * @brief Assemble Schur complement
   *
   */
  inline MoFEMErrorCode assembleSchurMat(
 
      Mat S,
 
      const UId &uid_row, VectorInt &row_ind,
 
      const UId &uid_col, VectorInt &col_ind,
 
      MatrixDouble &m, InsertMode iora
 
  );
};
 
// Assemble specialisations
MoFEMErrorCode OpSchurAssembleBaseImpl::assembleSchurMat(
 
    Mat S,
 
    const UId &uid_row, VectorInt &row_ind,
 
    const UId &uid_col, VectorInt &col_ind,
 
    MatrixDouble &m, InsertMode iora
 
) {
  return ::MatSetValues(
 
      S, row_ind.size(), &*row_ind.begin(), col_ind.size(), &*col_ind.begin(),
      &*m.data().begin(), iora
 
  );
}
 
/**
 * @brief Clear Schur complement internal data
 *
 */
struct OpSchurAssembleBegin : public OpSchurAssembleBaseImpl {
 
  OpSchurAssembleBegin();
 
  MoFEMErrorCode doWork(int side, EntityType type,
                        EntitiesFieldData::EntData &data);
};
 
struct OpSchurZeroRowsAndCols : public OpSchurAssembleBaseImpl {
 
  OpSchurZeroRowsAndCols(
      boost::shared_ptr<std::vector<unsigned char>> marker_ptr,
      double diag_val);
 
  MoFEMErrorCode doWork(int side, EntityType type,
                        EntitiesFieldData::EntData &data);
 
private:
  boost::shared_ptr<std::vector<unsigned char>> markerPtr;
  double diagVal;
};
 
/**
 * @brief Assemble Schur complement (Implementation)
 *
 */
template <typename OP_SCHUR_ASSEMBLE_BASE>
struct OpSchurAssembleEndImpl : public OP_SCHUR_ASSEMBLE_BASE {
 
  using OP = OP_SCHUR_ASSEMBLE_BASE;
 
  /**
   * @brief Construct a new Op Schur Assemble End object
   *
   * @param fields_name list of fields
   * @param field_ents list of entities on which schur complement is applied
   * (can be empty)
   * @param schur_ao map indices to schur matrix indices
   * @param schur_mat schur matrix
   * @param sym_schur true if schur is symmetric
   * @param symm_op true if block diagonal is symmetric
   */
  OpSchurAssembleEndImpl(
 
      std::vector<std::string> fields_name,
      std::vector<boost::shared_ptr<Range>> field_ents,
 
      SmartPetscObj<AO> schur_ao, SmartPetscObj<Mat> schur_mat,
 
      bool sym_schur, bool symm_op);
 
protected:
  template <typename I>
  MoFEMErrorCode doWorkImpl(
 
      int side, EntityType type, EntitiesFieldData::EntData &data
 
  );
 
  std::vector<std::string> fieldsName;
  std::vector<boost::shared_ptr<Range>> fieldEnts;
  SmartPetscObj<AO> schurAO;
  SmartPetscObj<Mat> schurMat;
  bool symSchur;
 
  MatrixDouble blockMat;
  MatrixDouble invMat;
  MatrixDouble bM, abM, abcM;
};
 
struct SchurDSYSV; ///< SY  symmetric
struct SchurDGESV; ///< GE  general (i.e., nonsymmetric, in some cases
                   ///< rectangular)
 
/**
 * @brief Assemble Schur complement
 *
 */
template <typename I> struct OpSchurAssembleEnd;
 
template <>
struct OpSchurAssembleEnd<SchurDSYSV>
    : public OpSchurAssembleEndImpl<OpSchurAssembleBaseImpl> {
  using OpSchurAssembleEndImpl<OpSchurAssembleBaseImpl>::OpSchurAssembleEndImpl;
  MoFEMErrorCode doWork(int side, EntityType type,
                        EntitiesFieldData::EntData &data);
};
 
template <>
struct OpSchurAssembleEnd<SchurDGESV>
    : public OpSchurAssembleEndImpl<OpSchurAssembleBaseImpl> {
  using OpSchurAssembleEndImpl<OpSchurAssembleBaseImpl>::OpSchurAssembleEndImpl;
  MoFEMErrorCode doWork(int side, EntityType type,
                        EntitiesFieldData::EntData &data);
};
 
/**
 * @brief Schur complement data storage
 *
 */
struct SchurElemMats : public boost::enable_shared_from_this<SchurElemMats> {
 
  SchurElemMats(const size_t idx, const UId uid_row, const UId uid_col);
  virtual ~SchurElemMats() = default;
 
  const size_t iDX;
  UId uidRow;
  UId uidCol;
 
  inline auto &getMat() const { return locMats[iDX]; }
  inline auto &getRowInd() const { return rowIndices[iDX]; }
  inline auto &getColInd() const { return colIndices[iDX]; }
 
  static MoFEMErrorCode MatSetValues(Mat M,
                                     const EntitiesFieldData::EntData &row_data,
                                     const EntitiesFieldData::EntData &col_data,
                                     const MatrixDouble &mat, InsertMode iora);
 
protected:
  static MoFEMErrorCode
  assembleStorage(const EntitiesFieldData::EntData &row_data,
                  const EntitiesFieldData::EntData &col_data,
                  const MatrixDouble &mat, InsertMode iora);
 
  friend struct OpSchurAssembleBegin;
  friend struct OpSchurZeroRowsAndCols;
  template <typename OP_SCHUR_ASSEMBLE_BASE>
  friend struct OpSchurAssembleEndImpl;
 
  struct uid_mi_tag {};
  struct col_mi_tag {};
 
  using SchurElemStorage = multi_index_container<
      const SchurElemMats *,
      indexed_by<
 
          ordered_unique<
              tag<uid_mi_tag>,
              composite_key<
                  SchurElemMats,
 
                  member<SchurElemMats, const UId, &SchurElemMats::uidRow>,
                  member<SchurElemMats, const UId, &SchurElemMats::uidCol>
 
                  >>,
 
          ordered_non_unique<tag<col_mi_tag>, member<SchurElemMats, const UId,
                                                     &SchurElemMats::uidCol>>
 
          >>;
 
  static boost::ptr_vector<MatrixDouble> locMats;
  static boost::ptr_vector<VectorInt> rowIndices;
  static boost::ptr_vector<VectorInt> colIndices;
  static boost::ptr_vector<SchurElemMats> schurElemMats;
  static size_t maxIndexCounter;
 
  static SchurElemStorage schurL2Storage;
};
 
/**
 * @brief Storage of block matrix, shared by block matrix and its nested
 * (Schur) sub-matrices
 *
 * Block is dense sub-matrix coupling DOFs on one row entity with DOFs on one
 * column entity, stored row-major at mat_shift with leading dimension ld.
 *
 * Blocks which are not part of A00 are stored in data. When nested Schur
 * structure is created (see createSchurNestedMatrixStruture), blocks of A00
 * which belong to one finite element are stored in a buffer of that element.
 * Elements with the same block pattern (entities, numbers of DOFs and listed
 * pairs) share a Pattern, which defines the layout of element buffers.
 *
 * Element buffers start in listed layout (listed blocks only). At the first
 * factorisation each pattern is analysed on a probe element: nodes (entities)
 * whose diagonal block is a safe pivot are eliminated one by one (block LU in
 * place, fill only where the pattern creates it), the remaining nodes form a
 * dense core factorised with partial pivoting. Element buffers are then relaid
 * element by element, and factors are kept until MatZeroEntries. Elements
 * failing the pivot test at a later factorisation are factorised dense in a
 * buffer of their own. See doc/development/schur.md.
 *
 * Factorisation works on M = A^T, which is how LAPACK reads row-major element
 * blocks. Slot (r, c), i.e. block of A coupling row node r with column node c,
 * read column-major is block (c, r) of M.
 *
 * If preconditioner blocks are requested, they are allocated for A00 element
 * buffers only, with the same layout. Then factors of (A00 + preconditioner)
 * are stored in place of preconditioner blocks, and A00 blocks are intact.
*/
struct BlockStorage {
 
  /**
   * @brief block data indexes
   *
   */
  struct Indexes {
 
    Indexes(UId uid_row, UId uid_col, UId uid_fe, int row, int col, int nb_rows,
            int nb_cols, int loc_row, int loc_col)
        : uid_row(uid_row), uid_col(uid_col), uid_fe(uid_fe), row(row),
          col(col), nb_rows(nb_rows), nb_cols(nb_cols), loc_row(loc_row),
          loc_col(loc_col), mat_shift(0), ld(nb_cols), elem(-1) {}
 
    inline UId getRowUId() const { return uid_row; }
    inline UId getColUId() const { return uid_col; }
    inline UId getFEUId() const { return uid_fe; }
    inline int getRow() const { return row; }
    inline int getCol() const { return col; }
    inline int getNbRows() const { return nb_rows; }
    inline int getNbCols() const { return nb_cols; }
    inline int getLocRow() const { return loc_row; }
    inline int getLocCol() const { return loc_col; }
    inline size_t getMatShift() const { return mat_shift; }
    inline int getLd() const { return ld; }
    inline int getElem() const { return elem; }
 
    inline void setStorage(size_t shift, int leading_dim, int element) {
      mat_shift = shift;
      ld = leading_dim;
      elem = element;
    }
 
  private:
    UId uid_row;
    UId uid_col;
    UId uid_fe;
    int row;
    int col;
    int nb_rows;
    int nb_cols;
    int loc_row;
    int loc_col;
    size_t mat_shift; ///< shift in data, or in element buffer if elem >= 0
    int ld;           ///< leading dimension of block in storage
    int elem;         ///< index of A00 element, -1 if none
  };
 
  /// DOFs on one entity (node) of A00 element
  struct ElemEntity {
    UId uid;
    int nb;         ///< number of DOFs
    int offset;     ///< offset in element vector (layout of pattern)
    int locParent;  ///< local index in block matrix numbering
    int globParent; ///< global index in block matrix numbering
    int locBlock;   ///< local index in A00 sub-problem numbering
    int globBlock;  ///< global index in A00 sub-problem numbering
  };
 
  /**
   * @brief Block pattern of A00 element and layout of element buffer
   *
   * Node is an entity of the element. Slot (r, c) couples row node r with
   * column node c. It is stored row-major at slotShift with leading dimension
   * slotLd, which read column-major is block (c, r) of M = A^T.
   */
  struct Pattern {
 
    enum State {
      LISTED, ///< listed blocks only, not analysed
      BLOCK,  ///< block elimination of order nodes, then dense core
      DENSE   ///< dense element matrix (core contains all nodes)
    };
 
    int nbNodes = 0;
    int size = 0; ///< DOFs per element
    int nbElems = 0;
    int nbListed = 0;
    std::vector<int> nodeSize;
    std::vector<std::string> nodeName;
    std::vector<char> listed; ///< nbNodes x nbNodes, slot (r, c) is listed
 
    State state = LISTED;
    std::vector<long> slotShift; ///< nbNodes x nbNodes, -1 if not stored
    std::vector<int> slotLd;
    std::vector<int> nodeOffset; ///< offset of node DOFs in element vector
    std::vector<int> order;      ///< nodes eliminated block by block
    std::vector<int> core;       ///< nodes of dense core
    size_t coreShift = 0;        ///< shift of dense core in element buffer
    int coreSize = 0;            ///< DOFs in dense core
    int coreOffset = 0;          ///< offset of core DOFs in element vector
    size_t elemSize = 0;         ///< doubles in element buffer
 
    int nbFallback = 0; ///< elements factorised dense in last factorisation
    double beRef = 0;   ///< backward error of dense solve on probe
    double beMax = 0;   ///< largest backward error in last factorisation
    bool warned = false;
 
    inline long sShift(int r, int c) const {
      return slotShift[r * nbNodes + c];
    }
    inline int sLd(int r, int c) const { return slotLd[r * nbNodes + c]; }
    inline bool isListed(int r, int c) const { return listed[r * nbNodes + c]; }
    /// shift of block (a, b) of M = A^T, column-major
    inline long mShift(int a, int b) const { return sShift(b, a); }
    /// leading dimension of block (a, b) of M = A^T
    inline int mLd(int a, int b) const { return sLd(b, a); }
  };
 
  /// A00 element
  struct Elem {
    int pattern;             ///< index of pattern
    int size;                ///< DOFs of element
    int entBegin;            ///< first entity in elemEntities
    int entEnd;              ///< one past last entity in elemEntities
    int pivShift;            ///< shift in pivots
    bool local = false;      ///< all entities have local index on this process
    bool owned = false;      ///< all entities are owned by this process
    bool factorised = false; ///< factors are stored for this element
    bool dense = false;      ///< factors are in elemDense (pivot test failed)
  };
 
  std::vector<Indexes> blockIndex; ///< blocks sorted by (row uid, col uid)
  std::vector<int> feOrder;  ///< blocks sorted by (fe uid, row uid, col uid)
  std::vector<int> colOrder; ///< blocks sorted by (col uid, row uid)
 
  std::vector<Pattern> patterns;
  std::vector<Elem> elems;
  std::vector<ElemEntity> elemEntities;
  std::vector<std::vector<double>> elemData;  ///< A00 element buffers
  std::vector<std::vector<double>> elemPrec;  ///< preconditioner buffers
  std::vector<std::vector<double>> elemDense; ///< dense factors (fallback)
  int maxElemSize = 0;                        ///< maximal DOFs of element
  size_t maxElemBuffer = 0; ///< maximal size of element buffer
 
  size_t dataSize = 0;      ///< required size of data
  std::vector<double> data; ///< blocks not in A00, allocated on first use
  std::vector<__CLPK_integer> pivots;
 
  /// data is allocated on first use, such that storage is not allocated twice
  /// when layout is changed by createSchurNestedMatrixStruture
  inline void ensureAllocated() {
    if (data.size() != dataSize)
      data.assign(dataSize, 0.);
  }
 
  enum FactorStorage {
    FACTOR_NONE,
    FACTOR_IN_DATA,
    FACTOR_IN_PRECONDITIONER,
    FACTOR_FAILED ///< factorisation failed, data is corrupted
  };
  FactorStorage factorStorage = FACTOR_NONE;
  bool nested = false;            ///< nested Schur structure has been created
  bool hasPreconditioner = false; ///< preconditioner blocks are enabled
 
  // options, see createSchurNestedMatrixStruture
  bool optDense = false;           ///< -schur_a00_dense
  double optPivotThreshold = 0;    ///< -schur_a00_pivot_threshold
  double optRcondMin = 1e-12;      ///< -schur_a00_rcond_min
  double optBackwardError = 1e-12; ///< -schur_a00_backward_error
 
  inline bool isFactorised() const { return factorStorage != FACTOR_NONE; }
 
  /// Fails if factorisation failed, storage has to be zeroed
  inline MoFEMErrorCode checkNotFailed() const {
    MoFEMFunctionBeginHot;
    if (factorStorage == FACTOR_FAILED)
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "A00 factorisation failed and block matrix data is corrupted, "
              "call MatZeroEntries and assemble again");
    MoFEMFunctionReturnHot(0);
  }
 
  /// Call before modification of data, fails if factors are stored in place
  inline MoFEMErrorCode modified() const {
    MoFEMFunctionBeginHot;
    CHKERR checkNotFailed();
    if (isFactorised())
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "A00 element matrices are factorised in place, call "
              "MatZeroEntries before assembling or modifying block matrix");
    MoFEMFunctionReturnHot(0);
  }
 
  inline void resetFactors() {
    factorStorage = FACTOR_NONE;
    for (auto &e : elems) {
      e.factorised = false;
      e.dense = false;
    }
    for (auto &d : elemDense)
      std::vector<double>().swap(d);
  }
 
  /// values of block, preconditioner buffer if prec is true
  inline double *blockPtr(const Indexes &b, bool prec = false) {
    if (b.getElem() >= 0)
      return (prec ? elemPrec : elemData)[b.getElem()].data() + b.getMatShift();
    return data.data() + b.getMatShift();
  }
 
  static inline bool lessRowCol(const Indexes &a, const Indexes &b) {
    if (a.getRowUId() != b.getRowUId())
      return a.getRowUId() < b.getRowUId();
    return a.getColUId() < b.getColUId();
  }
 
  /// find block by row and column entity uid, return nullptr if not found
  inline const Indexes *find(const UId &row, const UId &col) const {
    auto it = std::lower_bound(
        blockIndex.begin(), blockIndex.end(), std::make_pair(row, col),
        [](const Indexes &a, const std::pair<UId, UId> &p) {
          if (a.getRowUId() != p.first)
            return a.getRowUId() < p.first;
          return a.getColUId() < p.second;
        });
    if (it != blockIndex.end() && it->getRowUId() == row &&
        it->getColUId() == col)
      return &*it;
    return nullptr;
  }
 
  inline Indexes *findMutable(const UId &row, const UId &col) {
    return const_cast<Indexes *>(find(row, col));
  }
 
  struct RowUIdLess {
    bool operator()(const Indexes &a, const UId &b) const {
      return a.getRowUId() < b;
    }
    bool operator()(const UId &a, const Indexes &b) const {
      return a < b.getRowUId();
    }
  };
 
  /// range of blocks with given row entity uid
  inline auto rowRange(const UId &row) const {
    return std::equal_range(blockIndex.begin(), blockIndex.end(), row,
                            RowUIdLess());
  }
 
  /// range (in colOrder) of blocks with given column entity uid
  inline auto colRange(const UId &col) const {
    auto lo = std::lower_bound(
        colOrder.begin(), colOrder.end(), col,
        [this](int i, const UId &c) { return blockIndex[i].getColUId() < c; });
    auto hi = std::upper_bound(
        colOrder.begin(), colOrder.end(), col,
        [this](const UId &c, int i) { return c < blockIndex[i].getColUId(); });
    return std::make_pair(lo, hi);
  }
};
 
/**
 * @brief Context of block matrix shell
 *
 * Block matrix, and each of nested (Schur) sub-matrices, keeps list of blocks
 * in its own local numbering. Storage of block values is shared between
 * matrices created from the same block structure.
 */
struct BlockStructure {
 
  /// numbering of A00 element matrix entities used by this matrix
  enum Numbering { PARENT, BLOCK };
 
  /// block in numbering of this matrix (blocks not stored in element buffers)
  struct Block {
    size_t shift;
    int ld;
    int row;
    int col;
    int nbRows;
    int nbCols;
    int locRow;
    int locCol;
  };
 
  boost::shared_ptr<BlockStorage> storage;
  std::vector<Block> blocks;
  bool hasElems = false; ///< A00 elements are part of this matrix
  Numbering numbering = PARENT;
 
  SmartPetscObj<Vec> ghostX;
  SmartPetscObj<Vec> ghostY;
 
  bool multiplyByPreconditioner = false;
 
  inline int locIdx(const BlockStorage::ElemEntity &e) const {
    return numbering == BLOCK ? e.locBlock : e.locParent;
  }
  inline int globIdx(const BlockStorage::ElemEntity &e) const {
    return numbering == BLOCK ? e.globBlock : e.globParent;
  }
};
 
PetscLogEvent SchurEvents::MOFEM_EVENT_schurMatSetValues;
PetscLogEvent SchurEvents::MOFEM_EVENT_opSchurAssembleEnd;
PetscLogEvent SchurEvents::MOFEM_EVENT_BlockStructureSetValues;
PetscLogEvent SchurEvents::MOFEM_EVENT_BlockStructureMult;
PetscLogEvent SchurEvents::MOFEM_EVENT_BlockStructureSolve;
PetscLogEvent SchurEvents::MOFEM_EVENT_zeroRowsAndCols;
PetscLogEvent SchurEvents::MOFEM_EVENT_AssembleSchurMat;
 
SchurEvents::SchurEvents() {
  PetscLogEventRegister("schurMatSetVal", 0, &MOFEM_EVENT_schurMatSetValues);
  PetscLogEventRegister("opSchurAsmEnd", 0, &MOFEM_EVENT_opSchurAssembleEnd);
  PetscLogEventRegister("blockSetVal", 0, &MOFEM_EVENT_BlockStructureSetValues);
  PetscLogEventRegister("blockMult", 0, &MOFEM_EVENT_BlockStructureMult);
  PetscLogEventRegister("blockSolve", 0, &MOFEM_EVENT_BlockStructureSolve);
  PetscLogEventRegister("schurZeroRandC", 0, &MOFEM_EVENT_zeroRowsAndCols);
  PetscLogEventRegister("assembleSchurMat", 0, &MOFEM_EVENT_AssembleSchurMat);
}
 
SchurElemMats::SchurElemStorage SchurElemMats::schurL2Storage;
boost::ptr_vector<MatrixDouble> SchurElemMats::locMats;
boost::ptr_vector<VectorInt> SchurElemMats::rowIndices;
boost::ptr_vector<VectorInt> SchurElemMats::colIndices;
boost::ptr_vector<SchurElemMats> SchurElemMats::schurElemMats;
size_t SchurElemMats::maxIndexCounter = 0;
 
SchurElemMats::SchurElemMats(const size_t idx, const UId uid_row,
                             const UId uid_col)
    : iDX(idx), uidRow(uid_row), uidCol(uid_col) {}
 
MoFEMErrorCode
SchurElemMats::assembleStorage(const EntitiesFieldData::EntData &row_data,
                               const EntitiesFieldData::EntData &col_data,
                               const MatrixDouble &mat, InsertMode iora) {
  MoFEMFunctionBegin;
 
#ifndef NDEBUG
  PetscLogEventBegin(SchurEvents::MOFEM_EVENT_schurMatSetValues, 0, 0, 0, 0);
#endif // NDEBUG
 
  // get row indices, in case of store, get indices from storage
  // storage keeps marked indices to manage boundary conditions
  auto get_row_indices = [&]() -> const VectorInt & {
    if (auto e_ptr = row_data.getFieldEntities()[0]) {
      if (auto stored_data_ptr =
              e_ptr->getSharedStoragePtr<EssentialBcStorage>()) {
        return stored_data_ptr->entityIndices;
      }
    }
    return row_data.getIndices();
  };
 
  const auto &row_ind = get_row_indices();
  const auto &col_ind = col_data.getIndices();
 
  const auto nb_rows = row_ind.size();
  const auto nb_cols = col_ind.size();
 
#ifndef NDEBUG
  if (mat.size1() != nb_rows) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "Wrong mat size %zu != %zu", mat.size1(), nb_rows);
  }
  if (mat.size2() != nb_cols) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "Wrong mat size %zu != %zu", mat.size2(), nb_cols);
  }
#endif // NDEBUG
 
  // get entity uid
  auto get_uid = [](auto &data) {
    if (data.getFieldEntities().size() == 1) {
 
      return data.getFieldEntities()[0]->getLocalUniqueId();
 
    } else {
 
      // Is assumed that sum of entities ids gives unique id, that is not true,
      // but corner case is improbable.
 
      // @todo: debug version should have test
 
      auto &uid0 = data.getFieldEntities()[0]->getLocalUniqueId();
      auto field_id0 = FieldEntity::getFieldBitNumberFromUniqueId(uid0);
      auto ent0 = FieldEntity::getHandleFromUniqueId(uid0);
      auto type0 = type_from_handle(ent0);
      auto id = id_from_handle(ent0);
 
      for (auto i = 1; i < data.getFieldEntities().size(); ++i) {
 
        // get entity id from ent
        id += id_from_handle(
 
            // get entity handle from unique uid
            FieldEntity::getHandleFromUniqueId(
                data.getFieldEntities()[i]->getLocalUniqueId())
 
        );
      }
 
      return FieldEntity::getLocalUniqueIdCalculate(
          field_id0,
 
          ent_form_type_and_id(type0, id)
 
      );
    }
  };
 
  auto uid_row = get_uid(row_data);
  auto uid_col = get_uid(col_data);
 
  auto it =
      SchurElemMats::schurL2Storage.template get<SchurElemMats::uid_mi_tag>()
          .find(boost::make_tuple(uid_row, uid_col));
 
  if (it ==
      SchurElemMats::schurL2Storage.template get<SchurElemMats::uid_mi_tag>()
          .end()) {
 
    // get size of arrays of matrices
    const auto size = SchurElemMats::locMats.size();
 
    // expand memory allocation
    if (SchurElemMats::maxIndexCounter == size) {
      SchurElemMats::locMats.push_back(new MatrixDouble());
      SchurElemMats::rowIndices.push_back(new VectorInt());
      SchurElemMats::colIndices.push_back(new VectorInt());
      SchurElemMats::schurElemMats.push_back(
          new SchurElemMats(SchurElemMats::maxIndexCounter, uid_row, uid_col));
    } else {
      SchurElemMats::schurElemMats[SchurElemMats::maxIndexCounter].uidRow =
          uid_row;
      SchurElemMats::schurElemMats[SchurElemMats::maxIndexCounter].uidCol =
          uid_col;
    }
 
    // add matrix to storage
    auto p = SchurElemMats::schurL2Storage.emplace(
        &SchurElemMats::schurElemMats[SchurElemMats::maxIndexCounter++]);
#ifndef NDEBUG
    if (!p.second) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY, "Failed to insert");
    }
#endif // NDEBUG
 
    auto asmb = [&](auto &sm) {
      sm.resize(nb_rows, nb_cols, false);
      noalias(sm) = mat;
    };
 
    asmb((*p.first)->getMat());
 
    auto add_indices = [](auto &storage, auto &ind) {
      storage.resize(ind.size(), false);
      noalias(storage) = ind;
    };
 
    add_indices((*p.first)->getRowInd(), row_ind);
    add_indices((*p.first)->getColInd(), col_ind);
 
  } else {
    // entry (submatrix) already exists
 
    auto asmb = [&](auto &sm) {
      MoFEMFunctionBeginHot;
 
#ifndef NDEBUG
      if (sm.size1() != nb_rows) {
        SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
                "Wrong mat or storage size %zu != %zu", sm.size1(), nb_rows);
      }
      if (sm.size2() != nb_cols) {
        SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
                "Wrong mat or storage size %zu != %zu", sm.size2(), nb_cols);
      }
#endif // NDEBUG
 
      switch (iora) {
      case ADD_VALUES:
        sm += mat;
        break;
      case INSERT_VALUES:
        noalias(sm) = mat;
        break;
      default:
        SETERRQ(PETSC_COMM_SELF, MOFEM_NOT_IMPLEMENTED,
                "Assembly type not implemented");
      }
      MoFEMFunctionReturnHot(0);
    };
 
    CHKERR asmb((*it)->getMat());
 
    // no need to set indices
  }
 
#ifndef NDEBUG
  PetscLogEventEnd(SchurEvents::MOFEM_EVENT_schurMatSetValues, 0, 0, 0, 0);
#endif // NDEBUG
 
  MoFEMFunctionReturn(0);
}
 
OpSchurAssembleBegin::OpSchurAssembleBegin()
    : OpSchurAssembleBaseImpl(NOSPACE, OPSPACE) {}
 
MoFEMErrorCode OpSchurAssembleBegin::doWork(int side, EntityType type,
                                            EntitiesFieldData::EntData &data) {
  MoFEMFunctionBegin;
#ifndef NDEBUG
  if constexpr (debug_schur)
    MOFEM_LOG("SELF", Sev::noisy) << "Schur assemble begin";
#endif
  SchurElemMats::schurL2Storage.clear();
  SchurElemMats::maxIndexCounter = 0;
 
  MoFEMFunctionReturn(0);
}
 
OpSchurZeroRowsAndCols::OpSchurZeroRowsAndCols(
    boost::shared_ptr<std::vector<unsigned char>> marker_ptr, double diag_val)
    : OpSchurAssembleBaseImpl(NOSPACE, OPSPACE), markerPtr(marker_ptr),
      diagVal(diag_val) {}
 
MoFEMErrorCode
OpSchurZeroRowsAndCols::doWork(int side, EntityType type,
                               EntitiesFieldData::EntData &data) {
  MoFEMFunctionBegin;
 
  auto fe_ptr = getFEMethod();
  auto prb_ptr = fe_ptr->problemPtr;
  auto &dof = prb_ptr->getNumeredRowDofsPtr()->get<PetscGlobalIdx_mi_tag>();
 
  auto find_loc_dof_index = [&](auto glob) {
    auto it = dof.find(glob);
    if (it != dof.end()) {
      return (*it)->getPetscLocalDofIdx();
    }
    MOFEM_LOG("SELF", Sev::error) << "Global dof " << glob;
    CHK_THROW_MESSAGE(
        MOFEM_DATA_INCONSISTENCY,
        "Global dof index not found in local numered dofs storage");
    return -1;
  };
 
  for (auto &s : SchurElemMats::schurL2Storage) {
 
    auto zero_ind = [&](auto &ind) {
      for (auto i = 0; i < ind.size(); ++i) {
        if (ind[i] >= 0) {
          if ((*markerPtr)[find_loc_dof_index(ind[i])]) {
            ind[i] = -1;
          }
        }
      }
    };
 
    zero_ind(s->getRowInd());
    zero_ind(s->getColInd());
  }
 
  MoFEMFunctionReturn(0);
};
 
template <typename OP_SCHUR_ASSEMBLE_BASE>
OpSchurAssembleEndImpl<OP_SCHUR_ASSEMBLE_BASE>::OpSchurAssembleEndImpl(
    std::vector<std::string> fields_name,
    std::vector<boost::shared_ptr<Range>> field_ents,
    SmartPetscObj<AO> schur_ao, SmartPetscObj<Mat> schur_mat, bool sym_schur,
    bool symm_op)
    : OP(NOSPACE, OP::OPSPACE, symm_op), fieldsName(fields_name),
      fieldEnts(field_ents), schurAO(schur_ao), schurMat(schur_mat),
      symSchur(sym_schur) {}
 
template <typename OP_SCHUR_ASSEMBLE_BASE>
template <typename I>
MoFEMErrorCode OpSchurAssembleEndImpl<OP_SCHUR_ASSEMBLE_BASE>::doWorkImpl(
    int side, EntityType type, EntitiesFieldData::EntData &data) {
 
  MoFEMFunctionBegin;
 
#ifndef NDEBUG
  PetscLogEventBegin(SchurEvents::MOFEM_EVENT_opSchurAssembleEnd, 0, 0, 0, 0);
#endif
 
#ifndef NDEBUG
  if constexpr (debug_schur)
    MOFEM_LOG("SELF", Sev::noisy) << "Schur assemble begin -> end";
#endif
 
  auto get_a00_uids = [&]() {
    auto get_field_bit = [&](auto &name) {
      return OP::getPtrFE()->mField.get_field_bit_number(name);
    };
 
    std::vector<std::pair<UId, UId>> a00_uids;
    a00_uids.reserve(fieldsName.size());
    for (auto ss = 0; ss != fieldsName.size(); ++ss) {
      auto field_bit = get_field_bit(fieldsName[ss]);
      auto row_ents = fieldEnts[ss];
      if (row_ents) {
        for (auto p = row_ents->pair_begin(); p != row_ents->pair_end(); ++p) {
          auto lo_uid =
              FieldEntity::getLoLocalEntityBitNumber(field_bit, p->first);
          auto hi_uid =
              FieldEntity::getHiLocalEntityBitNumber(field_bit, p->second);
          a00_uids.push_back(std::make_pair(lo_uid, hi_uid));
        }
      } else {
        auto lo_uid = FieldEntity::getLoLocalEntityBitNumber(
            field_bit, get_id_for_min_type<MBVERTEX>());
        auto hi_uid = FieldEntity::getHiLocalEntityBitNumber(
            field_bit, get_id_for_max_type<MBENTITYSET>());
        a00_uids.push_back(std::make_pair(lo_uid, hi_uid));
      }
    }
    return a00_uids;
  };
 
#ifndef NDEBUG
  auto get_field_name = [&](auto uid) {
    return OP::getPtrFE()->mField.get_field_name(field_bit_from_bit_number(
        FieldEntity::getFieldBitNumberFromUniqueId(uid)));
  };
  auto list_storage = [&](auto &storage) {
    MoFEMFunctionBegin;
    int i = 0;
    for (auto &p : storage) {
      MOFEM_LOG("SELF", Sev::noisy)
          << "List schur storage: " << i << " " << p->iDX << ": "
          << get_field_name(p->uidRow) << " " << get_field_name(p->uidCol)
          << " : " << p->getMat().size1() << " " << p->getMat().size2();
      ++i;
    }
    MoFEMFunctionReturn(0);
  };
#endif // NDEBUG
 
  auto assemble_dense_blocks = [&]() {
    using matrix_range = ublas::matrix_range<MatrixDouble>;
    using range = ublas::range;
    MoFEMFunctionBegin;
    auto &storage = SchurElemMats::schurL2Storage;
 
    auto assemble_schur = [this](auto &m, auto &uid_row, auto &uid_col,
                                 auto *row_ind_ptr, auto *col_ind_ptr) {
      MoFEMFunctionBegin;
 
      if (schurMat) {
 
        if (auto ierr = this->assembleSchurMat(
 
                schurMat, uid_row, *row_ind_ptr, uid_col, *col_ind_ptr, m,
                ADD_VALUES
 
                )) {
#ifndef NDEBUG
          auto field_ents = OP::getPtrFE()->mField.get_field_ents();
          auto row_ent_it = field_ents->find(uid_row);
          auto col_ent_it = field_ents->find(uid_col);
          MOFEM_LOG_CHANNEL("SELF");
          if (row_ent_it != field_ents->end())
            MOFEM_LOG("SELF", Sev::error)
                << "Assemble row entity: " << (*row_ent_it)->getName() << " "
                << (*col_ent_it)->getEntTypeName() << " side "
                << (*row_ent_it)->getSideNumber();
          if (col_ent_it != field_ents->end())
            MOFEM_LOG("SELF", Sev::error)
                << "Assemble col entity: " << (*col_ent_it)->getName() << " "
                << (*col_ent_it)->getEntTypeName() << " side "
                << (*col_ent_it)->getSideNumber();
#endif // NDEBUG
          CHK_THROW_MESSAGE(ierr, "MatSetValues");
        }
      }
 
      MoFEMFunctionReturn(0);
    };
 
    auto a00_uids = get_a00_uids();
 
    auto get_block_indexing = [&](auto &a00_uids) {
      // iterate over a00 uids and find blocks
      std::vector<const SchurElemMats *> block_list;
      block_list.reserve(storage.size());
      for (auto &rp_uid : a00_uids) {
        auto [rlo_uid, rhi_uid] = rp_uid;
        for (auto &cp_uid : a00_uids) {
          auto [clo_uid, chi_uid] = cp_uid;
 
          auto it =
              storage.template get<SchurElemMats::uid_mi_tag>().lower_bound(
                  boost::make_tuple(rlo_uid, clo_uid));
          auto hi_it =
              storage.template get<SchurElemMats::uid_mi_tag>().upper_bound(
                  boost::make_tuple(rhi_uid, chi_uid));
 
          for (; it != hi_it; ++it) {
            if ((*it)->uidRow >= rlo_uid && (*it)->uidRow < rhi_uid &&
                (*it)->uidCol >= clo_uid && (*it)->uidCol < chi_uid) {
              block_list.push_back(*it);
            }
          }
        }
      }
 
      // create block indexes map for blockMat
      std::map<UId, std::pair<size_t, const VectorInt *>>
          block_indexing; // uid block map
      for (auto d : block_list) {
        if (block_indexing.find(d->uidRow) == block_indexing.end()) {
          block_indexing[d->uidRow] =
              std::make_pair(d->getRowInd().size(), &(d->getRowInd()));
        }
      }
 
      // set indexes to block
      int mat_block_size = 0; // size of block matrix
      for (auto &p_uid : a00_uids) {
        auto [lo_uid, hi_uid] = p_uid;
        auto lo = block_indexing.lower_bound(lo_uid);
        auto up = block_indexing.upper_bound(hi_uid);
        for (; lo != up; ++lo) {
          lo->second.first = mat_block_size;
          mat_block_size += lo->second.second->size();
        }
      }
 
      return std::make_tuple(block_list, block_indexing, mat_block_size);
    };
 
    auto get_schur_block_list = [&](auto &block_indexing) {
      std::vector<const SchurElemMats *> block_list;
      block_list.reserve(storage.size());
      for (auto &s : storage) {
        if (block_indexing.find(s->uidRow) == block_indexing.end() &&
            block_indexing.find(s->uidCol) == block_indexing.end()) {
          block_list.push_back(s);
        }
      }
      return block_list;
    };
 
    auto bi = get_block_indexing(a00_uids);
    auto &[block_list, block_indexing, block_mat_size] = bi;
 
    if (block_mat_size == 0) {
 
      if (schurAO) {
        for (auto &s : storage) {
          CHKERR AOApplicationToPetsc(schurAO, s->getRowInd().size(),
                                      &*s->getRowInd().begin());
          CHKERR AOApplicationToPetsc(schurAO, s->getColInd().size(),
                                      &*s->getColInd().begin());
        }
      }
 
      for (auto &s : storage) {
        auto &m = s->getMat();
        CHKERR assemble_schur(m, s->uidRow, s->uidCol, &(s->getRowInd()),
                              &(s->getColInd()));
      }
      MoFEMFunctionReturnHot(0);
    }
 
    blockMat.resize(block_mat_size, block_mat_size, false);
    blockMat.clear();
 
    auto get_range = [](auto &bi) {
      return range(bi.first, bi.first + bi.second->size());
    };
 
    for (auto &s : block_list) {
      auto &m = s->getMat();
#ifndef NDEBUG
      if (block_indexing.find(s->uidRow) == block_indexing.end())
        SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY, "Wrong rlo_uid");
      if (block_indexing.find(s->uidCol) == block_indexing.end())
        SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY, "Wrong clo_uid");
#endif // NDEBUG
 
      auto &rbi = block_indexing.at(s->uidRow);
      auto &cbi = block_indexing.at(s->uidCol);
 
      auto sub_mat = matrix_range(blockMat, get_range(rbi), get_range(cbi));
      sub_mat = m;
    }
 
    auto get_zeroed_indices = [&](auto extractor_uid, auto extractor_ind) {
      auto &[block_list, block_indexing, block_mat_size] = bi;
      std::vector<int> zeroed_indices;
      zeroed_indices.reserve(block_mat_size);
      for (auto &s : block_list) {
        auto &bi = block_indexing.at(extractor_uid(s));
        auto &ind = extractor_ind(s);
        for (auto it = ind.begin(); it != ind.end(); ++it) {
          if (*it < 0) {
            auto idx = bi.first + std::distance(ind.begin(), it);
            zeroed_indices.push_back(idx);
          }
        }
      }
      std::sort(zeroed_indices.begin(), zeroed_indices.end());
      auto it = std::unique(zeroed_indices.begin(), zeroed_indices.end());
      zeroed_indices.resize(std::distance(zeroed_indices.begin(), it));
      return zeroed_indices;
    };
    auto zero_rows = get_zeroed_indices(
        [](auto &s) { return s->uidRow; },
        [](auto &s) -> VectorInt & { return s->getRowInd(); });
    auto zero_cols = get_zeroed_indices(
        [](auto &s) { return s->uidCol; },
        [](auto &s) -> VectorInt & { return s->getColInd(); });
 
    for (auto i : zero_rows) {
      for (auto j = 0; j != blockMat.size2(); ++j) {
        blockMat(i, j) = 0;
      }
    }
    for (auto j : zero_cols) {
      for (auto i = 0; i != blockMat.size1(); ++i) {
        blockMat(i, j) = 0;
      }
    }
    for (auto i : zero_rows) {
      blockMat(i, i) = 1;
    }
 
    CHKERR I::invertMat(blockMat, invMat);
 
    // clear storage and block list from a00 blocks, no more needed
    for (auto &s : block_list) {
      auto it = storage.template get<SchurElemMats::uid_mi_tag>().find(
          boost::make_tuple(s->uidRow, s->uidCol));
      storage.template get<SchurElemMats::uid_mi_tag>().erase(it);
    }
    block_list.clear();
 
    if (schurAO) {
      for (auto &s : storage) {
        if (block_indexing.find(s->uidRow) == block_indexing.end()) {
          CHKERR AOApplicationToPetsc(schurAO, s->getRowInd().size(),
                                      &*s->getRowInd().begin());
        }
        if (block_indexing.find(s->uidCol) == block_indexing.end()) {
          CHKERR AOApplicationToPetsc(schurAO, s->getColInd().size(),
                                      &*s->getColInd().begin());
        }
      }
    }
 
    auto schur_block_list = get_schur_block_list(block_indexing);
    for (auto &s : schur_block_list) {
      auto &m = s->getMat();
      CHKERR assemble_schur(m, s->uidRow, s->uidCol, &(s->getRowInd()),
                            &(s->getColInd()));
    }
    for (auto &s : schur_block_list) {
      auto it = storage.template get<SchurElemMats::uid_mi_tag>().find(
          boost::make_tuple(s->uidRow, s->uidCol));
      storage.template get<SchurElemMats::uid_mi_tag>().erase(it);
    }
    schur_block_list.clear();
 
    for (
 
        auto rp_uid_it = a00_uids.begin(); rp_uid_it != a00_uids.end();
        ++rp_uid_it
 
    ) {
      auto [rlo_uid, rhi_uid] = *rp_uid_it;
      for (auto rm = block_indexing.lower_bound(rlo_uid);
           rm != block_indexing.upper_bound(rhi_uid); ++rm) {
        auto &rbi = rm->second;
 
        auto a_lo_tmp =
            storage.template get<SchurElemMats::col_mi_tag>().lower_bound(
                rm->first);
        auto a_hi =
            storage.template get<SchurElemMats::col_mi_tag>().upper_bound(
                rm->first);
 
        for (
 
            auto cp_uid_it = (symSchur) ? rp_uid_it : a00_uids.begin();
            cp_uid_it != a00_uids.end(); ++cp_uid_it
 
        ) {
          auto [clo_uid, chi_uid] = *cp_uid_it;
          for (auto cm = block_indexing.lower_bound(clo_uid);
               cm != block_indexing.upper_bound(chi_uid); ++cm) {
            auto &cbi = cm->second;
 
            auto c_lo_tmp =
                storage.template get<SchurElemMats::uid_mi_tag>().lower_bound(
                    boost::make_tuple(cm->first, 0));
            auto c_hi =
                storage.template get<SchurElemMats::uid_mi_tag>().upper_bound(
                    boost::make_tuple(cm->first, FieldEntity::getHiBitNumberUId(
                                                     BITFIELDID_SIZE - 1)));
 
            auto sub_inv_mat =
                matrix_range(invMat, get_range(rbi), get_range(cbi));
            bM.resize(sub_inv_mat.size1(), sub_inv_mat.size2());
            noalias(bM) = sub_inv_mat;
 
            for (auto a_lo = a_lo_tmp; a_lo != a_hi; ++a_lo) {
#ifndef NDEBUG
              if (block_indexing.find((*a_lo)->uidRow) != block_indexing.end())
                SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
                        "Wrong a_lo->uidRow");
#endif
 
              auto &a = (*a_lo)->getMat();
              abM.resize(a.size1(), bM.size2(), false);
              abM.clear();
              cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
 
                          a.size1(), bM.size2(), a.size2(), 1.,
 
                          &*a.data().begin(), a.size2(),
 
                          &*bM.data().begin(), bM.size2(), 0.,
 
                          &*abM.data().begin(), abM.size2());
 
              for (auto c_lo = c_lo_tmp; c_lo != c_hi; ++c_lo) {
#ifndef NDEBUG
                if (block_indexing.find((*c_lo)->uidCol) !=
                    block_indexing.end())
                  SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
                          "Wrong a_lo->uidRow");
#endif
 
                auto &c = (*c_lo)->getMat();
 
                abcM.resize(abM.size1(), c.size2(), false);
                abcM.clear();
 
                cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
                            abM.size1(), c.size2(), abM.size2(), -1.,
 
                            &*abM.data().begin(), abM.size2(),
 
                            &*c.data().begin(), c.size2(), 0.,
 
                            &*abcM.data().begin(), abcM.size2());
 
                CHKERR assemble_schur(abcM, (*a_lo)->uidRow, (*c_lo)->uidCol,
                                      &((*a_lo)->getRowInd()),
                                      &((*c_lo)->getColInd()));
 
                if (symSchur && rp_uid_it != cp_uid_it) {
                  abcM = trans(abcM);
                  CHKERR assemble_schur(abcM, (*c_lo)->uidCol, (*a_lo)->uidRow,
                                        &((*c_lo)->getColInd()),
                                        &((*a_lo)->getRowInd()));
                }
              }
            }
          }
        }
      }
    }
 
    MoFEMFunctionReturn(0);
  };
 
  // Assemble Schur complements
  CHKERR assemble_dense_blocks();
 
#ifndef NDEBUG
  if constexpr (debug_schur)
    MOFEM_LOG("SELF", Sev::noisy) << "Schur assemble done";
#endif
 
#ifndef NDEBUG
  PetscLogEventEnd(SchurEvents::MOFEM_EVENT_opSchurAssembleEnd, 0, 0, 0, 0);
#endif
 
  MoFEMFunctionReturn(0);
}
 
struct SchurDSYSV {
 
  static auto invertMat(MatrixDouble &m, MatrixDouble &inv) {
    MoFEMFunctionBegin;
 
    const auto nb = m.size1();
#ifndef NDEBUG
    if (nb != m.size2()) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "It should be square matrix %zu != %zu", nb, m.size2());
    }
#endif
 
    inv.resize(nb, nb, false);
    inv.swap(m);
    m.resize(2 * nb, 2 * nb, false);
 
    VectorInt ipiv(nb);
    int info;
 
    // FIXME: add lapack_dsytrf and lapack_dsytri
 
    info = lapack_dgetrf(nb, nb, &*inv.data().begin(), nb, &*ipiv.begin());
    if (info)
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "lapack error info = %d", info);
    info = lapack_dgetri(nb, &*inv.data().begin(), nb, &*ipiv.begin(),
                         &*m.data().begin(), m.data().size());
    if (info)
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "lapack error info = %d", info);
 
    MoFEMFunctionReturn(0);
  }
};
 
struct SchurDGESV {
 
  static auto invertMat(MatrixDouble &m, MatrixDouble &inv) {
    MoFEMFunctionBeginHot;
 
    const auto nb = m.size1();
#ifndef NDEBUG
    if (nb != m.size2()) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "It should be square matrix %zu != %zu", nb, m.size2());
    }
#endif
 
    inv.resize(nb, nb, false);
    inv.swap(m);
 
    VectorInt ipiv(nb);
    int info;
 
    info = lapack_dgetrf(nb, nb, &*inv.data().begin(), nb, &*ipiv.begin());
    if (info)
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "lapack error info = %d", info);
    info = lapack_dgetri(nb, &*inv.data().begin(), nb, &*ipiv.begin(),
                         &*m.data().begin(), m.data().size());
    if (info)
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "lapack error info = %d", info);
 
    MoFEMFunctionReturnHot(0);
  }
};
 
MoFEMErrorCode
OpSchurAssembleEnd<SchurDSYSV>::doWork(int side, EntityType type,
                                       EntitiesFieldData::EntData &data) {
  return doWorkImpl<SchurDSYSV>(side, type, data);
}
 
MoFEMErrorCode
OpSchurAssembleEnd<SchurDGESV>::doWork(int side, EntityType type,
                                       EntitiesFieldData::EntData &data) {
  return doWorkImpl<SchurDGESV>(side, type, data);
}
 
/**
 * @brief Apply in place LU factors of element matrix
 *
 * Element matrix A is stored row-major, i.e. read column-major by LAPACK it is
 * M = A^T = P L U. For trans == false y = A x = U^T L^T P^T x, for trans ==
 * true y = A^T x = P L U x. Vector t is overwritten by result.
 */
static inline void apply_lu_factors(int n, const double *lu,
                                    const __CLPK_integer *ipiv, double *t,
                                    bool trans) {
  if (!n)
    return;
  if (!trans) {
    for (int k = 0; k != n; ++k) {
      const int p = ipiv[k] - 1;
      if (p != k)
        std::swap(t[k], t[p]);
    }
    cblas_dtrmv(CblasColMajor, CblasLower, CblasTrans, CblasUnit, n, lu, n, t,
                1);
    cblas_dtrmv(CblasColMajor, CblasUpper, CblasTrans, CblasNonUnit, n, lu, n,
                t, 1);
  } else {
    cblas_dtrmv(CblasColMajor, CblasUpper, CblasNoTrans, CblasNonUnit, n, lu, n,
                t, 1);
    cblas_dtrmv(CblasColMajor, CblasLower, CblasNoTrans, CblasUnit, n, lu, n, t,
                1);
    for (int k = n - 1; k >= 0; --k) {
      const int p = ipiv[k] - 1;
      if (p != k)
        std::swap(t[k], t[p]);
    }
  }
}
 
/// copy row-major block into row-major matrix at (ridx, cidx)
static inline void copy_block(double *dst, int ld_dst, int ridx, int cidx,
                              const double *src, int ld_src, int nb_rows,
                              int nb_cols) {
  for (int i = 0; i != nb_rows; ++i)
    cblas_dcopy(nb_cols, src + static_cast<size_t>(i) * ld_src, 1,
                dst + static_cast<size_t>(ridx + i) * ld_dst + cidx, 1);
}
 
static inline bool is_zero_matrix(const double *ptr, size_t size) {
  return std::all_of(ptr, ptr + size, [](double v) { return v == 0.; });
}
 
extern "C" {
void dgecon_(char *norm, __CLPK_integer *n, __CLPK_doublereal *a,
             __CLPK_integer *lda, __CLPK_doublereal *anorm,
             __CLPK_doublereal *rcond, __CLPK_doublereal *work,
             __CLPK_integer *iwork, __CLPK_integer *info);
}
 
/// 1-norm of column-major m x n block, infinity if block is not finite
static inline double norm1_block(int m, int n, const double *a, int ld) {
  double r = 0;
  for (int j = 0; j != n; ++j) {
    // note that std::max ignores NaN as second argument
    const double s = cblas_dasum(m, a + static_cast<size_t>(j) * ld, 1);
    if (!std::isfinite(s))
      return std::numeric_limits<double>::infinity();
    r = std::max(r, s);
  }
  return r;
}
 
/// y += op(A) x, op(A) is m x kk, A is column-major
static inline void add_gemv(bool trans_a, int m, int kk, const double *a,
                            int lda, const double *x, double *y) {
  if (!m || !kk)
    return;
  cblas_dgemv(CblasColMajor, trans_a ? CblasTrans : CblasNoTrans,
              trans_a ? kk : m, trans_a ? m : kk, 1., a, lda, x, 1, 1., y, 1);
}
 
/// C -= op(A) B, op(A) is m x kk, A, B and C are column-major
static inline void sub_gemm(bool trans_a, int m, int kk, int nrhs,
                            const double *a, int lda, const double *b, int ldb,
                            double *c, int ldc) {
  if (!m || !kk || !nrhs)
    return;
  if (nrhs == 1)
    cblas_dgemv(CblasColMajor, trans_a ? CblasTrans : CblasNoTrans,
                trans_a ? kk : m, trans_a ? m : kk, -1., a, lda, b, 1, 1., c,
                1);
  else
    cblas_dgemm(CblasColMajor, trans_a ? CblasTrans : CblasNoTrans,
                CblasNoTrans, m, nrhs, kk, -1., a, lda, b, ldb, 1., c, ldc);
}
 
/**
 * @brief Factorise diagonal block in place and test it as block pivot
 *
 * Block is admissible pivot if it is not singular and its reciprocal condition
 * number (1-norm estimate) is not below rcond_min. If u > 0, its smallest
 * singular value (estimated as rcond * norm) has also to be not below u times
 * the largest norm of its couplings with nodes not yet eliminated (block
 * threshold pivoting). That test is off by default, since it rejects mass
 * blocks of hierarchical bases, which are ill-conditioned but safe pivots.
 * Stability is verified by backward error, see elem_backward_error.
 */
static bool factorise_pivot_block(int n, double *d, int ld, __CLPK_integer *piv,
                                  double coupling, double u, double rcond_min,
                                  std::vector<double> &work,
                                  std::vector<__CLPK_integer> &iwork,
                                  double *rcond_out = nullptr,
                                  double *anorm_out = nullptr) {
  const double anorm = norm1_block(n, n, d, ld);
  double rcond = 0;
  if (rcond_out)
    *rcond_out = rcond;
  if (anorm_out)
    *anorm_out = anorm;
  if (anorm == 0 || !std::isfinite(anorm))
    return false;
  if (lapack_dgetrf(n, n, d, ld, piv) != 0)
    return false;
  work.resize(4 * n);
  iwork.resize(n);
  char norm = '1';
  __CLPK_integer nn = n, lda = ld, info = 0;
  double an = anorm;
  dgecon_(&norm, &nn, d, &lda, &an, &rcond, work.data(), iwork.data(), &info);
  if (rcond_out)
    *rcond_out = rcond;
  if (info != 0 || !std::isfinite(rcond))
    return false;
  if (rcond < rcond_min)
    return false;
  if (u > 0 && rcond * anorm < u * coupling)
    return false;
  return true;
}
 
using BlockPattern = BlockStorage::Pattern;
 
/// offsets of nodes in element vector in order of nodes
static std::vector<int> natural_offsets(const BlockPattern &p) {
  std::vector<int> off(p.nbNodes);
  int o = 0;
  for (int r = 0; r != p.nbNodes; ++r) {
    off[r] = o;
    o += p.nodeSize[r];
  }
  return off;
}
 
/// doubles in listed blocks of one element
static size_t listed_size(const BlockPattern &p) {
  size_t s = 0;
  for (int r = 0; r != p.nbNodes; ++r)
    for (int c = 0; c != p.nbNodes; ++c)
      if (p.isListed(r, c))
        s += static_cast<size_t>(p.nodeSize[r]) * p.nodeSize[c];
  return s;
}
 
/// layout with listed blocks only
static void make_listed_layout(BlockPattern &p) {
  const int k = p.nbNodes;
  p.state = BlockPattern::LISTED;
  p.order.clear();
  p.core.clear();
  p.nodeOffset = natural_offsets(p);
  p.slotShift.assign(k * k, -1);
  p.slotLd.assign(k * k, 0);
  size_t shift = 0;
  for (int r = 0; r != k; ++r)
    for (int c = 0; c != k; ++c)
      if (p.isListed(r, c)) {
        p.slotShift[r * k + c] = shift;
        p.slotLd[r * k + c] = p.nodeSize[c];
        shift += static_cast<size_t>(p.nodeSize[r]) * p.nodeSize[c];
      }
  p.coreShift = shift;
  p.coreSize = 0;
  p.coreOffset = p.size;
  p.elemSize = shift;
}
 
/**
 * @brief Blocks of M = A^T present after eliminating nodes in given order
 *
 * @return nbNodes x nbNodes flags, (a, b) is block a, b of M
 */
static std::vector<char> symbolic_elimination(const BlockPattern &p,
                                              const std::vector<int> &order) {
  const int k = p.nbNodes;
  std::vector<char> pm(k * k, 0);
  for (int r = 0; r != k; ++r)
    for (int c = 0; c != k; ++c)
      pm[c * k + r] = p.isListed(r, c);
  std::vector<char> done(k, 0);
  for (auto e : order) {
    done[e] = 1;
    for (int i = 0; i != k; ++i)
      if (!done[i] && pm[i * k + e])
        for (int j = 0; j != k; ++j)
          if (!done[j] && pm[e * k + j])
            pm[i * k + j] = 1;
  }
  return pm;
}
 
/**
 * @brief Layout for block elimination of nodes in order, followed by dense
 * core of remaining nodes
 *
 * Element vector is ordered as eliminated nodes in order, then core nodes.
 * Slots with at least one eliminated node, listed or fill, are contiguous with
 * leading dimension equal to number of columns. Slots with both nodes in core
 * are parts of dense core, which is (core size)^2 matrix. With empty order
 * the element matrix is dense.
 */
static void make_block_layout(BlockPattern &p, const std::vector<int> &order) {
  const int k = p.nbNodes;
  auto pm = symbolic_elimination(p, order);
  std::vector<char> in_core(k, 1);
  for (auto e : order)
    in_core[e] = 0;
  p.order = order;
  p.core.clear();
  for (int i = 0; i != k; ++i)
    if (in_core[i])
      p.core.push_back(i);
 
  p.nodeOffset.assign(k, 0);
  std::vector<int> pos(k);
  int off = 0, t = 0;
  for (auto e : p.order) {
    p.nodeOffset[e] = off;
    off += p.nodeSize[e];
    pos[e] = t++;
  }
  p.coreOffset = off;
  for (auto c : p.core) {
    p.nodeOffset[c] = off;
    off += p.nodeSize[c];
    pos[c] = t++;
  }
  p.coreSize = off - p.coreOffset;
 
  // slots with eliminated node, ordered by elimination
  std::vector<std::array<int, 4>> slots;
  for (int r = 0; r != k; ++r)
    for (int c = 0; c != k; ++c)
      if (!(in_core[r] && in_core[c]) && pm[c * k + r])
        slots.push_back({std::min(pos[r], pos[c]), pos[r], pos[c], r * k + c});
  std::sort(slots.begin(), slots.end());
 
  p.slotShift.assign(k * k, -1);
  p.slotLd.assign(k * k, 0);
  size_t shift = 0;
  for (auto &s : slots) {
    const int r = s[3] / k, c = s[3] % k;
    p.slotShift[s[3]] = shift;
    p.slotLd[s[3]] = p.nodeSize[c];
    shift += static_cast<size_t>(p.nodeSize[r]) * p.nodeSize[c];
  }
  p.coreShift = shift;
  for (auto r : p.core)
    for (auto c : p.core) {
      p.slotShift[r * k + c] =
          p.coreShift +
          static_cast<size_t>(p.nodeOffset[r] - p.coreOffset) * p.coreSize +
          (p.nodeOffset[c] - p.coreOffset);
      p.slotLd[r * k + c] = p.coreSize;
    }
  p.elemSize = p.coreShift + static_cast<size_t>(p.coreSize) * p.coreSize;
  p.state = order.empty() ? BlockPattern::DENSE : BlockPattern::BLOCK;
}
 
/// elimination order by structure only (minimal fill, diagonal present)
static std::vector<int> structural_order(const BlockPattern &p) {
  const int k = p.nbNodes;
  auto pm = symbolic_elimination(p, {});
  std::vector<char> rem(k, 1);
  std::vector<int> order;
  while (true) {
    int best = -1;
    double best_fill = 0;
    for (int e = 0; e != k; ++e) {
      if (!rem[e] || !pm[e * k + e])
        continue;
      double fill = 0;
      for (int i = 0; i != k; ++i)
        if (rem[i] && i != e && pm[i * k + e])
          for (int j = 0; j != k; ++j)
            if (rem[j] && j != e && pm[e * k + j] && !pm[i * k + j])
              fill += static_cast<double>(p.nodeSize[i]) * p.nodeSize[j];
      if (best < 0 || fill < best_fill) {
        best = e;
        best_fill = fill;
      }
    }
    if (best < 0)
      break;
    for (int i = 0; i != k; ++i)
      if (rem[i] && i != best && pm[i * k + best])
        for (int j = 0; j != k; ++j)
          if (rem[j] && j != best && pm[best * k + j])
            pm[i * k + j] = 1;
    rem[best] = 0;
    order.push_back(best);
  }
  return order;
}
 
/**
 * @brief Decide elimination order on probe element
 *
 * Greedy block elimination: among nodes whose updated diagonal block passes
 * the pivot test (see factorise_pivot_block), the node creating the least
 * fill (weighted by block sizes) is eliminated next. Nodes never admissible
 * form the dense core.
 *
 * @param w dense element matrix, row-major A (column-major M), in natural
 * node offsets, overwritten
 */
static std::vector<int> analyse_pattern(const BlockPattern &p, double *w,
                                        double u, double rcond_min) {
  const int k = p.nbNodes, n = p.size;
  auto off = natural_offsets(p);
  auto &ns = p.nodeSize;
  // block (a, b) of M in w, leading dimension n
  auto mb = [&](int a, int b) {
    return w + static_cast<size_t>(off[b]) * n + off[a];
  };
  auto pm = symbolic_elimination(p, {});
  std::vector<char> rem(k, 1);
  std::vector<int> order;
  std::vector<double> tmp, work;
  std::vector<__CLPK_integer> piv, iwork;
 
  while (true) {
    int best = -1;
    double best_fill = 0;
    for (int e = 0; e != k; ++e) {
      if (!rem[e] || !pm[e * k + e])
        continue;
      const int ne = ns[e];
      double coupling = 0;
      for (int j = 0; j != k; ++j) {
        if (!rem[j] || j == e)
          continue;
        if (pm[e * k + j])
          coupling = std::max(coupling, norm1_block(ne, ns[j], mb(e, j), n));
        if (pm[j * k + e])
          coupling = std::max(coupling, norm1_block(ns[j], ne, mb(j, e), n));
      }
      tmp.resize(static_cast<size_t>(ne) * ne);
      for (int c = 0; c != ne; ++c)
        cblas_dcopy(ne, mb(e, e) + static_cast<size_t>(c) * n, 1,
                    tmp.data() + static_cast<size_t>(c) * ne, 1);
      piv.resize(ne);
      double rcond, anorm;
      const bool admissible =
          factorise_pivot_block(ne, tmp.data(), ne, piv.data(), coupling, u,
                                rcond_min, work, iwork, &rcond, &anorm);
      MOFEM_TAG_AND_LOG("SELF", Sev::noisy, "BlockMat")
          << "A00 analysis step " << order.size() << " node " << p.nodeName[e]
          << "(" << ne << ") rcond " << rcond << " smin " << rcond * anorm
          << " norm " << anorm << " coupling " << coupling
          << (admissible ? " admissible" : " rejected");
      if (!admissible)
        continue;
      double fill = 0;
      for (int i = 0; i != k; ++i)
        if (rem[i] && i != e && pm[i * k + e])
          for (int j = 0; j != k; ++j)
            if (rem[j] && j != e && pm[e * k + j] && !pm[i * k + j])
              fill += static_cast<double>(ns[i]) * ns[j];
      if (best < 0 || fill < best_fill) {
        best = e;
        best_fill = fill;
      }
    }
    if (best < 0)
      break;
 
    const int e = best, ne = ns[e];
    piv.resize(ne);
    lapack_dgetrf(ne, ne, mb(e, e), n, piv.data());
    for (int j = 0; j != k; ++j)
      if (rem[j] && j != e && pm[e * k + j])
        lapack_dgetrs('N', ne, ns[j], mb(e, e), n, piv.data(), mb(e, j), n);
    for (int i = 0; i != k; ++i)
      if (rem[i] && i != e && pm[i * k + e])
        for (int j = 0; j != k; ++j)
          if (rem[j] && j != e && pm[e * k + j]) {
            cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, ns[i], ns[j],
                        ne, -1., mb(i, e), n, mb(e, j), n, 1., mb(i, j), n);
            pm[i * k + j] = 1;
          }
    rem[e] = 0;
    order.push_back(e);
  }
  return order;
}
 
/// copy listed blocks of element buffer from one layout to another
static void copy_listed(const BlockPattern &from, const double *src,
                        const BlockPattern &to, double *dst) {
  for (int r = 0; r != from.nbNodes; ++r)
    for (int c = 0; c != from.nbNodes; ++c)
      if (from.isListed(r, c))
        copy_block(dst + to.sShift(r, c), to.sLd(r, c), 0, 0,
                   src + from.sShift(r, c), from.sLd(r, c), from.nodeSize[r],
                   from.nodeSize[c]);
}
 
/// dense element matrix (row-major A, size x size) from listed blocks
static void elem_gather_dense(const BlockPattern &p, const double *buf,
                              double *dense) {
  const int n = p.size;
  for (int r = 0; r != p.nbNodes; ++r)
    for (int c = 0; c != p.nbNodes; ++c)
      if (p.isListed(r, c))
        copy_block(dense, n, p.nodeOffset[r], p.nodeOffset[c],
                   buf + p.sShift(r, c), p.sLd(r, c), p.nodeSize[r],
                   p.nodeSize[c]);
}
 
/// y += A x (trans false) or y += A^T x (trans true) from listed blocks
static void elem_mult_listed(const BlockPattern &p, const double *buf,
                             const double *x, double *y, bool trans) {
  for (int r = 0; r != p.nbNodes; ++r)
    for (int c = 0; c != p.nbNodes; ++c)
      if (p.isListed(r, c)) {
        const int nr = p.nodeSize[r], nc = p.nodeSize[c];
        const double *a = buf + p.sShift(r, c);
        if (!trans)
          cblas_dgemv(CblasRowMajor, CblasNoTrans, nr, nc, 1., a, p.sLd(r, c),
                      x + p.nodeOffset[c], 1, 1., y + p.nodeOffset[r], 1);
        else
          cblas_dgemv(CblasRowMajor, CblasTrans, nr, nc, 1., a, p.sLd(r, c),
                      x + p.nodeOffset[r], 1, 1., y + p.nodeOffset[c], 1);
      }
}
 
/**
 * @brief Factorise element buffer in place
 *
 * Block LU of M = A^T: for each node e in order, D_e = M_ee is factorised and
 * tested as pivot, rows M_ej are overwritten by D_e^{-1} M_ej, and M_ij -=
 * M_ie D_e^{-1} M_ej for remaining nodes i, j. Columns M_ie are kept. Then
 * dense core is factorised with partial pivoting. M = L U, L block lower with
 * diagonal D, U block upper with unit diagonal.
 *
 * @return 0 on success, 1 if pivot test failed, 2 if core is singular
 */
static int elem_factorise(const BlockPattern &p, double *buf,
                          __CLPK_integer *piv, double u, double rcond_min,
                          std::vector<double> &work,
                          std::vector<__CLPK_integer> &iwork) {
  const int m = p.order.size();
  std::vector<int> seq(p.order);
  seq.insert(seq.end(), p.core.begin(), p.core.end());
  auto &ns = p.nodeSize;
 
  for (int t = 0; t != m; ++t) {
    const int e = seq[t], ne = ns[e];
    double *d = buf + p.mShift(e, e);
    const int ldd = p.mLd(e, e);
 
    double coupling = 0;
    for (int q = t + 1; u > 0 && q < static_cast<int>(seq.size()); ++q) {
      const int j = seq[q];
      if (p.mShift(e, j) >= 0)
        coupling =
            std::max(coupling,
                     norm1_block(ne, ns[j], buf + p.mShift(e, j), p.mLd(e, j)));
      if (p.mShift(j, e) >= 0)
        coupling =
            std::max(coupling,
                     norm1_block(ns[j], ne, buf + p.mShift(j, e), p.mLd(j, e)));
    }
    if (!factorise_pivot_block(ne, d, ldd, piv + p.nodeOffset[e], coupling, u,
                               rcond_min, work, iwork))
      return 1;
 
    for (int q = t + 1; q < static_cast<int>(seq.size()); ++q) {
      const int j = seq[q];
      if (p.mShift(e, j) >= 0)
        lapack_dgetrs('N', ne, ns[j], d, ldd, piv + p.nodeOffset[e],
                      buf + p.mShift(e, j), p.mLd(e, j));
    }
    for (int qi = t + 1; qi < static_cast<int>(seq.size()); ++qi) {
      const int i = seq[qi];
      if (p.mShift(i, e) < 0)
        continue;
      for (int qj = t + 1; qj < static_cast<int>(seq.size()); ++qj) {
        const int j = seq[qj];
        if (p.mShift(e, j) < 0)
          continue;
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, ns[i], ns[j], ne,
                    -1., buf + p.mShift(i, e), p.mLd(i, e),
                    buf + p.mShift(e, j), p.mLd(e, j), 1., buf + p.mShift(i, j),
                    p.mLd(i, j));
      }
    }
  }
 
  if (p.coreSize) {
    if (lapack_dgetrf(p.coreSize, p.coreSize, buf + p.coreShift, p.coreSize,
                      piv + p.coreOffset) != 0)
      return 2;
  }
  return 0;
}
 
/**
 * @brief Solve with factorised element buffer
 *
 * trans false solves A X = R, i.e. M^T X = R = U^T L^T X, trans true solves
 * A^T X = R, i.e. M X = R = L U X. R is column-major (size x nrhs), with rows
 * in element vector layout, overwritten by X.
 *
 * @param nz_in optional flags of nodes with non-zero rows of R, nodes with
 * zero rows are skipped where possible
 */
static MoFEMErrorCode elem_solve(const BlockPattern &p, const double *buf,
                                 const __CLPK_integer *piv, double *r, int ldr,
                                 int nrhs, bool trans,
                                 const std::vector<char> *nz_in) {
  MoFEMFunctionBeginHot;
  const int k = p.nbNodes, m = p.order.size();
  std::vector<int> seq(p.order);
  seq.insert(seq.end(), p.core.begin(), p.core.end());
  auto &ns = p.nodeSize;
  auto &off = p.nodeOffset;
  std::vector<char> nz = nz_in ? *nz_in : std::vector<char>(k, 1);
  auto blk = [&](int a, int b) { return buf + p.mShift(a, b); };
  auto rr = [&](int a) { return r + off[a]; };
  auto lu = [&](int e) { return const_cast<double *>(blk(e, e)); };
  auto pv = [&](int e) { return const_cast<__CLPK_integer *>(piv + off[e]); };
  auto core_solve = [&](char t) {
    MoFEMFunctionBeginHot;
    if (!p.coreSize)
      MoFEMFunctionReturnHot(0);
    bool any = false;
    for (auto c : p.core)
      any = any || nz[c];
    if (!any)
      MoFEMFunctionReturnHot(0);
    auto info = lapack_dgetrs(
        t, p.coreSize, nrhs, const_cast<double *>(buf + p.coreShift),
        p.coreSize, const_cast<__CLPK_integer *>(piv + p.coreOffset),
        r + p.coreOffset, ldr);
    if (info)
      SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
              "error lapack dgetrs info = %d", static_cast<int>(info));
    for (auto c : p.core)
      nz[c] = 1;
    MoFEMFunctionReturnHot(0);
  };
  auto diag_solve = [&](int e, char t) {
    MoFEMFunctionBeginHot;
    if (!nz[e])
      MoFEMFunctionReturnHot(0);
    auto info =
        lapack_dgetrs(t, ns[e], nrhs, lu(e), p.mLd(e, e), pv(e), rr(e), ldr);
    if (info)
      SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
              "error lapack dgetrs info = %d", static_cast<int>(info));
    MoFEMFunctionReturnHot(0);
  };
 
  if (!trans) {
    // forward with U^T (unit diagonal)
    for (int t = 0; t != static_cast<int>(seq.size()); ++t) {
      const int e = seq[t];
      for (int q = 0; q != std::min(t, m); ++q) {
        const int j = seq[q];
        if (nz[j] && p.mShift(j, e) >= 0) {
          sub_gemm(true, ns[e], ns[j], nrhs, blk(j, e), p.mLd(j, e), rr(j), ldr,
                   rr(e), ldr);
          nz[e] = 1;
        }
      }
    }
    // backward with L^T
    CHKERR core_solve('T');
    for (int t = m - 1; t >= 0; --t) {
      const int e = seq[t];
      for (int q = t + 1; q != static_cast<int>(seq.size()); ++q) {
        const int i = seq[q];
        if (nz[i] && p.mShift(i, e) >= 0) {
          sub_gemm(true, ns[e], ns[i], nrhs, blk(i, e), p.mLd(i, e), rr(i), ldr,
                   rr(e), ldr);
          nz[e] = 1;
        }
      }
      CHKERR diag_solve(e, 'T');
    }
  } else {
    // forward with L
    for (int t = 0; t != static_cast<int>(seq.size()); ++t) {
      const int e = seq[t];
      for (int q = 0; q != std::min(t, m); ++q) {
        const int i = seq[q];
        if (nz[i] && p.mShift(e, i) >= 0) {
          sub_gemm(false, ns[e], ns[i], nrhs, blk(e, i), p.mLd(e, i), rr(i),
                   ldr, rr(e), ldr);
          nz[e] = 1;
        }
      }
      if (t < m)
        CHKERR diag_solve(e, 'N');
    }
    CHKERR core_solve('N');
    // backward with U (unit diagonal)
    for (int t = m - 1; t >= 0; --t) {
      const int e = seq[t];
      for (int q = t + 1; q != static_cast<int>(seq.size()); ++q) {
        const int j = seq[q];
        if (nz[j] && p.mShift(e, j) >= 0) {
          sub_gemm(false, ns[e], ns[j], nrhs, blk(e, j), p.mLd(e, j), rr(j),
                   ldr, rr(e), ldr);
          nz[e] = 1;
        }
      }
    }
  }
  MoFEMFunctionReturnHot(0);
}
 
/**
 * @brief Product with factorised element buffer, in place
 *
 * trans false computes A v = M^T v = U^T (L^T v), trans true computes A^T v =
 * M v = L (U v). Every stored block is used once.
 */
static void elem_mult_factors(const BlockPattern &p, const double *buf,
                              const __CLPK_integer *piv, double *v,
                              bool trans) {
  const int m = p.order.size();
  std::vector<int> seq(p.order);
  seq.insert(seq.end(), p.core.begin(), p.core.end());
  const int nseq = seq.size();
  auto &ns = p.nodeSize;
  auto &off = p.nodeOffset;
  auto blk = [&](int a, int b) { return buf + p.mShift(a, b); };
  auto vv = [&](int a) { return v + off[a]; };
 
  if (!trans) {
    // t = L^T v
    for (int t = 0; t != m; ++t) {
      const int e = seq[t];
      apply_lu_factors(ns[e], blk(e, e), piv + off[e], vv(e), false);
      for (int q = t + 1; q != nseq; ++q) {
        const int i = seq[q];
        if (p.mShift(i, e) >= 0)
          add_gemv(true, ns[e], ns[i], blk(i, e), p.mLd(i, e), vv(i), vv(e));
      }
    }
    apply_lu_factors(p.coreSize, buf + p.coreShift, piv + p.coreOffset,
                     v + p.coreOffset, false);
    // y = U^T t
    for (int t = nseq - 1; t >= 0; --t) {
      const int e = seq[t];
      for (int q = 0; q != std::min(t, m); ++q) {
        const int j = seq[q];
        if (p.mShift(j, e) >= 0)
          add_gemv(true, ns[e], ns[j], blk(j, e), p.mLd(j, e), vv(j), vv(e));
      }
    }
  } else {
    // t = U v
    for (int t = 0; t != m; ++t) {
      const int e = seq[t];
      for (int q = t + 1; q != nseq; ++q) {
        const int j = seq[q];
        if (p.mShift(e, j) >= 0)
          add_gemv(false, ns[e], ns[j], blk(e, j), p.mLd(e, j), vv(j), vv(e));
      }
    }
    // y = L t
    apply_lu_factors(p.coreSize, buf + p.coreShift, piv + p.coreOffset,
                     v + p.coreOffset, true);
    for (int t = nseq - 1; t >= 0; --t) {
      const int e = seq[t];
      if (t < m)
        apply_lu_factors(ns[e], blk(e, e), piv + off[e], vv(e), true);
      for (int q = 0; q != std::min(t, m); ++q) {
        const int i = seq[q];
        if (p.mShift(e, i) >= 0)
          add_gemv(false, ns[e], ns[i], blk(e, i), p.mLd(e, i), vv(i), vv(e));
      }
    }
  }
}
 
/// factor of dense backward error accepted for block factors
constexpr double backward_error_factor = 100;
 
/// right-hand side used to measure backward error
static inline double backward_error_rhs(int i) {
  return 1. + 0.5 * std::sin(1. + i);
}
 
/**
 * @brief Normwise backward error of solve with factors of element
 *
 * ||A x - b||_inf / (||A||_inf ||x||_inf + ||b||_inf), where x is obtained
 * with factors lu, and A x is computed from listed blocks of orig (matrix
 * before factorisation, in the same layout). Cost is a few passes over element
 * buffer.
 */
static double elem_backward_error(const BlockPattern &p, const double *orig,
                                  const double *lu, const __CLPK_integer *piv,
                                  std::vector<double> &x,
                                  std::vector<double> &r,
                                  std::vector<double> &rs) {
  const int n = p.size;
  x.resize(n);
  r.resize(n);
  rs.assign(n, 0.);
  double b_norm = 0;
  for (int i = 0; i != n; ++i) {
    x[i] = backward_error_rhs(i);
    r[i] = -x[i];
    b_norm = std::max(b_norm, std::abs(x[i]));
  }
  constexpr double inf = std::numeric_limits<double>::infinity();
  if (elem_solve(p, lu, piv, x.data(), n, 1, false, nullptr))
    return inf;
  // note that std::max ignores NaN as second argument, thus non-finite values
  // are rejected explicitly
  for (int i = 0; i != n; ++i)
    if (!std::isfinite(x[i]))
      return inf;
  elem_mult_listed(p, orig, x.data(), r.data(), false);
  // row sums of |A|
  for (int a = 0; a != p.nbNodes; ++a)
    for (int c = 0; c != p.nbNodes; ++c)
      if (p.isListed(a, c)) {
        const double *blk = orig + p.sShift(a, c);
        for (int i = 0; i != p.nodeSize[a]; ++i)
          rs[p.nodeOffset[a] + i] += cblas_dasum(
              p.nodeSize[c], blk + static_cast<size_t>(i) * p.sLd(a, c), 1);
      }
  double r_norm = 0, x_norm = 0, a_norm = 0;
  for (int i = 0; i != n; ++i) {
    if (!std::isfinite(r[i]) || !std::isfinite(rs[i]))
      return inf;
    r_norm = std::max(r_norm, std::abs(r[i]));
    x_norm = std::max(x_norm, std::abs(x[i]));
    a_norm = std::max(a_norm, rs[i]);
  }
  const double den = a_norm * x_norm + b_norm;
  if (!std::isfinite(den) || !(den > 0))
    return inf;
  const double be = r_norm / den;
  return std::isfinite(be) ? be : inf;
}
 
/// backward error accepted for block factors, given dense backward error
static inline double accepted_backward_error(double be_dense, double be_tol) {
  return std::isfinite(be_dense)
             ? std::max(backward_error_factor * be_dense, be_tol)
             : be_tol;
}
 
/// true if backward error is finite and not larger than accepted
static inline bool backward_error_accepted(double be, double be_dense,
                                           double be_tol) {
  return std::isfinite(be) && be <= accepted_backward_error(be_dense, be_tol);
}
 
/**
 * @brief Verify block layout on probe element
 *
 * Normwise backward errors of a solve with block factors and with dense
 * factors are compared. Block layout is accepted if its backward error is
 * finite and not larger than backward_error_factor times dense backward error
 * (if that is finite), or than be_tol.
 *
 * @param mat matrix of probe in layout nl (not factorised)
 */
static bool verify_probe(const BlockPattern &nl, const std::vector<double> &mat,
                         double u, double rcond_min, double be_tol,
                         double &be_block, double &be_dense) {
  const int n = nl.size;
  std::vector<double> x, r, rs, f, work;
  std::vector<__CLPK_integer> piv(n), iwork;
 
  // dense factors of probe, as element with dense layout
  BlockPattern dl = nl;
  make_block_layout(dl, {});
  std::vector<double> dense(dl.elemSize, 0.);
  copy_listed(nl, mat.data(), dl, dense.data());
  f = dense;
  be_dense = std::numeric_limits<double>::infinity();
  if (elem_factorise(dl, f.data(), piv.data(), u, rcond_min, work, iwork) == 0)
    be_dense =
        elem_backward_error(dl, dense.data(), f.data(), piv.data(), x, r, rs);
 
  be_block = std::numeric_limits<double>::infinity();
  f = mat;
  if (elem_factorise(nl, f.data(), piv.data(), u, rcond_min, work, iwork))
    return false;
  be_block =
      elem_backward_error(nl, mat.data(), f.data(), piv.data(), x, r, rs);
  return backward_error_accepted(be_block, be_dense, be_tol);
}
 
static std::string describe_nodes(const BlockPattern &p,
                                  const std::vector<int> &nodes) {
  std::string s;
  for (auto r : nodes) {
    if (!s.empty())
      s += " ";
    s += p.nodeName[r] + "(" + std::to_string(p.nodeSize[r]) + ")";
  }
  return s.empty() ? std::string("none") : s;
}
 
/**
 * @brief Relayout element buffers of pattern pi, element by element, in place
 *
 * Element buffers have capacity of dense element matrix (reserved, pages not
 * used are not touched), which is upper bound of any layout. Buffers are
 * relaid within their capacity through a copy of one element, so nothing is
 * reallocated or freed. Freed buffers would be kept resident by allocators.
 */
static void relayout_pattern(BlockStorage &s, int pi, BlockPattern nl) {
  auto &p = s.patterns[pi];
  const int k = p.nbNodes;
  std::vector<double> scratch;
  for (int ei = 0; ei != static_cast<int>(s.elems.size()); ++ei) {
    auto &e = s.elems[ei];
    if (e.pattern != pi)
      continue;
    auto relayout_buf = [&](std::vector<double> &buf) {
      scratch.assign(buf.begin(), buf.end());
      if (nl.elemSize > buf.capacity())
        buf.reserve(static_cast<size_t>(nl.size) * nl.size);
      buf.resize(nl.elemSize);
      std::fill(buf.begin(), buf.end(), 0.);
      copy_listed(p, scratch.data(), nl, buf.data());
    };
    relayout_buf(s.elemData[ei]);
    if (s.hasPreconditioner)
      relayout_buf(s.elemPrec[ei]);
    for (int r = 0; r != k; ++r)
      for (int c = 0; c != k; ++c)
        if (p.isListed(r, c)) {
          auto idx = s.findMutable(s.elemEntities[e.entBegin + r].uid,
                                   s.elemEntities[e.entBegin + c].uid);
          idx->setStorage(nl.sShift(r, c), nl.sLd(r, c), ei);
        }
    for (int r = 0; r != k; ++r)
      s.elemEntities[e.entBegin + r].offset = nl.nodeOffset[r];
  }
  p = nl;
  s.maxElemBuffer = 0;
  for (auto &q : s.patterns)
    s.maxElemBuffer = std::max(s.maxElemBuffer, q.elemSize);
}
 
/**
 * @brief Factorise A00 elements in place
 *
 * Factors are stored in element buffers (operator A00), or in preconditioner
 * buffers (operator A00 + preconditioner). In the second case A00 blocks are
 * intact. Patterns in listed layout are analysed first, see analyse_pattern.
 */
static MoFEMErrorCode factorise_elems(BlockStorage &storage,
                                      BlockStorage::FactorStorage where) {
  MoFEMFunctionBegin;
  CHKERR storage.checkNotFailed();
  if (storage.isFactorised()) {
    if (storage.factorStorage == where)
      MoFEMFunctionReturnHot(0);
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "A00 element matrices are already factorised in other storage");
  }
  const bool in_prec = where == BlockStorage::FACTOR_IN_PRECONDITIONER;
  if (in_prec && !storage.hasPreconditioner)
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "No preconditioner blocks");
 
  const double u = storage.optPivotThreshold;
  const double rcond_min = storage.optRcondMin;
 
  // matrix which is factorised
  auto get_matrix = [&](int ei, std::vector<double> &out) {
    out = storage.elemData[ei];
    if (in_prec)
      cblas_daxpy(out.size(), 1., storage.elemPrec[ei].data(), 1, out.data(),
                  1);
  };
 
  std::vector<double> mat, w;
  for (int pi = 0; pi != static_cast<int>(storage.patterns.size()); ++pi) {
    auto &p = storage.patterns[pi];
    if (p.state != BlockPattern::LISTED)
      continue;
 
    // probe is first element with non-zero matrix, owned if possible
    int probe = -1;
    for (int pass = 0; pass != 2 && probe < 0; ++pass)
      for (int ei = 0; ei != static_cast<int>(storage.elems.size()); ++ei) {
        auto &e = storage.elems[ei];
        if (e.pattern != pi || !e.local || (pass == 0 && !e.owned))
          continue;
        get_matrix(ei, mat);
        if (!is_zero_matrix(mat.data(), mat.size())) {
          probe = ei;
          break;
        }
      }
    if (probe < 0)
      continue;
 
    const int n = p.size;
    w.assign(static_cast<size_t>(n) * n, 0.);
    elem_gather_dense(p, mat.data(), w.data());
 
    BlockPattern nl = p;
    make_block_layout(nl, analyse_pattern(p, w.data(), u, rcond_min));
    double be_block = 0, be_dense = 0;
    bool verified = true;
    if (nl.state == BlockPattern::BLOCK) {
      std::vector<double> probe_mat(nl.elemSize, 0.);
      copy_listed(p, mat.data(), nl, probe_mat.data());
      verified = verify_probe(nl, probe_mat, u, rcond_min,
                              storage.optBackwardError, be_block, be_dense);
      nl.beRef = be_dense;
      if (!verified) {
        MOFEM_TAG_AND_LOG("SELF", Sev::warning, "BlockMat")
            << "A00 pattern " << pi << ": block elimination rejected on probe "
            << "(backward error " << be_block << ", dense " << be_dense
            << "), element matrices are dense";
        make_block_layout(nl, {});
      }
    }
 
    MOFEM_TAG_AND_LOG("WORLD", Sev::verbose, "BlockMat")
        << "A00 pattern " << pi << " (" << p.nbElems
        << " elements): eliminated " << describe_nodes(nl, nl.order)
        << "; core " << describe_nodes(nl, nl.core)
        << "; doubles per element: listed " << listed_size(p) << ", layout "
        << nl.elemSize << ", dense " << static_cast<size_t>(n) * n
        << "; probe backward error " << be_block << " (dense " << be_dense
        << ")";
 
    relayout_pattern(storage, pi, nl);
  }
 
  // from here data is modified, on failure storage is marked as corrupted
  storage.factorStorage = BlockStorage::FACTOR_FAILED;
  for (auto &p : storage.patterns)
    p.nbFallback = 0;
  for (auto &p : storage.patterns)
    p.beMax = 0;
  std::vector<double> scratch, work, be_x, be_r, be_rs;
  std::vector<__CLPK_integer> iwork;
  for (int ei = 0; ei != static_cast<int>(storage.elems.size()); ++ei) {
    auto &e = storage.elems[ei];
    e.factorised = false;
    e.dense = false;
    std::vector<double>().swap(storage.elemDense[ei]);
    if (!e.size || !e.local)
      continue;
    auto &p = storage.patterns[e.pattern];
    auto &buf = in_prec ? storage.elemPrec[ei] : storage.elemData[ei];
    if (in_prec)
      cblas_daxpy(buf.size(), 1., storage.elemData[ei].data(), 1, buf.data(),
                  1);
    // matrices of elements which are not assembled on this process (copies
    // of elements owned by other processes) are zero and are skipped
    if (is_zero_matrix(buf.data(), buf.size())) {
      if (e.owned)
        SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
                "A00 element matrix is zero (not assembled or singular)");
      continue;
    }
    if (p.state == BlockPattern::LISTED)
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "A00 pattern not analysed");
 
    auto piv = storage.pivots.data() + e.pivShift;
    if (p.state == BlockPattern::BLOCK)
      scratch.assign(buf.begin(), buf.end());
    auto ret = elem_factorise(p, buf.data(), piv, u, rcond_min, work, iwork);
    if (!ret && p.state == BlockPattern::BLOCK) {
      // block factors are accepted if solve with them is backward stable
      const double be = elem_backward_error(p, scratch.data(), buf.data(), piv,
                                            be_x, be_r, be_rs);
      p.beMax = std::max(p.beMax, be);
      if (!backward_error_accepted(be, p.beRef, storage.optBackwardError))
        ret = 3;
    }
    if (ret) {
      if (p.state != BlockPattern::BLOCK)
        SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
                "error lapack dgetrf, A00 element matrix is singular");
      // element factorised dense, block buffer is not used
      auto &dense = storage.elemDense[ei];
      dense.assign(static_cast<size_t>(e.size) * e.size, 0.);
      elem_gather_dense(p, scratch.data(), dense.data());
      if (lapack_dgetrf(e.size, e.size, dense.data(), e.size, piv) != 0)
        SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
                "error lapack dgetrf, A00 element matrix is singular");
      e.dense = true;
      ++p.nbFallback;
    }
    e.factorised = true;
  }
  for (int pi = 0; pi != static_cast<int>(storage.patterns.size()); ++pi) {
    auto &p = storage.patterns[pi];
    if (!p.nbFallback)
      continue;
    MOFEM_TAG_AND_LOG("SELF", p.warned ? Sev::verbose : Sev::warning,
                      "BlockMat")
        << "A00 pattern " << pi << ": " << p.nbFallback << " of " << p.nbElems
        << " elements failed block pivot or backward error test and are "
           "factorised dense";
    p.warned = true;
  }
  for (int pi = 0; pi != static_cast<int>(storage.patterns.size()); ++pi) {
    auto &p = storage.patterns[pi];
    if (p.state == BlockPattern::BLOCK) {
      MOFEM_TAG_AND_LOG("SELF", Sev::noisy, "BlockMat")
          << "A00 pattern " << pi << ": largest backward error " << p.beMax
          << " (dense on probe " << p.beRef << "), dense fallback "
          << p.nbFallback;
    }
  }
  storage.factorStorage = where;
  MoFEMFunctionReturn(0);
}
 
/// set orders of blocks by finite element and by column entity
static void set_block_orders(BlockStorage &storage) {
  auto &index = storage.blockIndex;
  auto &fe_order = storage.feOrder;
  fe_order.resize(index.size());
  std::iota(fe_order.begin(), fe_order.end(), 0);
  std::sort(fe_order.begin(), fe_order.end(), [&index](int a, int b) {
    if (index[a].getFEUId() != index[b].getFEUId())
      return index[a].getFEUId() < index[b].getFEUId();
    return a < b;
  });
  auto &col_order = storage.colOrder;
  col_order.resize(index.size());
  std::iota(col_order.begin(), col_order.end(), 0);
  std::sort(col_order.begin(), col_order.end(), [&index](int a, int b) {
    if (index[a].getColUId() != index[b].getColUId())
      return index[a].getColUId() < index[b].getColUId();
    return a < b;
  });
}
 
/// set blocks of block matrix (parent numbering)
static void set_parent_blocks(BlockStructure &s) {
  s.blocks.clear();
  for (auto &d : s.storage->blockIndex) {
    if (d.getElem() == -1)
      s.blocks.push_back({d.getMatShift(), d.getLd(), d.getRow(), d.getCol(),
                          d.getNbRows(), d.getNbCols(), d.getLocRow(),
                          d.getLocCol()});
  }
  s.blocks.shrink_to_fit();
  s.hasElems = !s.storage->elems.empty();
  s.numbering = BlockStructure::PARENT;
}
 
/**
 * @brief Iterate all blocks of matrix, including listed blocks of A00
 * elements
 *
 * fun(ptr, ld, row, col, nb_rows, nb_cols, loc_row, loc_col)
 */
template <typename FUN>
static inline void for_each_block(const BlockStructure &ctx, FUN &&fun) {
  auto &storage = *ctx.storage;
  double *data = storage.data.data();
  for (auto &b : ctx.blocks)
    fun(data + b.shift, b.ld, b.row, b.col, b.nbRows, b.nbCols, b.locRow,
        b.locCol);
  if (ctx.hasElems) {
    for (int ei = 0; ei != static_cast<int>(storage.elems.size()); ++ei) {
      auto &e = storage.elems[ei];
      auto &p = storage.patterns[e.pattern];
      double *buf = storage.elemData[ei].data();
      for (int r = 0; r != p.nbNodes; ++r) {
        auto &re = storage.elemEntities[e.entBegin + r];
        for (int c = 0; c != p.nbNodes; ++c) {
          if (!p.isListed(r, c))
            continue;
          auto &ce = storage.elemEntities[e.entBegin + c];
          fun(buf + p.sShift(r, c), p.sLd(r, c), ctx.globIdx(re),
              ctx.globIdx(ce), re.nb, ce.nb, ctx.locIdx(re), ctx.locIdx(ce));
        }
      }
    }
  }
}
 
boost::shared_ptr<BlockStructure> createBlockMatStructure(
 
    DM dm,                                //< dm
    SchurFEOpsFEandFields schur_fe_op_vec //< block elements
 
) {
 
  auto cmp_uid_lo = [](const boost::weak_ptr<FieldEntity> &a, const UId &b) {
    if (auto a_ptr = a.lock()) {
      if (a_ptr->getLocalUniqueId() < b)
        return true;
      else
        return false;
    } else {
      return false;
    }
  };
 
  auto cmp_uid_hi = [](const UId &b, const boost::weak_ptr<FieldEntity> &a) {
    if (auto a_ptr = a.lock()) {
      if (b < a_ptr->getLocalUniqueId())
        return true;
      else
        return false;
    } else {
      return true;
    }
  };
 
  // get uids for fields
  auto get_uid_pair = [](const auto &field_id) {
    auto lo_uid = FieldEntity::getLocalUniqueIdCalculate(
        field_id, get_id_for_min_type<MBVERTEX>());
    auto hi_uid = FieldEntity::getLocalUniqueIdCalculate(
        field_id, get_id_for_max_type<MBENTITYSET>());
    return std::make_pair(lo_uid, hi_uid);
  };
 
  // get uids pair
  auto get_it_pair = [cmp_uid_lo, cmp_uid_hi](auto &&field_ents, auto &&p_uid) {
    auto lo = std::lower_bound(field_ents.begin(), field_ents.end(),
                               p_uid.first, cmp_uid_lo);
    auto hi = std::upper_bound(field_ents.begin(), field_ents.end(),
                               p_uid.second, cmp_uid_hi);
    return std::make_pair(lo, hi);
  };
 
  // extract DOFs for rows/columns. DOFs are associated with fields entities
  // for given problem.
  auto row_extractor = [](auto &e) { return e->entityCacheRowDofs; };
  auto col_extractor = [](auto &e) { return e->entityCacheColDofs; };
 
  auto extract_data = [](auto &&its, auto extractor) {
    std::vector<std::tuple<UId, int, int, int>> data;
    data.reserve(std::distance(its.first, its.second));
    // iterate field dofs
    for (; its.first != its.second; ++its.first) {
      if (auto e = its.first->lock()) {
        if (auto cache = extractor(e).lock()) {
          int nb_dofs = 0;
          for (auto dof = cache->loHi[0]; dof != cache->loHi[1]; ++dof) {
            if ((*dof)->getPetscGlobalDofIdx() != -1)
              ++nb_dofs;
          }
          auto uid = e->getLocalUniqueId();
          if (nb_dofs) {
 
            auto glob = (*cache->loHi[0])->getPetscGlobalDofIdx();
            auto loc = (*cache->loHi[0])->getPetscLocalDofIdx();
            while (glob == -1 && cache->loHi[0] != cache->loHi[1]) {
              ++cache->loHi[0];
              glob = (*cache->loHi[0])->getPetscGlobalDofIdx();
              loc = (*cache->loHi[0])->getPetscLocalDofIdx();
            }
            data.emplace_back(uid, glob, nb_dofs, loc);
 
#ifndef NDEBUG
 
            for (auto lo = cache->loHi[0]; lo != cache->loHi[1]; ++lo) {
              auto glob = (*lo)->getPetscGlobalDofIdx();
              if (glob == -1) {
                CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                                  "Wrong global index");
              }
            }
 
#endif
          } else {
            data.emplace_back(uid, -1, 0, -1);
          }
        }
      }
    }
    return data;
  };
 
  auto data_ptr = boost::make_shared<BlockStructure>();
  auto storage = boost::make_shared<BlockStorage>();
  data_ptr->storage = storage;
  auto &index = storage->blockIndex;
  auto m_field_ptr = getInterfacePtr(dm);
 
  // create element to extract data
  auto fe_method = boost::shared_ptr<MoFEM::FEMethod>(new MoFEM::FEMethod());
 
  // first pass counts blocks, such that index is allocated once, second pass
  // fills index
  size_t nb_pairs = 0;
  for (auto count_only : {true, false}) {
 
    if (!count_only)
      index.reserve(nb_pairs);
 
    for (auto &d : schur_fe_op_vec) {
 
      // extract bit numbers  for row and column
      auto get_bit_numbers = [&d](auto op) {
        std::vector<FieldBitNumber> bit_numbers(d.second.size());
        std::transform(d.second.begin(), d.second.end(), bit_numbers.begin(),
                       op);
        return bit_numbers;
      };
 
      // extract bit numbers  for row
      auto row_bit_numbers = get_bit_numbers(
          [&](auto &p) { return m_field_ptr->get_field_bit_number(p.first); });
      // extract bit numbers  for row
      auto col_bit_numbers = get_bit_numbers(
          [&](auto &p) { return m_field_ptr->get_field_bit_number(p.second); });
 
      fe_method->preProcessHook = []() { return 0; };
      fe_method->postProcessHook = []() { return 0; };
      fe_method->operatorHook = [&]() {
        MoFEMFunctionBegin;
 
        auto fe_uid = fe_method->numeredEntFiniteElementPtr->getLocalUniqueId();
 
        for (auto f = 0; f != row_bit_numbers.size(); ++f) {
 
          auto row_data =
              extract_data(get_it_pair(fe_method->getRowFieldEnts(),
                                       get_uid_pair(row_bit_numbers[f])),
                           row_extractor);
          auto col_data =
              extract_data(get_it_pair(fe_method->getColFieldEnts(),
                                       get_uid_pair(col_bit_numbers[f])),
                           col_extractor);
 
          for (auto &r : row_data) {
            auto [r_uid, r_glob, r_nb_dofs, r_loc] = r;
            if (!r_nb_dofs)
              continue;
            for (auto &c : col_data) {
              auto [c_uid, c_glob, c_nb_dofs, c_loc] = c;
              if (!c_nb_dofs)
                continue;
              if (count_only)
                ++nb_pairs;
              else
                index.emplace_back(r_uid, c_uid, fe_uid, r_glob, c_glob,
                                   r_nb_dofs, c_nb_dofs, r_loc, c_loc);
            }
          }
        }
 
        MoFEMFunctionReturn(0);
      };
 
      CHKERR DMoFEMLoopFiniteElementsUpAndLowRank(dm, d.first, fe_method, 0,
                                                  m_field_ptr->get_comm_size());
    };
  }
 
  // unique blocks, block from first element which lists it is kept
  std::stable_sort(index.begin(), index.end(), BlockStorage::lessRowCol);
  index.erase(std::unique(index.begin(), index.end(),
                          [](auto &a, auto &b) {
                            return a.getRowUId() == b.getRowUId() &&
                                   a.getColUId() == b.getColUId();
                          }),
              index.end());
  index.shrink_to_fit();
 
  // blocks are stored in order of row entities
  size_t mem_size = 0;
  for (auto &v : index) {
    v.setStorage(mem_size, v.getNbCols(), -1);
    mem_size += static_cast<size_t>(v.getNbRows()) * v.getNbCols();
  }
 
  if (mem_size > storage->data.max_size())
    CHK_THROW_MESSAGE(MOFEM_OPERATION_UNSUCCESSFUL, "Vector too big");
  storage->dataSize = mem_size;
 
  set_block_orders(*storage);
  set_parent_blocks(*data_ptr);
 
  auto ghost_x = createDMVector(dm);
  auto ghost_y = createDMVector(dm);
  CHKERR VecSetDM(ghost_x, PETSC_NULLPTR);
  CHKERR VecSetDM(ghost_y, PETSC_NULLPTR);
 
  data_ptr->ghostX = ghost_x;
  data_ptr->ghostY = ghost_y;
 
  MPI_Comm comm;
  CHKERR PetscObjectGetComm((PetscObject)dm, &comm);
  std::array<size_t, 2> loc_stats = {index.size(), mem_size * sizeof(double)};
  std::array<size_t, 2> sum_stats, max_stats;
  MPI_Allreduce(loc_stats.data(), sum_stats.data(), 2, MPIU_SIZE_T, MPI_SUM,
                comm);
  MPI_Allreduce(loc_stats.data(), max_stats.data(), 2, MPIU_SIZE_T, MPI_MAX,
                comm);
  constexpr double mb = 1024. * 1024.;
  MOFEM_TAG_AND_LOG("WORLD", Sev::verbose, "BlockMat")
      << "Blocks " << sum_stats[0] << " (max per process " << max_stats[0]
      << "), data " << sum_stats[1] / mb << " MB (max per process "
      << max_stats[1] / mb << " MB), index "
      << (sum_stats[0] * (sizeof(BlockStorage::Indexes) + 2 * sizeof(int) +
                          sizeof(BlockStructure::Block))) /
             mb
      << " MB";
 
  return data_ptr;
}
 
/**
 * @brief Matrix-vector product of block matrix
 *
 * Blocks with owned columns are multiplied while ghost values of x are
 * communicated, then blocks with ghost columns. A00 elements are applied as
 * whole, from listed blocks, or from factors if elements are factorised.
 */
static MoFEMErrorCode mult_block_structure(Mat mat, Vec x, Vec y,
                                           InsertMode iora, bool trans) {
  MoFEMFunctionBegin;
  BlockStructure *ctx;
  CHKERR MatShellGetContext(mat, (void **)&ctx);
  auto &storage = *ctx->storage;
  storage.ensureAllocated();
 
  PetscLogEventBegin(SchurEvents::MOFEM_EVENT_BlockStructureMult, 0, 0, 0, 0);
 
  int x_loc_size;
  CHKERR VecGetLocalSize(x, &x_loc_size);
 
  Vec ghost_x = ctx->ghostX;
  Vec ghost_y = ctx->ghostY;
  if (trans)
    std::swap(ghost_x, ghost_y);
 
  CHKERR storage.checkNotFailed();
  const bool with_p = ctx->hasElems && ctx->multiplyByPreconditioner;
  if (with_p && !storage.hasPreconditioner)
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "No preconditioner blocks");
  const bool use_lu =
      ctx->hasElems && storage.isFactorised() &&
      (storage.factorStorage == BlockStorage::FACTOR_IN_DATA || with_p);
  auto &lu_bufs =
      storage.factorStorage == BlockStorage::FACTOR_IN_PRECONDITIONER
          ? storage.elemPrec
          : storage.elemData;
 
  CHKERR VecCopy(x, ghost_x);
  {
    Vec loc_ghost_y;
    CHKERR VecGhostGetLocalForm(ghost_y, &loc_ghost_y);
    CHKERR VecSet(loc_ghost_y, 0.);
    CHKERR VecGhostRestoreLocalForm(ghost_y, &loc_ghost_y);
  }
 
  std::vector<double> xe, ye;
  if (ctx->hasElems) {
    xe.resize(storage.maxElemSize);
    ye.resize(storage.maxElemSize);
  }
 
  auto mult_pass = [&](bool ghost_pass) {
    MoFEMFunctionBegin;
 
    const double *x_array;
    Vec loc_ghost_x;
    CHKERR VecGhostGetLocalForm(ghost_x, &loc_ghost_x);
    CHKERR VecGetArrayRead(loc_ghost_x, &x_array);
 
    double *y_array;
    Vec loc_ghost_y;
    CHKERR VecGhostGetLocalForm(ghost_y, &loc_ghost_y);
    CHKERR VecGetArray(loc_ghost_y, &y_array);
 
    const double *data = storage.data.data();
    const auto cblas_trans = trans ? CblasTrans : CblasNoTrans;
 
    for (auto &b : ctx->blocks) {
      const int x_loc = trans ? b.locRow : b.locCol;
      const int y_loc = trans ? b.locCol : b.locRow;
      // blocks with entities not available on this process are skipped
      if (x_loc < 0 || y_loc < 0)
        continue;
      if ((x_loc >= x_loc_size) != ghost_pass)
        continue;
      cblas_dgemv(CblasRowMajor, cblas_trans, b.nbRows, b.nbCols, 1.0,
                  data + b.shift, b.ld, x_array + x_loc, 1, 1.0,
                  y_array + y_loc, 1);
    }
 
    if (ctx->hasElems) {
      for (int ei = 0; ei != static_cast<int>(storage.elems.size()); ++ei) {
        auto &e = storage.elems[ei];
        const int n = e.size;
        if (!n || !e.local)
          continue;
        // factorised storage holds factors only for assembled elements,
        // other element matrices are zero
        if (use_lu && !e.factorised)
          continue;
        auto ent_begin = storage.elemEntities.data() + e.entBegin;
        auto ent_end = storage.elemEntities.data() + e.entEnd;
 
        // element with any ghost column is multiplied after ghost update,
        // element with entity not available on this process is skipped
        bool any_ghost = false;
        bool available = true;
        for (auto ent = ent_begin; ent != ent_end; ++ent) {
          if (!ent->nb)
            continue;
          const int loc = ctx->locIdx(*ent);
          if (loc < 0)
            available = false;
          else if (loc >= x_loc_size)
            any_ghost = true;
        }
        if (!available || any_ghost != ghost_pass)
          continue;
 
        for (auto ent = ent_begin; ent != ent_end; ++ent)
          if (ent->nb)
            cblas_dcopy(ent->nb, x_array + ctx->locIdx(*ent), 1,
                        xe.data() + ent->offset, 1);
 
        auto &p = storage.patterns[e.pattern];
        auto piv = storage.pivots.data() + e.pivShift;
        double *y_e;
        if (use_lu) {
          if (e.dense)
            apply_lu_factors(n, storage.elemDense[ei].data(), piv, xe.data(),
                             trans);
          else
            elem_mult_factors(p, lu_bufs[ei].data(), piv, xe.data(), trans);
          y_e = xe.data();
        } else {
          std::fill(ye.begin(), ye.begin() + n, 0.);
          elem_mult_listed(p, storage.elemData[ei].data(), xe.data(), ye.data(),
                           trans);
          if (with_p)
            elem_mult_listed(p, storage.elemPrec[ei].data(), xe.data(),
                             ye.data(), trans);
          y_e = ye.data();
        }
 
        for (auto ent = ent_begin; ent != ent_end; ++ent)
          if (ent->nb)
            cblas_daxpy(ent->nb, 1.0, y_e + ent->offset, 1,
                        y_array + ctx->locIdx(*ent), 1);
      }
    }
 
    CHKERR VecRestoreArrayRead(loc_ghost_x, &x_array);
    CHKERR VecRestoreArray(loc_ghost_y, &y_array);
    CHKERR VecGhostRestoreLocalForm(ghost_x, &loc_ghost_x);
    CHKERR VecGhostRestoreLocalForm(ghost_y, &loc_ghost_y);
    MoFEMFunctionReturn(0);
  };
 
  CHKERR VecGhostUpdateBegin(ghost_x, INSERT_VALUES, SCATTER_FORWARD);
  CHKERR mult_pass(false);
  CHKERR VecGhostUpdateEnd(ghost_x, INSERT_VALUES, SCATTER_FORWARD);
  CHKERR mult_pass(true);
 
  CHKERR VecGhostUpdateBegin(ghost_y, ADD_VALUES, SCATTER_REVERSE);
  CHKERR VecGhostUpdateEnd(ghost_y, ADD_VALUES, SCATTER_REVERSE);
 
  switch (iora) {
  case INSERT_VALUES:
    CHKERR VecCopy(ghost_y, y);
    break;
  case ADD_VALUES:
    CHKERR VecAXPY(y, 1., ghost_y);
    break;
  default:
    SETERRQ(PETSC_COMM_SELF, MOFEM_NOT_IMPLEMENTED, "Wrong InsertMode");
  }
 
  PetscLogEventEnd(SchurEvents::MOFEM_EVENT_BlockStructureMult, 0, 0, 0, 0);
 
  MoFEMFunctionReturn(0);
}
 
/**
 * @brief Solve with A00, i.e. x = A00^{-1} f, element by element
 *
 * Elements are factorised in place on first call, and factors are reused
 * until matrix is zeroed.
 */
static MoFEMErrorCode solve_block_structure(Mat mat, Vec f, Vec x,
                                            InsertMode iora, bool trans) {
  MoFEMFunctionBegin;
  BlockStructure *ctx;
  CHKERR MatShellGetContext(mat, (void **)&ctx);
  auto &storage = *ctx->storage;
  storage.ensureAllocated();
 
  PetscLogEventBegin(SchurEvents::MOFEM_EVENT_BlockStructureSolve, 0, 0, 0, 0);
 
  if (ctx->numbering != BlockStructure::BLOCK)
    SETERRQ(PETSC_COMM_SELF, MOFEM_NOT_IMPLEMENTED,
            "Solve is implemented only for A00 block of nested Schur matrix, "
            "see createSchurNestedMatrixStruture");
 
  CHKERR storage.checkNotFailed();
  const bool with_p = ctx->multiplyByPreconditioner;
  if (with_p && !storage.hasPreconditioner)
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "No preconditioner blocks");
 
  bool use_lu = true;
  if (with_p) {
    CHKERR factorise_elems(storage, BlockStorage::FACTOR_IN_PRECONDITIONER);
  } else if (!storage.hasPreconditioner) {
    CHKERR factorise_elems(storage, BlockStorage::FACTOR_IN_DATA);
  } else {
    // operator without preconditioner while preconditioner blocks exist,
    // element matrices are factorised on the fly
    use_lu = false;
  }
  auto &lu_bufs = with_p ? storage.elemPrec : storage.elemData;
 
  int nb_local;
  CHKERR VecGetLocalSize(x, &nb_local);
 
  if (iora == INSERT_VALUES)
    CHKERR VecZeroEntries(x);
 
  double *x_array;
  CHKERR VecGetArray(x, &x_array);
  const double *f_array;
  CHKERR VecGetArrayRead(f, &f_array);
 
  std::vector<double> t(storage.maxElemSize);
  std::vector<double> scratch;
  std::vector<__CLPK_integer> scratch_piv;
  if (!use_lu) {
    scratch.resize(static_cast<size_t>(storage.maxElemSize) *
                   storage.maxElemSize);
    scratch_piv.resize(storage.maxElemSize);
  }
 
  const char lapack_trans = trans ? 'N' : 'T';
 
  for (int ei = 0; ei != static_cast<int>(storage.elems.size()); ++ei) {
    auto &e = storage.elems[ei];
    const int n = e.size;
    if (!n || !e.local)
      continue;
    if (use_lu && !e.factorised)
      continue;
    auto ent_begin = storage.elemEntities.data() + e.entBegin;
    auto ent_end = storage.elemEntities.data() + e.entEnd;
 
    // only elements with owned DOFs are solved
    bool owned = true;
    for (auto ent = ent_begin; ent != ent_end; ++ent) {
      if (ent->nb && (ent->locBlock < 0 || ent->locBlock >= nb_local)) {
        owned = false;
        break;
      }
    }
    if (!owned)
      continue;
 
    for (auto ent = ent_begin; ent != ent_end; ++ent)
      if (ent->nb)
        cblas_dcopy(ent->nb, f_array + ent->locBlock, 1, t.data() + ent->offset,
                    1);
 
    auto &p = storage.patterns[e.pattern];
    auto piv = storage.pivots.data() + e.pivShift;
    __CLPK_integer info = 0;
    if (use_lu) {
      if (e.dense)
        info = lapack_dgetrs(lapack_trans, n, 1, storage.elemDense[ei].data(),
                             n, piv, t.data(), n);
      else
        CHKERR elem_solve(p, lu_bufs[ei].data(), piv, t.data(), n, 1, trans,
                          nullptr);
    } else {
      std::fill(scratch.begin(), scratch.begin() + static_cast<size_t>(n) * n,
                0.);
      elem_gather_dense(p, storage.elemData[ei].data(), scratch.data());
      info = lapack_dgetrf(n, n, scratch.data(), n, scratch_piv.data());
      if (info == 0)
        info = lapack_dgetrs(lapack_trans, n, 1, scratch.data(), n,
                             scratch_piv.data(), t.data(), n);
    }
    if (info != 0)
      SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
              "error lapack solve info = %d", static_cast<int>(info));
 
    for (auto ent = ent_begin; ent != ent_end; ++ent)
      if (ent->nb)
        cblas_daxpy(ent->nb, 1.0, t.data() + ent->offset, 1,
                    x_array + ent->locBlock, 1);
  }
 
  CHKERR VecRestoreArray(x, &x_array);
  CHKERR VecRestoreArrayRead(f, &f_array);
 
  PetscLogEventEnd(SchurEvents::MOFEM_EVENT_BlockStructureSolve, 0, 0, 0, 0);
 
  MoFEMFunctionReturn(0);
}
 
static PetscErrorCode mult(Mat mat, Vec x, Vec y) {
  return mult_block_structure(mat, x, y, INSERT_VALUES, false);
}
static PetscErrorCode mult_add(Mat mat, Vec x, Vec y) {
  return mult_block_structure(mat, x, y, ADD_VALUES, false);
}
static PetscErrorCode mult_transpose(Mat mat, Vec x, Vec y) {
  return mult_block_structure(mat, x, y, INSERT_VALUES, true);
}
static PetscErrorCode mult_transpose_add(Mat mat, Vec x, Vec y) {
  return mult_block_structure(mat, x, y, ADD_VALUES, true);
}
 
static PetscErrorCode solve(Mat mat, Vec x, Vec y) {
  return solve_block_structure(mat, x, y, INSERT_VALUES, false);
}
static PetscErrorCode solve_add(Mat mat, Vec x, Vec y) {
  return solve_block_structure(mat, x, y, ADD_VALUES, false);
}
static PetscErrorCode solve_transpose(Mat mat, Vec x, Vec y) {
  return solve_block_structure(mat, x, y, INSERT_VALUES, true);
}
static PetscErrorCode solve_transpose_add(Mat mat, Vec x, Vec y) {
  return solve_block_structure(mat, x, y, ADD_VALUES, true);
}
 
static PetscErrorCode zero_rows_columns(Mat A, PetscInt N,
                                        const PetscInt rows[], PetscScalar diag,
                                        Vec x, Vec b) {
 
  MoFEMFunctionBeginHot;
  BlockStructure *ctx;
  CHKERR MatShellGetContext(A, (void **)&ctx);
  auto &storage = *ctx->storage;
  CHKERR storage.modified();
  storage.ensureAllocated();
 
  PetscLogEventBegin(SchurEvents::MOFEM_EVENT_zeroRowsAndCols, 0, 0, 0, 0);
 
  // rows are gathered from all processes, since blocks with ghost rows or
  // columns are stored on this process
  std::vector<int> zero_idx;
  {
    MPI_Comm comm;
    CHKERR PetscObjectGetComm((PetscObject)A, &comm);
    int size;
    MPI_Comm_size(comm, &size);
    if (size > 1) {
      auto is = createISGeneral(comm, N, rows, PETSC_USE_POINTER);
      auto is_local = isAllGather(is);
      int is_nb_rows;
      CHKERR ISGetSize(is_local, &is_nb_rows);
      const int *ptr;
      CHKERR ISGetIndices(is_local, &ptr);
      zero_idx.assign(ptr, ptr + is_nb_rows);
      CHKERR ISRestoreIndices(is_local, &ptr);
    } else {
      zero_idx.assign(rows, rows + N);
    }
  }
  std::sort(zero_idx.begin(), zero_idx.end());
  zero_idx.erase(std::unique(zero_idx.begin(), zero_idx.end()), zero_idx.end());
 
  int loc_m, loc_n;
  CHKERR MatGetLocalSize(A, &loc_m, &loc_n);
 
  for_each_block(*ctx, [&](double *ptr, int ld, int row, int col, int nb_rows,
                           int nb_cols, int loc_row, int loc_col) {
    if (!nb_rows || !nb_cols)
      return;
 
    auto lo = std::lower_bound(zero_idx.begin(), zero_idx.end(), row);
    for (; lo != zero_idx.end() && *lo < row + nb_rows; ++lo) {
      auto *row_ptr = ptr + static_cast<size_t>(*lo - row) * ld;
      std::fill(row_ptr, row_ptr + nb_cols, 0.);
    }
 
    const bool diag_block = row == col && loc_row >= 0 && loc_row < loc_m &&
                            loc_col >= 0 && loc_col < loc_n;
    lo = std::lower_bound(zero_idx.begin(), zero_idx.end(), col);
    for (; lo != zero_idx.end() && *lo < col + nb_cols; ++lo) {
      const int c = *lo - col;
      for (int i = 0; i != nb_rows; ++i)
        ptr[static_cast<size_t>(i) * ld + c] = 0.;
      if (diag_block && c < nb_rows)
        ptr[static_cast<size_t>(c) * ld + c] = diag;
    }
  });
 
  PetscLogEventEnd(SchurEvents::MOFEM_EVENT_zeroRowsAndCols, 0, 0, 0, 0);
 
  MoFEMFunctionReturnHot(0);
}
 
static PetscErrorCode mat_zero(Mat m) {
  MoFEMFunctionBegin;
  BlockStructure *ctx;
  CHKERR MatShellGetContext(m, (void **)&ctx);
  auto &storage = *ctx->storage;
  storage.ensureAllocated();
  std::fill(storage.data.begin(), storage.data.end(), 0.);
  for (auto &buf : storage.elemData)
    std::fill(buf.begin(), buf.end(), 0.);
  for (auto &buf : storage.elemPrec)
    std::fill(buf.begin(), buf.end(), 0.);
  storage.resetFactors();
  MoFEMFunctionReturn(0);
}
 
static MoFEMErrorCode setSchurBlockMatOps(Mat mat_raw) {
  MoFEMFunctionBegin;
  CHKERR MatShellSetManageScalingShifts(mat_raw);
  CHKERR MatShellSetOperation(mat_raw, MATOP_MULT, (void (*)(void))mult);
  CHKERR MatShellSetOperation(mat_raw, MATOP_MULT_ADD,
                              (void (*)(void))mult_add);
  CHKERR MatShellSetOperation(mat_raw, MATOP_MULT_TRANSPOSE,
                              (void (*)(void))mult_transpose);
  CHKERR MatShellSetOperation(mat_raw, MATOP_MULT_TRANSPOSE_ADD,
                              (void (*)(void))mult_transpose_add);
  CHKERR MatShellSetOperation(mat_raw, MATOP_SOLVE, (void (*)(void))solve);
  CHKERR MatShellSetOperation(mat_raw, MATOP_SOLVE_ADD,
                              (void (*)(void))solve_add);
  CHKERR MatShellSetOperation(mat_raw, MATOP_SOLVE_TRANSPOSE,
                              (void (*)(void))solve_transpose);
  CHKERR MatShellSetOperation(mat_raw, MATOP_SOLVE_TRANSPOSE_ADD,
                              (void (*)(void))solve_transpose_add);
  CHKERR MatShellSetOperation(mat_raw, MATOP_ZERO_ENTRIES,
                              (void (*)(void))mat_zero);
  CHKERR MatShellSetOperation(mat_raw, MATOP_ZERO_ROWS_COLUMNS,
                              (void (*)(void))zero_rows_columns);
 
  MoFEMFunctionReturn(0);
};
 
SchurShellMatData createBlockMat(DM dm,
                                 boost::shared_ptr<BlockStructure> data) {
 
  auto problem_ptr = getProblemPtr(dm);
  auto nb_local = problem_ptr->nbLocDofsRow;
  auto nb_global = problem_ptr->nbDofsRow;
 
  // check in nb, rows is equal to nb. columns.
  if (nb_local != problem_ptr->nbLocDofsCol) {
    MOFEM_LOG("SELF", Sev::error)
        << "Wrong size " << nb_local << " != " << problem_ptr->nbLocDofsCol;
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                      "nb. cols is inconsistent with nb. rows");
  }
  if (nb_global != problem_ptr->nbDofsCol) {
    MOFEM_LOG("SELF", Sev::error)
        << "Wrong size " << nb_global << " != " << problem_ptr->nbDofsCol;
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                      "nb. cols is inconsistent with nb. rows");
  }
 
  // get comm from DM
  MPI_Comm comm;
  CHKERR PetscObjectGetComm((PetscObject)dm, &comm);
 
  Mat mat_raw;
  CHKERR MatCreateShell(comm, nb_local, nb_local, nb_global, nb_global,
                        (void *)data.get(), &mat_raw);
  CHKERR setSchurBlockMatOps(mat_raw);
  // CHKERR PetscObjectSetName((PetscObject)mat_raw, MoFEM_BLOCK_MAT);
 
  return std::make_pair(SmartPetscObj<Mat>(mat_raw), data);
}
 
MoFEMErrorCode
assembleBlockMatSchur(MoFEM::Interface &m_field, Mat B, Mat S,
                      std::vector<std::string> fields_name,
                      std::vector<boost::shared_ptr<Range>> field_ents,
                      SmartPetscObj<AO> ao) {
  MoFEMFunctionBegin;
  BlockStructure *ctx;
  CHKERR MatShellGetContext(B, (void **)&ctx);
  auto &storage = *ctx->storage;
  auto &index = storage.blockIndex;
  storage.ensureAllocated();
 
  PetscLogEventBegin(SchurEvents::MOFEM_EVENT_AssembleSchurMat, 0, 0, 0, 0);
 
  if (ao.use_count() == 0) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY, "No AO set");
  }
  CHKERR storage.checkNotFailed();
 
  const bool with_p = ctx->multiplyByPreconditioner;
  if (with_p && !storage.hasPreconditioner)
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "No preconditioner blocks");
 
  if (fields_name.size() != field_ents.size()) {
    SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
            "fields_name.size() != field_ents.size()");
  }
 
  std::vector<std::pair<UId, UId>> a00_fields_uids;
  a00_fields_uids.reserve(fields_name.size());
  auto it_field_ents = field_ents.begin();
  for (auto &f : fields_name) {
    int bn = m_field.get_field_bit_number(f);
    if (*it_field_ents) {
      for (auto p = (*it_field_ents)->pair_begin();
           p != (*it_field_ents)->pair_end(); ++p) {
        a00_fields_uids.emplace_back(
            DofEntity::getLoFieldEntityUId(bn, p->first),
            DofEntity::getHiFieldEntityUId(bn, p->second));
      }
    } else {
      a00_fields_uids.emplace_back(FieldEntity::getLoBitNumberUId(bn),
                                   FieldEntity::getHiBitNumberUId(bn));
    }
    ++it_field_ents;
  }
 
  auto is_a00 = [&](const UId &uid) {
    for (auto &uid_pair : a00_fields_uids) {
      if (uid >= uid_pair.first && uid < uid_pair.second)
        return true;
    }
    return false;
  };
 
  using Idx = BlockStorage::Indexes;
  std::vector<const Idx *> a00_blocks, a01_blocks, a10_blocks, a11_blocks;
  struct LocIdx {
    int index; ///< index in local matrix
    int size;  ///< number of DOFs
    int glob;  ///< global index in block matrix numbering
  };
  std::map<UId, LocIdx> a00_indexing, a11_indexing;
  std::vector<UId> col_uids;
  std::vector<double> a00_mat, a01t_mat, a10_mat, s_mat, tmp_block;
  std::vector<__CLPK_integer> ipiv;
  std::vector<int> glob_idx, row_glob_idx, col_glob_idx;
  std::vector<char> nz;
 
  // assemble block directly to Schur matrix
  auto set_values_block = [&](const Idx *b) {
    MoFEMFunctionBeginHot;
    const int nb_rows = b->getNbRows();
    const int nb_cols = b->getNbCols();
    row_glob_idx.resize(nb_rows);
    col_glob_idx.resize(nb_cols);
    std::iota(row_glob_idx.begin(), row_glob_idx.end(), b->getRow());
    std::iota(col_glob_idx.begin(), col_glob_idx.end(), b->getCol());
    CHKERR AOApplicationToPetsc(ao, nb_rows, row_glob_idx.data());
    CHKERR AOApplicationToPetsc(ao, nb_cols, col_glob_idx.data());
    const double *ptr = storage.blockPtr(*b);
    if (b->getLd() != nb_cols) {
      tmp_block.resize(static_cast<size_t>(nb_rows) * nb_cols);
      copy_block(tmp_block.data(), nb_cols, 0, 0, ptr, b->getLd(), nb_rows,
                 nb_cols);
      ptr = tmp_block.data();
    }
    CHKERR MatSetValues(S, nb_rows, row_glob_idx.data(), nb_cols,
                        col_glob_idx.data(), ptr, ADD_VALUES);
    MoFEMFunctionReturnHot(0);
  };
 
  // local indices of entities, ordered by uid
  auto set_local_indices = [](auto &indexing, auto &row_blocks,
                              auto &col_blocks) {
    indexing.clear();
    for (auto b : row_blocks)
      indexing.emplace(b->getRowUId(), LocIdx{0, b->getNbRows(), b->getRow()});
    for (auto b : col_blocks)
      indexing.emplace(b->getColUId(), LocIdx{0, b->getNbCols(), b->getCol()});
    int size = 0;
    for (auto &p : indexing) {
      p.second.index = size;
      size += p.second.size;
    }
    return size;
  };
 
  const auto &fe_order = storage.feOrder;
 
  // classify blocks of finite element starting at position p in fe_order,
  // advance p to next element
  auto classify_fe = [&](size_t &p) {
    a00_blocks.clear();
    a01_blocks.clear();
    a10_blocks.clear();
    a11_blocks.clear();
    const auto fe_uid = index[fe_order[p]].getFEUId();
    for (; p != fe_order.size() && index[fe_order[p]].getFEUId() == fe_uid;
         ++p) {
      auto b = &index[fe_order[p]];
      const bool r00 = is_a00(b->getRowUId());
      const bool c00 = is_a00(b->getColUId());
      if (r00 && c00)
        a00_blocks.push_back(b);
      else if (r00)
        a01_blocks.push_back(b);
      else if (c00)
        a10_blocks.push_back(b);
      else
        a11_blocks.push_back(b);
    }
    return fe_uid;
  };
 
  // return index of A00 element if eliminated blocks of finite element are
  // exactly listed blocks of that element, otherwise -1
  auto element_of = [&]() {
    const int elem = a00_blocks.front()->getElem();
    if (elem < 0)
      return -1;
    auto &p = storage.patterns[storage.elems[elem].pattern];
    if (static_cast<int>(a00_blocks.size()) != p.nbListed)
      return -1;
    for (auto b : a00_blocks)
      if (b->getElem() != elem)
        return -1;
    return elem;
  };
 
  // Factors stored in place can be used only if for every element the
  // eliminated blocks coincide with A00 element. That is checked before any
  // destructive factorisation.
  // process without A00 elements (nested structure exists) is consistent
  bool all_consistent = storage.nested;
  for (size_t p = 0; all_consistent && p != fe_order.size();) {
    classify_fe(p);
    // retained block which is part of A00 element means that eliminated set
    // is smaller than nested A00
    for (auto blocks : {&a01_blocks, &a10_blocks, &a11_blocks})
      for (auto b : *blocks)
        if (b->getElem() >= 0)
          all_consistent = false;
    if (!a00_blocks.empty() && element_of() < 0)
      all_consistent = false;
  }
 
  enum Mode { SCRATCH, DATA_FACTORS, PRECONDITIONER_FACTORS } mode = SCRATCH;
  if (with_p) {
    if (!all_consistent)
      SETERRQ(PETSC_COMM_SELF, MOFEM_NOT_IMPLEMENTED,
              "Schur complement with preconditioner blocks requires eliminated "
              "fields equal to A00 of nested matrix");
    CHKERR factorise_elems(storage, BlockStorage::FACTOR_IN_PRECONDITIONER);
    mode = PRECONDITIONER_FACTORS;
  } else if (storage.nested && !storage.hasPreconditioner) {
    if (all_consistent) {
      CHKERR factorise_elems(storage, BlockStorage::FACTOR_IN_DATA);
      mode = DATA_FACTORS;
    } else if (storage.isFactorised()) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "A00 is factorised in place, eliminated fields have to be equal "
              "to A00 of nested matrix");
    }
  }
 
  for (size_t p = 0; p != fe_order.size();) {
 
    const auto fe_uid = classify_fe(p);
 
    for (auto b : a11_blocks)
      CHKERR set_values_block(b);
 
    if (a00_blocks.empty())
      continue;
 
    // couplings of A00 entities assembled by other elements, e.g. skeleton
    // elements of hybridised problem. Blocks of A00 are sorted by row uid.
    for (size_t i = 0; i != a00_blocks.size(); ++i) {
      const auto r_uid = a00_blocks[i]->getRowUId();
      if (i && a00_blocks[i - 1]->getRowUId() == r_uid)
        continue;
      auto range = storage.rowRange(r_uid);
      for (auto it = range.first; it != range.second; ++it) {
        if (it->getFEUId() != fe_uid && !is_a00(it->getColUId()))
          a01_blocks.push_back(&*it);
      }
    }
    col_uids.clear();
    for (auto b : a00_blocks)
      col_uids.push_back(b->getColUId());
    std::sort(col_uids.begin(), col_uids.end());
    col_uids.erase(std::unique(col_uids.begin(), col_uids.end()),
                   col_uids.end());
    for (auto &c_uid : col_uids) {
      auto range = storage.colRange(c_uid);
      for (auto it = range.first; it != range.second; ++it) {
        auto &b = index[*it];
        if (b.getFEUId() != fe_uid && !is_a00(b.getRowUId()))
          a10_blocks.push_back(&b);
      }
    }
 
    const int a11_size =
        set_local_indices(a11_indexing, a10_blocks, a01_blocks);
    if (!a11_size)
      continue;
 
    // A01 is gathered transposed (i.e. column-major), then X = A00^{-1} A01 is
    // obtained directly by solve with factors of M = A00^T
    auto gather_a01t = [&](int a00_size) {
      a01t_mat.assign(static_cast<size_t>(a11_size) * a00_size, 0.);
      for (auto b : a01_blocks) {
        const auto &ri = a00_indexing.at(b->getRowUId());
        const auto &ci = a11_indexing.at(b->getColUId());
        const double *ptr = storage.blockPtr(*b);
        const int ld = b->getLd();
        for (int i = 0; i != b->getNbRows(); ++i)
          for (int j = 0; j != b->getNbCols(); ++j)
            a01t_mat[static_cast<size_t>(ci.index + j) * a00_size + ri.index +
                     i] = ptr[static_cast<size_t>(i) * ld + j];
      }
    };
 
    int a00_size;
    if (mode != SCRATCH) {
      const int elem = element_of();
      if (elem < 0)
        SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
                "Inconsistent A00 element");
      auto &e = storage.elems[elem];
      // element matrix not assembled on this process
      if (!e.factorised)
        continue;
      auto &pat = storage.patterns[e.pattern];
      // A00 rows and columns in layout of element vector
      a00_indexing.clear();
      for (int r = e.entBegin; r != e.entEnd; ++r) {
        auto &ent = storage.elemEntities[r];
        a00_indexing.emplace(ent.uid,
                             LocIdx{ent.offset, ent.nb, ent.globParent});
      }
      a00_size = e.size;
      gather_a01t(a00_size);
      auto piv = storage.pivots.data() + e.pivShift;
      if (e.dense) {
        auto info = lapack_dgetrs('T', a00_size, a11_size,
                                  storage.elemDense[elem].data(), a00_size, piv,
                                  a01t_mat.data(), a00_size);
        if (info != 0)
          SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
                  "error lapack dgetrs info = %d", static_cast<int>(info));
      } else {
        // nodes coupled to retained entities, other rows of A01 are zero
        nz.assign(pat.nbNodes, 0);
        for (auto b : a01_blocks)
          for (int r = 0; r != pat.nbNodes; ++r)
            if (storage.elemEntities[e.entBegin + r].uid == b->getRowUId())
              nz[r] = 1;
        auto &buf = mode == DATA_FACTORS ? storage.elemData[elem]
                                         : storage.elemPrec[elem];
        CHKERR elem_solve(pat, buf.data(), piv, a01t_mat.data(), a00_size,
                          a11_size, false, &nz);
      }
    } else {
      a00_size = set_local_indices(a00_indexing, a00_blocks, a00_blocks);
      const int elem = a00_blocks.front()->getElem();
      if (elem >= 0 && !storage.elems[elem].local)
        continue;
      a00_mat.assign(static_cast<size_t>(a00_size) * a00_size, 0.);
      for (auto b : a00_blocks) {
        const auto &ri = a00_indexing.at(b->getRowUId());
        const auto &ci = a00_indexing.at(b->getColUId());
        copy_block(a00_mat.data(), a00_size, ri.index, ci.index,
                   storage.blockPtr(*b), b->getLd(), b->getNbRows(),
                   b->getNbCols());
      }
      // copy of element owned by other process
      if (elem >= 0 && !storage.elems[elem].owned &&
          is_zero_matrix(a00_mat.data(), a00_mat.size()))
        continue;
      ipiv.resize(a00_size);
      auto info = lapack_dgetrf(a00_size, a00_size, a00_mat.data(), a00_size,
                                ipiv.data());
      if (info != 0)
        SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
                "error lapack dgetrf info = %d", static_cast<int>(info));
      gather_a01t(a00_size);
      info = lapack_dgetrs('T', a00_size, a11_size, a00_mat.data(), a00_size,
                           ipiv.data(), a01t_mat.data(), a00_size);
      if (info != 0)
        SETERRQ(PETSC_COMM_SELF, MOFEM_OPERATION_UNSUCCESSFUL,
                "error lapack dgetrs info = %d", static_cast<int>(info));
    }
 
    a10_mat.assign(static_cast<size_t>(a11_size) * a00_size, 0.);
    for (auto b : a10_blocks) {
      const auto &ri = a11_indexing.at(b->getRowUId());
      const auto &ci = a00_indexing.at(b->getColUId());
      copy_block(a10_mat.data(), a00_size, ri.index, ci.index,
                 storage.blockPtr(*b), b->getLd(), b->getNbRows(),
                 b->getNbCols());
    }
 
    // S = -A10 A00^{-1} A01
    s_mat.resize(static_cast<size_t>(a11_size) * a11_size);
    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, a11_size, a11_size,
                a00_size, -1., a10_mat.data(), a00_size, a01t_mat.data(),
                a00_size, 0., s_mat.data(), a11_size);
 
    glob_idx.resize(a11_size);
    for (auto &pi : a11_indexing) {
      auto &[idx, size, glob] = pi.second;
      std::iota(glob_idx.begin() + idx, glob_idx.begin() + idx + size, glob);
    }
    CHKERR AOApplicationToPetsc(ao, a11_size, glob_idx.data());
    CHKERR MatSetValues(S, a11_size, glob_idx.data(), a11_size, glob_idx.data(),
                        s_mat.data(), ADD_VALUES);
  }
 
  PetscLogEventEnd(SchurEvents::MOFEM_EVENT_AssembleSchurMat, 0, 0, 0, 0);
 
  MoFEMFunctionReturn(0);
}
 
boost::shared_ptr<std::vector<double>> getBlockMatStorageMat(Mat B) {
  BlockStructure *ctx;
  CHKERR MatShellGetContext(B, (void **)&ctx);
  ctx->storage->ensureAllocated();
  return boost::shared_ptr<std::vector<double>>(ctx->storage,
                                                &ctx->storage->data);
}
 
boost::shared_ptr<std::vector<double>>
getBlockMatPrconditionerStorageMat(Mat B) {
  // preconditioner blocks are stored in buffers of A00 elements
  return boost::make_shared<std::vector<double>>();
}
 
inline MoFEMErrorCode shell_block_mat_asmb_wrap_impl(
    BlockStructure *ctx, const EntitiesFieldData::EntData &row_data,
    const EntitiesFieldData::EntData &col_data, const MatrixDouble &mat,
    InsertMode iora, bool is_preconditioner) {
 
  MoFEMFunctionBegin;
 
  if (row_data.getIndices().empty())
    MoFEMFunctionReturnHot(0);
  if (col_data.getIndices().empty())
    MoFEMFunctionReturnHot(0);
 
#ifndef NDEBUG
 
  PetscLogEventBegin(SchurEvents::MOFEM_EVENT_BlockStructureSetValues, 0, 0, 0,
                     0);
 
#endif // NDEBUG
 
  switch (iora) {
  case ADD_VALUES:
  case INSERT_VALUES:
    break;
  default:
    SETERRQ(PETSC_COMM_SELF, MOFEM_NOT_IMPLEMENTED, "Wrong InsertMode");
  }
 
  auto &storage = *ctx->storage;
  CHKERR storage.modified();
  storage.ensureAllocated();
 
  auto get_row_data = [&]() -> std::pair<bool, VectorInt> {
    if (auto e_ptr = row_data.getFieldEntities()[0]) {
      if (auto stored_data_ptr =
              e_ptr->getSharedStoragePtr<EssentialBcStorage>()) {
        MOFEM_LOG("SELF", Sev::warning) << "Can lead to unhandled behaviour";
        return std::make_pair(true, stored_data_ptr->entityIndices);
      }
    }
    return std::make_pair(false, row_data.getIndices());
  };
 
  auto row_indices = get_row_data();
  const auto &row_ind = row_indices.second;
  const auto &col_ind = col_data.getIndices();
 
  std::vector<int> ent_row_indices;
  std::vector<int> ent_col_indices;
 
  auto get_ent_indices = [](auto &cache, auto &ind) {
    ind.clear();
    ind.reserve(std::distance(cache->loHi[0], cache->loHi[1]));
    for (auto e = cache->loHi[0]; e != cache->loHi[1]; ++e) {
      auto glob = (*e)->getPetscGlobalDofIdx();
      if (glob != -1)
        ind.push_back(glob);
    }
  };
 
  for (auto &rent : row_data.getFieldEntities()) {
    auto r_cache = rent->entityCacheRowDofs.lock();
    if (!r_cache)
      continue;
    get_ent_indices(r_cache, ent_row_indices);
    if (ent_row_indices.empty())
      continue;
    const auto &row_uid = rent->getLocalUniqueId();
 
    for (auto &cent : col_data.getFieldEntities()) {
      auto c_cache = cent->entityCacheColDofs.lock();
      if (!c_cache)
        continue;
      get_ent_indices(c_cache, ent_col_indices);
      if (ent_col_indices.empty())
        continue;
      const auto &col_uid = cent->getLocalUniqueId();
 
      auto idx = storage.find(row_uid, col_uid);
      if (!idx) {
        MOFEM_LOG_CHANNEL("SELF");
        MOFEM_TAG_AND_LOG("SELF", Sev::error, "BlockMat")
            << "missing block: row " << row_data.getFieldDofs()[0]->getName()
            << " col " << col_data.getFieldDofs()[0]->getName();
        SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
                "Block not allocated");
      }
      if (is_preconditioner && idx->getElem() == -1)
        SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
                "Preconditioner blocks are allocated only for A00 element "
                "matrices");
 
      const int nb_rows = idx->getNbRows();
      const int nb_cols = idx->getNbCols();
      const int ld = idx->getLd();
      double *s_mat = storage.blockPtr(*idx, is_preconditioner);
 
      if (mat.size1() == ent_row_indices.size() &&
          mat.size2() == ent_col_indices.size()) {
 
        const double *m = mat.data().data();
        if (ld == nb_cols) {
          if (iora == ADD_VALUES)
            cblas_daxpy(nb_rows * nb_cols, 1.0, m, 1, s_mat, 1);
          else
            cblas_dcopy(nb_rows * nb_cols, m, 1, s_mat, 1);
        } else {
          for (int i = 0; i != nb_rows; ++i) {
            if (iora == ADD_VALUES)
              cblas_daxpy(nb_cols, 1.0, m + static_cast<size_t>(i) * nb_cols, 1,
                          s_mat + static_cast<size_t>(i) * ld, 1);
            else
              cblas_dcopy(nb_cols, m + static_cast<size_t>(i) * nb_cols, 1,
                          s_mat + static_cast<size_t>(i) * ld, 1);
          }
        }
 
      } else {
 
        auto ri = row_ind.begin();
        auto rie = row_ind.end();
 
        int row = 0;
        for (auto re : ent_row_indices) {
          ri = std::find(ri, rie, re);
          if (ri != rie) {
 
            auto ci = col_ind.begin();
            auto cie = col_ind.end();
 
            int col = 0;
            for (auto ce : ent_col_indices) {
              ci = std::find(ci, cie, ce);
              if (ci != cie) {
                auto &m = s_mat[static_cast<size_t>(row) * ld + col];
                if (iora == ADD_VALUES) {
                  m += mat(std::distance(row_ind.begin(), ri),
                           std::distance(col_ind.begin(), ci));
                } else {
                  m = mat(std::distance(row_ind.begin(), ri),
                          std::distance(col_ind.begin(), ci));
                }
              }
              ++col;
            } // cols
          }
          ++row;
        } // rows
      }
 
    } // iterate entities
  }
 
#ifndef NDEBUG
  PetscLogEventEnd(SchurEvents::MOFEM_EVENT_BlockStructureSetValues, 0, 0, 0,
                   0);
#endif // NDEBUG
 
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode
shell_block_mat_asmb_wrap(Mat M, const EntitiesFieldData::EntData &row_data,
                          const EntitiesFieldData::EntData &col_data,
                          const MatrixDouble &mat, InsertMode iora) {
  MoFEMFunctionBegin;
  PetscBool is_mat_shell = PETSC_FALSE;
  PetscObjectTypeCompare((PetscObject)M, MATSHELL, &is_mat_shell);
  if (is_mat_shell) {
    BlockStructure *ctx;
    CHKERR MatShellGetContext(M, (void **)&ctx);
    CHKERR shell_block_mat_asmb_wrap_impl(ctx, row_data, col_data, mat, iora,
                                          false);
  } else {
    CHKERR MatSetValues<AssemblyTypeSelector<PETSC>>(M, row_data, col_data, mat,
                                                     iora);
  }
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode shell_block_preconditioner_mat_asmb_wrap(
    Mat M, const EntitiesFieldData::EntData &row_data,
    const EntitiesFieldData::EntData &col_data, const MatrixDouble &mat,
    InsertMode iora) {
  MoFEMFunctionBegin;
  PetscBool is_mat_shell = PETSC_FALSE;
  PetscObjectTypeCompare((PetscObject)M, MATSHELL, &is_mat_shell);
  if (is_mat_shell) {
    BlockStructure *ctx;
    CHKERR MatShellGetContext(M, (void **)&ctx);
    if (!ctx->storage->hasPreconditioner)
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "No preconditioner blocks");
    CHKERR shell_block_mat_asmb_wrap_impl(ctx, row_data, col_data, mat, iora,
                                          true);
  } else {
    CHKERR MatSetValues<AssemblyTypeSelector<PETSC>>(M, row_data, col_data, mat,
                                                     iora);
  }
  MoFEMFunctionReturn(0);
}
 
boost::shared_ptr<NestSchurData> createSchurNestedMatrixStruture(
 
    std::pair<SmartPetscObj<DM>, SmartPetscObj<DM>> dms,
    boost::shared_ptr<BlockStructure> block_mat_data_ptr,
 
    std::vector<std::string> fields_names, //< a00 fields
    std::vector<boost::shared_ptr<Range>>
        field_ents, //< a00 ranges (can be null),
    bool add_preconditioner_block) {
 
  if (!block_mat_data_ptr) {
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY, "Block data not set");
  }
 
  if (fields_names.size() != field_ents.size())
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                      "fields_names.size() != field_ents.size()");
 
  auto &storage = *block_mat_data_ptr->storage;
  auto &index = storage.blockIndex;
  const int nb_blocks = index.size();
 
  if (storage.isFactorised())
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                      "Block matrix is factorised in place");
  // layout of storage is changed, that would invalidate nested matrices
  // created before
  if (storage.nested)
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                      "Nested Schur structure already created for this block "
                      "matrix, create new one with createBlockMatStructure");
 
  {
    PetscBool dense = PETSC_FALSE;
    CHKERR PetscOptionsGetBool(PETSC_NULLPTR, "", "-schur_a00_dense", &dense,
                               PETSC_NULLPTR);
    storage.optDense = dense == PETSC_TRUE;
    CHKERR ::PetscOptionsGetReal(PETSC_NULLPTR, "",
                                 "-schur_a00_pivot_threshold",
                                 &storage.optPivotThreshold, PETSC_NULLPTR);
    CHKERR ::PetscOptionsGetReal(PETSC_NULLPTR, "", "-schur_a00_rcond_min",
                                 &storage.optRcondMin, PETSC_NULLPTR);
    CHKERR ::PetscOptionsGetReal(PETSC_NULLPTR, "", "-schur_a00_backward_error",
                                 &storage.optBackwardError, PETSC_NULLPTR);
  }
 
  int nb_local_parent;
  CHKERR VecGetLocalSize(block_mat_data_ptr->ghostX, &nb_local_parent);
 
  auto [schur_dm, block_dm] = dms;
  auto schur_prb = getProblemPtr(schur_dm);
  auto block_prb = getProblemPtr(block_dm);
  auto m_field_ptr = getInterfacePtr(block_dm);
 
  auto schur_vec_x = createDMVector(schur_dm);
  auto block_vec_x = createDMVector(block_dm);
  auto schur_vec_y = vectorDuplicate(schur_vec_x);
  auto block_vec_y = vectorDuplicate(block_vec_x);
  CHKERR VecSetDM(schur_vec_x, PETSC_NULLPTR);
  CHKERR VecSetDM(block_vec_x, PETSC_NULLPTR);
  CHKERR VecSetDM(schur_vec_y, PETSC_NULLPTR);
  CHKERR VecSetDM(block_vec_y, PETSC_NULLPTR);
 
  auto find_field_ent = [&](auto uid, auto prb, auto rc) {
    boost::shared_ptr<NumeredDofEntity_multiIndex> dofs;
 
    switch (rc) {
    case ROW:
      dofs = prb->getNumeredRowDofsPtr();
      break;
    case COL:
      dofs = prb->getNumeredColDofsPtr();
      break;
    default:
      CHK_THROW_MESSAGE(MOFEM_NOT_IMPLEMENTED, "Wrong RowCol");
      break;
    }
 
    auto lo = dofs->get<Unique_mi_tag>().lower_bound(uid);
    if (lo == dofs->get<Unique_mi_tag>().end())
      return boost::shared_ptr<NumeredDofEntity>();
    auto hi = dofs->get<Unique_mi_tag>().upper_bound(
        DofEntity::getUniqueIdCalculate(MAX_DOFS_ON_ENTITY - 1, uid));
    if (lo != hi)
      return *lo;
 
    return boost::shared_ptr<NumeredDofEntity>();
  };
 
  // indices of block entities in sub-problems, -1 if entity is not in
  // sub-problem
  struct SubIdx {
    int glob = -1;
    int loc = -1;
  };
  struct SubIdxs {
    SubIdx schurRow, schurCol, blockRow, blockCol;
  };
  std::vector<SubIdxs> sub(nb_blocks);
  auto get_sub = [&](auto uid, auto prb, auto rc) {
    SubIdx s;
    if (auto dof = find_field_ent(uid, prb, rc)) {
      s.glob = dof->getPetscGlobalDofIdx();
      s.loc = dof->getPetscLocalDofIdx();
    }
    return s;
  };
  for (int i = 0; i != nb_blocks; ++i) {
    auto &d = index[i];
    sub[i].schurRow = get_sub(d.getRowUId(), schur_prb, ROW);
    sub[i].schurCol = get_sub(d.getColUId(), schur_prb, COL);
    sub[i].blockRow = get_sub(d.getRowUId(), block_prb, ROW);
    sub[i].blockCol = get_sub(d.getColUId(), block_prb, COL);
  }
 
  // A00 elements: blocks with both entities in block sub-problem, grouped by
  // finite element, stored in element buffers
  std::vector<std::pair<size_t, int>> old_layout(nb_blocks);
  for (int i = 0; i != nb_blocks; ++i) {
    old_layout[i] = std::make_pair(index[i].getMatShift(), index[i].getLd());
    index[i].setStorage(0, index[i].getNbCols(), -1);
  }
 
  std::vector<int> a00;
  for (int i = 0; i != nb_blocks; ++i) {
    if (sub[i].blockRow.glob != -1 && sub[i].blockCol.glob != -1)
      a00.push_back(i);
  }
  std::sort(a00.begin(), a00.end(), [&index](int a, int b) {
    if (index[a].getFEUId() != index[b].getFEUId())
      return index[a].getFEUId() < index[b].getFEUId();
    return a < b;
  });
 
  storage.patterns.clear();
  storage.elems.clear();
  storage.elemEntities.clear();
  storage.maxElemSize = 0;
  int nb_pivots = 0;
  std::map<std::vector<long>, int> pattern_map;
  std::vector<BlockStorage::ElemEntity> ents;
  std::vector<long> key;
 
  for (size_t p = 0; p != a00.size();) {
    const auto fe_uid = index[a00[p]].getFEUId();
    const size_t p_begin = p;
    ents.clear();
    for (; p != a00.size() && index[a00[p]].getFEUId() == fe_uid; ++p) {
      const auto i = a00[p];
      auto &d = index[i];
      ents.push_back({d.getRowUId(), d.getNbRows(), 0, d.getLocRow(),
                      d.getRow(), sub[i].blockRow.loc, sub[i].blockRow.glob});
      ents.push_back({d.getColUId(), d.getNbCols(), 0, d.getLocCol(),
                      d.getCol(), sub[i].blockCol.loc, sub[i].blockCol.glob});
    }
    std::sort(ents.begin(), ents.end(),
              [](auto &a, auto &b) { return a.uid < b.uid; });
    ents.erase(std::unique(ents.begin(), ents.end(),
                           [](auto &a, auto &b) { return a.uid == b.uid; }),
               ents.end());
    const int k = ents.size();
    auto node_of = [&](const UId &uid) {
      return static_cast<int>(
          std::lower_bound(ents.begin(), ents.end(), uid,
                           [](auto &e, auto &u) { return e.uid < u; }) -
          ents.begin());
    };
 
    // pattern: fields, entity types and sizes of nodes, listed pairs
    std::vector<char> listed(k * k, 0);
    for (size_t q = p_begin; q != p; ++q) {
      auto &d = index[a00[q]];
      listed[node_of(d.getRowUId()) * k + node_of(d.getColUId())] = 1;
    }
    key.clear();
    key.push_back(k);
    int n = 0;
    for (auto &e : ents) {
      key.push_back(FieldEntity::getFieldBitNumberFromUniqueId(e.uid));
      key.push_back(
          type_from_handle(FieldEntity::getHandleFromUniqueId(e.uid)));
      key.push_back(e.nb);
      n += e.nb;
    }
    key.insert(key.end(), listed.begin(), listed.end());
    auto pit = pattern_map.find(key);
    if (pit == pattern_map.end()) {
      BlockStorage::Pattern pat;
      pat.nbNodes = k;
      pat.size = n;
      pat.listed = listed;
      pat.nbListed = std::count(listed.begin(), listed.end(), 1);
      for (auto &e : ents) {
        pat.nodeSize.push_back(e.nb);
        pat.nodeName.push_back(
            m_field_ptr->get_field_name(field_bit_from_bit_number(
                FieldEntity::getFieldBitNumberFromUniqueId(e.uid))));
      }
      if (storage.optDense)
        make_block_layout(pat, {});
      else
        make_listed_layout(pat);
      pit = pattern_map.emplace(key, storage.patterns.size()).first;
      storage.patterns.push_back(pat);
    }
    const int pi = pit->second;
    auto &pat = storage.patterns[pi];
    ++pat.nbElems;
 
    const int elem_idx = storage.elems.size();
    const int ent_begin = storage.elemEntities.size();
    for (int r = 0; r != k; ++r)
      ents[r].offset = pat.nodeOffset[r];
    storage.elems.push_back({pi, n, ent_begin, ent_begin + k, nb_pivots});
    storage.elemEntities.insert(storage.elemEntities.end(), ents.begin(),
                                ents.end());
    for (size_t q = p_begin; q != p; ++q) {
      auto &d = index[a00[q]];
      const int r = node_of(d.getRowUId()), c = node_of(d.getColUId());
      d.setStorage(pat.sShift(r, c), pat.sLd(r, c), elem_idx);
    }
 
    nb_pivots += n;
    storage.maxElemSize = std::max(storage.maxElemSize, n);
 
    auto &elem = storage.elems.back();
    elem.local = true;
    elem.owned = true;
    for (auto &e : ents) {
      if (!e.nb)
        continue;
      if (e.locParent < 0)
        elem.local = false;
      if (e.locParent < 0 || e.locParent >= nb_local_parent)
        elem.owned = false;
    }
  }
 
  // eliminated entities have to be element local, i.e. each A00 entity
  // belongs to exactly one element
  {
    std::vector<UId> uids;
    uids.reserve(storage.elemEntities.size());
    for (auto &e : storage.elemEntities)
      uids.push_back(e.uid);
    std::sort(uids.begin(), uids.end());
    if (std::adjacent_find(uids.begin(), uids.end()) != uids.end())
      CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY,
                        "Entity of A00 field is shared by more than one "
                        "element, eliminated fields have to be element local "
                        "(L2, broken spaces, or interior DOFs)");
  }
 
  // blocks not in A00 elements are stored in data
  size_t mem_size = 0;
  for (auto &d : index) {
    if (d.getElem() == -1) {
      d.setStorage(mem_size, d.getNbCols(), -1);
      mem_size += static_cast<size_t>(d.getNbRows()) * d.getNbCols();
    }
  }
 
  const int nb_elems = storage.elems.size();
  storage.hasPreconditioner = add_preconditioner_block;
  storage.elemData.assign(nb_elems, std::vector<double>());
  storage.elemPrec.assign(add_preconditioner_block ? nb_elems : 0,
                          std::vector<double>());
  storage.elemDense.assign(nb_elems, std::vector<double>());
  storage.maxElemBuffer = 0;
  for (int ei = 0; ei != nb_elems; ++ei) {
    const auto sz = storage.patterns[storage.elems[ei].pattern].elemSize;
    // capacity of dense matrix, which is upper bound of any layout, is
    // reserved, such that buffers are relaid in place, see relayout_pattern
    const auto n = storage.elems[ei].size;
    storage.elemData[ei].reserve(static_cast<size_t>(n) * n);
    storage.elemData[ei].assign(sz, 0.);
    if (add_preconditioner_block) {
      storage.elemPrec[ei].reserve(static_cast<size_t>(n) * n);
      storage.elemPrec[ei].assign(sz, 0.);
    }
    storage.maxElemBuffer = std::max(storage.maxElemBuffer, sz);
  }
 
  // move data to new layout, data is allocated on first use, thus usually
  // nothing is moved
  auto &data = storage.data;
  const bool has_values =
      std::any_of(data.begin(), data.end(), [](double v) { return v != 0.; });
  if (has_values) {
    std::vector<double> new_data(mem_size, 0.);
    for (int i = 0; i != nb_blocks; ++i) {
      auto &d = index[i];
      auto [old_shift, old_ld] = old_layout[i];
      double *dst = d.getElem() >= 0
                        ? storage.elemData[d.getElem()].data() + d.getMatShift()
                        : new_data.data() + d.getMatShift();
      copy_block(dst, d.getLd(), 0, 0, data.data() + old_shift, old_ld,
                 d.getNbRows(), d.getNbCols());
    }
    data.swap(new_data);
  } else {
    std::vector<double>().swap(data);
  }
  storage.dataSize = mem_size;
  storage.ensureAllocated();
  storage.pivots.assign(nb_pivots, 0);
  storage.factorStorage = BlockStorage::FACTOR_NONE;
 
  set_parent_blocks(*block_mat_data_ptr);
 
  // nested matrices share storage, each keeps blocks in its own numbering
  std::array<boost::shared_ptr<BlockStructure>, 4> data_ptrs;
  for (auto r = 0; r != 4; ++r) {
    data_ptrs[r] = boost::make_shared<BlockStructure>();
    data_ptrs[r]->storage = block_mat_data_ptr->storage;
  }
  data_ptrs[3]->hasElems = !storage.elems.empty();
  data_ptrs[3]->numbering = BlockStructure::BLOCK;
 
  for (int i = 0; i != nb_blocks; ++i) {
    auto &d = index[i];
    auto &s = sub[i];
    auto add = [&](auto &m, const SubIdx &r, const SubIdx &c) {
      m->blocks.push_back({d.getMatShift(), d.getLd(), r.glob, c.glob,
                           d.getNbRows(), d.getNbCols(), r.loc, c.loc});
    };
    if (s.schurRow.glob != -1 && s.schurCol.glob != -1)
      add(data_ptrs[0], s.schurRow, s.schurCol);
    if (s.schurRow.glob != -1 && s.blockCol.glob != -1)
      add(data_ptrs[1], s.schurRow, s.blockCol);
    if (s.blockRow.glob != -1 && s.schurCol.glob != -1)
      add(data_ptrs[2], s.blockRow, s.schurCol);
    // blocks with both entities in block sub-problem are element blocks
    if (s.blockRow.glob != -1 && s.blockCol.glob != -1 && d.getElem() == -1)
      CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY, "Block not in element");
  }
  for (auto r = 0; r != 3; ++r)
    data_ptrs[r]->blocks.shrink_to_fit();
 
  data_ptrs[0]->ghostX = schur_vec_x;
  data_ptrs[0]->ghostY = schur_vec_y;
  data_ptrs[1]->ghostX = block_vec_x;
  data_ptrs[1]->ghostY = schur_vec_y;
  data_ptrs[2]->ghostX = schur_vec_x;
  data_ptrs[2]->ghostY = block_vec_y;
  data_ptrs[3]->ghostX = block_vec_x;
  data_ptrs[3]->ghostY = block_vec_y;
 
  if (add_preconditioner_block) {
    data_ptrs[3]->multiplyByPreconditioner = true;
    block_mat_data_ptr->multiplyByPreconditioner = false;
  }
 
  MPI_Comm comm;
  CHKERR PetscObjectGetComm((PetscObject)schur_dm, &comm);
 
  auto create_shell_mat = [&](auto nb_r_loc, auto nb_c_loc, auto nb_r_glob,
                              auto nb_c_glob, auto data_ptr) {
    Mat mat_raw;
    CHKERR MatCreateShell(comm, nb_r_loc, nb_c_loc, nb_r_glob, nb_c_glob,
                          (void *)data_ptr.get(), &mat_raw);
    CHKERR setSchurBlockMatOps(mat_raw);
    return SmartPetscObj<Mat>(mat_raw);
  };
 
  auto schur_nb_global = schur_prb->getNbDofsRow();
  auto block_nb_global = block_prb->getNbDofsRow();
  auto schur_nb_local = schur_prb->getNbLocalDofsRow();
  auto block_nb_local = block_prb->getNbLocalDofsRow();
 
  std::array<SmartPetscObj<Mat>, 4> mats_array;
  mats_array[0] =
      create_shell_mat(schur_nb_local, schur_nb_local, schur_nb_global,
                       schur_nb_global, data_ptrs[0]);
  mats_array[1] =
      create_shell_mat(schur_nb_local, block_nb_local, schur_nb_global,
                       block_nb_global, data_ptrs[1]);
  mats_array[2] =
      create_shell_mat(block_nb_local, schur_nb_local, block_nb_global,
                       schur_nb_global, data_ptrs[2]);
  mats_array[3] =
      create_shell_mat(block_nb_local, block_nb_local, block_nb_global,
                       block_nb_global, data_ptrs[3]);
 
  MOFEM_TAG_AND_LOG("SYNC", Sev::verbose, "NestedSchur")
      << "(0, 0) " << schur_nb_local << " " << schur_nb_global << " "
      << data_ptrs[0]->blocks.size();
  MOFEM_TAG_AND_LOG("SYNC", Sev::verbose, "NestedSchur")
      << "(0, 1) " << schur_nb_local << " " << block_nb_local << " "
      << schur_nb_global << " " << block_nb_global << " "
      << data_ptrs[1]->blocks.size();
  MOFEM_TAG_AND_LOG("SYNC", Sev::verbose, "NestedSchur")
      << "(1, 0) " << block_nb_local << " " << schur_nb_local << " "
      << block_nb_global << " " << schur_nb_global << " "
      << data_ptrs[2]->blocks.size();
  MOFEM_TAG_AND_LOG("SYNC", Sev::verbose, "NestedSchur")
      << "(1, 1) " << block_nb_local << " " << block_nb_global << " "
      << storage.elems.size() << " elements (" << a00.size() << " blocks, "
      << storage.patterns.size() << " patterns) max size "
      << storage.maxElemSize;
 
  MOFEM_LOG_SEVERITY_SYNC(comm, Sev::verbose);
 
  {
    // element storage: listed blocks, dense element matrices, and block
    // elimination estimated from pattern structure only
    size_t listed_d = 0, dense_d = 0, struct_d = 0, buffers_d = 0;
    for (auto &pat : storage.patterns) {
      auto est = pat;
      make_block_layout(est, structural_order(pat));
      listed_d += pat.nbElems * listed_size(pat);
      dense_d += pat.nbElems * static_cast<size_t>(pat.size) * pat.size;
      struct_d += pat.nbElems * est.elemSize;
      buffers_d += pat.nbElems * pat.elemSize;
      MOFEM_TAG_AND_LOG("WORLD", Sev::verbose, "NestedSchur")
          << "A00 pattern (" << pat.nbElems << " elements): "
          << describe_nodes(pat,
                            [&]() {
                              std::vector<int> all(pat.nbNodes);
                              std::iota(all.begin(), all.end(), 0);
                              return all;
                            }())
          << "; listed pairs " << pat.nbListed << " of "
          << pat.nbNodes * pat.nbNodes << "; doubles per element: listed "
          << listed_size(pat) << ", dense "
          << static_cast<size_t>(pat.size) * pat.size
          << ", structural estimate " << est.elemSize << " (eliminated "
          << describe_nodes(est, est.order) << ")";
    }
    // data of elements and blocks with all rows and columns not owned by this
    // process (such blocks are not assembled by this process)
    size_t ghost_elems = 0, ghost_elems_data = 0, ghost_blocks_data = 0;
    for (int ei = 0; ei != nb_elems; ++ei) {
      auto &e = storage.elems[ei];
      bool all_ghost = true;
      for (int k = e.entBegin; k != e.entEnd; ++k)
        if (storage.elemEntities[k].locParent < nb_local_parent)
          all_ghost = false;
      if (all_ghost) {
        ++ghost_elems;
        ghost_elems_data += storage.elemData[ei].size();
      }
    }
    for (auto &d : index)
      if (d.getElem() == -1 && d.getLocRow() >= nb_local_parent &&
          d.getLocCol() >= nb_local_parent)
        ghost_blocks_data += static_cast<size_t>(d.getNbRows()) * d.getNbCols();
 
    constexpr size_t dsz = sizeof(double);
    std::array<size_t, 10> loc_stats = {
        storage.elems.size(),
        buffers_d * dsz,
        listed_d * dsz,
        dense_d * dsz,
        struct_d * dsz,
        mem_size * dsz,
        (add_preconditioner_block ? buffers_d : 0) * dsz,
        ghost_elems,
        ghost_elems_data * dsz,
        ghost_blocks_data * dsz};
    std::array<size_t, 10> sum_stats, max_stats;
    MPI_Allreduce(loc_stats.data(), sum_stats.data(), 10, MPIU_SIZE_T, MPI_SUM,
                  comm);
    MPI_Allreduce(loc_stats.data(), max_stats.data(), 10, MPIU_SIZE_T, MPI_MAX,
                  comm);
    constexpr double mb = 1024. * 1024.;
    MOFEM_TAG_AND_LOG("WORLD", Sev::verbose, "NestedSchur")
        << "A00 elements " << sum_stats[0] << " (max per process "
        << max_stats[0] << "), element buffers " << sum_stats[1] / mb
        << " MB (max per process " << max_stats[1] / mb
        << " MB); listed blocks " << sum_stats[2] / mb
        << " MB, dense element matrices would be " << sum_stats[3] / mb
        << " MB, structural estimate of block elimination " << sum_stats[4] / mb
        << " MB; other blocks data " << sum_stats[5] / mb
        << " MB (max per process " << max_stats[5] / mb
        << " MB); preconditioner blocks " << sum_stats[6] / mb << " MB";
    MOFEM_TAG_AND_LOG("WORLD", Sev::verbose, "NestedSchur")
        << "Not owned (ghost) elements " << sum_stats[7] << " data "
        << sum_stats[8] / mb << " MB, not owned other blocks data "
        << sum_stats[9] / mb << " MB";
  }
 
  auto schur_is = schur_prb->getSubData()->getSmartRowIs();
  auto block_is = block_prb->getSubData()->getSmartRowIs();
 
  storage.nested = true;
 
  return boost::make_shared<NestSchurData>(
 
      mats_array, data_ptrs, block_mat_data_ptr,
      std::make_pair(schur_is, block_is)
 
  );
}
 
std::pair<SmartPetscObj<Mat>, boost::shared_ptr<NestSchurData>>
createSchurNestedMatrix(boost::shared_ptr<NestSchurData> schur_net_data_ptr) {
 
  if (!schur_net_data_ptr)
    CHK_THROW_MESSAGE(MOFEM_DATA_INCONSISTENCY, "No data");
 
  auto [mat_arrays, data_ptrs, block_mat_data_ptr, iss] = *schur_net_data_ptr;
  auto [schur_is, block_is] = iss;
 
  std::array<IS, 2> is_a = {schur_is, block_is};
  std::array<Mat, 4> mats_a = {
 
      mat_arrays[0], mat_arrays[1], mat_arrays[2], mat_arrays[3]
 
  };
 
  MPI_Comm comm;
  CHKERR PetscObjectGetComm((PetscObject)mat_arrays[0], &comm);
 
  Mat mat_raw;
  CHKERR MatCreateNest(
 
      comm, 2, is_a.data(), 2, is_a.data(), mats_a.data(), &mat_raw
 
  );
 
  return std::make_pair(SmartPetscObj<Mat>(mat_raw), schur_net_data_ptr);
}
 
OpSchurAssembleBase *createOpSchurAssembleBegin() {
  return new OpSchurAssembleBegin();
}
 
OpSchurAssembleBase *
createOpSchurAssembleEnd(std::vector<std::string> fields_name,
                         std::vector<boost::shared_ptr<Range>> field_ents,
                         SmartPetscObj<AO> ao, SmartPetscObj<Mat> schur,
                         bool sym_schur, bool symm_op) {
  if (symm_op)
    return new OpSchurAssembleEnd<SchurDSYSV>(fields_name, field_ents, ao,
                                              schur, sym_schur, symm_op);
  else
    return new OpSchurAssembleEnd<SchurDGESV>(fields_name, field_ents, ao,
                                              schur, sym_schur, symm_op);
}
 
OpSchurAssembleBase *createOpSchurZeroRowsAndCols(
    boost::shared_ptr<std::vector<unsigned char>> marker_ptr, double diag_val) {
  return new OpSchurZeroRowsAndCols(marker_ptr, diag_val);
}
 
OpSchurAssembleBase *
createOpSchurAssembleEnd(std::vector<std::string> fields_name,
                         std::vector<boost::shared_ptr<Range>> field_ents,
                         std::vector<SmartPetscObj<AO>> sequence_of_aos,
                         std::vector<SmartPetscObj<Mat>> sequence_of_mats,
                         std::vector<bool> sym_schur, bool symm_op,
                         boost::shared_ptr<BlockStructure> diag_blocks) {
  return createOpSchurAssembleEnd(
      fields_name, field_ents, sequence_of_aos.back(), sequence_of_mats.back(),
      sym_schur.back(), symm_op);
}
 
OpSchurAssembleBase *
createOpSchurAssembleEnd(std::vector<std::string> fields_name,
                         std::vector<boost::shared_ptr<Range>> field_ents,
                         std::vector<SmartPetscObj<AO>> sequence_of_aos,
                         std::vector<SmartPetscObj<Mat>> sequence_of_mats,
                         std::vector<bool> sym_schur,
                         std::vector<double> diag_eps, bool symm_op,
                         boost::shared_ptr<BlockStructure> diag_blocks) {
  return createOpSchurAssembleEnd(
      fields_name, field_ents, sequence_of_aos.back(), sequence_of_mats.back(),
      sym_schur.back(), symm_op);
}
 
MoFEMErrorCode setSchurA00MatSolvePC(SmartPetscObj<PC> pc) {
  MoFEMFunctionBegin;
 
  auto apply = [](PC pc, Vec f, Vec x) {
    MoFEMFunctionBeginHot;
    Mat A, P;
    CHKERR PCGetOperators(pc, &A, &P);
    CHKERR MatSolve(P, f, x);
    MoFEMFunctionReturnHot(0);
  };
 
  auto apply_transpose = [](PC pc, Vec f, Vec x) {
    MoFEMFunctionBeginHot;
    Mat A, P;
    CHKERR PCGetOperators(pc, &A, &P);
    CHKERR MatSolveTranspose(P, f, x);
    MoFEMFunctionReturnHot(0);
  };
 
  CHKERR PCSetType(pc, PCSHELL);
  CHKERR PCShellSetApply(pc, apply);
  CHKERR PCShellSetApplyTranspose(pc, apply_transpose);
  CHKERR PCShellSetName(pc, "MoFEMSchurBlockPC");
 
  MoFEMFunctionReturn(0);
}
 
// This is used to assemble local element matrices for Schur complement
// and matrix for KSP
template <>
MoFEMErrorCode
MatSetValues<SchurElemMats>(Mat M, const EntitiesFieldData::EntData &row_data,
                            const EntitiesFieldData::EntData &col_data,
                            const MatrixDouble &mat, InsertMode iora) {
  return SchurElemMats::MatSetValues(M, row_data, col_data, mat, iora);
}
 
MoFEMErrorCode
schur_mat_set_values_wrap(Mat M, const EntitiesFieldData::EntData &row_data,
                          const EntitiesFieldData::EntData &col_data,
                          const MatrixDouble &mat, InsertMode iora) {
  return MatSetValues<AssemblyTypeSelector<PETSC>>(M, row_data, col_data, mat,
                                                   iora);
}
 
// Is assumed that standard PETSc assembly works for matrices used by KSP
SchurBackendMatSetValuesPtr::MatSetValuesPtr
    SchurBackendMatSetValuesPtr::matSetValuesPtr = schur_mat_set_values_wrap;
 
// We assemble matrix for KSP and store local matrices for Schur complement.
// Schur complement is calculated and assembled in OpSchurAssembleEnd.
MoFEMErrorCode
SchurElemMats::MatSetValues(Mat M, const EntitiesFieldData::EntData &row_data,
                            const EntitiesFieldData::EntData &col_data,
                            const MatrixDouble &mat, InsertMode iora) {
  MoFEMFunctionBegin;
  CHKERR assembleStorage(row_data, col_data, mat, iora);
  CHKERR SchurBackendMatSetValuesPtr::matSetValuesPtr(M, row_data, col_data,
                                                      mat, iora);
  MoFEMFunctionReturn(0);
}
 
// All is now wrapped in specialisation of
// MatSetValues<AssemblyTypeSelector<SCHUR>>
template <>
MoFEMErrorCode MatSetValues<AssemblyTypeSelector<SCHUR>>(
    Mat M, const EntitiesFieldData::EntData &row_data,
    const EntitiesFieldData::EntData &col_data, const MatrixDouble &mat,
    InsertMode iora) {
  return MatSetValues<SchurElemMats>(M, row_data, col_data, mat, iora);
}
 
// Standard (PETSc) assembly block matrices do noe work
template <>
MoFEMErrorCode
MatSetValues<BlockStructure>(Mat M, const EntitiesFieldData::EntData &row_data,
                             const EntitiesFieldData::EntData &col_data,
                             const MatrixDouble &mat, InsertMode iora) {
  return shell_block_mat_asmb_wrap(M, row_data, col_data, mat, iora);
}
 
struct SchurElemMatsBlock : public SchurElemMats {
 
  static MoFEMErrorCode MatSetValues(Mat M,
                                     const EntitiesFieldData::EntData &row_data,
                                     const EntitiesFieldData::EntData &col_data,
                                     const MatrixDouble &mat, InsertMode iora);
};
 
SchurBackendMatSetValuesPtr::MatSetValuesPtr
    SchurBackendMatSetValuesPtr::matSetValuesBlockPtr =
        shell_block_mat_asmb_wrap;
 
MoFEMErrorCode
SchurElemMatsBlock::MatSetValues(Mat M,
                                 const EntitiesFieldData::EntData &row_data,
                                 const EntitiesFieldData::EntData &col_data,
                                 const MatrixDouble &mat, InsertMode iora) {
  MoFEMFunctionBegin;
  CHKERR assembleStorage(row_data, col_data, mat, iora);
  CHKERR SchurBackendMatSetValuesPtr::matSetValuesBlockPtr(M, row_data,
                                                           col_data, mat, iora);
  MoFEMFunctionReturn(0);
}
 
template <>
MoFEMErrorCode
MatSetValues<SchurElemMatsBlock>(Mat M,
                                 const EntitiesFieldData::EntData &row_data,
                                 const EntitiesFieldData::EntData &col_data,
                                 const MatrixDouble &mat, InsertMode iora) {
  return SchurElemMatsBlock::MatSetValues(M, row_data, col_data, mat, iora);
}
 
struct SchurElemMatsPreconditionedBlock : public SchurElemMats {
 
  static MoFEMErrorCode MatSetValues(Mat M,
                                     const EntitiesFieldData::EntData &row_data,
                                     const EntitiesFieldData::EntData &col_data,
                                     const MatrixDouble &mat, InsertMode iora);
};
 
SchurBackendMatSetValuesPtr::MatSetValuesPtr
    SchurBackendMatSetValuesPtr::matSetValuesPreconditionedBlockPtr =
        shell_block_preconditioner_mat_asmb_wrap;
 
MoFEMErrorCode SchurElemMatsPreconditionedBlock::MatSetValues(
    Mat M, const EntitiesFieldData::EntData &row_data,
    const EntitiesFieldData::EntData &col_data, const MatrixDouble &mat,
    InsertMode iora) {
  MoFEMFunctionBegin;
  CHKERR assembleStorage(row_data, col_data, mat, iora);
  CHKERR SchurBackendMatSetValuesPtr::matSetValuesPreconditionedBlockPtr(
      M, row_data, col_data, mat, iora);
  MoFEMFunctionReturn(0);
}
 
template <>
MoFEMErrorCode MatSetValues<SchurElemMatsPreconditionedBlock>(
    Mat M, const EntitiesFieldData::EntData &row_data,
    const EntitiesFieldData::EntData &col_data, const MatrixDouble &mat,
    InsertMode iora) {
  return SchurElemMatsPreconditionedBlock::MatSetValues(M, row_data, col_data,
                                                        mat, iora);
}
 
MoFEMErrorCode
schurSwitchPreconditioner(boost::shared_ptr<BlockStructure> block_mat_data) {
  MoFEMFunctionBegin;
  if (block_mat_data->multiplyByPreconditioner) {
    block_mat_data->multiplyByPreconditioner = false;
  } else {
    if (!block_mat_data->storage->hasPreconditioner) {
      SETERRQ(PETSC_COMM_SELF, MOFEM_DATA_INCONSISTENCY,
              "preconditioner blocks not set");
    }
    block_mat_data->multiplyByPreconditioner = true;
  }
  MoFEMFunctionReturn(0);
}
 
MoFEMErrorCode
schurSaveBlockMesh(boost::shared_ptr<BlockStructure> block_mat_data,
                   std::string filename) {
  MoFEMFunctionBegin;
 
  moab::Core core;
  moab::Interface &moab = core;
 
  ReadUtilIface *iface;
  CHKERR moab.query_interface(iface);
 
  size_t size = 0;
  for_each_block(*block_mat_data,
                 [&](double *, int, int, int, int, int, int, int) { ++size; });
 
  EntityHandle starting_vertex;
  vector<double *> arrays_coord;
  CHKERR iface->get_node_coords(3, 4 * size, 0, starting_vertex, arrays_coord);
  EntityHandle starting_handle;
  EntityHandle *array;
  CHKERR iface->get_element_connect(size, 4, MBQUAD, 0, starting_handle, array);
 
  double def_val_nrm2 = 0;
  Tag th_nrm2;
  CHKERR moab.tag_get_handle("nrm2", 1, MB_TYPE_DOUBLE, th_nrm2,
                             MB_TAG_CREAT | MB_TAG_DENSE, &def_val_nrm2);
 
  int def_val_mat_shift = 0;
  Tag th_mat_shift;
  CHKERR moab.tag_get_handle("mat_shift", 1, MB_TYPE_INTEGER, th_mat_shift,
                             MB_TAG_CREAT | MB_TAG_DENSE, &def_val_mat_shift);
 
  block_mat_data->storage->ensureAllocated();
 
  int i = 0;
  for_each_block(*block_mat_data, [&](double *ptr, int ld, int row, int col,
                                      int nb_rows, int nb_cols, int, int) {
    // q0
    arrays_coord[0][4 * i + 0] = row;
    arrays_coord[1][4 * i + 0] = col;
    arrays_coord[2][4 * i + 0] = 0;
 
    // q1
    arrays_coord[0][4 * i + 1] = row + nb_rows;
    arrays_coord[1][4 * i + 1] = col;
    arrays_coord[2][4 * i + 1] = 0;
 
    // q2
    arrays_coord[0][4 * i + 2] = row + nb_rows;
    arrays_coord[1][4 * i + 2] = col + nb_cols;
    arrays_coord[2][4 * i + 2] = 0;
 
    // q3
    arrays_coord[0][4 * i + 3] = row;
    arrays_coord[1][4 * i + 3] = col + nb_cols;
    arrays_coord[2][4 * i + 3] = 0;
 
    // ele conn
    array[4 * i + 0] = starting_vertex + 4 * i + 0;
    array[4 * i + 1] = starting_vertex + 4 * i + 1;
    array[4 * i + 2] = starting_vertex + 4 * i + 2;
    array[4 * i + 3] = starting_vertex + 4 * i + 3;
 
    double nrm2 = 0;
    for (int r = 0; r != nb_rows; ++r) {
      auto row_nrm2 =
          cblas_dnrm2(nb_cols, ptr + static_cast<size_t>(r) * ld, 1);
      nrm2 += row_nrm2 * row_nrm2;
    }
    nrm2 = std::sqrt(nrm2);
    int mat_shift = i;
    EntityHandle ele = starting_handle + i;
    CHKERR moab.tag_set_data(th_nrm2, &ele, 1, &nrm2);
    CHKERR moab.tag_set_data(th_mat_shift, &ele, 1, &mat_shift);
 
    ++i;
  });
 
  CHKERR iface->update_adjacencies(starting_handle, size, 4, array);
  CHKERR moab.write_file(filename.c_str(), "VTK", "");
 
  MoFEMFunctionReturn(0);
}
 
} // namespace MoFEM