b2static_nonlinear_utile.H

//------------------------------------------------------------------------
// b2static_nonlinear_utile.H --
//
//
// written by Mathias Doreille
//            Neda Ebrahimi Pour <neda.ebrahimipour@dlr.de>
//            Thomas Blome <thomas.blome@dlr.de>
//
// (c) 2004-2012,2016 SMR Engineering & Development SA
//                    2502 Bienne, Switzerland
//
// (c) 2023 Deutsches Zentrum für Luft- und Raumfahrt (DLR) e.V.
//          Linder Höhe, 51147 Köln
//
// All Rights Reserved.  Proprietary source code.  The contents of
// this file may not be disclosed to third parties, copied or
// duplicated in any form, in whole or in part, without the prior
// written permission of SMR.
//------------------------------------------------------------------------
 
// #define CHECK_RESIDUE_DERIVATIVE 1
 
#ifndef B2STATIC_NONLINEAR_UTILE_H_
#define B2STATIC_NONLINEAR_UTILE_H_
 
#include <cmath>
#include <limits>
 
#include "b2ppconfig.h"
#include "model/b2boundary_condition.H"
#include "model/b2case.H"
#include "model/b2domain.H"
#include "model/b2element.H"
#include "model/b2model.H"
#include "model/b2solution.H"
#include "model/b2solver.H"
#include "solvers/b2constraint_util.H"
#include "solvers/b2solver_utils.H"
#include "utils/b2linear_algebra.H"
#include "utils/b2logging.H"
#include "utils/b2numerical_differentiation.H"
#include "utils/b2object.H"
#include "utils/b2util.H"
 
namespace b2000 { namespace solver {
 
template <typename T, typename MATRIX_FORMAT>
class StaticResidueFunction;
 
template <typename T, typename MATRIX_FORMAT>
class IncrementConstraintStrategy;
 
template <typename T, typename MATRIX_FORMAT>
class CorrectionStrategy;
 
template <typename T>
class CorrectionTerminationTest;
class LineSearch;
 
template <typename T>
class StaticResidueFunctionForTerminationTest : public Object {
public:
    virtual double get_energy_normalisation1() {
        UnimplementedError() << THROW;
        return 0;
    }
 
    virtual void mrbc_get_nonlinear_value(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, double time,
          EquilibriumSolution equilibrium_solution, b2linalg::Vector<T, b2linalg::Vdense_ref> value,
          b2linalg::Matrix<T, b2linalg::Mcompressed_col>& d_value_d_dof_trans,
          b2linalg::Vector<T, b2linalg::Vdense_ref> d_value_d_time, SolverHints* solver_hints) = 0;
 
    virtual size_t get_number_of_dof() = 0;
 
    virtual size_t get_number_of_non_lagrange_dof() = 0;
};
 
template <typename T>
class StaticResidueFunctionGeneric : public StaticResidueFunctionForTerminationTest<T> {
public:
    virtual void init(Model& model, const AttributeList& attribute) = 0;
 
    virtual Model& get_model() = 0;
 
    virtual T get_residue_normalization() {
        UnimplementedError() << THROW;
        return 0;
    }
 
    virtual double get_energy() = 0;
 
    virtual double get_energy_increment(const b2linalg::Vector<T>& dof_increment) = 0;
 
    virtual const b2linalg::Vector<T, b2linalg::Vdense_constref> get_nbc_sol() = 0;
 
    virtual const b2linalg::Vector<T, b2linalg::Vdense_constref> get_residuum_sol(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, const double time) = 0;
 
    virtual void get_solution(
          const b2linalg::Vector<T, b2linalg::Vdense_constref> dof_red, const double time,
          EquilibriumSolution equilibrium_solution,
          b2linalg::Vector<T, b2linalg::Vdense_ref> dof) = 0;
};
 
template <typename T, typename MATRIX_FORMAT>
class StaticResidueFunction : public StaticResidueFunctionGeneric<T> {
public:
    StaticResidueFunction()
        : logger(logging::get_logger("solver.static_nonlinear.ResidueFunction")) {}
 
    virtual const b2linalg::Vector<T, b2linalg::Vindex1_constref> get_matrix_diagonal(
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& d_residue_d_dof) = 0;
 
    virtual void get_residue(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, const double time,
          const double delta_time, EquilibriumSolution& equilibrium_solution,
          b2linalg::Vector<T, b2linalg::Vdense_ref> residue,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& d_residue_d_dof,
          b2linalg::Vector<T, b2linalg::Vdense_ref> d_residue_d_time, SolverHints* solver_hints,
          CorrectionTerminationTest<T>* termination_test = 0,
          GradientContainer* gradient_container = nullptr) = 0;
 
    void get_d_dof_d_path(
          const b2linalg::Vector<T, b2linalg::Vdense_constref> dof, const double time,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense_ref> q,
          b2linalg::Vector<T, b2linalg::Vdense_ref> arc,
          b2linalg::Matrix<T, b2linalg::Mrectangle_ref> d_dof_and_time_d_path,
          int approximation_order = 1, double h_scale = 1.0) {
        const size_t rs = K.size1();
        d_dof_and_time_d_path.set_zero();
        if (d_dof_and_time_d_path.size2() > 0) {
            EquilibriumSolution est(true);
            get_residue(
                  dof, time, 0, est, b2linalg::Vector<T, b2linalg::Vdense_ref>::null, K, q, 0);
            d_dof_and_time_d_path(rs - 1, 0) = 1;
            StaticResidueFunction<T, MATRIX_FORMAT>::logger
                  << logging::debug
                  << "get_d_dof_d_path: Factorize tangent "
                     "stiffness matrix ("
                  << K.size1() << " rows and columns)." << logging::LOGFLUSH;
            d_dof_and_time_d_path[0] = update_extension_and_inverse(
                                             K, q, arc(b2linalg::Interval(0, rs - 1)), arc[rs - 1])
                                       * d_dof_and_time_d_path[0];
            d_dof_and_time_d_path[0] *= 1.0 / (d_dof_and_time_d_path[0] * d_dof_and_time_d_path[0]);
            arc = d_dof_and_time_d_path[0];
            h_scale /= d_dof_and_time_d_path[0](b2linalg::Interval(0, rs - 1)).norm2();
        }
 
        /* python script to generate the J values
         * TODO replace it by the Leibnitz Formula for the n’th Derivative of a Product
         n = 20
         r = ''
         a = {(1, 2): 1}
         for i in range(3, n):
         b = {}
         def add_to_b(k1, k2, v):
         if k1 > k2:
         k1, k2 = k2, k1
         b[(k1, k2)] = b.setdefault((k1, k2), 0) + v
         for k, v in a.items():
         add_to_b(k[0] + 1, k[1], v)
         add_to_b(k[0], k[1] + 1, v)
         a = b
         r += repr([[-v, k[0], k[1]] for k, v in a.items() if k != (1, i)]) + ',\n'
         print """
         static const int J[%d][%d][3] = {
         {},""" % (n - 2, len(a))
         print r.replace('[', '{').replace(']', '}') + '};'
         */
        static const int J[18][10][3] = {
              {},
              {{-1, 2, 2}},
              {{-3, 2, 3}},
              {{-3, 3, 3}, {-4, 2, 4}},
              {{-10, 3, 4}, {-5, 2, 5}},
              {{-6, 2, 6}, {-10, 4, 4}, {-15, 3, 5}},
              {{-7, 2, 7}, {-35, 4, 5}, {-21, 3, 6}},
              {{-28, 3, 7}, {-56, 4, 6}, {-35, 5, 5}, {-8, 2, 8}},
              {{-36, 3, 8}, {-126, 5, 6}, {-84, 4, 7}, {-9, 2, 9}},
              {{-126, 6, 6}, {-210, 5, 7}, {-10, 2, 10}, {-120, 4, 8}, {-45, 3, 9}},
              {{-165, 4, 9}, {-55, 3, 10}, {-11, 2, 11}, {-462, 6, 7}, {-330, 5, 8}},
              {{-495, 5, 9}, {-792, 6, 8}, {-220, 4, 10}, {-66, 3, 11}, {-462, 7, 7}, {-12, 2, 12}},
              {{-1287, 6, 9},
               {-715, 5, 10},
               {-13, 2, 13},
               {-78, 3, 12},
               {-1716, 7, 8},
               {-286, 4, 11}},
              {{-2002, 6, 10},
               {-91, 3, 13},
               {-1716, 8, 8},
               {-1001, 5, 11},
               {-364, 4, 12},
               {-14, 2, 14},
               {-3003, 7, 9}},
              {{-6435, 8, 9},
               {-3003, 6, 11},
               {-1365, 5, 12},
               {-455, 4, 13},
               {-5005, 7, 10},
               {-15, 2, 15},
               {-105, 3, 14}},
              {{-120, 3, 15},
               {-11440, 8, 10},
               {-16, 2, 16},
               {-560, 4, 14},
               {-1820, 5, 13},
               {-6435, 9, 9},
               {-4368, 6, 12},
               {-8008, 7, 11}},
              {{-2380, 5, 14},
               {-12376, 7, 12},
               {-680, 4, 15},
               {-24310, 9, 10},
               {-6188, 6, 13},
               {-136, 3, 16},
               {-19448, 8, 11},
               {-17, 2, 17}},
              {{-18564, 7, 13},
               {-24310, 10, 10},
               {-31824, 8, 12},
               {-816, 4, 16},
               {-3060, 5, 15},
               {-43758, 9, 11},
               {-153, 3, 17},
               {-18, 2, 18},
               {-8568, 6, 14}},
        };
 
        int ao = approximation_order + d_dof_and_time_d_path.size2() - 2;
        for (size_t i = 1; i < d_dof_and_time_d_path.size2(); ++i, --ao) {
            for (int j = 0; J[i - 1][j][0] != 0; ++j) {
                d_dof_and_time_d_path(rs - 1, i) += J[i - 1][j][0]
                                                    * (d_dof_and_time_d_path[J[i - 1][j][1] - 1]
                                                       * d_dof_and_time_d_path[J[i - 1][j][2] - 1]);
            }
            TaylorPathResidue tpr = {dof, time, d_dof_and_time_d_path, i, *this, arc};
            std::vector<b2linalg::Vector<T, b2linalg::Vdense_ref> > res;
            res.push_back(d_dof_and_time_d_path[i](b2linalg::Interval(0, rs - 1)));
 
#ifdef TEST_D_DOF_D_PATH
            for (size_t ii = 1; ii != i + 2; ++ii) {
                central_numerical_differentiation<
                      TaylorPathResidue, Vector<T, Vdense_ref>, Vector<T, Vdense> >(
                      tpr, 0, res, ii, ao, ii, std::pow(1e-15, 1.0 / (ao + ii)));
                std::cout << "ii=" << ii << ", " << res[0][0] << std::endl;
                res[0].set_zero();
            }
 
            for (int ii = 0; ii != 10; ++ii) {
                Vector<T, Vdense> tmp;
                tpr(0.5 * ii, tmp);
                std::cout << "x=" << 0.5 * ii << ", " << tmp[0] << std::endl;
            }
#endif
            const int aoo = ao + ao % 2;
            central_numerical_differentiation<
                  TaylorPathResidue, b2linalg::Vector<T, b2linalg::Vdense_ref>,
                  b2linalg::Vector<T, b2linalg::Vdense> >(
                  tpr, 0, res, i + 1, aoo, i + 1, std::pow(1e-15, 1.0 / (aoo + i + 1)) * h_scale);
            if (i == 1) {
                StaticResidueFunction<T, MATRIX_FORMAT>::logger
                      << logging::debug
                      << "get_d_dof_d_path: Factorize tangent "
                         "stiffness matrix ("
                      << K.size1() << " rows and columns)." << logging::LOGFLUSH;
                d_dof_and_time_d_path[i] =
                      update_extension_and_inverse(
                            K, q, arc(b2linalg::Interval(0, rs - 1)), arc[rs - 1])
                      * d_dof_and_time_d_path[i];
            } else {
                d_dof_and_time_d_path[i] = inverse(K) * d_dof_and_time_d_path[i];
            }
            d_dof_and_time_d_path[i] *= -1;
        }
    }
 
    void get_d2_residue_d_dof_d_path(
          const b2linalg::Vector<T, b2linalg::Vdense_constref> dof, const double time,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& K,
          b2linalg::Matrix<T, b2linalg::Mrectangle_ref> d_dof_and_time_d_path,
          std::vector<b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible> >&
                d2_residue_d_dof_d_path,
          int approximation_order = 1, double h_scale = 1.0) {
        for (size_t i = 0; i != d2_residue_d_dof_d_path.size(); ++i) {
            d2_residue_d_dof_d_path[i].set_same_structure(K);
        }
        TaylorPath_d_value_d_dof tpdvdd = {dof, time, d_dof_and_time_d_path, *this, K};
        central_numerical_differentiation<
              TaylorPath_d_value_d_dof,
              b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>,
              b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible> >(
              tpdvdd, 0, d2_residue_d_dof_d_path, 1, approximation_order, 0,
              std::pow(1e-15, 1.0 / (approximation_order + d2_residue_d_dof_d_path.size() + 1))
                    * h_scale);
    }
 
    using type_t = ObjectTypeIncomplete<StaticResidueFunction>;
    static type_t type;
 
protected:
    logging::Logger& logger;
 
private:
    struct TaylorPathResidue {
        const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof;
        const double time;
        const b2linalg::Matrix<T, b2linalg::Mrectangle_ref>& d_dof_and_time_d_path;
        const size_t s;
        StaticResidueFunction& residue;
        const b2linalg::Vector<T, b2linalg::Vdense_constref> arc;
        void operator()(double x, b2linalg::Vector<T, b2linalg::Vdense>& res) {
            const b2linalg::Interval inter = b2linalg::Interval(0, dof.size());
            b2linalg::Vector<T, b2linalg::Vdense> d(d_dof_and_time_d_path.size1());
            d(inter) = dof;
            d[dof.size()] = time;
            b2linalg::Vector<T, b2linalg::Vdense> p(s);
            {
                int fact = 1;
                double v = 1;
                for (size_t i = 0; i != s; ++i) {
                    v *= x;
                    fact *= i + 1;
                    p[i] = v / fact;
                }
            }
            d += d_dof_and_time_d_path(b2linalg::Interval(0, s)) * p;
            res.resize(d_dof_and_time_d_path.size1() - 1);
            res.set_zero();
            EquilibriumSolution esf(false);
            residue.get_residue(
                  d(inter), d[dof.size()], 0, esf, res,
                  b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>::null,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0, 0);
        }
    };
 
    struct TaylorPath_d_value_d_dof {
        const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof;
        const double time;
        const b2linalg::Matrix<T, b2linalg::Mrectangle_ref>& d_dof_and_time_d_path;
        StaticResidueFunction& residue;
        const b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& K;
        void operator()(
              double x,
              b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& d_value_d_dof) {
            d_value_d_dof.set_same_structure(K);
            const b2linalg::Interval inter = b2linalg::Interval(0, dof.size());
            b2linalg::Vector<T, b2linalg::Vdense> d(d_dof_and_time_d_path.size1());
            d(inter) = dof;
            d[dof.size()] = time;
            b2linalg::Vector<T, b2linalg::Vdense> p(d_dof_and_time_d_path.size2());
            {
                int fact = 1;
                double v = 1;
                for (size_t i = 0; i != p.size(); ++i) {
                    v *= x;
                    fact *= i + 1;
                    p[i] = v / fact;
                }
            }
            d += d_dof_and_time_d_path * p;
            EquilibriumSolution esf(false);
            d_value_d_dof.set_same_structure(K);
            residue.get_residue(
                  d(inter), d[dof.size()], 0, esf, b2linalg::Vector<T, b2linalg::Vdense_ref>::null,
                  d_value_d_dof, b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0, 0);
        }
    };
};
 
template <typename T, typename MATRIX_FORMAT>
typename StaticResidueFunction<T, MATRIX_FORMAT>::type_t
      StaticResidueFunction<T, MATRIX_FORMAT>::type(
            "StaticResidueFunction", type_name<T, MATRIX_FORMAT>(),
            StringList("STATIC_RESIDUE_FUNCTION"), module);
 
template <typename T, typename MATRIX_FORMAT>
class IncrementConstraintStrategy : public Object {
public:
    enum Result {
        in_stage,
        in_stage_and_retry,
        end_of_stage_with_success,
        end_of_stage_with_error
    };
 
    IncrementConstraintStrategy()
        : max_time(0),
          logger(logging::get_logger("solver.static_nonlinear.increment_constraint_strategy")) {}
 
    virtual void init(Model& model, const AttributeList& attribute) {}
 
    void set_max_time(double time) { max_time = time; }
 
    virtual Result set_increment(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense_ref> q, b2linalg::Vector<T, b2linalg::Vdense_ref> d,
          double& time, EquilibriumSolution& equilibrium_solution) = 0;
 
    virtual void get_constraint(
          const b2linalg::Vector<T, b2linalg::Vdense>& dof, const double time, T* constraint,
          b2linalg::Vector<T, b2linalg::Vdense>* d_constraint_d_dof, T* d_constraint_d_time) = 0;
 
    virtual Result set_correction_result(
          b2linalg::Vector<T, b2linalg::Vdense>& dof, double& time, bool converged,
          int number_of_iteration) = 0;
 
    virtual double get_delta_time() const = 0;
 
    using type_t = ObjectTypeIncomplete<IncrementConstraintStrategy>;
    static type_t type;
 
protected:
    double max_time;
    logging::Logger& logger;
};
 
template <typename T, typename MATRIX_FORMAT>
typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::type_t
      IncrementConstraintStrategy<T, MATRIX_FORMAT>::type(
            "IncrementConstraintStrategy", type_name<T, MATRIX_FORMAT>(),
            StringList("INCREMENT_CONSRAINT_STRATEGY"), module);
 
template <typename T, typename MATRIX_FORMAT>
class CorrectionStrategy : public Object {
public:
    virtual void init(Model& model, const AttributeList& attribute) {}
 
    virtual typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result compute_correction(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense>& q,
          IncrementConstraintStrategy<T, MATRIX_FORMAT>& constraint,
          b2linalg::Vector<T, b2linalg::Vdense>& d, double& time,
          EquilibriumSolution& equilibrium_solution) = 0;
 
    using type_t = ObjectTypeIncomplete<CorrectionStrategy>;
    static type_t type;
 
protected:
    logging::Logger& logger{logging::get_logger("solver.static_nonlinear.correction_strategy")};
};
 
template <typename T, typename MATRIX_FORMAT>
typename CorrectionStrategy<T, MATRIX_FORMAT>::type_t CorrectionStrategy<T, MATRIX_FORMAT>::type(
      "CorrectionStrategy", type_name<T, MATRIX_FORMAT>(), StringList("CORRECTION_STRATEGY"),
      module);
 
template <typename T>
class CorrectionTerminationTest : public DofFieldFlux<T> {
public:
    enum Termination { converged, diverged, max_iteration, unfinished };
 
    virtual void init(Model& model, const AttributeList& attribute) {}
 
    virtual void new_step() {}
 
    virtual void new_iteration() {}
 
    virtual void new_evaluation() {}
 
    virtual void converged_step() {}
 
    virtual bool need_flux() { return false; }
 
    virtual void add_flux(b2linalg::Index& index, const b2linalg::Vector<T, b2linalg::Vdense>& v) {}
 
    virtual void add_flux(const b2linalg::Vector<T, b2linalg::Vdense_constref>& v) {}
 
    virtual double norm_termination(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& r,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& delta_dof,
          const double delta_dof_scale, const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof,
          const double constraint, const double d_constraint_d_time, const double delta_time,
          const double time, StaticResidueFunctionForTerminationTest<T>& residue,
          const b2linalg::Vector<T, b2linalg::Vindex1_constref>& diag_K) {
        return 1;
    }
 
    virtual Termination is_terminated(
          int number_of_iteration, const b2linalg::Vector<T, b2linalg::Vdense_constref>& r,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& delta_dof,
          const double delta_dof_scale, const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof_at_start_step,
          const double constraint, const double d_constraint_d_time, const double delta_time,
          const double time, StaticResidueFunctionForTerminationTest<T>& residue,
          const b2linalg::Vector<T, b2linalg::Vindex1_constref>& diag_K,
          double& distance_to_iterative_convergence, const SolverHints* solver_hints) = 0;
 
    virtual int get_nb_divergence() const { return 0; }
 
    using type_t = ObjectTypeIncomplete<CorrectionTerminationTest>;
    static type_t type;
 
protected:
    logging::Logger& logger{
          logging::get_logger("solver.static_nonlinear.correction_termination_test")};
};
 
template <typename T>
typename CorrectionTerminationTest<T>::type_t CorrectionTerminationTest<T>::type(
      "CorrectionTerminationTest", type_name<T>(), StringList("CORRECTION_TERMINATION_TEST"),
      module);
 
class LineSearch : public Object {
public:
    struct Function {
        virtual ~Function() {}
        virtual double operator()(double h) = 0;
    };
 
    virtual void init(Model& model, const AttributeList& attribute) = 0;
 
    virtual double get_minimum(Function& g, double g0, double g1) = 0;
 
    using type_t = ObjectTypeIncomplete<LineSearch>;
    static type_t type;
 
protected:
    logging::Logger& logger{logging::get_logger("solver.static_nonlinear.line_search")};
};
 
// =====================================================
// Implementation
// =====================================================
 
template <typename T, typename MATRIX_FORMAT>
class DefaultStaticResidueFunction : public StaticResidueFunction<T, MATRIX_FORMAT> {
public:
    using nbc_t = TypedNaturalBoundaryCondition<T, typename MATRIX_FORMAT::sparse_inversible>;
    using ebc_t = TypedEssentialBoundaryCondition<T>;
    using mrbc_t = TypedModelReductionBoundaryCondition<T>;
 
    DefaultStaticResidueFunction()
        : number_of_dof(0),
          number_of_non_lagrang_dof(0),
          model(nullptr),
          domain(nullptr),
          mrbc(0),
          ebc(0),
          nbc(0),
          constraint_has_penalty(false),
          penalty_factor(0),
          lagrange_mult_scale(0),
          autospc(false),
          rcfo_only_spc(false),
          elements_linear(false) {}
 
    void init(Model& model_, const AttributeList& attribute) {
        model = &model_;
        domain = &model->get_domain();
        ebc = &dynamic_cast<ebc_t&>(model->get_essential_boundary_condition(ebc_t::type.get_subtype(
              "TypedEssentialBoundaryConditionListComponent" + type_name<T>())));
        mrbc = &dynamic_cast<mrbc_t&>(
              model->get_model_reduction_boundary_condition(mrbc_t::type.get_subtype(
                    "TypedModelReductionBoundaryConditionListComponent" + type_name<T>())));
        nbc = &dynamic_cast<nbc_t&>(model->get_natural_boundary_condition(nbc_t::type.get_subtype(
              "TypedNaturalBoundaryConditionListComponent"
              + type_name<T, typename MATRIX_FORMAT::sparse_inversible>())));
        nbc_sol.resize(domain->get_number_of_dof());
        fi.resize(domain->get_number_of_dof());
 
        constraint_has_penalty =
              attribute.get_string("CONSTRAINT_METHOD", "REDUCTION") == "PENALTY";
 
        constraint_has_penalty |=
              attribute.get_string("NONLINEAR_CONSTRAINT_METHOD", "OTHER") == "PENALTY";
 
        number_of_dof = number_of_non_lagrang_dof =
              mrbc->get_size(false, b2linalg::Vector<T, b2linalg::Vdense>::null, 1);
        if (constraint_has_penalty) {
            penalty_factor = attribute.get_double("CONSTRAINT_PENALTY", 1e10);
        } else {
            if (attribute.get_bool("REMOVE_DEPENDENT_CONSTRAINTS", false)) {
                // Calculate the linear reduction matrix.
                b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_d_r_d_dof_lin;
                DefaultStaticResidueFunction<T, MATRIX_FORMAT>::mrbc->get_linear_value(
                      b2linalg::Vector<T, b2linalg::Vdense>::null, trans_d_r_d_dof_lin);
 
                // Calculate the constraint equations.
                b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_d_l_d_dof;
                r.resize(domain->get_number_of_dof());
                ebc->get_nonlinear_value(
                      r, 0, false, b2linalg::Vector<T, b2linalg::Vdense_ref>::null, trans_d_l_d_dof,
                      b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0);
 
                // Reduce them and get the dependent columns.
                b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_d_l_d_dof_red;
                trans_d_l_d_dof_red = trans_d_r_d_dof_lin * trans_d_l_d_dof;
 
                get_dependent_columns(trans_d_l_d_dof_red, ebc_to_remove);
                // trans_d_l_d_dof_red.get_dep_columns(ebc_to_remove, 3e-13, 1.5);
                if (!ebc_to_remove.empty()) {
                    StaticResidueFunction<T, MATRIX_FORMAT>::logger
                          << logging::info << "Remove " << ebc_to_remove.size()
                          << " dependent constraints." << logging::LOGFLUSH;
                }
            }
            number_of_dof += ebc->get_size(false) - ebc_to_remove.size();
            penalty_factor = attribute.get_double("CONSTRAINT_PENALTY", 1);
            if (attribute.get_string("NONLINEAR_CONSTRAINT_METHOD", "OTHER") == "LAGRANGE") {
                penalty_factor = 0;
            }
            lagrange_mult_scale = attribute.get_double("LAGRANGE_MULT_SCALE_PENALTY", 1.0);
        }
        autospc = attribute.get_bool("AUTOSPC", false);
 
        rcfo_only_spc = attribute.get_bool("RCFO_ONLY_SPC", false);
        if (rcfo_only_spc
            && attribute.get_string("CONSTRAINT_METHOD", "REDUCTION") == "REDUCTION") {
            Exception() << "This is not possible to compute reaction force "
                           "only due to the single point constraint with the reduction "
                           "method. Use Lagrange, augmented Lagrange or penalty method "
                           "instead."
                        << THROW;
        }
        elements_linear = attribute.get_bool("ELEMENTS_LINEAR", false);
    }
 
    Model& get_model() { return *model; }
 
    size_t get_number_of_dof() { return number_of_dof; }
 
    size_t get_number_of_non_lagrange_dof() { return number_of_non_lagrang_dof; }
 
    void get_solution(
          const b2linalg::Vector<T, b2linalg::Vdense_constref> dof, const double time,
          EquilibriumSolution equilibrium_solution,
          b2linalg::Vector<T, b2linalg::Vdense_ref> dof_sol) {
        mrbc_get_nonlinear_value(
              dof(b2linalg::Interval(
                    0, mrbc->get_size(false, b2linalg::Vector<T, b2linalg::Vdense>::null, 1))),
              time, equilibrium_solution, dof_sol,
              b2linalg::Matrix<T, b2linalg::Mcompressed_col>::null,
              b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0);
    }
 
    const b2linalg::Vector<T, b2linalg::Vdense_constref> get_residuum_sol(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, const double time) {
        if (rcfo_only_spc) {
            // recompute fi using only the spc contribution
 
            b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_d_l_d_dof;
            b2linalg::Vector<T> l(ebc->get_size(false));
            ebc->get_nonlinear_value(
                  r, time, false, l, trans_d_l_d_dof,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0);
            if (!ebc_to_remove.empty()) {
                l.remove(ebc_to_remove);
                trans_d_l_d_dof.remove_col(ebc_to_remove);
            }
 
            if (constraint_has_penalty) {
                l *= -penalty_factor;
            } else {
                b2linalg::Interval lagrange_mult_range(
                      mrbc->get_size(false, b2linalg::Vector<T, b2linalg::Vdense>::null, 1),
                      number_of_dof);
                if (MATRIX_FORMAT::symmetric && penalty_factor != 0) {
                    // Apply a penalty method with a small factor
                    // to obtain a definite positive matrix.
                    l *= -penalty_factor;
                    l += -lagrange_mult_scale * dof(lagrange_mult_range);
                } else {
                    l = -lagrange_mult_scale * dof(lagrange_mult_range);
                }
            }
            for (size_t j = 0; j != trans_d_l_d_dof.size2(); ++j) {
                if (trans_d_l_d_dof.get_nb_nonzero(j) != 1) { l[j] = 0; }
            }
            fi = trans_d_l_d_dof * l;
        }
        return fi;
    }
 
    const b2linalg::Vector<T, b2linalg::Vdense_constref> get_nbc_sol() { return nbc_sol; }
 
    T get_residue_normalization() { return std::max(fi * fi, nbc_sol * nbc_sol); }
 
    double get_energy_normalisation1() {
        double res = 0;
        for (size_t i = 0; i != r.size(); ++i) {
            res += std::max(norm(r[i] * nbc_sol[i]), norm(r[i] * fi[i]));
        }
        return res;
    }
 
    double get_energy() {
        double res = 0;
        for (size_t i = 0; i != r.size(); ++i) { res += r[i] * (fi[i] + nbc_sol[i]); }
        return res;
    }
 
    double get_energy_increment(const b2linalg::Vector<T>& dof_increment) {
        double res = 0;
        for (size_t i = 0; i != dof_increment.size(); ++i) {
            res += dof_increment[i] * (fi[i] + nbc_sol[i]);
        }
        return res;
    }
 
    const b2linalg::Vector<T, b2linalg::Vindex1_constref> get_matrix_diagonal(
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& d_residue_d_dof) {
        b2linalg::Vector<T, b2linalg::Vindex1_constref> res = d_residue_d_dof.get_diagonal();
        res.set_default_value(lagrange_mult_scale);
        return res;
    }
 
    void get_residue(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, const double time,
          const double delta_time, EquilibriumSolution& equilibrium_solution,
          b2linalg::Vector<T, b2linalg::Vdense_ref> residue,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& d_residue_d_dof,
          b2linalg::Vector<T, b2linalg::Vdense_ref> d_residue_d_time, SolverHints* solver_hints,
          CorrectionTerminationTest<T>* termination_test, GradientContainer* gradient_container) {
        // DefaultStaticResidueFunction<T, MATRIX_FORMAT>::logger <<
        //     logging::debug << "Default::get_residue" <<
        //         logging::LOGFLUSH;
        b2linalg::Interval disp_range(
              0, mrbc->get_size(false, b2linalg::Vector<T, b2linalg::Vdense>::null, 1));
        b2linalg::Interval lagrange_mult_range(
              mrbc->get_size(false, b2linalg::Vector<T, b2linalg::Vdense>::null, 1), number_of_dof);
 
        if (!d_residue_d_time.is_null()) { d_residue_d_dof.set_zero_same_struct(); }
 
        r.resize(domain->get_number_of_dof());
        b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_d_r_d_dof;
        b2linalg::Vector<T, b2linalg::Vdense> d_r_d_time(domain->get_number_of_dof());
        mrbc_get_nonlinear_value(
              dof(disp_range), time, equilibrium_solution, r, trans_d_r_d_dof, d_r_d_time,
              solver_hints);
        // DefaultStaticResidueFunction<T, MATRIX_FORMAT>::logger <<
        //     logging::debug << "Default::after mrbc_get_nonlinear_value" <<
        //         logging::LOGFLUSH;
 
        if (residue.is_null() && d_residue_d_dof.is_null() && d_residue_d_time.is_null()) {
            solver_utile.get_elements_values(
                  b2linalg::Matrix<T, b2linalg::Mrectangle_constref>(r), time, delta_time,
                  equilibrium_solution, elements_linear, trans_d_r_d_dof, d_r_d_time,
                  b2linalg::Vector<double, b2linalg::Vdense_constref>::null, *model,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, d_residue_d_dof,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, gradient_container, solver_hints,
                  StaticResidueFunction<T, MATRIX_FORMAT>::logger, termination_test);
            nbc->get_nonlinear_value(
                  r, time, equilibrium_solution, b2linalg::Vector<T, b2linalg::Vdense_ref>::null,
                  b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>::null,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, solver_hints);
            return;
        }
 
        b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible> d_fe_d_dof;
        b2linalg::Vector<T, b2linalg::Vdense> d_fe_d_time(domain->get_number_of_dof());
        nbc_sol.set_zero();
        nbc->get_nonlinear_value(
              r, time, equilibrium_solution,
              (residue.is_null() ? residue : b2linalg::Vector<T, b2linalg::Vdense_ref>(nbc_sol)),
              d_residue_d_dof.is_null()
                    ? b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>::null
                    : d_fe_d_dof,
              (d_residue_d_time.is_null() ? d_residue_d_time
                                          : b2linalg::Vector<T, b2linalg::Vdense_ref>(d_fe_d_time)),
              solver_hints);
        if (termination_test) { termination_test->add_flux(nbc_sol); }
        fi = -nbc_sol;
        d_fe_d_time = -d_fe_d_time;
        if (!d_residue_d_dof.is_null() && d_fe_d_dof.size1() != 0) {
            d_residue_d_dof += -(trans_d_r_d_dof * d_fe_d_dof * transposed(trans_d_r_d_dof));
        }
        // DefaultStaticResidueFunction<T, MATRIX_FORMAT>::logger <<
        //     logging::debug << "Default::after nbc_get_nonlinear_value" <<
        //         logging::LOGFLUSH;
 
        b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_d_l_d_dof;
        b2linalg::Vector<T> l;
        b2linalg::Vector<T> d_l_d_time;
 
        if (constraint_has_penalty) {
            if (!residue.is_null()) { l.resize(ebc->get_size(false)); }
            if (!d_residue_d_time.is_null()) { d_l_d_time.resize(ebc->get_size(false)); }
            ebc->get_nonlinear_value(
                  r, time, equilibrium_solution,
                  (residue.is_null() ? b2linalg::Vector<T>::null : l), trans_d_l_d_dof,
                  (d_residue_d_time.is_null()
                         ? b2linalg::Vector<T, b2linalg::Vdense_ref>::null
                         : b2linalg::Vector<T, b2linalg::Vdense_ref>(d_l_d_time)),
                  solver_hints);
        } else {
            const size_t num_lm_full = lagrange_mult_range.size() + ebc_to_remove.size();
            assert(ebc->get_size(false) == num_lm_full);
            // Vector<T> residue_lm_tmp(num_lm_full);
            b2linalg::Vector<T> residue_lm_tmp;
            if (!residue.is_null()) { residue_lm_tmp.resize(num_lm_full); }
 
            // Vector<T> d_residue_d_time_lm_tmp(num_lm_full);
            b2linalg::Vector<T> d_residue_d_time_lm_tmp;
            if (!d_residue_d_time.is_null()) { d_residue_d_time_lm_tmp.resize(num_lm_full); }
 
            ebc->get_nonlinear_value(
                  r, time, equilibrium_solution,
                  // Vector<T, Vdense_ref>(residue_lm_tmp),
                  (residue.is_null() ? b2linalg::Vector<T, b2linalg::Vdense_ref>::null
                                     : b2linalg::Vector<T, b2linalg::Vdense_ref>(residue_lm_tmp)),
                  trans_d_l_d_dof,
                  // Vector<T, Vdense_ref>(d_residue_d_time_lm_tmp),
                  (d_residue_d_time.is_null()
                         ? b2linalg::Vector<T, b2linalg::Vdense_ref>::null
                         : b2linalg::Vector<T, b2linalg::Vdense_ref>(d_residue_d_time_lm_tmp)),
                  solver_hints);
            if (!residue.is_null()) {
                if (!ebc_to_remove.empty()) { residue_lm_tmp.remove(ebc_to_remove); }
                residue(lagrange_mult_range) = residue_lm_tmp;
            }
            if (!ebc_to_remove.empty()) { trans_d_l_d_dof.remove_col(ebc_to_remove); }
            if (!d_residue_d_time.is_null()) {
                if (!ebc_to_remove.empty()) { d_residue_d_time_lm_tmp.remove(ebc_to_remove); }
                d_residue_d_time(lagrange_mult_range) = d_residue_d_time_lm_tmp;
                d_residue_d_time(lagrange_mult_range) += transposed(trans_d_l_d_dof) * d_r_d_time;
            }
        }
 
        if (!d_residue_d_time.is_null() && d_fe_d_dof.size1()) {
            d_fe_d_time -= d_fe_d_dof * d_r_d_time;
        }
 
        // DefaultStaticResidueFunction<T, MATRIX_FORMAT>::logger <<
        //     logging::debug << "Default::after ebc_get_nonlinear_value" <<
        //         logging::LOGFLUSH;
 
        StaticResidueFunction<T, MATRIX_FORMAT>::logger
              << logging::debug << "Element assembly ("
              << (d_residue_d_dof.is_null() ? "first" : "second") << " variation)"
              << logging::LOGFLUSH;
        solver_utile.get_elements_values(
              b2linalg::Matrix<T, b2linalg::Mrectangle_constref>(r), time, delta_time,
              equilibrium_solution, elements_linear, trans_d_r_d_dof, d_r_d_time,
              b2linalg::Vector<double, b2linalg::Vdense_constref>::null, *model,
              (residue.is_null() ? b2linalg::Vector<T, b2linalg::Vdense_ref>::null
                                 : b2linalg::Vector<T, b2linalg::Vdense_ref>(fi)),
              d_residue_d_dof,
              (d_residue_d_time.is_null() ? b2linalg::Vector<T, b2linalg::Vdense_ref>::null
                                          : b2linalg::Vector<T, b2linalg::Vdense_ref>(d_fe_d_time)),
              gradient_container, solver_hints, StaticResidueFunction<T, MATRIX_FORMAT>::logger,
              termination_test);
        if (equilibrium_solution.input_level == 0) { equilibrium_solution.input_level = 1; }
 
        if (StaticResidueFunction<T, MATRIX_FORMAT>::logger.is_enabled_for(logging::data)) {
            StaticResidueFunction<T, MATRIX_FORMAT>::logger << logging::data << "dof "
                                                            << logging::DBName("DOF.GLOB") << r
                                                            << logging::LOGFLUSH;
            if (!residue.is_null()) {
                StaticResidueFunction<T, MATRIX_FORMAT>::logger
                      << logging::data << "value " << logging::DBName("VALUE.GLOB") << fi
                      << logging::LOGFLUSH;
            }
            if (!d_residue_d_dof.is_null()) {
                StaticResidueFunction<T, MATRIX_FORMAT>::logger
                      << logging::data << "d_value_d_dof " << logging::DBName("D_VALUE_D_DOF.GLOB")
                      << d_residue_d_dof << logging::LOGFLUSH;
            }
        }
 
        if (!residue.is_null()) {
            b2linalg::Vector<T, b2linalg::Vdense> tmp;
            tmp = fi;
            if (constraint_has_penalty) {
                tmp += (trans_d_l_d_dof * l) * penalty_factor;
            } else {
                if (MATRIX_FORMAT::symmetric && penalty_factor != 0) {
                    // Apply a penalty method with a small factor to
                    // obtain a definite positive matrix.
                    b2linalg::Vector<T> tmp1;
                    tmp1 = residue(lagrange_mult_range) * penalty_factor;
                    tmp1 += dof(lagrange_mult_range) * lagrange_mult_scale;
                    tmp += trans_d_l_d_dof * tmp1;
                } else {
                    tmp += lagrange_mult_scale * (trans_d_l_d_dof * dof(lagrange_mult_range));
                }
                residue(lagrange_mult_range) *= lagrange_mult_scale;
            }
            residue(disp_range) = trans_d_r_d_dof * tmp;
            if (StaticResidueFunction<T, MATRIX_FORMAT>::logger.is_enabled_for(logging::data)) {
                //                    for (size_t i = 0; i != tmp.size(); ++i)
                //                        tmp[i] *= dof[i];
                dynamic_cast<SolutionStaticNonlinear<T>&>(model->get_solution())
                      .set_equilibrium(time, tmp);
            }
        }
 
        b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_LR;
        trans_LR = trans_d_r_d_dof * trans_d_l_d_dof;
        if (!d_residue_d_dof.is_null()) {
            if (penalty_factor != 0 && (constraint_has_penalty || MATRIX_FORMAT::symmetric)) {
                // Apply a penalty method with a small factor to
                // obtain a definite positive matrix when Lagrange
                // multipliers method is used.
                d_residue_d_dof(disp_range, disp_range) +=
                      (trans_LR * b2linalg::transposed(trans_LR)) * penalty_factor;
            }
            if (!constraint_has_penalty) {
                trans_LR *= lagrange_mult_scale;
                b2linalg::Matrix<T, b2linalg::Mcompressed_col> LR = b2linalg::transposed(trans_LR);
                d_residue_d_dof(lagrange_mult_range, disp_range) += LR;
                if (!MATRIX_FORMAT::symmetric) {
                    d_residue_d_dof(disp_range, lagrange_mult_range) += trans_LR;
                }
            }
 
            //
            // Check matrix for structural singularities. Fix them if
            // autospc is active.
            //
            if (1) {
                std::set<size_t> zero_columns_r;
                for (size_t i = 0; i != disp_range.get_end() /*d_residue_d_dof.size1()*/; ++i) {
                    if (d_residue_d_dof(i, i) == T(0)) {
                        bool all_zero = true;
                        for (size_t j = 0; all_zero && j != d_residue_d_dof.size2(); ++j) {
                            if (d_residue_d_dof(i, j) != T(0)) { all_zero = false; }
                        }
                        if (all_zero) {
                            zero_columns_r.insert(i);
                            if (autospc) { d_residue_d_dof(i, i) += T(1); }
                        }
                    }
                }
 
                if (!zero_columns_r.empty()) {
                    b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
                    nks_r.resize(d_residue_d_dof.size1(), 1);
                    nks_r.set_zero();
                    for (auto i : zero_columns_r) { nks_r(i, 0) = T(1); }
                    b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                          model->get_domain().get_number_of_dof(), nks_r.size2());
                    for (size_t i = 0; i != nks_r.size2(); ++i) {
                        get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                    }
                    for (size_t i = 0; i != nks.size1(); ++i) {
                        nks(i, 0) = (nks(i, 0) != T(0) ? T(1) : T(0));
                    }
                    model->get_solution().set_system_null_space(nks, false, "NS_STRUCTURE");
                }
            }
        }
 
        if (!d_residue_d_time.is_null()) {
            d_residue_d_time(disp_range) = trans_d_r_d_dof * d_fe_d_time;
            if (constraint_has_penalty) {
                d_residue_d_time(disp_range) +=
                      penalty_factor * (trans_LR /*trans_d_l_d_dof*/ * d_l_d_time);
            } else {
                d_residue_d_time(lagrange_mult_range) *= lagrange_mult_scale;
            }
        }
    }
 
    void mrbc_get_nonlinear_value(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, double time,
          EquilibriumSolution equilibrium_solution, b2linalg::Vector<T, b2linalg::Vdense_ref> value,
          b2linalg::Matrix<T, b2linalg::Mcompressed_col>& d_value_d_dof_trans,
          b2linalg::Vector<T, b2linalg::Vdense_ref> d_value_d_time, SolverHints* solver_hints) {
        mrbc->get_nonlinear_value(
              dof, time, equilibrium_solution, value, d_value_d_dof_trans,
              b2linalg::Matrix<T, b2linalg::Mrectangle_ref>(d_value_d_time), solver_hints);
    }
 
    using type_t = ObjectTypeComplete<
          DefaultStaticResidueFunction, typename StaticResidueFunction<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
protected:
    size_t number_of_dof;
    size_t number_of_non_lagrang_dof;
    Model* model;
    Domain* domain;
    mrbc_t* mrbc;
    ebc_t* ebc;
    nbc_t* nbc;
    b2linalg::Vector<T, b2linalg::Vdense> fi;
    b2linalg::Vector<T, b2linalg::Vdense> nbc_sol;
    b2linalg::Vector<T, b2linalg::Vdense> r;
    b2linalg::Index ebc_to_remove;
    SolverUtils<T, MATRIX_FORMAT> solver_utile;
    bool constraint_has_penalty;
    double penalty_factor;
    double lagrange_mult_scale;
    bool autospc;
    bool rcfo_only_spc;
    bool elements_linear;
};
 
template <typename T, typename MATRIX_FORMAT>
typename DefaultStaticResidueFunction<T, MATRIX_FORMAT>::type_t
      DefaultStaticResidueFunction<T, MATRIX_FORMAT>::type(
            "DefaultStaticResidueFunction", type_name<T, MATRIX_FORMAT>(),
            StringList("DEFAULT_STATIC_RESIDUE_FUNCTION"), module);
 
template <typename T, typename MATRIX_FORMAT>
class ScaledStaticResidueFunction : public DefaultStaticResidueFunction<T, MATRIX_FORMAT> {
public:
    using base_class = DefaultStaticResidueFunction<T, MATRIX_FORMAT>;
 
    T get_residue_normalization() { return 1; }
 
    void get_residue(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, const double time,
          const double delta_time, EquilibriumSolution& equilibrium_solution,
          b2linalg::Vector<T, b2linalg::Vdense_ref> residue,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& d_residue_d_dof,
          b2linalg::Vector<T, b2linalg::Vdense_ref> d_residue_d_time, SolverHints* solver_hints,
          CorrectionTerminationTest<T>* termination_test, GradientContainer* gradient_container) {
        if (scale_vector.empty()) {
            if (d_residue_d_dof.is_null() || d_residue_d_time.is_null()) {
                UnimplementedError() << THROW;
            }
            scale_vector.reserve(d_residue_d_dof.size1());
            for (size_t i = 0; i != d_residue_d_dof.size1(); ++i) { scale_vector.push_back(1); }
            b2linalg::Vector<T, b2linalg::Vdense> dof_tmp(dof.size());
            base_class::get_residue(
                  dof_tmp, time, delta_time, equilibrium_solution, residue, d_residue_d_dof,
                  d_residue_d_time, solver_hints, termination_test, gradient_container);
            for (size_t i = 0; i != d_residue_d_dof.size1(); ++i) {
                scale_vector[i] = 1 / std::sqrt(std::abs(d_residue_d_dof(i, i)));
            }
            if (d_residue_d_time.is_null()) { UnimplementedError() << THROW; }
            d_residue_d_time = inverse(d_residue_d_dof) * d_residue_d_time;
            T t = 0;
            for (size_t i = 0; i != d_residue_d_time.size(); ++i) {
                const T tt = d_residue_d_time[i] / scale_vector[i];
                t += tt * tt;
            }
            scale_vector *= std::sqrt(t);
        }
 
        base_class::get_residue(
              dof, time, delta_time, equilibrium_solution, residue, d_residue_d_dof,
              d_residue_d_time, solver_hints, termination_test, gradient_container);
    }
 
    void mrbc_get_nonlinear_value(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, double time,
          EquilibriumSolution equilibrium_solution, b2linalg::Vector<T, b2linalg::Vdense_ref> value,
          b2linalg::Matrix<T, b2linalg::Mcompressed_col>& d_value_d_dof_trans,
          b2linalg::Vector<T, b2linalg::Vdense_ref> d_value_d_time, SolverHints* solver_hints) {
        b2linalg::Vector<T, b2linalg::Vdense> dof_tmp;
        dof_tmp = dof;
        dof_tmp.scale(scale_vector);
        base_class::mrbc->get_nonlinear_value(
              dof_tmp, time, equilibrium_solution, value, d_value_d_dof_trans,
              b2linalg::Matrix<T, b2linalg::Mrectangle_ref>(d_value_d_time), solver_hints);
        if (!d_value_d_dof_trans.is_null()) { d_value_d_dof_trans.scale_row(scale_vector); }
    }
 
    using type_t = ObjectTypeComplete<
          ScaledStaticResidueFunction,
          typename DefaultStaticResidueFunction<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
private:
    b2linalg::Vector<T, b2linalg::Vdense> scale_vector;
};
 
template <typename T, typename MATRIX_FORMAT>
typename ScaledStaticResidueFunction<T, MATRIX_FORMAT>::type_t
      ScaledStaticResidueFunction<T, MATRIX_FORMAT>::type(
            "ScaledStaticResidueFunction", type_name<T, MATRIX_FORMAT>(),
            StringList("SCALED_STATIC_RESIDUE_FUNCTION"), module);
 
template <typename T, typename MATRIX_FORMAT>
class ArtificialDampingStaticResidueFunction
    : public DefaultStaticResidueFunction<T, MATRIX_FORMAT> {
public:
    ArtificialDampingStaticResidueFunction()
        : DefaultStaticResidueFunction<T, MATRIX_FORMAT>(),
          rayleigh_damping_alpha(0.0),
          rayleigh_damping_beta(0.0),
          dissipated_energy_fraction(0.0),
          sfactor(0.0),
          old_time(0.0) {}
 
    void init(Model& model, const AttributeList& attribute) {
        DefaultStaticResidueFunction<T, MATRIX_FORMAT>::init(model, attribute);
 
        const char* alpha_s = "RAYLEIGH_ALPHA";
        const char* beta_s = "RAYLEIGH_BETA";
        const char* def_s = "DISSIPATED_ENERGY_FRACTION";
 
        if (attribute.has_key(alpha_s) && attribute.has_key(beta_s)) {
            rayleigh_damping_alpha = attribute.get_double(alpha_s);
            rayleigh_damping_beta = attribute.get_double(beta_s);
            sfactor = 1;
        } else {
            rayleigh_damping_alpha = 1;
            rayleigh_damping_beta = 0;
            dissipated_energy_fraction = attribute.get_double(def_s, 1.e-4);
            // We do not recompute the sfactor between the stages
            if (d_residue_d_dofdot.size1() == 0) { sfactor = 0; }
        }
        old_dof.clear();
    }
 
    void get_residue(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof, const double time,
          const double delta_time, EquilibriumSolution& equilibrium_solution,
          b2linalg::Vector<T, b2linalg::Vdense_ref> residue,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible>& d_residue_d_dof,
          b2linalg::Vector<T, b2linalg::Vdense_ref> d_residue_d_time, SolverHints* solver_hints,
          CorrectionTerminationTest<T>* termination_test, GradientContainer* gradient_container) {
        EquilibriumSolution equilibrium_solution_input = equilibrium_solution;
        // DefaultStaticResidueFunction<T, MATRIX_FORMAT>::logger <<
        //     logging::debug << "ArtificialDamping::get_residue" <<
        //         logging::LOGFLUSH;
        if (d_residue_d_dofdot.size1()
                  != DefaultStaticResidueFunction<T, MATRIX_FORMAT>::number_of_dof
            && sfactor == 1) {
            compute_damping_matrix(b2linalg::Vector<T, b2linalg::Vdense>::null);
        }
        DefaultStaticResidueFunction<T, MATRIX_FORMAT>::get_residue(
              dof, time, delta_time, equilibrium_solution, residue, d_residue_d_dof,
              d_residue_d_time, solver_hints, termination_test, gradient_container);
        if (old_dof.size() == 0) {
            old_dof = dof;
            old_time = time;
            return;
        }
        if (delta_time <= 0) {
            Exception() << "Artificial damping can only be used with a load-controlled constraint "
                           "strategy (e.g. increment_control_type=load)."
                        << THROW;
        }
        const double i_delta_time = 1.0 / delta_time;
 
        b2linalg::Vector<T, b2linalg::Vdense> tmp;
        if (d_residue_d_dofdot.size1()
                  == DefaultStaticResidueFunction<T, MATRIX_FORMAT>::number_of_dof
            && (!residue.is_null() || !d_residue_d_time.is_null())
            && d_residue_d_dofdot.size1() != 0) {
            b2linalg::Vector<T, b2linalg::Vdense> delta_dof;
            delta_dof = dof - old_dof;
            tmp = sfactor * i_delta_time * (d_residue_d_dofdot * delta_dof);
        }
        if (d_residue_d_dofdot.size1()
                  == DefaultStaticResidueFunction<T, MATRIX_FORMAT>::number_of_dof
            && !residue.is_null() && !tmp.empty()) {
            residue += tmp;
        }
        if (!d_residue_d_dof.is_null()
            && d_residue_d_dofdot.size1()
                     == DefaultStaticResidueFunction<T, MATRIX_FORMAT>::number_of_dof
            && !tmp.empty()) {
            d_residue_d_dof += sfactor * i_delta_time * d_residue_d_dofdot;
        }
        if (d_residue_d_dofdot.size1()
                  == DefaultStaticResidueFunction<T, MATRIX_FORMAT>::number_of_dof
            && !d_residue_d_time.is_null() && !tmp.empty()) {
            d_residue_d_time += i_delta_time * tmp;
        }
        if (equilibrium_solution_input && time > old_time) {
            if (d_residue_d_dofdot.size1() == 0) {
                b2linalg::Vector<T, b2linalg::Vdense> dof_ur(
                      DefaultStaticResidueFunction<T, MATRIX_FORMAT>::model->get_domain()
                            .get_number_of_dof());
                DefaultStaticResidueFunction<T, MATRIX_FORMAT>::get_solution(
                      dof, time, equilibrium_solution_input, dof_ur);
                b2linalg::Vector<T, b2linalg::Vdense> old_dof_ur(dof_ur.size());
                DefaultStaticResidueFunction<T, MATRIX_FORMAT>::get_solution(
                      old_dof, old_time, equilibrium_solution_input, old_dof_ur);
                dof_ur -= old_dof_ur;
                const double s1 = DefaultStaticResidueFunction<T, MATRIX_FORMAT>::get_energy();
                const double s2 = compute_damping_matrix(dof_ur) / delta_time;
                sfactor = dissipated_energy_fraction * s1 / s2;
                DefaultStaticResidueFunction<T, MATRIX_FORMAT>::logger
                      << logging::debug << "scale factor for the stabilised method " << sfactor
                      << logging::LOGFLUSH;
            }
            {
                b2linalg::Vector<T, b2linalg::Vdense> delta_dof;
                delta_dof = dof - old_dof;
                b2linalg::Vector<T> tmp;
                tmp = sfactor * i_delta_time * (d_residue_d_dofdot * delta_dof);
                const double dissipated_energy = norm(tmp * delta_dof);
                double energy_increment;
                {
                    b2linalg::Vector<T, b2linalg::Vdense> dof_ur(
                          DefaultStaticResidueFunction<T, MATRIX_FORMAT>::model->get_domain()
                                .get_number_of_dof());
                    DefaultStaticResidueFunction<T, MATRIX_FORMAT>::get_solution(
                          dof, time, equilibrium_solution_input, dof_ur);
                    b2linalg::Vector<T, b2linalg::Vdense> old_dof_ur(dof_ur.size());
                    DefaultStaticResidueFunction<T, MATRIX_FORMAT>::get_solution(
                          old_dof, old_time, equilibrium_solution_input, old_dof_ur);
                    dof_ur -= old_dof_ur;
                    energy_increment =
                          DefaultStaticResidueFunction<T, MATRIX_FORMAT>::get_energy_increment(
                                dof_ur);
                }
                DefaultStaticResidueFunction<T, MATRIX_FORMAT>::logger
                      << logging::debug << "dissipated energy in the step = " << dissipated_energy
                      << ", by increment unit = " << dissipated_energy * i_delta_time
                      << ", energy increment = " << energy_increment << "." << logging::LOGFLUSH;
                Solution& solution =
                      DefaultStaticResidueFunction<T, MATRIX_FORMAT>::model->get_solution();
                solution.set_value("AD_DISSIPATED_ENERGY_IN_STEP", dissipated_energy);
                solution.set_value("MODEL_ENERGY_INCREMENT", energy_increment);
            }
            old_time = time;
            old_dof = dof;
        }
    }
 
    using type_t = ObjectTypeComplete<
          ArtificialDampingStaticResidueFunction,
          typename StaticResidueFunction<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
private:
    double rayleigh_damping_alpha;
    double rayleigh_damping_beta;
    double dissipated_energy_fraction;
    double sfactor;
    double old_time;
    b2linalg::Vector<T, b2linalg::Vdense> old_dof;
    b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible> d_residue_d_dofdot;
 
    double compute_damping_matrix(b2linalg::Vector<T, b2linalg::Vdense>& dof) {
        b2linalg::Matrix<T, b2linalg::Mcompressed_col> trans_d_r_d_dof;
        DefaultStaticResidueFunction<T, MATRIX_FORMAT>::mrbc->get_linear_value(
              b2linalg::Vector<T, b2linalg::Vdense>::null, trans_d_r_d_dof);
        d_residue_d_dofdot.resize(DefaultStaticResidueFunction<T, MATRIX_FORMAT>::number_of_dof);
        b2linalg::Index index;
        std::vector<b2linalg::Matrix<T, typename MATRIX_FORMAT::dense> > d_fe_d_dof_elem(3);
        std::vector<bool> d_value_d_dof_flags(3, true);
        d_value_d_dof_flags[1] = false;
        std::unique_ptr<Domain::ElementIterator> i =
              DefaultStaticResidueFunction<T, MATRIX_FORMAT>::domain->get_element_iterator();
        DefaultStaticResidueFunction<T, MATRIX_FORMAT>::domain->set_dof(
              b2linalg::Vector<double, b2linalg::Vdense_constref>::null);
        b2linalg::Vector<T, b2linalg::Vdense> dof_res(dof.size());
        double norm_inf_mass_matrix = 0;
        for (Element* e = i->next(); e != nullptr; e = i->next()) {
            if (auto te = dynamic_cast<TypedElement<T>*>(e); te != nullptr) {
                te->get_value(
                      *DefaultStaticResidueFunction<T, MATRIX_FORMAT>::model,
                      false,  // linear
                      true,   // equilibrium_solution
                      0,      // time
                      0,      // delta_time
                      b2linalg::Matrix<T, b2linalg::Mrectangle_constref>::null,
                      0,  // gradient_container
                      0,  // solver_hints
                      index, b2linalg::Vector<T, b2linalg::Vdense>::null, d_value_d_dof_flags,
                      d_fe_d_dof_elem, b2linalg::Vector<T, b2linalg::Vdense>::null);
            }
            norm_inf_mass_matrix += d_fe_d_dof_elem[2].norm_inf();
            d_fe_d_dof_elem[0] *= T(rayleigh_damping_beta);
            d_fe_d_dof_elem[0] += T(rayleigh_damping_alpha) * d_fe_d_dof_elem[2];
            if (!dof.is_null()) { dof_res(index) += d_fe_d_dof_elem[0] * dof(index); }
            d_residue_d_dofdot +=
                  trans_d_r_d_dof
                  * StructuredMatrix(trans_d_r_d_dof.size2(), index, d_fe_d_dof_elem[0])
                  * transposed(trans_d_r_d_dof);
        }
        if (norm_inf_mass_matrix < 1e-15 && rayleigh_damping_alpha != 0) {
            Exception() << "Artificial damping on a zero mass model." << THROW;
        }
        return dof * dof_res;
    }
};
 
template <typename T, typename MATRIX_FORMAT>
typename ArtificialDampingStaticResidueFunction<T, MATRIX_FORMAT>::type_t
      ArtificialDampingStaticResidueFunction<T, MATRIX_FORMAT>::type(
            "ArtificialDampingStaticResidueFunction", type_name<T, MATRIX_FORMAT>(),
            StringList("ARTIFICIAL_DAMPING_STATIC_RESIDUE_FUNCTION"), module);
 
template <typename T, typename MATRIX_FORMAT>
class LoadControlConstraintStrategy : public IncrementConstraintStrategy<T, MATRIX_FORMAT> {
public:
    LoadControlConstraintStrategy()
        : time(0),
          step_size(0),
          step_size_min(0),
          step_size_max(0),
          time_at_start_step(0),
          nb_iteration(5),
          corrector_nb_newton_iteration(0),
          refactorization(true) {}
 
    void init(Model& model, const AttributeList& attribute) {
        step_size = attribute.get_double("STEP_SIZE_INIT", 0.1);
        step_size_min = attribute.get_double("STEP_SIZE_MIN", 1e-12);
        step_size_max = attribute.get_double("STEP_SIZE_MAX", 1e100);
        step_size_match = attribute.get_double("STEP_SIZE_MATCH", 0);
        nb_iteration =
              attribute.get_int("NB_ITERATION", attribute.get_int("MAX_NEWTON_ITERATIONS", 50) / 2);
        corrector_nb_newton_iteration = attribute.get_double("NB_ITERATION_CORRECTION", 0.5);
        refactorization = true;
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_increment(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense_ref> q, b2linalg::Vector<T, b2linalg::Vdense_ref> d,
          double& time_, EquilibriumSolution& equilibrium_solution) {
        dof_at_start_step = d;
        time_at_start_step = time_;
        time = time_ + step_size;
        if (step_size_match != 0
            && int(time_at_start_step / step_size_match) < int(time / step_size_match)) {
            time = (int(time_at_start_step / step_size_match) + 1) * step_size_match;
        }
        if (time > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - step_size_min) {
            time = IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
        }
 
        Solution& solution = residue_function.get_model().get_solution();
        if (refactorization) {
            residue_function.get_residue(
                  d, time_, step_size, equilibrium_solution,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, K, q, 0);
        }
 
        b2linalg::Vector<T, b2linalg::Vdense> v(q.size() + 1);
        v[q.size()] = 1;
        Model& model = residue_function.get_model();
        const ssize_t nbnsv = model.get_case()->get_int("COMPUTE_NULL_SPACE_VECTOR", 0);
        b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
        b2linalg::SingularMatrixError ee;
        bool is_singular = false;
        try {
            if (refactorization) {
                IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                      << logging::debug
                      << "Compute prediction and factorize tangent "
                         "stiffness matrix ("
                      << K.size1() << " rows and columns)." << logging::LOGFLUSH;
                v = update_extension_and_inverse(
                          K, q, v(b2linalg::Interval(0, q.size())), 1.0, nks_r, nbnsv)
                    * v;
            } else {
                v = inverse(K, nks_r, nbnsv) * v;
            }
        } catch (b2linalg::SingularMatrixError& e) {
            ee = e;
            is_singular = true;
            if (!e.singular_dofs.empty()) {
                if (nks_r.size2() == 0) {
                    nks_r.resize(K.size1(), 1);
                    nks_r.set_zero();
                }
                for (auto i : e.singular_dofs) {
                    assert(i < nks_r.size1());
                    for (size_t j = 0; j != nks_r.size2(); ++j) { nks_r(i, j) = T(1); }
                }
            }
        }
        if (nks_r.size2() != 0) {
            b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                  model.get_domain().get_number_of_dof(), nks_r.size2());
            for (size_t i = 0; i != nks_r.size2(); ++i) {
                residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
            }
            solution.set_system_null_space(nks);
        }
        if (is_singular) { ee << THROW; }
 
        d += (time - time_) * v(b2linalg::Interval(0, q.size()));
        time_ = time;
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
    }
 
    void get_constraint(
          const b2linalg::Vector<T, b2linalg::Vdense>& dof, const double time_, T* constraint,
          b2linalg::Vector<T, b2linalg::Vdense>* d_constraint_d_dof, T* d_constraint_d_time) {
        if (constraint) { *constraint = time_ - time; }
        if (d_constraint_d_dof) { d_constraint_d_dof->set_zero(); }
        if (d_constraint_d_time) { *d_constraint_d_time = 1; }
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_correction_result(
          b2linalg::Vector<T, b2linalg::Vdense>& dof, double& time_, bool converged,
          int number_of_iteration) {
        if (converged) {
            if (time > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - 1e-8) {
                return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_success;
            }
            const double step_size_scale = std::pow(
                  corrector_nb_newton_iteration,
                  double(number_of_iteration - nb_iteration) / (nb_iteration - 1));
            step_size *= step_size_scale;
            step_size = std::max(step_size, step_size_min);
            if (step_size > step_size_max) {
                step_size = step_size_max;
                refactorization = false;
            } else if (step_size_scale > 1) {
                refactorization = false;
            } else {
                refactorization = true;
            }
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::debug << "new step size = " << step_size << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
        }
        refactorization = true;
        step_size *= 0.5;
        if (step_size < step_size_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error << "Cannot continue, the step size (" << step_size
                  << ") is below the minimum step size(" << step_size_min << ")."
                  << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        dof = dof_at_start_step;
        time_ = time = time_at_start_step;
        IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
              << logging::debug << "new step size = " << step_size << logging::LOGFLUSH;
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage_and_retry;
    }
 
    double get_delta_time() const { return step_size; }
 
    using type_t = ObjectTypeComplete<
          LoadControlConstraintStrategy,
          typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
private:
    double time;
    double step_size;
    double step_size_min;
    double step_size_max;
    double step_size_match;
    b2linalg::Vector<T, b2linalg::Vdense> dof_at_start_step;
    double time_at_start_step;
    int nb_iteration;
    double corrector_nb_newton_iteration;
    bool refactorization;
    bool autospc;
};
 
template <typename T, typename MATRIX_FORMAT>
typename LoadControlConstraintStrategy<T, MATRIX_FORMAT>::type_t
      LoadControlConstraintStrategy<T, MATRIX_FORMAT>::type(
            "LoadControlConstraintStrategy", type_name<T, MATRIX_FORMAT>(),
            StringList("LOAD_CONTROL_CONSTRAINT_STRATEGY"), module);
 
template <typename T, typename MATRIX_FORMAT>
class StateControlConstraintStrategy : public IncrementConstraintStrategy<T, MATRIX_FORMAT> {
public:
    StateControlConstraintStrategy()
        : stage_termination_procedure(false),
          step_size(0),
          step_size_min(0),
          step_size_max(0),
          time_at_start_step(0),
          nb_iteration(0),
          corrector_nb_newton_iteration(0) {}
 
    void init(Model& model, const AttributeList& attribute) {
        step_size = attribute.get_double("STEP_SIZE_INIT", 0.1);
        step_size_min = attribute.get_double("STEP_SIZE_MIN", 1e-12);
        step_size_max = attribute.get_double("STEP_SIZE_MAX", 1e100);
        nb_iteration =
              attribute.get_int("NB_ITERATION", attribute.get_int("MAX_NEWTON_ITERATIONS", 50) / 2);
        corrector_nb_newton_iteration = attribute.get_double("NB_ITERATION_CORRECTION", 0.5);
        stage_termination_procedure = false;
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_increment(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense_ref> q, b2linalg::Vector<T, b2linalg::Vdense_ref> d,
          double& time, EquilibriumSolution& equilibrium_solution) {
        dof_at_start_step = d;
        time_at_start_step = time;
        residue_function.get_residue(
              d, time, step_size, equilibrium_solution,
              b2linalg::Vector<T, b2linalg::Vdense_ref>::null, K, q, 0);
        b2linalg::Vector<T, b2linalg::Vdense> v(q.size() + 1);
        v[q.size()] = 1;
        Model& model = residue_function.get_model();
        const ssize_t nbnsv = model.get_case()->get_int("COMPUTE_NULL_SPACE_VECTOR", 0);
        b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
        try {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::debug
                  << "Compute prediction and factorize tangent "
                     "stiffness matrix ("
                  << K.size1() << " rows and columns)." << logging::LOGFLUSH;
            v = update_extension_and_inverse(
                      K, q, v(b2linalg::Interval(0, q.size())), 1.0, nks_r, nbnsv)
                * v;
        } catch (b2linalg::SingularMatrixError& e) {
            if (nks_r.size2() != 0) {
                b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                      model.get_domain().get_number_of_dof(), nks_r.size2());
                for (size_t i = 0; i != nks_r.size2(); ++i) {
                    residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                }
                residue_function.get_model().get_solution().set_system_null_space(nks);
            }
            throw;
        }
        if (nks_r.size2() != 0) {
            b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                  model.get_domain().get_number_of_dof(), nks_r.size2());
            for (size_t i = 0; i != nks_r.size2(); ++i) {
                residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
            }
            residue_function.get_model().get_solution().set_system_null_space(nks);
        }
 
        double d_time = std::sqrt(step_size / ((v * v) - 1));
        if (stage_termination_procedure
            || time + d_time
                     > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - step_size_min) {
            stage_termination_procedure = true;
            d += (time - IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time)
                 * v(b2linalg::Interval(0, q.size()));
            time = IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
        } else {
            d += d_time * v(b2linalg::Interval(0, q.size()));
            time += d_time;
        }
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
    }
 
    void get_constraint(
          const b2linalg::Vector<T, b2linalg::Vdense>& dof, const double time, T* constraint,
          b2linalg::Vector<T, b2linalg::Vdense>* d_constraint_d_dof, T* d_constraint_d_time) {
        if (stage_termination_procedure) {
            if (constraint) {
                *constraint = time - IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
            }
            if (d_constraint_d_dof) { d_constraint_d_dof->set_zero(); }
            if (d_constraint_d_time) { *d_constraint_d_time = 1; }
        } else {
            b2linalg::Vector<T, b2linalg::Vdense> d_dof;
            d_dof = dof - dof_at_start_step;
            if (constraint) { *constraint = d_dof * d_dof - step_size * step_size; }
            if (d_constraint_d_dof) { *d_constraint_d_dof = 2.0 * d_dof; }
            if (d_constraint_d_time) { *d_constraint_d_time = 0; }
        }
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_correction_result(
          b2linalg::Vector<T, b2linalg::Vdense>& dof, double& time, bool converged,
          int number_of_iteration) {
        if (converged) {
            if (time > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - 1e-8
                || stage_termination_procedure) {
                stage_termination_procedure = false;
                return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_success;
            }
            step_size *= std::pow(
                  corrector_nb_newton_iteration,
                  double(number_of_iteration - nb_iteration) / (nb_iteration - 1));
            step_size = std::min(std::max(step_size, step_size_min), step_size_max);
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
        }
        stage_termination_procedure = false;
        step_size *= 0.5;
        if (step_size < step_size_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error << "Cannot continue, the step size (" << step_size
                  << ") is below the minimum step size(" << step_size_min << ")."
                  << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        dof = dof_at_start_step;
        time = time_at_start_step;
        IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
              << logging::debug << "new step size = " << step_size << logging::LOGFLUSH;
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage_and_retry;
    }
 
    double get_delta_time() const { return step_size; }
 
    using type_t = ObjectTypeComplete<
          StateControlConstraintStrategy,
          typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
private:
    bool stage_termination_procedure;
    double step_size;
    double step_size_min;
    double step_size_max;
    b2linalg::Vector<T, b2linalg::Vdense> dof_at_start_step;
    double time_at_start_step;
    int nb_iteration;
    double corrector_nb_newton_iteration;
};
 
template <typename T, typename MATRIX_FORMAT>
typename StateControlConstraintStrategy<T, MATRIX_FORMAT>::type_t
      StateControlConstraintStrategy<T, MATRIX_FORMAT>::type(
            "StateControlConstraintStrategy", type_name<T, MATRIX_FORMAT>(),
            StringList("STATE_CONTROL_CONSTRAINT_STRATEGY"), module);
 
template <typename T, typename MATRIX_FORMAT>
class HyperplaneControlConstraintStrategy : public IncrementConstraintStrategy<T, MATRIX_FORMAT> {
public:
    HyperplaneControlConstraintStrategy()
        : stage_termination_procedure(false),
          step_size(0),
          step_size_min(0),
          step_size_max(0),
          step_size_lambda_min(0),
          step_size_lambda_max(0),
          step_size_dof_min(0),
          step_size_dof_max(0),
          step_size_dof_lambda_fact(-1),
          step_size_lambda_fact(-1),
          step_size_dof_fact(-1),
          time_at_start_step(0),
          nb_iteration(0),
          corrector_nb_newton_iteration(0) {}
 
    void init(Model& model, const AttributeList& attribute) {
        step_size = attribute.get_double("STEP_SIZE_INIT", 0.1);
        step_size_min = attribute.get_double(
              "STEP_SIZE_PATH_MIN", attribute.get_double("STEP_SIZE_MIN", 1e-12));
        step_size_max = attribute.get_double(
              "STEP_SIZE_PATH_MAX", attribute.get_double("STEP_SIZE_MAX", 1e100));
        step_size_lambda_min = attribute.get_double("STEP_SIZE_LAMBDA_MIN", 1e-12);
        step_size_lambda_max = attribute.get_double(
              "STEP_SIZE_LAMBDA_MAX", IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time);
        step_size_dof_min = attribute.get_double("STEP_SIZE_DOF_MIN", 1e-12);
        step_size_dof_max = attribute.get_double("STEP_SIZE_DOF_MAX", 1e100);
        nb_iteration =
              attribute.get_int("NB_ITERATION", attribute.get_int("MAX_NEWTON_ITERATIONS", 50) / 2);
        corrector_nb_newton_iteration = attribute.get_double("NB_ITERATION_CORRECTION", 0.5);
        stage_termination_procedure = false;
        predictor_taylor_order_path =
              attribute.get_int("TAYLOR_ORDER_PATH", attribute.get_int("TAYLOR_ORDER", 1));
        predictor_approximation_order_path = attribute.get_int(
              "APPROXIMATION_ORDER_PATH", attribute.get_int("APPROXIMATION_ORDER", 2));
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_increment(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense_ref> q, b2linalg::Vector<T, b2linalg::Vdense_ref> d,
          double& time, EquilibriumSolution& equilibrium_solution) {
        residue_function.get_residue(
              d, time, step_size, equilibrium_solution,
              b2linalg::Vector<T, b2linalg::Vdense_ref>::null, K, q, 0);
        size_t rs = residue_function.get_number_of_dof();
        if (dof_at_start_step.size() == 0) {
            d_dof_d_time_at_previous_step.resize(rs + 1);
            d_dof_d_time_at_previous_step[rs] = 1;
            velocity_at_start_step.resize(rs + 1);
            velocity_at_start_step[rs] = 1;
        } else {
            velocity_at_start_step.set_zero();
            velocity_at_start_step[rs] =
                  d_dof_d_time_at_previous_step * d_dof_d_time_at_previous_step;
        }
        dof_at_start_step = d;
        time_at_start_step = time;
        Model& model = residue_function.get_model();
        const ssize_t nbnsv = model.get_case()->get_int("COMPUTE_NULL_SPACE_VECTOR", 0);
        b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
        try {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::debug
                  << "Compute prediction and factorize tangent "
                     "stiffness matrix ("
                  << K.size1() << " rows and columns)." << logging::LOGFLUSH;
            velocity_at_start_step =
                  update_extension_and_inverse(
                        K, q, d_dof_d_time_at_previous_step(b2linalg::Interval(0, rs)),
                        d_dof_d_time_at_previous_step[rs], nks_r, nbnsv)
                  * velocity_at_start_step;
        } catch (b2linalg::SingularMatrixError& e) {
            if (nks_r.size2() != 0) {
                b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                      model.get_domain().get_number_of_dof(), nks_r.size2());
                for (size_t i = 0; i != nks_r.size2(); ++i) {
                    residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                }
                residue_function.get_model().get_solution().set_system_null_space(nks);
            }
            throw;
        }
        if (nks_r.size2() != 0) {
            b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                  model.get_domain().get_number_of_dof(), nks_r.size2());
            for (size_t i = 0; i != nks_r.size2(); ++i) {
                residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
            }
            residue_function.get_model().get_solution().set_system_null_space(nks);
        }
 
        const size_t nnld = residue_function.get_number_of_non_lagrange_dof();
        double norm_nld = velocity_at_start_step(b2linalg::Interval(0, nnld)).norm2();
        double norm_ld = velocity_at_start_step(b2linalg::Interval(nnld, rs - 1)).norm2();
        double norm_lambda = std::abs(velocity_at_start_step[rs]);
        {
            const double v =
                  1
                  / std::sqrt(norm_nld * norm_nld + norm_ld * norm_ld + norm_lambda * norm_lambda);
            velocity_at_start_step *= v;
            norm_nld *= v;
            norm_ld *= v;
            norm_lambda *= v;
        }
        // The first step size is a lambda step size
        if (step_size_lambda_fact == -1) {
            if (velocity_at_start_step[rs] == velocity_at_start_step[rs]) {
                step_size /= velocity_at_start_step[rs];
            }
        }
 
        {
            step_size_dof_lambda_fact = std::sqrt(norm_nld * norm_nld + norm_lambda * norm_lambda);
            if (step_size_dof_lambda_fact * step_size > step_size_max) {
                step_size = step_size_max / step_size_dof_lambda_fact;
            }
            if (step_size_dof_lambda_fact * step_size < step_size_min) {
                step_size = step_size_min / step_size_dof_lambda_fact;
            }
        }
        if (norm_lambda > 0) {
            step_size_lambda_fact = norm_lambda;
            if (step_size_lambda_fact * step_size > step_size_lambda_max) {
                step_size = step_size_lambda_max / step_size_lambda_fact;
            }
            if (step_size_lambda_fact * step_size < step_size_lambda_min) {
                step_size = step_size_lambda_min / step_size_lambda_fact;
            }
        }
        if (norm_nld > 0) {
            step_size_dof_fact = norm_nld;
            if (step_size_dof_fact * step_size > step_size_dof_max) {
                step_size = step_size_dof_max / step_size_dof_fact;
            }
            if (step_size_dof_fact * step_size < step_size_dof_min) {
                step_size = step_size_dof_min / step_size_dof_fact;
            }
        }
        if (time + step_size * step_size_lambda_fact
            > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time
                    - std::max(step_size_lambda_min, step_size_min * step_size_lambda_fact)) {
            stage_termination_procedure = true;
        }
        if (stage_termination_procedure) {
            d += ((IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - time)
                  / velocity_at_start_step[rs])
                 * velocity_at_start_step(b2linalg::Interval(0, rs));
            time = IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
        } else {
            d += step_size * velocity_at_start_step(b2linalg::Interval(0, rs));
            time += step_size * velocity_at_start_step[rs];
        }
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
    }
 
    void get_constraint(
          const b2linalg::Vector<T, b2linalg::Vdense>& dof, const double time, T* constraint,
          b2linalg::Vector<T, b2linalg::Vdense>* d_constraint_d_dof, T* d_constraint_d_time) {
        if (stage_termination_procedure
            || time >= IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time) {
            if (constraint) {
                *constraint = time - IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
            }
            if (d_constraint_d_dof) { d_constraint_d_dof->set_zero(); }
            if (d_constraint_d_time) { *d_constraint_d_time = 1; }
        } else {
            b2linalg::Vector<T, b2linalg::Vdense> d_dof;
            d_dof = dof - dof_at_start_step;
            size_t rs = velocity_at_start_step.size() - 1;
            if (constraint) {
                *constraint = velocity_at_start_step(b2linalg::Interval(0, rs)) * d_dof
                              + velocity_at_start_step[rs] * (time - time_at_start_step)
                              - step_size;
            }
            if (d_constraint_d_dof) {
                *d_constraint_d_dof = velocity_at_start_step(b2linalg::Interval(0, rs));
            }
            if (d_constraint_d_time) { *d_constraint_d_time = velocity_at_start_step[rs]; }
        }
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_correction_result(
          b2linalg::Vector<T, b2linalg::Vdense>& dof, double& time, bool converged,
          int number_of_iteration) {
        if (converged) {
            d_dof_d_time_at_previous_step(b2linalg::Interval(0, dof.size())) =
                  dof - dof_at_start_step;
            d_dof_d_time_at_previous_step[dof.size()] = time - time_at_start_step;
            if (time > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - 1e-8
                || stage_termination_procedure) {
                stage_termination_procedure = false;
                return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_success;
            }
            step_size *= std::pow(
                  corrector_nb_newton_iteration,
                  double(number_of_iteration - nb_iteration) / (nb_iteration - 1));
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
        }
        step_size *= 0.5;
        stage_termination_procedure = false;
        if (step_size * step_size_dof_lambda_fact < step_size_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size along the continuation "
                     "path ("
                  << step_size * step_size_dof_lambda_fact << ") is below the minimum ("
                  << step_size_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        if (step_size * step_size_lambda_fact < step_size_lambda_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size on the lambda parameter "
                     "control ("
                  << step_size * step_size_lambda_fact << ") is below the minimum ("
                  << step_size_lambda_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        if (step_size * step_size_dof_fact < step_size_dof_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size on the norm of the "
                     "dof ("
                  << step_size * step_size_dof_fact << ") is below the minimum ("
                  << step_size_dof_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        dof = dof_at_start_step;
        time = time_at_start_step;
        IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
              << logging::debug << "new step size = " << step_size << logging::LOGFLUSH;
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage_and_retry;
    }
 
    double get_delta_time() const { return step_size; }
 
    using type_t = ObjectTypeComplete<
          HyperplaneControlConstraintStrategy,
          typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
private:
    bool stage_termination_procedure;
    double step_size;
    double step_size_min;
    double step_size_max;
    double step_size_lambda_min;
    double step_size_lambda_max;
    double step_size_dof_min;
    double step_size_dof_max;
    double step_size_dof_lambda_fact;
    double step_size_lambda_fact;
    double step_size_dof_fact;
    int predictor_taylor_order_path;
    int predictor_approximation_order_path;
    b2linalg::Vector<T, b2linalg::Vdense> dof_at_start_step;
    double time_at_start_step;
    b2linalg::Vector<T, b2linalg::Vdense> velocity_at_start_step;
    b2linalg::Vector<T, b2linalg::Vdense> d_dof_d_time_at_previous_step;
    int nb_iteration;
    double corrector_nb_newton_iteration;
};
 
template <typename T, typename MATRIX_FORMAT>
typename HyperplaneControlConstraintStrategy<T, MATRIX_FORMAT>::type_t
      HyperplaneControlConstraintStrategy<T, MATRIX_FORMAT>::type(
            "HyperplaneControlConstraintStrategy", type_name<T, MATRIX_FORMAT>(),
            StringList("HYPERPLANE_CONTROL_CONSTRAINT_STRATEGY"), module);
 
template <typename T, typename MATRIX_FORMAT>
class NormalFlowControlConstraintStrategy : public IncrementConstraintStrategy<T, MATRIX_FORMAT> {
public:
    NormalFlowControlConstraintStrategy()
        : stage_termination_procedure(false),
          step_size(0),
          step_size_min(0),
          step_size_max(0),
          step_size_lambda_min(0),
          step_size_lambda_max(0),
          step_size_dof_min(0),
          step_size_dof_max(0),
          step_size_dof_lambda_fact(-1),
          step_size_lambda_fact(-1),
          step_size_dof_fact(-1),
          time_at_start_step(0),
          nb_iteration(0),
          corrector_nb_newton_iteration(0),
          K_ptr(0),
          q_ptr(0) {}
 
    void init(Model& model, const AttributeList& attribute) {
        step_size = attribute.get_double("STEP_SIZE_INIT", 0.1);
        step_size_min = attribute.get_double(
              "STEP_SIZE_PATH_MIN", attribute.get_double("STEP_SIZE_MIN", 1e-12));
        step_size_max = attribute.get_double(
              "STEP_SIZE_PATH_MAX", attribute.get_double("STEP_SIZE_MAX", 1e100));
        step_size_lambda_min = attribute.get_double("STEP_SIZE_LAMBDA_MIN", 1e-12);
        step_size_lambda_max = attribute.get_double(
              "STEP_SIZE_LAMBDA_MAX", IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time);
        step_size_dof_min = attribute.get_double("STEP_SIZE_DOF_MIN", 1e-12);
        step_size_dof_max = attribute.get_double("STEP_SIZE_DOF_MAX", 1e100);
        nb_iteration =
              attribute.get_int("NB_ITERATION", attribute.get_int("MAX_NEWTON_ITERATIONS", 50) / 2);
        corrector_nb_newton_iteration = attribute.get_double("NB_ITERATION_CORRECTION", 0.5);
        stage_termination_procedure = false;
        predictor_taylor_order_path =
              attribute.get_int("TAYLOR_ORDER_PATH", attribute.get_int("TAYLOR_ORDER", 1));
        predictor_approximation_order_path = attribute.get_int(
              "APPROXIMATION_ORDER_PATH", attribute.get_int("APPROXIMATION_ORDER", 2));
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_increment(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense_ref> q, b2linalg::Vector<T, b2linalg::Vdense_ref> d,
          double& time, EquilibriumSolution& equilibrium_solution) {
        residue_function.get_residue(
              d, time, step_size, equilibrium_solution,
              b2linalg::Vector<T, b2linalg::Vdense_ref>::null, K, q, 0);
        K_ptr = &K;
        q_ptr = &q;
        size_t rs = residue_function.get_number_of_dof();
        if (dof_at_start_step.size() == 0) {
            d_dof_d_time_at_previous_step.resize(rs + 1);
            d_dof_d_time_at_previous_step[rs] = 1;
            velocity_at_start_step.resize(rs + 1);
            velocity_at_start_step[rs] = 1;
        } else {
            velocity_at_start_step.set_zero();
            velocity_at_start_step[rs] =
                  d_dof_d_time_at_previous_step * d_dof_d_time_at_previous_step;
        }
        dof_at_start_step = d;
        time_at_start_step = time;
        Model& model = residue_function.get_model();
        const ssize_t nbnsv = model.get_case()->get_int("COMPUTE_NULL_SPACE_VECTOR", 0);
        b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
        try {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::debug
                  << "Compute prediction and factorize tangent "
                     "stiffness matrix ("
                  << K.size1() << " rows and columns)." << logging::LOGFLUSH;
            velocity_at_start_step =
                  update_extension_and_inverse(
                        K, q, d_dof_d_time_at_previous_step(b2linalg::Interval(0, rs)),
                        d_dof_d_time_at_previous_step[rs], nks_r, nbnsv)
                  * velocity_at_start_step;
        } catch (b2linalg::SingularMatrixError& e) {
            if (nks_r.size2() != 0) {
                b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                      model.get_domain().get_number_of_dof(), nks_r.size2());
                for (size_t i = 0; i != nks_r.size2(); ++i) {
                    residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                }
                residue_function.get_model().get_solution().set_system_null_space(nks);
            }
            throw;
        }
        if (nks_r.size2() != 0) {
            b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                  model.get_domain().get_number_of_dof(), nks_r.size2());
            for (size_t i = 0; i != nks_r.size2(); ++i) {
                residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
            }
            residue_function.get_model().get_solution().set_system_null_space(nks);
        }
        const size_t nnld = residue_function.get_number_of_non_lagrange_dof();
        double norm_nld = velocity_at_start_step(b2linalg::Interval(0, nnld)).norm2();
        double norm_ld = velocity_at_start_step(b2linalg::Interval(nnld, rs - 1)).norm2();
        double norm_lambda = std::abs(velocity_at_start_step[rs]);
        {
            const double v =
                  1
                  / std::sqrt(norm_nld * norm_nld + norm_ld * norm_ld + norm_lambda * norm_lambda);
            velocity_at_start_step *= v;
            norm_nld *= v;
            norm_ld *= v;
            norm_lambda *= v;
        }
        // The first step size is a lambda step size
        if (step_size_lambda_fact == -1) {
            if (velocity_at_start_step[rs] == velocity_at_start_step[rs]) {
                step_size /= velocity_at_start_step[rs];
            }
        }
 
        {
            step_size_dof_lambda_fact = std::sqrt(norm_nld * norm_nld + norm_lambda * norm_lambda);
            if (step_size_dof_lambda_fact * step_size > step_size_max) {
                step_size = step_size_max / step_size_dof_lambda_fact;
            }
            if (step_size_dof_lambda_fact * step_size < step_size_min) {
                step_size = step_size_min / step_size_dof_lambda_fact;
            }
        }
        if (norm_lambda > 0) {
            step_size_lambda_fact = norm_lambda;
            if (step_size_lambda_fact * step_size > step_size_lambda_max) {
                step_size = step_size_lambda_max / step_size_lambda_fact;
            }
            if (step_size_lambda_fact * step_size < step_size_lambda_min) {
                step_size = step_size_lambda_min / step_size_lambda_fact;
            }
        }
        if (norm_nld > 0) {
            step_size_dof_fact = norm_nld;
            if (step_size_dof_fact * step_size > step_size_dof_max) {
                step_size = step_size_dof_max / step_size_dof_fact;
            }
            if (step_size_dof_fact * step_size < step_size_dof_min) {
                step_size = step_size_dof_min / step_size_dof_fact;
            }
        }
        if (time + step_size * step_size_lambda_fact
            > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time
                    - std::max(step_size_lambda_min, step_size_min * step_size_lambda_fact)) {
            stage_termination_procedure = true;
        }
        if (stage_termination_procedure) {
            d += ((IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - time)
                  / velocity_at_start_step[rs])
                 * velocity_at_start_step(b2linalg::Interval(0, rs));
            time = IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
        } else {
            d += step_size * velocity_at_start_step(b2linalg::Interval(0, rs));
            time += step_size * velocity_at_start_step[rs];
        }
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
    }
 
    void get_constraint(
          const b2linalg::Vector<T, b2linalg::Vdense>& dof, const double time, T* constraint,
          b2linalg::Vector<T, b2linalg::Vdense>* d_constraint_d_dof, T* d_constraint_d_time) {
        if (stage_termination_procedure
            || time >= IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time) {
            if (constraint) {
                *constraint = time - IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
            }
            if (d_constraint_d_dof) { d_constraint_d_dof->set_zero(); }
            if (d_constraint_d_time) { *d_constraint_d_time = 1; }
        } else {
            b2linalg::Vector<T, b2linalg::Vdense> d_dof;
            d_dof = dof - dof_at_start_step;
            size_t rs = velocity_at_start_step.size() - 1;
            if (constraint) { *constraint = 0; }
            if (d_constraint_d_dof || d_constraint_d_time) {
                velocity_at_start_step(b2linalg::Interval(0, rs)) =
                      b2linalg::inverse((
                            b2linalg::Matrix<T, b2linalg::Msym_compressed_col_update_inv>&)(*K_ptr))
                      * (*q_ptr);
                velocity_at_start_step[rs] = 1;
                velocity_at_start_step.normalize();
                velocity_at_start_step *= -1;
                if (d_constraint_d_dof) {
                    *d_constraint_d_dof = velocity_at_start_step(b2linalg::Interval(0, rs));
                }
                if (d_constraint_d_time) { *d_constraint_d_time = velocity_at_start_step[rs]; }
            }
        }
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_correction_result(
          b2linalg::Vector<T, b2linalg::Vdense>& dof, double& time, bool converged,
          int number_of_iteration) {
        if (converged) {
            d_dof_d_time_at_previous_step(b2linalg::Interval(0, dof.size())) =
                  dof - dof_at_start_step;
            d_dof_d_time_at_previous_step[dof.size()] = time - time_at_start_step;
            if (time > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - 1e-8
                || stage_termination_procedure) {
                stage_termination_procedure = false;
                return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_success;
            }
            step_size *= std::pow(
                  corrector_nb_newton_iteration,
                  double(number_of_iteration - nb_iteration) / (nb_iteration - 1));
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
        }
        step_size *= 0.5;
        stage_termination_procedure = false;
        if (step_size * step_size_dof_lambda_fact < step_size_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size along the continuation "
                     "path ("
                  << step_size * step_size_dof_lambda_fact << ") is below the minimum ("
                  << step_size_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        if (step_size * step_size_lambda_fact < step_size_lambda_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size on the lambda parameter "
                     "control ("
                  << step_size * step_size_lambda_fact << ") is below the minimum ("
                  << step_size_lambda_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        if (step_size * step_size_dof_fact < step_size_dof_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size on the norm of the "
                     "dof ("
                  << step_size * step_size_dof_fact << ") is below the minimum ("
                  << step_size_dof_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        dof = dof_at_start_step;
        time = time_at_start_step;
        IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
              << logging::debug << "new step size = " << step_size << logging::LOGFLUSH;
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage_and_retry;
    }
 
    double get_delta_time() const { return step_size; }
 
    using type_t = ObjectTypeComplete<
          NormalFlowControlConstraintStrategy,
          typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
private:
    bool stage_termination_procedure;
    double step_size;
    double step_size_min;
    double step_size_max;
    double step_size_lambda_min;
    double step_size_lambda_max;
    double step_size_dof_min;
    double step_size_dof_max;
    double step_size_dof_lambda_fact;
    double step_size_lambda_fact;
    double step_size_dof_fact;
    int predictor_taylor_order_path;
    int predictor_approximation_order_path;
    b2linalg::Vector<T, b2linalg::Vdense> dof_at_start_step;
    double time_at_start_step;
    b2linalg::Vector<T, b2linalg::Vdense> velocity_at_start_step;
    b2linalg::Vector<T, b2linalg::Vdense> d_dof_d_time_at_previous_step;
    int nb_iteration;
    double corrector_nb_newton_iteration;
    b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>* K_ptr;
    b2linalg::Vector<T, b2linalg::Vdense_ref>* q_ptr;
};
 
template <typename T, typename MATRIX_FORMAT>
typename NormalFlowControlConstraintStrategy<T, MATRIX_FORMAT>::type_t
      NormalFlowControlConstraintStrategy<T, MATRIX_FORMAT>::type(
            "NormalFlowControlConstraintStrategy", type_name<T, MATRIX_FORMAT>(),
            StringList("NORMALFLOW_CONTROL_CONSTRAINT_STRATEGY"), module);
 
template <typename T, typename MATRIX_FORMAT>
class LocalHyperellipticControlConstraintStrategy
    : public IncrementConstraintStrategy<T, MATRIX_FORMAT> {
public:
    LocalHyperellipticControlConstraintStrategy()
        : stage_termination_procedure(false),
          step_size(0),
          step_size_min(0),
          step_size_max(0),
          step_size_lambda_min(0),
          step_size_lambda_max(0),
          step_size_dof_min(0),
          step_size_dof_max(0),
          step_size_dof_lambda_fact(-1),
          step_size_lambda_fact(-1),
          step_size_dof_fact(-1),
          ellipse_a_2(0),
          ellipse_b_2(0),
          time_at_start_step(0),
          nb_iteration(0),
          corrector_nb_newton_iteration(0) {}
 
    void init(Model& model, const AttributeList& attribute) {
        step_size = attribute.get_double("STEP_SIZE_INIT", 0.1);
        step_size_min = attribute.get_double(
              "STEP_SIZE_PATH_MIN", attribute.get_double("STEP_SIZE_MIN", 1e-12));
        step_size_max = attribute.get_double(
              "STEP_SIZE_PATH_MAX", attribute.get_double("STEP_SIZE_MAX", 1e100));
        step_size_lambda_min = attribute.get_double("STEP_SIZE_LAMBDA_MIN", 1e-12);
        step_size_lambda_max = attribute.get_double(
              "STEP_SIZE_LAMBDA_MAX", IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time);
        step_size_dof_min = attribute.get_double("STEP_SIZE_DOF_MIN", 1e-12);
        step_size_dof_max = attribute.get_double("STEP_SIZE_DOF_MAX", 1e100);
        nb_iteration =
              attribute.get_int("NB_ITERATION", attribute.get_int("MAX_NEWTON_ITERATIONS", 50) / 2);
        corrector_nb_newton_iteration = attribute.get_double("NB_ITERATION_CORRECTION", 0.5);
        ellipse_a_2 = attribute.get_double("HYPERELLIPTIC_A", 1.0);
        ellipse_a_2 *= ellipse_a_2;
        ellipse_b_2 = attribute.get_double("HYPERELLIPTIC_B", 1.0);
        ellipse_b_2 *= ellipse_b_2;
        stage_termination_procedure = false;
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_increment(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense_ref> q, b2linalg::Vector<T, b2linalg::Vdense_ref> d,
          double& time, EquilibriumSolution& equilibrium_solution) {
        residue_function.get_residue(
              d, time, step_size, equilibrium_solution,
              b2linalg::Vector<T, b2linalg::Vdense_ref>::null, K, q, 0);
        size_t rs = residue_function.get_number_of_dof();
        if (dof_at_start_step.size() == 0) {
            dof_at_start_step = d;
            time_at_start_step = time;
            d_dof_d_time_at_previous_step.resize(rs + 1);
            d_dof_d_time_at_previous_step[rs] = 1;
            velocity_at_start_step.resize(rs + 1);
            velocity_at_start_step[rs] = 1;
        } else {
            velocity_at_start_step.set_zero();
            velocity_at_start_step[rs] =
                  d_dof_d_time_at_previous_step * d_dof_d_time_at_previous_step;
        }
        dof_at_start_step = d;
        time_at_start_step = time;
 
        Model& model = residue_function.get_model();
        const ssize_t nbnsv = model.get_case()->get_int("COMPUTE_NULL_SPACE_VECTOR", 0);
        b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
        try {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::debug
                  << "Compute prediction and factorize tangent "
                     "stiffness matrix ("
                  << K.size1() << " rows and columns)." << logging::LOGFLUSH;
            velocity_at_start_step =
                  update_extension_and_inverse(
                        K, q, d_dof_d_time_at_previous_step(b2linalg::Interval(0, rs)),
                        d_dof_d_time_at_previous_step[rs], nks_r, nbnsv)
                  * velocity_at_start_step;
        } catch (b2linalg::SingularMatrixError& e) {
            if (nks_r.size2() != 0) {
                b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                      model.get_domain().get_number_of_dof(), nks_r.size2());
                for (size_t i = 0; i != nks_r.size2(); ++i) {
                    residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                }
                residue_function.get_model().get_solution().set_system_null_space(nks);
            }
            throw;
        }
        if (nks_r.size2() != 0) {
            b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                  model.get_domain().get_number_of_dof(), nks_r.size2());
            for (size_t i = 0; i != nks_r.size2(); ++i) {
                residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
            }
            residue_function.get_model().get_solution().set_system_null_space(nks);
        }
        const size_t nnld = residue_function.get_number_of_non_lagrange_dof();
        double norm_nld = velocity_at_start_step(b2linalg::Interval(0, nnld)).norm2();
        double norm_ld = velocity_at_start_step(b2linalg::Interval(nnld, rs - 1)).norm2();
        double norm_lambda = std::abs(velocity_at_start_step[rs]);
        {
            const double v =
                  1
                  / std::sqrt(norm_nld * norm_nld + norm_ld * norm_ld + norm_lambda * norm_lambda);
            velocity_at_start_step *= v;
            norm_nld *= v;
            norm_ld *= v;
            norm_lambda *= v;
        }
        // The first step size is a lambda step size
        if (step_size_lambda_fact == -1) {
            if (velocity_at_start_step[rs] == velocity_at_start_step[rs]) {
                step_size /= velocity_at_start_step[rs];
            }
        }
        {
            step_size_dof_lambda_fact = std::sqrt(norm_nld * norm_nld + norm_lambda * norm_lambda);
            if (step_size_dof_lambda_fact * step_size > step_size_max) {
                step_size = step_size_max / step_size_dof_lambda_fact;
            }
            if (step_size_dof_lambda_fact * step_size < step_size_min) {
                step_size = step_size_min / step_size_dof_lambda_fact;
            }
        }
        if (norm_lambda > 0) {
            step_size_lambda_fact = norm_lambda;
            if (step_size_lambda_fact * step_size > step_size_lambda_max) {
                step_size = step_size_lambda_max / step_size_lambda_fact;
            }
            if (step_size_lambda_fact * step_size < step_size_lambda_min) {
                step_size = step_size_lambda_min / step_size_lambda_fact;
            }
        }
        if (norm_nld > 0) {
            step_size_dof_fact = norm_nld;
            if (step_size_dof_fact * step_size > step_size_dof_max) {
                step_size = step_size_dof_max / step_size_dof_fact;
            }
            if (step_size_dof_fact * step_size < step_size_dof_min) {
                step_size = step_size_dof_min / step_size_dof_fact;
            }
        }
        if (time + step_size * step_size_lambda_fact
            > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time
                    - std::max(step_size_lambda_min, step_size_min * step_size_lambda_fact)) {
            stage_termination_procedure = true;
        }
        if (stage_termination_procedure) {
            d += ((IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - time)
                  / velocity_at_start_step[rs])
                 * velocity_at_start_step(b2linalg::Interval(0, rs));
            time = IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
        } else {
            d += step_size * velocity_at_start_step(b2linalg::Interval(0, rs));
            time += step_size * velocity_at_start_step[rs];
        }
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
    }
 
    void get_constraint(
          const b2linalg::Vector<T, b2linalg::Vdense>& dof, const double time, T* constraint,
          b2linalg::Vector<T, b2linalg::Vdense>* d_constraint_d_dof, T* d_constraint_d_time) {
        if (stage_termination_procedure
            || time >= IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time) {
            if (constraint) {
                *constraint = time - IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time;
            }
            if (d_constraint_d_dof) { d_constraint_d_dof->set_zero(); }
            if (d_constraint_d_time) { *d_constraint_d_time = 1; }
        } else {
            size_t rs = velocity_at_start_step.size() - 1;
            b2linalg::Vector<T, b2linalg::Vdense> d_dof(rs + 1);
            d_dof(b2linalg::Interval(0, rs)) = dof - dof_at_start_step;
            d_dof[rs] = time - time_at_start_step;
            T v_dd = velocity_at_start_step * d_dof;
            b2linalg::Vector<T, b2linalg::Vdense> d_dof_s;
            d_dof_s = d_dof - velocity_at_start_step * v_dd;
            if (constraint) {
                *constraint = ellipse_a_2 * v_dd + ellipse_b_2 * (d_dof_s * d_dof_s) - step_size;
            }
            if (d_constraint_d_dof || d_constraint_d_time) {
                double s = 0;
                for (size_t i = 0; i != d_dof_s.size(); ++i) {
                    s += d_dof_s[i] * (velocity_at_start_step[i] * velocity_at_start_step[i]);
                }
                if (d_constraint_d_dof) {
                    for (size_t i = 0; i != rs; ++i) {
                        (*d_constraint_d_dof)[i] = 2 * ellipse_a_2 * velocity_at_start_step[i]
                                                   + 2 * ellipse_b_2 * (d_dof_s[i] - s);
                    }
                }
                if (d_constraint_d_time) {
                    *d_constraint_d_time = 2 * ellipse_a_2 * velocity_at_start_step[rs]
                                           + 2 * ellipse_b_2 * (d_dof_s[rs] - s);
                }
            }
        }
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result set_correction_result(
          b2linalg::Vector<T, b2linalg::Vdense>& dof, double& time, bool converged,
          int number_of_iteration) {
        IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
              << logging::debug << "Insertion of the correction result in the predictor";
        if (converged) {
            d_dof_d_time_at_previous_step(b2linalg::Interval(0, dof.size())) =
                  dof - dof_at_start_step;
            d_dof_d_time_at_previous_step[dof.size()] = time - time_at_start_step;
            if (time > IncrementConstraintStrategy<T, MATRIX_FORMAT>::max_time - 1e-8
                || stage_termination_procedure) {
                stage_termination_procedure = false;
                IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                      << ", end of stage with success." << logging::LOGFLUSH;
                return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_success;
            }
            step_size *= std::pow(
                  corrector_nb_newton_iteration,
                  double(number_of_iteration - nb_iteration) / (nb_iteration - 1));
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << ", next step with step size=" << step_size << "." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage;
        }
        step_size *= 0.5;
        stage_termination_procedure = false;
        if (step_size * step_size_dof_lambda_fact < step_size_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size along the continuation "
                     "path ("
                  << step_size * step_size_dof_lambda_fact << ") is below the minimum ("
                  << step_size_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        if (step_size * step_size_lambda_fact < step_size_lambda_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size on the lambda parameter "
                     "control ("
                  << step_size * step_size_lambda_fact << ") is below the minimum ("
                  << step_size_lambda_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        if (step_size * step_size_dof_fact < step_size_dof_min) {
            IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
                  << logging::error
                  << "Cannot continue, the step size on the norm of the "
                     "dof ("
                  << step_size * step_size_dof_fact << ") is below the minimum ("
                  << step_size_dof_min << ")." << logging::LOGFLUSH;
            return IncrementConstraintStrategy<T, MATRIX_FORMAT>::end_of_stage_with_error;
        }
        dof = dof_at_start_step;
        time = time_at_start_step;
        IncrementConstraintStrategy<T, MATRIX_FORMAT>::logger
              << ", retry step with step size=" << step_size << "." << logging::LOGFLUSH;
        return IncrementConstraintStrategy<T, MATRIX_FORMAT>::in_stage_and_retry;
    }
 
    double get_delta_time() const { return step_size; }
 
    using type_t = ObjectTypeComplete<
          LocalHyperellipticControlConstraintStrategy,
          typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
private:
    bool stage_termination_procedure;
    double step_size;
    double step_size_min;
    double step_size_max;
    double step_size_lambda_min;
    double step_size_lambda_max;
    double step_size_dof_min;
    double step_size_dof_max;
    double step_size_dof_lambda_fact;
    double step_size_lambda_fact;
    double step_size_dof_fact;
    double ellipse_a_2;
    double ellipse_b_2;
    b2linalg::Vector<T, b2linalg::Vdense> dof_at_start_step;
    double time_at_start_step;
    b2linalg::Vector<T, b2linalg::Vdense> velocity_at_start_step;
    b2linalg::Vector<T, b2linalg::Vdense> d_dof_d_time_at_previous_step;
    int nb_iteration;
    double corrector_nb_newton_iteration;
};
 
template <typename T, typename MATRIX_FORMAT>
typename LocalHyperellipticControlConstraintStrategy<T, MATRIX_FORMAT>::type_t
      LocalHyperellipticControlConstraintStrategy<T, MATRIX_FORMAT>::type(
            "LocalHyperellipticControlConstraintStrategy", type_name<T, MATRIX_FORMAT>(),
            StringList(
                  "LOCAL_HYPERELLIPTIC_CONTROL_CONSTRAINT_STRATEGY",
                  "HYPERELLIPTIC_CONTROL_CONSTRAINT_STRATEGY"),
            module);
 
template <typename T, typename MATRIX_FORMAT>
class NewtonMethod : public CorrectionStrategy<T, MATRIX_FORMAT> {
public:
    enum Method { conventional, modified, delayed_modified };
 
    NewtonMethod() : termination_test(0), method(modified), periodic_update(100) {}
 
    virtual ~NewtonMethod() {
        if (termination_test) { termination_test->free(); }
    }
 
    void init(Model& model, const AttributeList& attribute) {
        std::string termination_s =
              attribute.get_string("CORRECTION_TERMINATION_TEST", "FLUX_NORMALISED");
        if (termination_s != "") { termination_s += "_CORRECTION_TERMINATION_TEST"; }
        termination_s = type_name<T>(termination_s);
        typename CorrectionTerminationTest<T>::type_t* termination_t =
              CorrectionTerminationTest<T>::type.get_subtype(termination_s, solver::module);
        if (termination_test) { termination_test->free(); }
        termination_test = termination_t->new_object(allocator);
        termination_test->init(model, attribute);
        std::string method_s = attribute.get_string("NEWTON_METHOD", "CONVENTIONAL");
        if (method_s == "CONVENTIONAL") {
            method = conventional;
        } else {
            periodic_update = attribute.get_int("NEWTON_PERIODIC_UPDATE", 100);
            if (method_s == "MODIFIED") {
                method = modified;
            } else if (method_s == "DELAYED_MODIFIED") {
                method = delayed_modified;
            }
        }
        if (attribute.get_int("FULLNEWTON", 0) != 0) { method = conventional; }
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result compute_correction(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense>& q,
          IncrementConstraintStrategy<T, MATRIX_FORMAT>& constraint,
          b2linalg::Vector<T, b2linalg::Vdense>& d, double& time,
          EquilibriumSolution& equilibrium_solution) {
        const size_t number_of_dof = residue_function.get_number_of_dof();
        typename CorrectionTerminationTest<T>::Termination result;
        b2linalg::Vector<T, b2linalg::Vdense> r(number_of_dof + 1);
        b2linalg::Interval residu_int(0, number_of_dof);
        b2linalg::Vector<T, b2linalg::Vdense> c_d(number_of_dof);
        T c_c;
        int nb_iter = 0;
        int nb_iter_first = 0;
        bool has_degradation = false;
        b2linalg::Vector<T, b2linalg::Vdense> d_old = d;
        double time_old = time;
        double e_old = std::numeric_limits<double>::max();
        bool always_refactorization = false;
        bool refactorization = false;
 
        if (CorrectionStrategy<T, MATRIX_FORMAT>::logger.is_enabled_for(logging::data)) {
            Model& model = residue_function.get_model();
            b2linalg::Vector<T, b2linalg::Vdense> g(model.get_domain().get_number_of_dof());
            residue_function.get_solution(d, time, equilibrium_solution, g);
            SolutionStaticNonlinear<T>& solution =
                  dynamic_cast<SolutionStaticNonlinear<T>&>(model.get_solution());
            solution.set_dof(
                  time, g, residue_function.get_nbc_sol(),
                  residue_function.get_residuum_sol(d, time), true);
        }
 
        int nb_iter_without_refactorization = 0;
        SolverHints solver_hints;
        termination_test->new_step();
        do {
            termination_test->new_iteration();
            termination_test->new_evaluation();
            refactorization = always_refactorization || (method == conventional)
                              || (method == delayed_modified && nb_iter == 0)
                              || (method == modified
                                  && ++nb_iter_without_refactorization % periodic_update == 0);
 
            if (refactorization) {
                nb_iter_without_refactorization = 0;
                solver_hints.clear();
                residue_function.get_residue(
                      d, time, constraint.get_delta_time(), equilibrium_solution, r(residu_int), K,
                      q, &solver_hints, (termination_test->need_flux() ? termination_test : 0));
#ifdef CHECK_RESIDUE_DERIVATIVE
                // std::cout << "K=" << K << std::endl;
                const double h = 1e-5;
                const double espilon = 1e-2;
                b2linalg::Vector<T> d_tmp(d);
                b2linalg::Vector<T> r_tmp1(d.size());
                b2linalg::Vector<T> r_tmp2(d.size());
                EquilibriumSolution qs(1, -1) for (size_t j = 0; j != d.size(); ++j) {
                    d_tmp[j] -= h / 2;
                    residue_function.get_residue(
                          d_tmp, time, constraint.get_delta_time(), es, r_tmp1,
                          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                          b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0, 0);
                    d_tmp[j] += h;
                    residue_function.get_residue(
                          d_tmp, time, constraint.get_delta_time(), es, r_tmp2,
                          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                          b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0, 0);
                    d_tmp[j] = d[j];
                    /*
                     std::cout << "r_tmp1=" << std::endl;
                     std::cout << r_tmp1 << std::endl;
                     std::cout << "r_tmp2=" << std::endl;
                     std::cout << r_tmp2 << std::endl;
                     */
                    r_tmp2 -= r_tmp1;
                    r_tmp2 *= 1.0 / h;
                    /*
                     std::cout << "num_der=" << std::endl;
                     std::cout << r_tmp2 << std::endl;
                     */
                    for (size_t i = 0; i != d.size(); ++i) {
                        if (norm(K(i, j) - r_tmp2[i])
                            > std::max(espilon, espilon * norm(r_tmp2[i]))) {
                            std::cout
                                  << "Error: K(" << i << ", " << j << ")=" << K(i, j)
                                  << ", computed =" << r_tmp2[i] << ", diff=" << K(i, j) - r_tmp2[i]
                                  << ", n_diff=" << (K(i, j) - r_tmp2[i]) / r_tmp2[i] << std::endl;
                        }
                    }
                }
                double time_tmp = time;
                time_tmp -= h / 2;
                residue_function.get_residue(
                      d, time_tmp, constraint.get_delta_time(), es, r_tmp1,
                      b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                      b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0, 0);
                time_tmp += h;
                residue_function.get_residue(
                      d, time_tmp, constraint.get_delta_time(), es, r_tmp2,
                      Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                      Vector<T, Vdense_ref>::null, 0, 0);
                r_tmp2 -= r_tmp1;
                r_tmp2 *= 1 / h;
                for (size_t i = 0; i != d.size(); ++i) {
                    if (norm(q[i] - r_tmp2[i]) > std::max(espilon, espilon * norm(r_tmp2[i]))) {
                        std::cout << "Error: d_K_d_time[" << i << "]=" << q[i]
                                  << ", computed =" << r_tmp2[i] << ", diff=" << q[i] - r_tmp2[i]
                                  << ", n_diff=" << (q[i] - r_tmp2[i]) / r_tmp2[i] << std::endl;
                    }
                }
#endif /* CHECK_RESIDUE_DERIVATIVE */
            } else {
                solver_hints.clear();
                residue_function.get_residue(
                      d, time, constraint.get_delta_time(), equilibrium_solution, r(residu_int),
                      b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                      b2linalg::Vector<T, b2linalg::Vdense_ref>::null, &solver_hints,
                      (termination_test->need_flux() ? termination_test : 0));
            }
            constraint.get_constraint(
                  d, time, &r[number_of_dof], refactorization || nb_iter == 0 ? &c_d : 0, &c_c);
 
            b2linalg::Vector<T> delta_d;
            Model& model = residue_function.get_model();
            const ssize_t nbnsv = model.get_case()->get_int("COMPUTE_NULL_SPACE_VECTOR", 0);
            b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
            try {
                if (refactorization || nb_iter == 0) {
                    CorrectionStrategy<T, MATRIX_FORMAT>::logger
                          << logging::debug
                          << "Compute correction and factorize "
                             "tangent stiffness matrix ("
                          << K.size1() << " rows and columns)." << logging::LOGFLUSH;
                    delta_d = update_extension_and_inverse(K, q, c_d, c_c, nks_r, nbnsv) * r;
                } else {
                    delta_d = inverse(K, nks_r, nbnsv) * r;
                }
            } catch (b2linalg::SingularMatrixError& e) {
                if (nks_r.size2() != 0) {
                    b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                          model.get_domain().get_number_of_dof(), nks_r.size2());
                    for (size_t i = 0; i != nks_r.size2(); ++i) {
                        residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                    }
                    residue_function.get_model().get_solution().set_system_null_space(nks);
                }
                throw;
            }
            if (nks_r.size2() != 0) {
                b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                      model.get_domain().get_number_of_dof(), nks_r.size2());
                for (size_t i = 0; i != nks_r.size2(); ++i) {
                    residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                }
                residue_function.get_model().get_solution().set_system_null_space(nks);
            }
 
            d += -delta_d(residu_int);
            time += -delta_d[number_of_dof];
 
            if (*solver_hints & SolverHints::degradation) { has_degradation = true; }
            if (!has_degradation) { ++nb_iter_first; }
 
            result = termination_test->is_terminated(
                  ++nb_iter, r(residu_int), delta_d(residu_int), 1, d, d_old, r[number_of_dof], c_c,
                  delta_d[number_of_dof], time, residue_function,
                  residue_function.get_matrix_diagonal(K),
                  equilibrium_solution.distance_to_iterative_convergence, &solver_hints);
 
            if (CorrectionStrategy<T, MATRIX_FORMAT>::logger.is_enabled_for(logging::data)) {
                Model& model = residue_function.get_model();
                b2linalg::Vector<T, b2linalg::Vdense> g(model.get_domain().get_number_of_dof());
                residue_function.get_solution(d, time, false, g);
                SolutionStaticNonlinear<T>& solution =
                      dynamic_cast<SolutionStaticNonlinear<T>&>(model.get_solution());
                solution.set_dof(
                      time, g, residue_function.get_nbc_sol(),
                      residue_function.get_residuum_sol(d, time), true);
            }
 
            if (!always_refactorization) {
                if (nb_iter > 0 && !refactorization
                    && result == CorrectionTerminationTest<T>::diverged) {
                    d = d_old;
                    time = time_old;
                    CorrectionStrategy<T, MATRIX_FORMAT>::logger
                          << logging::debug
                          << "Bad convergence, re-factorize the matrix from "
                             "the last good obtained solution at time="
                          << time << "." << logging::LOGFLUSH;
                } else if (result == CorrectionTerminationTest<T>::unfinished) {
                    if ((equilibrium_solution.distance_to_iterative_convergence >= 0
                         && equilibrium_solution.distance_to_iterative_convergence < e_old)
                        || (*solver_hints & SolverHints::degradation)) {
                        d_old = d;
                        time_old = time;
                        e_old = equilibrium_solution.distance_to_iterative_convergence;
                    }
                }
            }
        } while (result == CorrectionTerminationTest<T>::unfinished);
 
        if (result == CorrectionTerminationTest<T>::converged) {
            termination_test->converged_step();
        }
        return constraint.set_correction_result(
              d, time, result == CorrectionTerminationTest<T>::converged, nb_iter_first);
    }
 
    using type_t =
          ObjectTypeComplete<NewtonMethod, typename CorrectionStrategy<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
protected:
    CorrectionTerminationTest<T>* termination_test;
    Allocator allocator;
    Method method;
    int periodic_update;
};
 
template <typename T, typename MATRIX_FORMAT>
typename NewtonMethod<T, MATRIX_FORMAT>::type_t NewtonMethod<T, MATRIX_FORMAT>::type(
      "NewtonMethod", type_name<T, MATRIX_FORMAT>(), StringList("NEWTON_METHOD"), module);
 
template <typename T, typename MATRIX_FORMAT>
class AcceleratedNewtonMethod : public CorrectionStrategy<T, MATRIX_FORMAT> {
public:
    enum Method { conventional, modified, delayed_modified };
 
    AcceleratedNewtonMethod() : termination_test(0), line_search(nullptr), method(modified) {}
 
    ~AcceleratedNewtonMethod() {
        if (termination_test) { termination_test->free(); }
    }
 
    void init(Model& model, const AttributeList& attribute) {
        std::string termination_s =
              attribute.get_string("CORRECTION_TERMINATION_TEST", "FLUX_NORMALISED");
        if (termination_s != "") { termination_s += "_CORRECTION_TERMINATION_TEST"; }
        termination_s = type_name<T>(termination_s);
        typename CorrectionTerminationTest<T>::type_t* termination_t =
              CorrectionTerminationTest<T>::type.get_subtype(termination_s, solver::module);
        if (termination_test) { termination_test->free(); }
        termination_test = termination_t->new_object(allocator);
        termination_test->init(model, attribute);
 
        std::string line_search_s = attribute.get_string("LINE_SEARCH_ALGORITHM", "STRONG_WOLFE");
        if (line_search_s != "") { line_search_s += "_LINE_SEARCH"; }
        typename LineSearch::type_t* line_search_t =
              LineSearch::type.get_subtype(line_search_s, solver::module);
        line_search = line_search_t->new_object(allocator);
        line_search->init(model, attribute);
        std::string method_s = attribute.get_string("NEWTON_METHOD", "CONVENTIONAL");
        if (method_s == "CONVENTIONAL") {
            method = conventional;
        } else if (method_s == "MODIFIED") {
            method = modified;
        } else if (method_s == "DELAYED_MODIFIED") {
            method = delayed_modified;
        } else {
            KeyError("Unknown Newton method specified.") << THROW;
        }
        if (attribute.get_int("FULLNEWTON", 0) == 1) { method = conventional; }
    }
 
    typename IncrementConstraintStrategy<T, MATRIX_FORMAT>::Result compute_correction(
          StaticResidueFunction<T, MATRIX_FORMAT>& residue_function,
          b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>& K,
          b2linalg::Vector<T, b2linalg::Vdense>& q,
          IncrementConstraintStrategy<T, MATRIX_FORMAT>& constraint,
          b2linalg::Vector<T, b2linalg::Vdense>& d, double& time,
          EquilibriumSolution& equilibrium_solution) {
        typename CorrectionTerminationTest<T>::Termination result;
        int nb_dof = residue_function.get_number_of_dof();
        b2linalg::Vector<T, b2linalg::Vdense> d_old = d;
        b2linalg::Vector<T, b2linalg::Vdense> r(nb_dof + 1);
        b2linalg::Vector<T, b2linalg::Vdense> director(nb_dof + 1);
        b2linalg::Vector<T, b2linalg::Vdense> d_test(nb_dof);
        b2linalg::Vector<T, b2linalg::Vdense> r_test(nb_dof + 1);
        double time_test;
        b2linalg::Interval residu_int(0, nb_dof);
        b2linalg::Vector<T, b2linalg::Vdense> c_d(nb_dof);
        T c_c;
        int refactorization;  // 0=no, 1=yes second variation already computed, 2=yes second
                              // variation no computed
 
        SolverHints solver_hints;
        if (method == delayed_modified || method == conventional) {
            residue_function.get_residue(
                  d, time, constraint.get_delta_time(), equilibrium_solution, r(residu_int), K, q,
                  &solver_hints);
            constraint.get_constraint(d, time, &r[nb_dof], &c_d, &c_c);
            refactorization = 1;
        } else {
            residue_function.get_residue(
                  d, time, constraint.get_delta_time(), equilibrium_solution, r(residu_int),
                  b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, &solver_hints);
            constraint.get_constraint(d, time, &r[nb_dof], 0, &c_c);
            refactorization = 0;
        }
 
        T r0_norm = 0;
        for (size_t i = 0; i != d.size(); ++i) { r0_norm += norm(r[i] * r[i] * d[i]); }
        r0_norm += norm(r[nb_dof] * r[nb_dof] * time);
 
        int nb_iter = 0;
        int nb_iter_first = 0;
        bool has_degradation = false;
        double h = 0;
        bool hard_conv = false;
 
        if (CorrectionStrategy<T, MATRIX_FORMAT>::logger.is_enabled_for(logging::data)) {
            Model& model = residue_function.get_model();
            b2linalg::Vector<T, b2linalg::Vdense> g(model.get_domain().get_number_of_dof());
            residue_function.get_solution(d, time, equilibrium_solution, g);
            SolutionStaticNonlinear<T>& solution =
                  dynamic_cast<SolutionStaticNonlinear<T>&>(model.get_solution());
            solution.set_dof(
                  time, g, residue_function.get_nbc_sol(),
                  residue_function.get_residuum_sol(d, time), true);
        }
 
        int nb_divergence = 0;
        Model& model = residue_function.get_model();
        const ssize_t nbnsv = model.get_case()->get_int("COMPUTE_NULL_SPACE_VECTOR", 0);
        termination_test->new_step();
        do {
            termination_test->new_iteration();
            termination_test->new_evaluation();
            // Compute the director
            b2linalg::Matrix<T, b2linalg::Mrectangle> nks_r;
            try {
                if (refactorization > 0 || method == conventional || hard_conv || nb_divergence > 2
                    || nb_iter == 0) {
                    if (refactorization == 2 || method == conventional || hard_conv
                        || nb_divergence > 2) {
                        solver_hints.clear();
                        residue_function.get_residue(
                              d, time, constraint.get_delta_time(), equilibrium_solution,
                              b2linalg::Vector<T, b2linalg::Vdense_ref>::null, K, q, &solver_hints,
                              0);
                        constraint.get_constraint(d, time, &r[nb_dof], &c_d, &c_c);
                        refactorization = 2;
                    }
                    CorrectionStrategy<T, MATRIX_FORMAT>::logger
                          << logging::debug
                          << "Compute correction and factorize "
                             "tangent stiffness matrix ("
                          << K.size1() << " rows and columns)." << logging::LOGFLUSH;
                    director = update_extension_and_inverse(K, q, c_d, c_c, nks_r, nbnsv) * r;
                } else {
                    director = inverse(K, nks_r, nbnsv) * r;
                }
            } catch (b2linalg::SingularMatrixError& e) {
                if (nks_r.size2() != 0) {
                    b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                          model.get_domain().get_number_of_dof(), nks_r.size2());
                    for (size_t i = 0; i != nks_r.size2(); ++i) {
                        residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                    }
                    residue_function.get_model().get_solution().set_system_null_space(nks);
                }
                throw;
            }
            if (nks_r.size2() != 0) {
                b2linalg::Matrix<T, b2linalg::Mrectangle> nks(
                      model.get_domain().get_number_of_dof(), nks_r.size2());
                for (size_t i = 0; i != nks_r.size2(); ++i) {
                    residue_function.get_solution(nks_r[i], time, equilibrium_solution, nks[i]);
                }
                residue_function.get_model().get_solution().set_system_null_space(nks);
            }
 
            d_test = d - director(residu_int);
            time_test = time - director[nb_dof];
 
            solver_hints.clear();
            residue_function.get_residue(
                  d_test, time_test, constraint.get_delta_time(), equilibrium_solution,
                  r_test(residu_int),
                  b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, &solver_hints,
                  termination_test->need_flux() ? termination_test : 0);
            constraint.get_constraint(d_test, time_test, &r_test[nb_dof], 0, 0);
 
            double r1_norm = 0;
            for (size_t i = 0; i != d_test.size(); ++i) {
                r1_norm += norm(r_test[i] * r_test[i] * d_test[i]);
            }
            r1_norm += norm(r_test[nb_dof] * r_test[nb_dof] * time_test);
            double rh_norm = r1_norm;
 
            if (r1_norm > r0_norm + 1e-5) {
                b2linalg::Vector<T> r_tmp = r_test;
                // linear search
                solver_hints.clear();
                Function f(
                      residue_function, constraint, director, d, d_test, r_test, time, time_test,
                      rh_norm, &solver_hints);
                h = line_search->get_minimum(f, r0_norm, r1_norm);
 
                // d_test, time_test and rh_norm is set for h_aprox_min
 
                CorrectionStrategy<T, MATRIX_FORMAT>::logger
                      << logging::debug << "line search h = " << h << logging::LOGFLUSH;
 
                if (h < 0.1) {
                    if (refactorization == 2) {
                        h = 1.0;
                        d_test = d - director(residu_int);
                        r_test.swap(r_tmp);
                        time_test = time - director[nb_dof];
                        rh_norm = r1_norm;
                        result = CorrectionTerminationTest<T>::unfinished;
                        hard_conv = true;
                    } else {
                        // refactorization
                        refactorization = 2;
                        result = CorrectionTerminationTest<T>::unfinished;
                        continue;
                    }
                } else {
                    refactorization = 0;
                }
            } else {
                refactorization = 0;
                h = 1;
            }
            r0_norm = rh_norm;
            r.swap(r_test);
            d.swap(d_test);
            time = time_test;
 
            if (*solver_hints & SolverHints::degradation) { has_degradation = true; }
            if (!has_degradation) { nb_iter_first++; }
 
            result = termination_test->is_terminated(
                  ++nb_iter, r(residu_int), director(residu_int), h, d, d_old, r[nb_dof], c_c,
                  director[nb_dof] * h, time, residue_function,
                  residue_function.get_matrix_diagonal(K),
                  equilibrium_solution.distance_to_iterative_convergence, &solver_hints);
            nb_divergence = termination_test->get_nb_divergence();
 
            if (CorrectionStrategy<T, MATRIX_FORMAT>::logger.is_enabled_for(logging::data)) {
                Model& model = residue_function.get_model();
                b2linalg::Vector<T, b2linalg::Vdense> g(model.get_domain().get_number_of_dof());
                residue_function.get_solution(d, time, equilibrium_solution, g);
                SolutionStaticNonlinear<T>& solution =
                      dynamic_cast<SolutionStaticNonlinear<T>&>(model.get_solution());
                solution.set_dof(
                      time, g, residue_function.get_nbc_sol(),
                      residue_function.get_residuum_sol(d, time), true);
            }
        } while (result == CorrectionTerminationTest<T>::unfinished);
 
        if (result == CorrectionTerminationTest<T>::converged) {
            termination_test->converged_step();
        }
        return constraint.set_correction_result(
              d, time, result == CorrectionTerminationTest<T>::converged, nb_iter_first);
    }
 
    using type_t = ObjectTypeComplete<
          AcceleratedNewtonMethod, typename CorrectionStrategy<T, MATRIX_FORMAT>::type_t>;
    static type_t type;
 
protected:
    struct Function : public LineSearch::Function {
        Function(
              StaticResidueFunction<T, MATRIX_FORMAT>& residue_function_,
              IncrementConstraintStrategy<T, MATRIX_FORMAT>& constraint_,
              b2linalg::Vector<T, b2linalg::Vdense>& director_,
              b2linalg::Vector<T, b2linalg::Vdense>& d_,
              b2linalg::Vector<T, b2linalg::Vdense>& d_test_,
              b2linalg::Vector<T, b2linalg::Vdense>& r_test_, double time_, double& time_test_,
              double& rh_norm_, SolverHints* solver_hints_)
            : residue_function(residue_function_),
              constraint(constraint_),
              director(director_),
              d(d_),
              d_test(d_test_),
              r_test(r_test_),
              time(time_),
              time_test(time_test_),
              rh_norm(rh_norm_),
              solver_hints(solver_hints_) {}
 
        double operator()(double h) override {
            size_t nb_dof = r_test.size() - 1;
            b2linalg::Interval residu_int(0, r_test.size() - 1);
            d_test = d - h * director(residu_int);
            time_test = time - h * director[nb_dof];
            EquilibriumSolution es(1, -1);
            residue_function.get_residue(
                  d_test, time_test, constraint.get_delta_time(), es, r_test(residu_int),
                  b2linalg::Matrix<T, typename MATRIX_FORMAT::sparse_inversible_ext>::null,
                  b2linalg::Vector<T, b2linalg::Vdense_ref>::null, solver_hints);
            constraint.get_constraint(d_test, time_test, &r_test[nb_dof], 0, 0);
            rh_norm = 0;
            for (size_t i = 0; i != d_test.size(); ++i) {
                rh_norm += norm(r_test[i] * r_test[i] * d_test[i]);
            }
            rh_norm += norm(r_test[nb_dof] * r_test[nb_dof] * time_test);
            return rh_norm;
        }
 
        StaticResidueFunction<T, MATRIX_FORMAT>& residue_function;
        IncrementConstraintStrategy<T, MATRIX_FORMAT>& constraint;
        b2linalg::Vector<T, b2linalg::Vdense>& director;
        b2linalg::Vector<T, b2linalg::Vdense>& d;
        b2linalg::Vector<T, b2linalg::Vdense>& d_test;
        b2linalg::Vector<T, b2linalg::Vdense>& r_test;
        double time;
        double& time_test;
        double& rh_norm;
        SolverHints* solver_hints;
    };
 
    CorrectionTerminationTest<T>* termination_test;
    LineSearch* line_search;
    Allocator allocator;
    Method method;
};
 
template <typename T, typename MATRIX_FORMAT>
typename AcceleratedNewtonMethod<T, MATRIX_FORMAT>::type_t
      AcceleratedNewtonMethod<T, MATRIX_FORMAT>::type(
            "AcceleratedNewtonMethod", type_name<T, MATRIX_FORMAT>(),
            StringList("ACCELERATED_NEWTON_METHOD"), module);
 
template <typename T>
class DofAndResidueCorrectionTerminationTest : public CorrectionTerminationTest<T> {
public:
    void init(Model& model, const AttributeList& attribute) {
        e_r = attribute.get_double("TOL_RESIDUUM", 1e-3);
        e_d = attribute.get_double("TOL_SOLUTION", 1e-3);
        g_r = std::max(e_r, attribute.get_double("MAX_RESIDUUM", 1.0e8));
        g_d = std::max(e_d, attribute.get_double("MAX_SOLUTION", 1.0e8));
        max_number_of_iterations = attribute.get_int("MAX_NEWTON_ITERATIONS", 50);
        max_additional_iterations = 0;
        max_number_of_divergences = attribute.get_int("MAX_DIVERGENCES", 4);
        number_of_divergences = 0;
        last_rn = std::numeric_limits<double>::max();
        best_rn = std::numeric_limits<double>::max();
    }
 
    typename CorrectionTerminationTest<T>::Termination is_terminated(
          int number_of_iteration, const b2linalg::Vector<T, b2linalg::Vdense_constref>& res,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& delta_dof,
          const double delta_dof_scale, const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof_at_start_step,
          const double constraint, const double d_constraint_d_time, const double delta_time,
          const double time, StaticResidueFunctionForTerminationTest<T>& residue,
          const b2linalg::Vector<T, b2linalg::Vindex1_constref>& diag_K,
          double& distance_to_iterative_convergence, const SolverHints* solver_hints) {
        double r = res.norm2();
        double d = delta_dof.norm2() * delta_dof_scale;
        if (number_of_iteration == 1) {
            max_additional_iterations = 0;
            number_of_divergences = 0;
            last_rn = std::numeric_limits<double>::max();
            best_rn = std::numeric_limits<double>::max();
            return CorrectionTerminationTest<T>::unfinished;
        }
        double rn = norm(r);
        double dn = norm(d);
        CorrectionTerminationTest<T>::logger
              << logging::debug << "termination test: residue=" << rn << ", last correction=" << dn
              << ", number of correction iterations=" << number_of_iteration;
        if (rn != rn) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the residue is not a number." << logging::LOGFLUSH;
            distance_to_iterative_convergence = -1;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (dn != dn) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the last correction is not a number." << logging::LOGFLUSH;
            distance_to_iterative_convergence = -1;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::diverge)) {
            distance_to_iterative_convergence = -1;
            CorrectionTerminationTest<T>::logger << ", elements/materials/boundary conditions "
                                                    "indicated divergence."
                                                 << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        distance_to_iterative_convergence = std::max(rn / e_r, dn / e_d);
        if ((rn < e_r) && (dn < e_d)
            && (solver_hints == nullptr || **solver_hints == SolverHints::none)) {
            CorrectionTerminationTest<T>::logger << ", converged." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::converged;
        }
        if (number_of_iteration > 1 && rn > g_r) {
            CorrectionTerminationTest<T>::logger << ", diverged because the residue exceeds the "
                                                    "maximum ("
                                                 << g_r << ")." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (number_of_iteration > 1 && dn > g_d) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the last correction exceeds "
                     "the maximum ("
                  << g_d << ")." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
            const int remaining = std::max(
                  0, max_number_of_iterations + max_additional_iterations - number_of_iteration);
            max_additional_iterations += std::max(0, max_number_of_iterations - remaining);
            number_of_divergences = 0;
            last_rn = std::numeric_limits<double>::max();
            best_rn = std::numeric_limits<double>::max();
        } else if (
              rn > last_rn
              || (number_of_iteration + max_number_of_divergences > 5 && rn > best_rn)) {
            if (++number_of_divergences > max_number_of_divergences) {
                CorrectionTerminationTest<T>::logger
                      << logging::debug
                      << ", diverged because the maximum number of consecutive "
                         "divergences ("
                      << max_number_of_divergences << ") has been exceeded (last=" << last_rn
                      << ", best=" << best_rn << ")." << logging::LOGFLUSH;
                distance_to_iterative_convergence = -1;
                return CorrectionTerminationTest<T>::diverged;
            }
            last_rn = rn;
            best_rn = std::min(best_rn, rn);
        } else {
            number_of_divergences = 0;
            last_rn = rn;
            best_rn = std::min(best_rn, rn);
        }
 
        if (number_of_iteration > max_number_of_iterations + max_additional_iterations) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the number of correction iterations "
                     "exceeds the maximum ("
                  << max_number_of_iterations;
            if (max_additional_iterations > 0) {
                CorrectionTerminationTest<T>::logger << "+" << max_additional_iterations;
            }
            CorrectionTerminationTest<T>::logger << ")." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::max_iteration;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
            CorrectionTerminationTest<T>::logger
                  << ", elements/materials/boundary conditions indicated "
                     "degradation";
        } else if (
              distance_to_iterative_convergence >= 0.0 && distance_to_iterative_convergence <= 1.0
              && solver_hints != nullptr && (**solver_hints & SolverHints::unfinished)) {
            CorrectionTerminationTest<T>::logger
                  << ", elements/materials/boundary conditions indicated "
                     "need for further Newton iterations";
        }
        CorrectionTerminationTest<T>::logger << ", unfinished." << logging::LOGFLUSH;
        return CorrectionTerminationTest<T>::unfinished;
    }
 
    using type_t = ObjectTypeComplete<
          DofAndResidueCorrectionTerminationTest, typename CorrectionTerminationTest<T>::type_t>;
    static type_t type;
 
private:
    double e_r;
    double e_d;
    double g_r;
    double g_d;
    double last_rn;
    double best_rn;
    int max_number_of_iterations;
    int max_additional_iterations;
    int number_of_divergences;
    int max_number_of_divergences;
};
 
template <typename T>
typename DofAndResidueCorrectionTerminationTest<T>::type_t
      DofAndResidueCorrectionTerminationTest<T>::type(
            "DofAndResidueCorrectionTerminationTest", type_name<T>(),
            StringList("DOF_AND_RESIDUE_CORRECTION_TERMINATION_TEST"), module);
 
template <typename T>
class NormalizedDofAndResidueCorrectionTerminationTest : public CorrectionTerminationTest<T> {
public:
    void init(Model& model, const AttributeList& attribute) {
        e_r = attribute.get_double("TOL_RESIDUUM", 1e-3);
        e_r *= e_r;
        e_d = attribute.get_double("TOL_SOLUTION", 1e-3);
        e_d *= e_d;
        g_r = std::max(e_r, attribute.get_double("MAX_RESIDUUM", 10.0e10));
        g_d = std::max(e_d, attribute.get_double("MAX_SOLUTION", 10.0e10));
        max_number_of_iteration = attribute.get_int("MAX_NEWTON_ITERATIONS", 50);
        max_additional_iterations = 0;
        max_number_of_divergences = attribute.get_int("MAX_DIVERGENCES", 4);
        number_of_divergences = 0;
        last_rn = std::numeric_limits<double>::max();
        best_rn = std::numeric_limits<double>::max();
    }
 
    typename CorrectionTerminationTest<T>::Termination is_terminated(
          int number_of_iteration, const b2linalg::Vector<T, b2linalg::Vdense_constref>& res,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& delta_dof,
          const double delta_dof_scale, const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof_at_start_step,
          const double constraint, const double d_constraint_d_time, const double delta_time,
          const double time, StaticResidueFunctionForTerminationTest<T>& residue,
          const b2linalg::Vector<T, b2linalg::Vindex1_constref>& diag_K,
          double& distance_to_iterative_convergence, const SolverHints* solver_hints) {
        double r = res.norm2();
        double d = delta_dof.norm2() * delta_dof_scale;
        if (number_of_iteration == 1) {
            r_0 = r;
            d_0 = d;
            number_of_divergences = 0;
            last_rn = std::numeric_limits<double>::max();
            best_rn = std::numeric_limits<double>::max();
            return CorrectionTerminationTest<T>::unfinished;
        }
        double rn;
        if (norm(r_0) < 10e-15) {
            rn = 0;
        } else {
            rn = norm(r) / norm(r_0);
        }
        double dn;
        if (norm(d) < 10e-15) {
            dn = 0;
        } else {
            dn = norm(d) / norm(d_0);
        }
        CorrectionTerminationTest<T>::logger
              << logging::debug << "termination test: residue=" << r
              << ", normalised residue (scale=" << r_0 << ")=" << rn << ", last correction=" << d
              << ", normalised last correction (scale=" << d_0 << ")=" << dn
              << ", number of correction iterations=" << number_of_iteration;
        if (rn != rn) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the normalised residue is not a "
                     "number."
                  << logging::LOGFLUSH;
            distance_to_iterative_convergence = -1;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (dn != dn) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the normalised last correction is not "
                     "a number."
                  << logging::LOGFLUSH;
            distance_to_iterative_convergence = -1;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::diverge)) {
            distance_to_iterative_convergence = -1;
            CorrectionTerminationTest<T>::logger << ", elements/materials/boundary conditions "
                                                    "indicated divergence."
                                                 << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        distance_to_iterative_convergence = std::max(rn / e_r, dn / e_d);
        if ((rn < e_r) && (dn < e_d)
            && (solver_hints == nullptr || **solver_hints == SolverHints::none)) {
            CorrectionTerminationTest<T>::logger << ", converged." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::converged;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
            const int remaining = std::max(
                  0, max_number_of_iteration + max_additional_iterations - number_of_iteration);
            max_additional_iterations += std::max(0, max_number_of_iteration - remaining);
            number_of_divergences = 0;
            last_rn = std::numeric_limits<double>::max();
            best_rn = std::numeric_limits<double>::max();
        } else if ((number_of_iteration > 2 && rn > last_rn)) {
            if (++number_of_divergences > max_number_of_divergences) {
                CorrectionTerminationTest<T>::logger
                      << logging::debug
                      << ", diverged because the maximum number of consecutive "
                         "divergences ("
                      << max_number_of_divergences << ") has been exceeded." << logging::LOGFLUSH;
                distance_to_iterative_convergence = -1;
                return CorrectionTerminationTest<T>::diverged;
            }
            last_rn = rn;
            best_rn = std::min(best_rn, rn);
        } else {
            number_of_divergences = 0;
            last_rn = rn;
            best_rn = std::min(best_rn, rn);
        }
 
        if (number_of_iteration > max_number_of_iteration + max_additional_iterations) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the number of correction iterations "
                     "exceeds the maximum ("
                  << max_number_of_iteration;
            if (max_additional_iterations > 0) {
                CorrectionTerminationTest<T>::logger << "+" << max_additional_iterations;
            }
            CorrectionTerminationTest<T>::logger << ")." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::max_iteration;
        }
        if (number_of_iteration > 1 && rn > g_r) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the normalised residue exceeds the "
                     "maximum ("
                  << g_r << ")." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (number_of_iteration > 1 && dn > g_d) {
            CorrectionTerminationTest<T>::logger
                  << ", diverged because the normalised last correction exceeds "
                     "the maximum ("
                  << g_d << ")." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
            CorrectionTerminationTest<T>::logger
                  << ", elements/materials/boundary conditions indicated "
                     "degradation";
        } else if (
              distance_to_iterative_convergence >= 0.0 && distance_to_iterative_convergence <= 1.0
              && solver_hints != nullptr && (**solver_hints & SolverHints::unfinished)) {
            CorrectionTerminationTest<T>::logger
                  << ", elements/materials/boundary conditions indicated "
                     "need for further Newton iterations";
        }
        CorrectionTerminationTest<T>::logger << ", unfinished." << logging::LOGFLUSH;
        return CorrectionTerminationTest<T>::unfinished;
    }
 
    using type_t = ObjectTypeComplete<
          NormalizedDofAndResidueCorrectionTerminationTest,
          typename CorrectionTerminationTest<T>::type_t>;
    static type_t type;
 
private:
    double e_r;
    double e_d;
    double g_r;
    double g_d;
    double r_0;
    double d_0;
    double last_rn;
    double best_rn;
    int max_number_of_iteration;
    int max_additional_iterations;
    int number_of_divergences;
    int max_number_of_divergences;
};
 
template <typename T>
typename NormalizedDofAndResidueCorrectionTerminationTest<T>::type_t
      NormalizedDofAndResidueCorrectionTerminationTest<T>::type(
            "NormalizedDofAndResidueCorrectionTerminationTest", type_name<T>(),
            StringList("NORMALISED_DOF_AND_RESIDUE_CORRECTION_TERMINATION_TEST"), module);
 
template <typename T>
class RegularizedCorrectionTerminationTest : public CorrectionTerminationTest<T> {
public:
    void init(Model& model, const AttributeList& attribute) {
        e_r = attribute.get_double("TOL_RESIDUUM", 1e-3);
        e_d = attribute.get_double("TOL_SOLUTION", 1e-3);
        e_w = attribute.get_double("TOL_WORK", 1e-7);
        max_number_of_iteration = attribute.get_int("MAX_NEWTON_ITERATIONS", 50);
        max_divergence = attribute.get_int("MAX_DIVERGENCES", 4);
        max_additional_iterations = 0;
    }
 
    double norm_termination(
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& r,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& delta_dof,
          const double delta_dof_scale, const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof,
          const double constraint, const double d_constraint_d_time, const double delta_time,
          const double time, StaticResidueFunctionForTerminationTest<T>& residue,
          const b2linalg::Vector<T, b2linalg::Vindex1_constref>& diag_K) {
        const double w_normalization = residue.get_energy_normalisation1();
        double w = 0;
        double p = 0;
        double norm1_d_dof = 0;
        double diag_d_dof = 0;
        double diag_dof = 0;
        for (size_t i = 0; i != delta_dof.size(); ++i) {
            const double ddof = delta_dof[i] * delta_dof_scale;
            w += norm(r[i] * ddof);
            p += norm(r[i] * dof[i]);
            norm1_d_dof += norm(ddof);
            const double sqrt_diag_K = std::sqrt(norm(diag_K[i]));
            diag_d_dof += norm(sqrt_diag_K * ddof);
            diag_dof += norm(sqrt_diag_K * dof[i]);
        }
        w += norm(constraint * delta_time);
        p += norm(constraint * time);
        norm1_d_dof += norm(delta_time);
        diag_d_dof += norm(std::sqrt(norm(d_constraint_d_time)) * delta_time);
        diag_dof += norm(std::sqrt(norm(d_constraint_d_time)) * time);
        double q;
        if (last_norm1_d_dof < 1e-15) {
            q = 0;
        } else {
            q = 2 * norm1_d_dof / (3 * last_norm1_d_dof) + last_q / 3;
        }
        double e_dof;
        if (diag_dof < 1e-15) {
            e_dof = 0;
        } else if (q >= 0.99) {
            e_dof = q * diag_d_dof / (0.01 * diag_dof);
        } else {
            e_dof = q * diag_d_dof / ((1 - q) * diag_dof);
        }
        e_dof /= e_d;
        const double e_work = w / (w_normalization * e_w);
        const double e_res = p / (w_normalization * e_r);
 
        return std::max(e_work, std::max(e_res, e_dof));
    }
 
    typename CorrectionTerminationTest<T>::Termination is_terminated(
          int number_of_iteration, const b2linalg::Vector<T, b2linalg::Vdense_constref>& r,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& delta_dof,
          const double delta_dof_scale, const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof_at_start_step,
          const double constraint, const double d_constraint_d_time, const double delta_time,
          const double time, StaticResidueFunctionForTerminationTest<T>& residue,
          const b2linalg::Vector<T, b2linalg::Vindex1_constref>& diag_K,
          double& distance_to_iterative_convergence, const SolverHints* solver_hints) {
        double w2 = 0;
        for (size_t i = 0; i != dof.size(); ++i) { w2 += norm(r[i] * r[i] * dof[i]); }
        w2 += norm(constraint * constraint * time);
        if (w2 != w2) {
            CorrectionTerminationTest<T>::logger
                  << logging::debug
                  << "diverged because a term of the residue or the correction is "
                     "not a number."
                  << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::diverge)) {
            distance_to_iterative_convergence = -1;
            CorrectionTerminationTest<T>::logger << logging::info
                                                 << "elements/materials/boundary conditions "
                                                 << "indicated divergence." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (number_of_iteration == 1) {
            last_q = 0.99;
            last_w2 = w2;
            nb_divergence = 0;
            last_norm1_d_dof = 0;
            for (size_t i = 0; i != delta_dof.size(); ++i) {
                last_norm1_d_dof += norm(delta_dof[i] * delta_dof_scale);
            }
            distance_to_iterative_convergence = -1;
            return CorrectionTerminationTest<T>::unfinished;
        }
        {
            const double divergence = w2 == 0 ? 0 : (last_w2 == 0 ? w2 : w2 / last_w2);
            if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
                nb_divergence = 0;
            } else if (divergence > 1 + 1e-5 || divergence < -1e12) {
                nb_divergence += 2;
            } else if (divergence < -1) {
                nb_divergence += 1;
            } else {
                nb_divergence = 0;
            }
            // std::cout << "w2=" << w2 << ", divergence=" << divergence << ", nb_divergence=" <<
            // nb_divergence << std::endl;
            if (nb_divergence > max_divergence) {
                CorrectionTerminationTest<T>::logger
                      << logging::debug
                      << "diverged because the maximum number of consecutive "
                         "divergences ("
                      << max_divergence << ") has been exceeded." << logging::LOGFLUSH;
                distance_to_iterative_convergence = -1;
                return CorrectionTerminationTest<T>::diverged;
            }
            last_w2 = w2;
        }
        const double w_normalization = residue.get_energy_normalisation1();
        double w = 0;
        double p = 0;
        double norm1_d_dof = 0;
        double diag_d_dof = 0;
        double diag_dof = 0;
        for (size_t i = 0; i != delta_dof.size(); ++i) {
            const double ddof = delta_dof[i] * delta_dof_scale;
            w += norm(r[i] * ddof);
            p += norm(r[i] * dof[i]);
            norm1_d_dof += norm(ddof);
            const double sqrt_diag_K = std::sqrt(norm(diag_K[i]));
            diag_d_dof += norm(sqrt_diag_K * ddof);
            diag_dof += norm(sqrt_diag_K * dof[i]);
        }
        w += norm(constraint * delta_time);
        p += norm(constraint * time);
        norm1_d_dof += norm(delta_time);
        diag_d_dof += norm(std::sqrt(norm(d_constraint_d_time)) * delta_time);
        diag_dof += norm(std::sqrt(norm(d_constraint_d_time)) * time);
        double q;
        if (last_norm1_d_dof < 1e-15) {
            q = 0;
        } else {
            q = 2 * norm1_d_dof / (3 * last_norm1_d_dof) + last_q / 3;
        }
        double e_dof;
        if (diag_dof < 1e-15) {
            e_dof = 0;
        } else if (q >= 0.99) {
            e_dof = q * diag_d_dof / (0.01 * diag_dof);
        } else {
            e_dof = q * diag_d_dof / ((1 - q) * diag_dof);
        }
        const double e_work = w / w_normalization;
        const double e_res = p / w_normalization;
        distance_to_iterative_convergence =
              std::max(e_work / e_w, std::max(e_res / e_r, e_dof / e_d));
        if (e_work < e_w && e_res < e_r && e_dof < e_d
            && (solver_hints == nullptr || **solver_hints == SolverHints::none)) {
            CorrectionTerminationTest<T>::logger << logging::debug << "converged."
                                                 << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::converged;
        }
        if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
            const int remaining = std::max(
                  0, max_number_of_iteration + max_additional_iterations - number_of_iteration);
            max_additional_iterations += std::max(0, max_number_of_iteration - remaining);
        }
        if (number_of_iteration > max_number_of_iteration + max_additional_iterations) {
            CorrectionTerminationTest<T>::logger
                  << logging::debug
                  << "diverged because the number of correction iterations "
                     "exceeds the maximum ("
                  << max_number_of_iteration;
            if (max_additional_iterations > 0) {
                CorrectionTerminationTest<T>::logger << "+" << max_additional_iterations;
            }
            CorrectionTerminationTest<T>::logger << ")." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::max_iteration;
        }
 
        last_norm1_d_dof = norm1_d_dof;
        last_q = q;
        if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
            CorrectionTerminationTest<T>::logger
                  << logging::info
                  << "elements/materials/boundary conditions indicated "
                     "degradation, ";
        } else if (
              distance_to_iterative_convergence >= 0.0 && distance_to_iterative_convergence <= 1.0
              && solver_hints != nullptr && (**solver_hints & SolverHints::unfinished)) {
            CorrectionTerminationTest<T>::logger
                  << logging::info
                  << "elements/materials/boundary conditions indicated "
                     "need for further Newton iterations, ";
        }
        CorrectionTerminationTest<T>::logger
              << logging::debug << "unfinished, iter=" << number_of_iteration
              << ", e_work=" << e_work << ", e_residue=" << e_res << ", e_dof=" << e_dof << "."
              << logging::LOGFLUSH;
        return CorrectionTerminationTest<T>::unfinished;
    }
 
    int get_nb_divergence() const { return nb_divergence; }
 
    using type_t = ObjectTypeComplete<
          RegularizedCorrectionTerminationTest, typename CorrectionTerminationTest<T>::type_t>;
    static type_t type;
 
private:
    double e_r;
    double e_d;
    double e_w;
    double last_w2;
    double last_norm1_d_dof;
    double last_q;
    int nb_divergence;
    int max_divergence;
    int max_number_of_iteration;
    int max_additional_iterations;
};
 
template <typename T>
typename RegularizedCorrectionTerminationTest<T>::type_t
      RegularizedCorrectionTerminationTest<T>::type(
            "RegularizedCorrectionTerminationTest", type_name<T>(),
            StringList("REGULARISED_CORRECTION_TERMINATION_TEST"), module);
 
template <typename T>
class FluxNormalizedCorrectionTerminationTest : public CorrectionTerminationTest<T> {
public:
    void init(Model& model, const AttributeList& attribute) {
        min_number_of_iteration = attribute.get_int("MIN_NEWTON_ITERATIONS", 0);
        max_number_of_iteration = attribute.get_int("MAX_NEWTON_ITERATIONS", 50);
        max_additional_iteration = 0;
        max_divergence = attribute.get_int("MAX_DIVERGENCES", 4);
        I0 = attribute.get_int("MIN_NEWTON_ITERATIONS_CHECK_DIVERGENCE", 4);
        I0_effective = I0;
        tol_work_lagrange = attribute.get_double(
              "TOL_WORK.LAGRANGE_MULTIPLIER",
              attribute.get_double("TOL_WORK.LAGRANGE", attribute.get_double("TOL_WORK", 1e-7)));
        const std::pair<std::map<std::string, size_t>, b2linalg::Index>& df =
              model.get_domain().get_dof_field();
        df_index = &df.second;
 
        size_t s = 0;
        for (std::map<std::string, size_t>::const_iterator i = df.first.begin();
             i != df.first.end(); ++i) {
            s = std::max(s, i->second);
        }
 
        list_field.resize(s + 1);
        for (std::map<std::string, size_t>::const_iterator i = df.first.begin();
             i != df.first.end(); ++i) {
            list_field[i->second].init(i->first, attribute);
        }
    }
 
    void new_step() {
        for (typename std::vector<Field>::iterator i = list_field.begin(); i != list_field.end();
             ++i) {
            i->new_step();
        }
    }
 
    void new_iteration() {
        for (typename std::vector<Field>::iterator i = list_field.begin(); i != list_field.end();
             ++i) {
            i->new_iteration();
        }
    }
 
    void new_evaluation() {
        for (typename std::vector<Field>::iterator i = list_field.begin(); i != list_field.end();
             ++i) {
            i->new_evaluation();
        }
    }
 
    void converged_step() {
        for (typename std::vector<Field>::iterator i = list_field.begin(); i != list_field.end();
             ++i) {
            i->converged_step();
        }
    }
 
    bool need_flux() { return true; }
 
    void add_flux(b2linalg::Index& index, const b2linalg::Vector<T, b2linalg::Vdense>& v) {
        for (size_t i = 0; i != index.size(); ++i) {
            list_field[(*df_index)[index[i]]].add_flux(norm(v[i]));
        }
    }
 
    void add_flux(const b2linalg::Vector<T, b2linalg::Vdense_constref>& v) {
        for (size_t i = 0; i != v.size(); ++i) { list_field[(*df_index)[i]].add_flux(norm(v[i])); }
    }
 
    typename CorrectionTerminationTest<T>::Termination is_terminated(
          int number_of_iteration, const b2linalg::Vector<T, b2linalg::Vdense_constref>& r,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& delta_dof,
          const double delta_dof_scale, const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof,
          const b2linalg::Vector<T, b2linalg::Vdense_constref>& dof_at_start_step,
          const double constraint, const double d_constraint_d_time, const double delta_time,
          const double time, StaticResidueFunctionForTerminationTest<T>& residue,
          const b2linalg::Vector<T, b2linalg::Vindex1_constref>& diag_K,
          double& distance_to_iterative_convergence, const SolverHints* solver_hints) {
        distance_to_iterative_convergence = 0;
        // TODO optimise to not copy d_value_d_dof_trans each time
        b2linalg::Matrix<T, b2linalg::Mcompressed_col> d_value_d_dof_trans;
        residue.mrbc_get_nonlinear_value(
              dof, time, false, b2linalg::Vector<T, b2linalg::Vdense_ref>::null,
              d_value_d_dof_trans, b2linalg::Vector<T, b2linalg::Vdense_ref>::null, 0);
        b2linalg::Vector<T> tmp(d_value_d_dof_trans.size2());
        b2linalg::Interval dof_int(0, d_value_d_dof_trans.size1());
 
        tmp = transposed(d_value_d_dof_trans) * r(dof_int);
        std::vector<T> r_max(list_field.size());
        for (size_t i = 0; i != df_index->size(); ++i) {
            const size_t j = (*df_index)[i];
            if (r_max[j] == r_max[j]) {
                if (tmp[i] == tmp[i]) {
                    r_max[j] = std::max(r_max[j], norm(tmp[i]));
                } else {
                    r_max[j] = norm(tmp[i]);
                }
            }
        }
 
        tmp = transposed(d_value_d_dof_trans) * delta_dof(dof_int);
        std::vector<T> delta_dof_max(list_field.size());
        for (size_t i = 0; i != df_index->size(); ++i) {
            const size_t j = (*df_index)[i];
            if (delta_dof_max[j] == delta_dof_max[j]) {
                if (tmp[i] == tmp[i]) {
                    delta_dof_max[j] = std::max(delta_dof_max[j], norm(tmp[i]));
                } else {
                    delta_dof_max[j] = norm(tmp[i]);
                }
            }
        }
 
        tmp = transposed(d_value_d_dof_trans) * dof_at_start_step(dof_int);
        std::vector<T> step_dof_max(list_field.size());
        for (size_t i = 0; i != df_index->size(); ++i) {
            const size_t j = (*df_index)[i];
            if (step_dof_max[j] == step_dof_max[j]) {
                if (tmp[i] == tmp[i]) {
                    step_dof_max[j] = std::max(step_dof_max[j], norm(tmp[i]));
                } else {
                    step_dof_max[j] = norm(tmp[i]);
                }
            }
        }
 
        bool degradation = false;
        bool diverged = false;
        bool unfinished = false;
 
        // Check for solver hints and do logging.
        CorrectionTerminationTest<T>::logger
              << logging::debug << "Number of correction iterations = " << number_of_iteration;
        if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
            degradation = unfinished = true;
 
            // Allow more iterations.
            const int remaining = std::max(
                  0, max_number_of_iteration + max_additional_iteration - number_of_iteration);
            max_additional_iteration += std::max(0, max_number_of_iteration - remaining);
 
            // Divergence check resumes in I0 iterations.
            I0_effective = I0 + number_of_iteration;
 
            CorrectionTerminationTest<T>::logger
                  << ", elements/materials/boundary conditions indicated "
                     "degradation";
        } else if (solver_hints != nullptr && (**solver_hints & SolverHints::unfinished)) {
            unfinished = true;
            CorrectionTerminationTest<T>::logger
                  << ", elements/materials/boundary conditions indicated "
                     "need for further Newton iterations";
        }
        CorrectionTerminationTest<T>::logger << "." << logging::LOGFLUSH;
 
        // Check convergence of the fields.
        for (size_t i = 0; i != list_field.size(); ++i) {
            if (!list_field[i].is_init()) { continue; }
            typename CorrectionTerminationTest<T>::Termination r = list_field[i].is_terminated(
                  *this, number_of_iteration, r_max[i], step_dof_max[i],
                  delta_dof_max[i] * delta_dof_scale, distance_to_iterative_convergence,
                  solver_hints);
            if (!degradation && r == CorrectionTerminationTest<T>::diverged) { diverged = true; }
            if (r == CorrectionTerminationTest<T>::unfinished) { unfinished = true; }
        }
 
        // check the work one the lagrange multiplier
        if (residue.get_number_of_non_lagrange_dof() != dof.size()) {
            double max_l_work = 0;
            for (size_t i = residue.get_number_of_non_lagrange_dof(); i != dof.size(); ++i) {
                max_l_work = std::max(max_l_work, norm(r[i] * delta_dof[i]));
            }
            max_l_work *= delta_dof_scale;
            CorrectionTerminationTest<T>::logger
                  << logging::debug << "Field Lagrange multiplier, work_max = " << max_l_work;
            if (max_l_work > tol_work_lagrange) {
                unfinished = true;
                CorrectionTerminationTest<T>::logger << ", unfinished.";
            } else {
                CorrectionTerminationTest<T>::logger << ", converged.";
            }
            CorrectionTerminationTest<T>::logger << logging::LOGFLUSH;
            distance_to_iterative_convergence =
                  std::max(distance_to_iterative_convergence, max_l_work / tol_work_lagrange);
        }
 
        if (diverged) {
            distance_to_iterative_convergence = -1;
            return CorrectionTerminationTest<T>::diverged;
        }
        if (!unfinished && number_of_iteration < min_number_of_iteration) { unfinished = true; }
        if (unfinished) {
            if (number_of_iteration > max_number_of_iteration + max_additional_iteration) {
                CorrectionTerminationTest<T>::logger
                      << logging::debug
                      << "diverged because the number of correction iterations "
                         "exceeds the maximum ("
                      << max_number_of_iteration;
                if (max_additional_iteration > 0) {
                    CorrectionTerminationTest<T>::logger << "+" << max_additional_iteration;
                }
                CorrectionTerminationTest<T>::logger << ")." << logging::LOGFLUSH;
                return CorrectionTerminationTest<T>::diverged;
            }
            return CorrectionTerminationTest<T>::unfinished;
        }
        return CorrectionTerminationTest<T>::converged;
    }
 
    int get_nb_divergence() const { return 0; }
 
    using type_t = ObjectTypeComplete<
          FluxNormalizedCorrectionTerminationTest, typename CorrectionTerminationTest<T>::type_t>;
    static type_t type;
 
private:
    int I0;
    int I0_effective;
    int min_number_of_iteration;
    int max_number_of_iteration;
    int max_additional_iteration;
    int max_divergence;
    double tol_work_lagrange;
    const b2linalg::Index* df_index;
 
    class Field {
    public:
        Field()
            : is_init_(false),
              Rn(0),
              Rm(0),
              Rp(0),
              Ip(0),
              Rl(0),
              Cn(0),
              Cm(0),
              q_averaged_0(0),
              q_averaged_n(0),
              epsilon(0),
              epsilon_l(0),
              Cepsilon(0),
              delta_dof_max(0),
              delta_dof_max_in_step(0),
              nb_flux(0),
              q_time_nb_flux(0),
              nb_flux_active(0),
              q_time_nb_flux_active(0),
              q_averaged_nb_step(0),
              q_averaged_time_nb_step(0),
              q_max(0),
              q_max_averaged(0),
              nonquadratic_convergence(0),
              nb_divergence(0),
              divergence_in_step(false) {}
 
        void init(const std::string& name_, const AttributeList& attribute) {
            is_init_ = true;
            name = name_;
            Rn = attribute.get_double(
                  "TOL_RESIDUUM." + name_, attribute.get_double("TOL_RESIDUUM", 1e-5));
            Rm = std::max(
                  Rn, attribute.get_double(
                            "MAX_RESIDUUM." + name_, attribute.get_double("MAX_RESIDUUM", 1e16)));
            Rp = attribute.get_double(
                  "TOL_RESIDUUM_NONQUADRATIC_CONVERGENCE." + name_,
                  attribute.get_double("TOL_RESIDUUM_NONQUADRATIC_CONVERGENCE", 2e-2));
            Ip = attribute.get_int(
                  "NB_ITER_NONQUADRATIC_CONVERGENCE." + name_,
                  attribute.get_int("NB_ITER_NONQUADRATIC_CONVERGENCE", 9));
            Ip = std::max(size_t(3), Ip);
            Rl = attribute.get_double(
                  "TOL_RESIDUUM_LINEAR_PROBLEM." + name_,
                  attribute.get_double("TOL_RESIDUUM_LINEAR_PROBLEM", 1e-8));
            Cn = attribute.get_double(
                  "TOL_SOLUTION." + name_, attribute.get_double("TOL_SOLUTION", 1e-5));
            Cm = std::max(
                  Cn, attribute.get_double(
                            "MAX_SOLUTION." + name_, attribute.get_double("MAX_SOLUTION", 1e16)));
            q_averaged_0 = attribute.get_double(
                  "INITIAL_AVERAGE_FLUX." + name_,
                  attribute.get_double("INITIAL_AVERAGE_FLUX", 1e-2));
            q_averaged_n = attribute.get_double(
                  "AVERAGE_FLUX." + name, attribute.get_double("AVERAGE_FLUX", -1));
            epsilon = attribute.get_double(
                  "ZERO_FLUX_CRITERION." + name_,
                  attribute.get_double("ZERO_FLUX_CRITERION", 1e-5));
            epsilon_l = attribute.get_double(
                  "ZERO_FLUX_RELATIVE_CRITERION." + name_,
                  attribute.get_double("ZERO_FLUX_RELATIVE_CRITERION", 1e-5));
            Cepsilon = attribute.get_double(
                  "TOL_SOLUTION_ZERO_FLUX." + name_,
                  attribute.get_double("TOL_SOLUTION_ZERO_FLUX", 1e-5));
            q_averaged_nb_step = 0;
        }
 
        bool is_init() const { return is_init_; }
 
        void new_step() {
            delta_dof_max_in_step = 0;
            std::fill_n(last_r_max, 3, 0);
            std::fill_n(best_r_max, 3, std::numeric_limits<double>::max());
            nonquadratic_convergence = false;
            nb_divergence = 0;
            divergence_in_step = false;
        }
 
        void new_iteration() {
            last_r_max[0] = last_r_max[1];
            last_r_max[1] = last_r_max[2];
            best_r_max[0] = best_r_max[1];
            best_r_max[1] = best_r_max[2];
        }
 
        void new_evaluation() {
            nb_flux = 0;
            q_time_nb_flux = 0;
            nb_flux_active = 0;
            q_time_nb_flux_active = 0;
            q_max = 0;
        }
 
        void converged_step() {
            if (nb_flux == 0) { return; }
            if (q_averaged_n < 0) {
                double q;
                if (q_max >= 0.1 * q_max_averaged) {
                    q = q_time_nb_flux_active / nb_flux_active;
                } else {
                    q = q_time_nb_flux / nb_flux;
                }
                if (q_averaged_time_nb_step == 0) {
                    if (q > epsilon * q_averaged_0) {
                        q_averaged_nb_step = 1;
                        q_averaged_time_nb_step = q;
                    }
                } else if (q > epsilon * q_averaged_time_nb_step / q_averaged_nb_step) {
                    ++q_averaged_nb_step;
                    q_averaged_time_nb_step += q;
                }
            }
            q_max_averaged = std::max(q_max_averaged, q_max);
        }
 
        void add_flux(const double flux) {
            ++nb_flux;
            q_time_nb_flux += flux;
            if (flux >= epsilon_l * q_max_averaged) {
                ++nb_flux_active;
                q_time_nb_flux_active += flux;
            }
            q_max = std::max(q_max, flux);
        }
 
        typename CorrectionTerminationTest<T>::Termination is_terminated(
              FluxNormalizedCorrectionTerminationTest& parent, size_t nb_iteration, double r_max,
              double step_dof_max, double delta_dof_max, double& distance_to_iterative_convergence,
              const SolverHints* solver_hints) {
            if (solver_hints != nullptr && (**solver_hints & SolverHints::degradation)) {
                std::fill_n(best_r_max, 3, std::numeric_limits<double>::max());
            }
 
            if (nb_flux == 0) { return CorrectionTerminationTest<T>::converged; }
 
            // test on divergence
            if (r_max != r_max || delta_dof_max != delta_dof_max) {
                parent.logger << logging::debug << "Field " << name << " diverged due to nan"
                              << logging::LOGFLUSH;
                return CorrectionTerminationTest<T>::diverged;
            }
            if (std::isinf(r_max) || std::isinf(delta_dof_max)) {
                parent.logger << logging::debug << "Field " << name << " diverged due to inf"
                              << logging::LOGFLUSH;
                return CorrectionTerminationTest<T>::diverged;
            }
 
            if (nb_iteration > size_t(parent.I0_effective) && r_max > best_r_max[1] * 1.00001) {
                nb_divergence += 1;
                if (nb_divergence > parent.max_divergence) {
                    parent.logger << logging::debug << "Field " << name
                                  << ", r_max = " << std::min(r_max, last_r_max[1])
                                  << ", previous r_max = " << last_r_max[1]
                                  << ", best r_max = " << best_r_max[1] << ", diverged after "
                                  << nb_divergence << " consecutive divergence(s)."
                                  << logging::LOGFLUSH;
                    return CorrectionTerminationTest<T>::diverged;
                }
                divergence_in_step = true;
            } else {
                nb_divergence = 0;
            }
 
            if (nb_iteration > 1 && r_max > Rm) {
                parent.logger << logging::debug << "Field " << name
                              << ", r_max = " << std::min(r_max, last_r_max[1])
                              << ", previous r_max = " << last_r_max[1]
                              << ", best r_max = " << best_r_max[1]
                              << ", diverged because the residue exceeds the "
                              << "maximum (" << Rm << ")." << logging::LOGFLUSH;
                return CorrectionTerminationTest<T>::diverged;
            }
            if (nb_iteration > 1 && step_dof_max > Cm) {
                parent.logger << logging::debug << "Field " << name
                              << ", r_max = " << std::min(r_max, last_r_max[1])
                              << ", previous r_max = " << last_r_max[1]
                              << ", best r_max = " << best_r_max[1]
                              << ", diverged because the last correction exceeds "
                              << "the maximum (" << Cm << ")." << logging::LOGFLUSH;
                return CorrectionTerminationTest<T>::diverged;
            }
 
            last_r_max[2] = r_max;
            if (nb_iteration >= size_t(parent.I0_effective)) {
                best_r_max[2] = std::min(best_r_max[1], r_max);
            }
            double q_averaged;
            if (q_averaged_n > 0) {
                q_averaged = q_averaged_n;
            } else {
                double q;
                if (q_max >= 0.1 * q_max_averaged) {
                    q = q_time_nb_flux_active / nb_flux_active;
                } else {
                    q = q_time_nb_flux / nb_flux;
                }
                if (q_averaged_nb_step == 0) {
                    if (q > epsilon * q_averaged_0) {
                        q_averaged = q;
                    } else {
                        q_averaged = q_averaged_0;
                    }
                } else if (q > epsilon * q_averaged_time_nb_step / q_averaged_nb_step) {
                    q_averaged = (q + q_averaged_time_nb_step) / (q_averaged_nb_step + 1);
                } else {
                    q_averaged = q_averaged_time_nb_step / q_averaged_nb_step;
                }
            }
 
            parent.logger << logging::debug << "Field " << name << ", r_max = " << r_max
                          << ", averaged flux = " << q_averaged << ", c_max = " << delta_dof_max
                          << ", delta_dof_max = " << step_dof_max << ", ";
 
            // test on linear increment
            if (r_max <= Rl * q_averaged) {
                parent.logger << "linear increment, converged." << logging::LOGFLUSH;
                nb_divergence = 0;
                return CorrectionTerminationTest<T>::converged;
            }
 
            // test on zero flux
            if (q_max <= epsilon * q_averaged
                && (r_max <= epsilon * q_averaged || delta_dof_max <= Cepsilon * step_dof_max)) {
                parent.logger << "zero flux, converged" << logging::LOGFLUSH;
                nb_divergence = 0;
                return CorrectionTerminationTest<T>::converged;
            }
 
            // test on nonquadratic convergence
            if (nb_iteration > Ip) {
                if (!nonquadratic_convergence
                    && std::min(r_max, last_r_max[1]) < 2 * (last_r_max[0] * last_r_max[0])) {
                    nonquadratic_convergence = true;
                }
                if (nonquadratic_convergence && r_max <= Rp * q_averaged
                    && delta_dof_max <= Cn * step_dof_max) {
                    parent.logger << "nonquadratic convergence, converged." << logging::LOGFLUSH;
                    nb_divergence = 0;
                    return CorrectionTerminationTest<T>::converged;
                }
            }
 
            distance_to_iterative_convergence =
                  std::max(distance_to_iterative_convergence, r_max / (Rn * q_averaged));
            distance_to_iterative_convergence = std::max(
                  distance_to_iterative_convergence,
                  delta_dof_max * std::min(1.0, r_max / std::min(last_r_max[0], last_r_max[1]))
                        / (Cn * step_dof_max));
 
            if (r_max <= Rn * q_averaged
                && (delta_dof_max <= Cn * step_dof_max
                    || (!divergence_in_step && nb_iteration > 2
                        && r_max / std::min(last_r_max[0], last_r_max[1]) * delta_dof_max
                                 <= Cn * step_dof_max))) {
                parent.logger << "converged." << logging::LOGFLUSH;
                nb_divergence = 0;
                return CorrectionTerminationTest<T>::converged;
            }
 
            parent.logger << "unfinished." << logging::LOGFLUSH;
            return CorrectionTerminationTest<T>::unfinished;
        }
 
    private:
        bool is_init_;
        std::string name;
        double Rn;
        double Rm;
        double Rp;
        size_t Ip;
        double Rl;
        double Cn;
        double Cm;
        double q_averaged_0;
        double q_averaged_n;
        double epsilon;
        double epsilon_l;
        double Cepsilon;
 
        double delta_dof_max;
        double delta_dof_max_in_step;
 
        size_t nb_flux;
        double q_time_nb_flux;
        size_t nb_flux_active;
        double q_time_nb_flux_active;
        size_t q_averaged_nb_step;
        double q_averaged_time_nb_step;
        double q_max;
        double q_max_averaged;
        bool nonquadratic_convergence;
        double last_r_max[3];
        double best_r_max[3];
        int nb_divergence;
        bool divergence_in_step;
    };
 
    friend class Field;
    std::vector<Field> list_field;
};
 
template <typename T>
typename FluxNormalizedCorrectionTerminationTest<T>::type_t
      FluxNormalizedCorrectionTerminationTest<T>::type(
            "FluxNormalizedCorrectionTerminationTest", type_name<T>(),
            StringList("FLUX_NORMALISED_CORRECTION_TERMINATION_TEST"), module);
 
class StrongWolfeLineSearch : public LineSearch {
public:
    StrongWolfeLineSearch() : max_iter(8), c1(0.3), c2(0.9), h_derivative(0.01) {}
 
    void init(Model& model, const AttributeList& attribute) override;
 
    double get_minimum(LineSearch::Function& g, double g0, double g1) override;
 
    using type_t = ObjectTypeComplete<StrongWolfeLineSearch, LineSearch::type_t>;
    static type_t type;
 
private:
    void clear_interpolation() { value.clear(); }
 
    void add_value(double h, double v) { value[h] = v; }
 
    double get_minimum_interpolation(double low, double hig);
 
    using value_t = std::map<double, double>;
    value_t value;
 
    static double quadratic_interpolation_minimisation(
          double a1, double g_a1, double dg_a1, double a2, double g_a2) {
        double a2_ = a2 - a1;
        return a1 + dg_a1 * a2_ * a2_ * 0.5 / (g_a2 - g_a1 - dg_a1 * a2_);
    }
 
    static double cubic_interpolation_minimisation(
          double a1, double g_a1, double dg_a1, double a2, double g_a2, double a3, double g_a3) {
        // todo
        return 0;
    }
 
    int max_iter;
    double c1;  // sufficient decrease
    double c2;  // curvature condition
    double h_derivative;
};
 
}}  // namespace b2000::solver
 
#endif  // B2STATIC_NONLINEAR_UTILE_H_