diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index f2b048493b5f2de31b439f2023f7e211f8c76054..fbd99d9fe11f61a3ea84653dbbf8cb71bf7ee551 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,13 +1,14 @@
 set(SW_SOURCE_FILES
   assemblers.cc
-  body.cc
-  enumparser.cc
-  hdf5/frictionalboundary-writer.cc
-  hdf5/iteration-writer.cc
-  hdf5/patchinfo-writer.cc
-  hdf5/restart-io.cc
-  hdf5/surface-writer.cc
-  hdf5/time-writer.cc
+  data-structures/body.cc
+  data-structures/enumparser.cc
+  io/vtk.cc
+  io/hdf5/frictionalboundary-writer.cc
+  io/hdf5/iteration-writer.cc
+  io/hdf5/patchinfo-writer.cc
+  io/hdf5/restart-io.cc
+  io/hdf5/surface-writer.cc
+  io/hdf5/time-writer.cc
   one-body-problem-data/mygeometry.cc
   one-body-problem-data/mygrid.cc
   one-body-problem.cc
@@ -18,39 +19,40 @@ set(SW_SOURCE_FILES
   time-stepping/rate.cc
   time-stepping/rate/rateupdater.cc
   time-stepping/state.cc
-  vtk.cc
 )
 
 set(MSW_SOURCE_FILES
   assemblers.cc
-  body.cc
-  enumparser.cc
-  levelcontactnetwork.cc
-  hdf5/frictionalboundary-writer.cc
-  hdf5/iteration-writer.cc
-  hdf5/patchinfo-writer.cc
-  hdf5/restart-io.cc
-  hdf5/surface-writer.cc
-  hdf5/time-writer.cc
+  data-structures/body.cc
+  data-structures/contactnetwork.cc
+  data-structures/enumparser.cc
+  data-structures/levelcontactnetwork.cc
+  factories/cantorfactory.cc
+  factories/stackedblocksfactory.cc
+  io/vtk.cc
+  io/hdf5/frictionalboundary-writer.cc
+  io/hdf5/iteration-writer.cc
+  io/hdf5/patchinfo-writer.cc
+  io/hdf5/restart-io.cc
+  io/hdf5/surface-writer.cc
+  io/hdf5/time-writer.cc
   multi-body-problem-data/cuboidgeometry.cc
   multi-body-problem-data/mygrids.cc
   multi-body-problem.cc
   spatial-solving/fixedpointiterator.cc
   spatial-solving/solverfactory.cc
-  stackedblocksfactory.cc
   time-stepping/adaptivetimestepper.cc
   time-stepping/coupledtimestepper.cc
   time-stepping/rate.cc
   time-stepping/rate/rateupdater.cc
   time-stepping/state.cc
-  vtk.cc
 )
 
 set(UGW_SOURCE_FILES
   assemblers.cc # FIXME
+  io/uniform-grid-writer.cc
+  io/vtk.cc
   one-body-problem-data/mygrid.cc
-  uniform-grid-writer.cc
-  vtk.cc
 )
 
 foreach(_dim 2 3)
diff --git a/src/assemblers.hh b/src/assemblers.hh
index b3227a9a00425e81ecc1fa9df7b238a73139f0e7..195370d454400ba28d9a20ca9dc5630db916c124 100644
--- a/src/assemblers.hh
+++ b/src/assemblers.hh
@@ -16,7 +16,7 @@
 #include <dune/tectonic/globalfriction.hh>
 #include <dune/tectonic/globalfrictiondata.hh>
 
-#include "enums.hh"
+#include "data-structures/enums.hh"
 
 template <class GridView, int dimension> class MyAssembler {
 public:
diff --git a/src/assemblers_tmpl.cc b/src/assemblers_tmpl.cc
index 036dbc9ee59e8e5c5d964e06d0477d693cbb71b1..edc0a04178e95b4e0c730c368a808591c42b94d5 100644
--- a/src/assemblers_tmpl.cc
+++ b/src/assemblers_tmpl.cc
@@ -5,6 +5,6 @@
 #include "explicitgrid.hh"
 #include "explicitvectors.hh"
 
-#include "body.hh"
+#include "data-structures/body.hh"
 
 template class MyAssembler<Body<Grid, Vector, MY_DIM>::DeformedLeafGridView, MY_DIM>;
diff --git a/src/body.cc b/src/data-structures/body.cc
similarity index 100%
rename from src/body.cc
rename to src/data-structures/body.cc
diff --git a/src/body.hh b/src/data-structures/body.hh
similarity index 99%
rename from src/body.hh
rename to src/data-structures/body.hh
index df86f2fa62f571ee793741666fac454609289870..5359fbd096bb701967f70b319a0de6bc395c051a 100644
--- a/src/body.hh
+++ b/src/data-structures/body.hh
@@ -27,8 +27,8 @@
 #include <dune/tectonic/globalfriction.hh>
 #include <dune/tectonic/globalfrictiondata.hh>
 
-#include "assemblers.hh"
-#include "boundarycondition.hh"
+#include "../assemblers.hh"
+#include "../boundarycondition.hh"
 #include "enums.hh"
 #include "matrices.hh"
 
diff --git a/src/body_tmpl.cc b/src/data-structures/body_tmpl.cc
similarity index 58%
rename from src/body_tmpl.cc
rename to src/data-structures/body_tmpl.cc
index 25f890ffcff142320e4854264e6c2e9b564a832b..b35e1cfa9477a12275015ac21b81576dd3adda3d 100644
--- a/src/body_tmpl.cc
+++ b/src/data-structures/body_tmpl.cc
@@ -2,7 +2,7 @@
 #error MY_DIM unset
 #endif
 
-#include "explicitgrid.hh"
-#include "explicitvectors.hh"
+#include "../explicitgrid.hh"
+#include "../explicitvectors.hh"
 
 template class Body<Grid, Vector, MY_DIM>;
diff --git a/src/data-structures/contactnetwork.cc b/src/data-structures/contactnetwork.cc
new file mode 100644
index 0000000000000000000000000000000000000000..93708d26cf42da834ba82861cf69a8570c80ce23
--- /dev/null
+++ b/src/data-structures/contactnetwork.cc
@@ -0,0 +1,125 @@
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <dune/contact/projections/normalprojection.hh>
+
+#include "contactnetwork.hh"
+
+template <class GridType, int dimension>
+ContactNetwork<GridType, dimension>::ContactNetwork(int nBodies, int nCouplings, int level) :
+    level_(level),
+    bodies_(nBodies),
+    couplings_(nCouplings),
+    deformedGrids_(nBodies),
+    leafViews_(nBodies),
+    levelViews_(nBodies),
+    leafVertexCounts_(nBodies),
+    nBodyAssembler_(nBodies, nCouplings)
+{}
+
+template <class GridType, int dimension>
+void ContactNetwork<GridType, dimension>::assemble() {
+    for (size_t i=0; i<nBodies(); i++) {
+        bodies_[i]->assemble();
+    }
+
+    totalNodes("dirichlet", totalDirichletNodes_);
+
+    // set up dune-contact nBodyAssembler
+    nBodyAssembler_.setGrids(deformedGrids_);
+
+    for (size_t i=0; i<nCouplings(); i++) {
+      nBodyAssembler_.setCoupling(*couplings_[i], i);
+    }
+
+    nBodyAssembler_.assembleTransferOperator();
+    nBodyAssembler_.assembleObstacle();
+}
+
+template <class GridType, int dimension>
+void ContactNetwork<GridType, dimension>::assembleFriction(const Config::FrictionModel& frictionModel, const std::vector<ScalarVector>& weightedNormalStress) {
+    for (size_t i=0; i<nBodies(); i++) {
+        bodies_[i]->assembleFriction(frictionModel, weightedNormalStress[i]);
+    }
+}
+
+template <class GridType, int dimension>
+void ContactNetwork<GridType, dimension>::setBodies(const std::vector<std::shared_ptr<Body>>& bodies) {
+    assert(nBodies()==bodies.size());
+    bodies_ = bodies;
+
+    matrices_.elasticity.resize(nBodies());
+    matrices_.damping.resize(nBodies());
+    matrices_.mass.resize(nBodies());
+
+    for (size_t i=0; i<nBodies(); i++) {
+        deformedGrids_[i] = bodies_[i]->deformedGrid();
+
+        leafViews_[i] = std::make_unique<DeformedLeafGridView>(deformedGrids_[i]->leafGridView());
+        levelViews_[i] = std::make_unique<DeformedLevelGridView>(deformedGrids_[i]->levelGridView(0));
+
+        leafVertexCounts_[i] = leafViews_[i]->size(dimension);
+
+        matrices_.elasticity[i] = bodies_[i]->matrices().elasticity;
+        matrices_.damping[i] = bodies_[i]->matrices().damping;
+        matrices_.mass[i] = bodies_[i]->matrices().mass;
+    }
+}
+
+template <class GridType, int dimension>
+void ContactNetwork<GridType, dimension>::setCouplings(const std::vector<std::shared_ptr<CouplingPair>>& couplings) {
+    assert(this->nCouplings()==couplings.size());
+    couplings_ = couplings;
+}
+
+template <class GridType, int dimension>
+void ContactNetwork<GridType, dimension>::constructBody(const std::shared_ptr<BodyData<dimension>>& bodyData,
+                                   const std::shared_ptr<GridType>& grid,
+                                   std::shared_ptr<Body>& body) const
+{
+    body = std::make_shared<Body>(bodyData, grid);
+}
+
+template <class GridType, int dimension>
+void ContactNetwork<GridType, dimension>::constructCoupling(int nonMortarBodyIdx, int mortarBodyIdx,
+                                 const std::shared_ptr<DeformedLevelBoundaryPatch>& nonmortarPatch,
+                                 const std::shared_ptr<DeformedLevelBoundaryPatch>& mortarPatch,
+                                 std::shared_ptr<CouplingPair>& coupling) const
+{
+    coupling = std::make_shared<CouplingPair>();
+
+    auto contactProjection = std::make_shared<Dune::Contact::NormalProjection<DeformedLeafBoundaryPatch>>();
+    std::shared_ptr<typename CouplingPair::BackEndType> backend = nullptr;
+    coupling->set(nonMortarBodyIdx, mortarBodyIdx, nonmortarPatch, mortarPatch, 0.1, Dune::Contact::CouplingPairBase::STICK_SLIP, contactProjection, backend);
+}
+
+// collects all leaf boundary nodes from the different bodies identified by "tag" in a single BitSetVector
+template <class GridType, int dimension>
+void ContactNetwork<GridType, dimension>::totalNodes(const std::string& tag, Dune::BitSetVector<dimension>& totalNodes) {
+
+    int totalSize = 0;
+    for (size_t i=0; i<nBodies(); i++) {
+        totalSize += this->body(i)->leafVertexCount();
+    }
+
+    totalNodes.resize(totalSize);
+
+    int idx=0;
+    for (size_t i=0; i<nBodies(); i++) {
+        const auto& body = this->body(i);
+        std::vector<std::shared_ptr<typename Body::LeafBoundaryCondition>> boundaryConditions;
+        body->leafBoundaryConditions(tag, boundaryConditions);
+
+        const int idxBackup = idx;
+        for (size_t bc = 0; bc<boundaryConditions.size(); bc++) {
+            const auto& nodes = boundaryConditions[bc]->boundaryNodes();
+            for (size_t j=0; j<nodes->size(); j++, idx++)
+                for (int k=0; k<dimension; k++)
+                    totalNodes[idx][k] = (*nodes)[j][k];
+            idx = (bc==boundaryConditions.size()-1 ? idx : idxBackup);
+        }
+    }
+}
+
+#include "contactnetwork_tmpl.cc"
diff --git a/src/data-structures/contactnetwork.hh b/src/data-structures/contactnetwork.hh
new file mode 100644
index 0000000000000000000000000000000000000000..1a29a90b8eb61004e865194306830f09f1365042
--- /dev/null
+++ b/src/data-structures/contactnetwork.hh
@@ -0,0 +1,145 @@
+#ifndef SRC_LEVELCONTACTNETWORK_HH
+#define SRC_LEVELCONTACTNETWORK_HH
+
+#include <dune/common/fvector.hh>
+#include <dune/common/bitsetvector.hh>
+
+#include <dune/istl/bvector.hh>
+
+#include <dune/contact/common/deformedcontinuacomplex.hh>
+#include <dune/contact/common/couplingpair.hh>
+#include <dune/contact/assemblers/nbodyassembler.hh>
+//#include <dune/contact/assemblers/dualmortarcoupling.hh>
+#include <dune/contact/projections/normalprojection.hh>
+
+#include <dune/tectonic/bodydata.hh>
+#include <dune/tectonic/globalfriction.hh>
+
+#include "../assemblers.hh"
+#include "body.hh"
+#include "enums.hh"
+#include "matrices.hh"
+//#include "multi-body-problem-data/myglobalfrictiondata.hh"
+
+template <class GridType, int dimension> class ContactNetwork {
+  private:
+    using Vector = Dune::BlockVector<Dune::FieldVector<double, dimension>>;
+
+  public:
+    using Body = Body<GridType, Vector, dimension>;
+
+    using DeformedGridType = typename Body::DeformedGridType;
+    using DeformedLeafGridView = typename Body::DeformedLeafGridView;
+    using DeformedLevelGridView = typename Body::DeformedLevelGridView;
+
+    using DeformedLeafBoundaryPatch = BoundaryPatch<DeformedLeafGridView>;
+    using DeformedLevelBoundaryPatch = BoundaryPatch<DeformedLevelGridView>;
+
+
+
+
+   using Assembler = typename Body::Assembler;
+   using Matrix = typename Assembler::Matrix;
+   using LocalVector = typename Vector::block_type;
+   using ScalarMatrix = typename Assembler::ScalarMatrix;
+   using ScalarVector = typename Assembler::ScalarVector;
+   using field_type = typename Matrix::field_type;
+    
+   using CouplingPair = Dune::Contact::CouplingPair<DeformedGridType, DeformedGridType, field_type>;
+    
+   using Function = Dune::VirtualFunction<double, double>;
+
+  private:
+	const int level_;
+	
+    std::vector<std::shared_ptr<Body>> bodies_;
+	std::vector<std::shared_ptr<CouplingPair>> couplings_;
+	
+    std::vector<const DeformedGridType*> deformedGrids_;
+    std::vector<std::unique_ptr<const DeformedLeafGridView>> leafViews_;
+    std::vector<std::unique_ptr<const DeformedLevelGridView>> levelViews_;
+    std::vector<size_t> leafVertexCounts_;
+
+    Dune::Contact::NBodyAssembler<DeformedGridType, Vector> nBodyAssembler_;
+    Dune::BitSetVector<dimension> totalDirichletNodes_;
+
+    Matrices<Matrix,2> matrices_;
+   // std::vector<std::shared_ptr<MyGlobalFrictionData<LocalVector>>> frictionData_;
+   std::vector<std::shared_ptr<GlobalFriction<Matrix, Vector>>> globalFriction_;
+
+	public:
+        ContactNetwork(int nBodies, int nCouplings, int level = 0);
+
+        void assemble();
+        void assembleFriction(const Config::FrictionModel& frictionModel, const std::vector<ScalarVector>& weightedNormalStress);
+
+		int level() const {
+			return level_;
+		} 
+		
+        void setBodies(const std::vector<std::shared_ptr<Body>>& bodies);
+        void setCouplings(const std::vector<std::shared_ptr<CouplingPair>>& couplings);
+
+        void constructBody(const std::shared_ptr<BodyData<dimension>>& bodyData,
+                           const std::shared_ptr<GridType>& grid,
+                           std::shared_ptr<Body>& body) const;
+        void constructCoupling(int nonMortarBodyIdx, int mortarBodyIdx,
+                               const std::shared_ptr<DeformedLevelBoundaryPatch>& nonmortarPatch,
+                               const std::shared_ptr<DeformedLevelBoundaryPatch>& mortarPatch,
+                               std::shared_ptr<CouplingPair>& coupling) const;
+
+        void totalNodes(const std::string& tag, Dune::BitSetVector<dimension>& totalNodes);
+
+        size_t nBodies() const {
+            return bodies_.size();
+        }
+
+        size_t nCouplings() const {
+            return couplings_.size();
+        }
+
+        const std::vector<size_t>& leafVertexCounts() const {
+            return leafVertexCounts_;
+        }
+
+        const std::shared_ptr<Body>& body(int i) const {
+            return bodies_[i];
+        }
+
+        const std::vector<const DeformedGridType*>& deformedGrids() const {
+            return deformedGrids_;
+        }
+
+        const DeformedLeafGridView& leafView(int i) const {
+            return *leafViews_[i];
+        }
+
+        const DeformedLevelGridView& levelView(int i) const {
+            return *levelViews_[i];
+        }
+
+        int leafVertexCount(int i) const {
+            return leafVertexCounts_[i];
+        }
+
+        Dune::Contact::NBodyAssembler<DeformedGridType, Vector>& nBodyAssembler() {
+            return nBodyAssembler_;
+        }
+
+        const Dune::Contact::NBodyAssembler<DeformedGridType, Vector>& nBodyAssembler() const {
+            return nBodyAssembler_;
+        }
+
+        const Matrices<Matrix,2>& matrices() const {
+            return matrices_;
+        }
+
+        const std::vector<std::shared_ptr<GlobalFriction<Matrix, Vector>>>& globalFriction() const {
+            return globalFriction_;
+        }
+
+        const Dune::BitSetVector<dimension>& totalDirichletNodes() const {
+            return totalDirichletNodes_;
+        }
+};
+#endif
diff --git a/src/data-structures/contactnetwork_tmpl.cc b/src/data-structures/contactnetwork_tmpl.cc
new file mode 100644
index 0000000000000000000000000000000000000000..32c9a280f0a4babfa053e3bf37aebf154527b705
--- /dev/null
+++ b/src/data-structures/contactnetwork_tmpl.cc
@@ -0,0 +1,7 @@
+#ifndef MY_DIM
+#error MY_DIM unset
+#endif
+
+#include "../explicitgrid.hh"
+
+template class ContactNetwork<Grid, MY_DIM>;
diff --git a/src/enumparser.cc b/src/data-structures/enumparser.cc
similarity index 100%
rename from src/enumparser.cc
rename to src/data-structures/enumparser.cc
diff --git a/src/enumparser.hh b/src/data-structures/enumparser.hh
similarity index 100%
rename from src/enumparser.hh
rename to src/data-structures/enumparser.hh
diff --git a/src/enums.hh b/src/data-structures/enums.hh
similarity index 100%
rename from src/enums.hh
rename to src/data-structures/enums.hh
diff --git a/src/levelcontactnetwork.cc b/src/data-structures/levelcontactnetwork.cc
similarity index 100%
rename from src/levelcontactnetwork.cc
rename to src/data-structures/levelcontactnetwork.cc
diff --git a/src/levelcontactnetwork.hh b/src/data-structures/levelcontactnetwork.hh
similarity index 99%
rename from src/levelcontactnetwork.hh
rename to src/data-structures/levelcontactnetwork.hh
index be726c996f7cf0ca0300c4058534dbbf885047a2..ea47b6efdd690c45732ab4096ec4c09483f10c13 100644
--- a/src/levelcontactnetwork.hh
+++ b/src/data-structures/levelcontactnetwork.hh
@@ -15,7 +15,7 @@
 #include <dune/tectonic/bodydata.hh>
 #include <dune/tectonic/globalfriction.hh>
 
-#include "assemblers.hh"
+#include "../assemblers.hh"
 #include "body.hh"
 #include "enums.hh"
 #include "matrices.hh"
diff --git a/src/levelcontactnetwork_tmpl.cc b/src/data-structures/levelcontactnetwork_tmpl.cc
similarity index 75%
rename from src/levelcontactnetwork_tmpl.cc
rename to src/data-structures/levelcontactnetwork_tmpl.cc
index ee900a40719fa08df64fe9fb92838da6fc11fe05..b0c5200f198c29fe30a2407919ad55b94b36f461 100644
--- a/src/levelcontactnetwork_tmpl.cc
+++ b/src/data-structures/levelcontactnetwork_tmpl.cc
@@ -2,6 +2,6 @@
 #error MY_DIM unset
 #endif
 
-#include "explicitgrid.hh"
+#include "../explicitgrid.hh"
 
 template class LevelContactNetwork<Grid, MY_DIM>;
diff --git a/src/matrices.hh b/src/data-structures/matrices.hh
similarity index 100%
rename from src/matrices.hh
rename to src/data-structures/matrices.hh
diff --git a/src/program_state.hh b/src/data-structures/program_state.hh
similarity index 89%
rename from src/program_state.hh
rename to src/data-structures/program_state.hh
index ac5c10e322e4223f0423a92dc71e71156ea29f13..44b84f91086a1f35f01dd2e973c4c11cc3c088ea 100644
--- a/src/program_state.hh
+++ b/src/data-structures/program_state.hh
@@ -13,10 +13,12 @@
 
 #include <dune/tectonic/bodydata.hh>
 
-#include "assemblers.hh"
+#include "../assemblers.hh"
 #include "levelcontactnetwork.hh"
 #include "matrices.hh"
-#include "spatial-solving/solverfactory.hh"
+#include "../spatial-solving/solverfactory.hh"
+
+#include "../utils/debugutils.hh"
 
 template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class BodyState {
 public:
@@ -82,7 +84,6 @@ template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
       return bodyCount_;
   }
 
-
   // Set up initial conditions
   template <class GridType>
   void setupInitialConditions(const Dune::ParameterTree& parset, const LevelContactNetwork<GridType, dims>& levelContactNetwork) {
@@ -98,6 +99,7 @@ template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
       std::vector<const Matrix*> matrices_ptr(_matrices.size());
       for (size_t i=0; i<matrices_ptr.size(); i++) {
             matrices_ptr[i] = _matrices[i].get();
+            print(*matrices_ptr[i], "matrix " + std::to_string(i));
       }
 
       /*std::vector<Matrix> matrices(velocityMatrices.size());
@@ -127,6 +129,11 @@ template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
       using LinearFactory = SolverFactory<DeformedGridType, GlobalFriction<Matrix, Vector>, Matrix, Vector>;
       LinearFactory factory(parset.sub("solver.tnnmg"), nBodyAssembler, _dirichletNodes);
 
+      print(_dirichletNodes, "dirichletNodes: ");
+      print(bilinearForm, "matrix: ");
+      print(totalX, "totalX: ");
+      print(totalRhs, "totalRhs: ");
+
       auto multigridStep = factory.getStep();
       multigridStep->setProblem(bilinearForm, totalX, totalRhs);
 
@@ -162,7 +169,20 @@ template <class VectorTEMPLATE, class ScalarVectorTEMPLATE> class ProgramState {
     // 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
-    solveLinearProblem(levelContactNetwork.totalDirichletNodes(), levelContactNetwork.matrices().elasticity, ell0, u,
+    Dune::BitSetVector<dims> dirichletNodes = levelContactNetwork.totalDirichletNodes();
+    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;
+        }
+    }
+
+    solveLinearProblem(dirichletNodes, levelContactNetwork.matrices().elasticity, ell0, u,
                        parset.sub("u0.solver"));
 
     // Initial acceleration: Computed in agreement with Ma = ell0 - Au
diff --git a/src/factories/cantorfactory.cc b/src/factories/cantorfactory.cc
new file mode 100644
index 0000000000000000000000000000000000000000..4c30f26697f55681df4663adafd5238e90e76492
--- /dev/null
+++ b/src/factories/cantorfactory.cc
@@ -0,0 +1,238 @@
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <dune/common/fvector.hh>
+
+#include <dune/fufem/geometry/convexpolyhedron.hh>
+
+#include <dune/contact/projections/normalprojection.hh>
+
+#include "cantorfactory.hh"
+
+#include "../multi-body-problem-data/bc.hh"
+#include "../multi-body-problem-data/cuboidgeometry.hh"
+#include "../multi-body-problem-data/myglobalfrictiondata.hh"
+#include "../multi-body-problem-data/weakpatch.hh"
+
+#include "../utils/diameter.hh"
+
+template <int dim>
+class Cube {
+private:
+    using Vector = Dune::FieldVector<double, dim>;
+
+    Vector A_; // two corners that are diagonal define cube in any space dimension
+    Vector B_;
+
+    bool invariant_; // flag to set if Cube can be split()
+
+   /* bool isDiagonal(const Vector& A, const Vector& B) const {
+        Vector diff = A;
+        diff -= B;
+
+        for (size_t d=0; d<dim; d++) {
+            if (std::abs(diff[d])< 1e-14)
+                return false;
+        }
+        return true;
+    } */
+
+public:
+    Cube(bool invariant = false) {
+        invariant_ = invariant;
+    }
+
+    Cube(const Vector& A, const Vector& B, bool invariant = false) {
+        setCorners(A, B);
+        invariant_ = invariant;
+    }
+
+    void setCorners(const Vector& A, const Vector& B) {
+        //assert(isDiagonal(corners[0], corners[1]));
+        A_ = A;
+        B_ = B;
+    }
+
+    void split(std::vector<Cube>& newCubes) const {
+        assert(!invariant_);
+
+        Vector midPoint = A_;
+        midPoint += 1/2*(B_ - A_);
+
+        newCubes.push_back();
+    }
+};
+
+template <class GridType, int dims>
+void CantorFactory<GridType, dims>::setBodies() {
+    // set up cuboid geometries
+
+    double const length = 1.00;
+    double const width = 0.27;
+    double const weakLength = 0.20;
+
+    std::vector<LocalVector> origins(this->bodyCount_);
+    std::vector<LocalVector> lowerWeakOrigins(this->bodyCount_);
+    std::vector<LocalVector> upperWeakOrigins(this->bodyCount_);
+
+#if MY_DIM == 3
+        double const depth = 0.60;
+
+        origins[0] = {0,0,0};
+        lowerWeakOrigins[0] = {0.2,0,0};
+        upperWeakOrigins[0] = {0.2,width,0};
+
+        cuboidGeometries_[0] = std::make_shared<CuboidGeometry>(origins[0], lowerWeakOrigins[0], upperWeakOrigins[0], 0.0, weakLength, length, width, depth);
+        for (size_t i=1; i<this->bodyCount_-1; i++) {
+            origins[i] = cuboidGeometries_[i-1]->D;
+            lowerWeakOrigins[i] = {0.6,i*width,0};
+            upperWeakOrigins[i] = {0.6,(i+1)*width,0};
+
+            cuboidGeometries_[i] = std::make_shared<CuboidGeometry>(origins[i], lowerWeakOrigins[i], upperWeakOrigins[i], weakLength, weakLength, length, width, depth);
+        }
+        const size_t idx = this->bodyCount_-1;
+        origins[idx] = cuboidGeometries_[idx-1]->D;
+        lowerWeakOrigins[idx] = {0.6,idx*width,0};
+        upperWeakOrigins[idx] = {0.6,(idx+1)*width,0};
+
+        cuboidGeometries_[idx] = std::make_shared<CuboidGeometry>(origins[idx], lowerWeakOrigins[idx], upperWeakOrigins[idx], weakLength, 0.0, length, width, depth);
+#elif MY_DIM == 2
+        origins[0] = {0,0};
+        lowerWeakOrigins[0] = {0.2,0};
+        upperWeakOrigins[0] = {0.2,width};
+
+        cuboidGeometries_[0] = std::make_shared<CuboidGeometry>(origins[0], lowerWeakOrigins[0], upperWeakOrigins[0], 0.0, weakLength, length, width);
+        for (size_t i=1; i<this->bodyCount_-1; i++) {
+            origins[i] = cuboidGeometries_[i-1]->D;
+            lowerWeakOrigins[i] = {0.6,i*width};
+            upperWeakOrigins[i] = {0.6,(i+1)*width};
+
+            cuboidGeometries_[i] = std::make_shared<CuboidGeometry>(origins[i], lowerWeakOrigins[i], upperWeakOrigins[i], weakLength, weakLength, length, width);
+        }
+        const size_t idx = this->bodyCount_-1;
+        origins[idx] = cuboidGeometries_[idx-1]->D;
+        lowerWeakOrigins[idx] = {0.6,idx*width};
+        upperWeakOrigins[idx] = {0.6,(idx+1)*width};
+
+        cuboidGeometries_[idx] = std::make_shared<CuboidGeometry>(origins[idx], lowerWeakOrigins[idx], upperWeakOrigins[idx], weakLength, 0.0, length, width);
+#else
+#error CuboidGeometry only supports 2D and 3D!"
+#endif
+
+    // set up reference grids
+    gridConstructor_ = new GridsConstructor<GridType>(cuboidGeometries_);
+    auto& grids = gridConstructor_->getGrids();
+
+    for (size_t i=0; i<this->bodyCount_; i++) {
+        const auto& cuboidGeometry = *cuboidGeometries_[i];
+
+        // define weak patch and refine grid
+        auto lowerWeakPatch = lowerWeakPatches_[i] = std::make_shared<ConvexPolyhedron<LocalVector>>();
+        auto upperWeakPatch = upperWeakPatches_[i] = std::make_shared<ConvexPolyhedron<LocalVector>>();
+        getWeakPatch<LocalVector>(this->parset_.sub("boundary.friction.weakening"), cuboidGeometry, *lowerWeakPatch, *upperWeakPatch);
+        refine(*grids[i], *lowerWeakPatch, this->parset_.template get<double>("boundary.friction.smallestDiameter"), cuboidGeometry.lengthScale);
+        refine(*grids[i], *upperWeakPatch, this->parset_.template get<double>("boundary.friction.smallestDiameter"), cuboidGeometry.lengthScale);
+
+        // determine minDiameter and maxDiameter
+        double minDiameter = std::numeric_limits<double>::infinity();
+        double maxDiameter = 0.0;
+        for (auto &&e : elements(grids[i]->leafGridView())) {
+          auto const geometry = e.geometry();
+          auto const diam = diameter(geometry);
+          minDiameter = std::min(minDiameter, diam);
+          maxDiameter = std::max(maxDiameter, diam);
+        }
+        std::cout << "Grid" << i << " min diameter: " << minDiameter << std::endl;
+        std::cout << "Grid" << i << " max diameter: " << maxDiameter << std::endl;
+    }
+
+    for (size_t i=0; i<this->bodyCount_; i++) {
+        this->bodies_[i] = std::make_shared<typename Base::Body>(bodyData_, grids[i]);
+    }
+}
+
+template <class GridType, int dims>
+void CantorFactory<GridType, dims>::setCouplings() {
+    const auto deformedGrids = this->contactNetwork_.deformedGrids();
+
+    for (size_t i=0; i<this->bodyCount_; i++) {
+        const auto& cuboidGeometry = *cuboidGeometries_[i];
+        leafFaces_[i] = std::make_shared<LeafFaces>(this->contactNetwork_.leafView(i), cuboidGeometry);
+        levelFaces_[i] = std::make_shared<LevelFaces>(this->contactNetwork_.levelView(i), cuboidGeometry);
+    }
+
+    auto contactProjection = std::make_shared<Dune::Contact::NormalProjection<LeafBoundaryPatch>>();
+    std::shared_ptr<typename Base::CouplingPair::BackEndType> backend = nullptr;
+    for (size_t i=0; i<this->couplings_.size(); i++) {
+      auto& coupling = this->couplings_[i];
+      coupling = std::make_shared<typename Base::CouplingPair>();
+
+      nonmortarPatch_[i] = std::make_shared<LevelBoundaryPatch>(levelFaces_[i]->upper);
+      mortarPatch_[i] = std::make_shared<LevelBoundaryPatch>(levelFaces_[i+1]->lower);
+
+      coupling->set(i, i+1, nonmortarPatch_[i], mortarPatch_[i], 0.1, Base::CouplingPair::CouplingType::STICK_SLIP, contactProjection, backend);
+    }
+}
+
+template <class GridType, int dims>
+void CantorFactory<GridType, dims>::setBoundaryConditions() {
+    using LeafBoundaryCondition = BoundaryCondition<DeformedLeafGridView, dims>;
+    using FrictionBoundaryCondition = FrictionBoundaryCondition<DeformedLeafGridView, LocalVector, dims>;
+
+    using Function = Dune::VirtualFunction<double, double>;
+    std::shared_ptr<Function> neumannFunction = std::make_shared<NeumannCondition>();
+    std::shared_ptr<Function> velocityDirichletFunction = std::make_shared<VelocityDirichletCondition>();
+
+    const double lengthScale = CuboidGeometry::lengthScale;
+
+    for (size_t i=0; i<this->bodyCount_; i++) {
+      const auto& body = this->contactNetwork_.body(i);
+      const auto& leafVertexCount = body->leafVertexCount();
+
+      std::cout << "Grid" << i << " Number of DOFs: " << leafVertexCount << std::endl;
+
+      // friction boundary
+      if (i<this->bodyCount_-1 && upperWeakPatches_[i]->vertices.size()>0) {
+        std::shared_ptr<FrictionBoundaryCondition> frictionBoundary = std::make_shared<FrictionBoundaryCondition>();
+        frictionBoundary->setBoundaryPatch(leafFaces_[i]->upper);
+        frictionBoundary->setWeakeningPatch(upperWeakPatches_[i]);
+        frictionBoundary->setFrictionData(std::make_shared<MyGlobalFrictionData<LocalVector>>(this->parset_.sub("boundary.friction"), *upperWeakPatches_[i], lengthScale));
+        body->addBoundaryCondition(frictionBoundary);
+      }
+      if (i>0 && lowerWeakPatches_[i]->vertices.size()>0) {
+        std::shared_ptr<FrictionBoundaryCondition> frictionBoundary = std::make_shared<FrictionBoundaryCondition>();
+        frictionBoundary->setBoundaryPatch(leafFaces_[i]->lower);
+        frictionBoundary->setWeakeningPatch(lowerWeakPatches_[i]);
+        frictionBoundary->setFrictionData(std::make_shared<MyGlobalFrictionData<LocalVector>>(this->parset_.sub("boundary.friction"), *lowerWeakPatches_[i], lengthScale));
+        body->addBoundaryCondition(frictionBoundary);
+      }
+
+      // Neumann boundary
+      std::shared_ptr<LeafBoundaryCondition> neumannBoundary = std::make_shared<LeafBoundaryCondition>(std::make_shared<LeafBoundaryPatch>(body->leafView()), neumannFunction, "neumann");
+      body->addBoundaryCondition(neumannBoundary);
+
+      // Dirichlet Boundary
+      // identify Dirichlet nodes on leaf level
+      std::shared_ptr<Dune::BitSetVector<dims>> dirichletNodes = std::make_shared<Dune::BitSetVector<dims>>(leafVertexCount);
+      for (int j=0; j<leafVertexCount; j++) {
+        if (leafFaces_[i]->right.containsVertex(j))
+          (*dirichletNodes)[j][0] = true;
+
+        if (leafFaces_[i]->lower.containsVertex(j))
+          (*dirichletNodes)[j][0] = true;
+
+        #if MY_DIM == 3
+        if (leafFaces_[i]->front.containsVertex(j) || leafFaces_[i]->back.containsVertex(j))
+            dirichletNodes->at(j)[2] = true;
+        #endif
+      }
+
+      std::shared_ptr<LeafBoundaryCondition> dirichletBoundary = std::make_shared<LeafBoundaryCondition>("dirichlet");
+      dirichletBoundary->setBoundaryPatch(body->leafView(), dirichletNodes);
+      dirichletBoundary->setBoundaryFunction(velocityDirichletFunction);
+      body->addBoundaryCondition(dirichletBoundary);
+    }
+}
+
+#include "cantorfactory_tmpl.cc"
diff --git a/src/factories/cantorfactory.hh b/src/factories/cantorfactory.hh
new file mode 100644
index 0000000000000000000000000000000000000000..8818fed94c60ab15da62d023a4e8ee166b404886
--- /dev/null
+++ b/src/factories/cantorfactory.hh
@@ -0,0 +1,78 @@
+#ifndef SRC_CANTORFACTORY_HH
+#define SRC_CANTORFACTORY_HH
+
+#include <dune/common/bitsetvector.hh>
+#include <dune/common/function.hh>
+#include <dune/common/fvector.hh>
+
+#include <dune/fufem/boundarypatch.hh>
+
+#include "contactnetworkfactory.hh"
+
+#include "../multi-body-problem-data/mybody.hh"
+#include "../multi-body-problem-data/mygrids.hh"
+
+template <class GridType, int dims> class CantorFactory : public ContactNetworkFactory<GridType, dims>{
+private:
+    using Base = ContactNetworkFactory<GridType, dims>;
+
+    using LocalVector = typename Base::ContactNetwork::LocalVector;
+
+    using DeformedLeafGridView = typename Base::ContactNetwork::DeformedLeafGridView;
+    using DeformedLevelGridView = typename Base::ContactNetwork::DeformedLevelGridView;
+
+    using LevelBoundaryPatch = BoundaryPatch<DeformedLevelGridView>;
+    using LeafBoundaryPatch = BoundaryPatch<DeformedLeafGridView>;
+
+    using LeafFaces = MyFaces<DeformedLeafGridView>;
+    using LevelFaces = MyFaces<DeformedLevelGridView>;
+
+private:
+    const std::shared_ptr<MyBodyData<dims>> bodyData_;     // material properties of bodies
+
+    GridsConstructor<GridType>* gridConstructor_;
+
+    std::vector<std::shared_ptr<CuboidGeometry>> cuboidGeometries_;
+
+    std::vector<std::shared_ptr<LeafFaces>> leafFaces_;
+    std::vector<std::shared_ptr<LevelFaces>> levelFaces_;
+
+    std::vector<std::shared_ptr<ConvexPolyhedron<LocalVector>>> lowerWeakPatches_;
+    std::vector<std::shared_ptr<ConvexPolyhedron<LocalVector>>> upperWeakPatches_;
+
+    std::vector<std::shared_ptr<LevelBoundaryPatch>> nonmortarPatch_;
+    std::vector<std::shared_ptr<LevelBoundaryPatch>> mortarPatch_;
+
+public:
+    CantorFactory(const Dune::ParameterTree& parset) :
+        Base(parset, parset.get<size_t>("problem.bodyCount"), parset.get<size_t>("problem.bodyCount")-1),
+        bodyData_(std::make_shared<MyBodyData<dims>>(this->parset_, zenith_())),
+        cuboidGeometries_(this->bodyCount_),
+        leafFaces_(this->bodyCount_),
+        levelFaces_(this->bodyCount_),
+        lowerWeakPatches_(this->bodyCount_),
+        upperWeakPatches_(this->bodyCount_),
+        nonmortarPatch_(this->couplingCount_),
+        mortarPatch_(this->couplingCount_)
+    {}
+
+    ~CantorFactory() {
+        delete gridConstructor_;
+    }
+
+    void setBodies();
+    void setCouplings();
+    void setBoundaryConditions();
+
+private:
+    static constexpr Dune::FieldVector<double, MY_DIM> zenith_() {
+        #if MY_DIM == 2
+        return {0, 1};
+        #elif MY_DIM == 3
+        return {0, 1, 0};
+        #endif
+    }
+};
+#endif
+
+
diff --git a/src/factories/cantorfactory_tmpl.cc b/src/factories/cantorfactory_tmpl.cc
new file mode 100644
index 0000000000000000000000000000000000000000..00d2a833fc5a8301d7329d234d3b8e9e642fc4df
--- /dev/null
+++ b/src/factories/cantorfactory_tmpl.cc
@@ -0,0 +1,7 @@
+#ifndef MY_DIM
+#error MY_DIM unset
+#endif
+
+#include "../explicitgrid.hh"
+
+template class CantorFactory<Grid, MY_DIM>;
diff --git a/src/factories/contactnetworkfactory.hh b/src/factories/contactnetworkfactory.hh
new file mode 100644
index 0000000000000000000000000000000000000000..bee811e7ed717428a93d5c0bdb56cc8bd782401a
--- /dev/null
+++ b/src/factories/contactnetworkfactory.hh
@@ -0,0 +1,54 @@
+#ifndef SRC_CONTACTNETWORKFACTORY_HH
+#define SRC_CONTACTNETWORKFACTORY_HH
+
+#include <dune/common/parametertree.hh>
+
+#include "../data-structures/contactnetwork.hh"
+
+template <class GridType, int dims>
+class ContactNetworkFactory {
+public:
+    using ContactNetwork = ContactNetwork<GridType, dims>;
+
+protected:
+    using Body = typename ContactNetwork::Body;
+    using CouplingPair = typename ContactNetwork::CouplingPair;
+
+    const Dune::ParameterTree& parset_;
+    const size_t bodyCount_;
+    const size_t couplingCount_;
+
+    std::vector<std::shared_ptr<Body>> bodies_;
+    std::vector<std::shared_ptr<CouplingPair>> couplings_;
+
+    ContactNetwork contactNetwork_;
+
+private:
+    virtual void setBodies() = 0;
+    virtual void setCouplings() = 0;
+    virtual void setBoundaryConditions() = 0;
+
+public:
+    ContactNetworkFactory(const Dune::ParameterTree& parset, size_t bodyCount, size_t couplingCount) :
+        parset_(parset),
+        bodyCount_(bodyCount),
+        couplingCount_(couplingCount),
+        bodies_(bodyCount_),
+        couplings_(couplingCount_),
+        contactNetwork_(bodyCount_, couplingCount_) {}
+
+    void build() {
+        setBodies();
+        contactNetwork_.setBodies(bodies_);
+
+        setCouplings();
+        contactNetwork_.setCouplings(couplings_);
+
+        setBoundaryConditions();
+    }
+
+    ContactNetwork& levelContactNetwork() {
+        return contactNetwork_;
+    }
+};
+#endif
diff --git a/src/levelcontactnetworkfactory.hh b/src/factories/levelcontactnetworkfactory.hh
similarity index 96%
rename from src/levelcontactnetworkfactory.hh
rename to src/factories/levelcontactnetworkfactory.hh
index f9fb4e94ebd6c8396e12fe8ab49604150a1c5d86..fa026527746245fccd452a9ee305634f7a370ccf 100644
--- a/src/levelcontactnetworkfactory.hh
+++ b/src/factories/levelcontactnetworkfactory.hh
@@ -3,7 +3,7 @@
 
 #include <dune/common/parametertree.hh>
 
-#include "levelcontactnetwork.hh"
+#include "../data-structures/levelcontactnetwork.hh"
 
 template <class GridType, int dims>
 class LevelContactNetworkFactory {
diff --git a/src/stackedblocksfactory.cc b/src/factories/stackedblocksfactory.cc
similarity index 97%
rename from src/stackedblocksfactory.cc
rename to src/factories/stackedblocksfactory.cc
index 4c31d3a98c229bea5f6cd4640d1178cd49332b80..504ddff81aa9df3cce6434af4c1e9445890976c5 100644
--- a/src/stackedblocksfactory.cc
+++ b/src/factories/stackedblocksfactory.cc
@@ -6,12 +6,12 @@
 
 #include <dune/contact/projections/normalprojection.hh>
 
-#include "diameter.hh"
+#include "../multi-body-problem-data/bc.hh"
+#include "../multi-body-problem-data/cuboidgeometry.hh"
+#include "../multi-body-problem-data/myglobalfrictiondata.hh"
+#include "../multi-body-problem-data/weakpatch.hh"
 
-#include "multi-body-problem-data/bc.hh"
-#include "multi-body-problem-data/cuboidgeometry.hh"
-#include "multi-body-problem-data/myglobalfrictiondata.hh"
-#include "multi-body-problem-data/weakpatch.hh"
+#include "../utils/diameter.hh"
 
 #include "stackedblocksfactory.hh"
 
diff --git a/src/stackedblocksfactory.hh b/src/factories/stackedblocksfactory.hh
similarity index 96%
rename from src/stackedblocksfactory.hh
rename to src/factories/stackedblocksfactory.hh
index 572019d7234ca2c2ecb722a265056e4344f89ed4..cf486808766bd42e7b9fdc0c606fa753630ba6f4 100644
--- a/src/stackedblocksfactory.hh
+++ b/src/factories/stackedblocksfactory.hh
@@ -8,8 +8,9 @@
 #include <dune/fufem/boundarypatch.hh>
 
 #include "levelcontactnetworkfactory.hh"
-#include "multi-body-problem-data/mybody.hh"
-#include "multi-body-problem-data/mygrids.hh"
+
+#include "../multi-body-problem-data/mybody.hh"
+#include "../multi-body-problem-data/mygrids.hh"
 
 template <class GridType, int dims> class StackedBlocksFactory : public LevelContactNetworkFactory<GridType, dims>{
 private:
diff --git a/src/stackedblocksfactory_tmpl.cc b/src/factories/stackedblocksfactory_tmpl.cc
similarity index 76%
rename from src/stackedblocksfactory_tmpl.cc
rename to src/factories/stackedblocksfactory_tmpl.cc
index 173ea904917e531507cada5f42d8c72d86ea7258..c72f5199128bffe74ccf3c2991db6062999c4176 100644
--- a/src/stackedblocksfactory_tmpl.cc
+++ b/src/factories/stackedblocksfactory_tmpl.cc
@@ -2,6 +2,6 @@
 #error MY_DIM unset
 #endif
 
-#include "explicitgrid.hh"
+#include "../explicitgrid.hh"
 
 template class StackedBlocksFactory<Grid, MY_DIM>;
diff --git a/src/hdf5-writer.hh b/src/io/hdf5-writer.hh
similarity index 100%
rename from src/hdf5-writer.hh
rename to src/io/hdf5-writer.hh
diff --git a/src/hdf5/frictionalboundary-writer.cc b/src/io/hdf5/frictionalboundary-writer.cc
similarity index 100%
rename from src/hdf5/frictionalboundary-writer.cc
rename to src/io/hdf5/frictionalboundary-writer.cc
diff --git a/src/hdf5/frictionalboundary-writer.hh b/src/io/hdf5/frictionalboundary-writer.hh
similarity index 100%
rename from src/hdf5/frictionalboundary-writer.hh
rename to src/io/hdf5/frictionalboundary-writer.hh
diff --git a/src/hdf5/frictionalboundary-writer_tmpl.cc b/src/io/hdf5/frictionalboundary-writer_tmpl.cc
similarity index 74%
rename from src/hdf5/frictionalboundary-writer_tmpl.cc
rename to src/io/hdf5/frictionalboundary-writer_tmpl.cc
index 3f8251ae4db70b1f9244fa28de7dc0f078cea20d..883c59bad4d604df0bc7d3bed63a0bf236aa61eb 100644
--- a/src/hdf5/frictionalboundary-writer_tmpl.cc
+++ b/src/io/hdf5/frictionalboundary-writer_tmpl.cc
@@ -1,7 +1,7 @@
-#include "../explicitvectors.hh"
-#include "../explicitgrid.hh"
+#include "../../explicitvectors.hh"
+#include "../../explicitgrid.hh"
 
-#include "../program_state.hh"
+#include "../../data-structures/program_state.hh"
 
 using MyProgramState = ProgramState<Vector, ScalarVector>;
 using MyFriction = GlobalFriction<Matrix, Vector>;
diff --git a/src/hdf5/iteration-writer.cc b/src/io/hdf5/iteration-writer.cc
similarity index 100%
rename from src/hdf5/iteration-writer.cc
rename to src/io/hdf5/iteration-writer.cc
diff --git a/src/hdf5/iteration-writer.hh b/src/io/hdf5/iteration-writer.hh
similarity index 91%
rename from src/hdf5/iteration-writer.hh
rename to src/io/hdf5/iteration-writer.hh
index f3cb6e064ec3a4df7eaeb28869f39fe1bbe72f47..d0e010f18e5c7723fb7e1704bcd0d91cd11c899d 100644
--- a/src/hdf5/iteration-writer.hh
+++ b/src/io/hdf5/iteration-writer.hh
@@ -3,7 +3,7 @@
 
 #include <dune/fufem/hdf5/sequenceio.hh>
 
-#include "../time-stepping/adaptivetimestepper.hh"
+#include "../../time-stepping/adaptivetimestepper.hh"
 
 class IterationWriter {
 public:
diff --git a/src/hdf5/patchinfo-writer.cc b/src/io/hdf5/patchinfo-writer.cc
similarity index 100%
rename from src/hdf5/patchinfo-writer.cc
rename to src/io/hdf5/patchinfo-writer.cc
diff --git a/src/hdf5/patchinfo-writer.hh b/src/io/hdf5/patchinfo-writer.hh
similarity index 96%
rename from src/hdf5/patchinfo-writer.hh
rename to src/io/hdf5/patchinfo-writer.hh
index ef98981c9743d0265be0aeafaaa6bcbf25e12e56..cd44653427a458c467a95ae8320d15ed3c86ab88 100644
--- a/src/hdf5/patchinfo-writer.hh
+++ b/src/io/hdf5/patchinfo-writer.hh
@@ -8,7 +8,7 @@
 #include <dune/fufem/geometry/convexpolyhedron.hh>
 #include <dune/fufem/hdf5/sequenceio.hh>
 
-#include "../one-body-problem-data/mygeometry.hh"
+#include "../../one-body-problem-data/mygeometry.hh"
 
 template <class LocalVector, class GridView> class GridEvaluator {
   using Element = typename GridView::Grid::template Codim<0>::Entity;
diff --git a/src/hdf5/patchinfo-writer_tmpl.cc b/src/io/hdf5/patchinfo-writer_tmpl.cc
similarity index 80%
rename from src/hdf5/patchinfo-writer_tmpl.cc
rename to src/io/hdf5/patchinfo-writer_tmpl.cc
index 16bbf51270cf61e43c650084bbf6e9b43aec0fcb..0b281fead6022b1d4616db26be02fc99a08598dd 100644
--- a/src/hdf5/patchinfo-writer_tmpl.cc
+++ b/src/io/hdf5/patchinfo-writer_tmpl.cc
@@ -1,7 +1,7 @@
-#include "../explicitvectors.hh"
-#include "../explicitgrid.hh"
+#include "../../explicitvectors.hh"
+#include "../../explicitgrid.hh"
 
-#include "../program_state.hh"
+#include "../../data-structures/program_state.hh"
 
 using MyProgramState = ProgramState<Vector, ScalarVector>;
 using P1Basis = P1NodalBasis<DeformedGrid::LeafGridView, double>;
diff --git a/src/hdf5/restart-io.cc b/src/io/hdf5/restart-io.cc
similarity index 100%
rename from src/hdf5/restart-io.cc
rename to src/io/hdf5/restart-io.cc
diff --git a/src/hdf5/restart-io.hh b/src/io/hdf5/restart-io.hh
similarity index 100%
rename from src/hdf5/restart-io.hh
rename to src/io/hdf5/restart-io.hh
diff --git a/src/hdf5/restart-io_tmpl.cc b/src/io/hdf5/restart-io_tmpl.cc
similarity index 63%
rename from src/hdf5/restart-io_tmpl.cc
rename to src/io/hdf5/restart-io_tmpl.cc
index 717acace55ba64f209d707c38152406ca1d961e6..dadc25014587373b3ba8d6a0962909958a259b2c 100644
--- a/src/hdf5/restart-io_tmpl.cc
+++ b/src/io/hdf5/restart-io_tmpl.cc
@@ -1,6 +1,6 @@
-#include "../explicitvectors.hh"
+#include "../../explicitvectors.hh"
 
-#include "../program_state.hh"
+#include "../../data-structures/program_state.hh"
 
 using MyProgramState = ProgramState<Vector, ScalarVector>;
 
diff --git a/src/hdf5/restrict.hh b/src/io/hdf5/restrict.hh
similarity index 94%
rename from src/hdf5/restrict.hh
rename to src/io/hdf5/restrict.hh
index 497c5890f834a1a3e747fd265d00291ed958192a..78639f1b33a7f2f699466499d669a88ade9af227 100644
--- a/src/hdf5/restrict.hh
+++ b/src/io/hdf5/restrict.hh
@@ -5,7 +5,7 @@
 
 #include <dune/common/bitsetvector.hh>
 
-#include "../tobool.hh"
+#include "../../utils/tobool.hh"
 
 template <class Vector, class Patch>
 Vector restrictToSurface(Vector const &v1, Patch const &patch) {
diff --git a/src/hdf5/surface-writer.cc b/src/io/hdf5/surface-writer.cc
similarity index 100%
rename from src/hdf5/surface-writer.cc
rename to src/io/hdf5/surface-writer.cc
diff --git a/src/hdf5/surface-writer.hh b/src/io/hdf5/surface-writer.hh
similarity index 100%
rename from src/hdf5/surface-writer.hh
rename to src/io/hdf5/surface-writer.hh
diff --git a/src/hdf5/surface-writer_tmpl.cc b/src/io/hdf5/surface-writer_tmpl.cc
similarity index 53%
rename from src/hdf5/surface-writer_tmpl.cc
rename to src/io/hdf5/surface-writer_tmpl.cc
index 5f498b5fc05b1714153cdd36202d83b3a94890a1..9d0b3b05262ad4ceaea0904fec4f64830ba8eae1 100644
--- a/src/hdf5/surface-writer_tmpl.cc
+++ b/src/io/hdf5/surface-writer_tmpl.cc
@@ -1,7 +1,7 @@
-#include "../explicitvectors.hh"
-#include "../explicitgrid.hh"
+#include "../../explicitvectors.hh"
+#include "../../explicitgrid.hh"
 
-#include "../program_state.hh"
+#include "../../data-structures/program_state.hh"
 
 using MyProgramState = ProgramState<Vector, ScalarVector>;
 
diff --git a/src/hdf5/time-writer.cc b/src/io/hdf5/time-writer.cc
similarity index 100%
rename from src/hdf5/time-writer.cc
rename to src/io/hdf5/time-writer.cc
diff --git a/src/hdf5/time-writer.hh b/src/io/hdf5/time-writer.hh
similarity index 100%
rename from src/hdf5/time-writer.hh
rename to src/io/hdf5/time-writer.hh
diff --git a/src/hdf5/time-writer_tmpl.cc b/src/io/hdf5/time-writer_tmpl.cc
similarity index 54%
rename from src/hdf5/time-writer_tmpl.cc
rename to src/io/hdf5/time-writer_tmpl.cc
index 7b14edeb8eb05e7ebff3d22b61f8ef249c262ada..eb4486f8e2311c2fc2ee9284f4cdf6993898cf0f 100644
--- a/src/hdf5/time-writer_tmpl.cc
+++ b/src/io/hdf5/time-writer_tmpl.cc
@@ -1,6 +1,6 @@
-#include "../explicitvectors.hh"
+#include "../../explicitvectors.hh"
 
-#include "../program_state.hh"
+#include "../../data-structures/program_state.hh"
 
 using MyProgramState = ProgramState<Vector, ScalarVector>;
 
diff --git a/src/uniform-grid-writer.cc b/src/io/uniform-grid-writer.cc
similarity index 97%
rename from src/uniform-grid-writer.cc
rename to src/io/uniform-grid-writer.cc
index 804e54257bd5ab5f89758e946cc1310847938a2b..f35759acd77bdc01480b4140cfed0649b9136664 100644
--- a/src/uniform-grid-writer.cc
+++ b/src/io/uniform-grid-writer.cc
@@ -11,10 +11,13 @@
 // #include <dune/common/parametertreeparser.hh>
 
 #include "assemblers.hh"
-#include "diameter.hh"
 #include "gridselector.hh"
+
+#include "io/vtk.hh"
+
 #include "one-body-problem-data/mygrid.hh"
-#include "vtk.hh"
+
+#include "utils/diameter.hh"
 
 size_t const dims = MY_DIM;
 size_t const refinements = 5; // FIXME?
diff --git a/src/vtk.cc b/src/io/vtk.cc
similarity index 100%
rename from src/vtk.cc
rename to src/io/vtk.cc
diff --git a/src/vtk.hh b/src/io/vtk.hh
similarity index 100%
rename from src/vtk.hh
rename to src/io/vtk.hh
diff --git a/src/vtk_tmpl.cc b/src/io/vtk_tmpl.cc
similarity index 90%
rename from src/vtk_tmpl.cc
rename to src/io/vtk_tmpl.cc
index 600e7560688860256002bd92d08d320b94bc4344..d1d9f0a77484660bddac03480fc83a8aeb6b7a1c 100644
--- a/src/vtk_tmpl.cc
+++ b/src/io/vtk_tmpl.cc
@@ -2,8 +2,8 @@
 #error MY_DIM unset
 #endif
 
-#include "explicitgrid.hh"
-#include "explicitvectors.hh"
+#include "../explicitgrid.hh"
+#include "../explicitvectors.hh"
 
 #include <dune/fufem/functionspacebases/p0basis.hh>
 #include <dune/fufem/functionspacebases/p1nodalbasis.hh>
diff --git a/src/multi-body-problem-2D.cfg b/src/multi-body-problem-2D.cfg
index a61cfba2e7668aeba078d447f55ac2edf83f28a8..c98e933debf34bae0af1a0707a3eda16887a9035 100644
--- a/src/multi-body-problem-2D.cfg
+++ b/src/multi-body-problem-2D.cfg
@@ -1,6 +1,6 @@
 # -*- mode:conf -*-
 [boundary.friction]
-smallestDiameter= 2e-3  # [m]
+smallestDiameter = 1  # 2e-3 [m]
 
 [timeSteps]
 refinementTolerance = 1e-5
diff --git a/src/multi-body-problem-data/mygrids.cc b/src/multi-body-problem-data/mygrids.cc
index c7e9cd0f178c8471987b0ce8e95473be669a96fd..a754ee56ce8f229196fa25fd9e3849bc4ef1284b 100644
--- a/src/multi-body-problem-data/mygrids.cc
+++ b/src/multi-body-problem-data/mygrids.cc
@@ -6,7 +6,7 @@
 
 #include "mygrids.hh"
 #include "midpoint.hh"
-#include "../diameter.hh"
+#include "../utils/diameter.hh"
 
 #if MY_DIM == 3
 SimplexManager::SimplexManager(unsigned int shift) : shift_(shift) {}
diff --git a/src/multi-body-problem.cc b/src/multi-body-problem.cc
index ca941176059bcbe7a0280ebcb286639b829ea868..f02c02aaffff5563a256630e307ed5e709159865 100644
--- a/src/multi-body-problem.cc
+++ b/src/multi-body-problem.cc
@@ -51,51 +51,41 @@
 #include <dune/fufem/hdf5/file.hh>
 
 #include "assemblers.hh"
-#include "diameter.hh"
-#include "enumparser.hh"
-#include "enums.hh"
 #include "gridselector.hh"
-#include "hdf5-writer.hh"
-#include "hdf5/restart-io.hh"
-#include "levelcontactnetwork.hh"
-#include "matrices.hh"
-#include "program_state.hh"
-#include "multi-body-problem-data/bc.hh"
-#include "multi-body-problem-data/mybody.hh"
 
+#include "data-structures/enumparser.hh"
+#include "data-structures/enums.hh"
+#include "data-structures/levelcontactnetwork.hh"
+#include "data-structures/matrices.hh"
+#include "data-structures/program_state.hh"
+
+#include "factories/stackedblocksfactory.hh"
 
+#include "io/hdf5-writer.hh"
+#include "io/hdf5/restart-io.hh"
+#include "io/vtk.hh"
+
+#include "multi-body-problem-data/bc.hh"
+#include "multi-body-problem-data/mybody.hh"
 #include "multi-body-problem-data/mygrids.hh"
+
 //#include "spatial-solving/solverfactory.hh"
-#include "stackedblocksfactory.hh"
+
 //#include "time-stepping/adaptivetimestepper.hh"
 #include "time-stepping/rate.hh"
 #include "time-stepping/state.hh"
 #include "time-stepping/updaters.hh"
-#include "vtk.hh"
+
+#include "utils/debugutils.hh"
+#include "utils/diameter.hh"
 
 // for getcwd
 #include <unistd.h>
 
-#include <dune/grid/io/file/vtk/vtkwriter.hh>
-
 #define USE_OLD_TNNMG
 
 size_t const dims = MY_DIM;
 
-template <class GridView>
-void writeToVTK(const GridView& gridView, const std::string path, const std::string name) {
-    Dune::VTKWriter<GridView> vtkwriter(gridView);
-
-    //std::ofstream lStream( "garbage.txt" );
-    std::streambuf* lBufferOld = std::cout.rdbuf();
-    //std::cout.rdbuf(lStream.rdbuf());
-
-    vtkwriter.pwrite(name, path, path);
-
-    std::cout.rdbuf( lBufferOld );
-}
-
-
 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);
@@ -119,6 +109,10 @@ int main(int argc, char *argv[]) {
         std::cout << argv[0] << std::endl;
     }
 
+    std::ofstream out("../log.txt");
+    std::streambuf *coutbuf = std::cout.rdbuf(); //save old buffer
+    std::cout.rdbuf(out.rdbuf()); //redirect std::cout to log.txt
+
     auto const parset = getParameters(argc, argv);
 
     using Vector = Dune::BlockVector<Dune::FieldVector<double, dims>>;
@@ -344,6 +338,8 @@ int main(int argc, char *argv[]) {
       }
     }
 */
+    std::cout.rdbuf(coutbuf); //reset to standard output again
+
   } catch (Dune::Exception &e) {
     Dune::derr << "Dune reported error: " << e << std::endl;
   } catch (std::exception &e) {
diff --git a/src/one-body-problem.cc b/src/one-body-problem.cc
index a4548bdf22a8ed436e4815c8762558b614410af0..b95f4cd1fc66565592485c632074ae186f7774e5 100644
--- a/src/one-body-problem.cc
+++ b/src/one-body-problem.cc
@@ -47,26 +47,32 @@
 
 #include "assemblers.hh"
 #include "boundarycondition.hh"
-#include "diameter.hh"
-#include "enumparser.hh"
-#include "enums.hh"
 #include "gridselector.hh"
-#include "hdf5-writer.hh"
-#include "hdf5/restart-io.hh"
-#include "matrices.hh"
-#include "program_state.hh"
+
+#include "data-structures/enumparser.hh"
+#include "data-structures/enums.hh"
+#include "data-structures/matrices.hh"
+#include "data-structures/program_state.hh"
+
+#include "io/hdf5-writer.hh"
+#include "io/hdf5/restart-io.hh"
+#include "io/vtk.hh"
+
 #include "one-body-problem-data/bc.hh"
 #include "one-body-problem-data/mybody.hh"
 #include "one-body-problem-data/mygeometry.hh"
 #include "one-body-problem-data/myglobalfrictiondata.hh"
 #include "one-body-problem-data/mygrid.hh"
 #include "one-body-problem-data/weakpatch.hh"
+
 #include "spatial-solving/solverfactory.hh"
+
 #include "time-stepping/adaptivetimestepper.hh"
 #include "time-stepping/rate.hh"
 #include "time-stepping/state.hh"
 #include "time-stepping/updaters.hh"
-#include "vtk.hh"
+
+#include "utils/diameter.hh"
 
 // for getcwd
 #include <unistd.h>
diff --git a/src/spatial-solving/fixedpointiterator.cc b/src/spatial-solving/fixedpointiterator.cc
index 18931b364314eb88b2302308b847defe77ff418f..c3e90842cc708f2375e613b71e53f70eed241a8d 100644
--- a/src/spatial-solving/fixedpointiterator.cc
+++ b/src/spatial-solving/fixedpointiterator.cc
@@ -22,8 +22,8 @@
 
 #include <dune/fufem/functions/basisgridfunction.hh>
 
-#include "../enums.hh"
-#include "../enumparser.hh"
+#include "../data-structures/enums.hh"
+#include "../data-structures/enumparser.hh"
 
 #include "fixedpointiterator.hh"
 
diff --git a/src/spatial-solving/solverfactory.cc b/src/spatial-solving/solverfactory.cc
index d5f996fdbb23b0a5073c407d9b2144fee91ba014..2d83a0c13aa36ce8c2c5ea006cec0cdc7a4b7730 100644
--- a/src/spatial-solving/solverfactory.cc
+++ b/src/spatial-solving/solverfactory.cc
@@ -35,46 +35,33 @@ SolverFactory<DeformedGrid, Nonlinearity, MatrixType, VectorType>::SolverFactory
   multigridStep_->setHasObstacle(nBodyAssembler_.totalHasObstacle_);
   multigridStep_->setObstacles(nBodyAssembler_.totalObstacles_);
 
-  /*
+
   // create the transfer operators
   const int nCouplings = nBodyAssembler_.nCouplings();
   const auto grids = nBodyAssembler_.getGrids();
+
   for (size_t i=0; i<transferOperators_.size(); i++) {
-    std::vector<const Dune::BitSetVector<1>*> coarseHasObstacle(nCouplings);
-    std::vector<const Dune::BitSetVector<1>*> fineHasObstacle(nCouplings);
-
-    std::vector<const MatrixType*> mortarTransfer(nCouplings);
-    std::vector<std::array<int,2> > gridIdx(nCouplings);
-
-    for (int j=0; j<nCouplings; j++) {
-      coarseHasObstacle[j]  = nBodyAssembler_.contactCoupling_[j]->nonmortarBoundary_[i].getVertices();
-      fineHasObstacle[j]    = nBodyAssembler_.nonmortarBoundary_[j][i+1].getVertices();
-
-      mortarTransfer[j] = &nBodyAssembler_.contactCoupling_[j]->mortarLagrangeMatrix(i);
-      gridIdx[j]        = nBodyAssembler_.coupling_[j].gridIdx_;
-    }
-
-    transferOperators_[i] = new Dune::Contact::ContactMGTransfer<Vector>;
-    transferOperators_[i]->setup(grids, i, i+1,
-                                    nBodyAssembler_.localCoordSystems_[i],
-                                    nBodyAssembler_.localCoordSystems_[i+1],
-                                    coarseHasObstacle, fineHasObstacle,
-                                    mortarTransfer,
-                                    gridIdx);
-  }*/
-/*
-  grids[0], *grids[1], i,
-                          contactAssembler.contactCoupling_[0]->mortarLagrangeMatrix(),
-                          contactAssembler.localCoordSystems_,
-                          *contactAssembler.contactCoupling_[0]->nonmortarBoundary().getVertices());
-*/
-  multigridStep_->setTransferOperators(transferOperators_);
-}
+    transferOperators_[i] = std::make_shared<TransferOperator>();
+  }
 
-template <class DeformedGrid, class Nonlinearity, class MatrixType, class VectorType>
-SolverFactory<DeformedGrid, Nonlinearity, MatrixType, VectorType>::~SolverFactory() {
-  for (auto &&x : transferOperators_)
-    delete x;
+  std::vector<const Dune::BitSetVector<1>*> fineHasObstacle(nCouplings);
+  std::vector<const MatrixType*> mortarTransfer(nCouplings);
+  std::vector<std::array<int,2> > gridIdx(nCouplings);
+
+  for (int j=0; j<nCouplings; j++) {
+    fineHasObstacle[j]    = nBodyAssembler_.contactCoupling_[j]->nonmortarBoundary().getVertices();
+    mortarTransfer[j] = &nBodyAssembler_.contactCoupling_[j]->mortarLagrangeMatrix();
+    gridIdx[j]        = nBodyAssembler_.coupling_[j].gridIdx_;
+  }
+
+  TransferOperator::setupHierarchy(transferOperators_,
+                                   grids,
+                                   mortarTransfer,
+                                   nBodyAssembler_.localCoordSystems_,
+                                   fineHasObstacle,
+                                   gridIdx);
+
+  multigridStep_->setTransferOperators(transferOperators_);
 }
 
 template <class DeformedGrid, class Nonlinearity, class MatrixType, class VectorType>
diff --git a/src/spatial-solving/solverfactory.hh b/src/spatial-solving/solverfactory.hh
index ad4ecbf9b1af5be9334a5af705e077ce31b5c855..9e982c90b3367e1cb5a1e84410d2e4367145a0b0 100644
--- a/src/spatial-solving/solverfactory.hh
+++ b/src/spatial-solving/solverfactory.hh
@@ -16,7 +16,7 @@
 
 //#include <dune/contact/estimators/hierarchiccontactestimator.hh>
 #include <dune/contact/solvers/nsnewtonmgstep.hh>
-#include <dune/contact/solvers/contacttransfer.hh>
+#include <dune/contact/solvers/nsnewtoncontacttransfer.hh>
 #include <dune/contact/solvers/contactobsrestrict.hh>
 #include <dune/contact/solvers/contacttransferoperatorassembler.hh>
 #include <dune/contact/solvers/nsnewtoncontacttransfer.hh>
@@ -36,12 +36,11 @@ class SolverFactory {
 
 public:
   using Step = Dune::Contact::NonSmoothNewtonMGStep<MatrixType, VectorType>;
+  using TransferOperator = Dune::Contact::NonSmoothNewtonContactTransfer<VectorType>;
 
   SolverFactory(Dune::ParameterTree const &parset, const Dune::Contact::NBodyAssembler<DeformedGrid, VectorType>& nBodyAssembler,
                 const Dune::BitSetVector<DeformedGrid::dimension>& ignoreNodes);
 
-  ~SolverFactory();
-
   std::shared_ptr<Step> getStep();
 
   const Dune::Contact::NBodyAssembler<DeformedGrid, VectorType>& getNBodyAssembler() const {
@@ -59,6 +58,6 @@ class SolverFactory {
   ProjectedBlockGSStep<MatrixType, VectorType> postsmoother_;
   std::shared_ptr<Step> multigridStep_;
 
-  std::vector<Dune::Contact::ContactMGTransfer<VectorType>* > transferOperators_;
+  std::vector<std::shared_ptr<TransferOperator>> transferOperators_;
 };
 #endif
diff --git a/src/time-stepping/rate.hh b/src/time-stepping/rate.hh
index bef6b561b123e012960fe32eed1f72d0fcf0175f..8a3cc285779d0918bc93c25066eb4a924039e99e 100644
--- a/src/time-stepping/rate.hh
+++ b/src/time-stepping/rate.hh
@@ -3,7 +3,7 @@
 
 #include <memory>
 
-#include "../enums.hh"
+#include "../data-structures/enums.hh"
 #include "rate/rateupdater.hh"
 
 template <class Vector, class Matrix, class Function, int dimension>
diff --git a/src/time-stepping/rate/rateupdater.hh b/src/time-stepping/rate/rateupdater.hh
index 6a2c73cb1a210c090a257ed136ec6ead3bf9bfba..a9819ff5894f14673cc944451b774676426ce227 100644
--- a/src/time-stepping/rate/rateupdater.hh
+++ b/src/time-stepping/rate/rateupdater.hh
@@ -5,7 +5,7 @@
 
 #include <dune/common/bitsetvector.hh>
 
-#include "../../matrices.hh"
+#include "../../data-structures/matrices.hh"
 
 template <class Vector, class Matrix, class Function, size_t dim>
 class RateUpdater {
diff --git a/src/time-stepping/state.hh b/src/time-stepping/state.hh
index 18194643e251bedac41a1be7aa2c85987106b7a3..3c70b2db5d0b04263d194e6dc9b29a580962cfa7 100644
--- a/src/time-stepping/state.hh
+++ b/src/time-stepping/state.hh
@@ -5,7 +5,7 @@
 
 #include <dune/common/bitsetvector.hh>
 
-#include "../enums.hh"
+#include "../data-structures/enums.hh"
 #include "state/ageinglawstateupdater.hh"
 #include "state/sliplawstateupdater.hh"
 #include "state/stateupdater.hh"
diff --git a/src/time-stepping/state/ageinglawstateupdater.cc b/src/time-stepping/state/ageinglawstateupdater.cc
index 24985bc2f038a9358f37628a3b0ea8871bcdc7fe..1d7bb0ee570dd34c657684fcaac8f99a71a7b79a 100644
--- a/src/time-stepping/state/ageinglawstateupdater.cc
+++ b/src/time-stepping/state/ageinglawstateupdater.cc
@@ -1,7 +1,7 @@
 #include <cmath>
 
 #include "ageinglawstateupdater.hh"
-#include "../../tobool.hh"
+#include "../../utils/tobool.hh"
 
 template <class ScalarVector, class Vector>
 AgeingLawStateUpdater<ScalarVector, Vector>::AgeingLawStateUpdater(
diff --git a/src/time-stepping/state/sliplawstateupdater.cc b/src/time-stepping/state/sliplawstateupdater.cc
index 9a36d0a31c5b4cc5a0255dac3fca1c5cf159771c..196b02b09be73a622e9983441d09932be1ff8b7c 100644
--- a/src/time-stepping/state/sliplawstateupdater.cc
+++ b/src/time-stepping/state/sliplawstateupdater.cc
@@ -1,7 +1,7 @@
 #include <cmath>
 
 #include "sliplawstateupdater.hh"
-#include "../../tobool.hh"
+#include "../../utils/tobool.hh"
 
 template <class ScalarVector, class Vector>
 SlipLawStateUpdater<ScalarVector, Vector>::SlipLawStateUpdater(
diff --git a/src/utils/debugutils.hh b/src/utils/debugutils.hh
new file mode 100644
index 0000000000000000000000000000000000000000..75b5c8e8f60484be1d2a3abd826ae646e07e7228
--- /dev/null
+++ b/src/utils/debugutils.hh
@@ -0,0 +1,222 @@
+#ifndef DEBUG_UTILS_
+#define DEBUG_UTILS_
+
+#include <string>
+
+#include <dune/common/fvector.hh>
+#include <dune/common/fmatrix.hh>
+#include <dune/common/bitsetvector.hh>
+
+#include <dune/istl/matrix.hh>
+#include <dune/istl/bcrsmatrix.hh>
+
+#include <dune/grid/io/file/vtk/vtkwriter.hh>
+
+using namespace std;
+
+
+    template <int s>
+    void print(const Dune::BitSetVector<s>& x, std::string message){
+        std::cout << message << std::endl;
+        for (size_t i=0; i<x.size(); i++) {
+            std::cout << "block " << i << ": ";
+            for (size_t j=0; j<s; j++) {
+                 std::cout << x[i][j] << " ";
+            }
+        }
+        std::cout << std::endl << std::endl;
+    }
+
+    template <int dim, typename ctype=double>
+    void print(const Dune::FieldVector<ctype, dim>& x, std::string message){
+       std::cout << message << std::endl;
+       for (size_t i=0; i<x.size(); i++) {
+           std::cout << x[i] << " ";
+       }
+       std::cout << std::endl;
+   }
+
+    template <int dim, typename ctype=double>
+    void print(const Dune::BlockVector<Dune::FieldVector<ctype, dim>>& x, std::string message){
+       std::cout << message  << " " << "size " << x.size() << std::endl;
+       for (size_t i=0; i<x.size(); i++) {
+           print(x[i], "block " + std::to_string(i) + ":");
+       }
+       std::cout << std::endl << std::endl;
+   }
+
+   template <int dim, typename ctype=double>
+   void print(const Dune::FieldMatrix<ctype, dim, dim>& mat, std::string message){
+        std::cout << message << std::endl;
+        for (size_t i=0; i<mat.N(); i++) {
+            for (size_t j=0; j<mat.M(); j++) {
+                if (mat.exists(i,j))
+                    std::cout << mat[i][j] << " ";
+                else
+                    std::cout << "0 ";
+            }
+            std::cout << std::endl;
+        }
+        std::cout << std::endl;
+   }
+
+   template <int dim, typename ctype=double>
+   void print(const Dune::BCRSMatrix<Dune::FieldMatrix<ctype, dim, dim>>& mat, std::string message){
+       std::cout << message << " " << "size (" << mat.N() << ", " << mat.M() << ")" <<std::endl;
+       for (size_t i=0; i<mat.N(); i++) {
+           for (size_t j=0; j<mat.M(); j++) {
+               if (mat.exists(i,j))
+                    print(mat[i][j], "block (" + std::to_string(i) + ", " + std::to_string(j) + "):");
+           }
+           std::cout << std::endl;
+       }
+       std::cout << std::endl;
+   }
+
+   /*
+   template <class T=Dune::FieldMatrix<double,1,1>>
+   void print(const Dune::Matrix<T>& mat, std::string message){
+       std::cout << message << std::endl;
+       for (size_t i=0; i<mat.N(); i++) {
+           for (size_t j=0; j<mat.M(); j++) {
+                   std::cout << mat[i][j] << " ";
+           }
+           std::cout << std::endl;
+       }
+       std::cout << std::endl;
+   }
+
+   template <class T>
+   void print(const std::vector<T>& x, std::string message){
+      std::cout << message << std::endl;
+      for (size_t i=0; i<x.size(); i++) {
+          std::cout << x[i] << " ";
+      }
+      std::cout << std::endl << std::endl;
+   }
+
+   void print(const std::vector<Dune::FieldVector<double,1>>& x, std::string message){
+      std::cout << message << std::endl;
+      for (size_t i=0; i<x.size(); i++) {
+          std::cout << x[i] << " ";
+      }
+      std::cout << std::endl << std::endl;
+   }
+
+   void print(const std::set<size_t>& x, std::string message){
+      std::cout << message << std::endl;
+      std::set<size_t>::iterator dofIt = x.begin();
+      std::set<size_t>::iterator dofEndIt = x.end();
+      for (; dofIt != dofEndIt; ++dofIt) {
+          std::cout << *dofIt << " ";
+      }
+      std::cout << std::endl << std::endl;
+   }
+
+   void print(const std::set<int>& x, std::string message){
+      std::cout << message << std::endl;
+      std::set<int>::iterator dofIt = x.begin();
+      std::set<int>::iterator dofEndIt = x.end();
+      for (; dofIt != dofEndIt; ++dofIt) {
+          std::cout << *dofIt << " ";
+      }
+      std::cout << std::endl << std::endl;
+   }
+
+   void step(const double stepNumber, std::string message=""){
+      std::cout << message << " Step " << stepNumber << "!" << std::endl;
+   }*/
+
+   template <class GridView>
+   void writeToVTK(const GridView& gridView, const std::string path, const std::string name) {
+       Dune::VTKWriter<GridView> vtkwriter(gridView);
+
+       std::ofstream lStream( "garbage.txt" );
+       std::streambuf* lBufferOld = std::cout.rdbuf();
+       std::cout.rdbuf(lStream.rdbuf());
+
+       vtkwriter.pwrite(name, path, path);
+
+       std::cout.rdbuf( lBufferOld );
+   }
+
+   template <class VectorType, class DGBasisType>
+   void writeToVTK(const DGBasisType& basis, const VectorType& x, const std::string path, const std::string name) {
+       Dune::VTKWriter<typename DGBasisType::GridView> vtkwriter(basis.getGridView());
+       VectorType toBePlotted(x);
+
+       toBePlotted.resize(basis.getGridView().indexSet().size(DGBasisType::GridView::Grid::dimension));
+
+       std::ofstream lStream( "garbage.txt" );
+       std::streambuf* lBufferOld = std::cout.rdbuf();
+       std::cout.rdbuf( lStream.rdbuf() );
+
+       vtkwriter.addVertexData(toBePlotted,"data");
+       vtkwriter.pwrite(name, path, path);
+
+       std::cout.rdbuf( lBufferOld );
+   }
+/*
+   template <class BasisType, typename ctype=double>
+   void printBasisDofLocation(const BasisType& basis) {
+       typedef typename BasisType::GridView GridView;
+
+       const int dim = GridView::dimension;
+
+       std::map<int, int> indexTransformation;
+       std::map<int, Dune::FieldVector<ctype, dim>> indexCoords;
+
+
+       const GridView& gridView = basis.getGridView();
+       const int gridN = std::pow(gridView.indexSet().size(dim), 1.0/dim)-1;
+
+       typedef typename GridView::template Codim<0>::Iterator ElementIterator;
+       ElementIterator it = gridView.template begin<0>();
+       ElementIterator end = gridView.template end<0>();
+       for (; it != end; ++it) {
+           const typename BasisType::LocalFiniteElement& tFE = basis.getLocalFiniteElement(*it);
+           const auto& geometry = (*it).geometry();
+
+           for(int i=0; i<geometry.corners(); ++i) {
+               const auto& vertex = geometry.corner(i);
+               const auto& local = geometry.local(vertex);
+
+               int vertexIndex = vertex[0]*gridN;
+               for (int j=1; j<dim; ++j){
+                   vertexIndex += vertex[j]*gridN*std::pow(gridN+1, j);
+               }
+
+               const int localBasisSize = tFE.localBasis().size();
+               std::vector<Dune::FieldVector<double, 1>, std::allocator<Dune::FieldVector<double, 1> > > localBasisRep(localBasisSize);
+               tFE.localBasis().evaluateFunction(local, localBasisRep);
+
+               for(int k=0; k<localBasisSize; ++k) {
+                   if (localBasisRep[k]==1) {
+                       int dofIndex = basis.index((*it), k);
+
+                       if (indexTransformation.count(dofIndex)==0) {
+                           indexTransformation[dofIndex] = vertexIndex;
+                           indexCoords[dofIndex] = vertex;
+                       }
+
+                       break;
+                   }
+               }
+           }
+       }
+
+       std::cout << std::endl;
+       std::cout << "Coarse level: Geometric vertex indices vs. associated basis dof indices " << indexTransformation.size() << std::endl;
+       std::map<int,int>::iterator mapIt = indexTransformation.begin();
+       std::map<int,int>::iterator mapEndIt = indexTransformation.end();
+       for (; mapIt!=mapEndIt; ++mapIt) {
+           std::cout << mapIt->second << " => " << mapIt->first << " Coords: ";
+
+           const Dune::FieldVector<ctype, dim>& coords = indexCoords[mapIt->first];
+           for (size_t i=0; i<coords.size(); i++) {
+               std::cout << coords[i] << " ";
+           }
+           std::cout << std::endl;
+       }
+   } */
+#endif
diff --git a/src/diameter.hh b/src/utils/diameter.hh
similarity index 100%
rename from src/diameter.hh
rename to src/utils/diameter.hh
diff --git a/src/tobool.hh b/src/utils/tobool.hh
similarity index 100%
rename from src/tobool.hh
rename to src/utils/tobool.hh
diff --git a/todo.txt b/todo.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1a337f71f11c8961aa97e3238bb001b76ca36d85
--- /dev/null
+++ b/todo.txt
@@ -0,0 +1,29 @@
+------------
+--  ToDo  --
+------------
+
+
+1. LevelContactNetwork
+
+    
+1. build n-body system 
+    Data structure: LevelContactNetwork
+    Factories:      StackedBlocksFactory
+    
+    - extend to multilevel LevelContactNetwork
+    - write new multilevel Cantor network factory
+    
+2. initialize/set up program state
+    Data structure: ProgramState
+    
+    - test setupInitialConditions()
+
+3. assemble RSD friction
+
+4. set up TNNMG solver
+    - rate updater
+    - state updater
+    
+5. adaptive time stepper
+
+