FESADev/src/fesa/analysis/hht_dynamic_algebra.cpp

#include <algorithm>
#include <array>
#include <cmath>
#include <cstddef>
#include <iomanip>
#include <limits>
#include <sstream>
#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"

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;
  std::array<std::array<double, 6>, 6> block{};
  std::array<std::array<double, 6>, 6> lower{};
  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);
    for (std::size_t j = 0; j <= i; ++j) {
      double value = block[i][j];
      for (std::size_t k = 0; k < j; ++k) value -= lower[i][k] * lower[j][k];
      if (!std::isfinite(value) || (i == j && !(value > 0.0))) {
        return Failure(
            domain, "dynamic-positive-mass",
            "Physical mass requires strictly positive Cholesky pivots.");
      }
      lower[i][j] = i == j ? std::sqrt(value) : value / lower[j][j];
    }
  }
  std::array<double, 6> sums{};
  for (std::size_t column = 0; column < size; ++column) {
    std::array<double, 6> solution{};
    for (std::size_t i = 0; i < size; ++i) {
      double value = i == column ? 1.0 : 0.0;
      for (std::size_t k = 0; k < i; ++k) value -= lower[i][k] * solution[k];
      solution[i] = value / lower[i][i];
    }
    for (std::size_t reverse = size; reverse > 0U; --reverse) {
      const std::size_t i = reverse - 1U;
      double value = solution[i];
      for (std::size_t k = i + 1U; k < size; ++k)
        value -= lower[k][i] * solution[k];
      solution[i] = value / lower[i][i];
      sums[i] += std::abs(solution[i]);
    }
  }
  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) {
    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 = mass.Values()[index] / mass_scale;
        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]] +=
              entry * assembly.rotations[full / 6U].drill_null[full % 6U - 3U];
        }
      }
      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;
  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()});
  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 * mass.Values()[index] * b.second, 0U,
                             entries.size()});
        }
      }
    }
  }
  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 Builds psi_q from the current unit-null derivatives and force
/// Hessian.
Result<SparseMatrix> ConstraintGradient(const DynamicAssembly& assembly,
                                        const std::vector<std::size_t>& nodes) {
  std::vector<CooContribution> entries;
  const auto& stiffness = assembly.internal_tangent;
  for (std::size_t row = 0; row < nodes.size(); ++row) {
    const std::size_t node = nodes[row];
    const auto& rotation = assembly.rotations[node];
    for (std::size_t c = 0; c < 3; ++c) {
      const std::size_t full = 6U * node + 3U + c;
      const double g = assembly.frame.internal_force[full] -
                       assembly.frame.external_force[full];
      for (std::size_t d = 0; d < 3; ++d) {
        entries.push_back({row, 6U * node + 3U + d,
                           rotation.null_gradient(c, d) * g, 0U,
                           entries.size()});
      }
      for (std::size_t index = stiffness.RowOffsets()[full];
           index < stiffness.RowOffsets()[full + 1U]; ++index) {
        entries.push_back({row, stiffness.ColumnIndices()[index],
                           rotation.drill_null[c] * stiffness.Values()[index],
                           0U, entries.size()});
      }
    }
  }
  return MakeSparse(nodes.size(), stiffness.Columns(), std::move(entries));
}

/// @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;
    double stiffness = 0.0;
    for (std::size_t a = 0; a < 3; ++a) {
      for (std::size_t b = 0; b < 3; ++b) {
        stiffness += n[a] *
                     Entry(assembly.drilling_metric, 6U * nodes[i] + 3U + a,
                           6U * nodes[i] + 3U + b) *
                     n[b];
      }
    }
    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;
  for (std::size_t row = 0; row < nodes.size(); ++row) {
    entries.push_back({row, row, 0.0, 0U, entries.size()});
    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] *
                 assembly.rotations[full / 6U].drill_null[full % 6U - 3U] /
                 weights[row] / weights[column],
             0U, entries.size()});
      }
    }
  }
  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 = gradient.Multiply(frame->velocity);
  Vector acceleration_defect = gradient.Multiply(frame->acceleration);
  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];
    for (std::size_t index = gradient.RowOffsets()[row];
         index < gradient.RowOffsets()[row + 1U]; ++index) {
      const std::size_t column = gradient.ColumnIndices()[index];
      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];
    }
    acceleration_defect[row] += assembly.hidden_second[node];
    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

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);
  const Vector ma = initial.mass.Multiply(candidate.acceleration);
  for (const std::size_t full : dofs.FreeDofs()) {
    const double residual = ma[full] + initial.frame.inertia_force[full] +
                            initial.frame.internal_force[full] -
                            initial.frame.external_force[full];
    double scale = std::abs(initial.frame.inertia_force[full]) +
                   std::abs(initial.frame.internal_force[full]) +
                   std::abs(initial.frame.external_force[full]);
    for (std::size_t index = initial.mass.RowOffsets()[full];
         index < initial.mass.RowOffsets()[full + 1U]; ++index) {
      scale +=
          std::abs(initial.mass.Values()[index] *
                   candidate.acceleration[initial.mass.ColumnIndices()[index]]);
    }
    if (!Within(residual, scale, 1.0e-10)) {
      return Result<Vector>::Failure(Failure(
          domain, "dynamic-initial-equilibrium",
          "Initial physical acceleration failed the complete free equation."));
    }
  }
  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