FESADev/src/fesa/analysis/hht_dynamic_algebra.cpp

#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <exception>
#include <iomanip>
#include <limits>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include "analysis/hht_dynamic_internal.h"
#include "fesa/math/matrix.h"
#include "fesa/solvers/linear/linear_solver.h"
#include "fesa/solvers/linear/tangent_solver.h"
#include "math/dense_blas_internal.h"

namespace fesa {
namespace hht_internal {
namespace {

/// @brief Attaches a fail-closed dynamic diagnostic to the source model.
Status Failure(const Domain& domain, const std::string& code,
               const std::string& message) {
  return Status::Failure(FailureCategory::kSolver, {{Severity::kError,
                                                     code,
                                                     {domain.SourcePath(), 0U},
                                                     "*DYNAMIC",
                                                     {},
                                                     message}});
}

/// @brief Reads one structural entry, including an implicit exact zero.
double Entry(const SparseMatrix& matrix, const std::size_t row,
             const std::size_t column) {
  const auto first = matrix.ColumnIndices().begin() +
                     static_cast<std::ptrdiff_t>(matrix.RowOffsets()[row]);
  const auto last = matrix.ColumnIndices().begin() +
                    static_cast<std::ptrdiff_t>(matrix.RowOffsets()[row + 1U]);
  const auto found = std::lower_bound(first, last, column);
  return found != last && *found == column
             ? matrix.Values()[static_cast<std::size_t>(
                   found - matrix.ColumnIndices().begin())]
             : 0.0;
}

/// @brief Applies the fixed translation length scale by full-space row.
double Scale(const std::size_t full_dof, const double length) {
  return full_dof % 6U < 3U ? length : 1.0;
}

/// @brief Checks a dimensionless ratio without inventing a zero-scale floor.
bool Within(const double value, const double scale, const double tolerance) {
  return std::isfinite(value) && std::isfinite(scale) && scale >= 0.0 &&
         (scale == 0.0 ? value == 0.0 : std::abs(value) / scale <= tolerance);
}

/// @brief Verifies dimensions and finite values before algebraic contractions.
Status Validate(const Domain& domain, const DofManager& dofs,
                const DynamicAssembly& assembly, const double length) {
  const std::size_t count = dofs.FullDofCount();
  if (!std::isfinite(length) || !(length > 0.0) || count % 6U != 0U ||
      assembly.rotations.size() != count / 6U) {
    return Failure(domain, "dynamic-invalid-algebra-state",
                   "Dynamic algebra requires complete rotations and a positive "
                   "reference length.");
  }
  for (const Vector* vector :
       {&assembly.frame.velocity, &assembly.frame.acceleration,
        &assembly.frame.internal_force, &assembly.frame.external_force,
        &assembly.frame.inertia_force, &assembly.frame.residual_scale,
        &assembly.force_directional_second,
        &assembly.force_directional_second_scale}) {
    if (vector->Size() != count) {
      return Failure(
          domain, "dynamic-invalid-algebra-state",
          "Dynamic algebra vectors must match the full equation space.");
    }
    for (std::size_t i = 0; i < count; ++i) {
      if (!std::isfinite((*vector)[i])) {
        return Failure(domain, "dynamic-invalid-algebra-state",
                       "Dynamic algebra vectors must remain finite.");
      }
    }
  }
  for (const Vector* vector :
       {&assembly.hidden_second, &assembly.hidden_second_scale}) {
    if (vector->Size() != assembly.rotations.size()) {
      return Failure(
          domain, "dynamic-invalid-algebra-state",
          "Exact scalar hidden Hessian evidence must cover every node.");
    }
    for (std::size_t i = 0; i < vector->Size(); ++i) {
      if (!std::isfinite((*vector)[i]) ||
          (vector == &assembly.hidden_second_scale && (*vector)[i] < 0.0)) {
        return Failure(domain, "dynamic-invalid-algebra-state",
                       "Scalar hidden Hessian evidence must be finite with "
                       "nonnegative scale.");
      }
    }
  }
  for (const SparseMatrix* matrix : {&assembly.mass, &assembly.internal_tangent,
                                     &assembly.drilling_metric}) {
    if (matrix->Rows() != count || matrix->Columns() != count ||
        !matrix->Validate().IsOk()) {
      return Failure(
          domain, "dynamic-invalid-algebra-state",
          "Dynamic algebra operators must match the full equation space.");
    }
  }
  for (const auto& rotation : assembly.rotations) {
    if (!rotation.drill_null.IsFinite() ||
        !(std::abs(rotation.drill_null.Dot(rotation.drill_null) - 1.0) <=
          1.0e-12)) {
      return Failure(
          domain, "dynamic-invalid-analytic-null",
          "Every analytic drilling null must remain finite and unit length.");
    }
  }
  return Status::Ok();
}

using BasisRow = std::vector<std::pair<std::size_t, double>>;

/// @brief Holds sparse nodal physical-complement columns and their mass.
struct PhysicalSpace {
  std::vector<BasisRow> rows;
  SparseMatrix mass;
};

/// @brief Checks a nonempty nodal physical block without removing small pivots.
Status CheckLocalMass(const Domain& domain, const SparseMatrix& matrix,
                      const std::size_t begin, const std::size_t end) {
  if (begin == end) return Status::Ok();
  const std::size_t size = end - begin;
  const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
  Matrix block{size, size};
  double scale = 0.0;
  for (std::size_t i = 0; i < size; ++i) {
    for (std::size_t j = 0; j < size; ++j) {
      block(i, j) = Entry(matrix, begin + i, begin + j);
      scale = std::max(scale, std::abs(block(i, j)));
    }
  }
  if (!std::isfinite(scale) || !(scale > 0.0)) {
    return Failure(domain, "dynamic-positive-mass",
                   "A nonempty physical mass block has no positive scale.");
  }
  double norm = 0.0;
  for (std::size_t i = 0; i < size; ++i) {
    double sum = 0.0;
    for (std::size_t j = 0; j < size; ++j) {
      block(i, j) /= scale;
      sum += std::abs(block(i, j));
    }
    norm = std::max(norm, sum);
  }
  Matrix lower{size, size};
  Matrix inverse_lower{size, size};
  auto factor_failure = dense_blas_internal::CholeskyFailure::kNone;
  try {
    if (!dense_blas_internal::CholeskyLowerAndInverse(
            block, lower, inverse_lower, &factor_failure)) {
      if (factor_failure == dense_blas_internal::CholeskyFailure::kInverse)
        return Failure(
            domain, "dynamic-positive-mass-conditioning",
            "A nodal free physical block reciprocal condition is below 2^-32.");
      return Failure(
          domain, "dynamic-positive-mass",
          "Physical mass requires strictly positive Cholesky pivots.");
    }
  } catch (const std::exception& error) {
    return Failure(domain, "dynamic-positive-mass", error.what());
  }
  const Matrix inverse =
      dense_blas_internal::MultiplyTransposeLeft(inverse_lower, inverse_lower);
  std::array<double, 6> sums{};
  for (std::size_t column = 0; column < size; ++column) {
    for (std::size_t i = 0; i < size; ++i) {
      sums[i] += std::abs(inverse(i, column));
    }
  }
  const double inverse_norm = *std::max_element(sums.begin(), sums.end());
  const double rcond = 1.0 / norm / inverse_norm;
  if (!std::isfinite(inverse_norm) || !std::isfinite(rcond) ||
      !(rcond >= std::ldexp(1.0, -32))) {
    return Failure(
        domain, "dynamic-positive-mass-conditioning",
        "A nodal free physical block reciprocal condition is below 2^-32.");
  }
  return Status::Ok();
}

/// @brief Builds the actual free-space analytic complement, without rank cuts.
Result<PhysicalSpace> BuildPhysicalSpace(
    const Domain& domain, const DofManager& dofs,
    const DynamicAssembly& assembly, const double length,
    const std::vector<std::size_t>& null_nodes, LinearSolver& solver) {
  const auto scaled = ScaledFree(assembly.mass, dofs, length);
  if (!scaled.HasValue())
    return Result<PhysicalSpace>::Failure(scaled.GetStatus());
  const SparseMatrix& mass = scaled.Value();
  double mass_scale = 0.0;
  for (const double entry : mass.Values())
    mass_scale = std::max(mass_scale, std::abs(entry));
  const std::size_t no_null = std::numeric_limits<std::size_t>::max();
  std::vector<std::size_t> node_null(assembly.rotations.size(), no_null);
  for (std::size_t i = 0; i < null_nodes.size(); ++i)
    node_null[null_nodes[i]] = i;
  double mass_norm = 0.0;
  double symmetry_error = 0.0;
  double null_error = 0.0;
  if (mass_scale > 0.0) {
    Vector normalized_mass{mass.Values().size()};
    Vector null_coefficients{mass.Values().size()};
    for (std::size_t index = 0U; index < mass.Values().size(); ++index) {
      normalized_mass[index] = mass.Values()[index] / mass_scale;
      const auto full = dofs.FreeDofs()[mass.ColumnIndices()[index]];
      if (full % 6U >= 3U && node_null[full / 6U] != no_null)
        null_coefficients[index] =
            assembly.rotations[full / 6U].drill_null[full % 6U - 3U];
    }
    Vector null_products{0U};
    {
      const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
      null_products = dense_blas_internal::MultiplyDiagonal(normalized_mass,
                                                            null_coefficients);
    }
    for (std::size_t row = 0; row < mass.Rows(); ++row) {
      std::vector<double> action(null_nodes.size(), 0.0);
      for (std::size_t index = mass.RowOffsets()[row];
           index < mass.RowOffsets()[row + 1U]; ++index) {
        const std::size_t column = mass.ColumnIndices()[index];
        const double entry = normalized_mass[index];
        mass_norm = std::hypot(mass_norm, entry);
        symmetry_error = std::hypot(
            symmetry_error, entry - Entry(mass, column, row) / mass_scale);
        const std::size_t full = dofs.FreeDofs()[column];
        if (full % 6U >= 3U && node_null[full / 6U] != no_null) {
          action[node_null[full / 6U]] += null_products[index];
        }
      }
      for (const double value : action)
        null_error = std::hypot(null_error, value);
    }
  }
  if ((mass_norm > 0.0 && symmetry_error / mass_norm > 1.0e-12) ||
      (!null_nodes.empty() && mass_norm > 0.0 &&
       null_error / mass_norm /
               std::sqrt(static_cast<double>(null_nodes.size())) >
           1.0e-12)) {
    return Result<PhysicalSpace>::Failure(
        Failure(domain, "dynamic-invalid-mass-null",
                "Scaled free mass symmetry or analytic null action failed."));
  }
  std::vector<BasisRow> rows(dofs.FullDofCount());
  std::vector<std::size_t> node_starts;
  std::size_t dimension = 0U;
  for (std::size_t node = 0; node < assembly.rotations.size(); ++node) {
    node_starts.push_back(dimension);
    for (std::size_t component = 0; component < 3; ++component) {
      if (dofs.FreeEquation(6U * node + component).has_value()) {
        rows[6U * node + component].push_back({dimension++, 1.0});
      }
    }
    std::vector<std::size_t> free_rotations;
    for (std::size_t component = 0; component < 3; ++component) {
      if (dofs.FreeEquation(6U * node + 3U + component).has_value()) {
        free_rotations.push_back(component);
      }
    }
    if (node_null[node] == no_null) {
      for (const std::size_t component : free_rotations) {
        rows[6U * node + 3U + component].push_back({dimension++, 1.0});
      }
      continue;
    }
    const Vector3 n = assembly.rotations[node].drill_null;
    std::stable_sort(free_rotations.begin(), free_rotations.end(),
                     [&](const std::size_t a, const std::size_t b) {
                       return std::abs(n[a]) < std::abs(n[b]);
                     });
    std::vector<Vector3> basis;
    for (std::size_t column = 0; column + 1U < free_rotations.size();
         ++column) {
      std::array<double, 3> axis{};
      axis[free_rotations[column]] = 1.0;
      Vector3 tangent = Vector3{axis} - n[free_rotations[column]] * n;
      for (const Vector3& previous : basis)
        tangent = tangent - tangent.Dot(previous) * previous;
      const double norm = tangent.Norm();
      if (!std::isfinite(norm) || !(norm > 0.0)) {
        return Result<PhysicalSpace>::Failure(Failure(
            domain, "dynamic-invalid-analytic-null",
            "The free physical complement lost its analytic dimension."));
      }
      tangent = tangent / norm;
      basis.push_back(tangent);
      for (const std::size_t component : free_rotations) {
        rows[6U * node + 3U + component].push_back(
            {dimension, tangent[component]});
      }
      ++dimension;
    }
  }
  std::vector<CooContribution> entries;
  std::vector<std::pair<double, double>> mass_factors;
  node_starts.push_back(dimension);
  double complement_gram_error = 0.0;
  double null_gram_error = 0.0;
  double cross_gram_error = 0.0;
  for (std::size_t node = 0; node < assembly.rotations.size(); ++node) {
    const std::size_t first = node_starts[node];
    const std::size_t last = node_starts[node + 1U];
    std::size_t free_count = 0U;
    for (std::size_t c = 0; c < 6; ++c) {
      if (dofs.FreeEquation(6U * node + c).has_value()) ++free_count;
    }
    const bool has_null = node_null[node] != no_null;
    if (last - first + (has_null ? 1U : 0U) != free_count) {
      return Result<PhysicalSpace>::Failure(Failure(
          domain, "dynamic-invalid-analytic-null",
          "The free physical and null bases do not span the free space."));
    }
    for (std::size_t a = first; a < last; ++a) {
      for (std::size_t b = first; b < last; ++b) {
        double gram = a == b ? -1.0 : 0.0;
        for (std::size_t c = 0; c < 6; ++c) {
          double pa = 0.0;
          double pb = 0.0;
          for (const auto& entry : rows[6U * node + c]) {
            if (entry.first == a) pa = entry.second;
            if (entry.first == b) pb = entry.second;
          }
          gram += pa * pb;
        }
        complement_gram_error = std::hypot(complement_gram_error, gram);
      }
      if (has_null) {
        double gram = 0.0;
        for (std::size_t c = 0; c < 3; ++c) {
          for (const auto& entry : rows[6U * node + 3U + c]) {
            if (entry.first == a)
              gram += entry.second * assembly.rotations[node].drill_null[c];
          }
        }
        cross_gram_error = std::hypot(cross_gram_error, gram);
      }
    }
    if (has_null)
      null_gram_error =
          std::hypot(null_gram_error, assembly.rotations[node].drill_null.Dot(
                                          assembly.rotations[node].drill_null) -
                                          1.0);
  }
  const double full_gram_error = std::hypot(
      complement_gram_error,
      std::hypot(null_gram_error, std::sqrt(2.0) * cross_gram_error));
  if (!(cross_gram_error <= 1.0e-12) || !(full_gram_error <= 1.0e-12) ||
      (!null_nodes.empty() &&
       !(null_gram_error / std::sqrt(static_cast<double>(null_nodes.size())) <=
         1.0e-12))) {
    return Result<PhysicalSpace>::Failure(Failure(
        domain, "dynamic-invalid-analytic-null",
        "The scaled physical and analytic null Gram identities failed."));
  }
  for (std::size_t i = 0; i < dimension; ++i) {
    entries.push_back({i, i, 0.0, 0U, entries.size()});
    mass_factors.push_back({0.0, 0.0});
  }
  for (std::size_t row = 0; row < mass.Rows(); ++row) {
    const auto& left = rows[dofs.FreeDofs()[row]];
    for (std::size_t index = mass.RowOffsets()[row];
         index < mass.RowOffsets()[row + 1U]; ++index) {
      const auto& right = rows[dofs.FreeDofs()[mass.ColumnIndices()[index]]];
      for (const auto& a : left) {
        for (const auto& b : right) {
          entries.push_back({a.first, b.first, a.second, 0U, entries.size()});
          mass_factors.push_back({mass.Values()[index], b.second});
        }
      }
    }
  }
  Vector coefficients{entries.size()};
  Vector mass_values{entries.size()};
  Vector right_values{entries.size()};
  for (std::size_t i = 0U; i < entries.size(); ++i) {
    coefficients[i] = entries[i].value;
    mass_values[i] = mass_factors[i].first;
    right_values[i] = mass_factors[i].second;
  }
  Vector projected{0U};
  {
    const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
    projected =
        dense_blas_internal::MultiplyDiagonal(coefficients, mass_values);
    bool finite = true;
    for (std::size_t i = 0U; i < projected.Size(); ++i) {
      entries[i].value = projected[i];
      finite = finite && std::isfinite(projected[i]);
    }
    // A rejected intermediate cannot produce a finite original contribution.
    // Still compute the remaining finite entries so an earlier final-product
    // overflow keeps its diagnostic priority. No invalid operand reaches MKL.
    if (!finite) {
      std::vector<std::size_t> valid_positions;
      for (std::size_t i = 0U; i < projected.Size(); ++i)
        if (std::isfinite(projected[i])) valid_positions.push_back(i);
      Vector valid_left{valid_positions.size()};
      Vector valid_right{valid_positions.size()};
      for (std::size_t i = 0U; i < valid_positions.size(); ++i) {
        valid_left[i] = projected[valid_positions[i]];
        valid_right[i] = right_values[valid_positions[i]];
      }
      const Vector valid_products =
          dense_blas_internal::MultiplyDiagonal(valid_left, valid_right);
      for (std::size_t i = 0U; i < valid_positions.size(); ++i)
        entries[valid_positions[i]].value = valid_products[i];
      auto rejected = MakeSparse(dimension, dimension, std::move(entries));
      return Result<PhysicalSpace>::Failure(rejected.GetStatus());
    }
    projected = dense_blas_internal::MultiplyDiagonal(projected, right_values);
  }
  for (std::size_t i = 0U; i < entries.size(); ++i)
    entries[i].value = projected[i];
  auto block = MakeSparse(dimension, dimension, std::move(entries));
  if (!block.HasValue())
    return Result<PhysicalSpace>::Failure(block.GetStatus());
  for (std::size_t node = 0; node < assembly.rotations.size(); ++node) {
    const Status local = CheckLocalMass(
        domain, block.Value(), node_starts[node], node_starts[node + 1U]);
    if (!local.IsOk()) return Result<PhysicalSpace>::Failure(local);
  }
  if (dimension > 0U) {
    const Status factor = solver.Factorize(block.Value());
    if (!factor.IsOk()) return Result<PhysicalSpace>::Failure(factor);
  }
  return Result<PhysicalSpace>::Success(
      {std::move(rows), std::move(block.Value())});
}

/// @brief Selects the same scalar-primitive gradient used by the reduced graph.
Result<SparseMatrix> ConstraintGradient(const DynamicAssembly& assembly,
                                        const std::vector<std::size_t>& nodes) {
  if (!nodes.empty() && !assembly.hidden_gradient.has_value())
    return Result<SparseMatrix>::Failure(Status::Failure(
        FailureCategory::kSolver,
        {{Severity::kError, "dynamic-algebraic-gradient", {}, "DYNAMIC", {},
          "Canonical drilling-constraint gradient was not assembled."}}));
  std::vector<CooContribution> entries;
  for (std::size_t row = 0U; row < nodes.size(); ++row) {
    const auto& gradient = *assembly.hidden_gradient;
    const auto node = nodes[row];
    if (node >= gradient.Rows() ||
        gradient.Columns() != assembly.frame.acceleration.Size())
      return Result<SparseMatrix>::Failure(Status::Failure(
          FailureCategory::kSolver,
          {{Severity::kError, "dynamic-algebraic-gradient", {}, "DYNAMIC", {},
            "Canonical drilling-constraint gradient dimensions differ."}}));
    for (auto index = gradient.RowOffsets()[node];
         index < gradient.RowOffsets()[node + 1U]; ++index)
      entries.push_back({row, gradient.ColumnIndices()[index],
                         gradient.Values()[index], 0U, entries.size()});
  }
  return MakeSparse(nodes.size(), assembly.internal_tangent.Columns(),
                    std::move(entries));
}

/// @brief Accumulates one deterministic sparse contraction without lost sums.
void AddCompensated(const double term, double* sum, double* correction) {
  const double next = *sum + term;
  *correction += std::abs(*sum) >= std::abs(term)
                     ? (*sum - next) + term
                     : (term - next) + *sum;
  *sum = next;
}

/// @brief Stores W-scaled regularity with exact inverse-action infinity norms.
struct Regularity {
  SparseMatrix matrix;
  Vector weights;
};

/// @brief Factors W^-1 psi_q N W^-1 and checks the prescribed condition bound.
Result<Regularity> CheckRegularity(const Domain& domain,
                                   const DynamicAssembly& assembly,
                                   const std::vector<std::size_t>& nodes,
                                   const SparseMatrix& gradient,
                                   TangentSolver& solver) {
  const std::size_t no_null = std::numeric_limits<std::size_t>::max();
  std::vector<std::size_t> node_null(assembly.rotations.size(), no_null);
  Vector weights{nodes.size()};
  for (std::size_t i = 0; i < nodes.size(); ++i) {
    node_null[nodes[i]] = i;
    const auto& n = assembly.rotations[nodes[i]].drill_null;
    const double stiffness = OrderedNullMetric(
        assembly.drilling_metric, nodes[i], n);
    if (!std::isfinite(stiffness) || !(stiffness > 0.0)) {
      return Result<Regularity>::Failure(
          Failure(domain, "dynamic-algebraic-regularity",
                  "Every admissible drill null requires positive finite "
                  "drilling metric."));
    }
    weights[i] = std::sqrt(stiffness);
  }
  std::vector<CooContribution> entries;
  std::vector<double> null_coefficients;
  for (std::size_t row = 0; row < nodes.size(); ++row) {
    entries.push_back({row, row, 0.0, 0U, entries.size()});
    null_coefficients.push_back(0.0);
    for (std::size_t index = gradient.RowOffsets()[row];
         index < gradient.RowOffsets()[row + 1U]; ++index) {
      const std::size_t full = gradient.ColumnIndices()[index];
      if (full % 6U >= 3U && node_null[full / 6U] != no_null) {
        const std::size_t column = node_null[full / 6U];
        entries.push_back(
            {row, column, gradient.Values()[index], 0U, entries.size()});
        null_coefficients.push_back(
            assembly.rotations[full / 6U].drill_null[full % 6U - 3U]);
      }
    }
  }
  Vector gradient_values{entries.size()};
  Vector null_values{entries.size()};
  for (std::size_t i = 0U; i < entries.size(); ++i) {
    gradient_values[i] = entries[i].value;
    null_values[i] = null_coefficients[i];
  }
  Vector products{0U};
  {
    const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
    products =
        dense_blas_internal::MultiplyDiagonal(gradient_values, null_values);
  }
  for (std::size_t i = 0U; i < entries.size(); ++i)
    entries[i].value =
        products[i] / weights[entries[i].row] / weights[entries[i].column];
  auto matrix = MakeSparse(nodes.size(), nodes.size(), std::move(entries));
  if (!matrix.HasValue())
    return Result<Regularity>::Failure(matrix.GetStatus());
  if (nodes.empty())
    return Result<Regularity>::Success(
        {std::move(matrix.Value()), std::move(weights)});
  const Status factor = solver.FactorizeFresh(matrix.Value());
  if (!factor.IsOk()) return Result<Regularity>::Failure(factor);
  double norm = 0.0;
  for (std::size_t row = 0; row < nodes.size(); ++row) {
    double sum = 0.0;
    for (std::size_t index = matrix.Value().RowOffsets()[row];
         index < matrix.Value().RowOffsets()[row + 1U]; ++index)
      sum += std::abs(matrix.Value().Values()[index]);
    norm = std::max(norm, sum);
  }
  Vector inverse_sums{nodes.size()};
  // Columns are discarded after their absolute row sums are accumulated;
  // a global dense inverse is never retained.
  for (std::size_t column = 0; column < nodes.size(); ++column) {
    Vector rhs{nodes.size()};
    rhs[column] = 1.0;
    Vector solution{nodes.size()};
    const Status solved = solver.SolveCorrection(rhs, solution);
    if (!solved.IsOk()) return Result<Regularity>::Failure(solved);
    const Status checked = CheckLinearSolve(matrix.Value(), rhs, solution);
    if (!checked.IsOk()) return Result<Regularity>::Failure(checked);
    for (std::size_t row = 0; row < nodes.size(); ++row)
      inverse_sums[row] += std::abs(solution[row]);
  }
  double inverse_norm = 0.0;
  for (std::size_t row = 0; row < nodes.size(); ++row)
    inverse_norm = std::max(inverse_norm, inverse_sums[row]);
  const double condition = 1.0 / norm / inverse_norm;
  if (!std::isfinite(norm) || !std::isfinite(inverse_norm) ||
      !std::isfinite(condition) || !(condition >= std::ldexp(1.0, -32))) {
    std::ostringstream message;
    message << std::setprecision(17)
            << "Scaled null regularity reciprocal infinity condition "
            << condition << " is below 2^-32; norm=" << norm
            << ", inverse-action norm=" << inverse_norm << ".";
    return Result<Regularity>::Failure(
        Failure(domain, "dynamic-algebraic-conditioning", message.str()));
  }
  return Result<Regularity>::Success(
      {std::move(matrix.Value()), std::move(weights)});
}

/// @brief Recovers raw hidden equations and uncancelled derivative term scales.
Status HiddenEvidence(const Domain& domain, const DynamicAssembly& assembly,
                      const std::vector<std::size_t>& nodes,
                      const SparseMatrix& gradient,
                      DynamicAcceptedFrameState* frame,
                      const bool require_acceleration) {
  Vector psi{nodes.size()};
  Vector psi_scale{nodes.size()};
  Vector velocity_defect{nodes.size()};
  Vector acceleration_defect{nodes.size()};
  Vector velocity_scale{nodes.size()};
  Vector acceleration_scale{nodes.size()};
  for (std::size_t row = 0; row < nodes.size(); ++row) {
    const std::size_t node = nodes[row];
    const auto& rotation = assembly.rotations[node];
    double velocity_correction = 0.0;
    double acceleration_correction = 0.0;
    for (std::size_t index = gradient.RowOffsets()[row];
         index < gradient.RowOffsets()[row + 1U]; ++index) {
      const std::size_t column = gradient.ColumnIndices()[index];
      AddCompensated(gradient.Values()[index] * frame->velocity[column],
                     &velocity_defect[row], &velocity_correction);
      AddCompensated(gradient.Values()[index] * frame->acceleration[column],
                     &acceleration_defect[row], &acceleration_correction);
      velocity_scale[row] +=
          std::abs(gradient.Values()[index] * frame->velocity[column]);
      acceleration_scale[row] +=
          std::abs(gradient.Values()[index] * frame->acceleration[column]);
    }
    for (std::size_t c = 0; c < 3; ++c) {
      const std::size_t full = 6U * node + 3U + c;
      const double g =
          frame->internal_force[full] - frame->external_force[full];
      const double n = rotation.drill_null[c];
      psi[row] += n * g;
      psi_scale[row] += std::abs(n) * frame->residual_scale[full];
    }
    velocity_defect[row] += velocity_correction;
    AddCompensated(assembly.hidden_second[node], &acceleration_defect[row],
                   &acceleration_correction);
    acceleration_defect[row] += acceleration_correction;
    acceleration_scale[row] += assembly.hidden_second_scale[node];
    if (!Within(psi[row], psi_scale[row], 1.0e-10) ||
        (frame->is_initial &&
         !Within(velocity_defect[row], velocity_scale[row], 1.0e-10)) ||
        (frame->is_initial && require_acceleration &&
         !Within(acceleration_defect[row], acceleration_scale[row], 1.0e-10)) ||
        !std::isfinite(velocity_defect[row]) ||
        !std::isfinite(acceleration_defect[row]) ||
        !std::isfinite(velocity_scale[row]) ||
        !std::isfinite(acceleration_scale[row])) {
      std::ostringstream message;
      message << std::setprecision(17)
              << "Dynamic null consistency failed at node "
              << domain.Nodes()[node].source_id.instance_name << ":"
              << domain.Nodes()[node].source_id.source_label_text
              << "; psi=" << psi[row] << ", psi_scale=" << psi_scale[row]
              << ", velocity=" << velocity_defect[row]
              << ", velocity_scale=" << velocity_scale[row]
              << ", acceleration=" << acceleration_defect[row]
              << ", acceleration_scale=" << acceleration_scale[row]
              << ", initial=" << frame->is_initial
              << ", enforce_acceleration=" << require_acceleration << ".";
      return Failure(domain, "dynamic-hidden-constraint", message.str());
    }
  }
  frame->hidden_constraint = std::move(psi);
  frame->hidden_constraint_scale = std::move(psi_scale);
  frame->hidden_velocity_defect = std::move(velocity_defect);
  frame->hidden_velocity_defect_scale = std::move(velocity_scale);
  frame->hidden_acceleration_defect = std::move(acceleration_defect);
  frame->hidden_acceleration_defect_scale = std::move(acceleration_scale);
  return Status::Ok();
}

}  // namespace

double OrderedNullMetric(const SparseMatrix& metric, const std::size_t node,
                         const Vector3& direction) {
  Vector left{9U};
  Vector entries{9U};
  Vector right{9U};
  for (std::size_t i = 0U; i < 3U; ++i)
    for (std::size_t j = 0U; j < 3U; ++j) {
      const auto index = 3U * i + j;
      left[index] = direction[i];
      entries[index] = Entry(metric, 6U * node + 3U + i,
                              6U * node + 3U + j);
      right[index] = direction[j];
      if (!std::isfinite(left[index]) || !std::isfinite(entries[index]) ||
          !std::isfinite(right[index]))
        return std::numeric_limits<double>::quiet_NaN();
    }
  const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
  const Vector first = dense_blas_internal::MultiplyDiagonal(left, entries);
  for (std::size_t i = 0U; i < first.Size(); ++i)
    if (!std::isfinite(first[i]))
      return std::numeric_limits<double>::quiet_NaN();
  const Vector products = dense_blas_internal::MultiplyDiagonal(first, right);
  double value = 0.0;
  for (std::size_t i = 0U; i < products.Size(); ++i) value += products[i];
  return value;
}

std::vector<std::size_t> NullNodes(
    const DofManager& dofs,
    const std::vector<DynamicRotationKinematics>& rotations) {
  std::vector<std::size_t> nodes;
  for (std::size_t node = 0; node < rotations.size(); ++node) {
    bool admissible = true;
    for (std::size_t component = 0; component < 3; ++component) {
      if (!dofs.FreeEquation(6U * node + 3U + component).has_value() &&
          rotations[node].drill_null[component] != 0.0)
        admissible = false;
    }
    if (admissible) nodes.push_back(node);
  }
  return nodes;
}

Result<Vector> InitializeAcceleration(const Domain& domain,
                                      const DofManager& dofs,
                                      const DynamicAssembly& initial,
                                      const double length,
                                      LinearSolver& positive_solver,
                                      TangentSolver& tangent_solver) {
  const Status valid = Validate(domain, dofs, initial, length);
  if (!valid.IsOk()) return Result<Vector>::Failure(valid);
  const auto nodes = NullNodes(dofs, initial.rotations);
  auto physical =
      BuildPhysicalSpace(domain, dofs, initial, length, nodes, positive_solver);
  if (!physical.HasValue())
    return Result<Vector>::Failure(physical.GetStatus());
  auto gradient = ConstraintGradient(initial, nodes);
  if (!gradient.HasValue())
    return Result<Vector>::Failure(gradient.GetStatus());
  auto regularity =
      CheckRegularity(domain, initial, nodes, gradient.Value(), tangent_solver);
  if (!regularity.HasValue())
    return Result<Vector>::Failure(regularity.GetStatus());
  DynamicAcceptedFrameState candidate = initial.frame;
  candidate.is_initial = true;
  candidate.acceleration = Vector{dofs.FullDofCount()};
  const Status consistent = HiddenEvidence(domain, initial, nodes,
                                           gradient.Value(), &candidate, false);
  if (!consistent.IsOk()) return Result<Vector>::Failure(consistent);
  Vector rhs{physical.Value().mass.Rows()};
  // The initialization evaluation has zero acceleration, so I(q,v,0)=c(q,v).
  for (std::size_t full = 0; full < dofs.FullDofCount(); ++full) {
    if (initial.frame.acceleration[full] != 0.0) {
      return Result<Vector>::Failure(Failure(
          domain, "dynamic-invalid-initial-acceleration",
          "Initial inertia must be evaluated at zero unknown acceleration."));
    }
    const double force = initial.frame.external_force[full] -
                         initial.frame.internal_force[full] -
                         initial.frame.inertia_force[full];
    for (const auto& entry : physical.Value().rows[full]) {
      rhs[entry.first] += entry.second * Scale(full, length) * force;
    }
  }
  Vector solution{rhs.Size()};
  if (rhs.Size() > 0U) {
    const Status solved = positive_solver.Solve(rhs, solution);
    if (!solved.IsOk()) return Result<Vector>::Failure(solved);
    const Status checked =
        CheckLinearSolve(physical.Value().mass, rhs, solution);
    if (!checked.IsOk()) return Result<Vector>::Failure(checked);
  }
  for (std::size_t full = 0; full < dofs.FullDofCount(); ++full) {
    for (const auto& entry : physical.Value().rows[full]) {
      candidate.acceleration[full] +=
          Scale(full, length) * entry.second * solution[entry.first];
    }
  }
  const Status physical_hidden = HiddenEvidence(
      domain, initial, nodes, gradient.Value(), &candidate, false);
  if (!physical_hidden.IsOk()) return Result<Vector>::Failure(physical_hidden);
  if (!nodes.empty()) {
    Vector null_rhs{nodes.size()};
    for (std::size_t row = 0; row < nodes.size(); ++row) {
      null_rhs[row] = -candidate.hidden_acceleration_defect[row] /
                      regularity.Value().weights[row];
    }
    Vector correction{nodes.size()};
    const Status solved = tangent_solver.SolveCorrection(null_rhs, correction);
    if (!solved.IsOk()) return Result<Vector>::Failure(solved);
    const Status checked =
        CheckLinearSolve(regularity.Value().matrix, null_rhs, correction);
    if (!checked.IsOk()) return Result<Vector>::Failure(checked);
    for (std::size_t i = 0; i < nodes.size(); ++i) {
      for (std::size_t c = 0; c < 3; ++c) {
        candidate.acceleration[6U * nodes[i] + 3U + c] +=
            initial.rotations[nodes[i]].drill_null[c] * correction[i] /
            regularity.Value().weights[i];
      }
    }
  }
  const Status hidden = HiddenEvidence(domain, initial, nodes, gradient.Value(),
                                       &candidate, true);
  if (!hidden.IsOk()) return Result<Vector>::Failure(hidden);
  for (std::size_t full = 0U; full < candidate.acceleration.Size(); ++full) {
    if (!std::isfinite(candidate.acceleration[full])) {
      return Result<Vector>::Failure(Failure(
          domain, "dynamic-invalid-initial-acceleration",
          "Initial acceleration must be finite."));
    }
  }
  for (const std::size_t full : dofs.ConstrainedDofs()) {
    if (candidate.acceleration[full] != 0.0)
      return Result<Vector>::Failure(Failure(
          domain, "dynamic-invalid-initial-acceleration",
          "Initial acceleration must be exactly zero at constrained DOFs."));
  }
  return Result<Vector>::Success(std::move(candidate.acceleration));
}

Status CompleteConstraintEvidence(
    const Domain& domain, const DofManager& dofs,
    const DynamicAssembly& assembly, const double length,
    const std::vector<std::size_t>& initial_null_nodes,
    LinearSolver& positive_solver, TangentSolver& tangent_solver,
    DynamicAcceptedFrameState* frame) {
  const Status valid = Validate(domain, dofs, assembly, length);
  if (!valid.IsOk()) return valid;
  const auto nodes = NullNodes(dofs, assembly.rotations);
  if (nodes != initial_null_nodes) {
    return Failure(domain, "dynamic-mass-null-rank-change",
                   "The admissible analytic drilling-null inventory changed.");
  }
  const auto physical = BuildPhysicalSpace(domain, dofs, assembly, length,
                                           nodes, positive_solver);
  if (!physical.HasValue()) return physical.GetStatus();
  const auto gradient = ConstraintGradient(assembly, nodes);
  if (!gradient.HasValue()) return gradient.GetStatus();
  const auto regularity = CheckRegularity(domain, assembly, nodes,
                                          gradient.Value(), tangent_solver);
  if (!regularity.HasValue()) return regularity.GetStatus();
  DynamicAcceptedFrameState candidate = *frame;
  const Status hidden = HiddenEvidence(domain, assembly, nodes,
                                       gradient.Value(), &candidate, true);
  if (!hidden.IsOk()) return hidden;
  *frame = std::move(candidate);
  return Status::Ok();
}

}  // namespace hht_internal
}  // namespace fesa