From d2c5e7e98876227f3af52fe5e1741838d40e19c6 Mon Sep 17 00:00:00 2001
From: podlesny <podlesny@mi.fu-berlin.de>
Date: Sun, 12 May 2019 21:48:02 +0200
Subject: [PATCH] .

---
 CMakeLists.txt                                |   2 +-
 src/factories/stackedblocksfactory.cc         |   2 +-
 src/multi-body-problem-data/bc.hh             |   8 +
 src/multi-body-problem.cc                     |  42 +++-
 src/multi-body-problem.cfg                    |   8 +-
 src/spatial-solving/fixedpointiterator.cc     |  34 +++
 src/spatial-solving/solverfactory.cc          |  12 +-
 src/spatial-solving/solverfactory.hh          |  19 +-
 src/spatial-solving/tnnmg/functional.hh       |   2 +
 src/spatial-solving/tnnmg/linearization.hh    | 164 ++++++++++----
 src/spatial-solving/tnnmg/linesearchsolver.hh |   8 +
 src/spatial-solving/tnnmg/tnnmgstep.hh        | 206 ++++++++++++++++++
 src/spatial-solving/tnnmg/zerononlinearity.hh |   7 +-
 src/time-stepping/adaptivetimestepper.cc      |  23 +-
 src/time-stepping/coupledtimestepper.cc       |   3 +
 15 files changed, 475 insertions(+), 65 deletions(-)
 create mode 100644 src/spatial-solving/tnnmg/tnnmgstep.hh

diff --git a/CMakeLists.txt b/CMakeLists.txt
index fd655e11..778128f5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,7 +17,7 @@ include(DuneMacros)
 
 # start a dune project with information from dune.module
 dune_project()
-
+dune_enable_all_packages()
 find_package(HDF5 COMPONENTS C REQUIRED)
 
 add_subdirectory("src")
diff --git a/src/factories/stackedblocksfactory.cc b/src/factories/stackedblocksfactory.cc
index 0e161498..746691ab 100644
--- a/src/factories/stackedblocksfactory.cc
+++ b/src/factories/stackedblocksfactory.cc
@@ -51,7 +51,7 @@ void StackedBlocksFactory<GridType, dims>::setBodies() {
 #elif MY_DIM == 2
         origins[0] = {0,0};
         lowerWeakOrigins[0] = {0.2,0};
-        upperWeakOrigins[0] = {0.2,width};
+        upperWeakOrigins[0] = {0.6,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++) {
diff --git a/src/multi-body-problem-data/bc.hh b/src/multi-body-problem-data/bc.hh
index ebc6f744..82ccdaac 100644
--- a/src/multi-body-problem-data/bc.hh
+++ b/src/multi-body-problem-data/bc.hh
@@ -8,6 +8,14 @@ class VelocityDirichletCondition
   void evaluate(double const &relativeTime, double &y) const {
     // Assumed to vanish at time zero
     double const finalVelocity = -5e-5;
+    
+    std::cout << "VelocityDirichletCondition::evaluate()" << std::endl;
+    
+    if (relativeTime <= 0.1)
+        std::cout << "- loading phase" << std::endl;
+    else
+        std::cout << "- final velocity reached" << std::endl;
+    
     y = (relativeTime <= 0.1)
             ? finalVelocity * (1.0 - std::cos(relativeTime * M_PI / 0.1)) / 2.0
             : finalVelocity;
diff --git a/src/multi-body-problem.cc b/src/multi-body-problem.cc
index fda8fc8f..878d2cb2 100644
--- a/src/multi-body-problem.cc
+++ b/src/multi-body-problem.cc
@@ -138,13 +138,14 @@ int main(int argc, char *argv[]) {
      /* Vector def(levelContactNetwork.deformedGrids()[i]->size(dims));
       def = 1;
       deformedGridComplex.setDeformation(def, i);*/
-        auto& levelViews = levelContactNetwork.levelViews(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");
+        writeToVTK(levelContactNetwork.leafView(i), "", "body_" + std::to_string(i) + "_leaf"); */
     }
 
     // ----------------------------
@@ -226,13 +227,12 @@ int main(int argc, char *argv[]) {
                         frictionBoundaries) //, weakPatches)
                   : nullptr;*/
 
-    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "body");
+    const MyVTKWriter<MyVertexBasis, MyCellBasis> vtkWriter(cellBases, vertexBases, "/storage/mi/podlesny/software/dune/dune-tectonic/body");
 
     IterationRegister iterationCount;
 
     auto const report = [&](bool initial = false) {
-      /*
-      if (writeData) {
+      /*if (writeData) {
         dataWriter->reportSolution(programState, levelContactNetwork.globalFriction());
         if (!initial)
           dataWriter->reportIterations(programState, iterationCount);
@@ -243,14 +243,14 @@ int main(int argc, char *argv[]) {
           programState.timeStep % restartSpacing == 0) {
         restartIO->write(programState);
         restartFile->flush();
-      }
+      }*/
 
       if (parset.get<bool>("io.printProgress"))
         std::cout << "timeStep = " << std::setw(6) << programState.timeStep
                   << ", time = " << std::setw(12) << programState.relativeTime
                   << ", tau = " << std::setw(12) << programState.relativeTau
                   << std::endl;
-      */
+
       if (parset.get<bool>("io.vtk.write")) {
         std::vector<ScalarVector> stress(bodyCount);
 
@@ -276,6 +276,20 @@ int main(int argc, char *argv[]) {
     BitVector totalDirichletNodes;
     levelContactNetwork.totalNodes("dirichlet", totalDirichletNodes);
 
+    /*for (size_t i=0; i<totalDirichletNodes.size(); i++) {
+        bool val = false;
+        for (size_t d=0; d<dims; d++) {
+            val = val || totalDirichletNodes[i][d];
+        }
+
+        totalDirichletNodes[i] = val;
+        for (size_t d=0; d<dims; d++) {
+            totalDirichletNodes[i][d] = val;
+        }
+    }*/
+
+    //print(totalDirichletNodes, "totalDirichletNodes:");
+
     using Functional = Functional<Matrix&, Vector&, GlobalFriction<Matrix, Vector>&, Vector&, Vector&, field_type>;
     using NonlinearFactory = SolverFactory<Functional, BitVector>;
 
@@ -316,14 +330,28 @@ int main(int argc, char *argv[]) {
 
     auto const mustRefine = [&](Updaters &coarseUpdater,
                                 Updaters &fineUpdater) {
+
+        //return false;
+      std::cout << "Step 1" << std::endl;
+
       std::vector<ScalarVector> coarseAlpha;
       coarseUpdater.state_->extractAlpha(coarseAlpha);
 
+      std::cout << "Step 2" << std::endl;
+
       std::vector<ScalarVector> fineAlpha;
       fineUpdater.state_->extractAlpha(fineAlpha);
 
+      std::cout << "Step 3" << std::endl;
+
       ScalarVector::field_type energyNorm = 0;
       for (size_t i=0; i<stateEnergyNorms.size(); i++) {
+          std::cout << "for " << i << std::endl;
+
+          std::cout << not stateEnergyNorms[i] << std::endl;
+
+          if (coarseAlpha[i].size()==0 || fineAlpha[i].size()==0)
+              continue;
           energyNorm += stateEnergyNorms[i]->diff(fineAlpha[i], coarseAlpha[i]);
       }
       return energyNorm > refinementTolerance;
diff --git a/src/multi-body-problem.cfg b/src/multi-body-problem.cfg
index 80028801..8aa21764 100644
--- a/src/multi-body-problem.cfg
+++ b/src/multi-body-problem.cfg
@@ -3,11 +3,11 @@ gravity         = 9.81  # [m/s^2]
 
 [io]
 data.write      = false #true
-printProgress   = false
+printProgress   = true
 restarts.first  = 0
 restarts.spacing= 20
 restarts.write  = false #true
-vtk.write       = false
+vtk.write       = true
 
 [problem]
 finalTime       = 1000  # [s]
@@ -52,8 +52,8 @@ maximumIterations = 10000
 verbosity         = full
 
 [v.solver]
-maximumIterations = 100000
-verbosity         = quiet
+maximumIterations = 100
+verbosity         = full
 
 [v.fpi]
 maximumIterations = 10000
diff --git a/src/spatial-solving/fixedpointiterator.cc b/src/spatial-solving/fixedpointiterator.cc
index ccf52350..db3dd3cb 100644
--- a/src/spatial-solving/fixedpointiterator.cc
+++ b/src/spatial-solving/fixedpointiterator.cc
@@ -64,6 +64,8 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     Updaters updaters, const std::vector<Matrix>& velocityMatrices, const std::vector<Vector>& velocityRHSs,
     std::vector<Vector>& velocityIterates) {
 
+  std::cout << "FixedPointIterator::run()" << std::endl;
+
   const auto nBodies = nBodyAssembler_.nGrids();
 
   std::vector<const Matrix*> matrices_ptr(nBodies);
@@ -75,9 +77,13 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
   Matrix bilinearForm;
   nBodyAssembler_.assembleJacobian(matrices_ptr, bilinearForm);
 
+  print(bilinearForm, "bilinearForm:");
+
   Vector totalRhs;
   nBodyAssembler_.assembleRightHandSide(velocityRHSs, totalRhs);
 
+  print(totalRhs, "totalRhs:");
+
   Vector totalX;
   nBodyAssembler_.nodalToTransformed(velocityIterates, totalX);
 
@@ -96,11 +102,15 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     }
   }
 
+  std::cout << "- Problem assembled: success" << std::endl;
+
   // set up functional and TNMMG solver
   Functional J(bilinearForm, totalRhs, globalFriction_, lower, upper);
   Factory solverFactory(parset_.sub("solver.tnnmg"), J, ignoreNodes_);
   auto step = solverFactory.step();
 
+  std::cout << "- Functional and TNNMG step setup: success" << std::endl;
+
   EnergyNorm<Matrix, Vector> energyNorm(bilinearForm);
   LoopSolver<Vector> velocityProblemSolver(step.get(), velocityMaxIterations_,
                                            velocityTolerance_, &energyNorm,
@@ -119,13 +129,22 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     Vector totalVelocityIterate;
     nBodyAssembler_.nodalToTransformed(velocityIterates, totalVelocityIterate);
 
+    std::cout << "- FixedPointIteration iterate" << std::endl;
+
     // solve a velocity problem
     step->setProblem(totalVelocityIterate);
 
+    std::cout << "- Velocity problem set" << std::endl;
+
     velocityProblemSolver.preprocess();
+    std::cout << "-- Preprocessed" << std::endl;
     velocityProblemSolver.solve();
+    std::cout << "-- Solved" << std::endl;
     nBodyAssembler_.postprocess(totalVelocityIterate, velocityIterates);
 
+    //print(totalVelocityIterate, "totalVelocityIterate:");
+    //print(velocityIterates, "velocityIterates:");
+
     multigridIterations += velocityProblemSolver.getResult().iterations;
 
     std::vector<Vector> v_m;
@@ -140,15 +159,28 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     std::vector<Vector> v_rel;
     relativeVelocities(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 (size_t 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;
         }
     }
@@ -160,6 +192,8 @@ FixedPointIterator<Factory, NBodyAssembler, Updaters, ErrorNorm>::run(
     alpha = newAlpha;
   }
 
+  std::cout << "-FixedPointIteration finished!" << std::endl;
+
   if (fixedPointIteration == fixedPointMaxIterations_)
     DUNE_THROW(Dune::Exception, "FPI failed to converge");
 
diff --git a/src/spatial-solving/solverfactory.cc b/src/spatial-solving/solverfactory.cc
index 4804b788..001a3731 100644
--- a/src/spatial-solving/solverfactory.cc
+++ b/src/spatial-solving/solverfactory.cc
@@ -3,25 +3,29 @@
 #endif
 
 #include <dune/solvers/common/wrapownshare.hh>
+#include <dune/solvers/iterationsteps/blockgssteps.hh>
 
 #include "solverfactory.hh"
 
+#include "../utils/debugutils.hh"
 
 template <class Functional, class BitVector>
 SolverFactory<Functional, BitVector>::SolverFactory(
     const Dune::ParameterTree& parset,
     Functional& J,
     const BitVector& ignoreNodes) :
-      J_(Dune::Solvers::wrap_own_share<const Functional>(std::forward<Functional>(J))),
+      J_(Dune::Solvers::wrap_own_share<const Functional>(std::forward<Functional>(J)))
       //linear solver
       //preconditioner_(),
-      linearSolverStep_(),
-      energyNorm_(linearSolverStep_)
+      //linearSolverStep_(),
     {
     nonlinearSmoother_ = std::make_shared<NonlinearSmoother>(*J_, dummyIterate_, LocalSolver());
 
     // linearSolverStep_.setPreconditioner(preconditioner_);
-    linearSolver_ = std::make_shared<LinearSolver>(linearSolverStep_, parset.get<size_t>("linear.maximumIterations"), parset.get<double>("linear.tolerance"), energyNorm_, Solver::QUIET);
+    linearSolverStep_ = Dune::Solvers::BlockGSStepFactory<Matrix, Vector>::createPtr(Dune::Solvers::BlockGS::LocalSolvers::gs(0.0, 0.0));
+    energyNorm_ = std::make_shared<EnergyNorm<Matrix, Vector>>(*linearSolverStep_);
+    linearSolver_ = std::make_shared<LinearSolver>(*linearSolverStep_, parset.get<size_t>("linear.maximumIterations"), parset.get<double>("linear.tolerance"), *energyNorm_, Solver::QUIET);
+    //linearSolver_ = std::make_shared<LinearSolver>();
 
     tnnmgStep_ = std::make_shared<Step>(*J_, dummyIterate_, nonlinearSmoother_, linearSolver_, DefectProjection(), LineSearchSolver());
     tnnmgStep_->setPreSmoothingSteps(parset.get<int>("main.pre"));
diff --git a/src/spatial-solving/solverfactory.hh b/src/spatial-solving/solverfactory.hh
index ebf30349..e39eb945 100644
--- a/src/spatial-solving/solverfactory.hh
+++ b/src/spatial-solving/solverfactory.hh
@@ -8,13 +8,18 @@
 
 #include <dune/solvers/norms/energynorm.hh>
 #include <dune/solvers/solvers/loopsolver.hh>
-#include <dune/solvers/iterationsteps/cgstep.hh>
+#include <dune/solvers/iterationsteps/lineariterationstep.hh>
+//#include <dune/solvers/iterationsteps/cgstep.hh>
+//#include <dune/solvers/iterationsteps/amgstep.hh>
+//#include <dune/solvers/solvers/umfpacksolver.hh>
 
 #include <dune/tnnmg/iterationsteps/tnnmgstep.hh>
 #include <dune/tnnmg/iterationsteps/nonlineargsstep.hh>
 #include <dune/tnnmg/projections/obstacledefectprojection.hh>
+#include <dune/tnnmg/linearsolvers/fastamgsolver.hh>
 
 //#include "tnnmg/functional.hh"
+//#include "tnnmg/tnnmgstep.hh"
 #include "tnnmg/linearization.hh"
 #include "tnnmg/linesearchsolver.hh"
 #include "tnnmg/localbisectionsolver.hh"
@@ -30,10 +35,14 @@ class SolverFactory {
     using LocalSolver = LocalBisectionSolver;
     using NonlinearSmoother = Dune::TNNMG::NonlinearGSStep<Functional, LocalSolver>;
 
-    using Linearization = typename Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<Functional, BitVector>;
+    using Linearization = Linearization<Functional, BitVector>;
     //using Preconditioner = Dune::Solvers::Preconditioner; //TODO Platzhalter für PatchPreconditioner
-    using LinearSolverStep = typename Dune::Solvers::CGStep<Matrix, Vector, BitVector>;
+    //using LinearSolverStep = typename Dune::Solvers::CGStep<Matrix, Vector, BitVector>;
+    //using LinearSolverStep = typename Dune::Solvers::AMGStep<Matrix, Vector>;
+    using LinearSolverStep = LinearIterationStep<Matrix, Vector>;
     using LinearSolver = typename Dune::Solvers::LoopSolver<Vector, BitVector>;
+    //using LinearSolver = typename Dune::Solvers::UMFPackSolver<Matrix, Vector>;
+    //using LinearSolver = typename Dune::TNNMG::FastAMGSolver<Matrix, Vector>;
 
     using DefectProjection = typename Dune::TNNMG::ObstacleDefectProjection;
 
@@ -57,8 +66,8 @@ class SolverFactory {
 
   // linear solver
   //Preconditioner preconditioner_;
-  LinearSolverStep linearSolverStep_;
-  EnergyNorm<Matrix, Vector> energyNorm_;
+  std::shared_ptr<LinearSolverStep> linearSolverStep_;
+  std::shared_ptr<EnergyNorm<Matrix, Vector>> energyNorm_;
   std::shared_ptr<LinearSolver> linearSolver_;
 
   // TNNMGStep
diff --git a/src/spatial-solving/tnnmg/functional.hh b/src/spatial-solving/tnnmg/functional.hh
index 8b41cd3e..76135944 100644
--- a/src/spatial-solving/tnnmg/functional.hh
+++ b/src/spatial-solving/tnnmg/functional.hh
@@ -97,6 +97,8 @@ class DirectionalRestriction :
   {
     phi_.directionalDomain(origin_, direction_, domain_);
 
+    std::cout << domain_[0] << " " << domain_[1] << "Phi domain:" << std::endl;
+    std::cout << this->defectLower_ << " " << this->defectUpper_ << "defect obstacles:" << std::endl;
     if (domain_[0] < this->defectLower_) {
        domain_[0] = this->defectLower_;
     }
diff --git a/src/spatial-solving/tnnmg/linearization.hh b/src/spatial-solving/tnnmg/linearization.hh
index 4e7ce132..73b90b42 100644
--- a/src/spatial-solving/tnnmg/linearization.hh
+++ b/src/spatial-solving/tnnmg/linearization.hh
@@ -1,52 +1,111 @@
 #ifndef DUNE_TECTONIC_LINEARIZATION_HH
 #define DUNE_TECTONIC_LINEARIZATION_HH
 
-#include <dune/tnnmg/functionals/bcqfconstrainedlinearization.hh>
-#include <dune/istl/matrixindexset.hh>
+#include <cstddef>
 
-//#include <>
+#include <dune/common/typetraits.hh>
+#include <dune/common/hybridutilities.hh>
 
-/*#include <dune/common/bitsetvector.hh>
-#include <dune/common/parametertree.hh>
-#include <dune/common/fmatrixev.hh>
+#include <dune/matrix-vector/algorithm.hh>
 
-#include <dune/fufem/arithmetic.hh>
-#include <dune/solvers/common/interval.hh>
-#include <dune/solvers/computeenergy.hh>
-#include <dune/tnnmg/problem-classes/bisection.hh>
-#include <dune/tnnmg/problem-classes/blocknonlineargsproblem.hh>
+#include <dune/istl/matrixindexset.hh>
 
-#include <dune/tectonic/globalfriction.hh>
-#include <dune/tectonic/minimisation.hh>
-#include <dune/tectonic/mydirectionalconvexfunction.hh>
-#include <dune/tectonic/quadraticenergy.hh>*/
+//#include <dune/tnnmg/functionals/bcqfconstrainedlinearization.hh>
 
+#include "../../utils/debugutils.hh"
 
+/**
+ * derived from Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<F,BV>
+ */
 template<class F, class BV>
-class Linearization : public Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<F,BV> {
+class Linearization {
+public:
+  using Matrix = typename F::Matrix;
+  using Vector = typename F::Vector;
+  using BitVector = BV;
+  using ConstrainedMatrix = Matrix;
+  using ConstrainedVector = Vector;
+  using ConstrainedBitVector = BitVector;
+
 private:
-  using Base = Dune::TNNMG::BoxConstrainedQuadraticFunctionalConstrainedLinearization<F,BV>;
-  using Vector = typename Base::Vector;
-  using BitVector = typename Base::BitVector;
+  using This = Linearization<F,BV>;
 
   template <class T>
-  void determineRegularityTruncation(const Vector& x, BitVector&& truncationFlags, const T& truncationTolerance)
+  void determineRegularityTruncation(const Vector& x, BitVector& truncationFlags, const T& truncationTolerance)
   {
     for (size_t i = 0; i < x.size(); ++i) {
-      if (this->f_.phi().regularity(i, x[i]) > truncationTolerance) {
+      if (f_.phi().regularity(i, x[i]) > truncationTolerance) {
         truncationFlags[i] = true;
       }
     }
   }
 
+  template<class NV, class NBV, class T>
+  static void determineTruncation(const NV& x, const NV& lower, const NV& upper, NBV&& truncationFlags, const T& truncationTolerance)
+  {
+    namespace H = Dune::Hybrid;
+    H::ifElse(Dune::IsNumber<NV>(), [&](auto id){
+      if ((id(x) <= id(lower)+truncationTolerance) || (id(x) >= id(upper) - truncationTolerance))
+        id(truncationFlags) = true;
+    }, [&](auto id){
+      H::forEach(H::integralRange(H::size(id(x))), [&](auto&& i) {
+        This::determineTruncation(x[i], lower[i], upper[i], truncationFlags[i], truncationTolerance);
+      });
+    });
+  }
+
+  template<class NV, class NBV>
+  static void truncateVector(NV& x, const NBV& truncationFlags)
+  {
+    namespace H = Dune::Hybrid;
+    H::ifElse(Dune::IsNumber<NV>(), [&](auto id){
+      if (id(truncationFlags))
+        id(x) = 0;
+    }, [&](auto id){
+      H::forEach(H::integralRange(H::size(id(x))), [&](auto&& i) {
+        This::truncateVector(x[i], truncationFlags[i]);
+      });
+    });
+  }
+
+  template<class NM, class RBV, class CBV>
+  static void truncateMatrix(NM& A, const RBV& rowTruncationFlags, const CBV& colTruncationFlags)
+  {
+    namespace H = Dune::Hybrid;
+    using namespace Dune::MatrixVector;
+    H::ifElse(Dune::IsNumber<NM>(), [&](auto id){
+      if(id(rowTruncationFlags) or id(colTruncationFlags))
+        A = 0;
+    }, [&](auto id){
+      H::forEach(H::integralRange(H::size(id(rowTruncationFlags))), [&](auto&& i) {
+        auto&& Ai = A[i];
+        sparseRangeFor(Ai, [&](auto&& Aij, auto&& j) {
+            This::truncateMatrix(Aij, rowTruncationFlags[i], colTruncationFlags[j]);
+        });
+      });
+    });
+  }
+
 public:
   Linearization(const F& f, const BitVector& ignore) :
-    Base(f, ignore)
+      f_(f),
+      ignore_(ignore),
+      truncationTolerance_(1e-10)
   {}
 
   void bind(const Vector& x) {
-      auto&& A = this->f_.quadraticPart();
-      auto&& phi = this->f_.phi();
+      //std::cout << "Linearization::bind()" << std::endl;
+
+      auto&& A = f_.quadraticPart();
+      auto&& phi = f_.phi();
+
+      // determine which components to truncate
+      // ----------------
+      truncationFlags_ = ignore_;
+
+      determineTruncation(x, f_.lowerObstacle(), f_.upperObstacle(), truncationFlags_, truncationTolerance_); // obstacle truncation
+      determineRegularityTruncation(x, truncationFlags_, truncationTolerance_); // truncation due to regularity deficit of nonlinearity
+
 
       // compute hessian
       // ---------------
@@ -57,43 +116,70 @@ class Linearization : public Dune::TNNMG::BoxConstrainedQuadraticFunctionalConst
       phi.addHessianIndices(indices);
 
       // construct matrix from pattern and initialize it
-      indices.exportIdx(this->hessian_);
-      this->hessian_ = 0.0;
+      indices.exportIdx(hessian_);
+      hessian_ = 0.0;
 
       // quadratic part
       for (size_t i = 0; i < A.N(); ++i) {
         auto const end = std::end(A[i]);
         for (auto it = std::begin(A[i]); it != end; ++it)
-          this->hessian_[i][it.index()] += *it;
+          hessian_[i][it.index()] += *it;
       }
 
+      //print(hessian_, "Hessian:");
+      //std::cout << "--------" << (double) hessian_[21][21] << "------------" << std::endl;
+
       // nonlinearity part
-      phi.addHessian(x, this->hessian_);
+      phi.addHessian(x, hessian_);
 
       // compute gradient
       // ----------------
 
       // quadratic part
-      this->negativeGradient_ = derivative(this->f_)(x); // A*x - b
+      negativeGradient_ = derivative(f_)(x); // A*x - b
 
       // nonlinearity part
-      phi.addGradient(x, this->negativeGradient_);
+      phi.addGradient(x, negativeGradient_);
 
       // -grad is needed for Newton step
-      this->negativeGradient_ *= -1;
+      negativeGradient_ *= -1;
 
 
-      // determine which components to truncate
-      // ----------------
-      this->truncationFlags_ = this->ignore_;
-
-      determineTruncation(x, this->f_.lowerObstacle(), this->f_.upperObstacle(), this->truncationFlags_, this->truncationTolerance_); // obstacle truncation
-      determineRegularityTruncation(x, this->truncationFlags_, this->truncationTolerance_); // truncation due to regularity deficit of nonlinearity
 
       // truncate gradient and hessian
-      truncateVector(this->negativeGradient_, this->truncationFlags_);
-      truncateMatrix(this->hessian_, this->truncationFlags_, this->truncationFlags_);
+      truncateVector(negativeGradient_, truncationFlags_);
+      truncateMatrix(hessian_, truncationFlags_, truncationFlags_);
+
+      //print(hessian_, "Hessian:");
+  }
+
+
+  void extendCorrection(ConstrainedVector& cv, Vector& v) const {
+    v = cv;
+    truncateVector(v, truncationFlags_);
+  }
+
+  const BitVector& truncated() const {
+    return truncationFlags_;
+  }
+
+  const auto& negativeGradient() const {
+    return negativeGradient_;
+  }
+
+  const auto& hessian() const {
+    return hessian_;
   }
+
+private:
+  const F& f_;
+  const BitVector& ignore_;
+
+  double truncationTolerance_;
+
+  Vector negativeGradient_;
+  Matrix hessian_;
+  BitVector truncationFlags_;
 };
 
 #endif
diff --git a/src/spatial-solving/tnnmg/linesearchsolver.hh b/src/spatial-solving/tnnmg/linesearchsolver.hh
index 834740a2..352005b2 100644
--- a/src/spatial-solving/tnnmg/linesearchsolver.hh
+++ b/src/spatial-solving/tnnmg/linesearchsolver.hh
@@ -5,6 +5,7 @@
 
 #include <dune/tnnmg/problem-classes/bisection.hh>
 
+#include "../../utils/almostequal.hh"
 
 /**
  * \brief A local solver for scalar quadratic obstacle problems with nonlinearity
@@ -22,10 +23,17 @@ class LineSearchSolver
     if (D[1] > 0) // NOTE: Numerical instability can actually get us here
         return;
 
+    if (almost_equal(f.domain()[0], f.domain()[1], 2)) {
+        x = f.domain()[0];
+        return;
+    }
+
     if (not ignore) {
         int bisectionsteps = 0;
         const Bisection globalBisection;
+        std::cout << "LineSearchSolver: starting bisection" << std::endl;
         x = globalBisection.minimize(f, 1.0, 0.0, bisectionsteps);
+        std::cout << "LineSearchSolver: bisection terminated" << std::endl;
         // x = globalBisection.minimize(f, vNorm, 0.0, bisectionsteps) / vNorm;  // used rescaled v in f?
     }
   }
diff --git a/src/spatial-solving/tnnmg/tnnmgstep.hh b/src/spatial-solving/tnnmg/tnnmgstep.hh
new file mode 100644
index 00000000..40f77d06
--- /dev/null
+++ b/src/spatial-solving/tnnmg/tnnmgstep.hh
@@ -0,0 +1,206 @@
+#ifndef DUNE_TNNMG_ITERATIONSTEPS_TRUNCATED_NONSMOOTH_NEWTON_MULTIGRID_HH
+#define DUNE_TNNMG_ITERATIONSTEPS_TRUNCATED_NONSMOOTH_NEWTON_MULTIGRID_HH
+
+#include <string>
+#include <sstream>
+#include <vector>
+#include <iomanip>
+
+#include <dune/common/timer.hh>
+
+#include <dune/solvers/common/resize.hh>
+#include "dune/solvers/iterationsteps/iterationstep.hh"
+#include "dune/solvers/iterationsteps/lineariterationstep.hh"
+#include <dune/solvers/solvers/iterativesolver.hh>
+#include <dune/solvers/solvers/linearsolver.hh>
+
+#include <dune/tectonic/../../src/utils/debugutils.hh>
+
+
+/**
+ * \brief One iteration of the TNNMG method
+ *
+ * \tparam F Functional to minimize
+ * \tparam BV Bit-vector type for marking ignored components
+ */
+template<class F, class BV, class Linearization,
+                                  class DefectProjection,
+                                  class LineSearchSolver>
+class TNNMGStep :
+  public Dune::Solvers::IterationStep<typename F::Vector, BV>
+{
+  using Base = Dune::Solvers::IterationStep<typename F::Vector, BV>;
+
+public:
+
+  using Vector = typename F::Vector;
+  using ConstrainedVector = typename Linearization::ConstrainedVector;
+  using ConstrainedMatrix = typename Linearization::ConstrainedMatrix;
+  using BitVector = typename Base::BitVector;
+  using ConstrainedBitVector = typename Linearization::ConstrainedBitVector;
+  using Functional = F;
+  using IterativeSolver = Dune::Solvers::IterativeSolver< ConstrainedVector, Dune::Solvers::DefaultBitVector_t<ConstrainedVector> >;
+  using LinearSolver = Dune::Solvers::LinearSolver< ConstrainedMatrix,  ConstrainedVector >;
+
+  /** \brief Constructor with an iterative solver object for the linear correction
+   * \param iterativeSolver This is a callback used to solve the constrained linearized system
+   * \param projection This is a callback used to compute a projection into a defect-admissible set
+   * \param lineSolver This is a callback used to minimize a directional restriction of the functional
+   *        for computing a damping parameter
+   */
+  TNNMGStep(const Functional& f,
+            Vector& x,
+            std::shared_ptr<IterationStep<Vector,BitVector> > nonlinearSmoother,
+            std::shared_ptr<IterativeSolver> iterativeSolver,
+            const DefectProjection& projection,
+            const LineSearchSolver& lineSolver)
+  : Base(x),
+    f_(&f),
+    nonlinearSmoother_(nonlinearSmoother),
+    projection_(projection),
+    lineSolver_(lineSolver)
+  {}
+
+  /** \brief Constructor with a linear solver object for the linear correction
+   * \param linearSolver This is a callback used to solve the constrained linearized system
+   * \param projection This is a callback used to compute a projection into a defect-admissible set
+   * \param lineSolver This is a callback used to minimize a directional restriction of the functional
+   *        for computing a damping parameter
+   */
+  TNNMGStep(const Functional& f,
+            Vector& x,
+            std::shared_ptr<IterationStep<Vector,BitVector> > nonlinearSmoother,
+            std::shared_ptr<LinearSolver> linearSolver,
+            const DefectProjection& projection,
+            const LineSearchSolver& lineSolver)
+  : Base(x),
+    f_(&f),
+    nonlinearSmoother_(nonlinearSmoother),
+    projection_(projection),
+    lineSolver_(lineSolver)
+  {}
+
+  /** \brief Constructor with a LinearIterationStep object for the linear correction
+   * \param linearIterationStep This is a callback used to solve the constrained linearized system
+   * \param projection This is a callback used to compute a projection into a defect-admissible set
+   * \param lineSolver This is a callback used to minimize a directional restriction of the functional
+   *        for computing a damping parameter
+   */
+  TNNMGStep(const Functional& f,
+            Vector& x,
+            std::shared_ptr<Dune::Solvers::IterationStep<Vector,BitVector> > nonlinearSmoother,
+            std::shared_ptr<Dune::Solvers::LinearIterationStep<ConstrainedMatrix,ConstrainedVector> > linearIterationStep,
+            unsigned int noOfLinearIterationSteps,
+            const DefectProjection& projection,
+            const LineSearchSolver& lineSolver)
+  : Base(x),
+    f_(&f),
+    nonlinearSmoother_(nonlinearSmoother),
+    projection_(projection),
+    lineSolver_(lineSolver)
+  {}
+
+  using Base::getIterate;
+
+  void preprocess() override
+  {
+    nonlinearSmoother_->setIgnore(this->ignore());
+    nonlinearSmoother_->preprocess();
+  }
+
+  void setPreSmoothingSteps(std::size_t i)
+  {
+    preSmoothingSteps_ = i;
+  }
+
+  /**
+   * \brief Do one TNNMG step
+   */
+  void iterate() override
+  {
+
+    const auto& f = *f_;
+    const auto& ignore = (*this->ignoreNodes_);
+    auto& x = *getIterate();
+
+    print(f.quadraticPart(), "f.quadraticPart():");
+
+    print(f.linearPart(), "f.linearPart():");
+
+    // Nonlinear presmoothing
+    for (std::size_t i=0; i<preSmoothingSteps_; ++i)
+        nonlinearSmoother_->iterate();
+
+    print(x, "TNNMG iterate after smoothing:");
+
+    // Compute constraint/truncated linearization
+    if (not linearization_)
+      linearization_ = std::make_shared<Linearization>(f, ignore);
+
+    linearization_->bind(x);
+
+    auto&& A = linearization_->hessian();
+    auto&& r = linearization_->negativeGradient();
+
+    print(A, "TNNMG Linear problem matrix:");
+    print(r, "TNNMG Linear problem rhs:");
+
+    // Compute inexact solution of the linearized problem
+    Solvers::resizeInitializeZero(correction_, x);
+    Solvers::resizeInitializeZero(constrainedCorrection_, r);
+
+    linearCorrection_(A, constrainedCorrection_, r);
+
+    print(constrainedCorrection_, "contrained correction:");
+
+    linearization_->extendCorrection(constrainedCorrection_, correction_);
+
+    print(correction_, "correction:");
+
+    // Project onto admissible set
+    projection_(f, x, correction_);
+
+    // Line search
+    auto fv = directionalRestriction(f, x, correction_);
+    dampingFactor_ = 0;
+    lineSolver_(dampingFactor_, fv, false);
+    if (std::isnan(dampingFactor_))
+      dampingFactor_ = 0;
+    correction_ *= dampingFactor_;
+
+    x += correction_;
+  }
+
+  /**
+   * \brief Export the last computed damping factor
+   */
+  double lastDampingFactor() const
+  {
+    return dampingFactor_;
+  }
+
+  /**
+   * \brief Export the last used linearization
+   */
+  const Linearization& linearization() const
+  {
+    return *linearization_;
+  }
+
+private:
+
+  const Functional* f_;
+
+  std::shared_ptr<IterationStep<Vector,BitVector> > nonlinearSmoother_;
+  std::size_t preSmoothingSteps_ = 1;
+
+  std::shared_ptr<Linearization> linearization_;
+
+  typename Linearization::ConstrainedVector constrainedCorrection_;
+  Vector correction_;
+  DefectProjection projection_;
+  LineSearchSolver lineSolver_;
+  double dampingFactor_;
+};
+
+#endif
diff --git a/src/spatial-solving/tnnmg/zerononlinearity.hh b/src/spatial-solving/tnnmg/zerononlinearity.hh
index 4885d72a..2ea3e459 100644
--- a/src/spatial-solving/tnnmg/zerononlinearity.hh
+++ b/src/spatial-solving/tnnmg/zerononlinearity.hh
@@ -34,7 +34,7 @@ class ZeroNonlinearity
     void addHessianIndices(IndexSet& indices) const {}
 
     /** \brief Returns the interval \f$ [0,0]\f$ */
-    void subDiff(int i, double x, Dune::Solvers::Interval<double>& D, int j) const
+    void subDiff(int i, double x, Dune::Solvers::Interval<double>& D) const
     {
         D[0] = 0.0;
         D[1] = 0.0;
@@ -48,12 +48,13 @@ class ZeroNonlinearity
     }
 
     /** \brief Returns 0 */
-    double regularity(int i, double x, int j) const
+    template <class VectorType>
+    double regularity(int i, const VectorType& x) const
     {
         return 0.0;
     }
 
-    void domain(int i, Dune::Solvers::Interval<double>& dom, int j) const
+    void domain(int i, Dune::Solvers::Interval<double>& dom) const
     {
         dom[0] = -std::numeric_limits<double>::max();
         dom[1] = std::numeric_limits<double>::max();
diff --git a/src/time-stepping/adaptivetimestepper.cc b/src/time-stepping/adaptivetimestepper.cc
index c4a7371e..74396864 100644
--- a/src/time-stepping/adaptivetimestepper.cc
+++ b/src/time-stepping/adaptivetimestepper.cc
@@ -59,16 +59,24 @@ IterationRegister AdaptiveTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNo
     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;
+
   if (R1_.updaters == Updaters())
     R1_ = step(current_, relativeTime_, relativeTau_);
 
+  std::cout << "AdaptiveTimeStepper Step 1" << std::endl;
+
+
   bool didCoarsen = false;
   iterationRegister_.reset();
   UpdatersWithCount R2;
   UpdatersWithCount C;
   while (relativeTime_ + relativeTau_ <= 1.0) {
     R2 = step(R1_.updaters, relativeTime_ + relativeTau_, relativeTau_);
+    std::cout << "AdaptiveTimeStepper R2 computed!" << std::endl << std::endl;
     C = step(current_, relativeTime_, 2 * relativeTau_);
+    std::cout << "AdaptiveTimeStepper C computed!" << std::endl << std::endl;
     if (mustRefine_(R2.updaters, C.updaters))
       break;
 
@@ -76,15 +84,21 @@ IterationRegister AdaptiveTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNo
     relativeTau_ *= 2;
     R1_ = C;
   }
+
+  std::cout << "AdaptiveTimeStepper Step 1" << std::endl;
   UpdatersWithCount F1;
   UpdatersWithCount F2;
   if (!didCoarsen) {
     while (true) {
       F1 = step(current_, relativeTime_, relativeTau_ / 2.0);
+      std::cout << "AdaptiveTimeStepper F1 computed!" << std::endl << std::endl;
       F2 = step(F1.updaters, relativeTime_ + relativeTau_ / 2.0,
                 relativeTau_ / 2.0);
-      if (!mustRefine_(F2.updaters, R1_.updaters))
+      std::cout << "AdaptiveTimeStepper F2 computed!" << std::endl << std::endl;
+      if (!mustRefine_(F2.updaters, R1_.updaters)) {
+        std::cout << "Sufficiently refined!" << std::endl;
         break;
+      }
 
       relativeTau_ /= 2.0;
       R1_ = F1;
@@ -92,11 +106,18 @@ IterationRegister AdaptiveTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNo
     }
   }
 
+  std::cout << "AdaptiveTimeStepper::advance() ...";
+
   iterationRegister_.registerFinalCount(R1_.count);
   relativeTime_ += relativeTau_;
   current_ = R1_.updaters;
+
+  //UpdatersWithCount emptyR1;
+  //R1_ = emptyR1;
   R1_ = R2;
 
+  std::cout << " done" << std::endl;
+
   return iterationRegister_;
 }
 
diff --git a/src/time-stepping/coupledtimestepper.cc b/src/time-stepping/coupledtimestepper.cc
index a102bb04..c5fbb89e 100644
--- a/src/time-stepping/coupledtimestepper.cc
+++ b/src/time-stepping/coupledtimestepper.cc
@@ -28,6 +28,9 @@ template <class Factory, class NBodyAssembler, class Updaters, class ErrorNorm>
 FixedPointIterationCounter
 CoupledTimeStepper<Factory, NBodyAssembler, Updaters, ErrorNorm>::step(double relativeTime,
                                                        double relativeTau) {
+
+  std::cout << "CoupledTimeStepper::step()" << std::endl;
+
   updaters_.state_->nextTimeStep();
   updaters_.rate_->nextTimeStep();
 
-- 
GitLab