FESADev/src/fesa/analysis/solid_hht_force_identity.h

#ifndef FESA_ANALYSIS_SOLID_HHT_FORCE_IDENTITY_H_
#define FESA_ANALYSIS_SOLID_HHT_FORCE_IDENTITY_H_

#include <cmath>
#include <initializer_list>
#include <limits>

#include "fesa/analysis/solid_dynamic_analysis_state.h"
#include "fesa/math/sparse_matrix.h"
#include "solid_hht_high_low.h"

namespace fesa::detail {
struct SolidHhtForceIdentity {
  double original{0};
  double solved{0};
  double scale{0};
  double defect{0};
  double metric{0};
};

struct SolidHhtPairedForceIdentity {
  math_internal::HighLow original;
  math_internal::HighLow solved;
  math_internal::HighLow scale;
  math_internal::HighLow defect;
  math_internal::HighLow metric;
};

/// @brief Keeps both physical parts and the actual old residual in the approved
/// weighted identity. Its scale includes each uncancelled mass product.
inline bool EvaluateSolidHhtHighLowForceIdentity(
    const SparseMatrix& mass, std::size_t row,
    math_internal::HighLow algorithmic_inertia,
    const SolidDynamicAcceptedFrameState& next,
    const SolidDynamicAcceptedFrameState& old, double alpha,
    SolidHhtPairedForceIdentity* result) {
  using namespace math_internal;
  if (!ArithmeticEnvironmentSupported() || row >= mass.Rows() ||
      !IsFinite(algorithmic_inertia) || !IsNormalized(algorithmic_inertia) ||
      !IsFinite(HighLow(alpha)))
    return false;
  for (const auto* frame : {&next, &old})
    if (frame->acceleration.Size() != mass.Columns() ||
        frame->low.acceleration.Size() != mass.Columns() ||
        frame->algorithmic_acceleration.Size() != mass.Columns() ||
        frame->low.algorithmic_acceleration.Size() != mass.Columns() ||
        frame->internal_force.Size() != mass.Rows() ||
        frame->low.internal_force.Size() != mass.Rows() ||
        frame->external_force.Size() != mass.Rows() ||
        frame->low.external_force.Size() != mass.Rows() ||
        frame->residual.Size() != mass.Rows() ||
        frame->low.residual.Size() != mass.Rows())
      return false;
  const HighLow a(alpha);
  const auto c = Add(HighLow(1), a);
  const HighLow f(next.internal_force[row], next.low.internal_force[row]);
  const HighLow F(next.external_force[row], next.low.external_force[row]);
  const HighLow old_f(old.internal_force[row], old.low.internal_force[row]);
  const HighLow old_F(old.external_force[row], old.low.external_force[row]);
  const HighLow r(next.residual[row], next.low.residual[row]);
  const HighLow old_r(old.residual[row], old.low.residual[row]);
  result->original =
      Subtract(Add(algorithmic_inertia, Multiply(c, Subtract(f, F))),
               Multiply(a, Subtract(old_f, old_F)));
  result->solved = Subtract(Multiply(c, r), Multiply(a, old_r));
  HighLow mass_scale;
  HighLow relation_action;
  for (std::size_t k = mass.RowOffsets()[row]; k < mass.RowOffsets()[row + 1];
       ++k) {
    const auto j = mass.ColumnIndices()[k];
    const HighLow coefficient(mass.Values()[k]);
    const HighLow A(next.algorithmic_acceleration[j],
                    next.low.algorithmic_acceleration[j]);
    const HighLow physical_a(next.acceleration[j], next.low.acceleration[j]);
    const HighLow old_a(old.acceleration[j], old.low.acceleration[j]);
    const auto relation =
        Add(Subtract(A, Multiply(c, physical_a)), Multiply(a, old_a));
    mass_scale =
        Add(mass_scale,
            Add(Add(Abs(Multiply(coefficient, A)),
                    Multiply(c, Abs(Multiply(coefficient, physical_a)))),
                Multiply(Abs(a), Abs(Multiply(coefficient, old_a)))));
    relation_action =
        Add(relation_action, Abs(Multiply(coefficient, relation)));
    if (!IsFinite(mass_scale) || !IsFinite(relation_action)) return false;
  }
  result->scale = Add(
      Add(Add(mass_scale, relation_action), Multiply(c, Add(Abs(f), Abs(F)))),
      Add(Add(Multiply(Abs(a), Add(Abs(old_f), Abs(old_F))),
              Multiply(c, Abs(r))),
          Abs(Multiply(a, old_r))));
  result->defect = Abs(Subtract(result->original, result->solved));
  if (!IsFinite(result->original) || !IsFinite(result->solved) ||
      !IsFinite(result->scale) || !IsFinite(result->defect))
    return false;
  if (Compare(result->scale, HighLow(0)) == 0) {
    if (Compare(result->defect, HighLow(0)) != 0) return false;
    result->metric = HighLow(0);
  } else {
    result->metric = Divide(result->defect, result->scale);
  }
  return IsFinite(result->metric) && IsNormalized(result->metric);
}

/// @brief Evaluates one original/equivalent HHT row, retaining old equilibrium
/// error. Checks each scalar before it can enter an evidence maximum.
inline bool EvaluateSolidHhtForceIdentity(
    const SparseMatrix& mass, std::size_t row, double algorithmic_inertia,
    const SolidDynamicAcceptedFrameState& next,
    const SolidDynamicAcceptedFrameState& old, double alpha,
    SolidHhtForceIdentity* result) {
  for (double x :
       {alpha, algorithmic_inertia, next.internal_force[row],
        next.external_force[row], old.internal_force[row],
        old.external_force[row], next.residual[row], old.residual[row]})
    if (!std::isfinite(x)) return false;
  result->original =
      algorithmic_inertia +
      (1 + alpha) * (next.internal_force[row] - next.external_force[row]) -
      alpha * (old.internal_force[row] - old.external_force[row]);
  result->solved = (1 + alpha) * next.residual[row] - alpha * old.residual[row];
  double mass_action_scale = 0;
  double relation_defect_action = 0;
  for (std::size_t entry = mass.RowOffsets()[row];
       entry < mass.RowOffsets()[row + 1]; ++entry) {
    const auto column = mass.ColumnIndices()[entry];
    const double coefficient = mass.Values()[entry];
    const double a = next.acceleration[column];
    const double A = next.algorithmic_acceleration[column];
    const double a_old = old.acceleration[column];
    const double relation_defect = A - (1 + alpha) * a + alpha * a_old;
    const double action_scale = std::abs(coefficient * A) +
                                (1 + alpha) * std::abs(coefficient * a) +
                                std::abs(alpha) * std::abs(coefficient * a_old);
    const double defect_action = std::abs(coefficient * relation_defect);
    for (double x : {a, A, a_old, relation_defect, action_scale, defect_action})
      if (!std::isfinite(x)) return false;
    mass_action_scale += action_scale;
    relation_defect_action += defect_action;
  }
  result->scale = mass_action_scale + relation_defect_action +
                  (1 + alpha) * (std::abs(next.internal_force[row]) +
                                 std::abs(next.external_force[row])) +
                  std::abs(alpha) * (std::abs(old.internal_force[row]) +
                                     std::abs(old.external_force[row])) +
                  (1 + alpha) * std::abs(next.residual[row]) +
                  std::abs(alpha * old.residual[row]);
  result->defect = std::abs(result->original - result->solved);
  for (double x : {result->original, result->solved, mass_action_scale,
                   relation_defect_action, result->scale, result->defect})
    if (!std::isfinite(x)) return false;
  result->metric =
      result->scale == 0
          ? (result->defect == 0 ? 0 : std::numeric_limits<double>::infinity())
          : result->defect / result->scale;
  return std::isfinite(result->metric);
}
/// @brief Finite checked failure-shadow accumulation, separate from physics.
inline bool AccumulateSolidHhtForceShadow(double term, double product_error,
                                          double* sum, double* correction,
                                          double* absolute_sum) {
  if (!std::isfinite(term) || !std::isfinite(product_error) ||
      !std::isfinite(*sum) || !std::isfinite(*correction) ||
      !std::isfinite(*absolute_sum))
    return false;
  const double next = *sum + term;
  const double absolute_next = *absolute_sum + std::abs(term);
  const double error_next = *correction + product_error;
  const double lost = std::abs(*sum) >= std::abs(term) ? (*sum - next) + term
                                                       : (term - next) + *sum;
  const double correction_next = error_next + lost;
  if (!std::isfinite(next) || !std::isfinite(absolute_next) ||
      !std::isfinite(error_next) || !std::isfinite(lost) ||
      !std::isfinite(correction_next))
    return false;
  *sum = next;
  *correction = correction_next;
  *absolute_sum = absolute_next;
  return true;
}
}  // namespace fesa::detail
#endif  // FESA_ANALYSIS_SOLID_HHT_FORCE_IDENTITY_H_