#include "analysis/hht_reduced_internal.h"
#include "analysis/hht_development_probe.h"

#include <algorithm>
#include <cmath>
#include <cstring>
#include <limits>
#include <map>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>

#include "fesa/core/source_identity.h"
#include "fesa/solvers/linear/tangent_solver.h"
#include "math/dense_blas_internal.h"
#include "math/high_low_arithmetic.h"
#include "math/finite_values.h"
#include "math/compensated_sum.h"

namespace fesa::hht_reduced_internal {
namespace {

using RateSum = math_internal::CompensatedSum;

Status Failure(const std::string& message,
               const std::string& code = "dynamic-invalid-state") {
  return Status::Failure(FailureCategory::kSolver,
                         {{Severity::kError, code, {},
                           "DYNAMIC", {}, message}});
}

bool Finite(const Vector& values) {
  return math_internal::IsFinite(values);
}

bool ExactlyEqual(const Vector& left, const Vector& right) {
  if (left.Size() != right.Size()) return false;
  for (std::size_t i = 0U; i < left.Size(); ++i)
    if (left[i] != right[i]) return false;
  return true;
}

Vector Local(const Vector& full,
             const hht_internal::RuntimeShell& shell) {
  Vector local{24U};
  for (std::size_t i = 0U; i < 24U; ++i) local[i] = full[shell.scatter[i]];
  return local;
}

/// @brief Preserves the exact floating representation, including signed zero.
bool SameBits(const double left, const double right) {
  return std::memcmp(&left, &right, sizeof(double)) == 0;
}

/// @brief Finds only the current complete directional response, otherwise miss.
const hht_internal::DirectionalShellConstraintData* MatchingConstraints(
    const Domain& domain, const hht_internal::RuntimeShell& shell,
    const hht_internal::DynamicAssembly& assembly, const std::size_t index,
    const std::size_t shell_count, const Vector& q, const Vector& v) {
  if (assembly.frame.is_initial || !assembly.trial_constraints.has_value())
    return nullptr;
  const auto& owner = *assembly.trial_constraints;
  const auto& frame = assembly.frame;
  if (owner.shells.size() != shell_count || index >= owner.shells.size() ||
      owner.source_path != domain.SourcePath() ||
      !std::isfinite(owner.time) || !std::isfinite(frame.time) ||
      !SameBits(owner.time, frame.time) ||
      owner.frame_index != frame.frame_index ||
      owner.increment_index != frame.increment_index ||
      owner.iteration_count != frame.iteration_count)
    return nullptr;
  const auto& record = owner.shells[index];
  if (!record.directional_internal ||
      record.element_index != shell.element_index ||
      record.scatter != shell.scatter ||
      !hht_internal::ReusableTrialContext(
          record.evaluation_context,
          hht_internal::ReadTrialEvaluationContext()))
    return nullptr;
  const auto configuration =
      nonlinear_mitc4_internal::ConfigurationSnapshot::Read(*shell.internal_kernel);
  if (record.configuration.source.file != configuration.source.file ||
      record.configuration.source.line != configuration.source.line ||
      record.configuration.identity != configuration.identity ||
      record.configuration.drilling_mode != configuration.drilling_mode)
    return nullptr;
  for (std::size_t i = 0U; i < configuration.values.size(); ++i)
    if (!std::isfinite(record.configuration.values[i]) ||
        !std::isfinite(configuration.values[i]) ||
        !SameBits(record.configuration.values[i], configuration.values[i]))
      return nullptr;
  for (std::size_t i = 0U; i < 24U; ++i)
    if (!std::isfinite(record.q[i]) || !std::isfinite(q[i]) ||
        !std::isfinite(record.v[i]) || !std::isfinite(v[i]) ||
        !SameBits(record.q[i], q[i]) || !SameBits(record.v[i], v[i]))
      return nullptr;
  for (std::size_t node = 0U; node < 4U; ++node) {
    const auto global_node = shell.scatter[6U * node] / 6U;
    if (global_node >= domain.Nodes().size() ||
        !SourceIdentityEquals(record.node_source_ids[node],
                    domain.Nodes()[global_node].source_id))
      return nullptr;
  }
  if (record.drilling_constraint.Size() != 4U ||
      record.drilling_constraint_gradient.Rows() != 4U ||
      record.drilling_constraint_gradient.Columns() != 24U)
    return nullptr;
  for (std::size_t node = 0U; node < 4U; ++node) {
    const auto& hessian = record.drilling_constraint_hessian[node];
    const auto& third =
        record.drilling_constraint_third_directional_gradient[node];
    if (hessian.Rows() != 24U || hessian.Columns() != 24U ||
        third.Size() != 24U || !std::isfinite(record.drilling_constraint[node]))
      return nullptr;
    for (std::size_t i = 0U; i < 24U; ++i) {
      if (!std::isfinite(record.drilling_constraint_gradient(node, i)) ||
          !std::isfinite(third[i]))
        return nullptr;
      for (std::size_t j = 0U; j < 24U; ++j)
        if (!std::isfinite(hessian(i, j))) return nullptr;
    }
  }
  return &record;
}

NonlinearMitc4TrialDofs Trial(const Vector& local) {
  NonlinearMitc4TrialDofs trial{};
  for (std::size_t node = 0U; node < 4U; ++node) {
    const auto first = 6U * node;
    trial.displacement[node] =
        Vector3{local[first], local[first + 1U], local[first + 2U]};
    trial.rotation_vector[node] = Vector3{local[first + 3U],
                                           local[first + 4U],
                                           local[first + 5U]};
  }
  return trial;
}

Result<SparseMatrix> DenseSparse(const Matrix& matrix) {
  std::vector<CooContribution> entries;
  entries.reserve(matrix.Rows() * matrix.Columns());
  for (std::size_t row = 0U; row < matrix.Rows(); ++row)
    for (std::size_t column = 0U; column < matrix.Columns(); ++column) {
      const double value = matrix(row, column);
      if (value != 0.0)
        entries.push_back(
            {row, column, value, 0U, row * matrix.Columns() + column});
    }
  return hht_internal::MakeSparse(matrix.Rows(), matrix.Columns(),
                                  std::move(entries));
}

bool ValidBasis(const FixedChart& chart) {
  if (chart.p_columns.size() != chart.p_dofs.size() ||
      chart.z_columns.size() != chart.z_dofs.size()) return false;
  for (const auto* columns : {&chart.p_columns, &chart.z_columns})
    for (const auto& column : *columns) {
      if (column.channel > 1U || column.support.empty() ||
          column.support_radii.size() != column.support.size()) return false;
      std::size_t previous = 0U;
      for (std::size_t i = 0U; i < column.support.size(); ++i) {
        const auto& entry = column.support[i];
        if (entry.dof >= chart.full_dof_count || !std::isfinite(entry.value) ||
            column.support_radii[i] < 0.0 ||
            !std::isfinite(column.support_radii[i]) ||
            (i != 0U && entry.dof <= previous)) return false;
        previous = entry.dof;
      }
    }
  return true;
}

double ColumnDot(const FixedBasisColumn& column, const Vector& full) {
  RateSum sum;
  for (const auto& entry : column.support) sum.Add(entry.value * full[entry.dof]);
  return sum.Value();
}

double GradientDot(const std::vector<ConstraintEntry>& gradient,
                   const FixedBasisColumn& column) {
  RateSum sum;
  for (const auto& entry : gradient)
    for (const auto& basis : column.support)
      if (entry.dof == basis.dof) sum.Add(entry.value * basis.value);
  return sum.Value();
}

FixedBasisColumn ComponentColumn(const std::size_t full) {
  return {full / 6U, full % 6U < 3U ? 0U : 1U, false, {{full, 1.0}}, {0.0}};
}

bool BitwiseEqual(const Vector& left, const Vector& right) {
  return left.Size() == right.Size() &&
         (left.Size() == 0U ||
          std::memcmp(left.Data(), right.Data(), left.Size() * sizeof(double)) == 0);
}

double RadiusUp(const double value) {
  return value == 0.0 ? 0.0 : std::nextafter(value, std::numeric_limits<double>::infinity());
}

double RadiusProduct(const double left, const double right) {
  const double value = left * right;
  return value == 0.0 && left != 0.0 && right != 0.0
             ? std::numeric_limits<double>::denorm_min() : RadiusUp(value);
}

double RadiusQuotient(const double numerator, const double denominator) {
  const double value = numerator / denominator;
  return value == 0.0 && numerator != 0.0
             ? std::numeric_limits<double>::denorm_min() : RadiusUp(value);
}

double BasisRoundoff(const double value) {
  if (value == 0.0) return 0.0;
  const double upper = std::nextafter(value, std::numeric_limits<double>::infinity());
  const double lower = std::nextafter(value, -std::numeric_limits<double>::infinity());
  const double gap = (std::max)(std::abs(upper - value), std::abs(value - lower));
  const double half = 0.5 * gap;
  return half == 0.0 ? gap : RadiusUp(half);
}

struct BasisValue {
  double value = 0.0;
  double radius = 0.0;
};

BasisValue BasisAdd(const BasisValue left, const BasisValue right) {
  const double value = left.value + right.value;
  return {value, RadiusUp(RadiusUp(left.radius + right.radius) + BasisRoundoff(value))};
}

BasisValue BasisMultiply(const BasisValue left, const BasisValue right) {
  const double value = left.value * right.value;
  const double first = RadiusProduct(std::abs(left.value), right.radius);
  const double second = RadiusProduct(std::abs(right.value), left.radius);
  const double cross = RadiusProduct(left.radius, right.radius);
  const double rounding = value == 0.0 && left.value != 0.0 && right.value != 0.0
                              ? std::numeric_limits<double>::denorm_min()
                              : BasisRoundoff(value);
  return {value, RadiusUp(RadiusUp(RadiusUp(first + second) + cross) + rounding)};
}

BasisValue BasisDivide(const BasisValue numerator, const BasisValue denominator) {
  const double value = numerator.value / denominator.value;
  const double lower = std::nextafter(std::abs(denominator.value) - denominator.radius,
                                     -std::numeric_limits<double>::infinity());
  if (!(lower > 0.0)) return {value, std::numeric_limits<double>::infinity()};
  const double top = RadiusUp(numerator.radius + RadiusProduct(std::abs(value), denominator.radius));
  const double rounding = value == 0.0 && numerator.value != 0.0
                              ? std::numeric_limits<double>::denorm_min()
                              : BasisRoundoff(value);
  const double amplified = RadiusProduct(rounding,
      RadiusUp(1.0 + RadiusQuotient(denominator.radius, lower)));
  return {value, RadiusUp(RadiusQuotient(top, lower) + amplified)};
}

BasisValue BasisNorm(const std::array<BasisValue, 3U>& vector) {
  const double scale = (std::max)({std::abs(vector[0U].value),
                                 std::abs(vector[1U].value), std::abs(vector[2U].value)});
  if (!(scale > 0.0) || !std::isfinite(scale)) return {};
  std::array<BasisValue, 3U> y{};
  std::array<BasisValue, 3U> square{};
  for (std::size_t i = 0U; i < 3U; ++i) {
    // The actual chosen scale is held exact in this construction indicator.
    y[i] = BasisDivide(vector[i], {scale, 0.0});
    square[i] = BasisMultiply(y[i], y[i]);
  }
  const auto sum = BasisAdd(BasisAdd(square[0U], square[1U]), square[2U]);
  const double root = std::sqrt(sum.value);
  const double lower = std::nextafter(sum.value - sum.radius,
                                     -std::numeric_limits<double>::infinity());
  if (!(lower > 0.0)) return {scale * root, std::numeric_limits<double>::infinity()};
  const double denominator = std::nextafter(root - BasisRoundoff(root),
                                            -std::numeric_limits<double>::infinity());
  if (!(denominator > 0.0))
    return {scale * root, std::numeric_limits<double>::infinity()};
  const BasisValue square_root{root,
      RadiusUp(RadiusQuotient(sum.radius, denominator) + BasisRoundoff(root))};
  return BasisMultiply({scale, 0.0}, square_root);
}

FixedBasisColumn PlaneColumn(const std::size_t node,
                             const std::array<BasisValue, 3U>& vector) {
  FixedBasisColumn column{node, 1U, true, {}, {}};
  for (std::size_t i = 0U; i < 3U; ++i) {
    column.support.push_back({6U * node + 3U + i, vector[i].value});
    column.support_radii.push_back(vector[i].radius);
  }
  return column;
}

}  // namespace

Result<FixedChart> BuildFixedChart(
    const DofManager& dofs,
    const std::vector<DynamicRotationKinematics>& rotations) {
  if (dofs.FullDofCount() != rotations.size() * 6U)
    return Result<FixedChart>::Failure(
        Failure("Reduced chart rotation and full-DOF inventories differ.",
                "dynamic-mass-regularity"));
  FixedChart chart{};
  chart.full_dof_count = dofs.FullDofCount();
  std::vector<bool> is_z(chart.full_dof_count, false);
  std::map<std::size_t, FixedBasisColumn> plane_columns;
  for (std::size_t node = 0U; node < rotations.size(); ++node) {
    bool admissible = true;
    for (std::size_t component = 0U; component < 3U; ++component) {
      const auto full = 6U * node + 3U + component;
      if (!dofs.FreeEquation(full).has_value() &&
          rotations[node].drill_null[component] != 0.0) {
        admissible = false;
      }
    }
    if (!admissible) continue;
    std::size_t pivot = 0U;
    for (std::size_t component = 1U; component < 3U; ++component) {
      if (std::abs(rotations[node].drill_null[component]) >
          std::abs(rotations[node].drill_null[pivot]))
        pivot = component;
    }
    const auto pivot_dof = 6U * node + 3U + pivot;
    if (!dofs.FreeEquation(pivot_dof).has_value() ||
        rotations[node].drill_null[pivot] == 0.0) {
      return Result<FixedChart>::Failure(Failure(
          "Reduced chart has no free nonzero canonical drilling pivot.",
          "dynamic-mass-regularity"));
    }
    is_z[pivot_dof] = true;
    chart.null_nodes.push_back(node);
    chart.pivot_components.push_back(pivot);
    chart.z_dofs.push_back(pivot_dof);
    chart.initial_nulls.push_back({rotations[node].drill_null[0U],
                                   rotations[node].drill_null[1U],
                                   rotations[node].drill_null[2U]});
    bool fully_free = true;
    for (std::size_t component = 0U; component < 3U; ++component)
      fully_free = fully_free &&
                   dofs.FreeEquation(6U * node + 3U + component).has_value();
    if (!fully_free) {
      chart.z_columns.push_back(ComponentColumn(pivot_dof));
      chart.minimum_axes.push_back(3U);
      continue;
    }
    if (!math_internal::ArithmeticEnvironmentSupported())
      return Result<FixedChart>::Failure(Failure(
          "Fixed null basis requires round-to-nearest and gradual underflow.",
          "dynamic-mass-regularity"));
    const auto& raw = rotations[node].drill_null;
    std::array<BasisValue, 3U> raw_values{};
    for (std::size_t i = 0U; i < 3U; ++i) raw_values[i] = {raw[i], 0.0};
    const auto norm = BasisNorm(raw_values);
    if (!std::isfinite(norm.value) || !(norm.value > 0.0) || !std::isfinite(norm.radius))
      return Result<FixedChart>::Failure(Failure(
          "Fixed null basis requires a positive finite analytic-null norm.",
          "dynamic-mass-regularity"));
    std::array<BasisValue, 3U> n{};
    for (std::size_t i = 0U; i < 3U; ++i) n[i] = BasisDivide(raw_values[i], norm);
    std::size_t axis = 0U;
    for (std::size_t i = 1U; i < 3U; ++i)
      if (std::abs(n[i].value) < std::abs(n[axis].value)) axis = i;
    std::array<BasisValue, 3U> t1{};
    for (std::size_t i = 0U; i < 3U; ++i) {
      const auto product = BasisMultiply(n[axis], n[i]);
      t1[i] = BasisAdd({i == axis ? 1.0 : 0.0, 0.0}, {-product.value, product.radius});
    }
    const auto tangent_norm = BasisNorm(t1);
    if (!std::isfinite(tangent_norm.value) || !(tangent_norm.value > 0.0) ||
        !std::isfinite(tangent_norm.radius))
      return Result<FixedChart>::Failure(Failure(
          "Fixed null basis tangent norm is invalid.", "dynamic-mass-regularity"));
    for (auto& value : t1) value = BasisDivide(value, tangent_norm);
    std::array<BasisValue, 3U> t2{};
    for (std::size_t i = 0U; i < 3U; ++i) {
      const auto first = BasisMultiply(n[(i + 1U) % 3U], t1[(i + 2U) % 3U]);
      const auto second = BasisMultiply(n[(i + 2U) % 3U], t1[(i + 1U) % 3U]);
      t2[i] = BasisAdd(first, {-second.value, second.radius});
    }
    const std::array<std::array<BasisValue, 3U>, 3U> basis{t1, t2, n};
    for (std::size_t i = 0U; i < 3U; ++i)
      for (std::size_t j = 0U; j < 3U; ++j) {
        RateSum dot;
        for (std::size_t c = 0U; c < 3U; ++c)
          dot.Add(basis[i][c].value * basis[j][c].value);
        const double defect = std::abs(dot.Value() - (i == j ? 1.0 : 0.0));
        if (!std::isfinite(defect) || defect > 1e-12)
          return Result<FixedChart>::Failure(Failure(
              "Fixed null basis fails the original Gram check.",
              "dynamic-mass-regularity"));
      }
    std::size_t retained = 0U;
    for (std::size_t component = 0U; component < 3U; ++component)
      if (component != pivot)
        plane_columns.emplace(6U * node + 3U + component,
                              PlaneColumn(node, retained++ == 0U ? t1 : t2));
    chart.z_columns.push_back(PlaneColumn(node, n));
    chart.minimum_axes.push_back(axis);
  }
  for (const auto full : dofs.FreeDofs())
    if (!is_z[full]) {
      chart.p_dofs.push_back(full);
      const auto plane = plane_columns.find(full);
      chart.p_columns.push_back(plane == plane_columns.end()
                                    ? ComponentColumn(full) : plane->second);
    }
  if (!ValidBasis(chart))
    return Result<FixedChart>::Failure(Failure(
        "Fixed null basis construction radius is incomplete or nonfinite.",
        "dynamic-mass-regularity"));
  return Result<FixedChart>::Success(std::move(chart));
}

Result<Vector> Lift(const FixedChart& chart, const Vector& p, const Vector& z) {
  if (!ValidBasis(chart) || p.Size() != chart.p_columns.size() ||
      z.Size() != chart.z_columns.size())
    return Result<Vector>::Failure(
        Failure("Reduced chart coordinate dimensions differ from its split."));
  if (!Finite(p) || !Finite(z))
    return Result<Vector>::Failure(
        Failure("Reduced chart coordinates must remain finite."));
  std::vector<RateSum> sums(chart.full_dof_count);
  for (std::size_t i = 0U; i < p.Size(); ++i)
    for (const auto& entry : chart.p_columns[i].support)
      sums[entry.dof].Add(entry.value * p[i]);
  for (std::size_t i = 0U; i < z.Size(); ++i)
    for (const auto& entry : chart.z_columns[i].support)
      sums[entry.dof].Add(entry.value * z[i]);
  Vector full{chart.full_dof_count};
  for (std::size_t i = 0U; i < full.Size(); ++i) full[i] = sums[i].Value();
  if (!Finite(full)) return Result<Vector>::Failure(
      Failure("Reduced basis lift is nonfinite."));
  return Result<Vector>::Success(std::move(full));
}

Result<Vector> ProjectPhysical(const FixedChart& chart, const Vector& full) {
  if (!ValidBasis(chart) || full.Size() != chart.full_dof_count || !Finite(full))
    return Result<Vector>::Failure(Failure("Reduced physical projection inventory is invalid."));
  Vector result{chart.p_columns.size()};
  for (std::size_t i = 0U; i < result.Size(); ++i)
    result[i] = ColumnDot(chart.p_columns[i], full);
  if (!Finite(result)) return Result<Vector>::Failure(Failure("Reduced physical projection is nonfinite."));
  return Result<Vector>::Success(std::move(result));
}

Result<Vector> ProjectAlgebraic(const FixedChart& chart, const Vector& full) {
  if (!ValidBasis(chart) || full.Size() != chart.full_dof_count || !Finite(full))
    return Result<Vector>::Failure(Failure("Reduced algebraic projection inventory is invalid."));
  Vector result{chart.z_columns.size()};
  for (std::size_t i = 0U; i < result.Size(); ++i)
    result[i] = ColumnDot(chart.z_columns[i], full);
  if (!Finite(result)) return Result<Vector>::Failure(Failure("Reduced algebraic projection is nonfinite."));
  return Result<Vector>::Success(std::move(result));
}

std::size_t PhysicalCoordinateChannel(const FixedChart& chart,
                                      const std::size_t coordinate) {
  return chart.p_columns[coordinate].channel;
}

Result<ConstraintOperators> EvaluateConstraintOperators(
    const Domain& domain, const DofManager& dofs,
    const std::vector<hht_internal::RuntimeShell>& shells,
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    const Vector& force_rate, const Vector& force_acceleration) {
  const auto count = dofs.FullDofCount();
  if (!ValidBasis(chart) || chart.full_dof_count != count || force_rate.Size() != count ||
      force_acceleration.Size() != count || !Finite(force_rate) ||
      !Finite(force_acceleration) ||
      assembly.frame.residual_scale.Size() != count ||
      chart.null_nodes.size() != chart.z_dofs.size() ||
      chart.null_nodes.size() != chart.pivot_components.size())
    return Result<ConstraintOperators>::Failure(
        Failure("Reduced constraint operator inventory is invalid.",
                "dynamic-mass-regularity"));
  if (hht_internal::NullNodes(dofs, assembly.rotations) != chart.null_nodes)
    return Result<ConstraintOperators>::Failure(Failure(
        "The admissible drilling-null inventory changed in the fixed chart.",
        "dynamic-mass-regularity"));
  for (std::size_t row = 0U; row < chart.null_nodes.size(); ++row) {
    const auto node = chart.null_nodes[row];
    RateSum transverse;
    if (node < assembly.rotations.size())
      for (const auto& entry : chart.z_columns[row].support)
        transverse.Add(assembly.rotations[node].drill_null[entry.dof % 6U - 3U] *
                       entry.value);
    if (node >= assembly.rotations.size() ||
        !std::isfinite(transverse.Value()) || transverse.Value() == 0.0)
      return Result<ConstraintOperators>::Failure(Failure(
          "The fixed reduced basis is no longer transverse to the actual null.",
          "dynamic-mass-regularity"));
  }
  const auto rows = chart.null_nodes.size();
  ConstraintOperators result{};
  result.rows.resize(rows);
  std::vector<std::map<std::size_t, double>> gradients(rows);
  std::vector<std::map<std::size_t, double>> time_gradients(rows);
  std::vector<std::map<std::size_t, double>> third_gradients(rows);
  std::vector<std::map<std::pair<std::size_t, std::size_t>, double>>
      hessians(rows);
  std::vector<std::size_t> node_row(domain.Nodes().size(), rows);
  for (std::size_t row = 0U; row < rows; ++row) {
    if (chart.null_nodes[row] >= node_row.size())
      return Result<ConstraintOperators>::Failure(
          Failure("Reduced chart null node is outside the Domain.",
                  "dynamic-mass-regularity"));
    node_row[chart.null_nodes[row]] = row;
  }
  std::vector<bool> external_done(domain.Nodes().size(), false);
  std::size_t shell_index = 0U;
  for (const auto& shell : shells) {
    const auto q = Local(assembly.frame.generalized_coordinates, shell);
    const auto v = Local(assembly.frame.velocity, shell);
    const auto* saved = MatchingConstraints(domain, shell, assembly,
                                            shell_index++, shells.size(), q, v);
    std::optional<NonlinearMitc4InternalEquilibrium> fresh;
    if (saved == nullptr) {
      FESA_HHT_PROBE_COUNT(kConstraintInternalEvaluation);
      auto internal =
          shell.internal->EvaluateTrial(Trial(q), &v);
      if (!internal.HasValue())
        return Result<ConstraintOperators>::Failure(internal.GetStatus());
      fresh = std::move(internal.Value());
    }
    const auto& constraint = saved != nullptr ? saved->drilling_constraint
                                              : fresh->drilling_constraint;
    const auto& gradient_data = saved != nullptr
                                    ? saved->drilling_constraint_gradient
                                    : fresh->drilling_constraint_gradient;
    const auto& third_data =
        saved != nullptr
            ? saved->drilling_constraint_third_directional_gradient
            : fresh->drilling_constraint_third_directional_gradient;
    const auto& hessian_data = saved != nullptr
                                   ? saved->drilling_constraint_hessian
                                   : fresh->drilling_constraint_hessian;
    for (std::size_t local_node = 0U; local_node < 4U; ++local_node) {
      const auto global_node = shell.scatter[6U * local_node] / 6U;
      if (global_node >= node_row.size() || node_row[global_node] == rows)
        continue;
      const auto row = node_row[global_node];
      result.rows[row].value += constraint[local_node];
      for (std::size_t i = 0U; i < 24U; ++i) {
        const auto global_i = shell.scatter[i];
        const double gradient = gradient_data(local_node, i);
        const double third = third_data[local_node][i];
        if (gradient != 0.0) gradients[row][global_i] += gradient;
        if (third != 0.0) third_gradients[row][global_i] += third;
        for (std::size_t j = 0U; j < 24U; ++j) {
          const double hessian = hessian_data[local_node](i, j);
          if (hessian != 0.0)
            hessians[row][{global_i, shell.scatter[j]}] += hessian;
        }
      }
      if (external_done[global_node]) continue;
      external_done[global_node] = true;
      const auto first = 6U * global_node + 3U;
      const auto external = shell.internal->EvaluateExternalDrillingConstraint(
          local_node, Vector3{q[6U * local_node + 3U],
                              q[6U * local_node + 4U],
                              q[6U * local_node + 5U]},
          Vector3{v[6U * local_node + 3U], v[6U * local_node + 4U],
                  v[6U * local_node + 5U]},
          Vector3{assembly.frame.external_force[first],
                  assembly.frame.external_force[first + 1U],
                  assembly.frame.external_force[first + 2U]},
          Vector3{force_rate[first], force_rate[first + 1U],
                  force_rate[first + 2U]},
          Vector3{force_acceleration[first], force_acceleration[first + 1U],
                  force_acceleration[first + 2U]});
      if (!external.HasValue())
        return Result<ConstraintOperators>::Failure(external.GetStatus());
      const auto rate_term = shell.internal->EvaluateExternalDrillingConstraint(
          local_node, Vector3{q[6U * local_node + 3U],
                              q[6U * local_node + 4U],
                              q[6U * local_node + 5U]},
          {}, Vector3{force_rate[first], force_rate[first + 1U],
                       force_rate[first + 2U]}, {}, {});
      if (!rate_term.HasValue())
        return Result<ConstraintOperators>::Failure(rate_term.GetStatus());
      const auto acceleration_term =
          shell.internal->EvaluateExternalDrillingConstraint(
              local_node, Vector3{q[6U * local_node + 3U],
                                  q[6U * local_node + 4U],
                                  q[6U * local_node + 5U]},
              {}, Vector3{force_acceleration[first],
                           force_acceleration[first + 1U],
                           force_acceleration[first + 2U]}, {}, {});
      if (!acceleration_term.HasValue())
        return Result<ConstraintOperators>::Failure(
            acceleration_term.GetStatus());
      result.rows[row].value += external.Value().value;
      for (std::size_t i = 0U; i < 3U; ++i) {
        const double gradient = external.Value().gradient[i];
        const double third = external.Value().gradient_directional_second[i];
        if (gradient != 0.0) gradients[row][first + i] += gradient;
        if (third != 0.0) third_gradients[row][first + i] += third;
        for (std::size_t j = 0U; j < 3U; ++j) {
          const double hessian = external.Value().hessian(i, j);
          if (hessian != 0.0)
            hessians[row][{first + i, first + j}] += hessian;
        }
      }
      for (std::size_t i = 0U; i < 3U; ++i) {
        // Use the direct Fdot primitive rather than subtracting H*v from a
        // mixed directional quantity.  In particular, constant public CLOAD
        // keeps psi_t and psi_qt exactly zero in binary64.
        if (rate_term.Value().gradient[i] != 0.0)
          time_gradients[row][first + i] += rate_term.Value().gradient[i];
      }
      result.rows[row].time_first += rate_term.Value().value;
      result.rows[row].time_second += acceleration_term.Value().value;
    }
  }
  for (std::size_t row = 0U; row < rows; ++row) {
    auto& destination = result.rows[row];
    for (const auto& entry : gradients[row])
      if (entry.second != 0.0)
        destination.gradient.push_back({entry.first, entry.second});
    for (const auto& entry : time_gradients[row])
      if (entry.second != 0.0)
        destination.time_gradient.push_back({entry.first, entry.second});
    for (const auto& entry : third_gradients[row])
      if (entry.second != 0.0)
        destination.third_directional_gradient.push_back({entry.first,
                                                           entry.second});
    for (const auto& entry : hessians[row])
      if (entry.second != 0.0)
        destination.hessian.push_back(
            {entry.first.first, entry.first.second, entry.second});
    const auto node = chart.null_nodes[row];
    const auto& null = assembly.rotations[node].drill_null;
    for (std::size_t component = 0U; component < 3U; ++component)
      destination.value_scale +=
          std::abs(null[component]) *
          assembly.frame.residual_scale[6U * node + 3U + component];
    if (!std::isfinite(destination.value) ||
        !std::isfinite(destination.time_first) ||
        !std::isfinite(destination.time_second) ||
        !std::isfinite(destination.value_scale))
      return Result<ConstraintOperators>::Failure(
          Failure("Reduced drilling operator reduction is nonfinite."));
    for (const auto& entry : destination.gradient)
      if (!std::isfinite(entry.value))
        return Result<ConstraintOperators>::Failure(
            Failure("Reduced drilling gradient is nonfinite."));
    for (const auto& entry : destination.time_gradient)
      if (!std::isfinite(entry.value))
        return Result<ConstraintOperators>::Failure(
            Failure("Reduced drilling time gradient is nonfinite."));
    for (const auto& entry : destination.third_directional_gradient)
      if (!std::isfinite(entry.value))
        return Result<ConstraintOperators>::Failure(
            Failure("Reduced drilling third derivative is nonfinite."));
    for (const auto& entry : destination.hessian)
      if (!std::isfinite(entry.value))
        return Result<ConstraintOperators>::Failure(
            Failure("Reduced drilling Hessian is nonfinite."));
  }
  return Result<ConstraintOperators>::Success(std::move(result));
}

Matrix GraphJacobian(const ConstraintOperators& operators,
                     const FixedChart& chart) {
  Matrix result{operators.rows.size(), chart.z_dofs.size()};
  for (std::size_t row = 0U; row < result.Rows(); ++row)
    for (std::size_t column = 0U; column < result.Columns(); ++column)
      result(row, column) = GradientDot(operators.rows[row].gradient,
                                        chart.z_columns[column]);
  return result;
}

Matrix PhysicalJacobian(const ConstraintOperators& operators,
                        const FixedChart& chart) {
  Matrix result{operators.rows.size(), chart.p_dofs.size()};
  for (std::size_t row = 0U; row < result.Rows(); ++row)
    for (std::size_t column = 0U; column < result.Columns(); ++column)
      result(row, column) = GradientDot(operators.rows[row].gradient,
                                        chart.p_columns[column]);
  return result;
}

Vector ContractConstraintHessian(const ConstraintOperators& operators,
                                 const Vector& left, const Vector& right) {
  const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
  Vector result{operators.rows.size()};
  for (std::size_t row = 0U; row < operators.rows.size(); ++row) {
    const auto& hessian = operators.rows[row].hessian;
    Vector coefficients{hessian.size()};
    Vector left_values{hessian.size()};
    Vector right_values{hessian.size()};
    for (std::size_t i = 0U; i < hessian.size(); ++i) {
      coefficients[i] = hessian[i].value;
      left_values[i] = left[hessian[i].row];
      right_values[i] = right[hessian[i].column];
    }
    try {
      const Vector first =
          dense_blas_internal::MultiplyDiagonal(coefficients, left_values);
      const Vector products =
          dense_blas_internal::MultiplyDiagonal(first, right_values);
      // Retain the original entry order and left-associated products; only
      // ordinary products move to MKL. This is failure-only replay evidence.
      for (std::size_t i = 0U; i < products.Size(); ++i)
        result[row] += products[i];
    } catch (const std::invalid_argument&) {
      // A nonfinite input/intermediate cannot yield a finite original sum.
      // Keep the existing caller's finite failure without a scalar fallback.
      result[row] = std::numeric_limits<double>::quiet_NaN();
    }
  }
  return result;
}

Result<EmbeddingRoot> SolveEmbeddingRoot(const FixedChart& chart,
                                         const Vector& p, const Vector& seed,
                                         const ConstraintEvaluator& evaluate,
                                         TangentSolver& solver) {
  FESA_HHT_PROBE_SCOPE(kInner);
  if (!evaluate || p.Size() != chart.p_dofs.size() ||
      seed.Size() != chart.z_dofs.size())
    return Result<EmbeddingRoot>::Failure(
        Failure("Reduced embedding root inputs are invalid."));
  Vector z = seed;
  Vector prior_z{seed.Size()};
  Vector prior_q{chart.full_dof_count};
  bool has_prior_z = false;
  double last_residual_norm = 0.0;
  double last_correction_norm = 0.0;
  std::size_t last_residual_row = 0U;
  std::size_t last_correction_row = 0U;
  for (std::size_t iteration = 0U; iteration <= 100U; ++iteration) {
    auto q = Lift(chart, p, z);
    if (!q.HasValue()) return Result<EmbeddingRoot>::Failure(q.GetStatus());
    auto operators = evaluate(q.Value());
    if (!operators.HasValue())
      return Result<EmbeddingRoot>::Failure(operators.GetStatus());
    if (operators.Value().rows.size() != z.Size())
      return Result<EmbeddingRoot>::Failure(
          Failure("Reduced graph row count changed during its solve.",
                  "dynamic-mass-regularity"));
    Vector residual{z.Size()};
    double norm = 0.0;
    double relative_norm = 0.0;
    bool position_certified = true;
    for (std::size_t row = 0U; row < z.Size(); ++row) {
      residual[row] = operators.Value().rows[row].value;
      if (!std::isfinite(residual[row]))
        return Result<EmbeddingRoot>::Failure(
            Failure("Reduced graph residual is nonfinite."));
      const double scale = operators.Value().rows[row].value_scale;
      if (!std::isfinite(scale) || scale < 0.0)
        return Result<EmbeddingRoot>::Failure(
            Failure("Reduced graph residual scale is invalid."));
      const double relative =
          scale == 0.0 ? (residual[row] == 0.0
                              ? 0.0
                              : std::numeric_limits<double>::infinity())
                       : std::abs(residual[row]) / scale;
      if (scale != 0.0 && !std::isfinite(relative))
        return Result<EmbeddingRoot>::Failure(
            Failure("Reduced graph residual normalization is nonfinite."));
      relative_norm = std::max(relative_norm, relative);
      position_certified = position_certified &&
                           (scale == 0.0 ? residual[row] == 0.0
                                         : relative <= 1.0e-10);
      if (std::abs(residual[row]) > norm) {
        norm = std::abs(residual[row]);
        last_residual_row = row;
      }
    }
    last_residual_norm = norm;
    // Position termination reuses the existing N3 hidden-constraint scale and
    // N2 rotational-coordinate correction criterion.  The caller still
    // certifies every full endpoint quantity before commit.
    if (norm == 0.0 ||
        (position_certified &&
         (iteration == 0U || last_correction_norm <= 1.0e-10)))
      return Result<EmbeddingRoot>::Success(
          {p, z, std::move(q.Value()), std::move(operators.Value()), norm,
           relative_norm, last_correction_norm, iteration});
    if (iteration == 100U) {
      std::ostringstream message;
      message << "stage=inner-graph: reduced graph exhausted 100 Newton corrections; last residual infinity norm "
              << last_residual_norm << " at row " << last_residual_row
              << ", last correction infinity norm " << last_correction_norm
              << " at column " << last_correction_row
              << ", normalized residual infinity norm " << relative_norm << ".";
      return Result<EmbeddingRoot>::Failure(
          Failure(message.str(), "dynamic-nonconvergence"));
    }
    const Matrix graph = GraphJacobian(operators.Value(), chart);
    auto correction = SolveGraphSystem(graph, residual, solver);
    if (!correction.HasValue())
      return Result<EmbeddingRoot>::Failure(correction.GetStatus());
    last_correction_norm = 0.0;
    bool changed = false;
    Vector next_z{z.Size()};
    for (std::size_t i = 0U; i < z.Size(); ++i) {
      const double next = z[i] - correction.Value()[i];
      changed = changed || next != z[i];
      next_z[i] = next;
      if (std::abs(correction.Value()[i]) > last_correction_norm) {
        last_correction_norm = std::abs(correction.Value()[i]);
        last_correction_row = i;
      }
    }
    // A binary64 graph can reach an unrepresentable residual before it reaches
    // literal zero.  Return that immutable trial only so the caller can apply
    // the existing full hidden-constraint acceptance gates; it is never an
    // independent reduced acceptance tolerance.
    if (!changed)
      return Result<EmbeddingRoot>::Success(
          {p, z, std::move(q.Value()), std::move(operators.Value()), norm,
           relative_norm, last_correction_norm, iteration});
    // A two-cycle is another binary64 stagnation state.  Keep the already
    // evaluated trial (rather than accepting the graph) so the caller's
    // existing original hidden-constraint evidence decides whether it is
    // admissible.
    if (has_prior_z && ExactlyEqual(next_z, prior_z))
      return Result<EmbeddingRoot>::Success(
          {p, z, std::move(q.Value()), std::move(operators.Value()), norm,
           relative_norm, last_correction_norm, iteration});
    auto next_q = Lift(chart, p, next_z);
    if (!next_q.HasValue()) return Result<EmbeddingRoot>::Failure(next_q.GetStatus());
    // Fixed-basis lifting can absorb changed private coordinates. Return only
    // the current evaluated trial; original full endpoint checks still decide.
    if (BitwiseEqual(next_q.Value(), q.Value()) ||
        (has_prior_z && BitwiseEqual(next_q.Value(), prior_q)))
      return Result<EmbeddingRoot>::Success(
          {p, z, std::move(q.Value()), std::move(operators.Value()), norm,
           relative_norm, last_correction_norm, iteration});
    prior_z = z;
    prior_q = q.Value();
    has_prior_z = true;
    z = std::move(next_z);
    FESA_HHT_PROBE_COUNT(kInnerCorrection);
  }
  return Result<EmbeddingRoot>::Failure(Failure("Unreachable graph loop."));
}

Result<EmbeddingFirstDerivatives> EvaluateEmbeddingFirstDerivatives(
    const FixedChart& chart, const EmbeddingRoot& root, TangentSolver& solver) {
  if (!ValidBasis(chart) || root.p.Size() != chart.p_dofs.size() || root.z.Size() != chart.z_dofs.size() ||
      root.q.Size() != chart.full_dof_count ||
      root.operators.rows.size() != chart.z_dofs.size())
    return Result<EmbeddingFirstDerivatives>::Failure(
        Failure("Reduced embedding root does not match its fixed chart.",
                "dynamic-mass-regularity"));
  const Matrix graph = GraphJacobian(root.operators, chart);
  const Matrix physical = PhysicalJacobian(root.operators, chart);
  Matrix jacobian{chart.full_dof_count, chart.p_dofs.size()};
  for (std::size_t column = 0U; column < chart.p_dofs.size(); ++column) {
    Vector rhs{chart.z_dofs.size()};
    for (std::size_t row = 0U; row < rhs.Size(); ++row)
      rhs[row] = -physical(row, column);
    auto dz = SolveGraphSystem(graph, rhs, solver);
    if (!dz.HasValue())
      return Result<EmbeddingFirstDerivatives>::Failure(dz.GetStatus());
    Vector unit{chart.p_columns.size()};
    unit[column] = 1.0;
    auto action = Lift(chart, unit, dz.Value());
    if (!action.HasValue())
      return Result<EmbeddingFirstDerivatives>::Failure(action.GetStatus());
    for (std::size_t row = 0U; row < action.Value().Size(); ++row)
      jacobian(row, column) = action.Value()[row];
  }
  Vector rhs{chart.z_dofs.size()};
  for (std::size_t row = 0U; row < rhs.Size(); ++row)
    rhs[row] = -root.operators.rows[row].time_first;
  auto dzdt = SolveGraphSystem(graph, rhs, solver);
  if (!dzdt.HasValue())
    return Result<EmbeddingFirstDerivatives>::Failure(dzdt.GetStatus());
  auto time = Lift(chart, Vector{chart.p_columns.size()}, dzdt.Value());
  if (!time.HasValue())
    return Result<EmbeddingFirstDerivatives>::Failure(time.GetStatus());
  return Result<EmbeddingFirstDerivatives>::Success(
      {std::move(jacobian), std::move(time.Value())});
}

Result<ReconstructedRates> ReconstructPhysicalRates(
    const FixedChart& chart, const EmbeddingRoot& root, const Vector& vp,
    const Vector& ap, TangentSolver& solver) {
  FESA_HHT_PROBE_SCOPE(kRates);
  if (!ValidBasis(chart) || root.q.Size() != chart.full_dof_count ||
      root.operators.rows.size() != chart.z_dofs.size() ||
      vp.Size() != chart.p_dofs.size() || ap.Size() != chart.p_dofs.size() ||
      !Finite(vp) || !Finite(ap))
    return Result<ReconstructedRates>::Failure(
        Failure("Reduced physical-rate reconstruction inputs are invalid."));
  const Matrix graph = GraphJacobian(root.operators, chart);
  auto p_velocity = Lift(chart, vp, Vector{chart.z_columns.size()});
  auto p_acceleration = Lift(chart, ap, Vector{chart.z_columns.size()});
  if (!p_velocity.HasValue())
    return Result<ReconstructedRates>::Failure(p_velocity.GetStatus());
  if (!p_acceleration.HasValue())
    return Result<ReconstructedRates>::Failure(p_acceleration.GetStatus());
  Vector velocity_rhs{chart.z_dofs.size()};
  for (std::size_t row = 0U; row < root.operators.rows.size(); ++row) {
    RateSum rhs;
    rhs.Add(-root.operators.rows[row].time_first);
    for (const auto& entry : root.operators.rows[row].gradient)
      rhs.Add(-entry.value * p_velocity.Value()[entry.dof]);
    velocity_rhs[row] = rhs.Value();
  }
  auto z_velocity = SolveGraphSystem(graph, velocity_rhs, solver);
  if (!z_velocity.HasValue())
    return Result<ReconstructedRates>::Failure(z_velocity.GetStatus());
  auto velocity = Lift(chart, vp, z_velocity.Value());
  if (!velocity.HasValue())
    return Result<ReconstructedRates>::Failure(velocity.GetStatus());
  Vector acceleration_rhs{chart.z_dofs.size()};
  for (std::size_t row = 0U; row < root.operators.rows.size(); ++row) {
    RateSum hessian_velocity;
    for (const auto& entry : root.operators.rows[row].hessian)
      hessian_velocity.Add(entry.value * velocity.Value()[entry.row] *
                           velocity.Value()[entry.column]);
    RateSum time_velocity;
    for (const auto& entry : root.operators.rows[row].time_gradient)
      time_velocity.Add(entry.value * velocity.Value()[entry.dof]);
    RateSum rhs;
    rhs.Add(-hessian_velocity.Value());
    rhs.Add(-2.0 * time_velocity.Value());
    rhs.Add(-root.operators.rows[row].time_second);
    for (const auto& entry : root.operators.rows[row].gradient)
      rhs.Add(-entry.value * p_acceleration.Value()[entry.dof]);
    acceleration_rhs[row] = rhs.Value();
  }
  auto z_acceleration = SolveGraphSystem(graph, acceleration_rhs, solver);
  if (!z_acceleration.HasValue())
    return Result<ReconstructedRates>::Failure(z_acceleration.GetStatus());
  auto acceleration = Lift(chart, ap, z_acceleration.Value());
  if (!acceleration.HasValue())
    return Result<ReconstructedRates>::Failure(acceleration.GetStatus());
  return Result<ReconstructedRates>::Success(
      {std::move(velocity.Value()), std::move(acceleration.Value()),
       std::move(z_velocity.Value()), std::move(z_acceleration.Value())});
}

bool ShouldRefineAngular(
    const FixedChart& chart, const std::array<double, 2U>& residual_metrics,
    const std::array<double, 4U>& translation_corrections,
    const bool translation_private_algorithmic_pass) {
  bool has_translation = false;
  bool has_rotation = !chart.z_dofs.empty();
  if (!ValidBasis(chart)) return false;
  for (const auto& column : chart.p_columns) {
    if (column.channel == 0U) has_translation = true;
    else has_rotation = true;
  }
  if (!has_translation || !has_rotation ||
      !translation_private_algorithmic_pass ||
      !std::isfinite(residual_metrics[0U]) ||
      !std::isfinite(residual_metrics[1U]) ||
      residual_metrics[0U] < 0.0 || residual_metrics[0U] > 1e-10 ||
      residual_metrics[1U] <= 1e-10)
    return false;
  for (const auto correction : translation_corrections)
    if (!std::isfinite(correction) || correction < 0.0 ||
        correction > 1e-10)
      return false;
  return true;
}

Result<AugmentedSolveSelection> SelectAugmentedSolve(
    const AugmentedReducedSystem& full, const FixedChart& chart,
    const bool angular_only) {
  const std::size_t np = chart.p_dofs.size();
  const std::size_t size = np + 3U * chart.z_dofs.size();
  if (!ValidBasis(chart) || full.physical_count != np ||
      full.algebraic_count != chart.z_dofs.size() ||
      full.tangent.Rows() != size || full.tangent.Columns() != size ||
      full.rhs.Size() != size || !Finite(full.rhs))
    return Result<AugmentedSolveSelection>::Failure(
        Failure("Reduced Newton selector inventory is invalid."));
  std::vector<std::size_t> indices;
  for (std::size_t i = 0U; i < np; ++i) {
    if (chart.p_dofs[i] >= chart.full_dof_count)
      return Result<AugmentedSolveSelection>::Failure(
          Failure("Reduced Newton selector DOF is invalid."));
    if (!angular_only || chart.p_columns[i].channel == 1U)
      indices.push_back(i);
  }
  for (std::size_t i = np; i < size; ++i) indices.push_back(i);
  if (!angular_only)
    return Result<AugmentedSolveSelection>::Success(
        {full.tangent, full.rhs, std::move(indices), false});
  std::vector<std::size_t> inverse(size, size);
  for (std::size_t i = 0U; i < indices.size(); ++i) inverse[indices[i]] = i;
  Vector rhs{indices.size()};
  std::vector<CooContribution> entries;
  for (std::size_t row = 0U; row < indices.size(); ++row) {
    rhs[row] = full.rhs[indices[row]];
    for (std::size_t k = full.tangent.RowOffsets()[indices[row]];
         k < full.tangent.RowOffsets()[indices[row] + 1U]; ++k) {
      const auto column = inverse[full.tangent.ColumnIndices()[k]];
      if (column != size)
        entries.push_back({row, column, full.tangent.Values()[k], 0U, k});
    }
  }
  auto tangent = hht_internal::MakeSparse(indices.size(), indices.size(),
                                           std::move(entries));
  if (!tangent.HasValue())
    return Result<AugmentedSolveSelection>::Failure(tangent.GetStatus());
  return Result<AugmentedSolveSelection>::Success(
      {std::move(tangent.Value()), std::move(rhs), std::move(indices), true});
}

Result<Vector> EmbedAugmentedSolution(
    const AugmentedSolveSelection& selected, const Vector& solution,
    const std::size_t full_size) {
  if (solution.Size() != selected.full_indices.size() || !Finite(solution))
    return Result<Vector>::Failure(
        Failure("Reduced Newton selected solution is invalid."));
  Vector full{full_size};
  std::size_t previous = 0U;
  for (std::size_t i = 0U; i < selected.full_indices.size(); ++i) {
    const auto index = selected.full_indices[i];
    if (index >= full_size || (i != 0U && index <= previous))
      return Result<Vector>::Failure(
          Failure("Reduced Newton solution embed inventory is invalid."));
    full[index] = solution[i];
    previous = index;
  }
  return Result<Vector>::Success(std::move(full));
}

Result<AugmentedReducedSystem> AssembleAugmentedReducedSystem(
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    const ConstraintOperators& operators, const double velocity_rate,
    const double acceleration_rate, const double reference_length,
    const double time_increment) {
  const std::size_t np = chart.p_columns.size();
  const std::size_t nz = chart.z_columns.size();
  const std::size_t total = np + 3U * nz;
  const auto& mass = assembly.mass;
  const auto& position = assembly.position_tangent;
  const auto& velocity = assembly.velocity_tangent;
  const auto& internal = assembly.internal_tangent;
  const double h2 = time_increment * time_increment;
  if (!ValidBasis(chart) || operators.rows.size() != nz ||
      chart.full_dof_count != mass.Rows() || mass.Rows() != mass.Columns() ||
      position.Rows() != mass.Rows() || velocity.Rows() != mass.Rows() ||
      internal.Rows() != mass.Rows() ||
      assembly.frame.residual.Size() != chart.full_dof_count ||
      !std::isfinite(velocity_rate) || !std::isfinite(acceleration_rate) ||
      !std::isfinite(reference_length) || !(reference_length > 0.0) ||
      !std::isfinite(time_increment) || !(time_increment > 0.0) ||
      !std::isfinite(h2) || !(h2 > 0.0) ||
      !std::isfinite(1.0 / time_increment) || !std::isfinite(1.0 / h2))
    return Result<AugmentedReducedSystem>::Failure(
        Failure("Reduced augmented tangent inventory or scaling is invalid."));
  if (mass.RowOffsets() != position.RowOffsets() ||
      mass.ColumnIndices() != position.ColumnIndices() ||
      mass.RowOffsets() != velocity.RowOffsets() ||
      mass.ColumnIndices() != velocity.ColumnIndices() ||
      mass.RowOffsets() != internal.RowOffsets() ||
      mass.ColumnIndices() != internal.ColumnIndices())
    return Result<AugmentedReducedSystem>::Failure(
        Failure("Reduced augmented tangent requires one common full pattern."));
  using Support = std::pair<std::size_t, double>;
  std::vector<std::vector<Support>> p_support(chart.full_dof_count);
  std::vector<std::vector<Support>> z_support(chart.full_dof_count);
  for (std::size_t i = 0U; i < np; ++i)
    for (const auto& entry : chart.p_columns[i].support)
      p_support[entry.dof].push_back({i, entry.value});
  for (std::size_t i = 0U; i < nz; ++i)
    for (const auto& entry : chart.z_columns[i].support)
      z_support[entry.dof].push_back({i, entry.value});
  std::map<std::pair<std::size_t, std::size_t>, RateSum> blocks;
  const auto add = [&](const std::size_t row, const std::size_t column,
                       const double value) { blocks[{row, column}].Add(value); };
  auto residual = ProjectPhysical(chart, assembly.frame.residual);
  if (!residual.HasValue())
    return Result<AugmentedReducedSystem>::Failure(residual.GetStatus());
  Vector rhs{total};
  for (std::size_t pr = 0U; pr < np; ++pr) {
    rhs[pr] = -residual.Value()[pr];
    for (const auto& row_basis : chart.p_columns[pr].support) {
      const auto row = row_basis.dof;
      for (std::size_t offset = mass.RowOffsets()[row];
           offset < mass.RowOffsets()[row + 1U]; ++offset) {
        const auto column = mass.ColumnIndices()[offset];
        const double kq = position.Values()[offset] + internal.Values()[offset];
        const double iv = velocity.Values()[offset];
        const double m = mass.Values()[offset];
        for (const auto& basis : p_support[column])
          add(pr, basis.first,
              row_basis.value * (kq + velocity_rate * iv + acceleration_rate * m) *
                  basis.second);
        for (const auto& basis : z_support[column]) {
          add(pr, np + basis.first, row_basis.value * kq * basis.second);
          add(pr, np + nz + basis.first, row_basis.value * iv * basis.second);
          add(pr, np + 2U * nz + basis.first, row_basis.value * m * basis.second);
        }
      }
    }
  }
  for (std::size_t constraint = 0U; constraint < nz; ++constraint) {
    const auto& row = operators.rows[constraint];
    std::map<std::size_t, RateSum> gradient_w;
    std::map<std::size_t, RateSum> curvature;
    for (const auto& entry : row.time_gradient)
      gradient_w[entry.dof].Add(entry.value);
    for (const auto& entry : row.hessian) {
      gradient_w[entry.row].Add(entry.value * assembly.frame.velocity[entry.column]);
      curvature[entry.row].Add(entry.value * assembly.frame.acceleration[entry.column]);
    }
    for (const auto& entry : row.third_directional_gradient)
      curvature[entry.dof].Add(entry.value);
    const auto contract = [&](const std::map<std::size_t, RateSum>& values,
                              const FixedBasisColumn& basis) {
      RateSum sum;
      for (const auto& entry : basis.support) {
        const auto found = values.find(entry.dof);
        if (found != values.end()) sum.Add(found->second.Value() * entry.value);
      }
      return sum.Value();
    };
    const auto qrow = np + constraint;
    const auto vrow = np + nz + constraint;
    const auto arow = np + 2U * nz + constraint;
    for (std::size_t i = 0U; i < np; ++i) {
      const double c = GradientDot(row.gradient, chart.p_columns[i]);
      const double w = contract(gradient_w, chart.p_columns[i]);
      const double a = contract(curvature, chart.p_columns[i]);
      add(qrow, i, c);
      add(vrow, i, velocity_rate * c + w);
      add(arow, i, acceleration_rate * c + a + 2.0 * velocity_rate * w);
    }
    for (std::size_t i = 0U; i < nz; ++i) {
      const double c = GradientDot(row.gradient, chart.z_columns[i]);
      const double w = contract(gradient_w, chart.z_columns[i]);
      const double a = contract(curvature, chart.z_columns[i]);
      add(qrow, np + i, c);
      add(vrow, np + i, w);
      add(vrow, np + nz + i, c);
      add(arow, np + i, a);
      add(arow, np + nz + i, 2.0 * w);
      add(arow, np + 2U * nz + i, c);
    }
  }
  Vector row_scale{total};
  Vector unknown_scale{total};
  for (std::size_t i = 0U; i < np; ++i)
    row_scale[i] = unknown_scale[i] =
        chart.p_columns[i].channel == 0U ? reference_length : 1.0;
  for (std::size_t i = 0U; i < nz; ++i) {
    row_scale[np + i] = unknown_scale[np + i] = 1.0;
    row_scale[np + nz + i] = time_increment;
    unknown_scale[np + nz + i] = 1.0 / time_increment;
    row_scale[np + 2U * nz + i] = h2;
    unknown_scale[np + 2U * nz + i] = 1.0 / h2;
  }
  std::vector<CooContribution> entries;
  entries.reserve(blocks.size());
  for (const auto& block : blocks) {
    const double raw = block.second.Value();
    const double scaled = row_scale[block.first.first] * raw *
                          unknown_scale[block.first.second];
    if (!std::isfinite(raw) || !std::isfinite(scaled) ||
        (raw != 0.0 && scaled == 0.0))
      return Result<AugmentedReducedSystem>::Failure(
          Failure("Reduced augmented operator scaling overflowed or underflowed."));
    entries.push_back({block.first.first, block.first.second, scaled, 0U, entries.size()});
  }
  for (std::size_t i = 0U; i < rhs.Size(); ++i) {
    const double raw = rhs[i];
    rhs[i] *= row_scale[i];
    if (!std::isfinite(rhs[i]) || (raw != 0.0 && rhs[i] == 0.0))
      return Result<AugmentedReducedSystem>::Failure(
          Failure("Reduced augmented RHS scaling overflowed or underflowed."));
  }
  auto tangent = hht_internal::MakeSparse(total, total, std::move(entries));
  if (!tangent.HasValue())
    return Result<AugmentedReducedSystem>::Failure(tangent.GetStatus());
  return Result<AugmentedReducedSystem>::Success(
      {std::move(tangent.Value()), std::move(rhs), np, nz,
       std::move(row_scale), std::move(unknown_scale)});
}

Result<AugmentedCorrection> MapAugmentedCorrection(
    const FixedChart& chart, const AugmentedReducedSystem& system,
    const Vector& solution, const double velocity_rate,
    const double acceleration_rate) {
  const std::size_t np = chart.p_dofs.size();
  const std::size_t nz = chart.z_dofs.size();
  if (!ValidBasis(chart) || system.physical_count != np || system.algebraic_count != nz ||
      solution.Size() != np + 3U * nz || !Finite(solution) ||
      system.unknown_scale.Size() != solution.Size() ||
      !std::isfinite(velocity_rate) || !std::isfinite(acceleration_rate))
    return Result<AugmentedCorrection>::Failure(
        Failure("Reduced augmented correction inventory is invalid."));
  AugmentedCorrection result{Vector{np}, Vector{nz}, Vector{nz}, Vector{nz},
                             Vector{chart.full_dof_count},
                             Vector{chart.full_dof_count},
                             Vector{chart.full_dof_count}};
  for (std::size_t i = 0U; i < solution.Size(); ++i) {
    const double scale = system.unknown_scale[i];
    const double raw = solution[i] * scale;
    if (!std::isfinite(scale) || !(scale > 0.0) || !std::isfinite(raw) ||
        (solution[i] != 0.0 && raw == 0.0))
      return Result<AugmentedCorrection>::Failure(
          Failure("Reduced augmented scale undo overflowed or underflowed."));
  }
  for (std::size_t i = 0U; i < np; ++i) {
    result.p[i] = solution[i] * system.unknown_scale[i];
  }
  for (std::size_t i = 0U; i < nz; ++i) {
    result.z[i] = solution[np + i] * system.unknown_scale[np + i];
    result.z_velocity[i] = solution[np + nz + i] * system.unknown_scale[np + nz + i];
    result.z_acceleration[i] = solution[np + 2U * nz + i] * system.unknown_scale[np + 2U * nz + i];
  }
  Vector p_velocity{np};
  Vector p_acceleration{np};
  for (std::size_t i = 0U; i < np; ++i) {
    p_velocity[i] = velocity_rate * result.p[i];
    p_acceleration[i] = acceleration_rate * result.p[i];
  }
  auto coordinate = Lift(chart, result.p, result.z);
  auto velocity = Lift(chart, p_velocity, result.z_velocity);
  auto acceleration = Lift(chart, p_acceleration, result.z_acceleration);
  if (!coordinate.HasValue()) return Result<AugmentedCorrection>::Failure(coordinate.GetStatus());
  if (!velocity.HasValue()) return Result<AugmentedCorrection>::Failure(velocity.GetStatus());
  if (!acceleration.HasValue()) return Result<AugmentedCorrection>::Failure(acceleration.GetStatus());
  result.coordinate = std::move(coordinate.Value());
  result.velocity = std::move(velocity.Value());
  result.acceleration = std::move(acceleration.Value());
  if (!Finite(result.coordinate) || !Finite(result.velocity) ||
      !Finite(result.acceleration))
    return Result<AugmentedCorrection>::Failure(
        Failure("Reduced augmented correction mapped to nonfinite rates."));
  return Result<AugmentedCorrection>::Success(std::move(result));
}

Result<Vector> SolveGraphSystem(const Matrix& matrix, const Vector& rhs,
                                TangentSolver& solver) {
  if (matrix.Rows() != matrix.Columns() || rhs.Size() != matrix.Rows() ||
      !Finite(rhs))
    return Result<Vector>::Failure(
        Failure("Reduced graph solve dimensions or RHS are invalid."));
  for (std::size_t i = 0U; i < matrix.Rows(); ++i)
    for (std::size_t j = 0U; j < matrix.Columns(); ++j)
      if (!std::isfinite(matrix(i, j)))
        return Result<Vector>::Failure(
            Failure("Reduced graph matrix must remain finite."));
  if (rhs.Size() == 0U) return Result<Vector>::Success(Vector{0U});
  auto sparse = DenseSparse(matrix);
  if (!sparse.HasValue()) return Result<Vector>::Failure(sparse.GetStatus());
  Status status = solver.FactorizeFresh(sparse.Value());
  if (!status.IsOk()) return Result<Vector>::Failure(status);
  Vector solution{rhs.Size()};
  status = solver.SolveCorrection(rhs, solution);
  if (!status.IsOk()) return Result<Vector>::Failure(status);
  status = hht_internal::CheckLinearSolve(sparse.Value(), rhs, solution);
  if (!status.IsOk()) return Result<Vector>::Failure(status);
  return Result<Vector>::Success(std::move(solution));
}

Result<double> CheckGraphRegularity(
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    const ConstraintOperators& operators, TangentSolver& solver) {
  const std::size_t size = chart.z_dofs.size();
  if (!ValidBasis(chart) || operators.rows.size() != size || chart.null_nodes.size() != size ||
      chart.pivot_components.size() != size ||
      assembly.drilling_metric.Rows() != chart.full_dof_count ||
      assembly.drilling_metric.Columns() != chart.full_dof_count ||
      assembly.rotations.size() * 6U != chart.full_dof_count)
    return Result<double>::Failure(
        Failure("Reduced graph regularity inventory is invalid.",
                "dynamic-mass-regularity"));
  auto transverse = CheckSelectorTransversality(assembly, chart, solver);
  if (!transverse.HasValue()) return Result<double>::Failure(transverse.GetStatus());
  if (size == 0U) return Result<double>::Success(0.0);
  Vector weights{size};
  for (std::size_t row = 0U; row < size; ++row) {
    const auto node = chart.null_nodes[row];
    if (node >= assembly.rotations.size())
      return Result<double>::Failure(
          Failure("Reduced graph regularity node is invalid.",
                  "dynamic-mass-regularity"));
    const auto& null = assembly.rotations[node].drill_null;
    const double metric = hht_internal::OrderedNullMetric(
        assembly.drilling_metric, node, null);
    if (!std::isfinite(metric) || !(metric > 0.0))
      return Result<double>::Failure(Failure(
          "Reduced graph requires a positive finite null drilling metric.",
          "dynamic-mass-regularity"));
    weights[row] = std::sqrt(metric);
  }
  const Matrix graph = GraphJacobian(operators, chart);
  Matrix scaled{size, size};
  double norm = 0.0;
  for (std::size_t row = 0U; row < size; ++row) {
    double row_sum = 0.0;
    for (std::size_t column = 0U; column < size; ++column) {
      scaled(row, column) = graph(row, column) / weights[row] / weights[column];
      if (!std::isfinite(scaled(row, column)))
        return Result<double>::Failure(
            Failure("Reduced graph scaled operator is nonfinite.",
                    "dynamic-mass-regularity"));
      row_sum += std::abs(scaled(row, column));
    }
    norm = std::max(norm, row_sum);
  }
  auto sparse = DenseSparse(scaled);
  if (!sparse.HasValue()) return Result<double>::Failure(sparse.GetStatus());
  Status status = solver.FactorizeFresh(sparse.Value());
  if (!status.IsOk()) return Result<double>::Failure(status);
  double inverse_norm = 0.0;
  status = hht_internal::AccumulateCheckedCanonicalNorm(
      sparse.Value(), solver, inverse_norm);
  if (!status.IsOk()) return Result<double>::Failure(status);
  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 << "Reduced graph reciprocal infinity condition " << condition
            << " is below 2^-32.";
    return Result<double>::Failure(
        Failure(message.str(), "dynamic-mass-regularity"));
  }
  return Result<double>::Success(condition);
}

Result<double> CheckSelectorTransversality(
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    TangentSolver& solver) {
  if (!ValidBasis(chart) || chart.null_nodes.size() != chart.z_columns.size() ||
      assembly.rotations.size() * 6U != chart.full_dof_count)
    return Result<double>::Failure(Failure(
        "Reduced spanning operator inventory is invalid.", "dynamic-mass-regularity"));
  if (chart.p_columns.empty() && chart.z_columns.empty())
    return Result<double>::Success(0.0);
  // N1 scaling cancels for identity translation columns. Angular blocks use 1.
  double norm = 1.0;
  double inverse_norm = 1.0;
  for (std::size_t node = 0U; node < assembly.rotations.size(); ++node) {
    std::vector<std::size_t> rows;
    std::vector<const FixedBasisColumn*> columns;
    for (const auto& column : chart.p_columns)
      if (column.channel == 1U && column.node == node) columns.push_back(&column);
    std::size_t null_row = chart.null_nodes.size();
    for (std::size_t i = 0U; i < chart.null_nodes.size(); ++i)
      if (chart.null_nodes[i] == node) null_row = i;
    for (std::size_t component = 0U; component < 3U; ++component) {
      const auto full = 6U * node + 3U + component;
      bool present = false;
      for (const auto* column : columns)
        for (const auto& entry : column->support) present = present || entry.dof == full;
      if (null_row != chart.null_nodes.size())
        for (const auto& entry : chart.z_columns[null_row].support)
          present = present || entry.dof == full;
      if (present) rows.push_back(full);
    }
    const bool has_null = null_row != chart.null_nodes.size();
    if (columns.size() + (has_null ? 1U : 0U) != rows.size())
      return Result<double>::Failure(Failure(
          "Reduced spanning block is not square.", "dynamic-mass-regularity"));
    if (rows.empty()) continue;
    Matrix block{rows.size(), rows.size()};
    for (std::size_t row = 0U; row < rows.size(); ++row)
      for (std::size_t column = 0U; column < columns.size(); ++column)
        for (const auto& entry : columns[column]->support)
          if (entry.dof == rows[row]) block(row, column) = entry.value;
    if (has_null) {
      std::array<BasisValue, 3U> values{};
      for (std::size_t i = 0U; i < 3U; ++i)
        values[i] = {assembly.rotations[node].drill_null[i], 0.0};
      const double length = BasisNorm(values).value;
      if (!std::isfinite(length) || !(length > 0.0))
        return Result<double>::Failure(Failure(
            "Reduced spanning block has an invalid actual null.", "dynamic-mass-regularity"));
      for (std::size_t row = 0U; row < rows.size(); ++row)
        block(row, columns.size()) = assembly.rotations[node].drill_null[rows[row] % 6U - 3U] / length;
    }
    for (std::size_t row = 0U; row < block.Rows(); ++row) {
      double sum = 0.0;
      for (std::size_t column = 0U; column < block.Columns(); ++column)
        sum += std::abs(block(row, column));
      norm = (std::max)(norm, sum);
    }
    auto sparse = DenseSparse(block);
    if (!sparse.HasValue()) return Result<double>::Failure(sparse.GetStatus());
    auto status = solver.FactorizeFresh(sparse.Value());
    if (!status.IsOk()) return Result<double>::Failure(status);
    status = hht_internal::AccumulateCheckedCanonicalNorm(
        sparse.Value(), solver, inverse_norm);
    if (!status.IsOk()) return Result<double>::Failure(status);
  }
  const double condition = 1.0 / norm / inverse_norm;
  if (!std::isfinite(condition) || condition < std::ldexp(1.0, -32))
    return Result<double>::Failure(Failure(
        "Reduced normalized spanning reciprocal condition is below 2^-32.",
        "dynamic-mass-regularity"));
  return Result<double>::Success(condition);
}

}  // namespace fesa::hht_reduced_internal