RungeKutta.hh

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2025, Davide Stocco and Enrico Bertolazzi.                                      *
 *                                                                                               *
 * The Sandals project is distributed under the BSD 2-Clause License.                            *
 *                                                                                               *
 * Davide Stocco                                                               Enrico Bertolazzi *
 * University of Trento                                                     University of Trento *
 * e-mail: davide.stocco@unitn.it                             e-mail: enrico.bertolazzi@unitn.it *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
#pragma once
 
#ifndef SANDALS_RUNGEKUTTA_HH
#define SANDALS_RUNGEKUTTA_HH
 
#include <optional>
 
#include <Sandals.hh>
 
#include <Sandals/System/Implicit.hh>
#include <Sandals/System/Explicit.hh>
#include <Sandals/System/SemiExplicit.hh>
#include <Sandals/System/Linear.hh>
 
#include <Sandals/Tableau.hh>
#include <Sandals/Solution.hh>
 
namespace Sandals {
 
  /*\
   |   ____                         _  __     _   _
   |  |  _ \ _   _ _ __   __ _  ___| |/ /   _| |_| |_ __ _
   |  | |_) | | | | '_ \ / _` |/ _ \ ' / | | | __| __/ _` |
   |  |  _ <| |_| | | | | (_| |  __/ . \ |_| | |_| || (_| |
   |  |_| \_\\__,_|_| |_|\__, |\___|_|\_\__,_|\__|\__\__,_|
   |                     |___/
  \*/

  template <typename Real, Integer S, Integer N, Integer M = 0>
  class RungeKutta
  {
  public:
    using real_type = Real; 
    using VectorX = Eigen::Vector<Real, Eigen::Dynamic>; 
    using MatrixJX = Eigen::Matrix<Real, N, N>; 
 
  private:
    using MatrixX = Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic>; 
    using VectorK = Eigen::Vector<Real, N*S>; 
    using MatrixK = Eigen::Matrix<Real, N, S>; 
    using MatrixJK = Eigen::Matrix<Real, N*N, S>; 
    using MatrixJ = Eigen::Matrix<Real, N*S, N*S>; 
    using VectorP = Eigen::Matrix<Real, N+M, 1>; 
    using MatrixP = Eigen::Matrix<Real, N+M, N+M>; 
    using NewtonX = Optimist::RootFinder::Newton<Real, N>; 
    using NewtonK = Optimist::RootFinder::Newton<Real, N*S>; 
    using VectorS = typename Tableau<Real, S>::Vector; 
    using MatrixS = typename Tableau<Real, S>::Matrix; 
    using VectorN = typename Implicit<Real, N, M>::VectorF; 
    using MatrixN = typename Implicit<Real, N, M>::MatrixJF; 
    using VectorM = typename Implicit<Real, N, M>::VectorH; 
    using MatrixM = typename Implicit<Real, N, M>::MatrixJH; 
    using FunctionSC = std::function<void(Integer const, VectorX const &, Real const)>; 
 
  public:
    SANDALS_BASIC_CONSTANTS(Real) 
    const Real SQRT_EPSILON{std::sqrt(EPSILON)};  \
 
    using System = typename Implicit<Real, N, M>::Pointer; 
    using Type = typename Tableau<Real, S>::Type; 
    using Time = Eigen::Vector<Real, Eigen::Dynamic>; 
 
  private:
    mutable NewtonX m_newtonX;                     
    mutable NewtonK m_newtonK;                     
    mutable Eigen::FullPivLU<MatrixP> m_lu;        
 
    Tableau<Real, S> m_tableau;                    
    System     m_system;                           
    Real       m_absolute_tolerance{1e-6};         
    Real       m_relative_tolerance{1e-3};         
    Real       m_safety_factor{0.9};               
    Real       m_min_safety_factor{0.1};           
    Real       m_max_safety_factor{10.0};          
    Real       m_min_step{EPSILON_HIGH};           
    Integer    m_max_substeps{5};                  
    bool       m_adaptive{true};                   
    bool       m_verbose{false};                   
    bool       m_reverse{false};                   
    FunctionSC m_step_callback{nullptr};           
 
    Real    m_projection_tolerance{EPSILON_HIGH}; 
    Integer m_max_projection_iterations{5};       
    bool    m_projection{true};                   
 
  public:
    RungeKutta(const RungeKutta &) = delete;

    RungeKutta & operator=(RungeKutta const & ) = delete;

    RungeKutta(Tableau<Real, S> const & t_tableau)
      : m_tableau(t_tableau) {
      this->verbose_mode(this->m_verbose);
    }

    RungeKutta(Tableau<Real, S> const & t_tableau, System t_system)
      : m_tableau(t_tableau), m_system(t_system) {
      this->verbose_mode(this->m_verbose);
    }

    Type type() const {return this->m_tableau.type;}

    bool is_erk() const {return this->m_tableau.type == Type::ERK;}

    bool is_irk() const {return this->m_tableau.type == Type::IRK;}

    bool is_dirk() const {return this->m_tableau.type == Type::DIRK;}

    Tableau<Real, S> & tableau() {return this->m_tableau;}

    Tableau<Real, S> const & tableau() const {return this->m_tableau;}

    static constexpr Integer stages() {return S;}

    std::string name() const {return this->m_tableau.name;}

    Integer order() const {return this->m_tableau.order;}

    bool is_embedded() const {return this->m_tableau.is_embedded;}

    MatrixS A() const {return this->m_tableau.A;}

    VectorS b() const {return this->m_tableau.b;}

    VectorS b_embedded() const {return this->m_tableau.b_e;}

    VectorS c() const {return this->m_tableau.c;}

    System system() {return this->m_system;}

    void system(System t_system) {this->m_system = t_system;}

    void implicit_system(
      typename ImplicitWrapper<Real, N, M>::FunctionF F,
      typename ImplicitWrapper<Real, N, M>::FunctionJF JF_x,
      typename ImplicitWrapper<Real, N, M>::FunctionJF JF_x_dot,
      typename ImplicitWrapper<Real, N, M>::FunctionH h = ImplicitWrapper<Real, N, M>::DefaultH,
      typename ImplicitWrapper<Real, N, M>::FunctionJH Jh_x = ImplicitWrapper<Real, N, M>::DefaultJH,
      typename ImplicitWrapper<Real, N, M>::FunctionID in_domain = ImplicitWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<ImplicitWrapper<Real, N, M>>(F, JF_x, JF_x_dot, h, Jh_x, in_domain);
    }

    void implicit_system(
      std::string name,
      typename ImplicitWrapper<Real, N, M>::FunctionF F,
      typename ImplicitWrapper<Real, N, M>::FunctionJF JF_x,
      typename ImplicitWrapper<Real, N, M>::FunctionJF JF_x_dot,
      typename ImplicitWrapper<Real, N, M>::FunctionH h = ImplicitWrapper<Real, N, M>::DefaultH,
      typename ImplicitWrapper<Real, N, M>::FunctionJH Jh_x = ImplicitWrapper<Real, N, M>::DefaultJH,
      typename ImplicitWrapper<Real, N, M>::FunctionID in_domain = ImplicitWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<ImplicitWrapper<Real, N, M>>(name, F, JF_x, JF_x_dot, h, Jh_x, in_domain);
    }

    void explicit_system(
      typename ExplicitWrapper<Real, N, M>::FunctionF f,
      typename ExplicitWrapper<Real, N, M>::FunctionJF Jf_x,
      typename ExplicitWrapper<Real, N, M>::FunctionH h = ExplicitWrapper<Real, N, M>::DefaultH,
      typename ExplicitWrapper<Real, N, M>::FunctionJH Jh_x = ExplicitWrapper<Real, N, M>::DefaultJH,
      typename ExplicitWrapper<Real, N, M>::FunctionID in_domain = ExplicitWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<ExplicitWrapper<Real, N, M>>(f, Jf_x, h, Jh_x, in_domain);
    }

    void explicit_system(
      std::string name,
      typename ExplicitWrapper<Real, N, M>::FunctionF f,
      typename ExplicitWrapper<Real, N, M>::FunctionJF Jf_x,
      typename ExplicitWrapper<Real, N, M>::FunctionH h = ExplicitWrapper<Real, N, M>::DefaultH,
      typename ExplicitWrapper<Real, N, M>::FunctionJH Jh_x = ExplicitWrapper<Real, N, M>::DefaultJH,
      typename ExplicitWrapper<Real, N, M>::FunctionID in_domain = ExplicitWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<ExplicitWrapper<Real, N, M>>(name, f, Jf_x, h, Jh_x, in_domain);
    }

    void linear_system(
      typename LinearWrapper<Real, N, M>::FunctionE E,
      typename LinearWrapper<Real, N, M>::FunctionA A,
      typename LinearWrapper<Real, N, M>::FunctionB b,
      typename LinearWrapper<Real, N, M>::FunctionH h = LinearWrapper<Real, N, M>::DefaultH,
      typename LinearWrapper<Real, N, M>::FunctionJH Jh_x = LinearWrapper<Real, N, M>::DefaultJH,
      typename LinearWrapper<Real, N, M>::FunctionID in_domain = LinearWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<LinearWrapper<Real, N, M>>(E, A, b, h, Jh_x, in_domain);
    }

    void linear_system(
      std::string name,
      typename LinearWrapper<Real, N, M>::FunctionE E,
      typename LinearWrapper<Real, N, M>::FunctionA A,
      typename LinearWrapper<Real, N, M>::FunctionB b,
      typename LinearWrapper<Real, N, M>::FunctionH h = LinearWrapper<Real, N, M>::DefaultH,
      typename LinearWrapper<Real, N, M>::FunctionJH Jh_x = LinearWrapper<Real, N, M>::DefaultJH,
      typename LinearWrapper<Real, N, M>::FunctionID in_domain = LinearWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<LinearWrapper<Real, N, M>>(name, E, A, b, h, Jh_x, in_domain);
    }

    void semi_explicit_system(
      typename SemiExplicitWrapper<Real, N, M>::FunctionA A,
      typename SemiExplicitWrapper<Real, N, M>::FunctionTA TA_x,
      typename SemiExplicitWrapper<Real, N, M>::FunctionB b,
      typename SemiExplicitWrapper<Real, N, M>::FunctionJB Jb_x,
      typename SemiExplicitWrapper<Real, N, M>::FunctionH h = SemiExplicitWrapper<Real, N, M>::DefaultH,
      typename SemiExplicitWrapper<Real, N, M>::FunctionJH Jh_x = SemiExplicitWrapper<Real, N, M>::DefaultJH,
      typename SemiExplicitWrapper<Real, N, M>::FunctionID in_domain = SemiExplicitWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<SemiExplicitWrapper<Real, N, M>>(A, TA_x, b, Jb_x, h, Jh_x, in_domain);
    }

    void semi_explicit_system(
      std::string name,
      typename SemiExplicitWrapper<Real, N, M>::FunctionA A,
      typename SemiExplicitWrapper<Real, N, M>::FunctionTA TA_x,
      typename SemiExplicitWrapper<Real, N, M>::FunctionB b,
      typename SemiExplicitWrapper<Real, N, M>::FunctionJB Jb_x,
      typename SemiExplicitWrapper<Real, N, M>::FunctionH h = SemiExplicitWrapper<Real, N, M>::DefaultH,
      typename SemiExplicitWrapper<Real, N, M>::FunctionJH Jh_x = SemiExplicitWrapper<Real, N, M>::DefaultJH,
      typename SemiExplicitWrapper<Real, N, M>::FunctionID in_domain = SemiExplicitWrapper<Real, N, M>::DefaultID
    ) {
      this->m_system = std::make_shared<SemiExplicitWrapper<Real, N, M>>(name, A, TA_x, b, Jb_x, h, Jh_x, in_domain);
    }

    bool has_system() {return this->m_system != nullptr;}

    Real absolute_tolerance() {return this->m_absolute_tolerance;}

    void absolute_tolerance(Real const t_absolute_tolerance) {this->m_absolute_tolerance = t_absolute_tolerance;}

    Real relative_tolerance() {return this->m_relative_tolerance;}

    void relative_tolerance(Real const t_relative_tolerance) {this->m_relative_tolerance = t_relative_tolerance;}

    Real & safety_factor() {return this->m_safety_factor;}

    void safety_factor(Real const t_safety_factor) {this->m_safety_factor = t_safety_factor;}

    Real & min_safety_factor() {return this->m_min_safety_factor;}

    void min_safety_factor(Real const t_min_safety_factor) {this->m_min_safety_factor = t_min_safety_factor;}

    Real & max_safety_factor() {return this->m_max_safety_factor;}

    void max_safety_factor(Real const t_max_safety_factor) {this->m_max_safety_factor = t_max_safety_factor;}

    Real & min_step() {return this->m_min_step;}

    void min_step(Real const t_min_step) {this->m_min_step = t_min_step;}

    Integer & max_substeps() {return this->m_max_substeps;}

    void max_substeps(Integer const t_max_substeps) {this->m_max_substeps = t_max_substeps;}

    bool adaptive_mode() {return this->m_adaptive;}

    void adaptive(bool t_adaptive) {this->m_adaptive = t_adaptive;}

    void enable_adaptive_mode() {this->m_adaptive = true;}

    void disable_adaptive_mode() {this->m_adaptive = false;}

    bool verbose_mode() {return this->m_verbose;}

    void verbose_mode(bool t_verbose) {
      this->m_verbose = t_verbose;
      this->m_newtonX.verbose_mode(t_verbose);
      this->m_newtonK.verbose_mode(t_verbose);
    }

    void enable_verbose_mode() {this->verbose_mode(true);}

    void disable_verbose_mode() {this->verbose_mode(false);}

    bool reverse_mode() {return this->m_reverse;}

    void reverse(bool t_reverse) {this->m_reverse = t_reverse;}

    void enable_reverse_mode() {this->m_reverse = true;}

    void disable_reverse_mode() {this->m_reverse = false;}

    FunctionSC step_callback() {return this->m_step_callback;}

    void step_callback(FunctionSC const & t_step_callback) {this->m_step_callback = t_step_callback;}

    Real projection_tolerance() {return this->m_projection_tolerance;}

    void projection_tolerance(Real const t_projection_tolerance)
    {this->m_projection_tolerance = t_projection_tolerance;}

    Integer & max_projection_iterations() {return this->m_max_projection_iterations;}

    void max_projection_iterations(Integer const t_max_projection_iterations)
    {this->m_max_projection_iterations = t_max_projection_iterations;}

    bool projection() {return this->m_projection;}

    void projection(bool t_projection) {this->m_projection = t_projection;}

    void enable_projection() {this->m_projection = true;}

    void disable_projection() {this->m_projection = false;}

    Real estimate_step(VectorN const & x, VectorN const & x_e, Real const h_k) const
    {
      Real desired_error{this->m_absolute_tolerance + this->m_relative_tolerance *
        std::max(x.array().abs().maxCoeff(), x_e.array().abs().maxCoeff())};
      Real truncation_error{(x - x_e).array().abs().maxCoeff()};
      return h_k * std::min(this->m_max_safety_factor, std::max(this->m_min_safety_factor,
        this->m_safety_factor * std::pow(desired_error/truncation_error,
        1.0/std::max(this->m_tableau.order, this->m_tableau.order_e))));
    }

    std::string info() const
    {
      std::ostringstream os;
      os
        << "Runge-Kutta method:\t" << this->name() << std::endl
        << "\t- order:\t" << this->order() << std::endl
        << "\t- stages:\t" << this->stages() << std::endl
        << "\t- type:\t";
      switch (this->type()) {
        case Type::ERK: os << "explicit"; break;
        case Type::IRK: os << "implicit"; break;
        case Type::DIRK: os << "diagonally implicit"; break;
      }
      os
        << std::endl
        << "\t- embedded:\t" << this->is_embedded() << std::endl;
      if (this->has_system()) {
        os << "\t- system:\t" << this->m_system->name() << std::endl;
      } else {
        os << "\t- system:\t" << "none" << std::endl;
      }
      return os.str();
    }

    void info(std::ostream &os) {os << this->info();}
 
    /*\
     |   _____ ____  _  __
     |  | ____|  _ \| |/ /
     |  |  _| | |_) | ' /
     |  | |___|  _ <| . \
     |  |_____|_| \_\_|\_\
     |
    \*/

    bool erk_explicit_step(VectorN const & x_old, Real const t_old, Real const h_old, VectorN & x_new,
      Real & h_new, MatrixK & K) const
    {
      using Eigen::all;
      using Eigen::seqN;
 
      // Compute the K variables in the case of an explicit method and explicit system
      VectorN x_node;
      for (Integer i{0}; i < S; ++i) {
        x_node = x_old + K(all, seqN(0, i)) * this->m_tableau.A(i, seqN(0, i)).transpose();
        if (!this->m_reverse) {
          K.col(i) = h_old * static_cast<Explicit<Real, N, M> const *>(this->m_system.get())->f(x_node, t_old + h_old*this->m_tableau.c(i));
        } else {
          K.col(i) = h_old * static_cast<Explicit<Real, N, M> const *>(this->m_system.get())->f_reverse(x_node, t_old + h_old*this->m_tableau.c(i));
        }
      }
      if (!K.allFinite()) {return false;}
 
      // Perform the step and obtain the next state
      x_new = x_old + K * this->m_tableau.b;
 
      // Adapt next step
      if (this->m_adaptive && this->m_tableau.is_embedded) {
        VectorN x_emb = x_old + K * this->m_tableau.b_e;
        h_new = this->estimate_step(x_new, x_emb, h_old);
      }
      return true;
    }

    bool erk_explicit_propagate(VectorN const & x, Real const t, Real const h, MatrixK const & K,
      MatrixJX & Jx) const
    {
      using Eigen::all;
      using Eigen::seqN;
 
      // Propagate the derivative of K with respect to x
      VectorN x_node;
      MatrixN Jf_x;
      std::array<MatrixN, S> dK_dx;
      for (Integer i{0}; i < S; ++i) {
        // Compute the node
        x_node = x + K(all, seqN(0, i)) * this->m_tableau.A(i, seqN(0, i)).transpose();
 
        // Compute the Jacobian of f with respect to x at the node
        if (!this->m_reverse) {
          Jf_x = static_cast<Explicit<Real, N, M> const *>(this->m_system.get())->Jf_x(x_node, t + h*this->m_tableau.c(i));
        } else {
          Jf_x = static_cast<Explicit<Real, N, M> const *>(this->m_system.get())->Jf_x_reverse(x_node, t + h*this->m_tableau.c(i));
        }
 
        // Propagate the derivative of K with respect to x
        dK_dx[i] = h * Jf_x;
        for (Integer j{0}; j < i; ++j) {
          dK_dx[i] += h * Jf_x * dK_dx[j] * this->m_tableau.A(i, j);
        }
 
        // Check for NaNs or Infs
        if (!dK_dx[i].allFinite()) {return false;}
      }
 
      // Compute the derivative of x_new with respect to x
      Jx.setIdentity();
      for (Integer i{0}; i < S; ++i) {Jx += h * dK_dx[i] * this->m_tableau.b(i);}
 
      return true;
    }

    void erk_implicit_function(Integer const s, VectorN const & x, Real const t, Real const h,
      MatrixK const & K, VectorN & fun) const
    {
      using Eigen::all;
      using Eigen::seqN;
      VectorN x_node(x + K(all, seqN(0, s)) * this->m_tableau.A(s, seqN(0, s)).transpose());
      if (!this->m_reverse) {
        fun = this->m_system->F(x_node, K.col(s)/h, t + h * this->m_tableau.c(s));
      } else {
        fun = this->m_system->F_reverse(x_node, K.col(s)/h, t + h * this->m_tableau.c(s));
      }
    }

    void erk_implicit_jacobian(Integer const s, VectorN const & x, Real const t, Real const h,
      MatrixK const & K, MatrixN & jac) const
    {
      using Eigen::all;
      using Eigen::seqN;
      VectorN x_node(x + K(all, seqN(0, s)) * this->m_tableau.A(s, seqN(0, s)).transpose());
      if (!this->m_reverse) {
        jac = this->m_system->JF_x_dot(x_node, K.col(s)/h, t + h * this->m_tableau.c(s)) / h;
      } else {
        jac = this->m_system->JF_x_dot_reverse(x_node, K.col(s)/h, t + h * this->m_tableau.c(s)) / h;
      }
    }

    bool erk_implicit_step(VectorN const & x_old, Real const t_old, Real const h_old, VectorN & x_new,
      Real & h_new, MatrixK & K) const
    {
      VectorN K_sol;
      VectorN K_ini(VectorN::Zero());
 
      // Check if the solver converged
      for (Integer s{0}; s < S; ++s) {
        if (this->m_newtonX.solve(
            [this, s, &K, &x_old, t_old, h_old](VectorN const & K_fun, VectorN & fun)
              {K.col(s) = K_fun; this->erk_implicit_function(s, x_old, t_old, h_old, K, fun);},
            [this, s, &K, &x_old, t_old, h_old](VectorN const & K_jac, MatrixN & jac)
              {K.col(s) = K_jac; this->erk_implicit_jacobian(s, x_old, t_old, h_old, K, jac);},
            K_ini, K_sol)) {
          K.col(s) = K_sol;
        } else {
          return false;
        }
      }
 
      // Perform the step and obtain the next state
      x_new = x_old + K * this->m_tableau.b;
 
      // Adapt next step
      if (this->m_adaptive && this->m_tableau.is_embedded) {
        VectorN x_emb(x_old + K * this->m_tableau.b_e);
        h_new = this->estimate_step(x_new, x_emb, h_old);
      }
      return true;
    }

    bool erk_implicit_propagate(VectorN const & x, Real const t, Real const h, MatrixK const & K,
      MatrixJX & Jx) const
    {
      using Eigen::all;
      using Eigen::seqN;
 
      // Propagate the derivative of K with respect to x
      VectorN x_node, x_dot_node;
      MatrixN JF_x, JF_x_dot, jac;
      std::array<MatrixN, S> dK_dx;
      for (Integer i{0}; i < S; ++i) {
        // Compute the node
        x_node = x + K(all, seqN(0, i)) * this->m_tableau.A(i, seqN(0, i)).transpose();
        x_dot_node = K.col(i) / h;
 
        // Compute the Jacobians of F with respect to x and x_dot at the node
        if (!this->m_reverse) {
          JF_x     = this->m_system->JF_x(x_node, x_dot_node, t + h*this->m_tableau.c(i));
          JF_x_dot = this->m_system->JF_x_dot(x_node, x_dot_node, t + h*this->m_tableau.c(i));
        } else {
          JF_x     = this->m_system->JF_x_reverse(x_node, x_dot_node, t + h*this->m_tableau.c(i));
          JF_x_dot = this->m_system->JF_x_dot_reverse(x_node, x_dot_node, t + h*this->m_tableau.c(i));
        }
 
        // Propagate the derivative of K with respect to x
        jac = MatrixN::Identity();
        for (Integer j{0}; j < i; ++j) {
          jac += dK_dx[j] * this->m_tableau.A(i, j);
        }
        // Solve the linear system: JF_x_dot/h * dK_dx[i] + JF_x * jac = 0
        // Rearranged: dK_dx[i] = -h * JF_x * jac * (JF_x_dot)^(-1)
        // But since JF_x_dot/h is the Jacobian w.r.t. x_dot, we solve for dK_dx[i]
        dK_dx[i] = -(JF_x * jac).lu().solve(JF_x_dot / h);
 
        // Check for NaNs or Infs
        if (!dK_dx[i].allFinite()) {return false;}
      }
 
      // Compute the derivative of x_new with respect to x
      Jx.setIdentity();
      for (Integer i{0}; i < S; ++i) {Jx += h * dK_dx[i] * this->m_tableau.b(i);}
 
      return true;
    }
 
    /*\
     |   ___ ____  _  __
     |  |_ _|  _ \| |/ /
     |   | || |_) | ' /
     |   | ||  _ <| . \
     |  |___|_| \_\_|\_\
     |
    \*/

    void irk_function(VectorN const & x, Real const t, Real const h, VectorK const & K, VectorK & fun) const
    {
      VectorN x_node;
      MatrixK K_mat{K.reshaped(N, S)};
      MatrixK fun_mat;
      for (Integer i{0}; i < S; ++i) {
        x_node = x + K_mat * this->m_tableau.A.row(i).transpose();
        if (!this->m_reverse) {
          fun_mat.col(i) = this->m_system->F(x_node, K_mat.col(i)/h, t + h * this->m_tableau.c(i));
        } else {
          fun_mat.col(i) = this->m_system->F_reverse(x_node, K_mat.col(i)/h, t + h * this->m_tableau.c(i));
        }
      }
      fun = fun_mat.reshaped(N*S, 1);
    }

    void irk_jacobian(VectorN const & x, Real const t, Real const h, VectorK const & K, MatrixJ & jac) const
    {
      using Eigen::seqN;
 
      // Reset the Jacobian matrix
      jac.setZero();
 
      // Loop through each equation of the system
      MatrixK K_mat{K.reshaped(N, S)};
      Real t_node;
      VectorN x_node, x_dot_node;
      MatrixN JF_x, JF_x_dot;
      auto idx = seqN(0, N), jdx = seqN(0, N);
      for (Integer i{0}; i < S; ++i) {
        t_node = t + h * this->m_tableau.c(i);
        x_node = x + K_mat * this->m_tableau.A.row(i).transpose();
 
        // Compute the Jacobians with respect to x and x_dot
        x_dot_node = K_mat.col(i) / h;
        if (!this->m_reverse) {
          JF_x     = this->m_system->JF_x(x_node, x_dot_node, t_node);
          JF_x_dot = this->m_system->JF_x_dot(x_node, x_dot_node, t_node);
        } else {
          JF_x     = this->m_system->JF_x_reverse(x_node, x_dot_node, t_node);
          JF_x_dot = this->m_system->JF_x_dot_reverse(x_node, x_dot_node, t_node);
        }
 
        // Combine the Jacobians with respect to x and x_dot to obtain the Jacobian with respect to K
        idx = seqN(i*N, N);
        for (Integer j{0}; j < S; ++j) {
          jdx = seqN(j*N, N);
          if (i == j) {
            jac(idx, jdx) = this->m_tableau.A(i,j) * JF_x + JF_x_dot / h;
          } else {
            jac(idx, jdx) = this->m_tableau.A(i,j) * JF_x;
          }
        }
      }
    }

    bool irk_step(VectorN const & x_old, Real const t_old, Real const h_old, VectorN & x_new,
      Real & h_new, MatrixK & K) const
    {
      VectorK K_vec;
      VectorK K_ini(VectorK::Zero());
 
      // Check if the solver converged
      if (!this->m_newtonK.solve(
          [this, &x_old, t_old, h_old](VectorK const & K_fun, VectorK & fun)
            {this->irk_function(x_old, t_old, h_old, K_fun, fun);},
          [this, &x_old, t_old, h_old](VectorK const & K_jac, MatrixJ & jac)
            {this->irk_jacobian(x_old, t_old, h_old, K_jac, jac);},
          K_ini, K_vec))
        {return false;}
 
      // Reshape the K vector to a matrix
      K = K_vec.reshaped(N, S);
      if (!K.allFinite()) {return false;}
 
      // Perform the step and obtain the next state
      x_new = x_old + K * this->m_tableau.b;
 
      // Adapt next step
      if (this->m_adaptive && this->m_tableau.is_embedded) {
        VectorN x_emb(x_old + K * this->m_tableau.b_e);
        h_new = this->estimate_step(x_new, x_emb, h_old);
      }
      return true;
    }

    bool irk_propagate(VectorN const & x, Real const t, Real const h, MatrixK const & K,
      MatrixJX & Jx) const
    {
      using Eigen::all;
      using Eigen::seqN;
 
      // Propagate the derivative of K with respect to x
      VectorN x_node, x_dot_node;
      MatrixN JF_x, JF_x_dot;
      Eigen::Matrix<Real, N*S, N*S> A;
      Eigen::Matrix<Real, N*S, N> b;
      std::array<MatrixN, S> dK_dx;
      for (Integer i{0}; i < S; ++i) {
        // Compute the node
        x_node = x + K * this->m_tableau.A.row(i).transpose();
        x_dot_node = K.col(i) / h;
 
        // Compute the Jacobians of F with respect to x and x_dot at the node
        if (!this->m_reverse) {
          JF_x     = this->m_system->JF_x(x_node, x_dot_node, t + h*this->m_tableau.c(i));
          JF_x_dot = this->m_system->JF_x_dot(x_node, x_dot_node, t + h*this->m_tableau.c(i));
        } else {
          JF_x     = this->m_system->JF_x_reverse(x_node, x_dot_node, t + h*this->m_tableau.c(i));
          JF_x_dot = this->m_system->JF_x_dot_reverse(x_node, x_dot_node, t + h*this->m_tableau.c(i));
        }
 
        // Fill the big linear system
        A.block(i*N, i*N, N, N) = JF_x_dot;
        for (Integer j{0}; j < S; ++j) {
          A.block(i*N, j*N, N, N) += h * this->m_tableau.A(i, j) * JF_x;
        }
        b.block(i*N, 0, N, N) = -h * JF_x;
 
        // Check for NaNs or Infs
        if (!JF_x.allFinite() || !JF_x_dot.allFinite()) {return false;}
      }
 
      // Solve the big linear system
      Eigen::Matrix<Real, N*S, N> dK_dx_mat(A.lu().solve(b));
      for (Integer i{0}; i < S; ++i) {
        dK_dx[i] = dK_dx_mat.block(i*N, 0, N, N);
        if (!dK_dx[i].allFinite()) {return false;}
      }
 
      // Compute the derivative of x_new with respect to x
      Jx.setIdentity();
      for (Integer i{0}; i < S; ++i) {Jx += h * dK_dx[i] * this->m_tableau.b(i);}
 
      return true;
    }
 
    /*\
     |   ____ ___ ____  _  __
     |  |  _ \_ _|  _ \| |/ /
     |  | | | | || |_) | ' /
     |  | |_| | ||  _ <| . \
     |  |____/___|_| \_\_|\_\
     |
    \*/

    void dirk_function(Integer n, VectorN const & x, Real const t, Real const h, MatrixK const & K,
      VectorN & fun) const
    {
      using Eigen::all;
      using Eigen::seqN;
      VectorN x_node(x + K(all, seqN(0, n+1)) * this->m_tableau.A(n, seqN(0, n+1)).transpose());
      if (!this->m_reverse) {
        fun = this->m_system->F(x_node, K.col(n)/h, t + h * this->m_tableau.c(n));
      } else {
        fun = this->m_system->F_reverse(x_node, K.col(n)/h, t + h * this->m_tableau.c(n));
      }
    }

    void dirk_jacobian(Integer n, VectorN const & x, Real const t, Real const h, MatrixK const & K,
      MatrixN & jac) const
    {
      using Eigen::all;
      using Eigen::seqN;
      Real t_node{t + h * this->m_tableau.c(n)};
      VectorN x_node(x + K(all, seqN(0, n+1)) * this->m_tableau.A(n, seqN(0, n+1)).transpose());
      VectorN x_dot_node(K.col(n)/h);
      if (!this->m_reverse) {
        jac = this->m_tableau.A(n,n) * this->m_system->JF_x(x_node, x_dot_node, t_node) +
          this->m_system->JF_x_dot(x_node, x_dot_node, t_node) / h;
      } else {
        jac = this->m_tableau.A(n,n) * this->m_system->JF_x_reverse(x_node, x_dot_node, t_node) +
          this->m_system->JF_x_dot_reverse(x_node, x_dot_node, t_node) / h;
      }
    }

    bool dirk_step(VectorN const & x_old, Real const t_old, Real const h_old, VectorN & x_new,
      Real & h_new, MatrixK & K) const
    {
      VectorN K_sol;
      VectorN K_ini(VectorN::Zero());
 
      // Check if the solver converged at each step
      for (Integer n{0}; n < S; ++n) {
        if (this->m_newtonX.solve(
            [this, n, &K, &x_old, t_old, h_old](VectorN const & K_fun, VectorN & fun)
              {K.col(n) = K_fun; this->dirk_function(n, x_old, t_old, h_old, K, fun);},
            [this, n, &K, &x_old, t_old, h_old](VectorN const & K_jac, MatrixN & jac)
              {K.col(n) = K_jac; this->dirk_jacobian(n, x_old, t_old, h_old, K, jac);},
            K_ini, K_sol)) {
            K.col(n) = K_sol;
          } else {
            return false;
          }
      }
 
      // Perform the step and obtain the next state
      x_new = x_old + K * this->m_tableau.b;
 
      // Adapt next step
      if (this->m_adaptive && this->m_tableau.is_embedded) {
        VectorN x_emb(x_old + K * this->m_tableau.b_e);
        h_new = this->estimate_step(x_new, x_emb, h_old);
      }
      return true;
    }

    bool dirk_propagate(VectorN const & x, Real const t, Real const h, MatrixK const & K,
      MatrixJX & Jx) const
    {
      using Eigen::all;
      using Eigen::seqN;
 
      // Propagate the derivative of K with respect to x for DIRK methods
      VectorN x_node, x_dot_node;
      MatrixN JF_x, JF_x_dot, jac;
      std::array<MatrixN, S> dK_dx;
      for (Integer i{0}; i < S; ++i) {
        // Compute the node
        x_node = x + K(all, seqN(0, i+1)) * this->m_tableau.A(i, seqN(0, i+1)).transpose();
        x_dot_node = K.col(i) / h;
 
        // Compute the Jacobians of F with respect to x and x_dot at the node
        if (!this->m_reverse) {
          JF_x     = this->m_system->JF_x(x_node, x_dot_node, t + h*this->m_tableau.c(i));
          JF_x_dot = this->m_system->JF_x_dot(x_node, x_dot_node, t + h*this->m_tableau.c(i));
        } else {
          JF_x     = this->m_system->JF_x_reverse(x_node, x_dot_node, t + h*this->m_tableau.c(i));
          JF_x_dot = this->m_system->JF_x_dot_reverse(x_node, x_dot_node, t + h*this->m_tableau.c(i));
        }
 
        // Propagate the derivative of K with respect to x
        jac = MatrixN::Identity();
        for (Integer j{0}; j < i; ++j) {
          jac += dK_dx[j] * this->m_tableau.A(i, j);
        }
        // Solve the linear system: JF_x_dot/h * dK_dx[i] + JF_x * jac = 0
        // Rearranged: dK_dx[i] = -h * JF_x * jac * (JF_x_dot)^(-1)
        dK_dx[i] = -(JF_x * jac).lu().solve(JF_x_dot / h);
 
        // Check for NaNs or Infs
        if (!dK_dx[i].allFinite()) {return false;}
      }
 
      // Compute the derivative of x_new with respect to x
      Jx.setIdentity();
      for (Integer i{0}; i < S; ++i) {Jx += h * dK_dx[i] * this->m_tableau.b(i);}
 
      return true;
    }

    bool step(VectorN const & x_old, Real const t_old, Real const h_old, VectorN & x_new, Real & h_new,
      MatrixK & K) const
    {
      #define CMD "Sandals::RungeKutta::step(...): "
 
      SANDALS_ASSERT(this->m_system->in_domain(x_old, t_old), CMD "in " << this->m_tableau.name <<
        " solver, at t = " << t_old << ", x = " << x_old.transpose() << ", system out of domain.");
 
      if (this->is_erk() && this->m_system->is_explicit()) {
        return this->erk_explicit_step(x_old, t_old, h_old, x_new, h_new, K);
      } else if (this->is_erk() && this->m_system->is_implicit()) {
        return this->erk_implicit_step(x_old, t_old, h_old, x_new, h_new, K);
      } else if (this->is_dirk()) {
        return this->dirk_step(x_old, t_old, h_old, x_new, h_new, K);
      } else {
        return this->irk_step(x_old, t_old, h_old, x_new, h_new, K);
      }
 
      #undef CMD
    }

    bool propagate(VectorN const & x, Real const t, Real const h, MatrixK const & K, MatrixJX & Jx) const
    {
      #define CMD "Sandals::RungeKutta::propagate(...): "
 
      if (this->is_erk() && this->m_system->is_explicit()) {
        return this->erk_explicit_propagate(x, t, h, K, Jx);
      } else if (this->is_erk() && this->m_system->is_implicit()) {
        return this->erk_implicit_propagate(x, t, h, K, Jx);
      } else if (this->is_dirk()) {
        return this->dirk_propagate(x, t, h, K, Jx);
      } else {
        return this->irk_propagate(x, t, h, K, Jx);
      }
 
      #undef CMD
    }

    template <bool Propagate = true>
    bool advance(VectorN const & x_old, Real const t_old, Real const h_old, VectorN & x_new,
      Real & h_new, MatrixJX & Jx) const
    {
      #define CMD "Sandals::RungeKutta::advance(...): "
 
      // Check step size
      SANDALS_ASSERT(h_old > Real(0.0), CMD "in " << this->m_tableau.name << " solver, h = "<<
        h_old << ", expected > 0.");
 
      // Reset the derivative propagation matrix
      if constexpr (Propagate) {Jx.setIdentity();}
 
      // If the integration step failed, try again with substepping
      MatrixK K;
      if (!this->step(x_old, t_old, h_old, x_new, h_new, K)) {
 
        // Store temporary variables
        VectorN x_tmp(x_old);
        Real t_tmp{t_old}, h_tmp{h_old / Real(2.0)};
 
        // Substepping logic
        Integer max_k{this->m_max_substeps * this->m_max_substeps}, k{2};
        Real h_new_tmp;
        while (k > 0) {
          // Calculate the next step with substepping logic
          if (this->step(x_tmp, t_tmp, h_tmp, x_new, h_new_tmp, K)) {
 
            if constexpr (Propagate) {
              // Propagate the derivative of K with respect to x
              MatrixJX Jx_tmp;
              if (!this->propagate(x_tmp, t_tmp, h_tmp, K, Jx_tmp)) {
                SANDALS_WARNING(CMD "in " << this->m_tableau.name << " solver, at t = " << t_tmp <<
                  ", Jacobian propagation failed, aborting.");
                return false;
              }
 
              // Update the derivative propagation
              if constexpr (Propagate) {Jx *= Jx_tmp;}
            }
 
            // Accept the step
            h_tmp = h_new_tmp;
 
            // If substepping is enabled, double the step size
            if (k > 0 && k < max_k) {
              k -= 1;
              // If the substepping index is even, double the step size
              if (k % 2 == 0) {
                h_tmp = Real(2.0) * h_tmp;
                if (this->m_verbose) {
                  SANDALS_WARNING(CMD "in " << this->m_tableau.name << " solver, at t = " << t_tmp <<
                    ", integration succedded disable one substepping layer.");
                }
              }
            }
 
            // Check the infinity norm of the solution
            SANDALS_ASSERT(std::isfinite(x_tmp.maxCoeff()), CMD "in " << this->m_tableau.name <<
              " solver, at t = " << t_tmp << ", ||x||_inf = inf, computation interrupted.");
 
          } else {
 
            // If the substepping index is too high, abort the integration
            k += 2;
            SANDALS_ASSERT(k < max_k, CMD "in " << this->m_tableau.name << " solver, at t = " <<
              t_tmp << ", integration failed with h = " << h_tmp << ", aborting.");
            return false;
 
            // Otherwise, try again with a smaller step
            if (this->m_verbose) {SANDALS_WARNING(CMD "in " << this->m_tableau.name << " solver, " <<
              "at t = " << t_tmp << ", integration failed, adding substepping layer.");}
            h_tmp /= Real(2.0);
            continue;
          }
 
          // Store independent variable (or time)
          t_tmp += h_tmp;
        }
 
        // Store output states substepping solutions
        x_new = x_tmp;
        h_new = h_tmp;
 
      } else {
        // Propagate the derivative of the solution with respect to the states x
        if constexpr (Propagate) {
          if (!this->propagate(x_old, t_old, h_old, K, Jx)) {
            SANDALS_WARNING(CMD "in " << this->m_tableau.name << " solver, at t = " << t_old <<
              ", Jacobian propagation failed, aborting.");
            return false;
          }
        }
      }
 
      // Project intermediate solution on the invariants
      if (this->m_projection) {
        VectorN x_projected;
        if (this->project(x_new, t_old + h_new, x_projected)) {
          x_new = x_projected;
        } else {
          return false;
        }
      }
      return true;
 
      #undef CMD
    }

    template <bool Propagate = true>
    bool solve(VectorX const & t_mesh, VectorN const & ics, Solution<Real, N, M> & sol, MatrixJX & Jx) const
    {
      using Eigen::last;
 
      #define CMD "Sandals::RungeKutta::solve(...): "
 
      // Instantiate output
      sol.resize(t_mesh.size());
 
      // Store initial conditions
      sol.t(0)     = t_mesh(0);
      sol.x.col(0) = ics;
      sol.h.col(0) = this->m_system->h(ics, t_mesh(0));
 
      // Check initial conditions
      if constexpr (Propagate) {
        if (!Jx.allFinite()) {
          SANDALS_ERROR(CMD "in " << this->m_tableau.name << " solver, initial Jacobian " <<
            "contains NaNs or Infs, aborting.");
          return false;
        }
      }
 
      // Callback on initial conditions
      if (this->m_step_callback) {this->m_step_callback(0, ics, t_mesh(0));}
 
      // Update the current step
      Integer step{0};
      VectorN x_old_step(ics), x_new_step(ics);
      MatrixJX Jx_step;
      Real t_step{t_mesh(0)}, h_step{t_mesh(1) - t_mesh(0)}, h_tmp_step{h_step}, h_new_step;
      bool mesh_point_bool, saturation_bool;
 
      while (true) {
        // Integrate system
        if (!this->template advance<Propagate>(x_old_step, t_step, h_step, x_new_step, h_new_step, Jx_step))
        {return false;}
 
        // Update the current step
        t_step += h_step;
 
        // Saturate the suggested timestep
        mesh_point_bool = std::abs(t_step - t_mesh(step+1)) < SQRT_EPSILON;
        saturation_bool = t_step + h_new_step > t_mesh(step+1) + SQRT_EPSILON;
        if (this->m_adaptive && this->m_tableau.is_embedded && !mesh_point_bool && saturation_bool) {
          h_tmp_step = h_new_step; // Used to store the previous step and keep the integration pace
          h_step     = t_mesh(step+1) - t_step;
        } else {
          h_step = h_new_step;
        }
 
        // Store solution if the step is a mesh point
        if (!this->m_adaptive || mesh_point_bool) {
 
          // Update temporaries
          step += 1;
          h_step = h_tmp_step;
 
          // Update outputs
          sol.t(step)     = t_step;
          sol.x.col(step) = x_new_step;
          sol.h.col(step) = this->m_system->h(x_new_step, t_step);
 
          // Callback after the step is completed
          if (this->m_step_callback) {this->m_step_callback(step, x_new_step, t_step);}
 
          // Check if the current step is the last one
          if (std::abs(t_step - t_mesh(last)) < SQRT_EPSILON) {break;}
 
          // Update the previous step
          x_old_step = x_new_step;
 
          // Propagate the derivative of the solution with respect to the states x
          if constexpr (Propagate) {Jx *= Jx_step;}
        }
      }
      return true;
 
      #undef CMD
    }

    bool solve(VectorX const & t_mesh, VectorN const & ics, Solution<Real, N, M> & sol) const
    {
      MatrixJX Jx; // Dummy variable
      return this->template solve<false>(t_mesh, ics, sol, Jx);
    }

    template <bool Propagate = true>
    bool adaptive_solve(VectorX const & t_mesh, VectorN const & ics, Solution<Real, N, M> & sol,
      MatrixJX & Jx) const
    {
      using Eigen::all;
      using Eigen::last;
 
      #define CMD "Sandals::RungeKutta::adaptive_solve(...): "
 
      // Check initial conditions
      if constexpr (Propagate) {
        if (!Jx.allFinite()) {
          SANDALS_ERROR(CMD "in " << this->m_tableau.name << " solver, initial Jacobian " <<
            "contains NaNs or Infs, aborting.");
          return false;
        }
      }
 
      // Check if the adaptive method is enabled and the method is embedded
      if (!this->is_embedded()) {
        SANDALS_WARNING(CMD "the method is not embedded, using solve(...) method.");
        return this->solve(t_mesh, ics, sol);
      } else if (!this->m_adaptive) {
        SANDALS_WARNING(CMD "adaptive method is disabled, using solve(...) method.");
        return this->solve(t_mesh, ics, sol);
      }
 
      // Instantiate output
      Real t_step{t_mesh(0)}, h_step{t_mesh(1) - t_mesh(0)}, h_new_step, scale{100.0};
      Real h_min{std::max(this->m_min_step, h_step/scale)}, h_max{scale*h_step};
      if (this->m_tableau.is_embedded) {
        Integer safety_length{static_cast<Integer>(std::ceil(std::abs(t_mesh(last) - t_mesh(0))/(2.0*h_min)))};
        sol.resize(safety_length);
      } else {
        sol.resize(t_mesh.size());
      }
 
      // Store initial conditions
      sol.t(0)     = t_mesh(0);
      sol.x.col(0) = ics;
      sol.h.col(0) = this->m_system->h(ics, t_mesh(0));
 
      // Reset the derivative propagation matrix
      if constexpr (Propagate) {Jx.setIdentity();}
 
      // Callback on initial conditions
      if (this->m_step_callback) {this->m_step_callback(0, ics, t_mesh(0));}
 
      // Instantiate temporary variables
      Integer step{0};
      VectorN x_old_step(ics), x_new_step(ics);
      MatrixJX Jx_step;
 
      while (true) {
        // Integrate system
        this->template advance<Propagate>(x_old_step, t_step, h_step, x_new_step, h_new_step, Jx_step);
 
        // Update the current step
        t_step += h_step;
 
        // Saturate the suggested timestep
        if (this->m_adaptive && this->m_tableau.is_embedded) {
          h_step = std::max(std::min(h_new_step, h_max), h_min);
        }
 
        SANDALS_ASSERT(step < sol.size(), CMD "safety length exceeded.");
 
        // Update temporaries
        step += 1;
 
        // Update outputs
        sol.t(step)     = t_step;
        sol.x.col(step) = x_new_step;
        sol.h.col(step) = this->m_system->h(x_new_step, t_step);
 
        // Callback after the step is completed
        if (this->m_step_callback) {this->m_step_callback(step, x_new_step, t_step);}
 
        // Check if the current step is the last one
        if (std::abs(t_step - t_mesh(last)) < SQRT_EPSILON) {break;}
        else if (t_step + h_step > t_mesh(last)) {h_step = t_mesh(last) - t_step;}
 
        // Update the previous step
        x_old_step = x_new_step;
 
        // Propagate the derivative of the solution with respect to the states x
        if constexpr (Propagate) {Jx *= Jx_step;}
      }
 
      // Resize the output
      sol.conservative_resize(step);
 
      return true;
 
      #undef CMD
    }

    bool adaptive_solve(VectorX const & t_mesh, VectorN const & ics, Solution<Real, N, M> & sol) const
    {
      MatrixJX Jx; // Dummy variable
      return this->template adaptive_solve<false>(t_mesh, ics, sol, Jx);
    }

    bool project(VectorN const & x, Real const t, VectorN & x_projected) const
    {
      #define CMD "Sandals::RungeKutta::project(...): "
 
      // Check if there are any constraints
      x_projected = x;
      if (M > Integer(0)) {
 
        VectorM h;
        MatrixM Jh_x;
        VectorP b, x_step;
        MatrixP A;
        A.setZero();
        A.template block<N, N>(0, 0) = MatrixN::Identity();
        for (Integer k{0}; k < this->m_max_projection_iterations; ++k) {
 
          /* Standard projection method
               [A]          {x}     =      {b}
           / I  Jh_x^T \ /   dx   \   / x_t - x_k \
           |           | |        | = |           |
           \ Jh_x    0 / \ lambda /   \    -h     /
          */
 
          // Evaluate the invariants vector and its Jacobian
          h    = this->m_system->h(x_projected, t);
          Jh_x = this->m_system->Jh_x(x_projected, t);
 
          // Check if the solution is found
          if (h.norm() < this->m_projection_tolerance) {return true;}
 
          // Build the linear system
          A.template block<N, M>(0, N) = Jh_x.transpose();
          A.template block<M, N>(N, 0) = Jh_x;
          b.template head<N>() = x - x_projected;
          b.template tail<M>() = -h;
 
          // Compute the solution of the linear system
          this->m_lu.compute(A);
          SANDALS_ASSERT(this->m_lu.rank() == N+M, CMD "singular Jacobian detected.");
          x_step = this->m_lu.solve(b);
 
          // Check if the step is too small
          if (x_step.norm() < this->m_projection_tolerance * this->m_projection_tolerance) {return false;}
 
          // Update the solution
          x_projected.noalias() += x_step(Eigen::seqN(0, N));
        }
        if (this->m_verbose) {SANDALS_WARNING(CMD "maximum number of iterations reached.");}
        return false;
      } else {
        return true;
      }
 
      #undef CMD
    }

    bool project_ics(VectorN const & x, Real const t, std::vector<Integer> const & projected_equations,
      std::vector<Integer> const & projected_invariants, VectorN & x_projected) const
    {
      #define CMD "Sandals::RungeKutta::project_ics(...): "
 
      Integer X{static_cast<Integer>(projected_equations.size())};
      Integer H{static_cast<Integer>(projected_invariants.size())};
 
      // Check if there are any constraints
      x_projected = x;
      if (H > Integer(0)) {
 
        VectorM h;
        MatrixM Jh_x;
        VectorX b(X+H), x_step(X+H);
        MatrixX A(X+H, X+H);
        A.setZero();
        A.block(0, 0, X, X) = MatrixX::Identity(X+H, X+H);
        Eigen::FullPivLU<MatrixX> lu;
        for (Integer k{0}; k < this->m_max_projection_iterations; ++k) {
 
          /* Standard projection method
               [A]         {x}    =        {b}
           / I  Jh_x^T \ /   dx   \   / x_t - x_k \
           |           | |        | = |           |
           \ Jh_x    0 / \ lambda /   \    -h     /
          */
 
          // Evaluate the invariants vector and its Jacobian
          h    = this->m_system->h(x_projected, t);
          Jh_x = this->m_system->Jh_x(x_projected, t);
 
          // Select only the projected invariants
          h    = h(projected_invariants);
          Jh_x = Jh_x(projected_invariants, projected_equations);
 
          // Check if the solution is found
          if (h.norm() < this->m_projection_tolerance) {return true;}
 
          // Build the linear system
          A.block(0, X, X, H) = Jh_x.transpose();
          A.block(X, 0, H, X) = Jh_x;
          b.head(X) = x(projected_equations) - x_projected(projected_equations);
          b.tail(H) = -h;
 
          // Compute the solution of the linear system
          lu.compute(A);
          SANDALS_ASSERT(lu.rank() == X+H, CMD "singular Jacobian detected.");
          x_step = this->m_lu.solve(b);
 
          // Check if the step is too small
          if (x_step.norm() < this->m_projection_tolerance * this->m_projection_tolerance) {return false;}
 
          // Update the solution
          x_projected(projected_equations).noalias() += x_step;
        }
        if (this->m_verbose) {SANDALS_WARNING(CMD "maximum number of iterations reached.");}
        return false;
      } else {
        return true;
      }
 
      #undef CMD
    }

    Real estimate_order(std::vector<VectorX> const & t_mesh, VectorN const & ics, std::function<MatrixX(VectorX)> & sol) const
    {
      using Eigen::last;
 
      #define CMD "Sandals::RungeKutta::estimate_order(...): "
 
      SANDALS_ASSERT(t_mesh.size() > Integer(1), CMD "expected at least two time meshes.");
 
      for (Integer i{0}; i < static_cast<Integer>(t_mesh.size()); ++i) {
 
        // Check if the time mesh has the same initial and final time
        SANDALS_ASSERT((t_mesh[0](0) - t_mesh[i](0)) < SQRT_EPSILON,
          CMD "expected the same initial time.");
        SANDALS_ASSERT((t_mesh[0](last) - t_mesh[i](last)) < SQRT_EPSILON,
          CMD "expected the same final time.");
 
        // Check if the mesh step is fixed all over the time mesh
        for (Integer j{1}; j < static_cast<Integer>(t_mesh[i].size()); ++j) {
          SANDALS_ASSERT((t_mesh[i](j) - t_mesh[i](j-1)) - (t_mesh[i](1) - t_mesh[i](0)) < SQRT_EPSILON,
            CMD "expected a fixed step.");
        }
      }
 
      // Solve the system for each time scale
      Solution<Real, N, M> sol_num;
      MatrixX sol_ana;
      VectorX h_vec(t_mesh.size()), e_vec(t_mesh.size());
      for (Integer i{0}; i < static_cast<Integer>(t_mesh.size()); ++i) {
        SANDALS_ASSERT(this->solve(t_mesh[i], ics, sol_num), CMD "failed to solve the system for " <<
          "the" << i << "-th time mesh.");
        sol_ana = sol(sol_num.t);
        SANDALS_ASSERT(sol_ana.rows() == sol_num.x.rows(),
          CMD "expected the same number of states in analytical solution.");
        SANDALS_ASSERT(sol_ana.cols() == sol_num.x.cols(),
          CMD "expected the same number of steps in analytical solution.");
        h_vec(i) = std::abs(sol_num.t(1) - sol_num.t(0));
        e_vec(i) = (sol_ana - sol_num.x).array().abs().maxCoeff();
      }
 
      // Compute the order of the method thorugh least squares
      VectorX A(h_vec.array().log());
      VectorX b(e_vec.array().log());
      return ((A.transpose() * A).ldlt().solve(A.transpose() * b))(0);
 
      #undef CMD
    }
 
  }; // class RungeKutta
 
} // namespace Sandals
 
#endif // SANDALS_RUNGEKUTTA_HH