FESADev/src/fesa/analysis/solid_hht_high_low.h

#ifndef FESA_ANALYSIS_SOLID_HHT_HIGH_LOW_H_
#define FESA_ANALYSIS_SOLID_HHT_HIGH_LOW_H_

#include "fesa/analysis/hht_time_integration.h"
#include "fesa/analysis/solid_dynamic_analysis_state.h"
#include "fesa/math/sparse_matrix.h"
#include "math/high_low_arithmetic.h"

namespace fesa::detail {
/// @brief Keeps both physical parts of the fixed-increment predictor.
struct SolidHhtPairedPredictor {
  Vector displacement{0};
  Vector displacement_low{0};
  Vector velocity{0};
  Vector velocity_low{0};
};

/// @brief Owns physical and auxiliary rates without collapsing either part.
struct SolidHhtPairedRates {
  Vector velocity{0};
  Vector velocity_low{0};
  Vector acceleration{0};
  Vector acceleration_low{0};
  Vector algorithmic_acceleration{0};
  Vector algorithmic_acceleration_low{0};
};

struct SolidHhtPairedVector {
  Vector high{0};
  Vector low{0};
};

inline Status SolidHhtPairedKinematicsFailure() {
  return Status::Failure(FailureCategory::kSolver,
                         {{Severity::kError,
                           "solid-dynamic-invalid-paired-kinematics",
                           {},
                           "DYNAMIC",
                           {},
                           "Incomplete, unnormalized or nonfinite paired HHT "
                           "kinematics."}});
}

inline bool ValidSolidHhtPair(const Vector& high, const Vector& low,
                              std::size_t size) {
  if (high.Size() != size || low.Size() != size) return false;
  for (std::size_t i = 0; i < size; ++i) {
    const math_internal::HighLow value(high[i], low[i]);
    if (!math_internal::IsFinite(value) || !math_internal::IsNormalized(value))
      return false;
  }
  return true;
}

inline bool ValidSolidHhtParameters(double h, const HhtParameters& p) {
  if (!math_internal::ArithmeticEnvironmentSupported()) return false;
  const auto expected = MakeHhtParameters(p.alpha);
  if (!expected.HasValue() || p.beta != expected.Value().beta ||
      p.gamma != expected.Value().gamma)
    return false;
  const math_internal::HighLow step(h);
  const auto square = math_internal::Multiply(step, step);
  return math_internal::IsFinite(step) &&
         math_internal::Compare(step, math_internal::HighLow(0)) > 0 &&
         math_internal::IsFinite(square) &&
         math_internal::Compare(square, math_internal::HighLow(0)) > 0;
}

/// @brief Uses the authoritative constant Float64 mass coefficients without
/// collapsing either physical vector part or the ordered row accumulation.
inline Result<SolidHhtPairedVector> MultiplySolidHhtMassHighLow(
    const SparseMatrix& mass, const Vector& high, const Vector& low) {
  using namespace math_internal;
  if (!ArithmeticEnvironmentSupported() || mass.Rows() != mass.Columns() ||
      !ValidSolidHhtPair(high, low, mass.Columns()))
    return Result<SolidHhtPairedVector>::Failure(
        SolidHhtPairedKinematicsFailure());
  SolidHhtPairedVector result{Vector(mass.Rows()), Vector(mass.Rows())};
  for (std::size_t row = 0; row < mass.Rows(); ++row) {
    HighLow sum;
    for (std::size_t k = mass.RowOffsets()[row]; k < mass.RowOffsets()[row + 1];
         ++k) {
      const auto col = mass.ColumnIndices()[k];
      sum = Add(sum, Multiply(HighLow(mass.Values()[k]),
                              HighLow(high[col], low[col])));
      if (!IsFinite(sum) || !IsNormalized(sum))
        return Result<SolidHhtPairedVector>::Failure(
            SolidHhtPairedKinematicsFailure());
    }
    result.high[row] = sum.high;
    result.low[row] = sum.low;
  }
  return Result<SolidHhtPairedVector>::Success(std::move(result));
}

/// @brief Applies the unchanged Newmark predictor to the full physical state.
inline Result<SolidHhtPairedPredictor> PredictSolidHhtHighLow(
    const SolidDynamicAcceptedFrameState& old, double h,
    const HhtParameters& p) {
  using namespace math_internal;
  const auto size = old.displacement.Size();
  if (!ValidSolidHhtPair(old.displacement, old.low.displacement, size) ||
      !ValidSolidHhtPair(old.velocity, old.low.velocity, size) ||
      !ValidSolidHhtPair(old.algorithmic_acceleration,
                         old.low.algorithmic_acceleration, size) ||
      !ValidSolidHhtParameters(h, p))
    return Result<SolidHhtPairedPredictor>::Failure(
        SolidHhtPairedKinematicsFailure());
  SolidHhtPairedPredictor result{Vector(size), Vector(size), Vector(size),
                                 Vector(size)};
  const auto hh = Multiply(HighLow(h), HighLow(h));
  const auto u_coefficient =
      Multiply(hh, Subtract(HighLow(.5), HighLow(p.beta)));
  const auto v_coefficient =
      Multiply(HighLow(h), Subtract(HighLow(1), HighLow(p.gamma)));
  if (!IsFinite(u_coefficient) || !IsFinite(v_coefficient))
    return Result<SolidHhtPairedPredictor>::Failure(
        SolidHhtPairedKinematicsFailure());
  for (std::size_t i = 0; i < size; ++i) {
    const HighLow u(old.displacement[i], old.low.displacement[i]);
    const HighLow v(old.velocity[i], old.low.velocity[i]);
    const HighLow A(old.algorithmic_acceleration[i],
                    old.low.algorithmic_acceleration[i]);
    const auto up =
        Add(Add(u, Multiply(HighLow(h), v)), Multiply(u_coefficient, A));
    const auto vp = Add(v, Multiply(v_coefficient, A));
    if (!IsFinite(up) || !IsNormalized(up) || !IsFinite(vp) ||
        !IsNormalized(vp))
      return Result<SolidHhtPairedPredictor>::Failure(
          SolidHhtPairedKinematicsFailure());
    result.displacement[i] = up.high;
    result.displacement_low[i] = up.low;
    result.velocity[i] = vp.high;
    result.velocity_low[i] = vp.low;
  }
  return Result<SolidHhtPairedPredictor>::Success(std::move(result));
}

/// @brief Recovers A, v and physical a from the paired displacement difference.
inline Result<SolidHhtPairedRates> EvaluateSolidHhtHighLowRates(
    const Vector& high, const Vector& low,
    const SolidHhtPairedPredictor& predictor, const Vector& old_acceleration,
    const Vector& old_acceleration_low, double h, const HhtParameters& p) {
  using namespace math_internal;
  const auto size = high.Size();
  if (!ValidSolidHhtPair(high, low, size) ||
      !ValidSolidHhtPair(predictor.displacement, predictor.displacement_low,
                         size) ||
      !ValidSolidHhtPair(predictor.velocity, predictor.velocity_low, size) ||
      !ValidSolidHhtPair(old_acceleration, old_acceleration_low, size) ||
      !ValidSolidHhtParameters(h, p))
    return Result<SolidHhtPairedRates>::Failure(
        SolidHhtPairedKinematicsFailure());
  SolidHhtPairedRates result{Vector(size), Vector(size), Vector(size),
                             Vector(size), Vector(size), Vector(size)};
  const auto denominator =
      Multiply(Multiply(HighLow(p.beta), HighLow(h)), HighLow(h));
  const auto velocity_coefficient = Multiply(HighLow(p.gamma), HighLow(h));
  const auto physical_coefficient = Add(HighLow(1), HighLow(p.alpha));
  if (!IsFinite(denominator) || Compare(denominator, HighLow(0)) <= 0 ||
      !IsFinite(velocity_coefficient) || !IsFinite(physical_coefficient))
    return Result<SolidHhtPairedRates>::Failure(
        SolidHhtPairedKinematicsFailure());
  for (std::size_t i = 0; i < size; ++i) {
    const auto A = Divide(Subtract(HighLow(high[i], low[i]),
                                   HighLow(predictor.displacement[i],
                                           predictor.displacement_low[i])),
                          denominator);
    const auto v =
        Add(HighLow(predictor.velocity[i], predictor.velocity_low[i]),
            Multiply(velocity_coefficient, A));
    const auto a = Divide(
        Add(A, Multiply(HighLow(p.alpha),
                        HighLow(old_acceleration[i], old_acceleration_low[i]))),
        physical_coefficient);
    if (!IsFinite(A) || !IsNormalized(A) || !IsFinite(v) || !IsNormalized(v) ||
        !IsFinite(a) || !IsNormalized(a))
      return Result<SolidHhtPairedRates>::Failure(
          SolidHhtPairedKinematicsFailure());
    result.algorithmic_acceleration[i] = A.high;
    result.algorithmic_acceleration_low[i] = A.low;
    result.velocity[i] = v.high;
    result.velocity_low[i] = v.low;
    result.acceleration[i] = a.high;
    result.acceleration_low[i] = a.low;
  }
  return Result<SolidHhtPairedRates>::Success(std::move(result));
}
}  // namespace fesa::detail
#endif  // FESA_ANALYSIS_SOLID_HHT_HIGH_LOW_H_