dune/tectonic/tests/nodalweightstest.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <iostream>
+#include <exception>
+#include <fstream>
+
+#include <dune/grid/uggrid.hh>
+
+#include <dune/fufem/assemblers/assembler.hh>
+#include <dune/fufem/assemblers/localassemblers/neumannboundaryassembler.hh>
+#include <dune/fufem/functions/constantfunction.hh>
+#include <dune/fufem/functionspacebases/p1nodalbasis.hh>
+
+#include <dune/contact/projections/normalprojection.hh>
+#include <dune/contact/common/couplingpair.hh>
+#include <dune/contact/assemblers/dualmortarcoupling.hh>
+
+#include "../utils/debugutils.hh"
+#include "common.hh"
+
+#include "../nodalweights.hh"
+
+const int dim = 2;
+const int n = 5;
+const bool simplexGrid = true;
+
+const std::string path = "";
+const std::string outputFile = "nodalweightstest.log";
+
+#if HAVE_UG
+    using GridType = typename Dune::UGGrid<dim>;
+#else
+#error No UG!
+#endif
+    using LevelView = typename GridType::LevelGridView;
+    using LeafView = typename GridType::LeafGridView;
+    using LevelBoundaryPatch = BoundaryPatch<LevelView>;
+    using LeafBoundaryPatch = BoundaryPatch<LeafView>;
+
+    using c_type = double;
+    using GlobalCoords = Dune::FieldVector<c_type, dim>;
+    using BlockVector = Dune::BlockVector<GlobalCoords>;
+
+    using ScalarVector = Dune::FieldVector<c_type, 1>;
+    using ScalarBlockVector = Dune::BlockVector<ScalarVector>;
+
+    using CouplingPair = Dune::Contact::CouplingPair<GridType, GridType, c_type>;
+    using CouplingType = Dune::Contact::DualMortarCoupling<c_type, GridType>;
+
+int main(int argc, char *argv[]) { try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    std::ofstream out(path + outputFile);
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to outputFile
+
+    std::cout << "------------------------" << std::endl;
+    std::cout << "-- NodalWeights Test: --" << std::endl;
+    std::cout << "------------------------" << std::endl << std::endl;
+
+    // building grids
+    std::vector<std::shared_ptr<GridType>> grids(2);
+
+    GlobalCoords lowerLeft0({0, 0});
+    GlobalCoords upperRight0({2, 1});
+    buildGrid(lowerLeft0, upperRight0, n, grids[0]);
+
+    GlobalCoords lowerLeft1({0, 1});
+    GlobalCoords upperRight1({2, 2});
+    buildGrid(lowerLeft1, upperRight1, n, grids[1]);
+
+    // writing grids
+    for (size_t i=0; i<grids.size(); i++) {
+        const auto& levelView = grids[i]->levelGridView(0);
+        writeToVTK(levelView, path, "body_" + std::to_string(i) + "_level0");
+    }
+
+    // compute coupling boundaries
+    LevelView gridView0 = grids[0]->levelGridView(0);
+    LevelView gridView1 = grids[1]->levelGridView(0);
+    LevelBoundaryPatch upper(gridView0);
+    LevelBoundaryPatch lower(gridView1);
+
+    lower.insertFacesByProperty([&](typename LevelView::Intersection const &in) {
+        return xyBetween(lowerLeft1, {upperRight1[0], lowerLeft1[1]}, in.geometry().center());
+    });
+
+    upper.insertFacesByProperty([&](typename LevelView::Intersection const &in) {
+        return xyBetween({lowerLeft0[0], upperRight0[1]}, upperRight0, in.geometry().center());
+    });
+
+    // set contact coupling
+    Dune::Contact::NormalProjection<LeafBoundaryPatch> contactProjection;
+
+    CouplingPair coupling;
+    coupling.set(0, 1, upper, lower, 0.1, CouplingPair::CouplingType::STICK_SLIP, contactProjection, nullptr);
+
+    double coveredArea_ = 0.8;
+    CouplingType contactCoupling;
+    contactCoupling.setGrids(*grids[0], *grids[1]);
+    contactCoupling.setupContactPatch(*coupling.patch0(),*coupling.patch1());
+    contactCoupling.gridGlueBackend_ = coupling.backend();
+    contactCoupling.setCoveredArea(coveredArea_);
+    contactCoupling.setup();
+
+    using Basis = P1NodalBasis<LevelView, c_type>;
+    Basis basis0(grids[0]->levelGridView(0));
+    Basis basis1(grids[1]->levelGridView(0));
+
+    printBasisDofLocation(basis0);
+    printBasisDofLocation(basis1);
+
+    ScalarBlockVector nWeights0, nWeights1;
+    NodalWeights<Basis, Basis> nodalWeights(basis0, basis1);
+    nodalWeights.assemble(*contactCoupling.getGlue(), nWeights0, nWeights1, true);
+
+    ScalarBlockVector weights0;
+    {
+        Assembler<Basis, Basis> assembler(basis0, basis0);
+        NeumannBoundaryAssembler<GridType, typename ScalarBlockVector::block_type> boundaryAssembler(
+                     std::make_shared<ConstantFunction<
+                     GlobalCoords, typename ScalarBlockVector::block_type>>(1));
+             assembler.assembleBoundaryFunctional(boundaryAssembler, weights0, upper);
+    }
+
+    ScalarBlockVector weights1;
+    {
+        Assembler<Basis, Basis> assembler(basis1, basis1);
+        NeumannBoundaryAssembler<GridType, typename ScalarBlockVector::block_type> boundaryAssembler(
+                     std::make_shared<ConstantFunction<
+                     GlobalCoords, typename ScalarBlockVector::block_type>>(1));
+             assembler.assembleBoundaryFunctional(boundaryAssembler, weights1, lower);
+    }
+
+    print(weights0, "assembled weights0: ");
+    print(nWeights0, "nWeights0: ");
+
+    print(weights1, "assembled weights1: ");
+    print(nWeights1, "nWeights1: ");
+
+    bool sizePassed = true;
+    if (weights0.size() != nWeights0.size() | weights1.size() != nWeights1.size()) {
+        sizePassed = false;
+    }
+
+    bool entriesPassed0 = true;
+    for (size_t i=0; i<weights0.size(); i++) {
+        entriesPassed0 = entriesPassed0 & isClose(weights0[i], nWeights0[i]);
+    }
+
+    bool entriesPassed1 = true;
+    for (size_t i=0; i<weights1.size(); i++) {
+        entriesPassed1 = entriesPassed1 & isClose(weights1[i], nWeights1[i]);
+    }
+
+    bool passed = sizePassed & entriesPassed0 & entriesPassed1;
+
+    std::cout << "Overall, the test " << (passed ? "was successful!" : "failed!") << std::endl;
+
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+    return passed ? 0 : 1;
+
+} catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+} catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+} // end try
+} // end main

dune/tectonic/tests/nonoverlappingcouplingtest.cc

+// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
+// vi: set et ts=4 sw=2 sts=2:
+#include "config.h"
+
+#include <array>
+
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/stdstreams.hh>
+#include <iostream>
+
+#include <dune/common/fvector.hh>
+#include <dune/grid/yaspgrid.hh>
+#include <dune/grid/geometrygrid.hh>
+#include <dune/geometry/quadraturerules.hh>
+
+#include <dune/grid-glue/extractors/codim1extractor.hh>
+
+#include <dune/grid-glue/merging/contactmerge.hh>
+#include <dune/grid-glue/gridglue.hh>
+
+#include <dune/grid-glue/test/couplingtest.hh>
+#include <dune/grid-glue/test/communicationtest.hh>
+
+using namespace Dune;
+using namespace Dune::GridGlue;
+
+template<typename GridView>
+typename Dune::GridGlue::Codim1Extractor<GridView>::Predicate
+makeVerticalFacePredicate(double sliceCoord)
+{
+  using Element = typename GridView::Traits::template Codim<0>::Entity;
+  auto predicate = [sliceCoord](const Element& element, unsigned int face) -> bool {
+    const int dim = GridView::dimension;
+    const auto& refElement = Dune::ReferenceElements<double, dim>::general(element.type());
+
+    int numVertices = refElement.size(face, 1, dim);
+
+    for (int i=0; i<numVertices; i++)
+      if ( std::abs(element.geometry().corner(refElement.subEntity(face,1,i,dim))[0] - sliceCoord) > 1e-6 )
+        return false;
+
+    return true;
+  };
+  return predicate;
+}
+
+/** \brief trafo used for yaspgrids */
+template<int dim, typename ctype>
+class ShiftTrafo
+  : public AnalyticalCoordFunction< ctype, dim, dim, ShiftTrafo<dim,ctype> >
+{
+  double shift;
+public:
+  ShiftTrafo(double x) : shift(x) {};
+
+  //! evaluate method for global mapping
+  void evaluate ( const Dune::FieldVector<ctype, dim> &x, Dune::FieldVector<ctype, dim> &y ) const
+  {
+    y = x;
+    y[0] += shift;
+  }
+};
+
+template <int dim>
+void testMatchingCubeGrids()
+{
+
+  // ///////////////////////////////////////
+  //   Make two cube grids
+  // ///////////////////////////////////////
+
+  using GridType = Dune::YaspGrid<dim, Dune::EquidistantOffsetCoordinates<double, dim> >;
+
+  std::array<int, dim> elements;
+  elements.fill(8);
+  FieldVector<double,dim> lower(0);
+  FieldVector<double,dim> upper(1);
+
+  GridType cubeGrid0(lower, upper, elements);
+
+  lower[0] += 1;
+  upper[0] += 1;
+
+  GridType cubeGrid1(lower, upper, elements);
+
+
+  // ////////////////////////////////////////
+  //   Set up coupling at their interface
+  // ////////////////////////////////////////
+
+  typedef typename GridType::LevelGridView DomGridView;
+  typedef typename GridType::LevelGridView TarGridView;
+
+  typedef Codim1Extractor<DomGridView> DomExtractor;
+  typedef Codim1Extractor<TarGridView> TarExtractor;
+
+  const typename DomExtractor::Predicate domdesc = makeVerticalFacePredicate<DomGridView>(1);
+  const typename TarExtractor::Predicate tardesc = makeVerticalFacePredicate<TarGridView>(1);
+
+  auto domEx = std::make_shared<DomExtractor>(cubeGrid0.levelGridView(0), domdesc);
+  auto tarEx = std::make_shared<TarExtractor>(cubeGrid1.levelGridView(0), tardesc);
+
+  typedef Dune::GridGlue::GridGlue<DomExtractor,TarExtractor> GlueType;
+
+  // Testing with ContactMerge
+  typedef ContactMerge<dim,double> ContactMergeImpl;
+
+  auto contactMerger = std::make_shared<ContactMergeImpl>(0.01);
+  GlueType contactGlue(domEx, tarEx, contactMerger);
+
+  contactGlue.build();
+
+  std::cout << "Gluing successful, " << contactGlue.size() << " remote intersections found!" << std::endl;
+  assert(contactGlue.size() > 0);
+
+  // ///////////////////////////////////////////
+  //   Test the coupling
+  // ///////////////////////////////////////////
+
+  testCoupling(contactGlue);
+  testCommunication(contactGlue);
+}
+
+
+template <int dim>
+void testNonMatchingCubeGrids()
+{
+
+  // ///////////////////////////////////////
+  //   Make two cube grids
+  // ///////////////////////////////////////
+
+  using GridType = Dune::YaspGrid<dim, Dune::EquidistantOffsetCoordinates<double, dim> >;
+
+  std::array<int, dim> elements;
+  elements.fill(8);
+  FieldVector<double,dim> lower(0);
+  FieldVector<double,dim> upper(1);
+
+  GridType cubeGrid0(lower, upper, elements);
+
+  elements.fill(10);
+  lower[0] += 1;
+  upper[0] += 1;
+
+  GridType cubeGrid1(lower, upper, elements);
+
+
+  // ////////////////////////////////////////
+  //   Set up coupling at their interface
+  // ////////////////////////////////////////
+
+  typedef typename GridType::LevelGridView DomGridView;
+  typedef typename GridType::LevelGridView TarGridView;
+
+  typedef Codim1Extractor<DomGridView> DomExtractor;
+  typedef Codim1Extractor<TarGridView> TarExtractor;
+
+  const typename DomExtractor::Predicate domdesc = makeVerticalFacePredicate<DomGridView>(1);
+  const typename TarExtractor::Predicate tardesc = makeVerticalFacePredicate<TarGridView>(1);
+
+  auto domEx = std::make_shared<DomExtractor>(cubeGrid0.levelGridView(0), domdesc);
+  auto tarEx = std::make_shared<TarExtractor>(cubeGrid1.levelGridView(0), tardesc);
+
+  typedef Dune::GridGlue::GridGlue<DomExtractor,TarExtractor> GlueType;
+
+  // Testing with ContactMerge
+  typedef ContactMerge<dim,double> ContactMergeImpl;
+
+  auto contactMerger = std::make_shared<ContactMergeImpl>(0.01);
+  GlueType contactGlue(domEx, tarEx, contactMerger);
+
+  contactGlue.build();
+
+  std::cout << "Gluing successful, " << contactGlue.size() << " remote intersections found!" << std::endl;
+  assert(contactGlue.size() > 0);
+
+  // ///////////////////////////////////////////
+  //   Test the coupling
+  // ///////////////////////////////////////////
+
+  testCoupling(contactGlue);
+  testCommunication(contactGlue);
+}
+
+
+template<int dim, bool par>
+class MeshGenerator
+{
+  bool tar;
+public:
+  MeshGenerator(bool b) : tar(b) {};
+
+  using GridType = Dune::YaspGrid<dim, Dune::EquidistantOffsetCoordinates<double, dim> >;
+
+  std::shared_ptr<GridType> generate()
+  {
+    std::array<int, dim> elements;
+    elements.fill(8);
+    FieldVector<double,dim> lower(0);
+    FieldVector<double,dim> upper(1);
+
+    if (tar)
+    {
+      elements.fill(10);
+      lower[0] += 1;
+      upper[0] += 1;
+    }
+
+    return std::make_shared<GridType>(lower, upper, elements);
+  }
+};
+
+
+template<int dim>
+class MeshGenerator<dim, true>
+{
+  bool tar;
+public:
+  MeshGenerator(bool b) : tar(b) {};
+
+  typedef YaspGrid<dim> HostGridType;
+  typedef GeometryGrid<HostGridType, ShiftTrafo<dim,double> > GridType;
+
+  std::shared_ptr<GridType> generate()
+  {
+    std::array<int,dim> elements;
+    std::fill(elements.begin(), elements.end(), 8);
+    std::bitset<dim> periodic(0);
+    FieldVector<double,dim> size(1);
+    int overlap = 1;
+    double shift = 0.0;
+
+    if (tar)
+    {
+      std::fill(elements.begin(), elements.end(), 10);
+      shift = 1.0;
+    }
+
+    HostGridType * hostgridp = new HostGridType(
+      size, elements, periodic, overlap
+#if HAVE_MPI
+      , MPI_COMM_WORLD
+#endif
+      );
+    ShiftTrafo<dim,double> * trafop = new ShiftTrafo<dim,double>(shift);
+    return std::make_shared<GridType>(*hostgridp, *trafop);
+  }
+};
+
+
+template <int dim, class DomGen, class TarGen>
+void testParallelCubeGrids()
+{
+  // ///////////////////////////////////////
+  //   Make two cube grids
+  // ///////////////////////////////////////
+
+  typedef typename DomGen::GridType GridType0;
+  typedef typename TarGen::GridType GridType1;
+
+  DomGen domGen(0);
+  TarGen tarGen(1);
+
+  double slice = 1.0;
+
+  std::shared_ptr<GridType0> cubeGrid0 = domGen.generate();
+  std::shared_ptr<GridType1> cubeGrid1 = tarGen.generate();
+
+  // ////////////////////////////////////////
+  //   Set up Traits
+  // ////////////////////////////////////////
+
+  typedef typename GridType0::LevelGridView DomGridView;
+  typedef typename GridType1::LevelGridView TarGridView;
+
+  typedef Codim1Extractor<DomGridView> DomExtractor;
+  typedef Codim1Extractor<TarGridView> TarExtractor;
+
+  const typename DomExtractor::Predicate domdesc = makeVerticalFacePredicate<DomGridView>(slice);
+  const typename TarExtractor::Predicate tardesc = makeVerticalFacePredicate<TarGridView>(slice);
+
+  auto domEx = std::make_shared<DomExtractor>(cubeGrid0->levelGridView(0), domdesc);
+  auto tarEx = std::make_shared<TarExtractor>(cubeGrid1->levelGridView(0), tardesc);
+
+  // ////////////////////////////////////////
+  //   Set up coupling at their interface
+  // ////////////////////////////////////////
+
+  typedef Dune::GridGlue::GridGlue<DomExtractor,TarExtractor> GlueType;
+
+  // Testing with ContactMerge
+  typedef ContactMerge<dim,double> ContactMergeImpl;
+
+  auto contactMerger = std::make_shared<ContactMergeImpl>(0.01);
+  GlueType contactGlue(domEx, tarEx, contactMerger);
+
+  contactGlue.build();
+
+  std::cout << "Gluing successful, " << contactGlue.size() << " remote intersections found!" << std::endl;
+  assert(contactGlue.size() > 0);
+
+  // ///////////////////////////////////////////
+  //   Test the coupling
+  // ///////////////////////////////////////////
+
+  testCoupling(contactGlue);
+  testCommunication(contactGlue);
+}
+
+#if HAVE_MPI
+void eh( MPI_Comm *comm, int *err, ... )
+{
+  int len = 1024;
+  char error_txt[len];
+
+  MPI_Error_string(*err, error_txt, &len);
+  assert(len <= 1024);
+  DUNE_THROW(Dune::Exception, "MPI ERROR -- " << error_txt);
+}
+#endif // HAVE_MPI
+
+int main(int argc, char *argv[])
+{
+  Dune::MPIHelper::instance(argc, argv);
+  Dune::dinfo.attach(std::cout);
+
+#if HAVE_MPI
+  MPI_Errhandler errhandler;
+  MPI_Comm_create_errhandler(eh, &errhandler);
+  MPI_Comm_set_errhandler(MPI_COMM_WORLD, errhandler);
+#endif
+
+  // 2d Tests
+  typedef MeshGenerator<2,false>  Seq;
+  typedef MeshGenerator<2,true>   Par;
+
+  // Test two unit squares
+  std::cout << "==== 2D hybrid =============================================\n";
+  testMatchingCubeGrids<2>();
+  std::cout << "============================================================\n";
+  testNonMatchingCubeGrids<2>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<2,Seq,Seq>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<2,Par,Seq>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<2,Seq,Par>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<2,Par,Par>();
+  std::cout << "============================================================\n";
+
+  // 3d Tests
+#if ! HAVE_MPI
+  typedef MeshGenerator<3,false>  Seq3d;
+  typedef MeshGenerator<3,true>   Par3d;
+
+  // Test two unit cubes
+  std::cout << "==== 3D hybrid =============================================\n";
+  testMatchingCubeGrids<3>();
+  std::cout << "============================================================\n";
+  testNonMatchingCubeGrids<3>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<3,Seq3d,Seq3d>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<3,Par3d,Seq3d>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<3,Seq3d,Par3d>();
+  std::cout << "============================================================\n";
+  testParallelCubeGrids<3,Par3d,Par3d>();
+  std::cout << "============================================================\n";
+#endif // HAVE_MPI
+
+  return 0;
+}

dune/tectonic/tests/solverfactorytest.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#define MY_DIM 2
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <exception>
+
+#include <dune/common/exceptions.hh>
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/stdstreams.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/function.hh>
+#include <dune/common/bitsetvector.hh>
+#include <dune/common/parametertree.hh>
+#include <dune/common/parametertreeparser.hh>
+
+#include <dune/fufem/formatstring.hh>
+
+#include <dune/tnnmg/functionals/boxconstrainedquadraticfunctional.hh>
+#include <dune/tnnmg/functionals/bcqfconstrainedlinearization.hh>
+#include <dune/tnnmg/localsolvers/scalarobstaclesolver.hh>
+#include <dune/tnnmg/iterationsteps/tnnmgstep.hh>
+
+#include <dune/tectonic/geocoordinate.hh>
+
+#include "assemblers.hh"
+#include "gridselector.hh"
+#include "explicitgrid.hh"
+#include "explicitvectors.hh"
+
+#include "data-structures/enumparser.hh"
+#include "data-structures/enums.hh"
+#include "data-structures/contactnetwork.hh"
+#include "data-structures/matrices.hh"
+#include "data-structures/program_state.hh"
+
+#include "io/vtk.hh"
+
+#include "spatial-solving/tnnmg/linesearchsolver.hh"
+#include "spatial-solving/preconditioners/nbodycontacttransfer.hh"
+
+#include "factories/stackedblocksfactory.hh"
+
+#include "time-stepping/rate.hh"
+#include "time-stepping/state.hh"
+#include "time-stepping/updaters.hh"
+
+#include "utils/tobool.hh"
+#include "utils/debugutils.hh"
+
+
+const int dim = MY_DIM;
+
+Dune::ParameterTree parset;
+size_t bodyCount;
+
+std::vector<BodyState<Vector, ScalarVector>* > bodyStates;
+std::vector<Vector> u;
+std::vector<Vector> v;
+std::vector<Vector> a;
+std::vector<ScalarVector> alpha;
+std::vector<ScalarVector> weightedNormalStress;
+double relativeTime;
+double relativeTau;
+size_t timeStep = 0;
+size_t timeSteps = 0;
+
+const std::string path = "";
+const std::string outputFile = "solverfactorytest.log";
+
+std::vector<std::vector<double>> allReductionFactors;
+
+template<class IterationStepType, class NormType, class ReductionFactorContainer>
+Dune::Solvers::Criterion reductionFactorCriterion(
+      const IterationStepType& iterationStep,
+      const NormType& norm,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = norm.diff(*lastIterate, *iterationStep.getIterate());
+        double convRate = (normOfOldCorrection > 0) ? normOfCorrection / normOfOldCorrection : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+
+template<class IterationStepType, class Functional, class ReductionFactorContainer>
+Dune::Solvers::Criterion energyCriterion(
+      const IterationStepType& iterationStep,
+      const Functional& f,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = std::abs(f(*lastIterate) - f(*iterationStep.getIterate())); //norm.diff(*lastIterate, *iterationStep.getIterate());
+
+        double convRate = (normOfOldCorrection != 0.0) ? 1.0 - (normOfCorrection / normOfOldCorrection) : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+template <class ContactNetwork>
+void solveProblem(const ContactNetwork& contactNetwork,
+                  const Matrix& mat, const Vector& rhs, Vector& x,
+                  const BitVector& ignore, const Vector& lower, const Vector& upper, bool initial = false) {
+
+    using Solver = typename Dune::Solvers::LoopSolver<Vector, BitVector>;
+    using Norm =  EnergyNorm<Matrix, Vector>;
+
+    //using LocalSolver = LocalBisectionSolver;
+    //using Linearization = Linearization<Functional, BitVector>;
+
+    /*print(ignore, "ignore:");
+    for (size_t i=0; i<x.size(); i++) {
+        std::cout << x[i] << std::endl;
+    }*/
+
+    // set up reference solver
+    Vector refX = x;
+    using ContactFunctional = Dune::TNNMG::BoxConstrainedQuadraticFunctional<Matrix&, Vector&, Vector&, Vector&, double>;
+    auto I = ContactFunctional(mat, rhs, lower, upper);
+
+    std::cout << "Energy start iterate: " << I(x) << std::endl;
+
+    using LocalSolver = Dune::TNNMG::ScalarObstacleSolver;
+    auto localSolver = Dune::TNNMG::gaussSeidelLocalSolver(LocalSolver());
+
+    using NonlinearSmoother = Dune::TNNMG::NonlinearGSStep<ContactFunctional, decltype(localSolver), BitVector>;
+    auto nonlinearSmoother = std::make_shared<NonlinearSmoother>(I, refX, localSolver);
+
+    using Linearization = Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<ContactFunctional, BitVector>;
+    using DefectProjection = Dune::TNNMG::ObstacleDefectProjection;
+    using Step = Dune::TNNMG::TNNMGStep<ContactFunctional, BitVector, Linearization, DefectProjection, LocalSolver>;
+
+    // set multigrid solver
+    auto smoother = TruncatedBlockGSStep<Matrix, Vector>();
+
+    using TransferOperator = NBodyContactTransfer<ContactNetwork, Vector>;
+    using TransferOperators = std::vector<std::shared_ptr<TransferOperator>>;
+
+    TransferOperators transfer(contactNetwork.nLevels()-1);
+    for (size_t i=0; i<transfer.size(); i++) {
+        transfer[i] = std::make_shared<TransferOperator>();
+        transfer[i]->setup(contactNetwork, i, i+1);
+    }
+
+    // Remove any recompute filed so that initially the full transferoperator is assembled
+    for (size_t i=0; i<transfer.size(); i++)
+        std::dynamic_pointer_cast<TruncatedMGTransfer<Vector> >(transfer[i])->setRecomputeBitField(nullptr);
+
+    auto linearMultigridStep = std::make_shared<Dune::Solvers::MultigridStep<Matrix, Vector> >();
+    linearMultigridStep->setMGType(1, 3, 3);
+    linearMultigridStep->setSmoother(&smoother);
+    linearMultigridStep->setTransferOperators(transfer);
+
+    int mu = parset.get<int>("solver.tnnmg.main.multi"); // #multigrid steps in Newton step
+    auto step = Step(I, refX, nonlinearSmoother, linearMultigridStep, mu, DefectProjection(), LocalSolver());
+
+    // compute reference solution with generic functional and solver
+    auto norm = Norm(mat);
+
+    auto refSolver = Solver(step, parset.get<size_t>("u0.solver.maximumIterations"),
+                            parset.get<double>("u0.solver.tolerance"), norm, Solver::FULL);
+
+    step.setIgnore(ignore);
+    step.setPreSmoothingSteps(parset.get<int>("solver.tnnmg.main.pre"));
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", I(refX));
+            },
+            "   energy      ");
+
+    double initialEnergy = I(refX);
+    refSolver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = I(refX);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", step.lastDampingFactor());
+            },
+            "   damping     ");
+
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", step.linearization().truncated().count());
+            },
+            "   truncated   ");
+
+    if (timeStep>0 and initial) {
+        allReductionFactors[timeStep].resize(0);
+        refSolver.addCriterion(reductionFactorCriterion(step, norm, allReductionFactors[timeStep]));
+    }
+
+    refSolver.preprocess();
+    refSolver.solve();
+
+    //print(refX, "refX: ");
+
+    // set up solver factory solver
+    using Preconditioner = MultilevelPatchPreconditioner<ContactNetwork, Matrix, Vector>;
+
+    const auto& preconditionerParset = parset.sub("solver.tnnmg.preconditioner");
+
+    Dune::BitSetVector<1> activeLevels(contactNetwork.nLevels(), true);
+    Preconditioner preconditioner(preconditionerParset, contactNetwork, activeLevels);
+    preconditioner.setPatchDepth(preconditionerParset.get<size_t>("patchDepth"));
+    preconditioner.build();
+
+    auto cgStep = std::make_shared<Dune::Solvers::CGStep<Matrix, Vector> >();
+    cgStep->setPreconditioner(preconditioner);
+
+    // set up functional
+    auto& globalFriction = contactNetwork.globalFriction();
+
+  /*  std::vector<const Dune::BitSetVector<1>*> nodes;
+     contactNetwork.frictionNodes(nodes);
+     print(*nodes[0], "frictionNodes: ");
+     print(*nodes[1], "frictionNodes: ");
+
+     print(ignore, "ignore: ");*/
+
+    //using MyFunctional = Functional<Matrix&, Vector&, std::decay_t<decltype(globalFriction)>&, Vector&, Vector&, typename Matrix::field_type>;
+    //MyFunctional J(mat, rhs, globalFriction, lower, upper);
+    using MyFunctional = Functional<Matrix&, Vector&, ZeroNonlinearity, Vector&, Vector&, typename Matrix::field_type>;
+    MyFunctional J(mat, rhs, ZeroNonlinearity(), lower, upper);
+
+    //std::cout << "ref energy: " << J(refX) << std::endl;
+
+    // set up TNMMG solver
+    // dummy solver, uses direct solver for linear correction no matter what is set here
+    //Norm mgNorm(*linearMultigridStep);
+    //auto mgSolver = std::make_shared<Solver>(linearMultigridStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
+    Norm mgNorm(*cgStep);
+    auto mgSolver = std::make_shared<Solver>(cgStep, parset.get<int>("solver.tnnmg.main.multi"), parset.get<double>("solver.tnnmg.preconditioner.basesolver.tolerance"), mgNorm, Solver::QUIET);
+
+    using Factory = SolverFactory<MyFunctional, BitVector>;
+    Factory factory(parset.sub("solver.tnnmg"), J, ignore);
+    factory.build(mgSolver);
+
+   /* std::vector<BitVector> bodyDirichletNodes;
+    nBodyAssembler.postprocess(_dirichletNodes, bodyDirichletNodes);
+    for (size_t i=0; i<bodyDirichletNodes.size(); i++) {
+      print(bodyDirichletNodes[i], "bodyDirichletNodes_" + std::to_string(i) + ": ");
+    }*/
+
+   /* print(bilinearForm, "matrix: ");
+    print(totalX, "totalX: ");
+    print(totalRhs, "totalRhs: ");*/
+
+    auto tnnmgStep = factory.step();
+    factory.setProblem(x);
+
+    LoopSolver<Vector> solver(
+        tnnmgStep.get(), parset.get<size_t>("u0.solver.maximumIterations"),
+                parset.get<double>("u0.solver.tolerance"), &norm,
+        Solver::FULL); //, true, &refX); // absolute error
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", J(x));
+            },
+            "   energy      ");
+
+    initialEnergy = J(x);
+    solver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = J(x);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", tnnmgStep->lastDampingFactor());
+            },
+            "   damping     ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", tnnmgStep->linearization().truncated().count());
+            },
+            "   truncated   ");
+
+
+    /*std::vector<double> factors;
+    solver.addCriterion(reductionFactorCriterion(*tnnmgStep, norm, factors));
+
+    solver.addCriterion(energyCriterion(*tnnmgStep, J, factors));*/
+
+    solver.preprocess();
+    solver.solve();
+
+    auto diff = x;
+    diff -= refX;
+    std::cout << "Solver error in energy norm: " << norm(diff) << std::endl;
+
+    std::cout << "Energy end iterate: " << J(x) << std::endl;
+}
+
+
+template <class ContactNetwork>
+void setupInitialConditions(const ContactNetwork& contactNetwork) {
+  using Matrix = typename ContactNetwork::Matrix;
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+
+  std::cout << std::endl << "-- setupInitialConditions --" << std::endl;
+  std::cout << "----------------------------" << std::endl;
+
+  // Solving a linear problem with a multigrid solver
+  auto const solveLinearProblem = [&](
+      const BitVector& _dirichletNodes, const std::vector<std::shared_ptr<Matrix>>& _matrices,
+      const std::vector<Vector>& _rhs, std::vector<Vector>& _x) {
+
+    std::vector<const Matrix*> matrices_ptr(_matrices.size());
+    for (size_t i=0; i<matrices_ptr.size(); i++) {
+          matrices_ptr[i] = _matrices[i].get();
+    }
+
+    // assemble full global contact problem
+    Matrix bilinearForm;
+
+    nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+    Vector totalRhs, oldTotalRhs;
+    nBodyAssembler.assembleRightHandSide(_rhs, totalRhs);
+    oldTotalRhs = totalRhs;
+
+    Vector totalX, oldTotalX;
+    nBodyAssembler.nodalToTransformed(_x, totalX);
+    oldTotalX = totalX;
+
+    // get lower and upper obstacles
+    const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+    Vector lower(totalObstacles.size());
+    Vector upper(totalObstacles.size());
+
+    for (size_t j=0; j<totalObstacles.size(); ++j) {
+        const auto& totalObstaclesj = totalObstacles[j];
+        auto& lowerj = lower[j];
+        auto& upperj = upper[j];
+      for (size_t d=0; d<dim; ++d) {
+          lowerj[d] = totalObstaclesj[d][0];
+          upperj[d] = totalObstaclesj[d][1];
+      }
+    }
+
+    // print problem
+   /* print(bilinearForm, "bilinearForm");
+    print(totalRhs, "totalRhs");
+    print(_dirichletNodes, "ignore");
+    print(totalObstacles, "totalObstacles");
+    print(lower, "lower");
+    print(upper, "upper");*/
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalX, _dirichletNodes, lower, upper, true);
+
+    nBodyAssembler.postprocess(totalX, _x);
+  };
+
+  timeStep = parset.get<size_t>("initialTime.timeStep");
+  relativeTime = parset.get<double>("initialTime.relativeTime");
+  relativeTau = parset.get<double>("initialTime.relativeTau");
+
+  bodyStates.resize(bodyCount);
+  u.resize(bodyCount);
+  v.resize(bodyCount);
+  a.resize(bodyCount);
+  alpha.resize(bodyCount);
+  weightedNormalStress.resize(bodyCount);
+
+  for (size_t i=0; i<bodyCount; i++) {
+    size_t leafVertexCount =  contactNetwork.body(i)->nVertices();
+
+    u[i].resize(leafVertexCount),
+    v[i].resize(leafVertexCount),
+    a[i].resize(leafVertexCount),
+    alpha[i].resize(leafVertexCount),
+    weightedNormalStress[i].resize(leafVertexCount),
+
+    bodyStates[i] = new BodyState<Vector, ScalarVector>(&u[i], &v[i], &a[i], &alpha[i], &weightedNormalStress[i]);
+  }
+
+  std::vector<Vector> ell0(bodyCount);
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial velocity
+    u[i] = 0.0;
+    v[i] = 0.0;
+
+    ell0[i].resize(u[i].size());
+    ell0[i] = 0.0;
+
+    contactNetwork.body(i)->externalForce()(relativeTime, ell0[i]);
+  }
+
+  // Initial displacement: Start from a situation of minimal stress,
+  // which is automatically attained in the case [v = 0 = a].
+  // Assuming dPhi(v = 0) = 0, we thus only have to solve Au = ell0
+  BitVector dirichletNodes;
+  contactNetwork.totalNodes("dirichlet", dirichletNodes);
+  /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+      bool val = false;
+      for (size_t d=0; d<dims; d++) {
+          val = val || dirichletNodes[i][d];
+      }
+
+      dirichletNodes[i] = val;
+      for (size_t d=0; d<dims; d++) {
+          dirichletNodes[i][d] = val;
+      }
+  }*/
+
+  std::cout << "solving linear problem for u..." << std::endl;
+
+  solveLinearProblem(dirichletNodes, contactNetwork.matrices().elasticity, ell0, u);
+
+  //print(u, "initial u:");
+
+  // Initial acceleration: Computed in agreement with Ma = ell0 - Au
+  // (without Dirichlet constraints), again assuming dPhi(v = 0) = 0
+  std::vector<Vector> accelerationRHS = ell0;
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial state
+    alpha[i] = parset.get<double>("boundary.friction.initialAlpha");
+
+    // Initial normal stress
+
+    const auto& body = contactNetwork.body(i);
+    /*std::vector<std::shared_ptr<typename ContactNetwork::LeafBody::BoundaryCondition>> frictionBoundaryConditions;
+    body->boundaryConditions("friction", frictionBoundaryConditions);
+    for (size_t j=0; j<frictionBoundaryConditions.size(); j++) {
+        ScalarVector frictionBoundaryStress(weightedNormalStress[i].size());
+
+        body->assembler()->assembleWeightedNormalStress(
+          *frictionBoundaryConditions[j]->boundaryPatch(), frictionBoundaryStress, body->data()->getYoungModulus(),
+          body->data()->getPoissonRatio(), u[i]);
+
+        weightedNormalStress[i] += frictionBoundaryStress;
+    }*/
+
+    Dune::MatrixVector::subtractProduct(accelerationRHS[i], *body->matrices().elasticity, u[i]);
+  }
+
+  for (size_t i=0; i<contactNetwork.nCouplings(); i++) {
+    const auto& coupling = contactNetwork.coupling(i);
+    const auto& contactCoupling = contactNetwork.nBodyAssembler().getContactCouplings()[i];
+
+    const auto nonmortarIdx = coupling->gridIdx_[0];
+    const auto& body = contactNetwork.body(nonmortarIdx);
+
+    ScalarVector frictionBoundaryStress(weightedNormalStress[nonmortarIdx].size());
+
+    body->assembler()->assembleWeightedNormalStress(
+      contactCoupling->nonmortarBoundary(), frictionBoundaryStress, body->data()->getYoungModulus(),
+      body->data()->getPoissonRatio(), u[nonmortarIdx]);
+
+    weightedNormalStress[nonmortarIdx] += frictionBoundaryStress;
+  }
+
+  //print(weightedNormalStress, "weightedNormalStress: ");
+
+  std::cout << "solving linear problem for a..." << std::endl;
+
+  BitVector noNodes(dirichletNodes.size(), false);
+  solveLinearProblem(noNodes, contactNetwork.matrices().mass, accelerationRHS, a);
+
+  //print(a, "initial a:");
+}
+
+template <class ContactNetwork>
+void relativeVelocities(const ContactNetwork& contactNetwork, const Vector& v, std::vector<Vector>& v_rel) {
+
+    const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    const size_t nBodies = nBodyAssembler.nGrids();
+   // const auto& contactCouplings = nBodyAssembler.getContactCouplings();
+
+    std::vector<const Dune::BitSetVector<1>*> bodywiseNonmortarBoundaries;
+    contactNetwork.frictionNodes(bodywiseNonmortarBoundaries);
+
+    size_t globalIdx = 0;
+    v_rel.resize(nBodies);
+    for (size_t bodyIdx=0; bodyIdx<nBodies; bodyIdx++) {
+        const auto& nonmortarBoundary = *bodywiseNonmortarBoundaries[bodyIdx];
+        auto& v_rel_ = v_rel[bodyIdx];
+
+        v_rel_.resize(nonmortarBoundary.size());
+
+        for (size_t i=0; i<v_rel_.size(); i++) {
+            if (toBool(nonmortarBoundary[i])) {
+                v_rel_[i] = v[globalIdx];
+            }
+            globalIdx++;
+        }
+    }
+
+}
+
+template <class Updaters, class ContactNetwork>
+void run(Updaters& updaters, ContactNetwork& contactNetwork,
+    const std::vector<Matrix>& velocityMatrices, const std::vector<Vector>& velocityRHSs,
+    std::vector<Vector>& velocityIterates) {
+
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+  const auto nBodies = nBodyAssembler.nGrids();
+
+  std::vector<const Matrix*> matrices_ptr(nBodies);
+  for (int i=0; i<nBodies; i++) {
+      matrices_ptr[i] = &velocityMatrices[i];
+  }
+
+  // assemble full global contact problem
+  Matrix bilinearForm;
+  nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+  Vector totalRhs;
+  nBodyAssembler.assembleRightHandSide(velocityRHSs, totalRhs);
+
+  // get lower and upper obstacles
+  const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+  Vector lower(totalObstacles.size());
+  Vector upper(totalObstacles.size());
+
+  for (size_t j=0; j<totalObstacles.size(); ++j) {
+      const auto& totalObstaclesj = totalObstacles[j];
+      auto& lowerj = lower[j];
+      auto& upperj = upper[j];
+    for (size_t d=0; d<dim; ++d) {
+        lowerj[d] = totalObstaclesj[d][0];
+        upperj[d] = totalObstaclesj[d][1];
+    }
+  }
+
+  // compute velocity obstacles
+  /*Vector vLower, vUpper;
+  std::vector<Vector> u0, v0;
+  updaters.rate_->extractOldVelocity(v0);
+  updaters.rate_->extractOldDisplacement(u0);
+
+  Vector totalu0, totalv0;
+  nBodyAssembler.nodalToTransformed(u0, totalu0);
+  nBodyAssembler.nodalToTransformed(v0, totalv0);
+
+  updaters.rate_->velocityObstacles(totalu0, lower, totalv0, vLower);
+  updaters.rate_->velocityObstacles(totalu0, upper, totalv0, vUpper);*/
+
+  const auto& errorNorms = contactNetwork.stateEnergyNorms();
+
+  auto& globalFriction = contactNetwork.globalFriction();
+
+  BitVector totalDirichletNodes;
+  contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+  size_t fixedPointMaxIterations = parset.get<size_t>("v.fpi.maximumIterations");
+  double fixedPointTolerance = parset.get<double>("v.fpi.tolerance");
+  double lambda = parset.get<double>("v.fpi.lambda");
+
+  size_t fixedPointIteration;
+  size_t multigridIterations = 0;
+
+  std::vector<ScalarVector> alpha(nBodies);
+  updaters.state_->extractAlpha(alpha);
+
+  Vector totalVelocityIterate;
+  nBodyAssembler.nodalToTransformed(velocityIterates, totalVelocityIterate);
+
+  // project in onto admissible set
+  const size_t blocksize = Vector::block_type::dimension;
+  for (size_t i=0; i<totalVelocityIterate.size(); i++) {
+      for (size_t j=0; j<blocksize; j++) {
+          if (totalVelocityIterate[i][j] < lower[i][j]) {
+              totalVelocityIterate[i][j] = lower[i][j];
+          }
+
+          if (totalVelocityIterate[i][j] > upper[i][j]) {
+              totalVelocityIterate[i][j] = upper[i][j];
+          }
+      }
+  }
+
+  for (fixedPointIteration = 0; fixedPointIteration < fixedPointMaxIterations;
+       ++fixedPointIteration) {
+
+    // contribution from nonlinearity
+    //print(alpha, "alpha: ");
+
+    globalFriction.updateAlpha(alpha);
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalVelocityIterate, totalDirichletNodes, lower, upper, false);
+
+    nBodyAssembler.postprocess(totalVelocityIterate, velocityIterates);
+
+    std::vector<Vector> v_m; //TODO : wrong, isnt used atm;
+    updaters.rate_->extractOldVelocity(v_m);
+
+    for (size_t i=0; i<v_m.size(); i++) {
+      v_m[i] *= 1.0 - lambda;
+      Dune::MatrixVector::addProduct(v_m[i], lambda, velocityIterates[i]);
+    }
+
+    // extract relative velocities in mortar basis
+    std::vector<Vector> v_rel;
+    relativeVelocities(contactNetwork, totalVelocityIterate, v_rel);
+
+    //print(v_rel, "v_rel");
+
+    std::cout << "- State problem set" << std::endl;
+
+    // solve a state problem
+    updaters.state_->solve(v_rel);
+
+    std::cout << "-- Solved" << std::endl;
+
+    std::vector<ScalarVector> newAlpha(nBodies);
+    updaters.state_->extractAlpha(newAlpha);
+
+    bool breakCriterion = true;
+    for (int i=0; i<nBodies; i++) {
+        if (alpha[i].size()==0 || newAlpha[i].size()==0)
+            continue;
+
+        //print(alpha[i], "alpha i:");
+        //print(newAlpha[i], "new alpha i:");
+        if (errorNorms[i]->diff(alpha[i], newAlpha[i]) >= fixedPointTolerance) {
+            breakCriterion = false;
+            std::cout << "fixedPoint error: " << errorNorms[i]->diff(alpha[i], newAlpha[i]) << std::endl;
+            break;
+        }
+    }
+
+    if (lambda < 1e-12 or breakCriterion) {
+      std::cout << "-FixedPointIteration finished! " << (lambda < 1e-12 ? "lambda" : "breakCriterion") << std::endl;
+      fixedPointIteration++;
+      break;
+    }
+    alpha = newAlpha;
+  }
+
+  std::cout << "-FixedPointIteration finished! " << std::endl;
+
+  if (fixedPointIteration == fixedPointMaxIterations)
+    DUNE_THROW(Dune::Exception, "FPI failed to converge");
+
+  updaters.rate_->postProcess(velocityIterates);
+}
+
+
+template <class Updaters, class ContactNetwork>
+void step(Updaters& updaters, ContactNetwork& contactNetwork)  {
+  updaters.state_->nextTimeStep();
+  updaters.rate_->nextTimeStep();
+
+  auto const newRelativeTime = relativeTime + relativeTau;
+  typename ContactNetwork::ExternalForces externalForces;
+  contactNetwork.externalForces(externalForces);
+  std::vector<Vector> ell(externalForces.size());
+  for (size_t i=0; i<externalForces.size(); i++) {
+    (*externalForces[i])(newRelativeTime, ell[i]);
+  }
+
+  std::vector<Matrix> velocityMatrix;
+  std::vector<Vector> velocityRHS;
+  std::vector<Vector> velocityIterate;
+
+  double finalTime = parset.get<double>("problem.finalTime");
+  auto const tau = relativeTau * finalTime;
+  updaters.state_->setup(tau);
+  updaters.rate_->setup(ell, tau, newRelativeTime, velocityRHS, velocityIterate, velocityMatrix);
+
+  run(updaters, contactNetwork, velocityMatrix, velocityRHS, velocityIterate);
+}
+
+template <class Updaters, class ContactNetwork>
+void advanceStep(Updaters& current, ContactNetwork& contactNetwork) {
+    step(current, contactNetwork);
+    relativeTime += relativeTau;
+}
+
+void getParameters(int argc, char *argv[]) {
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree(
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem-%dD.cfg", dim), parset);
+  Dune::ParameterTreeParser::readOptions(argc, argv, parset);
+}
+
+int main(int argc, char *argv[]) { try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    std::ofstream out(path + outputFile);
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to outputFile
+
+    std::cout << "-------------------------" << std::endl;
+    std::cout << "-- SolverFactory Test: --" << std::endl;
+    std::cout << "-------------------------" << std::endl << std::endl;
+
+    getParameters(argc, argv);
+
+    // ----------------------
+    // set up contact network
+    // ----------------------
+    StackedBlocksFactory<Grid, Vector> stackedBlocksFactory(parset);
+    using ContactNetwork = typename StackedBlocksFactory<Grid, Vector>::ContactNetwork;
+    stackedBlocksFactory.build();
+
+    ContactNetwork& contactNetwork = stackedBlocksFactory.contactNetwork();
+    bodyCount = contactNetwork.nBodies();
+
+    for (size_t i=0; i<contactNetwork.nLevels(); i++) {
+        const auto& level = *contactNetwork.level(i);
+
+        for (size_t j=0; j<level.nBodies(); j++) {
+            writeToVTK(level.body(j)->gridView(), "debug_print/bodies/", "body_" + std::to_string(j) + "_level_" + std::to_string(i));
+        }
+    }
+
+    for (size_t i=0; i<bodyCount; i++) {
+        writeToVTK(contactNetwork.body(i)->gridView(), "debug_print/bodies/", "body_" + std::to_string(i) + "_leaf");
+    }
+
+    // ----------------------------
+    // assemble contactNetwork
+    // ----------------------------
+    contactNetwork.assemble();
+
+    //printMortarBasis<Vector>(contactNetwork.nBodyAssembler());
+
+    // -----------------
+    // init input/output
+    // -----------------
+    timeSteps = parset.get<size_t>("timeSteps.timeSteps");
+    allReductionFactors.resize(timeSteps+1);
+
+    setupInitialConditions(contactNetwork);
+
+    auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    for (size_t i=0; i<bodyCount; i++) {
+      contactNetwork.body(i)->setDeformation(u[i]);
+    }
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();
+
+    // ------------------------
+    // assemble global friction
+    // ------------------------
+    contactNetwork.assembleFriction(parset.get<Config::FrictionModel>("boundary.friction.frictionModel"), weightedNormalStress);
+
+    auto& globalFriction = contactNetwork.globalFriction();
+    globalFriction.updateAlpha(alpha);
+
+    using Assembler = MyAssembler<DefLeafGridView, dim>;
+    using field_type = Matrix::field_type;
+    using MyVertexBasis = typename Assembler::VertexBasis;
+    using MyCellBasis = typename Assembler::CellBasis;
+    std::vector<Vector> vertexCoordinates(bodyCount);
+    std::vector<const MyVertexBasis* > vertexBases(bodyCount);
+    std::vector<const MyCellBasis* > cellBases(bodyCount);
+
+    for (size_t i=0; i<bodyCount; i++) {
+      const auto& body = contactNetwork.body(i);
+      vertexBases[i] = &(body->assembler()->vertexBasis);
+      cellBases[i] = &(body->assembler()->cellBasis);
+
+      auto& vertexCoords = vertexCoordinates[i];
+      vertexCoords.resize(body->nVertices());
+
+      Dune::MultipleCodimMultipleGeomTypeMapper<
+          DefLeafGridView, Dune::MCMGVertexLayout> const vertexMapper(body->gridView(), Dune::mcmgVertexLayout());
+      for (auto &&v : vertices(body->gridView()))
+        vertexCoords[vertexMapper.index(v)] = geoToPoint(v.geometry());
+    }
+
+    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "/storage/mi/podlesny/software/dune/dune-tectonic/body");
+
+    auto const report = [&](bool initial = false) {
+      if (parset.get<bool>("io.printProgress"))
+        std::cout << "timeStep = " << std::setw(6) << timeStep
+                  << ", time = " << std::setw(12) << relativeTime
+                  << ", tau = " << std::setw(12) << relativeTau
+                  << std::endl;
+
+      if (parset.get<bool>("io.vtk.write")) {
+        std::vector<ScalarVector> stress(bodyCount);
+
+        for (size_t i=0; i<bodyCount; i++) {
+          const auto& body = contactNetwork.body(i);
+          body->assembler()->assembleVonMisesStress(body->data()->getYoungModulus(),
+                                           body->data()->getPoissonRatio(),
+                                           u[i], stress[i]);
+
+        }
+
+        vtkWriter.write(timeStep, u, v, alpha, stress);
+      }
+    };
+    report(true);
+
+    // -------------------
+    // Set up TNNMG solver
+    // -------------------
+
+    /*BitVector totalDirichletNodes;
+    contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+    print(totalDirichletNodes, "totalDirichletNodes:");*/
+
+    //using Functional = Functional<Matrix&, Vector&, ZeroNonlinearity&, Vector&, Vector&, field_type>;
+    //using Functional = Functional<Matrix&, Vector&, GlobalFriction<Matrix, Vector>&, Vector&, Vector&, field_type>;
+    //using NonlinearFactory = SolverFactory<Functional, BitVector>;
+
+    using BoundaryFunctions = typename ContactNetwork::BoundaryFunctions;
+    using BoundaryNodes = typename ContactNetwork::BoundaryNodes;
+    using Updaters = Updaters<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>,
+                               StateUpdater<ScalarVector, Vector>>;
+
+    BoundaryFunctions velocityDirichletFunctions;
+    contactNetwork.boundaryFunctions("dirichlet", velocityDirichletFunctions);
+
+    BoundaryNodes dirichletNodes;
+    contactNetwork.boundaryNodes("dirichlet", dirichletNodes);
+
+    /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+        for (size_t j=0; j<dirichletNodes[i].size(); j++) {
+        print(*dirichletNodes[i][j], "dirichletNodes_body_" + std::to_string(i) + "_boundary_" + std::to_string(j));
+        }
+    }*/
+
+
+    /*for (size_t i=0; i<frictionNodes.size(); i++) {
+        print(*frictionNodes[i], "frictionNodes_body_" + std::to_string(i));
+    }*/
+
+    Updaters current(
+        initRateUpdater(
+            parset.get<Config::scheme>("timeSteps.scheme"),
+            velocityDirichletFunctions,
+            dirichletNodes,
+            contactNetwork.matrices(),
+            u,
+            v,
+            a),
+        initStateUpdater<ScalarVector, Vector>(
+            parset.get<Config::stateModel>("boundary.friction.stateModel"),
+            alpha,
+            nBodyAssembler.getContactCouplings(),
+            contactNetwork.couplings())
+            );
+
+
+
+    //const auto& stateEnergyNorms = contactNetwork.stateEnergyNorms();
+
+    for (timeStep=1; timeStep<=timeSteps; timeStep++) {
+
+      advanceStep(current, contactNetwork);
+
+      relativeTime += relativeTau;
+
+      current.rate_->extractDisplacement(u);
+      current.rate_->extractVelocity(v);
+      current.rate_->extractAcceleration(a);
+      current.state_->extractAlpha(alpha);
+
+      contactNetwork.setDeformation(u);
+
+      report();
+    }
+
+    // output reduction factors
+    size_t count = 0;
+    for (size_t i=0; i<allReductionFactors.size(); i++) {
+        count = std::max(count, allReductionFactors[i].size());
+    }
+    std::vector<double> avgReductionFactors(count);
+    for (size_t i=0; i<count; i++) {
+        avgReductionFactors[i] = 1;
+        size_t c = 0;
+
+        for (size_t j=0; j<allReductionFactors.size(); j++) {
+            if (!(i<allReductionFactors[j].size()))
+                continue;
+
+            avgReductionFactors[i] *= allReductionFactors[j][i];
+            c++;
+        }
+
+        avgReductionFactors[i] = std::pow(avgReductionFactors[i], 1.0/((double)c));
+    }
+
+    print(avgReductionFactors, "average reduction factors: ");
+
+    bool passed = true;
+
+    std::cout << "Overall, the test " << (passed ? "was successful!" : "failed!") << std::endl;
+
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+    return passed ? 0 : 1;
+
+} catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+} catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+} // end try
+} // end main

dune/tectonic/tests/supportpatchfactorytest.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#define MY_DIM 2
+
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/parametertree.hh>
+#include <dune/common/parametertreeparser.hh>
+
+#include <dune/fufem/formatstring.hh>
+
+#include "../assemblers.hh"
+#include "../gridselector.hh"
+#include "../explicitgrid.hh"
+#include "../explicitvectors.hh"
+
+#include "../factories/threeblocksfactory.cc"
+
+#include "../spatial-solving/preconditioners/supportpatchfactory.hh"
+#include "../utils/debugutils.hh"
+
+
+
+//#include <dune/tectonic/transformedglobalratestatefriction.hh>
+
+#include "common.hh"
+
+const int dim = MY_DIM;
+const int n = 5;
+const bool simplexGrid = true;
+
+const std::string path = "";
+const std::string outputFile = "supportpatchfactorytest.log";
+
+Dune::ParameterTree getParameters(int argc, char *argv[]) {
+  Dune::ParameterTree parset;
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree(
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem-%dD.cfg", dim), parset);
+  Dune::ParameterTreeParser::readOptions(argc, argv, parset);
+  return parset;
+}
+
+template <class SupportPatchFactory>
+void testSuite(const SupportPatchFactory& supportPatchFactory, const size_t bodyID, const int patchDepth = 0) {
+/*    const auto& coarseContactNetwork = supportPatchFactory.coarseContactNetwork();
+    const auto& gridView = coarseContactNetwork.body(bodyID)->gridView();
+
+    for (const auto& e : elements(gridView)) {
+
+    }
+
+    using Patch = typename SupportPatchFactory::Patch;
+    Patch patch0, patch1, patch2, patch3;
+
+    // (const size_t bodyID, const Element& coarseElement, const size_t localVertex, Patch& patchDofs, const int patchDepth = 0)
+
+    // interior patch inside of one body
+    supportPatchFactory.build(0, const Element& coarseElement, const size_t localVertex, patch0, patchDepth);
+
+    // patch at friction interface with two bodies in contact
+    supportPatchFactory.build(0, const Element& coarseElement, const size_t localVertex, patch1 patchDepth);
+
+    // patch at friction interface with two bodies in contact and with dirichlet boundary
+    supportPatchFactory.build(0, const Element& coarseElement, const size_t localVertex, patch2, patchDepth);
+
+    // crosspoint patch, where all 3 bodies are in contact
+    supportPatchFactory.build(0, const Element& coarseElement, const size_t localVertex, patch3, patchDepth);*/
+}
+
+int main(int argc, char *argv[]) { try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    std::ofstream out(path + outputFile);
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to outputFile
+
+    std::cout << "-------------------------------" << std::endl;
+    std::cout << "-- SupportPatchFactory Test: --" << std::endl;
+    std::cout << "-------------------------------" << std::endl << std::endl;
+
+    const auto parset = getParameters(argc, argv);
+
+    // set up contact network
+    ThreeBlocksFactory<Grid, Vector> threeBlocksFactory(parset);
+    //using ContactNetwork = typename ThreeBlocksFactory<Grid, Vector>::ContactNetwork;
+    //threeBlocksFactory.build();
+
+    //ContactNetwork& contactNetwork = threeBlocksFactory.contactNetwork();
+   // threeBlocksFactory.build();
+
+   // auto& contactNetwork = threeBlocksFactory.contactNetwork();
+    //const size_t bodyCount = contactNetwork.nBodies();
+
+    //for (size_t i=0; i<bodyCount; i++) {
+        //printDofLocation(levelContactNetwork.leafView(i));
+
+        /*auto& levelViews = levelContactNetwork.levelViews(i);
+
+        for (size_t j=0; j<levelViews.size(); j++) {
+            writeToVTK(*levelViews[j], "", "body_" + std::to_string(i) + "_level_" + std::to_string(j));
+        }
+
+        writeToVTK(levelContactNetwork.leafView(i), "", "body_" + std::to_string(i) + "_leaf"); */
+    //}
+
+    // assemble levelContactNetwork
+    //contactNetwork.assemble();
+
+  /*  const auto& coarseContactNetwork = contactNetwork.level(0);
+    const auto& fineContactNetwork = contactNetwork.level(1);
+
+    SupportPatchFactory<decltype(coarseContactNetwork)> supportPatchFactory(coarseContactNetwork, fineContactNetwork);
+
+
+    // set levelContactNetwork
+    LevelContactNetwork levelContactNetwork;
+    NBodyAssembler nBodyAssembler(grids.size(), 1);
+
+    std::vector<const GridType*> grids_ptr(grids.size());
+    for (size_t i=0; i<grids_ptr.size(); i++) {
+        grids_ptr[i] = grids[i].get();
+    }
+
+    nBodyAssembler.setGrids(grids_ptr);
+    nBodyAssembler.setCoupling(coupling, 0);
+
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();*/
+
+    bool passed = true;
+
+    std::cout << "Overall, the test " << (passed ? "was successful!" : "failed!") << std::endl;
+
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+    return passed ? 0 : 1;
+
+} catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+} catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+} // end try
+} // end main

dune/tectonic/tests/tnnmgtest/CMakeLists.txt

+add_custom_target(tectonic_tests_tnnmgtest SOURCES
+ # functionaltest.cfg
+)
+
+set(TNNMGTEST_SOURCE_FILES
+  ../../assemblers.cc
+  ../../data-structures/body/body.cc
+  ../../data-structures/network/levelcontactnetwork.cc
+  ../../data-structures/network/contactnetwork.cc
+  ../../data-structures/enumparser.cc
+  ../../factories/twoblocksfactory.cc
+  ../../factories/threeblocksfactory.cc
+  ../../factories/stackedblocksfactory.cc
+  #../../io/vtk.cc
+  ../../problem-data/grid/cuboidgeometry.cc
+  ../../problem-data/grid/mygrids.cc
+  ../../problem-data/grid/simplexmanager.cc
+  ../../spatial-solving/solverfactory.cc
+  staticcontacttest.cc
+)
+
+foreach(_dim 2 3)
+  set(_tnnmgtest_target functionaltest-${_dim}D)
+
+  dune_add_test(NAME ${_tnnmgtest_target} SOURCES ${TNNMGTEST_SOURCE_FILES})
+
+  add_dune_ug_flags(${_tnnmgtest_target})
+  add_dune_hdf5_flags(${_tnnmgtest_target})
+
+  set_property(TARGET ${_tnnmgtest_target} APPEND PROPERTY COMPILE_DEFINITIONS "MY_DIM=${_dim}")
+endforeach()

dune/tectonic/tests/tnnmgtest/functionaltest.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#define MY_DIM 2
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <exception>
+
+#include <dune/common/exceptions.hh>
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/stdstreams.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/function.hh>
+#include <dune/common/bitsetvector.hh>
+#include <dune/common/parametertree.hh>
+#include <dune/common/parametertreeparser.hh>
+
+#include <dune/fufem/formatstring.hh>
+
+#include <dune/tnnmg/functionals/boxconstrainedquadraticfunctional.hh>
+#include <dune/tnnmg/functionals/bcqfconstrainedlinearization.hh>
+#include <dune/tnnmg/localsolvers/scalarobstaclesolver.hh>
+#include <dune/tnnmg/iterationsteps/tnnmgstep.hh>
+
+#include <dune/tectonic/geocoordinate.hh>
+
+#include "assemblers.hh"
+#include "gridselector.hh"
+#include "explicitgrid.hh"
+#include "explicitvectors.hh"
+
+#include "data-structures/enumparser.hh"
+#include "data-structures/enums.hh"
+#include "data-structures/contactnetwork.hh"
+#include "data-structures/matrices.hh"
+#include "data-structures/program_state.hh"
+
+#include "io/vtk.hh"
+
+#include "spatial-solving/tnnmg/linesearchsolver.hh"
+#include "spatial-solving/preconditioners/nbodycontacttransfer.hh"
+
+#include "factories/stackedblocksfactory.hh"
+
+#include "time-stepping/rate.hh"
+#include "time-stepping/state.hh"
+#include "time-stepping/updaters.hh"
+
+#include "utils/tobool.hh"
+#include "utils/debugutils.hh"
+
+
+const int dim = MY_DIM;
+
+Dune::ParameterTree parset;
+size_t bodyCount;
+
+std::vector<BodyState<Vector, ScalarVector>* > bodyStates;
+std::vector<Vector> u;
+std::vector<Vector> v;
+std::vector<Vector> a;
+std::vector<ScalarVector> alpha;
+std::vector<ScalarVector> weightedNormalStress;
+double relativeTime;
+double relativeTau;
+size_t timeStep = 0;
+size_t timeSteps = 0;
+
+const std::string path = "";
+const std::string outputFile = "solverfactorytest.log";
+
+std::vector<std::vector<double>> allReductionFactors;
+
+template<class IterationStepType, class NormType, class ReductionFactorContainer>
+Dune::Solvers::Criterion reductionFactorCriterion(
+      const IterationStepType& iterationStep,
+      const NormType& norm,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = norm.diff(*lastIterate, *iterationStep.getIterate());
+        double convRate = (normOfOldCorrection > 0) ? normOfCorrection / normOfOldCorrection : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+
+template<class IterationStepType, class Functional, class ReductionFactorContainer>
+Dune::Solvers::Criterion energyCriterion(
+      const IterationStepType& iterationStep,
+      const Functional& f,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = std::abs(f(*lastIterate) - f(*iterationStep.getIterate())); //norm.diff(*lastIterate, *iterationStep.getIterate());
+
+        double convRate = (normOfOldCorrection != 0.0) ? 1.0 - (normOfCorrection / normOfOldCorrection) : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+template <class ContactNetwork>
+void solveProblem(const ContactNetwork& contactNetwork,
+                  const Matrix& mat, const Vector& rhs, Vector& x,
+                  const BitVector& ignore, const Vector& lower, const Vector& upper, bool initial = false) {
+
+    using Solver = typename Dune::Solvers::LoopSolver<Vector, BitVector>;
+    using Norm =  EnergyNorm<Matrix, Vector>;
+
+    //using LocalSolver = LocalBisectionSolver;
+    //using Linearization = Linearization<Functional, BitVector>;
+
+    /*print(ignore, "ignore:");
+    for (size_t i=0; i<x.size(); i++) {
+        std::cout << x[i] << std::endl;
+    }*/
+
+    // set up reference solver
+    Vector refX = x;
+    using ContactFunctional = Dune::TNNMG::BoxConstrainedQuadraticFunctional<Matrix&, Vector&, Vector&, Vector&, double>;
+    auto I = ContactFunctional(mat, rhs, lower, upper);
+
+    std::cout << "Energy start iterate: " << I(x) << std::endl;
+
+    using LocalSolver = Dune::TNNMG::ScalarObstacleSolver;
+    auto localSolver = Dune::TNNMG::gaussSeidelLocalSolver(LocalSolver());
+
+    using NonlinearSmoother = Dune::TNNMG::NonlinearGSStep<ContactFunctional, decltype(localSolver), BitVector>;
+    auto nonlinearSmoother = std::make_shared<NonlinearSmoother>(I, refX, localSolver);
+
+    using Linearization = Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<ContactFunctional, BitVector>;
+    using DefectProjection = Dune::TNNMG::ObstacleDefectProjection;
+    using Step = Dune::TNNMG::TNNMGStep<ContactFunctional, BitVector, Linearization, DefectProjection, LocalSolver>;
+
+    // set multigrid solver
+    auto smoother = TruncatedBlockGSStep<Matrix, Vector>();
+
+    using TransferOperator = NBodyContactTransfer<ContactNetwork, Vector>;
+    using TransferOperators = std::vector<std::shared_ptr<TransferOperator>>;
+
+    TransferOperators transfer(contactNetwork.nLevels()-1);
+    for (size_t i=0; i<transfer.size(); i++) {
+        transfer[i] = std::make_shared<TransferOperator>();
+        transfer[i]->setup(contactNetwork, i, i+1);
+    }
+
+    // Remove any recompute filed so that initially the full transferoperator is assembled
+    for (size_t i=0; i<transfer.size(); i++)
+        std::dynamic_pointer_cast<TruncatedMGTransfer<Vector> >(transfer[i])->setRecomputeBitField(nullptr);
+
+    auto linearMultigridStep = std::make_shared<Dune::Solvers::MultigridStep<Matrix, Vector> >();
+    linearMultigridStep->setMGType(1, 3, 3);
+    linearMultigridStep->setSmoother(&smoother);
+    linearMultigridStep->setTransferOperators(transfer);
+
+    int mu = parset.get<int>("solver.tnnmg.main.multi"); // #multigrid steps in Newton step
+    auto step = Step(I, refX, nonlinearSmoother, linearMultigridStep, mu, DefectProjection(), LocalSolver());
+
+    // compute reference solution with generic functional and solver
+    auto norm = Norm(mat);
+
+    auto refSolver = Solver(step, parset.get<size_t>("u0.solver.maximumIterations"),
+                            parset.get<double>("u0.solver.tolerance"), norm, Solver::FULL);
+
+    step.setIgnore(ignore);
+    step.setPreSmoothingSteps(parset.get<int>("solver.tnnmg.main.pre"));
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", I(refX));
+            },
+            "   energy      ");
+
+    double initialEnergy = I(refX);
+    refSolver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = I(refX);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", step.lastDampingFactor());
+            },
+            "   damping     ");
+
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", step.linearization().truncated().count());
+            },
+            "   truncated   ");
+
+    if (timeStep>0 and initial) {
+        allReductionFactors[timeStep].resize(0);
+        refSolver.addCriterion(reductionFactorCriterion(step, norm, allReductionFactors[timeStep]));
+    }
+
+    refSolver.preprocess();
+    refSolver.solve();
+
+    //print(refX, "refX: ");
+
+    // set up solver factory solver
+    using Preconditioner = MultilevelPatchPreconditioner<ContactNetwork, Matrix, Vector>;
+
+    const auto& preconditionerParset = parset.sub("solver.tnnmg.preconditioner");
+
+    Dune::BitSetVector<1> activeLevels(contactNetwork.nLevels(), true);
+    Preconditioner preconditioner(preconditionerParset, contactNetwork, activeLevels);
+    preconditioner.setPatchDepth(preconditionerParset.get<size_t>("patchDepth"));
+    preconditioner.build();
+
+    auto cgStep = std::make_shared<Dune::Solvers::CGStep<Matrix, Vector> >();
+    cgStep->setPreconditioner(preconditioner);
+
+    // set up functional
+    auto& globalFriction = contactNetwork.globalFriction();
+
+  /*  std::vector<const Dune::BitSetVector<1>*> nodes;
+     contactNetwork.frictionNodes(nodes);
+     print(*nodes[0], "frictionNodes: ");
+     print(*nodes[1], "frictionNodes: ");
+
+     print(ignore, "ignore: ");*/
+
+    //using MyFunctional = Functional<Matrix&, Vector&, std::decay_t<decltype(globalFriction)>&, Vector&, Vector&, typename Matrix::field_type>;
+    //MyFunctional J(mat, rhs, globalFriction, lower, upper);
+    using MyFunctional = Functional<Matrix&, Vector&, ZeroNonlinearity, Vector&, Vector&, typename Matrix::field_type>;
+    MyFunctional J(mat, rhs, ZeroNonlinearity(), lower, upper);
+
+    //std::cout << "ref energy: " << J(refX) << std::endl;
+
+    // set up TNMMG solver
+    // dummy solver, uses direct solver for linear correction no matter what is set here
+    //Norm mgNorm(*linearMultigridStep);
+    //auto mgSolver = std::make_shared<Solver>(linearMultigridStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
+    Norm mgNorm(*cgStep);
+    auto mgSolver = std::make_shared<Solver>(cgStep, parset.get<int>("solver.tnnmg.main.multi"), parset.get<double>("solver.tnnmg.preconditioner.basesolver.tolerance"), mgNorm, Solver::QUIET);
+
+    using Factory = SolverFactory<MyFunctional, BitVector>;
+    Factory factory(parset.sub("solver.tnnmg"), J, ignore);
+    factory.build(mgSolver);
+
+   /* std::vector<BitVector> bodyDirichletNodes;
+    nBodyAssembler.postprocess(_dirichletNodes, bodyDirichletNodes);
+    for (size_t i=0; i<bodyDirichletNodes.size(); i++) {
+      print(bodyDirichletNodes[i], "bodyDirichletNodes_" + std::to_string(i) + ": ");
+    }*/
+
+   /* print(bilinearForm, "matrix: ");
+    print(totalX, "totalX: ");
+    print(totalRhs, "totalRhs: ");*/
+
+    auto tnnmgStep = factory.step();
+    factory.setProblem(x);
+
+    LoopSolver<Vector> solver(
+        tnnmgStep.get(), parset.get<size_t>("u0.solver.maximumIterations"),
+                parset.get<double>("u0.solver.tolerance"), &norm,
+        Solver::FULL); //, true, &refX); // absolute error
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", J(x));
+            },
+            "   energy      ");
+
+    initialEnergy = J(x);
+    solver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = J(x);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", tnnmgStep->lastDampingFactor());
+            },
+            "   damping     ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", tnnmgStep->linearization().truncated().count());
+            },
+            "   truncated   ");
+
+
+    /*std::vector<double> factors;
+    solver.addCriterion(reductionFactorCriterion(*tnnmgStep, norm, factors));
+
+    solver.addCriterion(energyCriterion(*tnnmgStep, J, factors));*/
+
+    solver.preprocess();
+    solver.solve();
+
+    auto diff = x;
+    diff -= refX;
+    std::cout << "Solver error in energy norm: " << norm(diff) << std::endl;
+
+    std::cout << "Energy end iterate: " << J(x) << std::endl;
+}
+
+
+template <class ContactNetwork>
+void setupInitialConditions(const ContactNetwork& contactNetwork) {
+  using Matrix = typename ContactNetwork::Matrix;
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+
+  std::cout << std::endl << "-- setupInitialConditions --" << std::endl;
+  std::cout << "----------------------------" << std::endl;
+
+  // Solving a linear problem with a multigrid solver
+  auto const solveLinearProblem = [&](
+      const BitVector& _dirichletNodes, const std::vector<std::shared_ptr<Matrix>>& _matrices,
+      const std::vector<Vector>& _rhs, std::vector<Vector>& _x) {
+
+    std::vector<const Matrix*> matrices_ptr(_matrices.size());
+    for (size_t i=0; i<matrices_ptr.size(); i++) {
+          matrices_ptr[i] = _matrices[i].get();
+    }
+
+    // assemble full global contact problem
+    Matrix bilinearForm;
+
+    nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+    Vector totalRhs, oldTotalRhs;
+    nBodyAssembler.assembleRightHandSide(_rhs, totalRhs);
+    oldTotalRhs = totalRhs;
+
+    Vector totalX, oldTotalX;
+    nBodyAssembler.nodalToTransformed(_x, totalX);
+    oldTotalX = totalX;
+
+    // get lower and upper obstacles
+    const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+    Vector lower(totalObstacles.size());
+    Vector upper(totalObstacles.size());
+
+    for (size_t j=0; j<totalObstacles.size(); ++j) {
+        const auto& totalObstaclesj = totalObstacles[j];
+        auto& lowerj = lower[j];
+        auto& upperj = upper[j];
+      for (size_t d=0; d<dim; ++d) {
+          lowerj[d] = totalObstaclesj[d][0];
+          upperj[d] = totalObstaclesj[d][1];
+      }
+    }
+
+    // print problem
+   /* print(bilinearForm, "bilinearForm");
+    print(totalRhs, "totalRhs");
+    print(_dirichletNodes, "ignore");
+    print(totalObstacles, "totalObstacles");
+    print(lower, "lower");
+    print(upper, "upper");*/
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalX, _dirichletNodes, lower, upper, true);
+
+    nBodyAssembler.postprocess(totalX, _x);
+  };
+
+  timeStep = parset.get<size_t>("initialTime.timeStep");
+  relativeTime = parset.get<double>("initialTime.relativeTime");
+  relativeTau = parset.get<double>("initialTime.relativeTau");
+
+  bodyStates.resize(bodyCount);
+  u.resize(bodyCount);
+  v.resize(bodyCount);
+  a.resize(bodyCount);
+  alpha.resize(bodyCount);
+  weightedNormalStress.resize(bodyCount);
+
+  for (size_t i=0; i<bodyCount; i++) {
+    size_t leafVertexCount =  contactNetwork.body(i)->nVertices();
+
+    u[i].resize(leafVertexCount),
+    v[i].resize(leafVertexCount),
+    a[i].resize(leafVertexCount),
+    alpha[i].resize(leafVertexCount),
+    weightedNormalStress[i].resize(leafVertexCount),
+
+    bodyStates[i] = new BodyState<Vector, ScalarVector>(&u[i], &v[i], &a[i], &alpha[i], &weightedNormalStress[i]);
+  }
+
+  std::vector<Vector> ell0(bodyCount);
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial velocity
+    u[i] = 0.0;
+    v[i] = 0.0;
+
+    ell0[i].resize(u[i].size());
+    ell0[i] = 0.0;
+
+    contactNetwork.body(i)->externalForce()(relativeTime, ell0[i]);
+  }
+
+  // Initial displacement: Start from a situation of minimal stress,
+  // which is automatically attained in the case [v = 0 = a].
+  // Assuming dPhi(v = 0) = 0, we thus only have to solve Au = ell0
+  BitVector dirichletNodes;
+  contactNetwork.totalNodes("dirichlet", dirichletNodes);
+  /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+      bool val = false;
+      for (size_t d=0; d<dims; d++) {
+          val = val || dirichletNodes[i][d];
+      }
+
+      dirichletNodes[i] = val;
+      for (size_t d=0; d<dims; d++) {
+          dirichletNodes[i][d] = val;
+      }
+  }*/
+
+  std::cout << "solving linear problem for u..." << std::endl;
+
+  solveLinearProblem(dirichletNodes, contactNetwork.matrices().elasticity, ell0, u);
+
+  //print(u, "initial u:");
+
+  // Initial acceleration: Computed in agreement with Ma = ell0 - Au
+  // (without Dirichlet constraints), again assuming dPhi(v = 0) = 0
+  std::vector<Vector> accelerationRHS = ell0;
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial state
+    alpha[i] = parset.get<double>("boundary.friction.initialAlpha");
+
+    // Initial normal stress
+
+    const auto& body = contactNetwork.body(i);
+    /*std::vector<std::shared_ptr<typename ContactNetwork::LeafBody::BoundaryCondition>> frictionBoundaryConditions;
+    body->boundaryConditions("friction", frictionBoundaryConditions);
+    for (size_t j=0; j<frictionBoundaryConditions.size(); j++) {
+        ScalarVector frictionBoundaryStress(weightedNormalStress[i].size());
+
+        body->assembler()->assembleWeightedNormalStress(
+          *frictionBoundaryConditions[j]->boundaryPatch(), frictionBoundaryStress, body->data()->getYoungModulus(),
+          body->data()->getPoissonRatio(), u[i]);
+
+        weightedNormalStress[i] += frictionBoundaryStress;
+    }*/
+
+    Dune::MatrixVector::subtractProduct(accelerationRHS[i], *body->matrices().elasticity, u[i]);
+  }
+
+  for (size_t i=0; i<contactNetwork.nCouplings(); i++) {
+    const auto& coupling = contactNetwork.coupling(i);
+    const auto& contactCoupling = contactNetwork.nBodyAssembler().getContactCouplings()[i];
+
+    const auto nonmortarIdx = coupling->gridIdx_[0];
+    const auto& body = contactNetwork.body(nonmortarIdx);
+
+    ScalarVector frictionBoundaryStress(weightedNormalStress[nonmortarIdx].size());
+
+    body->assembler()->assembleWeightedNormalStress(
+      contactCoupling->nonmortarBoundary(), frictionBoundaryStress, body->data()->getYoungModulus(),
+      body->data()->getPoissonRatio(), u[nonmortarIdx]);
+
+    weightedNormalStress[nonmortarIdx] += frictionBoundaryStress;
+  }
+
+  //print(weightedNormalStress, "weightedNormalStress: ");
+
+  std::cout << "solving linear problem for a..." << std::endl;
+
+  BitVector noNodes(dirichletNodes.size(), false);
+  solveLinearProblem(noNodes, contactNetwork.matrices().mass, accelerationRHS, a);
+
+  //print(a, "initial a:");
+}
+
+template <class ContactNetwork>
+void relativeVelocities(const ContactNetwork& contactNetwork, const Vector& v, std::vector<Vector>& v_rel) {
+
+    const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    const size_t nBodies = nBodyAssembler.nGrids();
+   // const auto& contactCouplings = nBodyAssembler.getContactCouplings();
+
+    std::vector<const Dune::BitSetVector<1>*> bodywiseNonmortarBoundaries;
+    contactNetwork.frictionNodes(bodywiseNonmortarBoundaries);
+
+    size_t globalIdx = 0;
+    v_rel.resize(nBodies);
+    for (size_t bodyIdx=0; bodyIdx<nBodies; bodyIdx++) {
+        const auto& nonmortarBoundary = *bodywiseNonmortarBoundaries[bodyIdx];
+        auto& v_rel_ = v_rel[bodyIdx];
+
+        v_rel_.resize(nonmortarBoundary.size());
+
+        for (size_t i=0; i<v_rel_.size(); i++) {
+            if (toBool(nonmortarBoundary[i])) {
+                v_rel_[i] = v[globalIdx];
+            }
+            globalIdx++;
+        }
+    }
+
+}
+
+template <class Updaters, class ContactNetwork>
+void run(Updaters& updaters, ContactNetwork& contactNetwork,
+    const std::vector<Matrix>& velocityMatrices, const std::vector<Vector>& velocityRHSs,
+    std::vector<Vector>& velocityIterates) {
+
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+  const auto nBodies = nBodyAssembler.nGrids();
+
+  std::vector<const Matrix*> matrices_ptr(nBodies);
+  for (int i=0; i<nBodies; i++) {
+      matrices_ptr[i] = &velocityMatrices[i];
+  }
+
+  // assemble full global contact problem
+  Matrix bilinearForm;
+  nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+  Vector totalRhs;
+  nBodyAssembler.assembleRightHandSide(velocityRHSs, totalRhs);
+
+  // get lower and upper obstacles
+  const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+  Vector lower(totalObstacles.size());
+  Vector upper(totalObstacles.size());
+
+  for (size_t j=0; j<totalObstacles.size(); ++j) {
+      const auto& totalObstaclesj = totalObstacles[j];
+      auto& lowerj = lower[j];
+      auto& upperj = upper[j];
+    for (size_t d=0; d<dim; ++d) {
+        lowerj[d] = totalObstaclesj[d][0];
+        upperj[d] = totalObstaclesj[d][1];
+    }
+  }
+
+  // compute velocity obstacles
+  /*Vector vLower, vUpper;
+  std::vector<Vector> u0, v0;
+  updaters.rate_->extractOldVelocity(v0);
+  updaters.rate_->extractOldDisplacement(u0);
+
+  Vector totalu0, totalv0;
+  nBodyAssembler.nodalToTransformed(u0, totalu0);
+  nBodyAssembler.nodalToTransformed(v0, totalv0);
+
+  updaters.rate_->velocityObstacles(totalu0, lower, totalv0, vLower);
+  updaters.rate_->velocityObstacles(totalu0, upper, totalv0, vUpper);*/
+
+  const auto& errorNorms = contactNetwork.stateEnergyNorms();
+
+  auto& globalFriction = contactNetwork.globalFriction();
+
+  BitVector totalDirichletNodes;
+  contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+  size_t fixedPointMaxIterations = parset.get<size_t>("v.fpi.maximumIterations");
+  double fixedPointTolerance = parset.get<double>("v.fpi.tolerance");
+  double lambda = parset.get<double>("v.fpi.lambda");
+
+  size_t fixedPointIteration;
+  size_t multigridIterations = 0;
+
+  std::vector<ScalarVector> alpha(nBodies);
+  updaters.state_->extractAlpha(alpha);
+
+  Vector totalVelocityIterate;
+  nBodyAssembler.nodalToTransformed(velocityIterates, totalVelocityIterate);
+
+  // project in onto admissible set
+  const size_t blocksize = Vector::block_type::dimension;
+  for (size_t i=0; i<totalVelocityIterate.size(); i++) {
+      for (size_t j=0; j<blocksize; j++) {
+          if (totalVelocityIterate[i][j] < lower[i][j]) {
+              totalVelocityIterate[i][j] = lower[i][j];
+          }
+
+          if (totalVelocityIterate[i][j] > upper[i][j]) {
+              totalVelocityIterate[i][j] = upper[i][j];
+          }
+      }
+  }
+
+  for (fixedPointIteration = 0; fixedPointIteration < fixedPointMaxIterations;
+       ++fixedPointIteration) {
+
+    // contribution from nonlinearity
+    //print(alpha, "alpha: ");
+
+    globalFriction.updateAlpha(alpha);
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalVelocityIterate, totalDirichletNodes, lower, upper, false);
+
+    nBodyAssembler.postprocess(totalVelocityIterate, velocityIterates);
+
+    std::vector<Vector> v_m; //TODO : wrong, isnt used atm;
+    updaters.rate_->extractOldVelocity(v_m);
+
+    for (size_t i=0; i<v_m.size(); i++) {
+      v_m[i] *= 1.0 - lambda;
+      Dune::MatrixVector::addProduct(v_m[i], lambda, velocityIterates[i]);
+    }
+
+    // extract relative velocities in mortar basis
+    std::vector<Vector> v_rel;
+    relativeVelocities(contactNetwork, totalVelocityIterate, v_rel);
+
+    //print(v_rel, "v_rel");
+
+    std::cout << "- State problem set" << std::endl;
+
+    // solve a state problem
+    updaters.state_->solve(v_rel);
+
+    std::cout << "-- Solved" << std::endl;
+
+    std::vector<ScalarVector> newAlpha(nBodies);
+    updaters.state_->extractAlpha(newAlpha);
+
+    bool breakCriterion = true;
+    for (int i=0; i<nBodies; i++) {
+        if (alpha[i].size()==0 || newAlpha[i].size()==0)
+            continue;
+
+        //print(alpha[i], "alpha i:");
+        //print(newAlpha[i], "new alpha i:");
+        if (errorNorms[i]->diff(alpha[i], newAlpha[i]) >= fixedPointTolerance) {
+            breakCriterion = false;
+            std::cout << "fixedPoint error: " << errorNorms[i]->diff(alpha[i], newAlpha[i]) << std::endl;
+            break;
+        }
+    }
+
+    if (lambda < 1e-12 or breakCriterion) {
+      std::cout << "-FixedPointIteration finished! " << (lambda < 1e-12 ? "lambda" : "breakCriterion") << std::endl;
+      fixedPointIteration++;
+      break;
+    }
+    alpha = newAlpha;
+  }
+
+  std::cout << "-FixedPointIteration finished! " << std::endl;
+
+  if (fixedPointIteration == fixedPointMaxIterations)
+    DUNE_THROW(Dune::Exception, "FPI failed to converge");
+
+  updaters.rate_->postProcess(velocityIterates);
+}
+
+
+template <class Updaters, class ContactNetwork>
+void step(Updaters& updaters, ContactNetwork& contactNetwork)  {
+  updaters.state_->nextTimeStep();
+  updaters.rate_->nextTimeStep();
+
+  auto const newRelativeTime = relativeTime + relativeTau;
+  typename ContactNetwork::ExternalForces externalForces;
+  contactNetwork.externalForces(externalForces);
+  std::vector<Vector> ell(externalForces.size());
+  for (size_t i=0; i<externalForces.size(); i++) {
+    (*externalForces[i])(newRelativeTime, ell[i]);
+  }
+
+  std::vector<Matrix> velocityMatrix;
+  std::vector<Vector> velocityRHS;
+  std::vector<Vector> velocityIterate;
+
+  double finalTime = parset.get<double>("problem.finalTime");
+  auto const tau = relativeTau * finalTime;
+  updaters.state_->setup(tau);
+  updaters.rate_->setup(ell, tau, newRelativeTime, velocityRHS, velocityIterate, velocityMatrix);
+
+  run(updaters, contactNetwork, velocityMatrix, velocityRHS, velocityIterate);
+}
+
+template <class Updaters, class ContactNetwork>
+void advanceStep(Updaters& current, ContactNetwork& contactNetwork) {
+    step(current, contactNetwork);
+    relativeTime += relativeTau;
+}
+
+void getParameters(int argc, char *argv[]) {
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree(
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem-%dD.cfg", dim), parset);
+  Dune::ParameterTreeParser::readOptions(argc, argv, parset);
+}
+
+int main(int argc, char *argv[]) { try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    std::ofstream out(path + outputFile);
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to outputFile
+
+    std::cout << "-------------------------" << std::endl;
+    std::cout << "-- SolverFactory Test: --" << std::endl;
+    std::cout << "-------------------------" << std::endl << std::endl;
+
+    getParameters(argc, argv);
+
+    // ----------------------
+    // set up contact network
+    // ----------------------
+    StackedBlocksFactory<Grid, Vector> stackedBlocksFactory(parset);
+    using ContactNetwork = typename StackedBlocksFactory<Grid, Vector>::ContactNetwork;
+    stackedBlocksFactory.build();
+
+    ContactNetwork& contactNetwork = stackedBlocksFactory.contactNetwork();
+    bodyCount = contactNetwork.nBodies();
+
+    for (size_t i=0; i<contactNetwork.nLevels(); i++) {
+        const auto& level = *contactNetwork.level(i);
+
+        for (size_t j=0; j<level.nBodies(); j++) {
+            writeToVTK(level.body(j)->gridView(), "debug_print/bodies/", "body_" + std::to_string(j) + "_level_" + std::to_string(i));
+        }
+    }
+
+    for (size_t i=0; i<bodyCount; i++) {
+        writeToVTK(contactNetwork.body(i)->gridView(), "debug_print/bodies/", "body_" + std::to_string(i) + "_leaf");
+    }
+
+    // ----------------------------
+    // assemble contactNetwork
+    // ----------------------------
+    contactNetwork.assemble();
+
+    //printMortarBasis<Vector>(contactNetwork.nBodyAssembler());
+
+    // -----------------
+    // init input/output
+    // -----------------
+    timeSteps = parset.get<size_t>("timeSteps.timeSteps");
+    allReductionFactors.resize(timeSteps+1);
+
+    setupInitialConditions(contactNetwork);
+
+    auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    for (size_t i=0; i<bodyCount; i++) {
+      contactNetwork.body(i)->setDeformation(u[i]);
+    }
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();
+
+    // ------------------------
+    // assemble global friction
+    // ------------------------
+    contactNetwork.assembleFriction(parset.get<Config::FrictionModel>("boundary.friction.frictionModel"), weightedNormalStress);
+
+    auto& globalFriction = contactNetwork.globalFriction();
+    globalFriction.updateAlpha(alpha);
+
+    using Assembler = MyAssembler<DefLeafGridView, dim>;
+    using field_type = Matrix::field_type;
+    using MyVertexBasis = typename Assembler::VertexBasis;
+    using MyCellBasis = typename Assembler::CellBasis;
+    std::vector<Vector> vertexCoordinates(bodyCount);
+    std::vector<const MyVertexBasis* > vertexBases(bodyCount);
+    std::vector<const MyCellBasis* > cellBases(bodyCount);
+
+    for (size_t i=0; i<bodyCount; i++) {
+      const auto& body = contactNetwork.body(i);
+      vertexBases[i] = &(body->assembler()->vertexBasis);
+      cellBases[i] = &(body->assembler()->cellBasis);
+
+      auto& vertexCoords = vertexCoordinates[i];
+      vertexCoords.resize(body->nVertices());
+
+      Dune::MultipleCodimMultipleGeomTypeMapper<
+          DefLeafGridView, Dune::MCMGVertexLayout> const vertexMapper(body->gridView(), Dune::mcmgVertexLayout());
+      for (auto &&v : vertices(body->gridView()))
+        vertexCoords[vertexMapper.index(v)] = geoToPoint(v.geometry());
+    }
+
+    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "/storage/mi/podlesny/software/dune/dune-tectonic/body");
+
+    auto const report = [&](bool initial = false) {
+      if (parset.get<bool>("io.printProgress"))
+        std::cout << "timeStep = " << std::setw(6) << timeStep
+                  << ", time = " << std::setw(12) << relativeTime
+                  << ", tau = " << std::setw(12) << relativeTau
+                  << std::endl;
+
+      if (parset.get<bool>("io.vtk.write")) {
+        std::vector<ScalarVector> stress(bodyCount);
+
+        for (size_t i=0; i<bodyCount; i++) {
+          const auto& body = contactNetwork.body(i);
+          body->assembler()->assembleVonMisesStress(body->data()->getYoungModulus(),
+                                           body->data()->getPoissonRatio(),
+                                           u[i], stress[i]);
+
+        }
+
+        vtkWriter.write(timeStep, u, v, alpha, stress);
+      }
+    };
+    report(true);
+
+    // -------------------
+    // Set up TNNMG solver
+    // -------------------
+
+    /*BitVector totalDirichletNodes;
+    contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+    print(totalDirichletNodes, "totalDirichletNodes:");*/
+
+    //using Functional = Functional<Matrix&, Vector&, ZeroNonlinearity&, Vector&, Vector&, field_type>;
+    //using Functional = Functional<Matrix&, Vector&, GlobalFriction<Matrix, Vector>&, Vector&, Vector&, field_type>;
+    //using NonlinearFactory = SolverFactory<Functional, BitVector>;
+
+    using BoundaryFunctions = typename ContactNetwork::BoundaryFunctions;
+    using BoundaryNodes = typename ContactNetwork::BoundaryNodes;
+    using Updaters = Updaters<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>,
+                               StateUpdater<ScalarVector, Vector>>;
+
+    BoundaryFunctions velocityDirichletFunctions;
+    contactNetwork.boundaryFunctions("dirichlet", velocityDirichletFunctions);
+
+    BoundaryNodes dirichletNodes;
+    contactNetwork.boundaryNodes("dirichlet", dirichletNodes);
+
+    /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+        for (size_t j=0; j<dirichletNodes[i].size(); j++) {
+        print(*dirichletNodes[i][j], "dirichletNodes_body_" + std::to_string(i) + "_boundary_" + std::to_string(j));
+        }
+    }*/
+
+
+    /*for (size_t i=0; i<frictionNodes.size(); i++) {
+        print(*frictionNodes[i], "frictionNodes_body_" + std::to_string(i));
+    }*/
+
+    Updaters current(
+        initRateUpdater(
+            parset.get<Config::scheme>("timeSteps.scheme"),
+            velocityDirichletFunctions,
+            dirichletNodes,
+            contactNetwork.matrices(),
+            u,
+            v,
+            a),
+        initStateUpdater<ScalarVector, Vector>(
+            parset.get<Config::stateModel>("boundary.friction.stateModel"),
+            alpha,
+            nBodyAssembler.getContactCouplings(),
+            contactNetwork.couplings())
+            );
+
+
+
+    //const auto& stateEnergyNorms = contactNetwork.stateEnergyNorms();
+
+    for (timeStep=1; timeStep<=timeSteps; timeStep++) {
+
+      advanceStep(current, contactNetwork);
+
+      relativeTime += relativeTau;
+
+      current.rate_->extractDisplacement(u);
+      current.rate_->extractVelocity(v);
+      current.rate_->extractAcceleration(a);
+      current.state_->extractAlpha(alpha);
+
+      contactNetwork.setDeformation(u);
+
+      report();
+    }
+
+    // output reduction factors
+    size_t count = 0;
+    for (size_t i=0; i<allReductionFactors.size(); i++) {
+        count = std::max(count, allReductionFactors[i].size());
+    }
+    std::vector<double> avgReductionFactors(count);
+    for (size_t i=0; i<count; i++) {
+        avgReductionFactors[i] = 1;
+        size_t c = 0;
+
+        for (size_t j=0; j<allReductionFactors.size(); j++) {
+            if (!(i<allReductionFactors[j].size()))
+                continue;
+
+            avgReductionFactors[i] *= allReductionFactors[j][i];
+            c++;
+        }
+
+        avgReductionFactors[i] = std::pow(avgReductionFactors[i], 1.0/((double)c));
+    }
+
+    print(avgReductionFactors, "average reduction factors: ");
+
+    bool passed = true;
+
+    std::cout << "Overall, the test " << (passed ? "was successful!" : "failed!") << std::endl;
+
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+    return passed ? 0 : 1;
+
+} catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+} catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+} // end try
+} // end main

dune/tectonic/tests/tnnmgtest/linearizationtest.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#define MY_DIM 2
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <exception>
+
+#include <dune/common/exceptions.hh>
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/stdstreams.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/function.hh>
+#include <dune/common/bitsetvector.hh>
+#include <dune/common/parametertree.hh>
+#include <dune/common/parametertreeparser.hh>
+
+#include <dune/fufem/formatstring.hh>
+
+#include <dune/tnnmg/functionals/boxconstrainedquadraticfunctional.hh>
+#include <dune/tnnmg/functionals/bcqfconstrainedlinearization.hh>
+#include <dune/tnnmg/localsolvers/scalarobstaclesolver.hh>
+#include <dune/tnnmg/iterationsteps/tnnmgstep.hh>
+
+#include <dune/tectonic/geocoordinate.hh>
+
+#include "assemblers.hh"
+#include "gridselector.hh"
+#include "explicitgrid.hh"
+#include "explicitvectors.hh"
+
+#include "data-structures/enumparser.hh"
+#include "data-structures/enums.hh"
+#include "data-structures/contactnetwork.hh"
+#include "data-structures/matrices.hh"
+#include "data-structures/program_state.hh"
+
+#include "io/vtk.hh"
+
+#include "spatial-solving/tnnmg/linesearchsolver.hh"
+#include "spatial-solving/preconditioners/nbodycontacttransfer.hh"
+
+#include "factories/stackedblocksfactory.hh"
+
+#include "time-stepping/rate.hh"
+#include "time-stepping/state.hh"
+#include "time-stepping/updaters.hh"
+
+#include "utils/tobool.hh"
+#include "utils/debugutils.hh"
+
+
+const int dim = MY_DIM;
+
+Dune::ParameterTree parset;
+size_t bodyCount;
+
+std::vector<BodyState<Vector, ScalarVector>* > bodyStates;
+std::vector<Vector> u;
+std::vector<Vector> v;
+std::vector<Vector> a;
+std::vector<ScalarVector> alpha;
+std::vector<ScalarVector> weightedNormalStress;
+double relativeTime;
+double relativeTau;
+size_t timeStep = 0;
+size_t timeSteps = 0;
+
+const std::string path = "";
+const std::string outputFile = "solverfactorytest.log";
+
+std::vector<std::vector<double>> allReductionFactors;
+
+template<class IterationStepType, class NormType, class ReductionFactorContainer>
+Dune::Solvers::Criterion reductionFactorCriterion(
+      const IterationStepType& iterationStep,
+      const NormType& norm,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = norm.diff(*lastIterate, *iterationStep.getIterate());
+        double convRate = (normOfOldCorrection > 0) ? normOfCorrection / normOfOldCorrection : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+
+template<class IterationStepType, class Functional, class ReductionFactorContainer>
+Dune::Solvers::Criterion energyCriterion(
+      const IterationStepType& iterationStep,
+      const Functional& f,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = std::abs(f(*lastIterate) - f(*iterationStep.getIterate())); //norm.diff(*lastIterate, *iterationStep.getIterate());
+
+        double convRate = (normOfOldCorrection != 0.0) ? 1.0 - (normOfCorrection / normOfOldCorrection) : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+template <class ContactNetwork>
+void solveProblem(const ContactNetwork& contactNetwork,
+                  const Matrix& mat, const Vector& rhs, Vector& x,
+                  const BitVector& ignore, const Vector& lower, const Vector& upper, bool initial = false) {
+
+    using Solver = typename Dune::Solvers::LoopSolver<Vector, BitVector>;
+    using Norm =  EnergyNorm<Matrix, Vector>;
+
+    //using LocalSolver = LocalBisectionSolver;
+    //using Linearization = Linearization<Functional, BitVector>;
+
+    /*print(ignore, "ignore:");
+    for (size_t i=0; i<x.size(); i++) {
+        std::cout << x[i] << std::endl;
+    }*/
+
+    // set up reference solver
+    Vector refX = x;
+    using ContactFunctional = Dune::TNNMG::BoxConstrainedQuadraticFunctional<Matrix&, Vector&, Vector&, Vector&, double>;
+    auto I = ContactFunctional(mat, rhs, lower, upper);
+
+    std::cout << "Energy start iterate: " << I(x) << std::endl;
+
+    using LocalSolver = Dune::TNNMG::ScalarObstacleSolver;
+    auto localSolver = Dune::TNNMG::gaussSeidelLocalSolver(LocalSolver());
+
+    using NonlinearSmoother = Dune::TNNMG::NonlinearGSStep<ContactFunctional, decltype(localSolver), BitVector>;
+    auto nonlinearSmoother = std::make_shared<NonlinearSmoother>(I, refX, localSolver);
+
+    using Linearization = Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<ContactFunctional, BitVector>;
+    using DefectProjection = Dune::TNNMG::ObstacleDefectProjection;
+    using Step = Dune::TNNMG::TNNMGStep<ContactFunctional, BitVector, Linearization, DefectProjection, LocalSolver>;
+
+    // set multigrid solver
+    auto smoother = TruncatedBlockGSStep<Matrix, Vector>();
+
+    using TransferOperator = NBodyContactTransfer<ContactNetwork, Vector>;
+    using TransferOperators = std::vector<std::shared_ptr<TransferOperator>>;
+
+    TransferOperators transfer(contactNetwork.nLevels()-1);
+    for (size_t i=0; i<transfer.size(); i++) {
+        transfer[i] = std::make_shared<TransferOperator>();
+        transfer[i]->setup(contactNetwork, i, i+1);
+    }
+
+    // Remove any recompute filed so that initially the full transferoperator is assembled
+    for (size_t i=0; i<transfer.size(); i++)
+        std::dynamic_pointer_cast<TruncatedMGTransfer<Vector> >(transfer[i])->setRecomputeBitField(nullptr);
+
+    auto linearMultigridStep = std::make_shared<Dune::Solvers::MultigridStep<Matrix, Vector> >();
+    linearMultigridStep->setMGType(1, 3, 3);
+    linearMultigridStep->setSmoother(&smoother);
+    linearMultigridStep->setTransferOperators(transfer);
+
+    int mu = parset.get<int>("solver.tnnmg.main.multi"); // #multigrid steps in Newton step
+    auto step = Step(I, refX, nonlinearSmoother, linearMultigridStep, mu, DefectProjection(), LocalSolver());
+
+    // compute reference solution with generic functional and solver
+    auto norm = Norm(mat);
+
+    auto refSolver = Solver(step, parset.get<size_t>("u0.solver.maximumIterations"),
+                            parset.get<double>("u0.solver.tolerance"), norm, Solver::FULL);
+
+    step.setIgnore(ignore);
+    step.setPreSmoothingSteps(parset.get<int>("solver.tnnmg.main.pre"));
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", I(refX));
+            },
+            "   energy      ");
+
+    double initialEnergy = I(refX);
+    refSolver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = I(refX);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", step.lastDampingFactor());
+            },
+            "   damping     ");
+
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", step.linearization().truncated().count());
+            },
+            "   truncated   ");
+
+    if (timeStep>0 and initial) {
+        allReductionFactors[timeStep].resize(0);
+        refSolver.addCriterion(reductionFactorCriterion(step, norm, allReductionFactors[timeStep]));
+    }
+
+    refSolver.preprocess();
+    refSolver.solve();
+
+    //print(refX, "refX: ");
+
+    // set up solver factory solver
+    using Preconditioner = MultilevelPatchPreconditioner<ContactNetwork, Matrix, Vector>;
+
+    const auto& preconditionerParset = parset.sub("solver.tnnmg.preconditioner");
+
+    Dune::BitSetVector<1> activeLevels(contactNetwork.nLevels(), true);
+    Preconditioner preconditioner(preconditionerParset, contactNetwork, activeLevels);
+    preconditioner.setPatchDepth(preconditionerParset.get<size_t>("patchDepth"));
+    preconditioner.build();
+
+    auto cgStep = std::make_shared<Dune::Solvers::CGStep<Matrix, Vector> >();
+    cgStep->setPreconditioner(preconditioner);
+
+    // set up functional
+    auto& globalFriction = contactNetwork.globalFriction();
+
+  /*  std::vector<const Dune::BitSetVector<1>*> nodes;
+     contactNetwork.frictionNodes(nodes);
+     print(*nodes[0], "frictionNodes: ");
+     print(*nodes[1], "frictionNodes: ");
+
+     print(ignore, "ignore: ");*/
+
+    //using MyFunctional = Functional<Matrix&, Vector&, std::decay_t<decltype(globalFriction)>&, Vector&, Vector&, typename Matrix::field_type>;
+    //MyFunctional J(mat, rhs, globalFriction, lower, upper);
+    using MyFunctional = Functional<Matrix&, Vector&, ZeroNonlinearity, Vector&, Vector&, typename Matrix::field_type>;
+    MyFunctional J(mat, rhs, ZeroNonlinearity(), lower, upper);
+
+    //std::cout << "ref energy: " << J(refX) << std::endl;
+
+    // set up TNMMG solver
+    // dummy solver, uses direct solver for linear correction no matter what is set here
+    //Norm mgNorm(*linearMultigridStep);
+    //auto mgSolver = std::make_shared<Solver>(linearMultigridStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
+    Norm mgNorm(*cgStep);
+    auto mgSolver = std::make_shared<Solver>(cgStep, parset.get<int>("solver.tnnmg.main.multi"), parset.get<double>("solver.tnnmg.preconditioner.basesolver.tolerance"), mgNorm, Solver::QUIET);
+
+    using Factory = SolverFactory<MyFunctional, BitVector>;
+    Factory factory(parset.sub("solver.tnnmg"), J, ignore);
+    factory.build(mgSolver);
+
+   /* std::vector<BitVector> bodyDirichletNodes;
+    nBodyAssembler.postprocess(_dirichletNodes, bodyDirichletNodes);
+    for (size_t i=0; i<bodyDirichletNodes.size(); i++) {
+      print(bodyDirichletNodes[i], "bodyDirichletNodes_" + std::to_string(i) + ": ");
+    }*/
+
+   /* print(bilinearForm, "matrix: ");
+    print(totalX, "totalX: ");
+    print(totalRhs, "totalRhs: ");*/
+
+    auto tnnmgStep = factory.step();
+    factory.setProblem(x);
+
+    LoopSolver<Vector> solver(
+        tnnmgStep.get(), parset.get<size_t>("u0.solver.maximumIterations"),
+                parset.get<double>("u0.solver.tolerance"), &norm,
+        Solver::FULL); //, true, &refX); // absolute error
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", J(x));
+            },
+            "   energy      ");
+
+    initialEnergy = J(x);
+    solver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = J(x);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", tnnmgStep->lastDampingFactor());
+            },
+            "   damping     ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", tnnmgStep->linearization().truncated().count());
+            },
+            "   truncated   ");
+
+
+    /*std::vector<double> factors;
+    solver.addCriterion(reductionFactorCriterion(*tnnmgStep, norm, factors));
+
+    solver.addCriterion(energyCriterion(*tnnmgStep, J, factors));*/
+
+    solver.preprocess();
+    solver.solve();
+
+    auto diff = x;
+    diff -= refX;
+    std::cout << "Solver error in energy norm: " << norm(diff) << std::endl;
+
+    std::cout << "Energy end iterate: " << J(x) << std::endl;
+}
+
+
+template <class ContactNetwork>
+void setupInitialConditions(const ContactNetwork& contactNetwork) {
+  using Matrix = typename ContactNetwork::Matrix;
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+
+  std::cout << std::endl << "-- setupInitialConditions --" << std::endl;
+  std::cout << "----------------------------" << std::endl;
+
+  // Solving a linear problem with a multigrid solver
+  auto const solveLinearProblem = [&](
+      const BitVector& _dirichletNodes, const std::vector<std::shared_ptr<Matrix>>& _matrices,
+      const std::vector<Vector>& _rhs, std::vector<Vector>& _x) {
+
+    std::vector<const Matrix*> matrices_ptr(_matrices.size());
+    for (size_t i=0; i<matrices_ptr.size(); i++) {
+          matrices_ptr[i] = _matrices[i].get();
+    }
+
+    // assemble full global contact problem
+    Matrix bilinearForm;
+
+    nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+    Vector totalRhs, oldTotalRhs;
+    nBodyAssembler.assembleRightHandSide(_rhs, totalRhs);
+    oldTotalRhs = totalRhs;
+
+    Vector totalX, oldTotalX;
+    nBodyAssembler.nodalToTransformed(_x, totalX);
+    oldTotalX = totalX;
+
+    // get lower and upper obstacles
+    const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+    Vector lower(totalObstacles.size());
+    Vector upper(totalObstacles.size());
+
+    for (size_t j=0; j<totalObstacles.size(); ++j) {
+        const auto& totalObstaclesj = totalObstacles[j];
+        auto& lowerj = lower[j];
+        auto& upperj = upper[j];
+      for (size_t d=0; d<dim; ++d) {
+          lowerj[d] = totalObstaclesj[d][0];
+          upperj[d] = totalObstaclesj[d][1];
+      }
+    }
+
+    // print problem
+   /* print(bilinearForm, "bilinearForm");
+    print(totalRhs, "totalRhs");
+    print(_dirichletNodes, "ignore");
+    print(totalObstacles, "totalObstacles");
+    print(lower, "lower");
+    print(upper, "upper");*/
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalX, _dirichletNodes, lower, upper, true);
+
+    nBodyAssembler.postprocess(totalX, _x);
+  };
+
+  timeStep = parset.get<size_t>("initialTime.timeStep");
+  relativeTime = parset.get<double>("initialTime.relativeTime");
+  relativeTau = parset.get<double>("initialTime.relativeTau");
+
+  bodyStates.resize(bodyCount);
+  u.resize(bodyCount);
+  v.resize(bodyCount);
+  a.resize(bodyCount);
+  alpha.resize(bodyCount);
+  weightedNormalStress.resize(bodyCount);
+
+  for (size_t i=0; i<bodyCount; i++) {
+    size_t leafVertexCount =  contactNetwork.body(i)->nVertices();
+
+    u[i].resize(leafVertexCount),
+    v[i].resize(leafVertexCount),
+    a[i].resize(leafVertexCount),
+    alpha[i].resize(leafVertexCount),
+    weightedNormalStress[i].resize(leafVertexCount),
+
+    bodyStates[i] = new BodyState<Vector, ScalarVector>(&u[i], &v[i], &a[i], &alpha[i], &weightedNormalStress[i]);
+  }
+
+  std::vector<Vector> ell0(bodyCount);
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial velocity
+    u[i] = 0.0;
+    v[i] = 0.0;
+
+    ell0[i].resize(u[i].size());
+    ell0[i] = 0.0;
+
+    contactNetwork.body(i)->externalForce()(relativeTime, ell0[i]);
+  }
+
+  // Initial displacement: Start from a situation of minimal stress,
+  // which is automatically attained in the case [v = 0 = a].
+  // Assuming dPhi(v = 0) = 0, we thus only have to solve Au = ell0
+  BitVector dirichletNodes;
+  contactNetwork.totalNodes("dirichlet", dirichletNodes);
+  /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+      bool val = false;
+      for (size_t d=0; d<dims; d++) {
+          val = val || dirichletNodes[i][d];
+      }
+
+      dirichletNodes[i] = val;
+      for (size_t d=0; d<dims; d++) {
+          dirichletNodes[i][d] = val;
+      }
+  }*/
+
+  std::cout << "solving linear problem for u..." << std::endl;
+
+  solveLinearProblem(dirichletNodes, contactNetwork.matrices().elasticity, ell0, u);
+
+  //print(u, "initial u:");
+
+  // Initial acceleration: Computed in agreement with Ma = ell0 - Au
+  // (without Dirichlet constraints), again assuming dPhi(v = 0) = 0
+  std::vector<Vector> accelerationRHS = ell0;
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial state
+    alpha[i] = parset.get<double>("boundary.friction.initialAlpha");
+
+    // Initial normal stress
+
+    const auto& body = contactNetwork.body(i);
+    /*std::vector<std::shared_ptr<typename ContactNetwork::LeafBody::BoundaryCondition>> frictionBoundaryConditions;
+    body->boundaryConditions("friction", frictionBoundaryConditions);
+    for (size_t j=0; j<frictionBoundaryConditions.size(); j++) {
+        ScalarVector frictionBoundaryStress(weightedNormalStress[i].size());
+
+        body->assembler()->assembleWeightedNormalStress(
+          *frictionBoundaryConditions[j]->boundaryPatch(), frictionBoundaryStress, body->data()->getYoungModulus(),
+          body->data()->getPoissonRatio(), u[i]);
+
+        weightedNormalStress[i] += frictionBoundaryStress;
+    }*/
+
+    Dune::MatrixVector::subtractProduct(accelerationRHS[i], *body->matrices().elasticity, u[i]);
+  }
+
+  for (size_t i=0; i<contactNetwork.nCouplings(); i++) {
+    const auto& coupling = contactNetwork.coupling(i);
+    const auto& contactCoupling = contactNetwork.nBodyAssembler().getContactCouplings()[i];
+
+    const auto nonmortarIdx = coupling->gridIdx_[0];
+    const auto& body = contactNetwork.body(nonmortarIdx);
+
+    ScalarVector frictionBoundaryStress(weightedNormalStress[nonmortarIdx].size());
+
+    body->assembler()->assembleWeightedNormalStress(
+      contactCoupling->nonmortarBoundary(), frictionBoundaryStress, body->data()->getYoungModulus(),
+      body->data()->getPoissonRatio(), u[nonmortarIdx]);
+
+    weightedNormalStress[nonmortarIdx] += frictionBoundaryStress;
+  }
+
+  //print(weightedNormalStress, "weightedNormalStress: ");
+
+  std::cout << "solving linear problem for a..." << std::endl;
+
+  BitVector noNodes(dirichletNodes.size(), false);
+  solveLinearProblem(noNodes, contactNetwork.matrices().mass, accelerationRHS, a);
+
+  //print(a, "initial a:");
+}
+
+template <class ContactNetwork>
+void relativeVelocities(const ContactNetwork& contactNetwork, const Vector& v, std::vector<Vector>& v_rel) {
+
+    const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    const size_t nBodies = nBodyAssembler.nGrids();
+   // const auto& contactCouplings = nBodyAssembler.getContactCouplings();
+
+    std::vector<const Dune::BitSetVector<1>*> bodywiseNonmortarBoundaries;
+    contactNetwork.frictionNodes(bodywiseNonmortarBoundaries);
+
+    size_t globalIdx = 0;
+    v_rel.resize(nBodies);
+    for (size_t bodyIdx=0; bodyIdx<nBodies; bodyIdx++) {
+        const auto& nonmortarBoundary = *bodywiseNonmortarBoundaries[bodyIdx];
+        auto& v_rel_ = v_rel[bodyIdx];
+
+        v_rel_.resize(nonmortarBoundary.size());
+
+        for (size_t i=0; i<v_rel_.size(); i++) {
+            if (toBool(nonmortarBoundary[i])) {
+                v_rel_[i] = v[globalIdx];
+            }
+            globalIdx++;
+        }
+    }
+
+}
+
+template <class Updaters, class ContactNetwork>
+void run(Updaters& updaters, ContactNetwork& contactNetwork,
+    const std::vector<Matrix>& velocityMatrices, const std::vector<Vector>& velocityRHSs,
+    std::vector<Vector>& velocityIterates) {
+
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+  const auto nBodies = nBodyAssembler.nGrids();
+
+  std::vector<const Matrix*> matrices_ptr(nBodies);
+  for (int i=0; i<nBodies; i++) {
+      matrices_ptr[i] = &velocityMatrices[i];
+  }
+
+  // assemble full global contact problem
+  Matrix bilinearForm;
+  nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+  Vector totalRhs;
+  nBodyAssembler.assembleRightHandSide(velocityRHSs, totalRhs);
+
+  // get lower and upper obstacles
+  const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+  Vector lower(totalObstacles.size());
+  Vector upper(totalObstacles.size());
+
+  for (size_t j=0; j<totalObstacles.size(); ++j) {
+      const auto& totalObstaclesj = totalObstacles[j];
+      auto& lowerj = lower[j];
+      auto& upperj = upper[j];
+    for (size_t d=0; d<dim; ++d) {
+        lowerj[d] = totalObstaclesj[d][0];
+        upperj[d] = totalObstaclesj[d][1];
+    }
+  }
+
+  // compute velocity obstacles
+  /*Vector vLower, vUpper;
+  std::vector<Vector> u0, v0;
+  updaters.rate_->extractOldVelocity(v0);
+  updaters.rate_->extractOldDisplacement(u0);
+
+  Vector totalu0, totalv0;
+  nBodyAssembler.nodalToTransformed(u0, totalu0);
+  nBodyAssembler.nodalToTransformed(v0, totalv0);
+
+  updaters.rate_->velocityObstacles(totalu0, lower, totalv0, vLower);
+  updaters.rate_->velocityObstacles(totalu0, upper, totalv0, vUpper);*/
+
+  const auto& errorNorms = contactNetwork.stateEnergyNorms();
+
+  auto& globalFriction = contactNetwork.globalFriction();
+
+  BitVector totalDirichletNodes;
+  contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+  size_t fixedPointMaxIterations = parset.get<size_t>("v.fpi.maximumIterations");
+  double fixedPointTolerance = parset.get<double>("v.fpi.tolerance");
+  double lambda = parset.get<double>("v.fpi.lambda");
+
+  size_t fixedPointIteration;
+  size_t multigridIterations = 0;
+
+  std::vector<ScalarVector> alpha(nBodies);
+  updaters.state_->extractAlpha(alpha);
+
+  Vector totalVelocityIterate;
+  nBodyAssembler.nodalToTransformed(velocityIterates, totalVelocityIterate);
+
+  // project in onto admissible set
+  const size_t blocksize = Vector::block_type::dimension;
+  for (size_t i=0; i<totalVelocityIterate.size(); i++) {
+      for (size_t j=0; j<blocksize; j++) {
+          if (totalVelocityIterate[i][j] < lower[i][j]) {
+              totalVelocityIterate[i][j] = lower[i][j];
+          }
+
+          if (totalVelocityIterate[i][j] > upper[i][j]) {
+              totalVelocityIterate[i][j] = upper[i][j];
+          }
+      }
+  }
+
+  for (fixedPointIteration = 0; fixedPointIteration < fixedPointMaxIterations;
+       ++fixedPointIteration) {
+
+    // contribution from nonlinearity
+    //print(alpha, "alpha: ");
+
+    globalFriction.updateAlpha(alpha);
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalVelocityIterate, totalDirichletNodes, lower, upper, false);
+
+    nBodyAssembler.postprocess(totalVelocityIterate, velocityIterates);
+
+    std::vector<Vector> v_m; //TODO : wrong, isnt used atm;
+    updaters.rate_->extractOldVelocity(v_m);
+
+    for (size_t i=0; i<v_m.size(); i++) {
+      v_m[i] *= 1.0 - lambda;
+      Dune::MatrixVector::addProduct(v_m[i], lambda, velocityIterates[i]);
+    }
+
+    // extract relative velocities in mortar basis
+    std::vector<Vector> v_rel;
+    relativeVelocities(contactNetwork, totalVelocityIterate, v_rel);
+
+    //print(v_rel, "v_rel");
+
+    std::cout << "- State problem set" << std::endl;
+
+    // solve a state problem
+    updaters.state_->solve(v_rel);
+
+    std::cout << "-- Solved" << std::endl;
+
+    std::vector<ScalarVector> newAlpha(nBodies);
+    updaters.state_->extractAlpha(newAlpha);
+
+    bool breakCriterion = true;
+    for (int i=0; i<nBodies; i++) {
+        if (alpha[i].size()==0 || newAlpha[i].size()==0)
+            continue;
+
+        //print(alpha[i], "alpha i:");
+        //print(newAlpha[i], "new alpha i:");
+        if (errorNorms[i]->diff(alpha[i], newAlpha[i]) >= fixedPointTolerance) {
+            breakCriterion = false;
+            std::cout << "fixedPoint error: " << errorNorms[i]->diff(alpha[i], newAlpha[i]) << std::endl;
+            break;
+        }
+    }
+
+    if (lambda < 1e-12 or breakCriterion) {
+      std::cout << "-FixedPointIteration finished! " << (lambda < 1e-12 ? "lambda" : "breakCriterion") << std::endl;
+      fixedPointIteration++;
+      break;
+    }
+    alpha = newAlpha;
+  }
+
+  std::cout << "-FixedPointIteration finished! " << std::endl;
+
+  if (fixedPointIteration == fixedPointMaxIterations)
+    DUNE_THROW(Dune::Exception, "FPI failed to converge");
+
+  updaters.rate_->postProcess(velocityIterates);
+}
+
+
+template <class Updaters, class ContactNetwork>
+void step(Updaters& updaters, ContactNetwork& contactNetwork)  {
+  updaters.state_->nextTimeStep();
+  updaters.rate_->nextTimeStep();
+
+  auto const newRelativeTime = relativeTime + relativeTau;
+  typename ContactNetwork::ExternalForces externalForces;
+  contactNetwork.externalForces(externalForces);
+  std::vector<Vector> ell(externalForces.size());
+  for (size_t i=0; i<externalForces.size(); i++) {
+    (*externalForces[i])(newRelativeTime, ell[i]);
+  }
+
+  std::vector<Matrix> velocityMatrix;
+  std::vector<Vector> velocityRHS;
+  std::vector<Vector> velocityIterate;
+
+  double finalTime = parset.get<double>("problem.finalTime");
+  auto const tau = relativeTau * finalTime;
+  updaters.state_->setup(tau);
+  updaters.rate_->setup(ell, tau, newRelativeTime, velocityRHS, velocityIterate, velocityMatrix);
+
+  run(updaters, contactNetwork, velocityMatrix, velocityRHS, velocityIterate);
+}
+
+template <class Updaters, class ContactNetwork>
+void advanceStep(Updaters& current, ContactNetwork& contactNetwork) {
+    step(current, contactNetwork);
+    relativeTime += relativeTau;
+}
+
+void getParameters(int argc, char *argv[]) {
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree(
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem-%dD.cfg", dim), parset);
+  Dune::ParameterTreeParser::readOptions(argc, argv, parset);
+}
+
+int main(int argc, char *argv[]) { try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    std::ofstream out(path + outputFile);
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to outputFile
+
+    std::cout << "-------------------------" << std::endl;
+    std::cout << "-- SolverFactory Test: --" << std::endl;
+    std::cout << "-------------------------" << std::endl << std::endl;
+
+    getParameters(argc, argv);
+
+    // ----------------------
+    // set up contact network
+    // ----------------------
+    StackedBlocksFactory<Grid, Vector> stackedBlocksFactory(parset);
+    using ContactNetwork = typename StackedBlocksFactory<Grid, Vector>::ContactNetwork;
+    stackedBlocksFactory.build();
+
+    ContactNetwork& contactNetwork = stackedBlocksFactory.contactNetwork();
+    bodyCount = contactNetwork.nBodies();
+
+    for (size_t i=0; i<contactNetwork.nLevels(); i++) {
+        const auto& level = *contactNetwork.level(i);
+
+        for (size_t j=0; j<level.nBodies(); j++) {
+            writeToVTK(level.body(j)->gridView(), "debug_print/bodies/", "body_" + std::to_string(j) + "_level_" + std::to_string(i));
+        }
+    }
+
+    for (size_t i=0; i<bodyCount; i++) {
+        writeToVTK(contactNetwork.body(i)->gridView(), "debug_print/bodies/", "body_" + std::to_string(i) + "_leaf");
+    }
+
+    // ----------------------------
+    // assemble contactNetwork
+    // ----------------------------
+    contactNetwork.assemble();
+
+    //printMortarBasis<Vector>(contactNetwork.nBodyAssembler());
+
+    // -----------------
+    // init input/output
+    // -----------------
+    timeSteps = parset.get<size_t>("timeSteps.timeSteps");
+    allReductionFactors.resize(timeSteps+1);
+
+    setupInitialConditions(contactNetwork);
+
+    auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    for (size_t i=0; i<bodyCount; i++) {
+      contactNetwork.body(i)->setDeformation(u[i]);
+    }
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();
+
+    // ------------------------
+    // assemble global friction
+    // ------------------------
+    contactNetwork.assembleFriction(parset.get<Config::FrictionModel>("boundary.friction.frictionModel"), weightedNormalStress);
+
+    auto& globalFriction = contactNetwork.globalFriction();
+    globalFriction.updateAlpha(alpha);
+
+    using Assembler = MyAssembler<DefLeafGridView, dim>;
+    using field_type = Matrix::field_type;
+    using MyVertexBasis = typename Assembler::VertexBasis;
+    using MyCellBasis = typename Assembler::CellBasis;
+    std::vector<Vector> vertexCoordinates(bodyCount);
+    std::vector<const MyVertexBasis* > vertexBases(bodyCount);
+    std::vector<const MyCellBasis* > cellBases(bodyCount);
+
+    for (size_t i=0; i<bodyCount; i++) {
+      const auto& body = contactNetwork.body(i);
+      vertexBases[i] = &(body->assembler()->vertexBasis);
+      cellBases[i] = &(body->assembler()->cellBasis);
+
+      auto& vertexCoords = vertexCoordinates[i];
+      vertexCoords.resize(body->nVertices());
+
+      Dune::MultipleCodimMultipleGeomTypeMapper<
+          DefLeafGridView, Dune::MCMGVertexLayout> const vertexMapper(body->gridView(), Dune::mcmgVertexLayout());
+      for (auto &&v : vertices(body->gridView()))
+        vertexCoords[vertexMapper.index(v)] = geoToPoint(v.geometry());
+    }
+
+    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "/storage/mi/podlesny/software/dune/dune-tectonic/body");
+
+    auto const report = [&](bool initial = false) {
+      if (parset.get<bool>("io.printProgress"))
+        std::cout << "timeStep = " << std::setw(6) << timeStep
+                  << ", time = " << std::setw(12) << relativeTime
+                  << ", tau = " << std::setw(12) << relativeTau
+                  << std::endl;
+
+      if (parset.get<bool>("io.vtk.write")) {
+        std::vector<ScalarVector> stress(bodyCount);
+
+        for (size_t i=0; i<bodyCount; i++) {
+          const auto& body = contactNetwork.body(i);
+          body->assembler()->assembleVonMisesStress(body->data()->getYoungModulus(),
+                                           body->data()->getPoissonRatio(),
+                                           u[i], stress[i]);
+
+        }
+
+        vtkWriter.write(timeStep, u, v, alpha, stress);
+      }
+    };
+    report(true);
+
+    // -------------------
+    // Set up TNNMG solver
+    // -------------------
+
+    /*BitVector totalDirichletNodes;
+    contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+    print(totalDirichletNodes, "totalDirichletNodes:");*/
+
+    //using Functional = Functional<Matrix&, Vector&, ZeroNonlinearity&, Vector&, Vector&, field_type>;
+    //using Functional = Functional<Matrix&, Vector&, GlobalFriction<Matrix, Vector>&, Vector&, Vector&, field_type>;
+    //using NonlinearFactory = SolverFactory<Functional, BitVector>;
+
+    using BoundaryFunctions = typename ContactNetwork::BoundaryFunctions;
+    using BoundaryNodes = typename ContactNetwork::BoundaryNodes;
+    using Updaters = Updaters<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>,
+                               StateUpdater<ScalarVector, Vector>>;
+
+    BoundaryFunctions velocityDirichletFunctions;
+    contactNetwork.boundaryFunctions("dirichlet", velocityDirichletFunctions);
+
+    BoundaryNodes dirichletNodes;
+    contactNetwork.boundaryNodes("dirichlet", dirichletNodes);
+
+    /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+        for (size_t j=0; j<dirichletNodes[i].size(); j++) {
+        print(*dirichletNodes[i][j], "dirichletNodes_body_" + std::to_string(i) + "_boundary_" + std::to_string(j));
+        }
+    }*/
+
+
+    /*for (size_t i=0; i<frictionNodes.size(); i++) {
+        print(*frictionNodes[i], "frictionNodes_body_" + std::to_string(i));
+    }*/
+
+    Updaters current(
+        initRateUpdater(
+            parset.get<Config::scheme>("timeSteps.scheme"),
+            velocityDirichletFunctions,
+            dirichletNodes,
+            contactNetwork.matrices(),
+            u,
+            v,
+            a),
+        initStateUpdater<ScalarVector, Vector>(
+            parset.get<Config::stateModel>("boundary.friction.stateModel"),
+            alpha,
+            nBodyAssembler.getContactCouplings(),
+            contactNetwork.couplings())
+            );
+
+
+
+    //const auto& stateEnergyNorms = contactNetwork.stateEnergyNorms();
+
+    for (timeStep=1; timeStep<=timeSteps; timeStep++) {
+
+      advanceStep(current, contactNetwork);
+
+      relativeTime += relativeTau;
+
+      current.rate_->extractDisplacement(u);
+      current.rate_->extractVelocity(v);
+      current.rate_->extractAcceleration(a);
+      current.state_->extractAlpha(alpha);
+
+      contactNetwork.setDeformation(u);
+
+      report();
+    }
+
+    // output reduction factors
+    size_t count = 0;
+    for (size_t i=0; i<allReductionFactors.size(); i++) {
+        count = std::max(count, allReductionFactors[i].size());
+    }
+    std::vector<double> avgReductionFactors(count);
+    for (size_t i=0; i<count; i++) {
+        avgReductionFactors[i] = 1;
+        size_t c = 0;
+
+        for (size_t j=0; j<allReductionFactors.size(); j++) {
+            if (!(i<allReductionFactors[j].size()))
+                continue;
+
+            avgReductionFactors[i] *= allReductionFactors[j][i];
+            c++;
+        }
+
+        avgReductionFactors[i] = std::pow(avgReductionFactors[i], 1.0/((double)c));
+    }
+
+    print(avgReductionFactors, "average reduction factors: ");
+
+    bool passed = true;
+
+    std::cout << "Overall, the test " << (passed ? "was successful!" : "failed!") << std::endl;
+
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+    return passed ? 0 : 1;
+
+} catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+} catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+} // end try
+} // end main

dune/tectonic/tests/tnnmgtest/linesearchsolvertest.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#define MY_DIM 2
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <exception>
+
+#include <dune/common/exceptions.hh>
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/stdstreams.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/function.hh>
+#include <dune/common/bitsetvector.hh>
+#include <dune/common/parametertree.hh>
+#include <dune/common/parametertreeparser.hh>
+
+#include <dune/fufem/formatstring.hh>
+
+#include <dune/tnnmg/functionals/boxconstrainedquadraticfunctional.hh>
+#include <dune/tnnmg/functionals/bcqfconstrainedlinearization.hh>
+#include <dune/tnnmg/localsolvers/scalarobstaclesolver.hh>
+#include <dune/tnnmg/iterationsteps/tnnmgstep.hh>
+
+#include <dune/tectonic/geocoordinate.hh>
+
+#include "assemblers.hh"
+#include "gridselector.hh"
+#include "explicitgrid.hh"
+#include "explicitvectors.hh"
+
+#include "data-structures/enumparser.hh"
+#include "data-structures/enums.hh"
+#include "data-structures/contactnetwork.hh"
+#include "data-structures/matrices.hh"
+#include "data-structures/program_state.hh"
+
+#include "io/vtk.hh"
+
+#include "spatial-solving/tnnmg/linesearchsolver.hh"
+#include "spatial-solving/preconditioners/nbodycontacttransfer.hh"
+
+#include "factories/stackedblocksfactory.hh"
+
+#include "time-stepping/rate.hh"
+#include "time-stepping/state.hh"
+#include "time-stepping/updaters.hh"
+
+#include "utils/tobool.hh"
+#include "utils/debugutils.hh"
+
+
+const int dim = MY_DIM;
+
+Dune::ParameterTree parset;
+size_t bodyCount;
+
+std::vector<BodyState<Vector, ScalarVector>* > bodyStates;
+std::vector<Vector> u;
+std::vector<Vector> v;
+std::vector<Vector> a;
+std::vector<ScalarVector> alpha;
+std::vector<ScalarVector> weightedNormalStress;
+double relativeTime;
+double relativeTau;
+size_t timeStep = 0;
+size_t timeSteps = 0;
+
+const std::string path = "";
+const std::string outputFile = "solverfactorytest.log";
+
+std::vector<std::vector<double>> allReductionFactors;
+
+template<class IterationStepType, class NormType, class ReductionFactorContainer>
+Dune::Solvers::Criterion reductionFactorCriterion(
+      const IterationStepType& iterationStep,
+      const NormType& norm,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = norm.diff(*lastIterate, *iterationStep.getIterate());
+        double convRate = (normOfOldCorrection > 0) ? normOfCorrection / normOfOldCorrection : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+
+template<class IterationStepType, class Functional, class ReductionFactorContainer>
+Dune::Solvers::Criterion energyCriterion(
+      const IterationStepType& iterationStep,
+      const Functional& f,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = std::abs(f(*lastIterate) - f(*iterationStep.getIterate())); //norm.diff(*lastIterate, *iterationStep.getIterate());
+
+        double convRate = (normOfOldCorrection != 0.0) ? 1.0 - (normOfCorrection / normOfOldCorrection) : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+template <class ContactNetwork>
+void solveProblem(const ContactNetwork& contactNetwork,
+                  const Matrix& mat, const Vector& rhs, Vector& x,
+                  const BitVector& ignore, const Vector& lower, const Vector& upper, bool initial = false) {
+
+    using Solver = typename Dune::Solvers::LoopSolver<Vector, BitVector>;
+    using Norm =  EnergyNorm<Matrix, Vector>;
+
+    //using LocalSolver = LocalBisectionSolver;
+    //using Linearization = Linearization<Functional, BitVector>;
+
+    /*print(ignore, "ignore:");
+    for (size_t i=0; i<x.size(); i++) {
+        std::cout << x[i] << std::endl;
+    }*/
+
+    // set up reference solver
+    Vector refX = x;
+    using ContactFunctional = Dune::TNNMG::BoxConstrainedQuadraticFunctional<Matrix&, Vector&, Vector&, Vector&, double>;
+    auto I = ContactFunctional(mat, rhs, lower, upper);
+
+    std::cout << "Energy start iterate: " << I(x) << std::endl;
+
+    using LocalSolver = Dune::TNNMG::ScalarObstacleSolver;
+    auto localSolver = Dune::TNNMG::gaussSeidelLocalSolver(LocalSolver());
+
+    using NonlinearSmoother = Dune::TNNMG::NonlinearGSStep<ContactFunctional, decltype(localSolver), BitVector>;
+    auto nonlinearSmoother = std::make_shared<NonlinearSmoother>(I, refX, localSolver);
+
+    using Linearization = Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<ContactFunctional, BitVector>;
+    using DefectProjection = Dune::TNNMG::ObstacleDefectProjection;
+    using Step = Dune::TNNMG::TNNMGStep<ContactFunctional, BitVector, Linearization, DefectProjection, LocalSolver>;
+
+    // set multigrid solver
+    auto smoother = TruncatedBlockGSStep<Matrix, Vector>();
+
+    using TransferOperator = NBodyContactTransfer<ContactNetwork, Vector>;
+    using TransferOperators = std::vector<std::shared_ptr<TransferOperator>>;
+
+    TransferOperators transfer(contactNetwork.nLevels()-1);
+    for (size_t i=0; i<transfer.size(); i++) {
+        transfer[i] = std::make_shared<TransferOperator>();
+        transfer[i]->setup(contactNetwork, i, i+1);
+    }
+
+    // Remove any recompute filed so that initially the full transferoperator is assembled
+    for (size_t i=0; i<transfer.size(); i++)
+        std::dynamic_pointer_cast<TruncatedMGTransfer<Vector> >(transfer[i])->setRecomputeBitField(nullptr);
+
+    auto linearMultigridStep = std::make_shared<Dune::Solvers::MultigridStep<Matrix, Vector> >();
+    linearMultigridStep->setMGType(1, 3, 3);
+    linearMultigridStep->setSmoother(&smoother);
+    linearMultigridStep->setTransferOperators(transfer);
+
+    int mu = parset.get<int>("solver.tnnmg.main.multi"); // #multigrid steps in Newton step
+    auto step = Step(I, refX, nonlinearSmoother, linearMultigridStep, mu, DefectProjection(), LocalSolver());
+
+    // compute reference solution with generic functional and solver
+    auto norm = Norm(mat);
+
+    auto refSolver = Solver(step, parset.get<size_t>("u0.solver.maximumIterations"),
+                            parset.get<double>("u0.solver.tolerance"), norm, Solver::FULL);
+
+    step.setIgnore(ignore);
+    step.setPreSmoothingSteps(parset.get<int>("solver.tnnmg.main.pre"));
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", I(refX));
+            },
+            "   energy      ");
+
+    double initialEnergy = I(refX);
+    refSolver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = I(refX);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", step.lastDampingFactor());
+            },
+            "   damping     ");
+
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", step.linearization().truncated().count());
+            },
+            "   truncated   ");
+
+    if (timeStep>0 and initial) {
+        allReductionFactors[timeStep].resize(0);
+        refSolver.addCriterion(reductionFactorCriterion(step, norm, allReductionFactors[timeStep]));
+    }
+
+    refSolver.preprocess();
+    refSolver.solve();
+
+    //print(refX, "refX: ");
+
+    // set up solver factory solver
+    using Preconditioner = MultilevelPatchPreconditioner<ContactNetwork, Matrix, Vector>;
+
+    const auto& preconditionerParset = parset.sub("solver.tnnmg.preconditioner");
+
+    Dune::BitSetVector<1> activeLevels(contactNetwork.nLevels(), true);
+    Preconditioner preconditioner(preconditionerParset, contactNetwork, activeLevels);
+    preconditioner.setPatchDepth(preconditionerParset.get<size_t>("patchDepth"));
+    preconditioner.build();
+
+    auto cgStep = std::make_shared<Dune::Solvers::CGStep<Matrix, Vector> >();
+    cgStep->setPreconditioner(preconditioner);
+
+    // set up functional
+    auto& globalFriction = contactNetwork.globalFriction();
+
+  /*  std::vector<const Dune::BitSetVector<1>*> nodes;
+     contactNetwork.frictionNodes(nodes);
+     print(*nodes[0], "frictionNodes: ");
+     print(*nodes[1], "frictionNodes: ");
+
+     print(ignore, "ignore: ");*/
+
+    //using MyFunctional = Functional<Matrix&, Vector&, std::decay_t<decltype(globalFriction)>&, Vector&, Vector&, typename Matrix::field_type>;
+    //MyFunctional J(mat, rhs, globalFriction, lower, upper);
+    using MyFunctional = Functional<Matrix&, Vector&, ZeroNonlinearity, Vector&, Vector&, typename Matrix::field_type>;
+    MyFunctional J(mat, rhs, ZeroNonlinearity(), lower, upper);
+
+    //std::cout << "ref energy: " << J(refX) << std::endl;
+
+    // set up TNMMG solver
+    // dummy solver, uses direct solver for linear correction no matter what is set here
+    //Norm mgNorm(*linearMultigridStep);
+    //auto mgSolver = std::make_shared<Solver>(linearMultigridStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
+    Norm mgNorm(*cgStep);
+    auto mgSolver = std::make_shared<Solver>(cgStep, parset.get<int>("solver.tnnmg.main.multi"), parset.get<double>("solver.tnnmg.preconditioner.basesolver.tolerance"), mgNorm, Solver::QUIET);
+
+    using Factory = SolverFactory<MyFunctional, BitVector>;
+    Factory factory(parset.sub("solver.tnnmg"), J, ignore);
+    factory.build(mgSolver);
+
+   /* std::vector<BitVector> bodyDirichletNodes;
+    nBodyAssembler.postprocess(_dirichletNodes, bodyDirichletNodes);
+    for (size_t i=0; i<bodyDirichletNodes.size(); i++) {
+      print(bodyDirichletNodes[i], "bodyDirichletNodes_" + std::to_string(i) + ": ");
+    }*/
+
+   /* print(bilinearForm, "matrix: ");
+    print(totalX, "totalX: ");
+    print(totalRhs, "totalRhs: ");*/
+
+    auto tnnmgStep = factory.step();
+    factory.setProblem(x);
+
+    LoopSolver<Vector> solver(
+        tnnmgStep.get(), parset.get<size_t>("u0.solver.maximumIterations"),
+                parset.get<double>("u0.solver.tolerance"), &norm,
+        Solver::FULL); //, true, &refX); // absolute error
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", J(x));
+            },
+            "   energy      ");
+
+    initialEnergy = J(x);
+    solver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = J(x);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", tnnmgStep->lastDampingFactor());
+            },
+            "   damping     ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", tnnmgStep->linearization().truncated().count());
+            },
+            "   truncated   ");
+
+
+    /*std::vector<double> factors;
+    solver.addCriterion(reductionFactorCriterion(*tnnmgStep, norm, factors));
+
+    solver.addCriterion(energyCriterion(*tnnmgStep, J, factors));*/
+
+    solver.preprocess();
+    solver.solve();
+
+    auto diff = x;
+    diff -= refX;
+    std::cout << "Solver error in energy norm: " << norm(diff) << std::endl;
+
+    std::cout << "Energy end iterate: " << J(x) << std::endl;
+}
+
+
+template <class ContactNetwork>
+void setupInitialConditions(const ContactNetwork& contactNetwork) {
+  using Matrix = typename ContactNetwork::Matrix;
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+
+  std::cout << std::endl << "-- setupInitialConditions --" << std::endl;
+  std::cout << "----------------------------" << std::endl;
+
+  // Solving a linear problem with a multigrid solver
+  auto const solveLinearProblem = [&](
+      const BitVector& _dirichletNodes, const std::vector<std::shared_ptr<Matrix>>& _matrices,
+      const std::vector<Vector>& _rhs, std::vector<Vector>& _x) {
+
+    std::vector<const Matrix*> matrices_ptr(_matrices.size());
+    for (size_t i=0; i<matrices_ptr.size(); i++) {
+          matrices_ptr[i] = _matrices[i].get();
+    }
+
+    // assemble full global contact problem
+    Matrix bilinearForm;
+
+    nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+    Vector totalRhs, oldTotalRhs;
+    nBodyAssembler.assembleRightHandSide(_rhs, totalRhs);
+    oldTotalRhs = totalRhs;
+
+    Vector totalX, oldTotalX;
+    nBodyAssembler.nodalToTransformed(_x, totalX);
+    oldTotalX = totalX;
+
+    // get lower and upper obstacles
+    const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+    Vector lower(totalObstacles.size());
+    Vector upper(totalObstacles.size());
+
+    for (size_t j=0; j<totalObstacles.size(); ++j) {
+        const auto& totalObstaclesj = totalObstacles[j];
+        auto& lowerj = lower[j];
+        auto& upperj = upper[j];
+      for (size_t d=0; d<dim; ++d) {
+          lowerj[d] = totalObstaclesj[d][0];
+          upperj[d] = totalObstaclesj[d][1];
+      }
+    }
+
+    // print problem
+   /* print(bilinearForm, "bilinearForm");
+    print(totalRhs, "totalRhs");
+    print(_dirichletNodes, "ignore");
+    print(totalObstacles, "totalObstacles");
+    print(lower, "lower");
+    print(upper, "upper");*/
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalX, _dirichletNodes, lower, upper, true);
+
+    nBodyAssembler.postprocess(totalX, _x);
+  };
+
+  timeStep = parset.get<size_t>("initialTime.timeStep");
+  relativeTime = parset.get<double>("initialTime.relativeTime");
+  relativeTau = parset.get<double>("initialTime.relativeTau");
+
+  bodyStates.resize(bodyCount);
+  u.resize(bodyCount);
+  v.resize(bodyCount);
+  a.resize(bodyCount);
+  alpha.resize(bodyCount);
+  weightedNormalStress.resize(bodyCount);
+
+  for (size_t i=0; i<bodyCount; i++) {
+    size_t leafVertexCount =  contactNetwork.body(i)->nVertices();
+
+    u[i].resize(leafVertexCount),
+    v[i].resize(leafVertexCount),
+    a[i].resize(leafVertexCount),
+    alpha[i].resize(leafVertexCount),
+    weightedNormalStress[i].resize(leafVertexCount),
+
+    bodyStates[i] = new BodyState<Vector, ScalarVector>(&u[i], &v[i], &a[i], &alpha[i], &weightedNormalStress[i]);
+  }
+
+  std::vector<Vector> ell0(bodyCount);
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial velocity
+    u[i] = 0.0;
+    v[i] = 0.0;
+
+    ell0[i].resize(u[i].size());
+    ell0[i] = 0.0;
+
+    contactNetwork.body(i)->externalForce()(relativeTime, ell0[i]);
+  }
+
+  // Initial displacement: Start from a situation of minimal stress,
+  // which is automatically attained in the case [v = 0 = a].
+  // Assuming dPhi(v = 0) = 0, we thus only have to solve Au = ell0
+  BitVector dirichletNodes;
+  contactNetwork.totalNodes("dirichlet", dirichletNodes);
+  /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+      bool val = false;
+      for (size_t d=0; d<dims; d++) {
+          val = val || dirichletNodes[i][d];
+      }
+
+      dirichletNodes[i] = val;
+      for (size_t d=0; d<dims; d++) {
+          dirichletNodes[i][d] = val;
+      }
+  }*/
+
+  std::cout << "solving linear problem for u..." << std::endl;
+
+  solveLinearProblem(dirichletNodes, contactNetwork.matrices().elasticity, ell0, u);
+
+  //print(u, "initial u:");
+
+  // Initial acceleration: Computed in agreement with Ma = ell0 - Au
+  // (without Dirichlet constraints), again assuming dPhi(v = 0) = 0
+  std::vector<Vector> accelerationRHS = ell0;
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial state
+    alpha[i] = parset.get<double>("boundary.friction.initialAlpha");
+
+    // Initial normal stress
+
+    const auto& body = contactNetwork.body(i);
+    /*std::vector<std::shared_ptr<typename ContactNetwork::LeafBody::BoundaryCondition>> frictionBoundaryConditions;
+    body->boundaryConditions("friction", frictionBoundaryConditions);
+    for (size_t j=0; j<frictionBoundaryConditions.size(); j++) {
+        ScalarVector frictionBoundaryStress(weightedNormalStress[i].size());
+
+        body->assembler()->assembleWeightedNormalStress(
+          *frictionBoundaryConditions[j]->boundaryPatch(), frictionBoundaryStress, body->data()->getYoungModulus(),
+          body->data()->getPoissonRatio(), u[i]);
+
+        weightedNormalStress[i] += frictionBoundaryStress;
+    }*/
+
+    Dune::MatrixVector::subtractProduct(accelerationRHS[i], *body->matrices().elasticity, u[i]);
+  }
+
+  for (size_t i=0; i<contactNetwork.nCouplings(); i++) {
+    const auto& coupling = contactNetwork.coupling(i);
+    const auto& contactCoupling = contactNetwork.nBodyAssembler().getContactCouplings()[i];
+
+    const auto nonmortarIdx = coupling->gridIdx_[0];
+    const auto& body = contactNetwork.body(nonmortarIdx);
+
+    ScalarVector frictionBoundaryStress(weightedNormalStress[nonmortarIdx].size());
+
+    body->assembler()->assembleWeightedNormalStress(
+      contactCoupling->nonmortarBoundary(), frictionBoundaryStress, body->data()->getYoungModulus(),
+      body->data()->getPoissonRatio(), u[nonmortarIdx]);
+
+    weightedNormalStress[nonmortarIdx] += frictionBoundaryStress;
+  }
+
+  //print(weightedNormalStress, "weightedNormalStress: ");
+
+  std::cout << "solving linear problem for a..." << std::endl;
+
+  BitVector noNodes(dirichletNodes.size(), false);
+  solveLinearProblem(noNodes, contactNetwork.matrices().mass, accelerationRHS, a);
+
+  //print(a, "initial a:");
+}
+
+template <class ContactNetwork>
+void relativeVelocities(const ContactNetwork& contactNetwork, const Vector& v, std::vector<Vector>& v_rel) {
+
+    const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    const size_t nBodies = nBodyAssembler.nGrids();
+   // const auto& contactCouplings = nBodyAssembler.getContactCouplings();
+
+    std::vector<const Dune::BitSetVector<1>*> bodywiseNonmortarBoundaries;
+    contactNetwork.frictionNodes(bodywiseNonmortarBoundaries);
+
+    size_t globalIdx = 0;
+    v_rel.resize(nBodies);
+    for (size_t bodyIdx=0; bodyIdx<nBodies; bodyIdx++) {
+        const auto& nonmortarBoundary = *bodywiseNonmortarBoundaries[bodyIdx];
+        auto& v_rel_ = v_rel[bodyIdx];
+
+        v_rel_.resize(nonmortarBoundary.size());
+
+        for (size_t i=0; i<v_rel_.size(); i++) {
+            if (toBool(nonmortarBoundary[i])) {
+                v_rel_[i] = v[globalIdx];
+            }
+            globalIdx++;
+        }
+    }
+
+}
+
+template <class Updaters, class ContactNetwork>
+void run(Updaters& updaters, ContactNetwork& contactNetwork,
+    const std::vector<Matrix>& velocityMatrices, const std::vector<Vector>& velocityRHSs,
+    std::vector<Vector>& velocityIterates) {
+
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+  const auto nBodies = nBodyAssembler.nGrids();
+
+  std::vector<const Matrix*> matrices_ptr(nBodies);
+  for (int i=0; i<nBodies; i++) {
+      matrices_ptr[i] = &velocityMatrices[i];
+  }
+
+  // assemble full global contact problem
+  Matrix bilinearForm;
+  nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+  Vector totalRhs;
+  nBodyAssembler.assembleRightHandSide(velocityRHSs, totalRhs);
+
+  // get lower and upper obstacles
+  const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+  Vector lower(totalObstacles.size());
+  Vector upper(totalObstacles.size());
+
+  for (size_t j=0; j<totalObstacles.size(); ++j) {
+      const auto& totalObstaclesj = totalObstacles[j];
+      auto& lowerj = lower[j];
+      auto& upperj = upper[j];
+    for (size_t d=0; d<dim; ++d) {
+        lowerj[d] = totalObstaclesj[d][0];
+        upperj[d] = totalObstaclesj[d][1];
+    }
+  }
+
+  // compute velocity obstacles
+  /*Vector vLower, vUpper;
+  std::vector<Vector> u0, v0;
+  updaters.rate_->extractOldVelocity(v0);
+  updaters.rate_->extractOldDisplacement(u0);
+
+  Vector totalu0, totalv0;
+  nBodyAssembler.nodalToTransformed(u0, totalu0);
+  nBodyAssembler.nodalToTransformed(v0, totalv0);
+
+  updaters.rate_->velocityObstacles(totalu0, lower, totalv0, vLower);
+  updaters.rate_->velocityObstacles(totalu0, upper, totalv0, vUpper);*/
+
+  const auto& errorNorms = contactNetwork.stateEnergyNorms();
+
+  auto& globalFriction = contactNetwork.globalFriction();
+
+  BitVector totalDirichletNodes;
+  contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+  size_t fixedPointMaxIterations = parset.get<size_t>("v.fpi.maximumIterations");
+  double fixedPointTolerance = parset.get<double>("v.fpi.tolerance");
+  double lambda = parset.get<double>("v.fpi.lambda");
+
+  size_t fixedPointIteration;
+  size_t multigridIterations = 0;
+
+  std::vector<ScalarVector> alpha(nBodies);
+  updaters.state_->extractAlpha(alpha);
+
+  Vector totalVelocityIterate;
+  nBodyAssembler.nodalToTransformed(velocityIterates, totalVelocityIterate);
+
+  // project in onto admissible set
+  const size_t blocksize = Vector::block_type::dimension;
+  for (size_t i=0; i<totalVelocityIterate.size(); i++) {
+      for (size_t j=0; j<blocksize; j++) {
+          if (totalVelocityIterate[i][j] < lower[i][j]) {
+              totalVelocityIterate[i][j] = lower[i][j];
+          }
+
+          if (totalVelocityIterate[i][j] > upper[i][j]) {
+              totalVelocityIterate[i][j] = upper[i][j];
+          }
+      }
+  }
+
+  for (fixedPointIteration = 0; fixedPointIteration < fixedPointMaxIterations;
+       ++fixedPointIteration) {
+
+    // contribution from nonlinearity
+    //print(alpha, "alpha: ");
+
+    globalFriction.updateAlpha(alpha);
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalVelocityIterate, totalDirichletNodes, lower, upper, false);
+
+    nBodyAssembler.postprocess(totalVelocityIterate, velocityIterates);
+
+    std::vector<Vector> v_m; //TODO : wrong, isnt used atm;
+    updaters.rate_->extractOldVelocity(v_m);
+
+    for (size_t i=0; i<v_m.size(); i++) {
+      v_m[i] *= 1.0 - lambda;
+      Dune::MatrixVector::addProduct(v_m[i], lambda, velocityIterates[i]);
+    }
+
+    // extract relative velocities in mortar basis
+    std::vector<Vector> v_rel;
+    relativeVelocities(contactNetwork, totalVelocityIterate, v_rel);
+
+    //print(v_rel, "v_rel");
+
+    std::cout << "- State problem set" << std::endl;
+
+    // solve a state problem
+    updaters.state_->solve(v_rel);
+
+    std::cout << "-- Solved" << std::endl;
+
+    std::vector<ScalarVector> newAlpha(nBodies);
+    updaters.state_->extractAlpha(newAlpha);
+
+    bool breakCriterion = true;
+    for (int i=0; i<nBodies; i++) {
+        if (alpha[i].size()==0 || newAlpha[i].size()==0)
+            continue;
+
+        //print(alpha[i], "alpha i:");
+        //print(newAlpha[i], "new alpha i:");
+        if (errorNorms[i]->diff(alpha[i], newAlpha[i]) >= fixedPointTolerance) {
+            breakCriterion = false;
+            std::cout << "fixedPoint error: " << errorNorms[i]->diff(alpha[i], newAlpha[i]) << std::endl;
+            break;
+        }
+    }
+
+    if (lambda < 1e-12 or breakCriterion) {
+      std::cout << "-FixedPointIteration finished! " << (lambda < 1e-12 ? "lambda" : "breakCriterion") << std::endl;
+      fixedPointIteration++;
+      break;
+    }
+    alpha = newAlpha;
+  }
+
+  std::cout << "-FixedPointIteration finished! " << std::endl;
+
+  if (fixedPointIteration == fixedPointMaxIterations)
+    DUNE_THROW(Dune::Exception, "FPI failed to converge");
+
+  updaters.rate_->postProcess(velocityIterates);
+}
+
+
+template <class Updaters, class ContactNetwork>
+void step(Updaters& updaters, ContactNetwork& contactNetwork)  {
+  updaters.state_->nextTimeStep();
+  updaters.rate_->nextTimeStep();
+
+  auto const newRelativeTime = relativeTime + relativeTau;
+  typename ContactNetwork::ExternalForces externalForces;
+  contactNetwork.externalForces(externalForces);
+  std::vector<Vector> ell(externalForces.size());
+  for (size_t i=0; i<externalForces.size(); i++) {
+    (*externalForces[i])(newRelativeTime, ell[i]);
+  }
+
+  std::vector<Matrix> velocityMatrix;
+  std::vector<Vector> velocityRHS;
+  std::vector<Vector> velocityIterate;
+
+  double finalTime = parset.get<double>("problem.finalTime");
+  auto const tau = relativeTau * finalTime;
+  updaters.state_->setup(tau);
+  updaters.rate_->setup(ell, tau, newRelativeTime, velocityRHS, velocityIterate, velocityMatrix);
+
+  run(updaters, contactNetwork, velocityMatrix, velocityRHS, velocityIterate);
+}
+
+template <class Updaters, class ContactNetwork>
+void advanceStep(Updaters& current, ContactNetwork& contactNetwork) {
+    step(current, contactNetwork);
+    relativeTime += relativeTau;
+}
+
+void getParameters(int argc, char *argv[]) {
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree(
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem-%dD.cfg", dim), parset);
+  Dune::ParameterTreeParser::readOptions(argc, argv, parset);
+}
+
+int main(int argc, char *argv[]) { try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    std::ofstream out(path + outputFile);
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to outputFile
+
+    std::cout << "-------------------------" << std::endl;
+    std::cout << "-- SolverFactory Test: --" << std::endl;
+    std::cout << "-------------------------" << std::endl << std::endl;
+
+    getParameters(argc, argv);
+
+    // ----------------------
+    // set up contact network
+    // ----------------------
+    StackedBlocksFactory<Grid, Vector> stackedBlocksFactory(parset);
+    using ContactNetwork = typename StackedBlocksFactory<Grid, Vector>::ContactNetwork;
+    stackedBlocksFactory.build();
+
+    ContactNetwork& contactNetwork = stackedBlocksFactory.contactNetwork();
+    bodyCount = contactNetwork.nBodies();
+
+    for (size_t i=0; i<contactNetwork.nLevels(); i++) {
+        const auto& level = *contactNetwork.level(i);
+
+        for (size_t j=0; j<level.nBodies(); j++) {
+            writeToVTK(level.body(j)->gridView(), "debug_print/bodies/", "body_" + std::to_string(j) + "_level_" + std::to_string(i));
+        }
+    }
+
+    for (size_t i=0; i<bodyCount; i++) {
+        writeToVTK(contactNetwork.body(i)->gridView(), "debug_print/bodies/", "body_" + std::to_string(i) + "_leaf");
+    }
+
+    // ----------------------------
+    // assemble contactNetwork
+    // ----------------------------
+    contactNetwork.assemble();
+
+    //printMortarBasis<Vector>(contactNetwork.nBodyAssembler());
+
+    // -----------------
+    // init input/output
+    // -----------------
+    timeSteps = parset.get<size_t>("timeSteps.timeSteps");
+    allReductionFactors.resize(timeSteps+1);
+
+    setupInitialConditions(contactNetwork);
+
+    auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    for (size_t i=0; i<bodyCount; i++) {
+      contactNetwork.body(i)->setDeformation(u[i]);
+    }
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();
+
+    // ------------------------
+    // assemble global friction
+    // ------------------------
+    contactNetwork.assembleFriction(parset.get<Config::FrictionModel>("boundary.friction.frictionModel"), weightedNormalStress);
+
+    auto& globalFriction = contactNetwork.globalFriction();
+    globalFriction.updateAlpha(alpha);
+
+    using Assembler = MyAssembler<DefLeafGridView, dim>;
+    using field_type = Matrix::field_type;
+    using MyVertexBasis = typename Assembler::VertexBasis;
+    using MyCellBasis = typename Assembler::CellBasis;
+    std::vector<Vector> vertexCoordinates(bodyCount);
+    std::vector<const MyVertexBasis* > vertexBases(bodyCount);
+    std::vector<const MyCellBasis* > cellBases(bodyCount);
+
+    for (size_t i=0; i<bodyCount; i++) {
+      const auto& body = contactNetwork.body(i);
+      vertexBases[i] = &(body->assembler()->vertexBasis);
+      cellBases[i] = &(body->assembler()->cellBasis);
+
+      auto& vertexCoords = vertexCoordinates[i];
+      vertexCoords.resize(body->nVertices());
+
+      Dune::MultipleCodimMultipleGeomTypeMapper<
+          DefLeafGridView, Dune::MCMGVertexLayout> const vertexMapper(body->gridView(), Dune::mcmgVertexLayout());
+      for (auto &&v : vertices(body->gridView()))
+        vertexCoords[vertexMapper.index(v)] = geoToPoint(v.geometry());
+    }
+
+    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "/storage/mi/podlesny/software/dune/dune-tectonic/body");
+
+    auto const report = [&](bool initial = false) {
+      if (parset.get<bool>("io.printProgress"))
+        std::cout << "timeStep = " << std::setw(6) << timeStep
+                  << ", time = " << std::setw(12) << relativeTime
+                  << ", tau = " << std::setw(12) << relativeTau
+                  << std::endl;
+
+      if (parset.get<bool>("io.vtk.write")) {
+        std::vector<ScalarVector> stress(bodyCount);
+
+        for (size_t i=0; i<bodyCount; i++) {
+          const auto& body = contactNetwork.body(i);
+          body->assembler()->assembleVonMisesStress(body->data()->getYoungModulus(),
+                                           body->data()->getPoissonRatio(),
+                                           u[i], stress[i]);
+
+        }
+
+        vtkWriter.write(timeStep, u, v, alpha, stress);
+      }
+    };
+    report(true);
+
+    // -------------------
+    // Set up TNNMG solver
+    // -------------------
+
+    /*BitVector totalDirichletNodes;
+    contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+    print(totalDirichletNodes, "totalDirichletNodes:");*/
+
+    //using Functional = Functional<Matrix&, Vector&, ZeroNonlinearity&, Vector&, Vector&, field_type>;
+    //using Functional = Functional<Matrix&, Vector&, GlobalFriction<Matrix, Vector>&, Vector&, Vector&, field_type>;
+    //using NonlinearFactory = SolverFactory<Functional, BitVector>;
+
+    using BoundaryFunctions = typename ContactNetwork::BoundaryFunctions;
+    using BoundaryNodes = typename ContactNetwork::BoundaryNodes;
+    using Updaters = Updaters<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>,
+                               StateUpdater<ScalarVector, Vector>>;
+
+    BoundaryFunctions velocityDirichletFunctions;
+    contactNetwork.boundaryFunctions("dirichlet", velocityDirichletFunctions);
+
+    BoundaryNodes dirichletNodes;
+    contactNetwork.boundaryNodes("dirichlet", dirichletNodes);
+
+    /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+        for (size_t j=0; j<dirichletNodes[i].size(); j++) {
+        print(*dirichletNodes[i][j], "dirichletNodes_body_" + std::to_string(i) + "_boundary_" + std::to_string(j));
+        }
+    }*/
+
+
+    /*for (size_t i=0; i<frictionNodes.size(); i++) {
+        print(*frictionNodes[i], "frictionNodes_body_" + std::to_string(i));
+    }*/
+
+    Updaters current(
+        initRateUpdater(
+            parset.get<Config::scheme>("timeSteps.scheme"),
+            velocityDirichletFunctions,
+            dirichletNodes,
+            contactNetwork.matrices(),
+            u,
+            v,
+            a),
+        initStateUpdater<ScalarVector, Vector>(
+            parset.get<Config::stateModel>("boundary.friction.stateModel"),
+            alpha,
+            nBodyAssembler.getContactCouplings(),
+            contactNetwork.couplings())
+            );
+
+
+
+    //const auto& stateEnergyNorms = contactNetwork.stateEnergyNorms();
+
+    for (timeStep=1; timeStep<=timeSteps; timeStep++) {
+
+      advanceStep(current, contactNetwork);
+
+      relativeTime += relativeTau;
+
+      current.rate_->extractDisplacement(u);
+      current.rate_->extractVelocity(v);
+      current.rate_->extractAcceleration(a);
+      current.state_->extractAlpha(alpha);
+
+      contactNetwork.setDeformation(u);
+
+      report();
+    }
+
+    // output reduction factors
+    size_t count = 0;
+    for (size_t i=0; i<allReductionFactors.size(); i++) {
+        count = std::max(count, allReductionFactors[i].size());
+    }
+    std::vector<double> avgReductionFactors(count);
+    for (size_t i=0; i<count; i++) {
+        avgReductionFactors[i] = 1;
+        size_t c = 0;
+
+        for (size_t j=0; j<allReductionFactors.size(); j++) {
+            if (!(i<allReductionFactors[j].size()))
+                continue;
+
+            avgReductionFactors[i] *= allReductionFactors[j][i];
+            c++;
+        }
+
+        avgReductionFactors[i] = std::pow(avgReductionFactors[i], 1.0/((double)c));
+    }
+
+    print(avgReductionFactors, "average reduction factors: ");
+
+    bool passed = true;
+
+    std::cout << "Overall, the test " << (passed ? "was successful!" : "failed!") << std::endl;
+
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+    return passed ? 0 : 1;
+
+} catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+} catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+} // end try
+} // end main

dune/tectonic/tests/tnnmgtest/localbisectionsolvertest.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#define MY_DIM 2
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <exception>
+
+#include <dune/common/exceptions.hh>
+#include <dune/common/parallel/mpihelper.hh>
+#include <dune/common/stdstreams.hh>
+#include <dune/common/fvector.hh>
+#include <dune/common/function.hh>
+#include <dune/common/bitsetvector.hh>
+#include <dune/common/parametertree.hh>
+#include <dune/common/parametertreeparser.hh>
+
+#include <dune/fufem/formatstring.hh>
+
+#include <dune/tnnmg/functionals/boxconstrainedquadraticfunctional.hh>
+#include <dune/tnnmg/functionals/bcqfconstrainedlinearization.hh>
+#include <dune/tnnmg/localsolvers/scalarobstaclesolver.hh>
+#include <dune/tnnmg/iterationsteps/tnnmgstep.hh>
+
+#include <dune/tectonic/geocoordinate.hh>
+
+#include "assemblers.hh"
+#include "gridselector.hh"
+#include "explicitgrid.hh"
+#include "explicitvectors.hh"
+
+#include "data-structures/enumparser.hh"
+#include "data-structures/enums.hh"
+#include "data-structures/contactnetwork.hh"
+#include "data-structures/matrices.hh"
+#include "data-structures/program_state.hh"
+
+#include "io/vtk.hh"
+
+#include "spatial-solving/tnnmg/linesearchsolver.hh"
+#include "spatial-solving/preconditioners/nbodycontacttransfer.hh"
+
+#include "factories/stackedblocksfactory.hh"
+
+#include "time-stepping/rate.hh"
+#include "time-stepping/state.hh"
+#include "time-stepping/updaters.hh"
+
+#include "utils/tobool.hh"
+#include "utils/debugutils.hh"
+
+
+const int dim = MY_DIM;
+
+Dune::ParameterTree parset;
+size_t bodyCount;
+
+std::vector<BodyState<Vector, ScalarVector>* > bodyStates;
+std::vector<Vector> u;
+std::vector<Vector> v;
+std::vector<Vector> a;
+std::vector<ScalarVector> alpha;
+std::vector<ScalarVector> weightedNormalStress;
+double relativeTime;
+double relativeTau;
+size_t timeStep = 0;
+size_t timeSteps = 0;
+
+const std::string path = "";
+const std::string outputFile = "solverfactorytest.log";
+
+std::vector<std::vector<double>> allReductionFactors;
+
+template<class IterationStepType, class NormType, class ReductionFactorContainer>
+Dune::Solvers::Criterion reductionFactorCriterion(
+      const IterationStepType& iterationStep,
+      const NormType& norm,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = norm.diff(*lastIterate, *iterationStep.getIterate());
+        double convRate = (normOfOldCorrection > 0) ? normOfCorrection / normOfOldCorrection : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+
+template<class IterationStepType, class Functional, class ReductionFactorContainer>
+Dune::Solvers::Criterion energyCriterion(
+      const IterationStepType& iterationStep,
+      const Functional& f,
+      ReductionFactorContainer& reductionFactors)
+{
+  double normOfOldCorrection = 1;
+  auto lastIterate = std::make_shared<typename IterationStepType::Vector>(*iterationStep.getIterate());
+
+  return Dune::Solvers::Criterion(
+      [&, lastIterate, normOfOldCorrection] () mutable {
+        double normOfCorrection = std::abs(f(*lastIterate) - f(*iterationStep.getIterate())); //norm.diff(*lastIterate, *iterationStep.getIterate());
+
+        double convRate = (normOfOldCorrection != 0.0) ? 1.0 - (normOfCorrection / normOfOldCorrection) : 0.0;
+
+        if (convRate>1.0)
+            std::cout << "Solver convergence rate of " << convRate << std::endl;
+
+        normOfOldCorrection = normOfCorrection;
+        *lastIterate = *iterationStep.getIterate();
+
+        reductionFactors.push_back(convRate);
+        return std::make_tuple(convRate < 0, Dune::formatString(" % '.5f", convRate));
+      },
+      " reductionFactor");
+}
+
+template <class ContactNetwork>
+void solveProblem(const ContactNetwork& contactNetwork,
+                  const Matrix& mat, const Vector& rhs, Vector& x,
+                  const BitVector& ignore, const Vector& lower, const Vector& upper, bool initial = false) {
+
+    using Solver = typename Dune::Solvers::LoopSolver<Vector, BitVector>;
+    using Norm =  EnergyNorm<Matrix, Vector>;
+
+    //using LocalSolver = LocalBisectionSolver;
+    //using Linearization = Linearization<Functional, BitVector>;
+
+    /*print(ignore, "ignore:");
+    for (size_t i=0; i<x.size(); i++) {
+        std::cout << x[i] << std::endl;
+    }*/
+
+    // set up reference solver
+    Vector refX = x;
+    using ContactFunctional = Dune::TNNMG::BoxConstrainedQuadraticFunctional<Matrix&, Vector&, Vector&, Vector&, double>;
+    auto I = ContactFunctional(mat, rhs, lower, upper);
+
+    std::cout << "Energy start iterate: " << I(x) << std::endl;
+
+    using LocalSolver = Dune::TNNMG::ScalarObstacleSolver;
+    auto localSolver = Dune::TNNMG::gaussSeidelLocalSolver(LocalSolver());
+
+    using NonlinearSmoother = Dune::TNNMG::NonlinearGSStep<ContactFunctional, decltype(localSolver), BitVector>;
+    auto nonlinearSmoother = std::make_shared<NonlinearSmoother>(I, refX, localSolver);
+
+    using Linearization = Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<ContactFunctional, BitVector>;
+    using DefectProjection = Dune::TNNMG::ObstacleDefectProjection;
+    using Step = Dune::TNNMG::TNNMGStep<ContactFunctional, BitVector, Linearization, DefectProjection, LocalSolver>;
+
+    // set multigrid solver
+    auto smoother = TruncatedBlockGSStep<Matrix, Vector>();
+
+    using TransferOperator = NBodyContactTransfer<ContactNetwork, Vector>;
+    using TransferOperators = std::vector<std::shared_ptr<TransferOperator>>;
+
+    TransferOperators transfer(contactNetwork.nLevels()-1);
+    for (size_t i=0; i<transfer.size(); i++) {
+        transfer[i] = std::make_shared<TransferOperator>();
+        transfer[i]->setup(contactNetwork, i, i+1);
+    }
+
+    // Remove any recompute filed so that initially the full transferoperator is assembled
+    for (size_t i=0; i<transfer.size(); i++)
+        std::dynamic_pointer_cast<TruncatedMGTransfer<Vector> >(transfer[i])->setRecomputeBitField(nullptr);
+
+    auto linearMultigridStep = std::make_shared<Dune::Solvers::MultigridStep<Matrix, Vector> >();
+    linearMultigridStep->setMGType(1, 3, 3);
+    linearMultigridStep->setSmoother(&smoother);
+    linearMultigridStep->setTransferOperators(transfer);
+
+    int mu = parset.get<int>("solver.tnnmg.main.multi"); // #multigrid steps in Newton step
+    auto step = Step(I, refX, nonlinearSmoother, linearMultigridStep, mu, DefectProjection(), LocalSolver());
+
+    // compute reference solution with generic functional and solver
+    auto norm = Norm(mat);
+
+    auto refSolver = Solver(step, parset.get<size_t>("u0.solver.maximumIterations"),
+                            parset.get<double>("u0.solver.tolerance"), norm, Solver::FULL);
+
+    step.setIgnore(ignore);
+    step.setPreSmoothingSteps(parset.get<int>("solver.tnnmg.main.pre"));
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", I(refX));
+            },
+            "   energy      ");
+
+    double initialEnergy = I(refX);
+    refSolver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = I(refX);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", step.lastDampingFactor());
+            },
+            "   damping     ");
+
+
+    refSolver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", step.linearization().truncated().count());
+            },
+            "   truncated   ");
+
+    if (timeStep>0 and initial) {
+        allReductionFactors[timeStep].resize(0);
+        refSolver.addCriterion(reductionFactorCriterion(step, norm, allReductionFactors[timeStep]));
+    }
+
+    refSolver.preprocess();
+    refSolver.solve();
+
+    //print(refX, "refX: ");
+
+    // set up solver factory solver
+    using Preconditioner = MultilevelPatchPreconditioner<ContactNetwork, Matrix, Vector>;
+
+    const auto& preconditionerParset = parset.sub("solver.tnnmg.preconditioner");
+
+    Dune::BitSetVector<1> activeLevels(contactNetwork.nLevels(), true);
+    Preconditioner preconditioner(preconditionerParset, contactNetwork, activeLevels);
+    preconditioner.setPatchDepth(preconditionerParset.get<size_t>("patchDepth"));
+    preconditioner.build();
+
+    auto cgStep = std::make_shared<Dune::Solvers::CGStep<Matrix, Vector> >();
+    cgStep->setPreconditioner(preconditioner);
+
+    // set up functional
+    auto& globalFriction = contactNetwork.globalFriction();
+
+  /*  std::vector<const Dune::BitSetVector<1>*> nodes;
+     contactNetwork.frictionNodes(nodes);
+     print(*nodes[0], "frictionNodes: ");
+     print(*nodes[1], "frictionNodes: ");
+
+     print(ignore, "ignore: ");*/
+
+    //using MyFunctional = Functional<Matrix&, Vector&, std::decay_t<decltype(globalFriction)>&, Vector&, Vector&, typename Matrix::field_type>;
+    //MyFunctional J(mat, rhs, globalFriction, lower, upper);
+    using MyFunctional = Functional<Matrix&, Vector&, ZeroNonlinearity, Vector&, Vector&, typename Matrix::field_type>;
+    MyFunctional J(mat, rhs, ZeroNonlinearity(), lower, upper);
+
+    //std::cout << "ref energy: " << J(refX) << std::endl;
+
+    // set up TNMMG solver
+    // dummy solver, uses direct solver for linear correction no matter what is set here
+    //Norm mgNorm(*linearMultigridStep);
+    //auto mgSolver = std::make_shared<Solver>(linearMultigridStep, parset.get<size_t>("solver.tnnmg.linear.maximumIterations"), parset.get<double>("solver.tnnmg.linear.tolerance"), mgNorm, Solver::QUIET);
+    Norm mgNorm(*cgStep);
+    auto mgSolver = std::make_shared<Solver>(cgStep, parset.get<int>("solver.tnnmg.main.multi"), parset.get<double>("solver.tnnmg.preconditioner.basesolver.tolerance"), mgNorm, Solver::QUIET);
+
+    using Factory = SolverFactory<MyFunctional, BitVector>;
+    Factory factory(parset.sub("solver.tnnmg"), J, ignore);
+    factory.build(mgSolver);
+
+   /* std::vector<BitVector> bodyDirichletNodes;
+    nBodyAssembler.postprocess(_dirichletNodes, bodyDirichletNodes);
+    for (size_t i=0; i<bodyDirichletNodes.size(); i++) {
+      print(bodyDirichletNodes[i], "bodyDirichletNodes_" + std::to_string(i) + ": ");
+    }*/
+
+   /* print(bilinearForm, "matrix: ");
+    print(totalX, "totalX: ");
+    print(totalRhs, "totalRhs: ");*/
+
+    auto tnnmgStep = factory.step();
+    factory.setProblem(x);
+
+    LoopSolver<Vector> solver(
+        tnnmgStep.get(), parset.get<size_t>("u0.solver.maximumIterations"),
+                parset.get<double>("u0.solver.tolerance"), &norm,
+        Solver::FULL); //, true, &refX); // absolute error
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", J(x));
+            },
+            "   energy      ");
+
+    initialEnergy = J(x);
+    solver.addCriterion(
+            [&](){
+            static double oldEnergy=initialEnergy;
+            double currentEnergy = J(x);
+            double decrease = currentEnergy - oldEnergy;
+            oldEnergy = currentEnergy;
+            return Dune::formatString("   % 12.5e", decrease);
+            },
+            "   decrease    ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12.5e", tnnmgStep->lastDampingFactor());
+            },
+            "   damping     ");
+
+    solver.addCriterion(
+            [&](){
+            return Dune::formatString("   % 12d", tnnmgStep->linearization().truncated().count());
+            },
+            "   truncated   ");
+
+
+    /*std::vector<double> factors;
+    solver.addCriterion(reductionFactorCriterion(*tnnmgStep, norm, factors));
+
+    solver.addCriterion(energyCriterion(*tnnmgStep, J, factors));*/
+
+    solver.preprocess();
+    solver.solve();
+
+    auto diff = x;
+    diff -= refX;
+    std::cout << "Solver error in energy norm: " << norm(diff) << std::endl;
+
+    std::cout << "Energy end iterate: " << J(x) << std::endl;
+}
+
+
+template <class ContactNetwork>
+void setupInitialConditions(const ContactNetwork& contactNetwork) {
+  using Matrix = typename ContactNetwork::Matrix;
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+
+  std::cout << std::endl << "-- setupInitialConditions --" << std::endl;
+  std::cout << "----------------------------" << std::endl;
+
+  // Solving a linear problem with a multigrid solver
+  auto const solveLinearProblem = [&](
+      const BitVector& _dirichletNodes, const std::vector<std::shared_ptr<Matrix>>& _matrices,
+      const std::vector<Vector>& _rhs, std::vector<Vector>& _x) {
+
+    std::vector<const Matrix*> matrices_ptr(_matrices.size());
+    for (size_t i=0; i<matrices_ptr.size(); i++) {
+          matrices_ptr[i] = _matrices[i].get();
+    }
+
+    // assemble full global contact problem
+    Matrix bilinearForm;
+
+    nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+    Vector totalRhs, oldTotalRhs;
+    nBodyAssembler.assembleRightHandSide(_rhs, totalRhs);
+    oldTotalRhs = totalRhs;
+
+    Vector totalX, oldTotalX;
+    nBodyAssembler.nodalToTransformed(_x, totalX);
+    oldTotalX = totalX;
+
+    // get lower and upper obstacles
+    const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+    Vector lower(totalObstacles.size());
+    Vector upper(totalObstacles.size());
+
+    for (size_t j=0; j<totalObstacles.size(); ++j) {
+        const auto& totalObstaclesj = totalObstacles[j];
+        auto& lowerj = lower[j];
+        auto& upperj = upper[j];
+      for (size_t d=0; d<dim; ++d) {
+          lowerj[d] = totalObstaclesj[d][0];
+          upperj[d] = totalObstaclesj[d][1];
+      }
+    }
+
+    // print problem
+   /* print(bilinearForm, "bilinearForm");
+    print(totalRhs, "totalRhs");
+    print(_dirichletNodes, "ignore");
+    print(totalObstacles, "totalObstacles");
+    print(lower, "lower");
+    print(upper, "upper");*/
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalX, _dirichletNodes, lower, upper, true);
+
+    nBodyAssembler.postprocess(totalX, _x);
+  };
+
+  timeStep = parset.get<size_t>("initialTime.timeStep");
+  relativeTime = parset.get<double>("initialTime.relativeTime");
+  relativeTau = parset.get<double>("initialTime.relativeTau");
+
+  bodyStates.resize(bodyCount);
+  u.resize(bodyCount);
+  v.resize(bodyCount);
+  a.resize(bodyCount);
+  alpha.resize(bodyCount);
+  weightedNormalStress.resize(bodyCount);
+
+  for (size_t i=0; i<bodyCount; i++) {
+    size_t leafVertexCount =  contactNetwork.body(i)->nVertices();
+
+    u[i].resize(leafVertexCount),
+    v[i].resize(leafVertexCount),
+    a[i].resize(leafVertexCount),
+    alpha[i].resize(leafVertexCount),
+    weightedNormalStress[i].resize(leafVertexCount),
+
+    bodyStates[i] = new BodyState<Vector, ScalarVector>(&u[i], &v[i], &a[i], &alpha[i], &weightedNormalStress[i]);
+  }
+
+  std::vector<Vector> ell0(bodyCount);
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial velocity
+    u[i] = 0.0;
+    v[i] = 0.0;
+
+    ell0[i].resize(u[i].size());
+    ell0[i] = 0.0;
+
+    contactNetwork.body(i)->externalForce()(relativeTime, ell0[i]);
+  }
+
+  // Initial displacement: Start from a situation of minimal stress,
+  // which is automatically attained in the case [v = 0 = a].
+  // Assuming dPhi(v = 0) = 0, we thus only have to solve Au = ell0
+  BitVector dirichletNodes;
+  contactNetwork.totalNodes("dirichlet", dirichletNodes);
+  /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+      bool val = false;
+      for (size_t d=0; d<dims; d++) {
+          val = val || dirichletNodes[i][d];
+      }
+
+      dirichletNodes[i] = val;
+      for (size_t d=0; d<dims; d++) {
+          dirichletNodes[i][d] = val;
+      }
+  }*/
+
+  std::cout << "solving linear problem for u..." << std::endl;
+
+  solveLinearProblem(dirichletNodes, contactNetwork.matrices().elasticity, ell0, u);
+
+  //print(u, "initial u:");
+
+  // Initial acceleration: Computed in agreement with Ma = ell0 - Au
+  // (without Dirichlet constraints), again assuming dPhi(v = 0) = 0
+  std::vector<Vector> accelerationRHS = ell0;
+  for (size_t i=0; i<bodyCount; i++) {
+    // Initial state
+    alpha[i] = parset.get<double>("boundary.friction.initialAlpha");
+
+    // Initial normal stress
+
+    const auto& body = contactNetwork.body(i);
+    /*std::vector<std::shared_ptr<typename ContactNetwork::LeafBody::BoundaryCondition>> frictionBoundaryConditions;
+    body->boundaryConditions("friction", frictionBoundaryConditions);
+    for (size_t j=0; j<frictionBoundaryConditions.size(); j++) {
+        ScalarVector frictionBoundaryStress(weightedNormalStress[i].size());
+
+        body->assembler()->assembleWeightedNormalStress(
+          *frictionBoundaryConditions[j]->boundaryPatch(), frictionBoundaryStress, body->data()->getYoungModulus(),
+          body->data()->getPoissonRatio(), u[i]);
+
+        weightedNormalStress[i] += frictionBoundaryStress;
+    }*/
+
+    Dune::MatrixVector::subtractProduct(accelerationRHS[i], *body->matrices().elasticity, u[i]);
+  }
+
+  for (size_t i=0; i<contactNetwork.nCouplings(); i++) {
+    const auto& coupling = contactNetwork.coupling(i);
+    const auto& contactCoupling = contactNetwork.nBodyAssembler().getContactCouplings()[i];
+
+    const auto nonmortarIdx = coupling->gridIdx_[0];
+    const auto& body = contactNetwork.body(nonmortarIdx);
+
+    ScalarVector frictionBoundaryStress(weightedNormalStress[nonmortarIdx].size());
+
+    body->assembler()->assembleWeightedNormalStress(
+      contactCoupling->nonmortarBoundary(), frictionBoundaryStress, body->data()->getYoungModulus(),
+      body->data()->getPoissonRatio(), u[nonmortarIdx]);
+
+    weightedNormalStress[nonmortarIdx] += frictionBoundaryStress;
+  }
+
+  //print(weightedNormalStress, "weightedNormalStress: ");
+
+  std::cout << "solving linear problem for a..." << std::endl;
+
+  BitVector noNodes(dirichletNodes.size(), false);
+  solveLinearProblem(noNodes, contactNetwork.matrices().mass, accelerationRHS, a);
+
+  //print(a, "initial a:");
+}
+
+template <class ContactNetwork>
+void relativeVelocities(const ContactNetwork& contactNetwork, const Vector& v, std::vector<Vector>& v_rel) {
+
+    const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    const size_t nBodies = nBodyAssembler.nGrids();
+   // const auto& contactCouplings = nBodyAssembler.getContactCouplings();
+
+    std::vector<const Dune::BitSetVector<1>*> bodywiseNonmortarBoundaries;
+    contactNetwork.frictionNodes(bodywiseNonmortarBoundaries);
+
+    size_t globalIdx = 0;
+    v_rel.resize(nBodies);
+    for (size_t bodyIdx=0; bodyIdx<nBodies; bodyIdx++) {
+        const auto& nonmortarBoundary = *bodywiseNonmortarBoundaries[bodyIdx];
+        auto& v_rel_ = v_rel[bodyIdx];
+
+        v_rel_.resize(nonmortarBoundary.size());
+
+        for (size_t i=0; i<v_rel_.size(); i++) {
+            if (toBool(nonmortarBoundary[i])) {
+                v_rel_[i] = v[globalIdx];
+            }
+            globalIdx++;
+        }
+    }
+
+}
+
+template <class Updaters, class ContactNetwork>
+void run(Updaters& updaters, ContactNetwork& contactNetwork,
+    const std::vector<Matrix>& velocityMatrices, const std::vector<Vector>& velocityRHSs,
+    std::vector<Vector>& velocityIterates) {
+
+  const auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+  const auto nBodies = nBodyAssembler.nGrids();
+
+  std::vector<const Matrix*> matrices_ptr(nBodies);
+  for (int i=0; i<nBodies; i++) {
+      matrices_ptr[i] = &velocityMatrices[i];
+  }
+
+  // assemble full global contact problem
+  Matrix bilinearForm;
+  nBodyAssembler.assembleJacobian(matrices_ptr, bilinearForm);
+
+  Vector totalRhs;
+  nBodyAssembler.assembleRightHandSide(velocityRHSs, totalRhs);
+
+  // get lower and upper obstacles
+  const auto& totalObstacles = nBodyAssembler.totalObstacles_;
+  Vector lower(totalObstacles.size());
+  Vector upper(totalObstacles.size());
+
+  for (size_t j=0; j<totalObstacles.size(); ++j) {
+      const auto& totalObstaclesj = totalObstacles[j];
+      auto& lowerj = lower[j];
+      auto& upperj = upper[j];
+    for (size_t d=0; d<dim; ++d) {
+        lowerj[d] = totalObstaclesj[d][0];
+        upperj[d] = totalObstaclesj[d][1];
+    }
+  }
+
+  // compute velocity obstacles
+  /*Vector vLower, vUpper;
+  std::vector<Vector> u0, v0;
+  updaters.rate_->extractOldVelocity(v0);
+  updaters.rate_->extractOldDisplacement(u0);
+
+  Vector totalu0, totalv0;
+  nBodyAssembler.nodalToTransformed(u0, totalu0);
+  nBodyAssembler.nodalToTransformed(v0, totalv0);
+
+  updaters.rate_->velocityObstacles(totalu0, lower, totalv0, vLower);
+  updaters.rate_->velocityObstacles(totalu0, upper, totalv0, vUpper);*/
+
+  const auto& errorNorms = contactNetwork.stateEnergyNorms();
+
+  auto& globalFriction = contactNetwork.globalFriction();
+
+  BitVector totalDirichletNodes;
+  contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+  size_t fixedPointMaxIterations = parset.get<size_t>("v.fpi.maximumIterations");
+  double fixedPointTolerance = parset.get<double>("v.fpi.tolerance");
+  double lambda = parset.get<double>("v.fpi.lambda");
+
+  size_t fixedPointIteration;
+  size_t multigridIterations = 0;
+
+  std::vector<ScalarVector> alpha(nBodies);
+  updaters.state_->extractAlpha(alpha);
+
+  Vector totalVelocityIterate;
+  nBodyAssembler.nodalToTransformed(velocityIterates, totalVelocityIterate);
+
+  // project in onto admissible set
+  const size_t blocksize = Vector::block_type::dimension;
+  for (size_t i=0; i<totalVelocityIterate.size(); i++) {
+      for (size_t j=0; j<blocksize; j++) {
+          if (totalVelocityIterate[i][j] < lower[i][j]) {
+              totalVelocityIterate[i][j] = lower[i][j];
+          }
+
+          if (totalVelocityIterate[i][j] > upper[i][j]) {
+              totalVelocityIterate[i][j] = upper[i][j];
+          }
+      }
+  }
+
+  for (fixedPointIteration = 0; fixedPointIteration < fixedPointMaxIterations;
+       ++fixedPointIteration) {
+
+    // contribution from nonlinearity
+    //print(alpha, "alpha: ");
+
+    globalFriction.updateAlpha(alpha);
+
+    solveProblem(contactNetwork, bilinearForm, totalRhs, totalVelocityIterate, totalDirichletNodes, lower, upper, false);
+
+    nBodyAssembler.postprocess(totalVelocityIterate, velocityIterates);
+
+    std::vector<Vector> v_m; //TODO : wrong, isnt used atm;
+    updaters.rate_->extractOldVelocity(v_m);
+
+    for (size_t i=0; i<v_m.size(); i++) {
+      v_m[i] *= 1.0 - lambda;
+      Dune::MatrixVector::addProduct(v_m[i], lambda, velocityIterates[i]);
+    }
+
+    // extract relative velocities in mortar basis
+    std::vector<Vector> v_rel;
+    relativeVelocities(contactNetwork, totalVelocityIterate, v_rel);
+
+    //print(v_rel, "v_rel");
+
+    std::cout << "- State problem set" << std::endl;
+
+    // solve a state problem
+    updaters.state_->solve(v_rel);
+
+    std::cout << "-- Solved" << std::endl;
+
+    std::vector<ScalarVector> newAlpha(nBodies);
+    updaters.state_->extractAlpha(newAlpha);
+
+    bool breakCriterion = true;
+    for (int i=0; i<nBodies; i++) {
+        if (alpha[i].size()==0 || newAlpha[i].size()==0)
+            continue;
+
+        //print(alpha[i], "alpha i:");
+        //print(newAlpha[i], "new alpha i:");
+        if (errorNorms[i]->diff(alpha[i], newAlpha[i]) >= fixedPointTolerance) {
+            breakCriterion = false;
+            std::cout << "fixedPoint error: " << errorNorms[i]->diff(alpha[i], newAlpha[i]) << std::endl;
+            break;
+        }
+    }
+
+    if (lambda < 1e-12 or breakCriterion) {
+      std::cout << "-FixedPointIteration finished! " << (lambda < 1e-12 ? "lambda" : "breakCriterion") << std::endl;
+      fixedPointIteration++;
+      break;
+    }
+    alpha = newAlpha;
+  }
+
+  std::cout << "-FixedPointIteration finished! " << std::endl;
+
+  if (fixedPointIteration == fixedPointMaxIterations)
+    DUNE_THROW(Dune::Exception, "FPI failed to converge");
+
+  updaters.rate_->postProcess(velocityIterates);
+}
+
+
+template <class Updaters, class ContactNetwork>
+void step(Updaters& updaters, ContactNetwork& contactNetwork)  {
+  updaters.state_->nextTimeStep();
+  updaters.rate_->nextTimeStep();
+
+  auto const newRelativeTime = relativeTime + relativeTau;
+  typename ContactNetwork::ExternalForces externalForces;
+  contactNetwork.externalForces(externalForces);
+  std::vector<Vector> ell(externalForces.size());
+  for (size_t i=0; i<externalForces.size(); i++) {
+    (*externalForces[i])(newRelativeTime, ell[i]);
+  }
+
+  std::vector<Matrix> velocityMatrix;
+  std::vector<Vector> velocityRHS;
+  std::vector<Vector> velocityIterate;
+
+  double finalTime = parset.get<double>("problem.finalTime");
+  auto const tau = relativeTau * finalTime;
+  updaters.state_->setup(tau);
+  updaters.rate_->setup(ell, tau, newRelativeTime, velocityRHS, velocityIterate, velocityMatrix);
+
+  run(updaters, contactNetwork, velocityMatrix, velocityRHS, velocityIterate);
+}
+
+template <class Updaters, class ContactNetwork>
+void advanceStep(Updaters& current, ContactNetwork& contactNetwork) {
+    step(current, contactNetwork);
+    relativeTime += relativeTau;
+}
+
+void getParameters(int argc, char *argv[]) {
+  Dune::ParameterTreeParser::readINITree("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem.cfg", parset);
+  Dune::ParameterTreeParser::readINITree(
+      Dune::Fufem::formatString("/home/mi/podlesny/software/dune/dune-tectonic/src/multi-body-problem-%dD.cfg", dim), parset);
+  Dune::ParameterTreeParser::readOptions(argc, argv, parset);
+}
+
+int main(int argc, char *argv[]) { try {
+    Dune::MPIHelper::instance(argc, argv);
+
+    std::ofstream out(path + outputFile);
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to outputFile
+
+    std::cout << "-------------------------" << std::endl;
+    std::cout << "-- SolverFactory Test: --" << std::endl;
+    std::cout << "-------------------------" << std::endl << std::endl;
+
+    getParameters(argc, argv);
+
+    // ----------------------
+    // set up contact network
+    // ----------------------
+    StackedBlocksFactory<Grid, Vector> stackedBlocksFactory(parset);
+    using ContactNetwork = typename StackedBlocksFactory<Grid, Vector>::ContactNetwork;
+    stackedBlocksFactory.build();
+
+    ContactNetwork& contactNetwork = stackedBlocksFactory.contactNetwork();
+    bodyCount = contactNetwork.nBodies();
+
+    for (size_t i=0; i<contactNetwork.nLevels(); i++) {
+        const auto& level = *contactNetwork.level(i);
+
+        for (size_t j=0; j<level.nBodies(); j++) {
+            writeToVTK(level.body(j)->gridView(), "debug_print/bodies/", "body_" + std::to_string(j) + "_level_" + std::to_string(i));
+        }
+    }
+
+    for (size_t i=0; i<bodyCount; i++) {
+        writeToVTK(contactNetwork.body(i)->gridView(), "debug_print/bodies/", "body_" + std::to_string(i) + "_leaf");
+    }
+
+    // ----------------------------
+    // assemble contactNetwork
+    // ----------------------------
+    contactNetwork.assemble();
+
+    //printMortarBasis<Vector>(contactNetwork.nBodyAssembler());
+
+    // -----------------
+    // init input/output
+    // -----------------
+    timeSteps = parset.get<size_t>("timeSteps.timeSteps");
+    allReductionFactors.resize(timeSteps+1);
+
+    setupInitialConditions(contactNetwork);
+
+    auto& nBodyAssembler = contactNetwork.nBodyAssembler();
+    for (size_t i=0; i<bodyCount; i++) {
+      contactNetwork.body(i)->setDeformation(u[i]);
+    }
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();
+
+    // ------------------------
+    // assemble global friction
+    // ------------------------
+    contactNetwork.assembleFriction(parset.get<Config::FrictionModel>("boundary.friction.frictionModel"), weightedNormalStress);
+
+    auto& globalFriction = contactNetwork.globalFriction();
+    globalFriction.updateAlpha(alpha);
+
+    using Assembler = MyAssembler<DefLeafGridView, dim>;
+    using field_type = Matrix::field_type;
+    using MyVertexBasis = typename Assembler::VertexBasis;
+    using MyCellBasis = typename Assembler::CellBasis;
+    std::vector<Vector> vertexCoordinates(bodyCount);
+    std::vector<const MyVertexBasis* > vertexBases(bodyCount);
+    std::vector<const MyCellBasis* > cellBases(bodyCount);
+
+    for (size_t i=0; i<bodyCount; i++) {
+      const auto& body = contactNetwork.body(i);
+      vertexBases[i] = &(body->assembler()->vertexBasis);
+      cellBases[i] = &(body->assembler()->cellBasis);
+
+      auto& vertexCoords = vertexCoordinates[i];
+      vertexCoords.resize(body->nVertices());
+
+      Dune::MultipleCodimMultipleGeomTypeMapper<
+          DefLeafGridView, Dune::MCMGVertexLayout> const vertexMapper(body->gridView(), Dune::mcmgVertexLayout());
+      for (auto &&v : vertices(body->gridView()))
+        vertexCoords[vertexMapper.index(v)] = geoToPoint(v.geometry());
+    }
+
+    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "/storage/mi/podlesny/software/dune/dune-tectonic/body");
+
+    auto const report = [&](bool initial = false) {
+      if (parset.get<bool>("io.printProgress"))
+        std::cout << "timeStep = " << std::setw(6) << timeStep
+                  << ", time = " << std::setw(12) << relativeTime
+                  << ", tau = " << std::setw(12) << relativeTau
+                  << std::endl;
+
+      if (parset.get<bool>("io.vtk.write")) {
+        std::vector<ScalarVector> stress(bodyCount);
+
+        for (size_t i=0; i<bodyCount; i++) {
+          const auto& body = contactNetwork.body(i);
+          body->assembler()->assembleVonMisesStress(body->data()->getYoungModulus(),
+                                           body->data()->getPoissonRatio(),
+                                           u[i], stress[i]);
+
+        }
+
+        vtkWriter.write(timeStep, u, v, alpha, stress);
+      }
+    };
+    report(true);
+
+    // -------------------
+    // Set up TNNMG solver
+    // -------------------
+
+    /*BitVector totalDirichletNodes;
+    contactNetwork.totalNodes("dirichlet", totalDirichletNodes);
+
+    print(totalDirichletNodes, "totalDirichletNodes:");*/
+
+    //using Functional = Functional<Matrix&, Vector&, ZeroNonlinearity&, Vector&, Vector&, field_type>;
+    //using Functional = Functional<Matrix&, Vector&, GlobalFriction<Matrix, Vector>&, Vector&, Vector&, field_type>;
+    //using NonlinearFactory = SolverFactory<Functional, BitVector>;
+
+    using BoundaryFunctions = typename ContactNetwork::BoundaryFunctions;
+    using BoundaryNodes = typename ContactNetwork::BoundaryNodes;
+    using Updaters = Updaters<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>,
+                               StateUpdater<ScalarVector, Vector>>;
+
+    BoundaryFunctions velocityDirichletFunctions;
+    contactNetwork.boundaryFunctions("dirichlet", velocityDirichletFunctions);
+
+    BoundaryNodes dirichletNodes;
+    contactNetwork.boundaryNodes("dirichlet", dirichletNodes);
+
+    /*for (size_t i=0; i<dirichletNodes.size(); i++) {
+        for (size_t j=0; j<dirichletNodes[i].size(); j++) {
+        print(*dirichletNodes[i][j], "dirichletNodes_body_" + std::to_string(i) + "_boundary_" + std::to_string(j));
+        }
+    }*/
+
+
+    /*for (size_t i=0; i<frictionNodes.size(); i++) {
+        print(*frictionNodes[i], "frictionNodes_body_" + std::to_string(i));
+    }*/
+
+    Updaters current(
+        initRateUpdater(
+            parset.get<Config::scheme>("timeSteps.scheme"),
+            velocityDirichletFunctions,
+            dirichletNodes,
+            contactNetwork.matrices(),
+            u,
+            v,
+            a),
+        initStateUpdater<ScalarVector, Vector>(
+            parset.get<Config::stateModel>("boundary.friction.stateModel"),
+            alpha,
+            nBodyAssembler.getContactCouplings(),
+            contactNetwork.couplings())
+            );
+
+
+
+    //const auto& stateEnergyNorms = contactNetwork.stateEnergyNorms();
+
+    for (timeStep=1; timeStep<=timeSteps; timeStep++) {
+
+      advanceStep(current, contactNetwork);
+
+      relativeTime += relativeTau;
+
+      current.rate_->extractDisplacement(u);
+      current.rate_->extractVelocity(v);
+      current.rate_->extractAcceleration(a);
+      current.state_->extractAlpha(alpha);
+
+      contactNetwork.setDeformation(u);
+
+      report();
+    }
+
+    // output reduction factors
+    size_t count = 0;
+    for (size_t i=0; i<allReductionFactors.size(); i++) {
+        count = std::max(count, allReductionFactors[i].size());
+    }
+    std::vector<double> avgReductionFactors(count);
+    for (size_t i=0; i<count; i++) {
+        avgReductionFactors[i] = 1;
+        size_t c = 0;
+
+        for (size_t j=0; j<allReductionFactors.size(); j++) {
+            if (!(i<allReductionFactors[j].size()))
+                continue;
+
+            avgReductionFactors[i] *= allReductionFactors[j][i];
+            c++;
+        }
+
+        avgReductionFactors[i] = std::pow(avgReductionFactors[i], 1.0/((double)c));
+    }
+
+    print(avgReductionFactors, "average reduction factors: ");
+
+    bool passed = true;
+
+    std::cout << "Overall, the test " << (passed ? "was successful!" : "failed!") << std::endl;
+
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+    return passed ? 0 : 1;
+
+} catch (Dune::Exception &e) {
+    Dune::derr << "Dune reported error: " << e << std::endl;
+} catch (std::exception &e) {
+    std::cerr << "Standard exception: " << e.what() << std::endl;
+} // end try
+} // end main

dune/tectonic/time-stepping/CMakeLists.txt

+add_subdirectory("rate")
+add_subdirectory("state")
+
+add_custom_target(tectonic_dune_time-stepping SOURCES
+  adaptivetimestepper.hh
+  adaptivetimestepper.cc
+  coupledtimestepper.hh
+  coupledtimestepper.cc 
+  rate.hh
+  rate.cc
+  state.hh
+  state.cc
+  step.hh
+  stepbase.hh
+  uniformtimestepper.hh
+  uniformtimestepper.cc
+  updaters.hh
+)
+
+#install headers
+install(FILES
+  adaptivetimestepper.hh
+  coupledtimestepper.hh
+  rate.hh
+  state.hh
+  step.hh
+  stepbase.hh
+  uniformtimestepper.hh
+  updaters.hh
+DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/dune/tectonic)

dune/tectonic/time-stepping/adaptivetimestepper.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <future>
+#include <thread>
+#include <chrono>
+
+#include <dune/solvers/norms/energynorm.hh>
+#include <dune/solvers/iterationsteps/multigridstep.hh>
+#include <dune/solvers/iterationsteps/cgstep.hh>
+#include <dune/solvers/solvers/loopsolver.hh>
+
+#include "../spatial-solving/preconditioners/multilevelpatchpreconditioner.hh"
+
+#include <dune/tectonic/utils/reductionfactors.hh>
+
+#include "adaptivetimestepper.hh"
+#include "step.hh"
+
+const unsigned int N_THREADS = std::thread::hardware_concurrency();
+
+void IterationRegister::registerCount(FixedPointIterationCounter count) {
+  totalCount += count;
+}
+
+void IterationRegister::registerFinalCount(FixedPointIterationCounter count) {
+  finalCount = count;
+}
+
+void IterationRegister::reset() {
+  totalCount = FixedPointIterationCounter();
+  finalCount = FixedPointIterationCounter();
+}
+
+/*
+ * Implementation: AdaptiveTimeStepper
+ */
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::AdaptiveTimeStepper(
+        const StepBase& stepBase,
+        ContactNetwork& contactNetwork,
+        Updaters &current,
+        double relativeTime,
+        double relativeTau,
+        double minTau,
+        std::function<bool(Updaters &, Updaters &)> mustRefine)
+    : relativeTime_(relativeTime),
+      relativeTau_(relativeTau),
+      minTau_(minTau),
+      stepBase_(stepBase),
+      contactNetwork_(contactNetwork),
+      current_(current),
+      R1_(),
+      mustRefine_(mustRefine) {
+
+    std::cout << N_THREADS << " concurrent threads are supported." << std::endl;
+}
+
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+bool AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::reachedEnd() {
+  return relativeTime_ >= 1.0;
+}
+
+// compute C and R2 in parallel
+// returns number of coarsenings done
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+int AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::coarsen() {
+    using Step = Step<Factory, ContactNetwork, Updaters, ErrorNorms>;
+
+    int coarseCount = 1; // one coarsening step was already done in determineStrategy()
+
+    UpdatersWithCount C;
+    UpdatersWithCount R2;
+
+    const auto& currentNBodyAssembler = contactNetwork_.nBodyAssembler();
+
+    while (relativeTime_ + relativeTau_ <= 1.0 && relativeTau_ < 0.1) {
+      //std::cout << "tau: " << relativeTau_ << std::endl;
+
+      setDeformation(current_);
+      auto C_Step = Step(stepBase_, current_, currentNBodyAssembler, relativeTime_, 2 * relativeTau_, iterationRegister_);
+      C_Step.run(Step::Mode::sameThread); //newThread
+
+      //updateReductionFactors(reductionFactors);
+      //std::cout << "AdaptiveTimeStepper C computed!" << std::endl << std::endl;
+
+      /*using ScalarVector = typename Updaters::StateUpdater::ScalarVector;
+      std::vector<ScalarVector> cAlpha(contactNetwork_.nBodies());
+      C.updaters.state_->extractAlpha(cAlpha);
+      print(cAlpha, "cAlpha: ");*/
+
+      setDeformation(R1_.updaters);
+      //auto R2_linearSolver = makeLinearSolver();
+      auto&& nBodyAssembler = step(currentNBodyAssembler);
+      auto R2_Step = Step(stepBase_, R1_.updaters, nBodyAssembler, relativeTime_ + relativeTau_, relativeTau_, iterationRegister_);
+      R2_Step.run(Step::Mode::sameThread); //newThread
+
+      //updateReductionFactors(reductionFactors);
+      //std::cout << "AdaptiveTimeStepper R2 computed!" << std::endl << std::endl;
+
+      C = C_Step.get();
+      R2 = R2_Step.get();
+
+      /*std::vector<ScalarVector> rAlpha(contactNetwork_.nBodies());
+      R2.updaters.state_->extractAlpha(rAlpha);
+      print(rAlpha, "rAlpha: ");*/
+
+      if (mustRefine_(C.updaters, R2.updaters))
+        break;
+
+      relativeTau_ *= 2;
+      R1_ = C;
+
+      coarseCount++;
+    }
+
+    current_ = R1_.updaters;
+    R1_ = R2;
+
+    return coarseCount;
+}
+
+// compute F1 and F2 sequentially
+// returns number of refinements done
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+int AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::refine(UpdatersWithCount& R2) {
+    using Step = Step<Factory, ContactNetwork, Updaters, ErrorNorms>;
+
+    int refineCount = 1; // one refinement step was already done in determineStrategy()
+
+    UpdatersWithCount F1;
+    UpdatersWithCount F2;
+
+    const auto& currentNBodyAssembler = contactNetwork_.nBodyAssembler();
+
+    while (true) {
+        if (relativeTau_ / 2.0 <= minTau_) {
+          std::cout << "Sufficiently refined!" << std::endl;
+          break;
+        }
+
+        setDeformation(current_);
+        //auto F1_linearSolver = makeLinearSolver();
+        auto F1_Step = Step(stepBase_, current_, currentNBodyAssembler, relativeTime_, relativeTau_ / 2.0, iterationRegister_);
+        F1_Step.run(Step::Mode::sameThread);
+        F1 = F1_Step.get();
+
+        //updateReductionFactors(reductionFactors);
+        std::cout << "AdaptiveTimeStepper F1 computed!" << std::endl << std::endl;
+
+        setDeformation(F1.updaters);
+        //auto F2_linearSolver = makeLinearSolver();
+        auto&& nBodyAssembler = step(currentNBodyAssembler);
+        auto F2_Step = Step(stepBase_, F1.updaters, nBodyAssembler, relativeTime_ + relativeTau_ / 2.0,
+                  relativeTau_ / 2.0, iterationRegister_);
+        F2_Step.run(Step::Mode::sameThread);
+        F2 = F2_Step.get();
+        //updateReductionFactors(reductionFactors);
+
+        if (!mustRefine_(R1_.updaters, F2.updaters)) {
+          std::cout << "Sufficiently refined!" << std::endl;
+          break;
+        }
+
+        relativeTau_ /= 2.0;
+        R1_ = F1;
+        R2 = F2;
+
+        refineCount++;
+    }
+
+    current_ = R1_.updaters;
+    R1_ = R2;
+
+    return refineCount;
+}
+
+
+/*
+ * determines how to adapt time step, returns
+ * -1: coarsen
+ *  0: keep
+ *  1: refine
+ */
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+int AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::determineStrategy(UpdatersWithCount& R2) {
+    using Step = Step<Factory, ContactNetwork, Updaters, ErrorNorms>;
+
+    int strategy = 0;
+
+    const auto& currentNBodyAssembler = contactNetwork_.nBodyAssembler();
+
+    UpdatersWithCount C;
+    UpdatersWithCount F1;
+    UpdatersWithCount F2;
+
+    //if (relativeTime_ + relativeTau_ > 1.0) {
+    //  return false;
+    //}
+
+    setDeformation(current_);
+    //auto C_linearSolver = makeLinearSolver();
+    auto C_Step = Step(stepBase_, current_, currentNBodyAssembler, relativeTime_, 2 * relativeTau_, iterationRegister_);
+    C_Step.run(Step::Mode::sameThread); // newThread
+    //updateReductionFactors(reductionFactors);
+    std::cout << "AdaptiveTimeStepper C computed!" << std::endl << std::endl;
+
+    setDeformation(R1_.updaters);
+    //auto R2_linearSolver = makeLinearSolver();
+    auto&& nBodyAssembler = step(currentNBodyAssembler);
+    auto R2_Step = Step(stepBase_, R1_.updaters, nBodyAssembler, relativeTime_ + relativeTau_, relativeTau_, iterationRegister_);
+    R2_Step.run(Step::Mode::sameThread); //newThread
+
+    //updateReductionFactors(reductionFactors);
+    std::cout << "AdaptiveTimeStepper R2 computed!" << std::endl << std::endl;
+
+    if (N_THREADS < 3) {
+      C = C_Step.get();
+      R2 = R2_Step.get();
+
+      if (!mustRefine_(C.updaters, R2.updaters)) {
+          strategy = -1;
+      }
+    }
+
+    if (strategy>-1) {
+        setDeformation(current_);
+        //auto F1_linearSolver = makeLinearSolver();
+        auto F1_Step = Step(stepBase_, current_, currentNBodyAssembler, relativeTime_, relativeTau_ / 2.0, iterationRegister_);
+        F1_Step.run(Step::Mode::sameThread); //newThread
+        //updateReductionFactors(reductionFactors);
+        std::cout << "AdaptiveTimeStepper F1 computed!" << std::endl << std::endl;
+
+        if (N_THREADS > 2) {
+          C = C_Step.get();
+          R2 = R2_Step.get();
+
+          if (!mustRefine_(C.updaters, R2.updaters)) {
+              strategy = -1;
+          }
+        }
+
+        F1 = F1_Step.get();
+
+        if (strategy>-1 && relativeTau_ / 2.0 > minTau_) {
+            setDeformation(F1.updaters);
+            //auto F2_linearSolver = makeLinearSolver();
+            auto&& nBodyAssembler = step(currentNBodyAssembler);
+            auto F2_Step = Step(stepBase_, F1.updaters, nBodyAssembler, relativeTime_ + relativeTau_ / 2.0,
+                                relativeTau_ / 2.0, iterationRegister_);
+            F2_Step.run(Step::Mode::sameThread);
+            F2 = F2_Step.get();
+            //updateReductionFactors(reductionFactors);
+
+            if (mustRefine_(R1_.updaters, F2.updaters)) {
+                strategy = 1;
+            }
+        }
+    }
+
+    switch (strategy) {
+        case -1:
+            relativeTau_ *= 2;
+            R1_ = C;
+            break;
+        case 0:
+            current_ = R1_.updaters;
+            R1_ = R2;
+            break;
+        case 1:
+            relativeTau_ /= 2.0;
+            R1_ = F1;
+            R2 = F2;
+            break;
+    }
+
+    return strategy;
+}
+
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+IterationRegister AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::advance() {
+  /*
+    |     C     | We check here if making the step R1 of size tau is a
+    |  R1 | R2  | good idea. To check if we can coarsen, we compare
+    |F1|F2|     | the result of (R1+R2) with C, i.e. two steps of size
+                  tau with one of size 2*tau. To check if we need to
+    refine, we compare the result of (F1+F2) with R1, i.e. two steps
+    of size tau/2 with one of size tau. The method makes multiple
+    coarsening/refining attempts, with coarsening coming first. */
+
+  std::cout << "AdaptiveTimeStepper::advance()" << std::endl;
+
+  using Step = Step<Factory, ContactNetwork, Updaters, ErrorNorms>;
+
+  if (R1_.updaters == Updaters()) {
+      //setDeformation(current_);
+      //auto R1_linearSolver = makeLinearSolver();
+      auto R1_Step = Step(stepBase_, current_, contactNetwork_.nBodyAssembler(), relativeTime_, relativeTau_, iterationRegister_);
+      R1_Step.run(Step::Mode::sameThread);
+      R1_ = R1_Step.get();
+   }
+
+  iterationRegister_.reset();
+  UpdatersWithCount R2;
+  int strat = determineStrategy(R2);
+
+  // coarsen
+  if (strat<0) {
+      int coarseningCount = coarsen();
+      std::cout << " done with coarseningCount: " << coarseningCount << std::endl;
+  }
+
+  // refine
+  if (strat>0) {
+      int refineCount = refine(R2);
+      std::cout << " done with refineCount: " << refineCount << std::endl;
+  }
+
+  iterationRegister_.registerFinalCount(R1_.count);
+  relativeTime_ += relativeTau_;
+
+  return iterationRegister_;
+}
+
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+void AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::setDeformation(const Updaters& updaters) {
+    std::vector<Vector> u;
+    updaters.rate_->extractDisplacement(u);
+
+    for (size_t i=0; i<contactNetwork_.nBodies(); i++) {
+        contactNetwork_.body(i)->setDeformation(u[i]);
+    }
+
+    // note: levelContactNetworks are not up-to-date; build() has to be called in order to update;
+    // unnecessary for standard multigrid as linear solver, might have to be called for patch preconditioner
+}
+
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
+typename AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::NBodyAssembler
+AdaptiveTimeStepper<Factory, ContactNetwork, Updaters, ErrorNorms>::step(const NBodyAssembler& oldNBodyAssembler) const {
+    NBodyAssembler nBodyAssembler = oldNBodyAssembler;
+
+    nBodyAssembler.assembleTransferOperator();
+    nBodyAssembler.assembleObstacle();
+
+    return nBodyAssembler;
+}
+
+#include "adaptivetimestepper_tmpl.cc"

src/time-stepping/adaptivetimestepper.hh → dune/tectonic/time-stepping/adaptivetimestepper.hh

@@ -2,8 +2,14 @@
 #define SRC_TIME_STEPPING_ADAPTIVETIMESTEPPER_HH
 
 #include <fstream>
+#include <future>
+#include <thread>
+
+//#include "../multi-threading/task.hh"
+#include "../spatial-solving/contact/nbodycontacttransfer.hh"
 
 #include "coupledtimestepper.hh"
+#include "stepbase.hh"
 
 struct IterationRegister {
   void registerCount(FixedPointIterationCounter count);
@@ -15,48 +21,67 @@ struct IterationRegister {
   FixedPointIterationCounter finalCount;
 };
 
-template <class Factory, class Updaters, class ErrorNorm>
+template <class Updaters>
+struct UpdatersWithCount {
+  Updaters updaters;
+  FixedPointIterationCounter count;
+};
+
+template <class Factory, class ContactNetwork, class Updaters, class ErrorNorms>
 class AdaptiveTimeStepper {
-  struct UpdatersWithCount {
-    Updaters updaters;
-    FixedPointIterationCounter count;
-  };
+  using UpdatersWithCount = UpdatersWithCount<Updaters>;
+
+  using StepBase = StepBase<Factory, ContactNetwork, Updaters, ErrorNorms>;
 
+  using NBodyAssembler = typename ContactNetwork::NBodyAssembler;
   using Vector = typename Factory::Vector;
-  using ConvexProblem = typename Factory::ConvexProblem;
-  using Nonlinearity = typename ConvexProblem::NonlinearityType;
+  using Matrix = typename Factory::Matrix;
 
-  using MyCoupledTimeStepper = CoupledTimeStepper<Factory, Updaters, ErrorNorm>;
+  using IgnoreVector = typename Factory::BitVector;
+
+  using MyCoupledTimeStepper = CoupledTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNorms>;
+
+  using GlobalFriction = typename MyCoupledTimeStepper::GlobalFriction;
+  using BitVector = typename MyCoupledTimeStepper::BitVector;
+  using ExternalForces = typename MyCoupledTimeStepper::ExternalForces;
 
 public:
-  AdaptiveTimeStepper(Factory &factory, Dune::ParameterTree const &parset,
-                      std::shared_ptr<Nonlinearity> globalFriction,
-                      Updaters &current, double relativeTime,
+  AdaptiveTimeStepper(const StepBase& stepBase,
+                      ContactNetwork& contactNetwork,
+                      Updaters &current,
+                      double relativeTime,
                       double relativeTau,
-                      std::function<void(double, Vector &)> externalForces,
-                      ErrorNorm const &errorNorm,
+                      double minTau,
                       std::function<bool(Updaters &, Updaters &)> mustRefine);
 
   bool reachedEnd();
+
   IterationRegister advance();
 
   double relativeTime_;
   double relativeTau_;
+  const double minTau_;
 
 private:
-  UpdatersWithCount step(Updaters const &oldUpdaters, double rTime,
-                         double rTau);
+  void setDeformation(const Updaters& updaters);
+
+  NBodyAssembler step(const NBodyAssembler& oldNBodyAssembler) const;
+
+  int refine(UpdatersWithCount& R2);
+
+  int coarsen();
+
+  int determineStrategy(UpdatersWithCount& R2);
+
+  const StepBase& stepBase_;
+  ContactNetwork& contactNetwork_;
 
-  double finalTime_;
-  Factory &factory_;
-  Dune::ParameterTree const &parset_;
-  std::shared_ptr<Nonlinearity> globalFriction_;
   Updaters &current_;
   UpdatersWithCount R1_;
-  std::function<void(double, Vector &)> externalForces_;
+
   std::function<bool(Updaters &, Updaters &)> mustRefine_;
-  ErrorNorm const &errorNorm_;
 
   IterationRegister iterationRegister_;
 };
+
 #endif

dune/tectonic/time-stepping/adaptivetimestepper_tmpl.cc

+#ifndef MY_DIM
+#error MY_DIM unset
+#endif
+
+#include <future>
+#include <thread>
+
+#include "../explicitgrid.hh"
+#include "../explicitvectors.hh"
+
+#include <dune/solvers/norms/energynorm.hh>
+#include <dune/solvers/solvers/loopsolver.hh>
+
+#include "../spatial-solving/tnnmg/functional.hh"
+#include "../spatial-solving/solverfactory.hh"
+
+#include "../data-structures/network/contactnetwork.hh"
+#include "../data-structures/friction/globalfriction.hh"
+
+#include "rate/rateupdater.hh"
+#include "state/stateupdater.hh"
+#include "updaters.hh"
+
+using MyContactNetwork = ContactNetwork<Grid, Vector>;
+
+using BoundaryNodes = typename MyContactNetwork::BoundaryNodes;
+using BoundaryFunctions = typename MyContactNetwork::BoundaryFunctions;
+
+using MyStateUpdater = StateUpdater<ScalarVector, Vector>;
+using MyRateUpdater = RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>;
+using MyUpdaters = Updaters<MyRateUpdater, MyStateUpdater>;
+
+using LinearSolver = Dune::Solvers::LoopSolver<Vector>;
+using ErrorNorms = typename MyContactNetwork::StateEnergyNorms;
+using MyNBodyAssembler = typename MyContactNetwork::NBodyAssembler;
+
+using MyGlobalFriction = GlobalFriction<Matrix, Vector>;
+using MyFunctional = Functional<Matrix&, Vector&, MyGlobalFriction&, Vector&, Vector&, double>;
+using MySolverFactory = SolverFactory<MyFunctional, BitVector>;
+
+template class AdaptiveTimeStepper<MySolverFactory, MyContactNetwork, MyUpdaters, ErrorNorms>;
+
+/*
+template std::packaged_task<typename AdaptiveTimeStepper<MySolverFactory, MyContactNetwork, MyUpdaters, ErrorNorms>::UpdatersWithCount()>
+AdaptiveTimeStepper<MySolverFactory, MyContactNetwork, MyUpdaters, ErrorNorms>::step<LinearSolver>(
+        const MyUpdaters&, const MyNBodyAssembler&, std::shared_ptr<LinearSolver>&, double, double); */

dune/tectonic/time-stepping/coupledtimestepper.cc

+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "coupledtimestepper.hh"
+
+#include "../utils/debugutils.hh"
+
+template <class Factory, class NBodyAssembler, class Updaters, class ErrorNorms>
+CoupledTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNorms>::CoupledTimeStepper(
+    double finalTime, Dune::ParameterTree const &parset,
+    const NBodyAssembler& nBodyAssembler,
+    const IgnoreVector& ignoreNodes,
+    GlobalFriction& globalFriction,
+    const std::vector<const BitVector*>& bodywiseNonmortarBoundaries,
+    Updaters updaters,
+    const ErrorNorms& errorNorms,
+    ExternalForces& externalForces)
+    : finalTime_(finalTime),
+      parset_(parset),
+      nBodyAssembler_(nBodyAssembler),
+      ignoreNodes_(ignoreNodes),
+      globalFriction_(globalFriction),
+      bodywiseNonmortarBoundaries_(bodywiseNonmortarBoundaries),
+      updaters_(updaters),
+      externalForces_(externalForces),
+      errorNorms_(errorNorms) {}
+
+template <class Factory, class NBodyAssembler, class Updaters, class ErrorNorms>
+template <class LinearSolver>
+FixedPointIterationCounter
+CoupledTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNorms>::step(std::shared_ptr<LinearSolver>& linearSolver, double relativeTime,
+                                                       double relativeTau) {
+
+  //std::cout << "CoupledTimeStepper::step()" << std::endl;
+
+  updaters_.state_->nextTimeStep();
+  updaters_.rate_->nextTimeStep();
+
+  auto const newRelativeTime = relativeTime + relativeTau;
+  std::vector<Vector> ell(externalForces_.size());
+  for (size_t i=0; i<externalForces_.size(); i++) {
+    (*externalForces_[i])(newRelativeTime, ell[i]);
+  }
+
+  std::vector<Matrix> velocityMatrix;
+  std::vector<Vector> velocityRHS;
+  std::vector<Vector> velocityIterate;
+
+  auto const tau = relativeTau * finalTime_;
+  updaters_.state_->setup(tau); 
+  updaters_.rate_->setup(ell, tau, newRelativeTime, velocityRHS, velocityIterate, velocityMatrix);
+
+/*  std::cout << "tau: " << tau << std::endl;
+  print(ell, "ell: ");
+  print(velocityRHS, "velocityRHS: ");
+  print(velocityIterate, "velocityIterate: ");
+  for (size_t i=0; i<velocityMatrix.size(); i++) {
+        print(velocityMatrix[i], "velocityMatrix: ");
+  }*/
+
+  FixedPointIterator fixedPointIterator(
+      parset_, nBodyAssembler_, ignoreNodes_, globalFriction_, bodywiseNonmortarBoundaries_, errorNorms_);
+  auto const iterations = fixedPointIterator.template run<LinearSolver>(updaters_, linearSolver,
+                                                 velocityMatrix, velocityRHS, velocityIterate);
+  return iterations;
+}
+
+#include "coupledtimestepper_tmpl.cc"

dune/tectonic/time-stepping/coupledtimestepper.hh

+#ifndef SRC_TIME_STEPPING_COUPLEDTIMESTEPPER_HH
+#define SRC_TIME_STEPPING_COUPLEDTIMESTEPPER_HH
+
+#include <functional>
+#include <memory>
+
+#include <dune/common/parametertree.hh>
+
+#include "../spatial-solving/fixedpointiterator.hh"
+
+template <class Factory, class NBodyAssembler, class Updaters, class ErrorNorms>
+class CoupledTimeStepper {
+  using Vector = typename Factory::Vector;
+  using Matrix = typename Factory::Matrix;
+  using IgnoreVector = typename Factory::BitVector;
+  using FixedPointIterator = FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorms>;
+
+public:
+  using GlobalFriction = typename FixedPointIterator::GlobalFriction;
+  using BitVector = typename FixedPointIterator::BitVector;
+  using ExternalForces = std::vector<std::unique_ptr<const std::function<void(double, Vector &)>>>;
+
+public:
+  CoupledTimeStepper(double finalTime,
+                     Dune::ParameterTree const &parset,
+                     const NBodyAssembler& nBodyAssembler,
+                     const IgnoreVector& ignoreNodes,
+                     GlobalFriction& globalFriction,
+                     const std::vector<const BitVector*>& bodywiseNonmortarBoundaries,
+                     Updaters updaters,
+                     const ErrorNorms& errorNorms,
+                     ExternalForces& externalForces);
+
+  template <class LinearSolver>
+  FixedPointIterationCounter step(std::shared_ptr<LinearSolver>& linearSolver, double relativeTime, double relativeTau);
+
+private:
+  double finalTime_;
+  Dune::ParameterTree const &parset_;
+  const NBodyAssembler& nBodyAssembler_;
+  const IgnoreVector& ignoreNodes_;
+
+  GlobalFriction& globalFriction_;
+  const std::vector<const BitVector*>& bodywiseNonmortarBoundaries_;
+
+  Updaters updaters_;
+  ExternalForces& externalForces_;
+  const ErrorNorms& errorNorms_;
+};
+
+#endif

dune/tectonic/time-stepping/coupledtimestepper_tmpl.cc

+#ifndef MY_DIM
+#error MY_DIM unset
+#endif
+
+#include "../explicitgrid.hh"
+#include "../explicitvectors.hh"
+
+#include <dune/solvers/norms/energynorm.hh>
+#include <dune/solvers/solvers/loopsolver.hh>
+
+#include "../spatial-solving/tnnmg/functional.hh"
+#include "../spatial-solving/solverfactory.hh"
+#include "../spatial-solving/tnnmg/zerononlinearity.hh"
+
+#include "../data-structures/network/contactnetwork.hh"
+#include "../data-structures/friction/globalfriction.hh"
+
+#include "rate/rateupdater.hh"
+#include "state/stateupdater.hh"
+#include "updaters.hh"
+
+using MyContactNetwork = ContactNetwork<Grid, Vector>;
+
+using BoundaryNodes = typename MyContactNetwork::BoundaryNodes;
+using BoundaryFunctions = typename MyContactNetwork::BoundaryFunctions;
+
+using MyStateUpdater = StateUpdater<ScalarVector, Vector>;
+using MyRateUpdater = RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>;
+using MyUpdaters = Updaters<MyRateUpdater, MyStateUpdater>;
+
+using LinearSolver = Dune::Solvers::LoopSolver<Vector>;
+using ErrorNorms = typename MyContactNetwork::StateEnergyNorms;
+using MyNBodyAssembler = typename MyContactNetwork::NBodyAssembler;
+
+using MyGlobalFriction = GlobalFriction<Matrix, Vector>;
+using MyFunctional = Functional<Matrix&, Vector&, MyGlobalFriction&, Vector&, Vector&, double>;
+using MySolverFactory = SolverFactory<MyFunctional, BitVector>;
+
+template class CoupledTimeStepper<MySolverFactory, MyNBodyAssembler, MyUpdaters, ErrorNorms>;
+template FixedPointIterationCounter CoupledTimeStepper<MySolverFactory, MyNBodyAssembler, MyUpdaters, ErrorNorms>::step<LinearSolver>(std::shared_ptr<LinearSolver>&, double, double);
+
+using NoFriction = ZeroNonlinearity;
+using NoFrictionFunctional = Functional<Matrix&, Vector&, NoFriction&, Vector&, Vector&, double>;
+using NoFrictionSolverFactory = SolverFactory<NoFrictionFunctional, BitVector>;
+
+template class CoupledTimeStepper<NoFrictionSolverFactory, MyNBodyAssembler, MyUpdaters, ErrorNorms>;
+template FixedPointIterationCounter CoupledTimeStepper<NoFrictionSolverFactory, MyNBodyAssembler, MyUpdaters, ErrorNorms>::step<LinearSolver>(std::shared_ptr<LinearSolver>&, double, double);

src/time-stepping/rate.cc → dune/tectonic/time-stepping/rate.cc

@@ -6,23 +6,26 @@
 #include "rate/backward_euler.hh"
 #include "rate/newmark.hh"
 
-template <class Vector, class Matrix, class Function, int dimension>
-std::shared_ptr<RateUpdater<Vector, Matrix, Function, dimension>>
-initRateUpdater(Config::scheme scheme,
-                Function const &velocityDirichletFunction,
-                Dune::BitSetVector<dimension> const &velocityDirichletNodes,
-                Matrices<Matrix> const &matrices, Vector const &u_initial,
-                Vector const &v_initial, Vector const &a_initial) {
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+auto initRateUpdater(Config::scheme scheme,
+                const BoundaryFunctions& velocityDirichletFunctions,
+                const BoundaryNodes& velocityDirichletNodes,
+                const Matrices<Matrix,2>& matrices,
+                const std::vector<Vector>& u_initial,
+                const std::vector<Vector>& v_initial,
+                const std::vector<Vector>& a_initial)
+-> std::shared_ptr<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>> {
+
   switch (scheme) {
     case Config::Newmark:
-      return std::make_shared<Newmark<Vector, Matrix, Function, dimension>>(
+      return std::make_shared<Newmark<Vector, Matrix, BoundaryFunctions, BoundaryNodes>>(
           matrices, u_initial, v_initial, a_initial, velocityDirichletNodes,
-          velocityDirichletFunction);
+          velocityDirichletFunctions);
     case Config::BackwardEuler:
       return std::make_shared<
-          BackwardEuler<Vector, Matrix, Function, dimension>>(
+          BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>>(
           matrices, u_initial, v_initial, a_initial, velocityDirichletNodes,
-          velocityDirichletFunction);
+          velocityDirichletFunctions);
     default:
       assert(false);
   }

dune/tectonic/time-stepping/rate.hh

+#ifndef SRC_TIME_STEPPING_RATE_HH
+#define SRC_TIME_STEPPING_RATE_HH
+
+#include <memory>
+
+#include "../data-structures/enums.hh"
+#include "rate/rateupdater.hh"
+
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+auto initRateUpdater(Config::scheme scheme,
+                const BoundaryFunctions& velocityDirichletFunctions,
+                const BoundaryNodes& velocityDirichletNodes,
+                const Matrices<Matrix,2>& matrices,
+                const std::vector<Vector>& u_initial,
+                const std::vector<Vector>& v_initial,
+                const std::vector<Vector>& a_initial)
+-> std::shared_ptr<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>>;
+#endif

dune/tectonic/time-stepping/rate/CMakeLists.txt

+add_custom_target(tectonic_dune_time-stepping_rate SOURCES
+  backward_euler.hh
+  backward_euler.cc
+  newmark.hh
+  newmark.cc
+  rateupdater.hh
+  rateupdater.cc
+)
+
+#install headers
+install(FILES
+  backward_euler.hh
+  newmark.hh
+  rateupdater.hh
+DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/dune/tectonic)

dune/tectonic/time-stepping/rate/backward_euler.cc

+#include <dune/matrix-vector/axpy.hh>
+#include <dune/istl/matrixindexset.hh>
+
+#include "backward_euler.hh"
+
+#include "../../utils/debugutils.hh"
+
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::BackwardEuler(
+        const Matrices<Matrix,2>& _matrices, const std::vector<Vector>& _u_initial,
+        const std::vector<Vector>& _v_initial, const std::vector<Vector>& _a_initial,
+        const BoundaryNodes& _dirichletNodes,
+        const BoundaryFunctions& _dirichletFunctions) :
+    RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>(
+        _matrices, _u_initial, _v_initial, _a_initial, _dirichletNodes,
+        _dirichletFunctions) {}
+
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+void BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::setup(
+        const std::vector<Vector>& ell,
+        double _tau,
+        double relativeTime,
+        std::vector<Vector>& rhs, std::vector<Vector>& iterate,
+        std::vector<Matrix>& AM) {
+
+    const size_t bodyCount = this->u.size();
+
+    rhs.resize(bodyCount);
+    iterate.resize(bodyCount);
+    AM.resize(bodyCount);
+
+    for (size_t i=0; i<bodyCount; i++) {
+        this->tau = _tau;
+
+        /* We start out with the formulation
+
+             M a + C v + A u = ell
+
+           Backward Euler means
+
+             a1 = 1.0/tau ( v1 - v0 )
+             u1 = tau v1 + u0
+
+           in summary, we get at time t=1
+
+             M [1.0/tau ( v1 - v0 )] + C v1
+             + A [tau v1 + u0] = ell
+
+           or
+
+             1.0/tau M v1 + C v1 + tau A v1
+             = [1.0/tau M + C + tau A] v1
+             = ell + 1.0/tau M v0 - A u0
+        */
+
+        // set up LHS (for fixed tau, we'd only really have to do this once)
+        Matrix& LHS = AM[i];
+        {
+        Dune::MatrixIndexSet indices(
+                    this->matrices.elasticity[i]->N(),
+                    this->matrices.elasticity[i]->M());
+        indices.import(*this->matrices.elasticity[i]);
+        indices.import(*this->matrices.mass[i]);
+        indices.import(*this->matrices.damping[i]);
+        indices.exportIdx(LHS);
+        }
+        LHS = 0.0;
+        Dune::MatrixVector::addProduct(LHS, 1.0 / this->tau, *this->matrices.mass[i]);
+        Dune::MatrixVector::addProduct(LHS, 1.0, *this->matrices.damping[i]);
+        Dune::MatrixVector::addProduct(LHS, this->tau, *this->matrices.elasticity[i]);
+
+        // set up RHS
+        {
+        Vector& rhss = rhs[i];
+        rhss = ell[i];
+        Dune::MatrixVector::addProduct(rhss, 1.0 / this->tau, *this->matrices.mass[i],
+                           this->v_o[i]);
+        Dune::MatrixVector::subtractProduct(rhss, *this->matrices.elasticity[i], this->u_o[i]);
+        }
+
+    }
+
+    iterate = this->v_o;
+
+    for (size_t i=0; i<bodyCount; i++) {
+        auto& bodyIterate = iterate[i];
+
+        const auto& bodyDirichletNodes = this->dirichletNodes[i];
+        const auto& bodyDirichletFunctions = this->dirichletFunctions[i];
+        for (size_t bc=0; bc<bodyDirichletNodes.size(); ++bc) {
+            const auto& bcDirichletNodes = *bodyDirichletNodes[bc];
+            size_t dim = bcDirichletNodes[0].size();
+
+            double dirichletValue;
+            bodyDirichletFunctions[bc]->evaluate(relativeTime, dirichletValue);
+
+            for (size_t k=0; k<bcDirichletNodes.size() ; ++k) {
+                for (size_t j=0; j<dim; ++j) {
+                    if (bcDirichletNodes[k][j]) {
+                        bodyIterate[k][j] = dirichletValue;
+                    }
+                }
+            }
+        }
+    }
+}
+
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+void BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::velocityObstacles(const Vector& u0, const Vector& uObstacles, const Vector& v0, Vector& v1Obstacles) {
+    // v1 = 1/tau ( u1 - u0 )
+    v1Obstacles.resize(u0.size());
+    v1Obstacles = 0;
+
+    Dune::MatrixVector::addProduct(v1Obstacles, 1.0 / this->tau, uObstacles);
+    Dune::MatrixVector::subtractProduct(v1Obstacles, 1.0 / this->tau, u0);
+}
+
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+void BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::postProcess(const std::vector<Vector>& iterate) {
+    this->postProcessCalled = true;
+
+    this->v = iterate;
+
+    this->u = this->u_o;
+
+    for (size_t i=0; i<this->u.size(); i++) {
+        Dune::MatrixVector::addProduct(this->u[i], this->tau, this->v[i]);
+
+        Vector& ai = this->a[i];
+        ai = this->v[i];
+        ai -= this->v_o[i];
+        ai /= this->tau;
+    }
+}
+
+template <class Vector, class Matrix, class BoundaryFunctions, class BoundaryNodes>
+auto BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>::clone() const
+-> std::shared_ptr<RateUpdater<Vector, Matrix, BoundaryFunctions, BoundaryNodes>> {
+
+    return std::make_shared<BackwardEuler<Vector, Matrix, BoundaryFunctions, BoundaryNodes>>(*this);
+}