From 279d08086ed3e386a321cfd0bf6ab6f4009e93e2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?St=C3=A9phane=20Del=20Pino?= <stephane.delpino44@gmail.com>
Date: Fri, 4 Aug 2023 19:03:38 +0200
Subject: [PATCH] [ci-skip] Implement BFGS (first try) and prepare a quality
 criterion

---
 src/mesh/MeshSmootherEscobar.cpp | 862 +++++++++++++++++--------------
 1 file changed, 485 insertions(+), 377 deletions(-)

diff --git a/src/mesh/MeshSmootherEscobar.cpp b/src/mesh/MeshSmootherEscobar.cpp
index 5f608eb2e..6ef78cc3d 100644
--- a/src/mesh/MeshSmootherEscobar.cpp
+++ b/src/mesh/MeshSmootherEscobar.cpp
@@ -263,8 +263,8 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                     if (current_sigma_min < sigma_min) {
                       sigma_min = current_sigma_min;
                       delta     = (sigma_min < eps)
-                                    ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min))
-                                    : 0;
+                                ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min))
+                                : 0;
                     }
                   }
 
@@ -367,6 +367,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
                   const NodeId node_id = node_list[i_node];
                   if (m_is_smoothed_node[node_id] and node_is_owned[node_id]) {
                     smooth(node_id, new_xr[node_id]);
+                    //                    new_xr[node_id] = 0.5 * (new_xr[node_id] + old_xr[node_id]);
                   }
                 });
 
@@ -428,7 +429,7 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
   }
 
   NodeValue<const bool>
-  _getSmoothedNode(CellValue<const int>& non_convex_cell_tag, const NodeValue<const bool>& m_is_fixed_node) const
+  _getSmoothedNode(const CellValue<const int>& non_convex_cell_tag, const NodeValue<const bool>& m_is_fixed_node) const
   {
     auto node_to_cell_matrix = m_given_mesh.connectivity().nodeToCellMatrix();
     NodeValue<bool> is_smoothed{m_given_mesh.connectivity()};
@@ -450,495 +451,593 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
     return is_smoothed;
   }
 
- public:
-  std::shared_ptr<const ItemValueVariant>
-  getQuality() const
-  {
-    return std::make_shared<ItemValueVariant>(m_given_mesh.xr());
-  }
-
   void
-  _getNewPositions(const NodeValue<const Rd>& old_xr, NodeValue<Rd>& new_xr) const
+  get_f_grad_hess(const NodeValue<const TinyVector<Dimension>>& old_xr,
+                  double& sigma_min,
+                  double& delta,
+                  const NodeId node_id,
+                  const TinyVector<Dimension>& x,
+                  double& f,
+                  TinyVector<Dimension>& f_gradient,
+                  TinyMatrix<Dimension>& f_hessian) const
   {
-    constexpr bool multi_connection = false;
+    constexpr bool multi_connection = true;
+    constexpr double eps            = 1E-14;
 
-    const ConnectivityType& connectivity = m_given_mesh.connectivity();
-
-    auto is_boundary_node = connectivity.isBoundaryNode();
-    auto node_is_owned    = connectivity.nodeIsOwned();
+    auto frobenius = [](const TinyMatrix<Dimension>& M) { return std::sqrt(trace(transpose(M) * M)); };
 
-    if constexpr (Dimension == 2) {
-      auto cell_to_node_matrix        = connectivity.cellToNodeMatrix();
-      auto node_to_cell_matrix        = connectivity.nodeToCellMatrix();
-      auto node_to_edge_matrix        = connectivity.nodeToEdgeMatrix();
-      auto edge_to_node_matrix        = connectivity.edgeToNodeMatrix();
-      auto node_number_in_their_cells = connectivity.nodeLocalNumbersInTheirCells();
-
-      NodeValue<double> quality{connectivity};
-
-      constexpr double eps = 1E-14;
-      quality.fill(2);
+    const ConnectivityType& connectivity = m_given_mesh.connectivity();
 
-      size_t maxiter = 0;
+    auto cell_to_node_matrix        = connectivity.cellToNodeMatrix();
+    auto node_to_cell_matrix        = connectivity.nodeToCellMatrix();
+    auto node_number_in_their_cells = connectivity.nodeLocalNumbersInTheirCells();
 
-      auto smooth = [=, &maxiter](const NodeId node_id, TinyVector<Dimension>& x) -> double {
-        const bool print = (node_id == 4);
+    auto cell_list           = node_to_cell_matrix[node_id];
+    auto node_number_in_cell = node_number_in_their_cells[node_id];
 
-        auto cell_list           = node_to_cell_matrix[node_id];
-        auto node_number_in_cell = node_number_in_their_cells[node_id];
+    const double alpha         = 2 * std::acos(-1) / cell_list.size();
+    const double sin_alpha     = std::sin(alpha);
+    const double cos_alpha     = std::cos(alpha);
+    const double inv_sin_alpha = 1. / sin_alpha;
+    const double inv_tan_alpha = 1. / std::tan(alpha);
 
-        const double alpha         = 2 * std::acos(-1) / cell_list.size();
-        const double sin_alpha     = std::sin(alpha);
-        const double cos_alpha     = std::cos(alpha);
-        const double inv_sin_alpha = 1. / sin_alpha;
-        const double inv_tan_alpha = 1. / std::tan(alpha);
+    const TinyMatrix<Dimension> W{1, cos_alpha,   //
+                                  0, sin_alpha};
 
-        const TinyMatrix<Dimension> W{1, cos_alpha,   //
-                                      0, sin_alpha};
+    const TinyMatrix<Dimension> inv_W = inverse(W);
 
-        const TinyMatrix<Dimension> inv_W = inverse(W);
+    std::array<TinyMatrix<Dimension>, Dimension>    //
+      S_gradient = {TinyMatrix<Dimension>{-1, -1,   //
+                                          +0, +0},
+                    TinyMatrix<Dimension>{+inv_tan_alpha, +inv_tan_alpha,   //
+                                          -inv_sin_alpha, -inv_sin_alpha}};
 
-        std::array<TinyMatrix<Dimension>, Dimension>    //
-          S_gradient = {TinyMatrix<Dimension>{-1, -1,   //
-                                              +0, +0},
-                        TinyMatrix<Dimension>{+inv_tan_alpha, +inv_tan_alpha,   //
-                                              -inv_sin_alpha, -inv_sin_alpha}};
+    f          = 0;
+    f_gradient = zero;
+    f_hessian  = zero;
 
-        SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
+    SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
+    for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
+      const size_t i_cell_node   = node_number_in_cell[i_cell];
+      auto cell_node_list        = cell_to_node_matrix[cell_list[i_cell]];
+      const size_t cell_nb_nodes = cell_node_list.size();
 
-        double sigma_min = std::numeric_limits<double>::max();
-        double delta     = 0;
+      const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
+      const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
 
-        auto frobenius = [](const TinyMatrix<Dimension>& M) { return std::sqrt(trace(transpose(M) * M)); };
+      const TinyMatrix<Dimension> A{a[0] - x[0], b[0] - x[0],   //
+                                    a[1] - x[1], b[1] - x[1]};
 
-        double final_f = 0;
+      S_list[i_cell] = A * inv_W;
+    }
 
-        std::cout.precision(15);
+    SmallArray<double> sigma_list(S_list.size());
+    for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) {
+      sigma_list[i_cell] = det(S_list[i_cell]);
+    }
 
-        for (size_t i_iter = 0; i_iter < 5; ++i_iter) {
-          double sigma_min = std::numeric_limits<double>::max();
-          delta            = sigma_min;
+    {
+      double current_sigma_min = std::min(min(sigma_list), delta);
 
-          maxiter = std::max(i_iter + 1, maxiter);
-          if (print) {
-            std::cout << "------------------------------\n";
-            std::cout << "iter = " << i_iter << '\n';
-            std::cout << "x    = " << x << '\n';
-          }
+      if constexpr (multi_connection) {
+        size_t nb_medium_range_simplices = 0;
+        for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
+          const CellId cell_id = cell_list[i_cell];
+          nb_medium_range_simplices += 2 * (cell_to_node_matrix[cell_id].size() - 3);
+        }
 
-          double f                         = 0;
-          TinyVector<Dimension> f_gradient = zero;
-          TinyMatrix<Dimension> f_hessian  = zero;
+        if (nb_medium_range_simplices > 0) {
+          SmallArray<double> medium_range_sigma_list(nb_medium_range_simplices);
 
-          SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
           for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
+            const CellId cell_id       = cell_list[i_cell];
             const size_t i_cell_node   = node_number_in_cell[i_cell];
             auto cell_node_list        = cell_to_node_matrix[cell_list[i_cell]];
             const size_t cell_nb_nodes = cell_node_list.size();
 
-            const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
-            const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
+            const double mr_alpha     = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size()));
+            const double mr_sin_alpha = std::sin(mr_alpha);
+            const double mr_cos_alpha = std::cos(mr_alpha);
+
+            const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha,   //
+                                             0, mr_sin_alpha};
+
+            const TinyMatrix mr_inv_W = inverse(mr_W);
+
+            {   // first simplex
+              const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
+              const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]];
 
-            const TinyMatrix<Dimension> A{a[0] - x[0], b[0] - x[0],   //
-                                          a[1] - x[1], b[1] - x[1]};
+              const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
+                                               a[1] - x[1], b[1] - x[1]};
 
-            S_list[i_cell] = A * inv_W;
+              const TinyMatrix mr_S = mr_A * mr_inv_W;
+              const double mr_sigma = det(mr_S);
+              if (mr_sigma < current_sigma_min) {
+                current_sigma_min = mr_sigma;
+              }
+            }
+
+            {   // second simplex
+              const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]];
+              const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
 
-            if (print) {
-              std::cout << "--- S --- cell = " << i_cell << " (" << node_to_cell_matrix[node_id][i_cell] << ")\n";
-              std::cout << "a = " << a << '\n';
-              std::cout << "b = " << b << '\n';
-              std::cout << "x = " << x << '\n';
-              std::cout << "A = " << A << '\n';
-              std::cout << "S = " << S_list[i_cell] << '\n';
+              const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
+                                               a[1] - x[1], b[1] - x[1]};
+
+              const TinyMatrix mr_S = mr_A * mr_inv_W;
+              const double mr_sigma = det(mr_S);
+              if (mr_sigma < current_sigma_min) {
+                current_sigma_min = mr_sigma;
+              }
             }
           }
+        }
+      }
+
+      if (current_sigma_min < sigma_min) {
+        sigma_min = current_sigma_min;
+        delta =
+          (sigma_min < eps) ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min)) : 0;
+      }
+    }
 
-          SmallArray<double> sigma_list(S_list.size());
-          for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) {
-            sigma_list[i_cell] = det(S_list[i_cell]);
+    auto escobarSSigma = [&frobenius, delta](const double sigma, const TinyMatrix<Dimension>& S,
+                                             const TinyVector<Dimension>& sigma_gradient,
+                                             const std::array<TinyMatrix<Dimension>, Dimension>& S_gradient,
+                                             const std::array<TinyMatrix<Dimension>, Dimension>& Sigma_gradient,
+                                             double& f, TinyVector<Dimension>& f_gradient,
+                                             TinyMatrix<Dimension>& f_hessian) {
+      const TinyMatrix<Dimension> Sigma = [&S] {
+        TinyMatrix<Dimension> transposed_comS;
+        for (size_t i = 0; i < Dimension; ++i) {
+          for (size_t j = 0; j < Dimension; ++j) {
+            transposed_comS(i, j) = cofactor(S, j, i);
           }
+        }
+        return transposed_comS;
+      }();
+
+      const double S_norm      = frobenius(S);
+      const double Sigma_norm  = frobenius(Sigma);
+      const double S_norm2     = S_norm * S_norm;
+      const double Sigma_norm2 = Sigma_norm * Sigma_norm;
+
+      double h = sigma + std::sqrt(sigma * sigma + 4 * delta * delta);
+
+      const double f_jr = S_norm * Sigma_norm / h;
+
+      TinyVector<Dimension> g{trace(transpose(S) * S_gradient[0]) / S_norm2                   //
+                                + trace(transpose(Sigma) * Sigma_gradient[0]) / Sigma_norm2   //
+                                - trace(Sigma * S_gradient[0]) / (h - sigma),
+                              //
+                              trace(transpose(S) * S_gradient[1]) / S_norm2                   //
+                                + trace(transpose(Sigma) * Sigma_gradient[1]) / Sigma_norm2   //
+                                - trace(Sigma * S_gradient[1]) / (h - sigma)};
+
+      const TinyVector<Dimension> f_jr_gradient = f_jr * g;
+
+      TinyMatrix<Dimension> f_jr_hessian = zero;
+      for (size_t i = 0; i < Dimension; ++i) {
+        for (size_t j = 0; j < Dimension; ++j) {
+          f_jr_hessian(i, j) =                                                                                       //
+            (trace(transpose(S_gradient[j]) * S_gradient[i]) / S_norm2                                               //
+             - 2 * trace(transpose(S) * S_gradient[j]) * trace(transpose(S) * S_gradient[i]) / (S_norm2 * S_norm2)   //
+                                                                                                                     //
+             + trace(transpose(Sigma_gradient[j]) * Sigma_gradient[i]) / Sigma_norm2   // + 0
+             - 2 * trace(transpose(Sigma) * Sigma_gradient[j]) * trace(transpose(Sigma) * Sigma_gradient[i]) /
+                 (Sigma_norm2 * Sigma_norm2)   //
+
+             - trace(Sigma_gradient[j] * S_gradient[i]) / (h - sigma)                     //
+             + sigma / (std::pow(h - sigma, 3)) * sigma_gradient[i] * sigma_gradient[j]   //
+             + g[j] * g[i]) *
+            f_jr;
+        }
+      }
 
-          {
-            double current_sigma_min = std::min(min(sigma_list), delta);
+      f += f_jr;
+      f_gradient += f_jr_gradient;
+      f_hessian += f_jr_hessian;
+    };
 
-            if constexpr (multi_connection) {
-              size_t nb_medium_range_simplices = 0;
-              for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
-                const CellId cell_id = cell_list[i_cell];
-                nb_medium_range_simplices += 2 * (cell_to_node_matrix[cell_id].size() - 3);
-              }
+    auto escobar = escobarSSigma;
 
-              if (nb_medium_range_simplices > 0) {
-                SmallArray<double> medium_range_sigma_list(nb_medium_range_simplices);
+    for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) {
+      const double sigma            = sigma_list[i_cell];
+      const TinyMatrix<Dimension> S = S_list[i_cell];
 
-                for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
-                  const CellId cell_id       = cell_list[i_cell];
-                  const size_t i_cell_node   = node_number_in_cell[i_cell];
-                  auto cell_node_list        = cell_to_node_matrix[cell_list[i_cell]];
-                  const size_t cell_nb_nodes = cell_node_list.size();
+      const TinyMatrix<Dimension> Sigma = [&S] {
+        TinyMatrix<Dimension> transposed_comS;
+        for (size_t i = 0; i < Dimension; ++i) {
+          for (size_t j = 0; j < Dimension; ++j) {
+            transposed_comS(i, j) = cofactor(S, j, i);
+          }
+        }
+        return transposed_comS;
+      }();
 
-                  const double mr_alpha     = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size()));
-                  const double mr_sin_alpha = std::sin(mr_alpha);
-                  const double mr_cos_alpha = std::cos(mr_alpha);
+      TinyVector<Dimension> sigma_gradient{trace(Sigma * S_gradient[0]),   //
+                                           trace(Sigma * S_gradient[1])};
 
-                  const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha,   //
-                                                   0, mr_sin_alpha};
+      const std::array<TinyMatrix<Dimension>, Dimension>   //
+        Sigma_gradient{TinyMatrix<Dimension>{+0, +1,       //
+                                             +0, -1},
+                       TinyMatrix<Dimension>{-inv_sin_alpha, -inv_tan_alpha,   //
+                                             +inv_sin_alpha, +inv_tan_alpha}};
 
-                  const TinyMatrix mr_inv_W = inverse(mr_W);
+      escobar(sigma, S, sigma_gradient, S_gradient, Sigma_gradient, f, f_gradient, f_hessian);
+    }
 
-                  if (print) {
-                    std::cout << "MC === i_cell = " << i_cell << '\n';
-                  }
+    if constexpr (multi_connection) {
+      for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
+        const CellId cell_id       = cell_list[i_cell];
+        auto cell_node_list        = cell_to_node_matrix[cell_list[i_cell]];
+        const size_t cell_nb_nodes = cell_node_list.size();
 
-                  {   // first simplex
-                    const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
-                    const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]];
+        if (cell_nb_nodes <= 3) {
+          continue;
+        }
 
-                    const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
-                                                     a[1] - x[1], b[1] - x[1]};
+        const size_t i_cell_node = node_number_in_cell[i_cell];
 
-                    const TinyMatrix mr_S = mr_A * mr_inv_W;
-                    const double mr_sigma = det(mr_S);
-                    if (mr_sigma < current_sigma_min) {
-                      current_sigma_min = mr_sigma;
-                    }
+        const double mr_alpha = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size()));
 
-                    if (print) {
-                      std::cout << "MC triangle 1 ===\n";
-                      std::cout << "MC a = " << a << '\n';
-                      std::cout << "MC b = " << b << '\n';
-                      std::cout << "MC x = " << x << '\n';
-                    }
-                  }
+        const double mr_sin_alpha     = std::sin(mr_alpha);
+        const double mr_cos_alpha     = std::cos(mr_alpha);
+        const double mr_inv_sin_alpha = 1. / mr_sin_alpha;
+        const double mr_inv_tan_alpha = 1. / std::tan(mr_alpha);
 
-                  {   // second simplex
-                    const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]];
-                    const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
+        const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha,   //
+                                         0, mr_sin_alpha};
 
-                    const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
-                                                     a[1] - x[1], b[1] - x[1]};
+        const TinyMatrix mr_inv_W = inverse(mr_W);
 
-                    const TinyMatrix mr_S = mr_A * mr_inv_W;
-                    const double mr_sigma = det(mr_S);
-                    if (mr_sigma < current_sigma_min) {
-                      current_sigma_min = mr_sigma;
-                    }
+        std::array<TinyMatrix<Dimension>, Dimension>                                      //
+          mr_S_gradient = {TinyMatrix<Dimension>{-1, (mr_cos_alpha - 1) / mr_sin_alpha,   //
+                                                 +0, +0},
+                           TinyMatrix<Dimension>{+0, +0,   //
+                                                 -1, (mr_cos_alpha - 1) / mr_sin_alpha}};
 
-                    if (print) {
-                      std::cout << "MC triangle 2 ===\n";
-                      std::cout << "MC a = " << a << '\n';
-                      std::cout << "MC b = " << b << '\n';
-                      std::cout << "MC x = " << x << '\n';
-                    }
-                  }
-                }
-              }
-            }
+        {   // first simplex
+          const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
+          const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]];
 
-            if (current_sigma_min < sigma_min) {
-              sigma_min = current_sigma_min;
-              delta     = (sigma_min < eps)
-                            ? std::max(std::sqrt(eps * (eps - sigma_min)), std::sqrt(eps) * std::abs(sigma_min))
-                            : 0;
-            }
+          const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
+                                           a[1] - x[1], b[1] - x[1]};
 
-            if (print) {
-              std::cout << " ==== \n";
-              std::cout << "delta = " << delta << '\n';
-            }
-          }
+          const TinyMatrix mr_S = mr_A * mr_inv_W;
+          const double mr_sigma = det(mr_S);
 
-          auto escobarSSigma = [&frobenius, delta,   //
-                                                     //
-                                &print, &node_to_cell_matrix,
-                                &node_id](const double sigma, const TinyMatrix<Dimension>& S,
-                                          const TinyVector<Dimension>& sigma_gradient,
-                                          const std::array<TinyMatrix<Dimension>, Dimension>& S_gradient,
-                                          const std::array<TinyMatrix<Dimension>, Dimension>& Sigma_gradient, double& f,
-                                          TinyVector<Dimension>& f_gradient, TinyMatrix<Dimension>& f_hessian) {
-            const TinyMatrix<Dimension> Sigma = [&S] {
-              TinyMatrix<Dimension> transposed_comS;
-              for (size_t i = 0; i < Dimension; ++i) {
-                for (size_t j = 0; j < Dimension; ++j) {
-                  transposed_comS(i, j) = cofactor(S, j, i);
-                }
+          const TinyMatrix<Dimension> mr_Sigma = [&mr_S] {
+            TinyMatrix<Dimension> transposed_comS;
+            for (size_t i = 0; i < Dimension; ++i) {
+              for (size_t j = 0; j < Dimension; ++j) {
+                transposed_comS(i, j) = cofactor(mr_S, j, i);
               }
-              return transposed_comS;
-            }();
+            }
+            return transposed_comS;
+          }();
+
+          TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]),   //
+                                                  trace(mr_Sigma * mr_S_gradient[1])};
 
-            const double S_norm      = frobenius(S);
-            const double Sigma_norm  = frobenius(Sigma);
-            const double S_norm2     = S_norm * S_norm;
-            const double Sigma_norm2 = Sigma_norm * Sigma_norm;
+          const std::array<TinyMatrix<Dimension>, Dimension>                                 //
+            mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha,   //
+                                                    +0, -1},
+                              TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0,   //
+                                                    +1, +0}};
 
-            double h = sigma + std::sqrt(sigma * sigma + 4 * delta * delta);
+          escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian);
+        }
 
-            const double f_jr = S_norm * Sigma_norm / h;
+        {   // second simplex
+          const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]];
+          const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
 
-            TinyVector<Dimension> g{trace(transpose(S) * S_gradient[0]) / S_norm2                   //
-                                      + trace(transpose(Sigma) * Sigma_gradient[0]) / Sigma_norm2   //
-                                      - trace(Sigma * S_gradient[0]) / (h - sigma),
-                                    //
-                                    trace(transpose(S) * S_gradient[1]) / S_norm2                   //
-                                      + trace(transpose(Sigma) * Sigma_gradient[1]) / Sigma_norm2   //
-                                      - trace(Sigma * S_gradient[1]) / (h - sigma)};
+          const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
+                                           a[1] - x[1], b[1] - x[1]};
 
-            const TinyVector<Dimension> f_jr_gradient = f_jr * g;
+          const TinyMatrix mr_S = mr_A * mr_inv_W;
+          const double mr_sigma = det(mr_S);
 
-            TinyMatrix<Dimension> f_jr_hessian = zero;
+          const TinyMatrix<Dimension> mr_Sigma = [&mr_S] {
+            TinyMatrix<Dimension> transposed_comS;
             for (size_t i = 0; i < Dimension; ++i) {
               for (size_t j = 0; j < Dimension; ++j) {
-                f_jr_hessian(i, j) =                                           //
-                  (trace(transpose(S_gradient[j]) * S_gradient[i]) / S_norm2   //
-                   - 2 * trace(transpose(S) * S_gradient[j]) * trace(transpose(S) * S_gradient[i]) /
-                       (S_norm2 * S_norm2)                                                   //
-                                                                                             //
-                   + trace(transpose(Sigma_gradient[j]) * Sigma_gradient[i]) / Sigma_norm2   // + 0
-                   - 2 * trace(transpose(Sigma) * Sigma_gradient[j]) * trace(transpose(Sigma) * Sigma_gradient[i]) /
-                       (Sigma_norm2 * Sigma_norm2)   //
-
-                   - trace(Sigma_gradient[j] * S_gradient[i]) / (h - sigma)                     //
-                   + sigma / (std::pow(h - sigma, 3)) * sigma_gradient[i] * sigma_gradient[j]   //
-                   + g[j] * g[i]) *
-                  f_jr;
+                transposed_comS(i, j) = cofactor(mr_S, j, i);
               }
             }
+            return transposed_comS;
+          }();
 
-            if (print) {
-              std::cout << "S     = " << S << '\n';
-              std::cout << "Sigma = " << Sigma << '\n';
+          TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]),   //
+                                                  trace(mr_Sigma * mr_S_gradient[1])};
 
-              std::cout << "normS     = " << S_norm << '\n';
-              std::cout << "normSigma = " << Sigma_norm << '\n';
+          const std::array<TinyMatrix<Dimension>, Dimension>                                 //
+            mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha,   //
+                                                    +0, -1},
+                              TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0,   //
+                                                    +1, +0}};
 
-              std::cout << "sigma = " << sigma << '\n';
-              std::cout << "f_jr  = " << f_jr << '\n';
-              std::cout << "g     = " << g << '\n';
-              std::cout << "grad_f_jr = " << f_jr_gradient << '\n';
-              std::cout << "hess_f_jr = " << f_jr_hessian << '\n';
-            }
+          escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian);
+        }
+      }
+    }
+  }
 
-            f += f_jr;
-            f_gradient += f_jr_gradient;
-            f_hessian += f_jr_hessian;
-          };
+  NodeValue<const double>
+  _getNodeQuality(const NodeValue<const TinyVector<Dimension>>& xr) const
+  {
+    if constexpr (Dimension == 2) {
+      NodeValue<double> node_quality{m_given_mesh.connectivity()};
+      for (NodeId node_id = 0; node_id < m_given_mesh.numberOfNodes(); ++node_id) {
+        if (not m_is_fixed_node[node_id]) {
+          double sigma_min = std::numeric_limits<double>::max();
+          double delta     = sigma_min;
+          double f;
+          TinyVector<Dimension> f_gradient;
+          TinyMatrix<Dimension> f_hessian;
+          get_f_grad_hess(xr, sigma_min, delta, node_id, xr[node_id], f, f_gradient, f_hessian);
+          node_quality[node_id] = f;
+        } else {
+          node_quality[node_id] = 1;
+        }
+      }
+      return node_quality;
 
-          auto escobar = escobarSSigma;
+    } else {
+      throw NotImplementedError("dimension != 2");
+    }
+  }
 
-          for (size_t i_cell = 0; i_cell < S_list.size(); ++i_cell) {
-            if (print) {
-              std::cout << "------ cell = " << i_cell << '\n';
-            }
+ public:
+  std::shared_ptr<const ItemValueVariant>
+  getQuality() const
+  {
+    return std::make_shared<ItemValueVariant>(this->_getNodeQuality(m_given_mesh.xr()));
+  }
 
-            const double sigma            = sigma_list[i_cell];
-            const TinyMatrix<Dimension> S = S_list[i_cell];
+  void
+  _getNewPositions(const NodeValue<const Rd>& old_xr, NodeValue<Rd>& new_xr) const
+  {
+    constexpr bool use_newton = false;
 
-            const TinyMatrix<Dimension> Sigma = [&S] {
-              TinyMatrix<Dimension> transposed_comS;
-              for (size_t i = 0; i < Dimension; ++i) {
-                for (size_t j = 0; j < Dimension; ++j) {
-                  transposed_comS(i, j) = cofactor(S, j, i);
-                }
-              }
-              return transposed_comS;
-            }();
+    const ConnectivityType& connectivity = m_given_mesh.connectivity();
 
-            TinyVector<Dimension> sigma_gradient{trace(Sigma * S_gradient[0]),   //
-                                                 trace(Sigma * S_gradient[1])};
+    auto is_boundary_node = connectivity.isBoundaryNode();
+    auto node_is_owned    = connectivity.nodeIsOwned();
 
-            const std::array<TinyMatrix<Dimension>, Dimension>   //
-              Sigma_gradient{TinyMatrix<Dimension>{+0, +1,       //
-                                                   +0, -1},
-                             TinyMatrix<Dimension>{-inv_sin_alpha, -inv_tan_alpha,   //
-                                                   +inv_sin_alpha, +inv_tan_alpha}};
+    if constexpr (Dimension == 2) {
+      auto cell_to_node_matrix        = connectivity.cellToNodeMatrix();
+      auto node_to_cell_matrix        = connectivity.nodeToCellMatrix();
+      auto node_to_edge_matrix        = connectivity.nodeToEdgeMatrix();
+      auto edge_to_node_matrix        = connectivity.edgeToNodeMatrix();
+      auto node_number_in_their_cells = connectivity.nodeLocalNumbersInTheirCells();
 
-            escobar(sigma, S, sigma_gradient, S_gradient, Sigma_gradient, f, f_gradient, f_hessian);
-          }
+      NodeValue<double> quality{connectivity};
 
-          if constexpr (multi_connection) {
-            for (size_t i_cell = 0; i_cell < cell_list.size(); ++i_cell) {
-              const CellId cell_id       = cell_list[i_cell];
-              auto cell_node_list        = cell_to_node_matrix[cell_list[i_cell]];
-              const size_t cell_nb_nodes = cell_node_list.size();
+      quality.fill(2);
 
-              if (print) {
-                std::cout << "=== MC cell " << i_cell << " ===\n";
-              }
+      size_t maxiter = 0;
 
-              if (cell_nb_nodes <= 3) {
-                continue;
-              }
+      auto smooth = [this, &maxiter, &old_xr](const NodeId node_id, TinyVector<Dimension>& x) -> double {
+        //        SmallArray<TinyMatrix<Dimension>> S_list(cell_list.size());
 
-              const size_t i_cell_node = node_number_in_cell[i_cell];
+        double delta = 0;
 
-              const double mr_alpha = 2 * std::acos(-1) / ((cell_nb_nodes - 2) * (cell_list.size()));
-              if (print) {
-                std::cout << "=== MC mr_alpha " << mr_alpha << " ===\n";
-              }
-              const double mr_sin_alpha     = std::sin(mr_alpha);
-              const double mr_cos_alpha     = std::cos(mr_alpha);
-              const double mr_inv_sin_alpha = 1. / mr_sin_alpha;
-              const double mr_inv_tan_alpha = 1. / std::tan(mr_alpha);
+        double final_f = 0;
 
-              const TinyMatrix<Dimension> mr_W{1, mr_cos_alpha,   //
-                                               0, mr_sin_alpha};
+        TinyMatrix<Dimension> H = identity;
 
-              const TinyMatrix mr_inv_W = inverse(mr_W);
+        const ConnectivityType& connectivity = m_given_mesh.connectivity();
 
-              std::array<TinyMatrix<Dimension>, Dimension>                                      //
-                mr_S_gradient = {TinyMatrix<Dimension>{-1, (mr_cos_alpha - 1) / mr_sin_alpha,   //
-                                                       +0, +0},
-                                 TinyMatrix<Dimension>{+0, +0,   //
-                                                       -1, (mr_cos_alpha - 1) / mr_sin_alpha}};
+        for (size_t i_iter = 0; i_iter < 200; ++i_iter) {
+          double sigma_min = std::numeric_limits<double>::max();
+          delta            = sigma_min;
 
-              {   // first simplex
-                const TinyVector a = old_xr[cell_node_list[(i_cell_node + 1) % cell_nb_nodes]];
-                const TinyVector b = old_xr[cell_node_list[(i_cell_node + 2) % cell_nb_nodes]];
+          double f                         = 0;
+          TinyVector<Dimension> f_gradient = zero;
+          TinyMatrix<Dimension> f_hessian  = zero;
 
-                const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
-                                                 a[1] - x[1], b[1] - x[1]};
+          auto node_to_edge_matrix = connectivity.nodeToEdgeMatrix();
+          auto edge_to_node_matrix = connectivity.edgeToNodeMatrix();
 
-                const TinyMatrix mr_S = mr_A * mr_inv_W;
-                const double mr_sigma = det(mr_S);
+          get_f_grad_hess(old_xr, sigma_min, delta, node_id, x, f, f_gradient, f_hessian);
 
-                const TinyMatrix<Dimension> mr_Sigma = [&mr_S] {
-                  TinyMatrix<Dimension> transposed_comS;
-                  for (size_t i = 0; i < Dimension; ++i) {
-                    for (size_t j = 0; j < Dimension; ++j) {
-                      transposed_comS(i, j) = cofactor(mr_S, j, i);
-                    }
-                  }
-                  return transposed_comS;
-                }();
+          const double max_edge_lenght = [=] {
+            auto node_edge_list = node_to_edge_matrix[node_id];
+            double max_lenght   = 0;
+            for (size_t i_edge = 0; i_edge < node_edge_list.size(); ++i_edge) {
+              const EdgeId edge_id            = node_edge_list[i_edge];
+              auto edge_node_list             = edge_to_node_matrix[edge_id];
+              const TinyVector<Dimension>& x0 = old_xr[edge_node_list[0]];
+              const TinyVector<Dimension>& x1 = old_xr[edge_node_list[1]];
+              const double lenght             = l2Norm(x0 - x1);
+              if (lenght > max_lenght) {
+                max_lenght = lenght;
+              }
+            }
+            return max_lenght;
+          }();
 
-                TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]),   //
-                                                        trace(mr_Sigma * mr_S_gradient[1])};
+          std::cout.precision(15);
 
-                const std::array<TinyMatrix<Dimension>, Dimension>                                 //
-                  mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha,   //
-                                                          +0, -1},
-                                    TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0,   //
-                                                          +1, +0}};
+          if constexpr (use_newton) {
+            const TinyVector delta_x_candidate = -inverse(f_hessian) * f_gradient;
 
-                if (print) {
-                  std::cout << "=== MC triangle 1 ===\n";
-                  std::cout << " A      = " << mr_A << '\n';
-                  std::cout << " W      = " << mr_W << '\n';
-                  std::cout << " inv(W) = " << mr_inv_W << '\n';
+            const double delta_x_norm = l2Norm(delta_x_candidate);
 
-                  std::cout << " A * inv(W) = " << mr_A * mr_inv_W << '\n';
+            double factor = 1;
+            if (delta_x_norm > 0.2 * max_edge_lenght) {
+              factor *= (0.2 * max_edge_lenght / delta_x_norm);
+            }
+            x += factor * delta_x_candidate;
 
-                  std::cout << " mr_S = " << mr_S << '\n';
-                  std::cout << " mr_Sigma = " << mr_Sigma << '\n';
+            if (delta_x_norm < 1E-8 * max_edge_lenght) {
+              break;
+            }
+          } else {
+            constexpr double c1 = 1e-4;
+            constexpr double c2 = 0.7;
 
-                  std::cout << " mr_S_gradient = " << mr_S_gradient[0] << " || " << mr_S_gradient[1] << '\n';
-                  std::cout << " mr_Sigma_gradient = " << mr_Sigma_gradient[0] << " || " << mr_Sigma_gradient[1]
-                            << '\n';
-                }
+            auto zoom = [&](const TinyVector<Dimension>& x, const TinyVector<Dimension>& n, const double& f0,
+                            const TinyVector<Dimension>& f_gradient0, const double& df0_dn, double alpha_low,
+                            double alpha_high) -> double {
+              double f_low                         = 0;
+              TinyVector<Dimension> f_gradient_low = zero;
+              TinyMatrix<Dimension> f_hessian_low  = zero;
 
-                escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian);
-              }
+              double init_delta = alpha_high - alpha_low;
 
-              {   // second simplex
-                const TinyVector a = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 2) % cell_nb_nodes]];
-                const TinyVector b = old_xr[cell_node_list[(i_cell_node + cell_nb_nodes - 1) % cell_nb_nodes]];
+              get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_low * f_gradient0, f_low, f_gradient_low,
+                              f_hessian_low);
 
-                const TinyMatrix<Dimension> mr_A{a[0] - x[0], b[0] - x[0],   //
-                                                 a[1] - x[1], b[1] - x[1]};
+              double f_high                         = 0;
+              TinyVector<Dimension> f_gradient_high = zero;
+              TinyMatrix<Dimension> f_hessian_high  = zero;
 
-                const TinyMatrix mr_S = mr_A * mr_inv_W;
-                const double mr_sigma = det(mr_S);
+              get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_high * f_gradient0, f_high, f_gradient_high,
+                              f_hessian_high);
 
-                const TinyMatrix<Dimension> mr_Sigma = [&mr_S] {
-                  TinyMatrix<Dimension> transposed_comS;
-                  for (size_t i = 0; i < Dimension; ++i) {
-                    for (size_t j = 0; j < Dimension; ++j) {
-                      transposed_comS(i, j) = cofactor(mr_S, j, i);
-                    }
-                  }
-                  return transposed_comS;
-                }();
+              // std::cout << "-- f_gradient     =" << f_gradient0 << " n=" << n << " df0_n=" << df0_dn << '\n';
+              // std::cout << "   f_gradient_low =" << f_gradient_low << " n=" << n   //
+              //           << " df_low_n=" << dot(f_gradient_low, n) << '\n';
+              // std::cout << "   f_gradient_high=" << f_gradient_high << " n=" << n
+              //           << " df_high_n=" << dot(f_gradient_high, n) << '\n';
 
-                TinyVector<Dimension> mr_sigma_gradient{trace(mr_Sigma * mr_S_gradient[0]),   //
-                                                        trace(mr_Sigma * mr_S_gradient[1])};
+              // std::cout << "starting box [" << alpha_low << ", " << alpha_high << "] --(f)--> [" << f_low << ", "
+              //           << f_high << "]\n";
 
-                const std::array<TinyMatrix<Dimension>, Dimension>                                 //
-                  mr_Sigma_gradient{TinyMatrix<Dimension>{+0, (1 - mr_cos_alpha) / mr_sin_alpha,   //
-                                                          +0, -1},
-                                    TinyMatrix<Dimension>{(mr_cos_alpha - 1) / mr_sin_alpha, +0,   //
-                                                          +1, +0}};
+              for (size_t i = 1; i <= 200; ++i) {
+                // std::cout << " box: " << alpha_low << " : " << alpha_high << '\n';
+                if (alpha_high - alpha_low <= 1E-5 * init_delta) {
+                  return alpha_low;
+                }
+                double alpha_i = 0.5 * (alpha_low + alpha_high);
+                // std::cout << i << ": alpha_i = " << alpha_i << "[alpha_low : alpha_max] = " << alpha_low << " : "
+                //           << alpha_high << "diff = " << alpha_high - alpha_low << '\n';
 
-                if (print) {
-                  std::cout << "=== MC triangle 2 ===\n";
-                  std::cout << " A = " << mr_A << '\n';
-                  std::cout << " W      = " << mr_W << '\n';
-                  std::cout << " inv(W) = " << mr_inv_W << '\n';
+                double f_i                         = 0;
+                TinyVector<Dimension> f_gradient_i = zero;
+                TinyMatrix<Dimension> f_hessian_i  = zero;
 
-                  std::cout << " A * inv(W) = " << mr_A * mr_inv_W << '\n';
+                get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_i * f_gradient0, f_i, f_gradient_i,
+                                f_hessian_i);
 
-                  std::cout << " mr_S = " << mr_S << '\n';
-                  std::cout << " mr_Sigma = " << mr_Sigma << '\n';
+                // std::cout << "   f_gradient_low =" << f_gradient_low << " n=" << n   //
+                //           << " df_low_n=" << dot(f_gradient_low, n) << '\n';
 
-                  std::cout << " mr_S_gradient = " << mr_S_gradient[0] << " || " << mr_S_gradient[1] << '\n';
-                  std::cout << " mr_Sigma_gradient = " << mr_Sigma_gradient[0] << " || " << mr_Sigma_gradient[1]
-                            << '\n';
-                }
+                // std::cout << "f0 = " << f0 << " f_i = " << f_i << '\n';
 
-                escobar(mr_sigma, mr_S, mr_sigma_gradient, mr_S_gradient, mr_Sigma_gradient, f, f_gradient, f_hessian);
-              }
-            }
-          }
+                if ((f_i > f0 + c1 * alpha_i * df0_dn) or (f_i >= f_low)) {
+                  alpha_high = alpha_i;
+                  f_high     = f_i;
+                } else {
+                  double dfi_dn = dot(f_gradient_i, n);
+                  if (std::abs(dfi_dn) <= -c2 * df0_dn) {
+                    return alpha_i;
+                  }
+                  if (dfi_dn * (alpha_high - alpha_low) >= 0) {
+                    alpha_high = alpha_low;
+                    f_high     = f_low;
+                  }
 
-          const double max_edge_lenght = [=] {
-            auto node_edge_list = node_to_edge_matrix[node_id];
-            double max_lenght   = 0;
-            for (size_t i_edge = 0; i_edge < node_edge_list.size(); ++i_edge) {
-              const EdgeId edge_id            = node_edge_list[i_edge];
-              auto edge_node_list             = edge_to_node_matrix[edge_id];
-              const TinyVector<Dimension>& x0 = old_xr[edge_node_list[0]];
-              const TinyVector<Dimension>& x1 = old_xr[edge_node_list[1]];
-              const double lenght             = l2Norm(x0 - x1);
-              if (lenght > max_lenght) {
-                max_lenght = lenght;
+                  alpha_low = alpha_i;
+                  f_low     = f_i;
+                }
+              }
+              throw UnexpectedError("zoom did not converge!");
+            };
+
+            auto line_search = [&](const double& f0, const TinyVector<Dimension>& f_gradient0,
+                                   const double alpha_max) -> double {
+              double alpha_i_1 = 0;
+              double alpha_i   = alpha_max;
+
+              const TinyVector<Dimension> n = -1 / l2Norm(f_gradient0) * f_gradient0;
+
+              const double df0_dn = dot(f_gradient0, n);
+
+              double f_i_1 = f0;
+
+              double f_i                         = 0;
+              TinyVector<Dimension> f_gradient_i = zero;
+              TinyMatrix<Dimension> f_hessian_i  = zero;
+
+              for (size_t i = 1; i <= 100; ++i) {
+                get_f_grad_hess(old_xr, sigma_min, delta, node_id, x - alpha_i * f_gradient0, f_i, f_gradient_i,
+                                f_hessian_i);
+
+                const double dfi_dn = dot(f_gradient_i, n);
+
+                // std::cout << "=== i=" << i << " alpha_i=" << alpha_i << " alpha_i_1=" << alpha_i_1 << " f0=" << f0
+                //           << " f_i=" << f_i << " f_i_1=" << f_i_1 << '\n';
+                if ((f_i > f0 + c1 * alpha_i * df0_dn) or ((f_i >= f_i_1) and (i > 1))) {
+                  // std::cout << "case 1: zoom[" << alpha_i << "," << alpha_i_1 << "] f_i=" << f_i << " f0=" << f0
+                  //           << '\n';
+                  return zoom(x, n, f0, f_gradient0, df0_dn, alpha_i_1, alpha_i);
+                } else if (std::abs(dfi_dn) <= -c2 * df0_dn) {
+                  // std::cout << "case 2: return alpha_i=" << alpha_i << '\n';
+                  return alpha_i;
+                } else if (dfi_dn >= 0) {
+                  // std::cout << "case 3: zoom[" << alpha_i << "," << alpha_i_1 << "] f_i=" << f_i << " f0=" << f0
+                  //           << '\n';
+                  return zoom(x, n, f0, f_gradient0, df0_dn, alpha_i, alpha_i_1);
+                } else {
+                  alpha_i_1 = alpha_i;
+                  f_i_1     = f_i;
+                  alpha_i   = 0.5 * (alpha_i + alpha_max);
+                }
               }
+              throw UnexpectedError("line search failed");
+            };
+
+            if (l2Norm(f_gradient) < 1e-12 * f) {
+              continue;
             }
-            return max_lenght;
-          }();
 
-          const TinyVector delta_x_candidate = -inverse(f_hessian) * f_gradient;
+            const double alpha_max = std::min(0.5 * max_edge_lenght / l2Norm(f_gradient), 1.);
+            //            std::cout << rang::fgB::yellow << "i_iter=" << i_iter << rang::fg::reset << '\n';
+            double alpha = line_search(f, f_gradient, alpha_max);
 
-          const double delta_x_norm = l2Norm(delta_x_candidate);
+            const TinyVector delta_x = -alpha * H * f_gradient;
 
-          if (print) {
-            std::cout << "==============\n";
-            std::cout << "i_iter = " << i_iter << '\n';
-            std::cout << "f_gradient = " << f_gradient << '\n';
-            std::cout << "f_hessian  = " << f_hessian << '\n';
-            std::cout << "inv(f_hes) = " << inverse(f_hessian) << '\n';
-            std::cout << "delta_x_candidate= " << delta_x_candidate << '\n';
-          }
+            double new_f                     = 0;
+            TinyVector<Dimension> new_f_grad = zero;
+            get_f_grad_hess(old_xr, sigma_min, delta, node_id, x + delta_x, new_f, new_f_grad, f_hessian);
 
-          double factor = 1;
-          if (delta_x_norm > 0.2 * max_edge_lenght) {
-            factor *= (0.2 * max_edge_lenght / delta_x_norm);
-          }
-          x += factor * delta_x_candidate;
+            x += delta_x;
 
-          if (print) {
-            std::cout << "factor = " << factor << '\n';
-            std::cout << "delta_x          = " << factor * delta_x_candidate << '\n';
-          }
-          if (l2Norm(f_gradient) < 1E-12) {
-            break;
+            // std::cout << "x -> " << x << '\n';
+            // std::cout << rang::fgB::red << "f(x)=" << f << " f(x+dx)=" << new_f << " delta = " << delta_x
+            //           << rang::fg::reset << '\n';
+            // std::cout << rang::fgB::magenta << "l2Norm(delta_x) = " << l2Norm(delta_x) << rang::fg::reset << '\n';
+
+            if (l2Norm(delta_x) < 1E-8 * max_edge_lenght) {
+              break;
+            } else {
+              if (((i_iter + 1) % 5) == 0) {
+                H = identity;
+              } else {
+                TinyVector y = new_f_grad - f_gradient;
+                // std::cout << "l2Norm(y) = " << l2Norm(y) << '\n';
+                const double rho = 1 / dot(delta_x, y);
+
+                constexpr TinyMatrix<Dimension> I = identity;
+
+                H = (I - rho * tensorProduct(delta_x, y)) * H * (I - rho * tensorProduct(y, delta_x)) +
+                    rho * tensorProduct(delta_x, delta_x);
+              }
+            }
           }
 
-          if (delta_x_norm < 1E-8 * max_edge_lenght) {
+          if (l2Norm(f_gradient) < 1E-12) {
             break;
           }
 
           final_f = f;
         }
+        //        std::exit(0);
 
         return final_f;
       };
@@ -954,6 +1053,9 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
       for (NodeId node_id = 0; node_id < connectivity.numberOfNodes(); ++node_id) {
         if (node_is_owned[node_id] and m_is_smoothed_node[node_id] and not is_boundary_node[node_id]) {
           quality[node_id] = smooth(node_id, new_xr[node_id]);
+          new_xr[node_id]  = 0.5 * (new_xr[node_id] + old_xr[node_id]);
+          // TinyVector<Dimension> x0{2, 3};
+          // quality[node_id] = smooth(node_id, x0);
           count++;
         }
       }
@@ -975,15 +1077,21 @@ class MeshSmootherEscobarHandler::MeshSmootherEscobar
     do {
       std::cout << "- convexfication step " << i_convexify++ << '\n';
       NodeValue<Rd> new_xr = copy(xr);
-      this->_getNewPositions(xr, new_xr);
-      xr                       = new_xr;
+
+      auto node_quality = this->_getNodeQuality(xr);
+
+      bool need_smooth = max(node_quality) > 50;
+
       auto non_convex_cell_tag = this->_getNonConvexCellTag(xr);
       nb_non_convex_cells      = sum(non_convex_cell_tag);
+
       std::cout << "  remaining non convex cells: " << nb_non_convex_cells << '\n';
       if (nb_non_convex_cells > 0) {
         m_is_smoothed_node = _getSmoothedNode(non_convex_cell_tag, m_is_fixed_node);
+        this->_getNewPositions(xr, new_xr);
+        xr = new_xr;
       }
-    } while ((nb_non_convex_cells > 0) and (i_convexify < 0));
+    } while ((nb_non_convex_cells > 0) and (i_convexify < 10000));
 
     return std::make_shared<MeshType>(m_given_mesh.shared_connectivity(), xr);
   }
-- 
GitLab