FESADev/src/fesa/analysis/solid_hht_representability.h

#ifndef FESA_ANALYSIS_SOLID_HHT_REPRESENTABILITY_H_
#define FESA_ANALYSIS_SOLID_HHT_REPRESENTABILITY_H_
#include <algorithm>
#include <array>
#include <cmath>
#include <limits>
#include <vector>

#include "fesa/analysis/solid_dynamic_analysis_state.h"
#include "fesa/math/sparse_matrix.h"
namespace fesa::detail {
struct SolidHhtRepresentableNeighbor {
  bool found{false};
  std::size_t full_dof{0};
  double displacement{0};
  double delta{0};
  double predicted_metric{0};
};
/// @brief Predicts a strictly improving one-ULP neighbor using separate
/// physical inertia/internal-force actions and every original free score.
/// This selects a trial only; callers must recompute actual endpoint gates.
inline bool SelectSolidHhtRepresentableNeighbor(
    const SparseMatrix& mass, const SparseMatrix& tangent,
    const std::vector<std::size_t>& free_dofs,
    const SolidDynamicAcceptedFrameState& frame, double h, double alpha,
    double beta, SolidHhtRepresentableNeighbor* selected,
    std::vector<SolidHhtRepresentableNeighbor>* ranked = nullptr) {
  *selected = {};
  if (ranked != nullptr) ranked->clear();
  const auto size = mass.Rows();
  if (tangent.Rows() != size || mass.Columns() != size ||
      tangent.Columns() != size || mass.RowOffsets() != tangent.RowOffsets() ||
      mass.ColumnIndices() != tangent.ColumnIndices() ||
      frame.displacement.Size() != size || frame.inertia_force.Size() != size ||
      frame.internal_force.Size() != size ||
      frame.external_force.Size() != size || frame.residual.Size() != size ||
      frame.residual_scale.Size() != size)
    return false;
  const double inertia_factor = 1 / ((1 + alpha) * beta * h * h);
  if (!std::isfinite(inertia_factor) || inertia_factor <= 0) return false;
  const auto score = [](double ma, double f, double F, double* metric) {
    const double scale = std::abs(ma) + std::abs(f) + std::abs(F);
    const double r = ma + f - F;
    if (!std::isfinite(scale) || !std::isfinite(r)) return false;
    *metric = scale == 0
                  ? (r == 0 ? 0 : std::numeric_limits<double>::infinity())
                  : std::abs(r) / scale;
    return std::isfinite(*metric);
  };
  std::vector<bool> is_free(size, false);
  std::vector<double> scores(size, 0);
  double best = 0;
  for (auto i : free_dofs) {
    if (i >= size || !std::isfinite(frame.displacement[i]) ||
        !score(frame.inertia_force[i], frame.internal_force[i],
               frame.external_force[i], &scores[i]))
      return false;
    is_free[i] = true;
    best = (std::max)(best, scores[i]);
  }
  struct Action {
    std::size_t row;
    double inertia;
    double internal;
  };
  std::vector<std::vector<Action>> columns(size);
  for (auto i : free_dofs)
    for (std::size_t k = mass.RowOffsets()[i]; k < mass.RowOffsets()[i + 1];
         ++k) {
      const auto j = mass.ColumnIndices()[k];
      if (!is_free[j]) continue;
      const double inertia = mass.Values()[k] * inertia_factor;
      const double internal = tangent.Values()[k];
      if (!std::isfinite(inertia) || !std::isfinite(internal)) return false;
      columns[j].push_back({i, inertia, internal});
    }
  const double original_metric = best;
  auto predicted = scores;
  for (auto j : free_dofs)
    for (const double direction : {-std::numeric_limits<double>::infinity(),
                                   std::numeric_limits<double>::infinity()}) {
      const double proposed = std::nextafter(frame.displacement[j], direction);
      const double delta = proposed - frame.displacement[j];
      if (!std::isfinite(proposed) || !std::isfinite(delta) || delta == 0)
        continue;
      predicted = scores;
      for (const auto& action : columns[j]) {
        const auto i = action.row;
        const double ma = frame.inertia_force[i] + action.inertia * delta;
        const double f = frame.internal_force[i] + action.internal * delta;
        if (!score(ma, f, frame.external_force[i], &predicted[i])) return false;
      }
      double metric = 0;
      for (auto i : free_dofs) metric = (std::max)(metric, predicted[i]);
      if (ranked != nullptr && metric < original_metric)
        ranked->push_back({true, j, proposed, delta, metric});
      // Stable full-DOF/direction ordering resolves ties without a floor.
      if (metric < best) {
        best = metric;
        *selected = {true, j, proposed, delta, metric};
      }
    }
  if (ranked != nullptr)
    std::stable_sort(ranked->begin(), ranked->end(),
                     [](const auto& a, const auto& b) {
                       return a.predicted_metric < b.predicted_metric;
                     });
  return true;
}
/// @brief Keeps complete eligible failed Newton entries for a fixed increment.
class SolidHhtNewtonHistory {
 public:
  bool Capture(const Vector& u, const std::vector<bool>& holding_mask,
               const std::array<double, 4>& incoming_metrics,
               std::uint32_t iteration, bool physical_pass) {
    if (!Valid(u, holding_mask, incoming_metrics)) return false;
    if (!physical_pass &&
        std::all_of(incoming_metrics.begin(), incoming_metrics.end(),
                    [](double metric) { return metric <= 1e-10; }))
      entries_.push_back({u, holding_mask, incoming_metrics, iteration});
    return true;
  }
  bool Match(const Vector& proposed_u,
             const std::vector<bool>& next_holding_mask,
             const std::array<double, 4>& fresh_metrics, bool physical_pass,
             SolidDynamicNewtonRepeatEvidence* evidence) const {
    *evidence = {};
    evidence->history_entry_count = entries_.size();
    if (!Valid(proposed_u, next_holding_mask, fresh_metrics)) return false;
    // A physically converged prospective endpoint must reach the normal gate.
    if (physical_pass ||
        !std::all_of(fresh_metrics.begin(), fresh_metrics.end(),
                     [](double metric) { return metric <= 1e-10; }))
      return true;
    for (const auto& entry : entries_) {
      evidence->changed_translations = 0;
      evidence->changed_holding_mask = 0;
      evidence->changed_correction_metrics = 0;
      for (std::size_t i = 0; i < proposed_u.Size(); ++i)
        evidence->changed_translations += proposed_u[i] != entry.u[i];
      for (std::size_t i = 0; i < next_holding_mask.size(); ++i)
        evidence->changed_holding_mask +=
            next_holding_mask[i] != entry.holding_mask[i];
      for (std::size_t i = 0; i < 4; ++i)
        evidence->changed_correction_metrics +=
            fresh_metrics[i] != entry.incoming_metrics[i];
      // Numeric correction magnitudes remain evidence. Their finite/PASS
      // controls, rather than their exact magnitudes, affect the transition.
      if (evidence->changed_translations == 0 &&
          evidence->changed_holding_mask == 0) {
        evidence->matched_entry_iteration = entry.iteration;
        evidence->repeat = true;
        break;
      }
    }
    return true;
  }
  std::size_t Size() const { return entries_.size(); }

 private:
  struct Entry {
    Vector u;
    std::vector<bool> holding_mask;
    std::array<double, 4> incoming_metrics;
    std::uint32_t iteration;
  };
  bool Valid(const Vector& u, const std::vector<bool>& mask,
             const std::array<double, 4>& metrics) const {
    if (mask.size() > u.Size() ||
        (!entries_.empty() &&
         (u.Size() != entries_.front().u.Size() ||
          mask.size() != entries_.front().holding_mask.size())))
      return false;
    for (std::size_t i = 0; i < u.Size(); ++i)
      if (!std::isfinite(u[i])) return false;
    for (double metric : metrics)
      if (!std::isfinite(metric) || metric < 0) return false;
    return true;
  }
  std::vector<Entry> entries_;
};
/// @brief Checks the fresh complete Newton trial before representable fallback.
/// Historical scores are deliberately absent: a lower half of a two-cycle
/// still needs actual proposed-step evaluation. False keeps common Newton
/// apply.
template <class Newton, class Neighbor>
Result<bool> TrySolidHhtActualStep(
    bool residual_pass, const std::array<double, 4>& fresh_corrections,
    Newton newton, Neighbor neighbor) {
  if (residual_pass || !std::all_of(fresh_corrections.begin(),
                                    fresh_corrections.end(), [](double metric) {
                                      return std::isfinite(metric) &&
                                             metric <= 1e-10;
                                    }))
    return Result<bool>::Success(false);
  auto keep_newton = newton();
  if (!keep_newton.HasValue())
    return Result<bool>::Failure(keep_newton.GetStatus());
  if (keep_newton.Value()) return Result<bool>::Success(false);
  return neighbor();
}
/// @brief Tries ranked predictions until actual endpoint evaluation improves.
/// Evaluator failure is terminal; a rejected prediction does not discard later
/// candidates, and success means only that a local trial improved.
template <class Evaluate>
Result<bool> TrySolidHhtRankedNeighbors(
    const std::vector<SolidHhtRepresentableNeighbor>& ranked,
    Evaluate evaluate) {
  for (const auto& neighbor : ranked) {
    auto actual = evaluate(neighbor);
    if (!actual.HasValue()) return Result<bool>::Failure(actual.GetStatus());
    if (actual.Value()) return Result<bool>::Success(true);
  }
  return Result<bool>::Success(false);
}
}  // namespace fesa::detail
#endif  // FESA_ANALYSIS_SOLID_HHT_REPRESENTABILITY_H_