#include "fesa/analysis/solid_hht_dynamic_procedure.h"

#include "equilibrium_algorithm.h"
#include "analysis/solid_hht_core.h"
#include "solid_hht_integrator.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <initializer_list>
#include <limits>
#include <memory>
#include <optional>
#include <string>
#include <utility>
#include <vector>

#include "fesa/analysis/hht_time_integration.h"
#include "fesa/assembly/load_assembler.h"
#include "fesa/assembly/parallel_for.h"
#include "fesa/constraints/essential_constraint_policy.h"
#include "fesa/elements/element_factory.h"
#include "fesa/elements/hex8_element.h"
#include "fesa/math/sparse_matrix.h"
#include "fesa/solvers/linear/linear_solver.h"
#include "fesa/solvers/linear/tangent_solver.h"
#include "solid_hht_force_identity.h"
#include "solid_hht_high_low.h"

namespace fesa {
namespace {
using math_internal::Abs;
using math_internal::Add;
using math_internal::Compare;
using math_internal::Divide;
using math_internal::HighLow;
using math_internal::IsFinite;
using math_internal::IsNormalized;
using math_internal::Multiply;
using math_internal::Negate;
using math_internal::Rounded;
using math_internal::Subtract;

HighLow Pair(const Vector& high, const Vector& low, std::size_t i) {
  return {high[i], low[i]};
}
void Store(HighLow value, double* high, double* low) {
  *high = value.high;
  *low = value.low;
}
void Store(HighLow value, Vector* high, Vector* low, std::size_t i) {
  Store(value, &(*high)[i], &(*low)[i]);
}
bool Valid(HighLow value) { return IsFinite(value) && IsNormalized(value); }
HighLow Maximum(HighLow left, HighLow right) {
  if (!Valid(left) || !Valid(right)) return math_internal::InvalidHighLow();
  return Compare(left, right) >= 0 ? left : right;
}
void InitializeLowVectors(SolidDynamicAcceptedFrameState* f, std::size_t size) {
  for (auto* v : {&f->low.displacement, &f->low.velocity, &f->low.acceleration,
                  &f->low.algorithmic_acceleration, &f->low.inertia_force,
                  &f->low.internal_force, &f->low.external_force,
                  &f->low.residual, &f->low.reaction, &f->low.residual_scale})
    *v = Vector(size);
}
struct PairedTangent {
  std::vector<HighLow> values;
};
/// @brief Reports source-context procedure failure without committing a trial.
Status Failure(const Domain& domain, const char* code,
               const std::string& message) {
  return Status::Failure(
      FailureCategory::kSolver,
      {{Severity::kError, code,
        domain.Dynamic() == nullptr ? SourceLocation{}
                                    : domain.Dynamic()->dynamic_location,
        "DYNAMIC",
        domain.Dynamic() == nullptr ? ""
                                    : domain.Dynamic()->source_step_name_raw,
        message}});
}
Status CheckArithmeticEnvironment(const Domain& domain) {
  return math_internal::ArithmeticEnvironmentSupported()
             ? Status::Ok()
             : Failure(domain, "solid-dynamic-arithmetic-environment",
                       "Paired HHT requires nearest rounding and gradual "
                       "underflow, including after backend callbacks.");
}
/// @brief Owns immutable kernels and the stable full-space translation scatter.
struct RuntimeSolid {
  std::unique_ptr<Element> owner;
  std::unique_ptr<SolidTrialElement> solid;
  std::vector<std::size_t> scatter;
};
/// @brief Creates checked kernels in declaration order, with no mutable element
/// state.
Result<std::vector<RuntimeSolid>> BuildSolids(const Domain& domain,
                                              const DofManager& dofs) {
  std::vector<RuntimeSolid> solids;
  ElementFactory factory;
  for (std::size_t e = 0; e < domain.Elements().Size(); ++e) {
    auto owner = factory.Create(domain.Elements()[e], domain);
    if (!owner.HasValue())
      return Result<std::vector<RuntimeSolid>>::Failure(owner.GetStatus());
    auto solid = factory.BindSolidTrial(*owner.Value());
    if (solid == nullptr)
      return Result<std::vector<RuntimeSolid>>::Failure(
          Failure(domain, "solid-dynamic-element-family",
                  "HHT requires exclusively Hex8 kernels."));
    auto scatter = dofs.ElementScatter(solid->DofLayout());
    if (!scatter.HasValue())
      return Result<std::vector<RuntimeSolid>>::Failure(scatter.GetStatus());
    solids.push_back(
        {std::move(owner.Value()), std::move(solid), std::move(scatter.Value())});
  }
  return Result<std::vector<RuntimeSolid>>::Success(std::move(solids));
}
/// @brief Preserves all 576 structural entries in stable element/local order.
void AppendMatrix(const Matrix& local, const RuntimeSolid& solid, std::size_t e,
                  std::vector<CooContribution>* coo) {
  for (std::size_t i = 0; i < 24; ++i)
    for (std::size_t j = 0; j < 24; ++j)
      coo->push_back(
          {solid.scatter[i], solid.scatter[j], local(i, j), e, 24 * i + j});
}
/// @brief Assembles constant reference row-sum lumped mass once.
Result<SparseMatrix> AssembleMass(const std::vector<RuntimeSolid>& solids,
                                  const DofManager& dofs) {
  std::vector<CooContribution> coo;
  coo.reserve(solids.size() * 576);
  for (std::size_t e = 0; e < solids.size(); ++e) {
    auto mass = solids[e].solid->ComputeMass();
    if (!mass.HasValue())
      return Result<SparseMatrix>::Failure(mass.GetStatus());
    AppendMatrix(mass.Value().values, solids[e], e, &coo);
  }
  return SparseMatrix::FromCoo(dofs.FullDofCount(), dofs.FullDofCount(),
                               std::move(coo), dofs.GetSparsePattern());
}
/// @brief Evaluates independent local trials, then deterministically reduces
/// recovery.
Result<PairedTangent> EvaluateSolids(const Domain& domain,
                                     const std::vector<RuntimeSolid>& solids,
                                     const DofManager& dofs,
                                     const ParallelFor& parallel,
                                     SolidDynamicAcceptedFrameState* f) {
  const auto environment = CheckArithmeticEnvironment(domain);
  if (!environment.IsOk()) return Result<PairedTangent>::Failure(environment);
  if (!detail::ValidSolidHhtPair(f->displacement, f->low.displacement,
                                 dofs.FullDofCount()))
    return Result<PairedTangent>::Failure(
        Failure(domain, "solid-dynamic-invalid-paired-state",
                "Local evaluation requires complete normalized pairs."));
  std::vector<std::optional<Result<Hex8HighLowResponse>>> responses(
      solids.size());
  parallel.Execute(solids.size(), [&](std::size_t e) {
    Vector local(24), low(24);
    for (std::size_t i = 0; i < 24; ++i) {
      local[i] = f->displacement[solids[e].scatter[i]];
      low[i] = f->low.displacement[solids[e].scatter[i]];
    }
    responses[e] = domain.Dynamic()->nlgeom
                       ? solids[e].solid->EvaluateNonlinearHighLow(local, low)
                       : solids[e].solid->EvaluateLinearHighLow(local, low);
  });
  const auto reduction_environment = CheckArithmeticEnvironment(domain);
  if (!reduction_environment.IsOk())
    return Result<PairedTangent>::Failure(reduction_environment);
  const auto& pattern = dofs.GetSparsePattern();
  PairedTangent tangent;
  tangent.values.resize(pattern.column_indices.size());
  f->internal_force = Vector(dofs.FullDofCount());
  f->low.internal_force = Vector(dofs.FullDofCount());
  f->strain_energy = 0;
  f->low.strain_energy = 0;
  f->solid_results.clear();
  for (std::size_t e = 0; e < solids.size(); ++e) {
    if (!responses[e])
      return Result<PairedTangent>::Failure(
          Failure(domain, "solid-dynamic-local-execution",
                  "Local worker did not evaluate its index."));
    if (!responses[e]->HasValue())
      return Result<PairedTangent>::Failure(responses[e]->GetStatus());
    const auto& r = responses[e]->Value();
    for (std::size_t i = 0; i < 24; ++i) {
      const auto row = solids[e].scatter[i];
      const auto force =
          Add(Pair(f->internal_force, f->low.internal_force, row),
              Pair(r.high.internal_force, r.internal_force_low, i));
      if (!Valid(force))
        return Result<PairedTangent>::Failure(
            Failure(domain, "solid-dynamic-nonfinite-assembly",
                    "Paired internal force reduction failed."));
      Store(force, &f->internal_force, &f->low.internal_force, row);
      for (std::size_t j = 0; j < 24; ++j) {
        const auto column = solids[e].scatter[j];
        const auto begin =
            pattern.column_indices.begin() + pattern.row_offsets[row];
        const auto end =
            pattern.column_indices.begin() + pattern.row_offsets[row + 1];
        const auto found = std::lower_bound(begin, end, column);
        if (found == end || *found != column)
          return Result<PairedTangent>::Failure(
              Failure(domain, "solid-dynamic-sparse-pattern",
                      "Local tangent is outside the fixed sparse pattern."));
        const auto k =
            static_cast<std::size_t>(found - pattern.column_indices.begin());
        tangent.values[k] =
            Add(tangent.values[k],
                HighLow(r.high.tangent(i, j), r.tangent_low(i, j)));
        if (!Valid(tangent.values[k]))
          return Result<PairedTangent>::Failure(
              Failure(domain, "solid-dynamic-nonfinite-assembly",
                      "Paired tangent reduction failed."));
      }
    }
    const auto energy = Add(HighLow(f->strain_energy, f->low.strain_energy),
                            HighLow(r.high.strain_energy, r.strain_energy_low));
    if (!Valid(energy))
      return Result<PairedTangent>::Failure(
          Failure(domain, "solid-dynamic-nonfinite-assembly",
                  "Paired strain energy reduction failed."));
    Store(energy, &f->strain_energy, &f->low.strain_energy);
    f->solid_results.insert(f->solid_results.end(), r.high.points.begin(),
                            r.high.points.end());
  }
  return Result<PairedTangent>::Success(std::move(tangent));
}
/// @brief Returns the finite positive reference nodal bounding-box diagonal.
Result<double> ReferenceLength(const Domain& domain) {
  auto lo = domain.Nodes().front().coordinates;
  auto hi = lo;
  for (const auto& node : domain.Nodes())
    for (std::size_t c = 0; c < 3; ++c) {
      lo[c] = (std::min)(lo[c], node.coordinates[c]);
      hi[c] = (std::max)(hi[c], node.coordinates[c]);
    }
  const double length =
      std::hypot(std::hypot(hi[0] - lo[0], hi[1] - lo[1]), hi[2] - lo[2]);
  if (!std::isfinite(length) || length <= 0)
    return Result<double>::Failure(
        Failure(domain, "solid-dynamic-reference-length",
                "Reference length is not finite positive."));
  return Result<double>::Success(length);
}
/// @brief Forms M/(beta*h^2)+(1+alpha)*Kt in the unchanged full sparse pattern.
Result<SparseMatrix> EffectiveTangent(const SparseMatrix& m,
                                      const PairedTangent& k,
                                      const DofManager& dofs, double h,
                                      const HhtParameters& p) {
  std::vector<CooContribution> coo;
  coo.reserve(m.Values().size());
  const auto denominator =
      Multiply(Multiply(HighLow(p.beta), HighLow(h)), HighLow(h));
  const auto coefficient = Add(HighLow(1), HighLow(p.alpha));
  for (std::size_t i = 0; i < m.Rows(); ++i)
    for (std::size_t j = m.RowOffsets()[i]; j < m.RowOffsets()[i + 1]; ++j) {
      const auto value = Add(Divide(HighLow(m.Values()[j]), denominator),
                             Multiply(coefficient, k.values[j]));
      if (!Valid(value))
        return Result<SparseMatrix>::Failure(
            detail::SolidHhtPairedKinematicsFailure());
      coo.push_back({i, m.ColumnIndices()[j], Rounded(value), 0, j});
    }
  return SparseMatrix::FromCoo(m.Rows(), m.Columns(), std::move(coo),
                               dofs.GetSparsePattern());
}

using Paired3 = std::array<HighLow, 3>;
void CrossAdd(const Paired3& x, const Paired3& force, Paired3* sum) {
  for (std::size_t c = 0; c < 3; ++c) {
    const auto j = (c + 1) % 3;
    const auto k = (c + 2) % 3;
    (*sum)[c] = Add((*sum)[c], Subtract(Multiply(x[j], force[k]),
                                        Multiply(x[k], force[j])));
  }
}
bool Store3(const Paired3& values, std::array<double, 3>* high,
            std::array<double, 3>* low) {
  for (std::size_t c = 0; c < 3; ++c) {
    if (!Valid(values[c])) return false;
    Store(values[c], &(*high)[c], &(*low)[c]);
  }
  return true;
}

/// @brief Recovers full physical endpoint quantities before any publication or
/// commit. The constant Float64 mass coefficients are lifted exactly once.
Status RecoverEndpoint(const Domain& domain, const DofManager& dofs,
                       const SparseMatrix& mass,
                       const SolidDynamicAcceptedFrameState* old,
                       HighLow initial_energy,
                       SolidDynamicAcceptedFrameState* f,
                       SolidDynamicIterationEvidence* e) {
  auto inertia = detail::MultiplySolidHhtMassHighLow(mass, f->acceleration,
                                                     f->low.acceleration);
  auto momentum =
      detail::MultiplySolidHhtMassHighLow(mass, f->velocity, f->low.velocity);
  if (!inertia.HasValue()) return inertia.GetStatus();
  if (!momentum.HasValue()) return momentum.GetStatus();
  f->inertia_force = std::move(inertia.Value().high);
  f->low.inertia_force = std::move(inertia.Value().low);
  const auto size = dofs.FullDofCount();
  for (auto* v : {&f->residual, &f->low.residual, &f->reaction,
                  &f->low.reaction, &f->residual_scale, &f->low.residual_scale})
    *v = Vector(size);
  HighLow numerator, scale_maximum, metric;
  for (std::size_t i = 0; i < size; ++i) {
    const auto ma = Pair(f->inertia_force, f->low.inertia_force, i);
    const auto internal = Pair(f->internal_force, f->low.internal_force, i);
    const auto external = Pair(f->external_force, f->low.external_force, i);
    const auto residual = Subtract(Add(ma, internal), external);
    const auto scale = Add(Add(Abs(ma), Abs(internal)), Abs(external));
    if (!Valid(residual) || !Valid(scale))
      return Failure(domain, "solid-dynamic-nonfinite-residual",
                     "Physical paired force arithmetic failed.");
    Store(residual, &f->residual, &f->low.residual, i);
    Store(scale, &f->residual_scale, &f->low.residual_scale, i);
  }
  for (auto i : dofs.ConstrainedDofs()) {
    f->reaction[i] = f->residual[i];
    f->low.reaction[i] = f->low.residual[i];
  }
  for (auto i : dofs.FreeDofs()) {
    const auto residual = Pair(f->residual, f->low.residual, i);
    const auto scale = Pair(f->residual_scale, f->low.residual_scale, i);
    numerator = Maximum(numerator, Abs(residual));
    scale_maximum = Maximum(scale_maximum, scale);
    if (Compare(scale, HighLow(0)) == 0) {
      e->zero_scale_exact =
          e->zero_scale_exact && Compare(residual, HighLow(0)) == 0;
    } else {
      const auto ratio = Divide(Abs(residual), scale);
      if (!Valid(ratio))
        return Failure(domain, "solid-dynamic-nonfinite-residual",
                       "Physical paired force ratio is unsupported.");
      if (Compare(ratio, metric) > 0) {
        metric = ratio;
        e->worst_full_dof = i;
        e->worst_force_terms = {
            Rounded(residual),
            Rounded(Pair(f->inertia_force, f->low.inertia_force, i)),
            Rounded(Pair(f->internal_force, f->low.internal_force, i)),
            Rounded(Pair(f->external_force, f->low.external_force, i)),
            Rounded(scale)};
      }
    }
  }
  if (!Valid(numerator) || !Valid(scale_maximum) || !Valid(metric))
    return Failure(domain, "solid-dynamic-nonfinite-residual",
                   "Physical paired evidence is nonfinite.");
  e->residual_numerator = Rounded(numerator);
  e->residual_scale = Rounded(scale_maximum);
  e->residual_metric = Rounded(metric);
  e->residual_pass =
      e->zero_scale_exact && Compare(metric, HighLow(1e-10)) <= 0;

  HighLow kinetic;
  HighLow work = old == nullptr
                     ? HighLow(0)
                     : HighLow(old->external_work, old->low.external_work);
  for (std::size_t i = 0; i < size; ++i) {
    kinetic =
        Add(kinetic,
            Multiply(Pair(f->velocity, f->low.velocity, i),
                     Pair(momentum.Value().high, momentum.Value().low, i)));
    if (old != nullptr) {
      const auto average =
          Multiply(HighLow(.5),
                   Add(Pair(old->external_force, old->low.external_force, i),
                       Pair(f->external_force, f->low.external_force, i)));
      const auto delta =
          Subtract(Pair(f->displacement, f->low.displacement, i),
                   Pair(old->displacement, old->low.displacement, i));
      work = Add(work, Multiply(average, delta));
    }
  }
  kinetic = Multiply(HighLow(.5), kinetic);
  const auto error = Subtract(
      Subtract(Add(kinetic, HighLow(f->strain_energy, f->low.strain_energy)),
               initial_energy),
      work);
  if (!Valid(kinetic) || !Valid(work) || !Valid(error))
    return Failure(domain, "solid-dynamic-nonfinite-energy",
                   "Paired energy or endpoint work is unsupported.");
  Store(kinetic, &f->kinetic_energy, &f->low.kinetic_energy);
  Store(work, &f->external_work, &f->low.external_work);
  Store(error, &f->energy_balance_error, &f->low.energy_balance_error);

  Paired3 linear{}, angular{}, linear_rate{}, angular_rate{}, applied_moment{},
      linear_equilibrium{}, current_internal_moment{}, reference_moment{},
      free_residual_moment{};
  for (std::size_t n = 0; n < domain.Nodes().size(); ++n) {
    Paired3 x{}, reference{}, mv{}, ma{}, applied{}, internal{},
        free_residual{}, u{};
    for (std::size_t c = 0; c < 3; ++c) {
      const auto i = 3 * n + c;
      reference[c] = HighLow(domain.Nodes()[n].coordinates[c]);
      u[c] = Pair(f->displacement, f->low.displacement, i);
      x[c] = Add(reference[c], u[c]);
      mv[c] = Pair(momentum.Value().high, momentum.Value().low, i);
      ma[c] = Pair(f->inertia_force, f->low.inertia_force, i);
      internal[c] = Pair(f->internal_force, f->low.internal_force, i);
      applied[c] = Add(Pair(f->external_force, f->low.external_force, i),
                       Pair(f->reaction, f->low.reaction, i));
      free_residual[c] = Subtract(Pair(f->residual, f->low.residual, i),
                                  Pair(f->reaction, f->low.reaction, i));
      linear[c] = Add(linear[c], mv[c]);
      linear_rate[c] = Add(linear_rate[c], ma[c]);
      linear_equilibrium[c] =
          Add(linear_equilibrium[c], Subtract(applied[c], ma[c]));
    }
    CrossAdd(x, mv, &angular);
    CrossAdd(x, ma, &angular_rate);
    CrossAdd(x, applied, &applied_moment);
    CrossAdd(u, internal, &current_internal_moment);
    CrossAdd(reference, internal, &reference_moment);
    CrossAdd(x, free_residual, &free_residual_moment);
  }
  if (!Store3(linear, &f->linear_momentum, &f->low.linear_momentum) ||
      !Store3(angular, &f->angular_momentum, &f->low.angular_momentum) ||
      !Store3(linear_rate, &f->linear_momentum_rate,
              &f->low.linear_momentum_rate) ||
      !Store3(angular_rate, &f->angular_momentum_rate,
              &f->low.angular_momentum_rate) ||
      !Store3(current_internal_moment,
              &f->linearization_current_internal_moment,
              &f->low.linearization_current_internal_moment) ||
      !Store3(reference_moment, &f->reference_internal_moment,
              &f->low.reference_internal_moment) ||
      !Store3(free_residual_moment, &f->current_free_residual_moment,
              &f->low.current_free_residual_moment))
    return Failure(
        domain, "solid-dynamic-nonfinite-momentum",
        "Paired momentum or current/reference moment is unsupported.");
  for (std::size_t c = 0; c < 3; ++c) {
    const auto moment = Subtract(applied_moment[c], angular_rate[c]);
    if (!Valid(linear_equilibrium[c]) || !Valid(moment))
      return Failure(domain, "solid-dynamic-nonfinite-equilibrium",
                     "Paired global equilibrium is unsupported.");
    Store(linear_equilibrium[c], &f->global_equilibrium[c],
          &f->low.global_equilibrium[c]);
    Store(moment, &f->global_equilibrium[3 + c],
          &f->low.global_equilibrium[3 + c]);
  }
  return Status::Ok();
}

/// @brief Uses full paired same-unit uncancelled formula terms.
HighLow UpdateMetric(HighLow left, std::initializer_list<HighLow> terms) {
  HighLow sum;
  auto scale = Abs(left);
  for (const auto x : terms) {
    sum = Add(sum, x);
    scale = Add(scale, Abs(x));
  }
  const auto defect = Abs(Subtract(left, sum));
  if (!Valid(defect) || !Valid(scale)) return math_internal::InvalidHighLow();
  if (Compare(scale, HighLow(0)) == 0)
    return Compare(defect, HighLow(0)) == 0 ? HighLow(0)
                                            : math_internal::InvalidHighLow();
  return Divide(defect, scale);
}
HighLow CheckUpdates(const SolidDynamicAcceptedFrameState& old,
                     const SolidDynamicAcceptedFrameState& f,
                     const HhtParameters& p, double h) {
  HighLow metric;
  const auto step = HighLow(h);
  const auto hh = Multiply(step, step);
  for (std::size_t i = 0; i < f.displacement.Size(); ++i) {
    const auto A =
        Pair(f.algorithmic_acceleration, f.low.algorithmic_acceleration, i);
    const auto oldA =
        Pair(old.algorithmic_acceleration, old.low.algorithmic_acceleration, i);
    const auto oldv = Pair(old.velocity, old.low.velocity, i);
    metric = Maximum(
        metric,
        UpdateMetric(
            Pair(f.displacement, f.low.displacement, i),
            {Pair(old.displacement, old.low.displacement, i),
             Multiply(step, oldv),
             Multiply(Multiply(hh, Subtract(HighLow(.5), HighLow(p.beta))),
                      oldA),
             Multiply(Multiply(hh, HighLow(p.beta)), A)}));
    metric = Maximum(
        metric,
        UpdateMetric(
            Pair(f.velocity, f.low.velocity, i),
            {oldv,
             Multiply(Multiply(step, Subtract(HighLow(1), HighLow(p.gamma))),
                      oldA),
             Multiply(Multiply(step, HighLow(p.gamma)), A)}));
    metric = Maximum(
        metric,
        UpdateMetric(A, {Multiply(Add(HighLow(1), HighLow(p.alpha)),
                                  Pair(f.acceleration, f.low.acceleration, i)),
                         Negate(Multiply(HighLow(p.alpha),
                                         Pair(old.acceleration,
                                              old.low.acceleration, i)))}));
  }
  return metric;
}
std::array<HighLow, 4> FreshCorrectionMetrics(const Vector& du, double length,
                                              const HhtParameters& p) {
  HighLow q;
  for (std::size_t i = 0; i < du.Size(); ++i)
    q = Maximum(q, Divide(Abs(HighLow(du[i])), HighLow(length)));
  return {
      q, Divide(Multiply(HighLow(p.gamma), q), HighLow(p.beta)),
      Divide(q, Multiply(Add(HighLow(1), HighLow(p.alpha)), HighLow(p.beta))),
      Divide(q, HighLow(p.beta))};
}
std::array<HighLow, 4> ObservedCorrectionMetrics(
    const SolidDynamicAcceptedFrameState& before,
    const SolidDynamicAcceptedFrameState& after, double h, double length) {
  std::array<HighLow, 4> metric{};
  const auto hh = Multiply(HighLow(h), HighLow(h));
  for (std::size_t i = 0; i < after.displacement.Size(); ++i) {
    metric[0] = Maximum(
        metric[0],
        Divide(Abs(Subtract(
                   Pair(after.displacement, after.low.displacement, i),
                   Pair(before.displacement, before.low.displacement, i))),
               HighLow(length)));
    metric[1] = Maximum(
        metric[1],
        Divide(Multiply(HighLow(h),
                        Abs(Subtract(
                            Pair(after.velocity, after.low.velocity, i),
                            Pair(before.velocity, before.low.velocity, i)))),
               HighLow(length)));
    metric[2] = Maximum(
        metric[2],
        Divide(Multiply(hh, Abs(Subtract(Pair(after.acceleration,
                                              after.low.acceleration, i),
                                         Pair(before.acceleration,
                                              before.low.acceleration, i)))),
               HighLow(length)));
    metric[3] = Maximum(
        metric[3],
        Divide(Multiply(hh, Abs(Subtract(
                                Pair(after.algorithmic_acceleration,
                                     after.low.algorithmic_acceleration, i),
                                Pair(before.algorithmic_acceleration,
                                     before.low.algorithmic_acceleration, i)))),
               HighLow(length)));
  }
  return metric;
}
bool CorrectionPass(const std::array<HighLow, 4>& metrics) {
  return std::all_of(metrics.begin(), metrics.end(), [](HighLow x) {
    return Valid(x) && Compare(x, HighLow(0)) >= 0 &&
           Compare(x, HighLow(1e-10)) <= 0;
  });
}
}  // namespace

SolidHhtDynamicProcedureExecutor::SolidHhtDynamicProcedureExecutor(
    const ParallelFor& parallel_for, LinearSolver& mass_solver,
    TangentSolver& tangent_solver)
    : parallel_for_(parallel_for),
      mass_solver_(mass_solver),
      tangent_solver_(tangent_solver) {}

Result<SolidDynamicExecutionResult> SolidHhtDynamicProcedureExecutor::Execute(
    const Domain& domain) const {
  auto model = AnalysisModel::Create(domain);
  if (!model.HasValue())
    return Result<SolidDynamicExecutionResult>::Failure(model.GetStatus());
  auto dofs = DofManager::Create(model.Value());
  if (!dofs.HasValue())
    return Result<SolidDynamicExecutionResult>::Failure(dofs.GetStatus());
  auto state = SolidDynamicAnalysisState::Create(domain, dofs.Value());
  if (!state.HasValue())
    return Result<SolidDynamicExecutionResult>::Failure(state.GetStatus());
  auto status = ExecuteInto(domain, state.Value());
  if (!status.IsOk())
    return Result<SolidDynamicExecutionResult>::Failure(status);
  return Result<SolidDynamicExecutionResult>::Success(
      {std::move(dofs.Value()), std::move(state.Value())});
}
Status SolidHhtDynamicProcedureExecutor::ExecuteInto(
    const Domain& domain, SolidDynamicAnalysisState& state) const {
  return ExecuteIntoForVerification(domain, state, -0.05);
}
Status SolidHhtDynamicProcedureExecutor::ExecuteIntoForVerification(
    const Domain& domain, SolidDynamicAnalysisState& state,
    double alpha) const {
  const detail::PairedSolidHhtIntegrator integrator;
  NewtonStepAlgorithm algorithm;
  return ExecuteSolidHhtCore(domain, state, alpha, parallel_for_, mass_solver_,
                              tangent_solver_, integrator, algorithm);
}

Status ExecuteSolidHhtCore(
    const Domain& domain, SolidDynamicAnalysisState& state, double alpha,
    const ParallelFor& parallel_for, LinearSolver& mass_solver,
    TangentSolver& tangent_solver, const detail::SolidHhtIntegrator& integrator,
    EquilibriumAlgorithm& algorithm) {
  SolidDynamicIterationRecord record;
  auto fail = [&](const Status& failure) {
    record.stage = "failed";
    record.outcome = "failed";
    record.failure_code = failure.Diagnostics().empty()
                              ? "solid-dynamic-failure"
                              : failure.Diagnostics().front().code;
    state.RecordIteration(record);
    return failure;
  };
  if (!state.IsInitialized() || !state.AcceptedFrames().empty() ||
      !domain.IsSolid() || domain.Dynamic() == nullptr)
    return fail(Failure(
        domain, "solid-dynamic-invalid-execution",
        "Execution requires an initialized empty Solid dynamic state."));
  auto environment = CheckArithmeticEnvironment(domain);
  if (!environment.IsOk()) return fail(environment);
  auto model = AnalysisModel::Create(domain);
  if (!model.HasValue()) return fail(model.GetStatus());
  auto dofs_result = DofManager::Create(model.Value());
  if (!dofs_result.HasValue()) return fail(dofs_result.GetStatus());
  const auto& dofs = dofs_result.Value();
  auto solids = BuildSolids(domain, dofs);
  if (!solids.HasValue()) return fail(solids.GetStatus());
  auto mass = AssembleMass(solids.Value(), dofs);
  if (!mass.HasValue()) return fail(mass.GetStatus());
  EssentialConstraintPolicy partition;
  auto mass_partition = partition.Partition(mass.Value(), dofs);
  if (!mass_partition.HasValue()) return fail(mass_partition.GetStatus());
  if (dofs.FreeDofCount() != 0) {
    const auto status = mass_solver.Factorize(mass_partition.Value().k_ff);
    if (!status.IsOk()) return fail(status);
    environment = CheckArithmeticEnvironment(domain);
    if (!environment.IsOk()) return fail(environment);
  }
  auto parameters = MakeHhtParameters(alpha);
  if (!parameters.HasValue()) return fail(parameters.GetStatus());
  const auto p = parameters.Value();
  auto length = ReferenceLength(domain);
  if (!length.HasValue()) return fail(length.GetStatus());
  const double h = domain.Dynamic()->initial_increment;
  SolidDynamicAcceptedFrameState initial;
  initial.is_initial = true;
  for (std::size_t n = 0; n < domain.Nodes().size(); ++n) {
    initial.nodal_internal_order.push_back(static_cast<EntityIndex>(n));
    initial.nodal_source_ids.push_back(domain.Nodes()[n].source_id);
  }
  for (auto* v : {&initial.displacement, &initial.velocity,
                  &initial.acceleration, &initial.algorithmic_acceleration})
    *v = Vector(dofs.FullDofCount());
  InitializeLowVectors(&initial, dofs.FullDofCount());
  auto force = LoadAssembler::AssembleFullNodalLoad(model.Value(), dofs, 0);
  if (!force.HasValue()) return fail(force.GetStatus());
  initial.external_force = std::move(force.Value());
  auto tangent =
      EvaluateSolids(domain, solids.Value(), dofs, parallel_for, &initial);
  if (!tangent.HasValue()) return fail(tangent.GetStatus());
  SolidDynamicIterationEvidence evidence;
  bool initial_converged = false;
  for (std::uint32_t refinement = 0; refinement <= 100; ++refinement) {
    initial.algorithmic_acceleration = initial.acceleration;
    initial.low.algorithmic_acceleration = initial.low.acceleration;
    evidence = {};
    evidence.correction_pass = evidence.update_pass = true;
    auto status = RecoverEndpoint(domain, dofs, mass.Value(), nullptr,
                                  HighLow(0), &initial, &evidence);
    if (!status.IsOk()) return fail(status);
    if (evidence.residual_pass) {
      initial_converged = true;
      break;
    }
    record.stage = "initial-mass";
    record.outcome = "residual-refinement";
    record.evidence = evidence;
    record.evidence.iteration_index = refinement;
    state.RecordIteration(record);
    if (refinement == 100) break;
    Vector rhs(dofs.FreeDofCount()), a(dofs.FreeDofCount());
    for (std::size_t j = 0; j < dofs.FreeDofCount(); ++j) {
      const auto i = dofs.FreeDofs()[j];
      rhs[j] = Rounded(Negate(Pair(initial.residual, initial.low.residual, i)));
    }
    status = mass_solver.Solve(rhs, a);
    if (!status.IsOk()) return fail(status);
    environment = CheckArithmeticEnvironment(domain);
    if (!environment.IsOk()) return fail(environment);
    if (a.Size() != dofs.FreeDofCount())
      return fail(
          Failure(domain, "solid-dynamic-invalid-backend-result",
                  "Initial mass solve returned incompatible acceleration."));
    for (std::size_t j = 0; j < a.Size(); ++j) {
      if (!std::isfinite(a[j]))
        return fail(
            Failure(domain, "solid-dynamic-invalid-backend-result",
                    "Initial mass solve returned nonfinite acceleration."));
      const auto i = dofs.FreeDofs()[j];
      const auto value =
          Add(Pair(initial.acceleration, initial.low.acceleration, i),
              HighLow(a[j] == 0 ? 0.0 : a[j]));
      if (!Valid(value)) return fail(detail::SolidHhtPairedKinematicsFailure());
      Store(value, &initial.acceleration, &initial.low.acceleration, i);
    }
  }
  if (!initial_converged)
    return fail(Failure(domain, "solid-dynamic-initial-mass-nonconvergence",
                        "Physical initial equilibrium did not converge in100 "
                        "mass refinements."));
  initial.iterations.push_back(evidence);
  auto status = state.CommitAcceptedFrame(initial);
  if (!status.IsOk()) {
    record.evidence = evidence;
    return fail(status);
  }
  record.stage = "accepted";
  record.outcome = "initial";
  record.evidence = evidence;
  state.RecordIteration(record);
  const auto initial_energy =
      Add(HighLow(initial.strain_energy, initial.low.strain_energy),
          HighLow(initial.kinetic_energy, initial.low.kinetic_energy));
  for (std::uint32_t increment = 1;
       increment <= domain.Dynamic()->increment_count; ++increment) {
    const auto& old = state.CommittedState();
    record = {};
    record.increment_index = increment;
    record.time = static_cast<double>(increment) * h;
    status = detail::RunSolidHhtStep(
        integrator, old, h, p, fail,
        [&](const detail::SolidHhtIntegrator& step_integrator,
            const detail::SolidHhtPairedPredictor& predictor)
            -> Result<SolidDynamicAcceptedFrameState> {
          auto candidate = old;
          candidate.frame_index = candidate.increment_index = increment;
          candidate.iteration_count = 0;
          candidate.time = record.time;
          candidate.is_initial = false;
          candidate.iterations.clear();
          // Begin from certified accepted geometry, not an unchecked predictor.
          force = LoadAssembler::AssembleFullNodalLoad(model.Value(), dofs,
                                                       candidate.time);
          if (!force.HasValue())
            return Result<SolidDynamicAcceptedFrameState>::Failure(fail(force.GetStatus()));
          candidate.external_force = std::move(force.Value());
          candidate.low.external_force = Vector(dofs.FullDofCount());
          std::array<HighLow, 4> correction{}, fresh_correction{};
          using StepResult = Result<std::optional<SolidDynamicAcceptedFrameState>>;
          const auto report_failure = fail;
          const auto record_iteration = [&](const auto& value) {
            state.RecordIteration(value);
          };
          SolidHhtStep<SolidDynamicAcceptedFrameState> iteration_step(
              [&](std::uint32_t iteration) -> StepResult {
                const auto fail = [&](const Status& failure) {
                  return StepResult::Failure(report_failure(failure));
                };
                record.stage = "evaluation";
                record.outcome = "paired-candidate";
                record.evidence = {};
                record.evidence.iteration_index = iteration;
                status =
                    detail::SetSolidHhtRates(step_integrator, old, predictor, h, p, &candidate);
                if (!status.IsOk())
                  return fail(status);
                tangent = EvaluateSolids(domain, solids.Value(), dofs, parallel_for,
                                         &candidate);
                if (!tangent.HasValue())
                  return fail(tangent.GetStatus());
                status =
                    RecoverEndpoint(domain, dofs, mass.Value(), &old, initial_energy,
                                    &candidate, &record.evidence);
                if (!status.IsOk())
                  return fail(status);
                auto MA = detail::MultiplySolidHhtMassHighLow(
                    mass.Value(), candidate.algorithmic_acceleration,
                    candidate.low.algorithmic_acceleration);
                if (!MA.HasValue())
                  return fail(MA.GetStatus());
                Vector rhs(dofs.FreeDofCount());
                HighLow identity_metric, original_norm, solved_norm, old_norm;
                for (std::size_t j = 0; j < dofs.FreeDofCount(); ++j) {
                  const auto i = dofs.FreeDofs()[j];
                  detail::SolidHhtPairedForceIdentity identity;
                  if (!detail::EvaluateSolidHhtHighLowForceIdentity(
                          mass.Value(), i, Pair(MA.Value().high, MA.Value().low, i),
                          candidate, old, p.alpha, &identity))
                    return fail(
                        Failure(domain, "solid-dynamic-nonfinite-residual",
                                "Paired weighted force arithmetic is unsupported."));
                  rhs[j] = Rounded(Negate(identity.solved));
                  identity_metric = Maximum(identity_metric, identity.metric);
                  original_norm = Maximum(original_norm, Abs(identity.original));
                  solved_norm = Maximum(solved_norm, Abs(identity.solved));
                  old_norm =
                      Maximum(old_norm, Abs(Pair(old.residual, old.low.residual, i)));
                  if (i == record.evidence.worst_full_dof)
                    record.evidence.worst_weighted_terms = {
                        Rounded(identity.original), Rounded(identity.solved),
                        Rounded(Pair(old.residual, old.low.residual, i)),
                        Rounded(identity.scale)};
                }
                if (!Valid(identity_metric) || !Valid(original_norm) ||
                    !Valid(solved_norm) || !Valid(old_norm) ||
                    Compare(identity_metric, HighLow(1e-12)) > 0)
                  return fail(
                      Failure(domain, "solid-dynamic-weighted-algebra-inconsistency",
                              "Paired original and solved forces disagree beyond "
                              "their uncancelled scale."));
                record.evidence.weighted_equivalence_metric =
                    Rounded(identity_metric);
                record.evidence.weighted_residual_numerator = Rounded(original_norm);
                record.evidence.recombined_residual_numerator = Rounded(solved_norm);
                record.evidence.old_residual_numerator = Rounded(old_norm);
                for (std::size_t c = 0; c < 4; ++c) {
                  if (!Valid(correction[c]) || !Valid(fresh_correction[c]))
                    return fail(detail::SolidHhtPairedKinematicsFailure());
                  record.evidence.correction_metrics[c] = Rounded(correction[c]);
                }
                record.evidence.correction_pass =
                    CorrectionPass(correction) && CorrectionPass(fresh_correction);
                const auto update = CheckUpdates(old, candidate, p, h);
                if (!Valid(update))
                  return fail(detail::SolidHhtPairedKinematicsFailure());
                record.evidence.update_metric = Rounded(update);
                record.evidence.update_pass = Compare(update, HighLow(1e-12)) <= 0;
                candidate.iterations.push_back(record.evidence);
                candidate.iteration_count = iteration;
                if (record.evidence.residual_pass &&
                    record.evidence.correction_pass && record.evidence.update_pass) {
                  return StepResult::Success(candidate);
                }
                record_iteration(record);
                if (iteration == 100)
                  return fail(Failure(
                      domain, "solid-dynamic-nonconvergence",
                      "Paired HHT endpoint did not converge in100 Newton solves."));
                auto effective =
                    EffectiveTangent(mass.Value(), tangent.Value(), dofs, h, p);
                if (!effective.HasValue())
                  return fail(effective.GetStatus());
                auto reduced = partition.Partition(effective.Value(), dofs);
                if (!reduced.HasValue())
                  return fail(reduced.GetStatus());
                Vector du(dofs.FreeDofCount());
                if (dofs.FreeDofCount() != 0) {
                  status = tangent_solver.FactorizeFresh(reduced.Value().k_ff);
                  if (!status.IsOk())
                    return fail(status);
                  environment = CheckArithmeticEnvironment(domain);
                  if (!environment.IsOk())
                    return fail(environment);
                  status = tangent_solver.SolveCorrection(rhs, du);
                  if (!status.IsOk())
                    return fail(status);
                  environment = CheckArithmeticEnvironment(domain);
                  if (!environment.IsOk())
                    return fail(environment);
                }
                if (du.Size() != dofs.FreeDofCount())
                  return fail(
                      Failure(domain, "solid-dynamic-invalid-backend-result",
                              "Tangent solve returned incompatible correction."));
                for (std::size_t j = 0; j < du.Size(); ++j)
                  if (!std::isfinite(du[j]))
                    return fail(
                        Failure(domain, "solid-dynamic-invalid-backend-result",
                                "Tangent solve returned nonfinite correction."));
                fresh_correction = FreshCorrectionMetrics(du, length.Value(), p);
                const auto before = candidate;
                for (std::size_t j = 0; j < du.Size(); ++j) {
                  const auto i = dofs.FreeDofs()[j];
                  const auto value =
                      Add(Pair(candidate.displacement, candidate.low.displacement, i),
                          HighLow(du[j] == 0 ? 0.0 : du[j]));
                  if (!Valid(value))
                    return fail(detail::SolidHhtPairedKinematicsFailure());
                  Store(value, &candidate.displacement, &candidate.low.displacement,
                        i);
                }
                status =
                    detail::SetSolidHhtRates(step_integrator, old, predictor, h, p, &candidate);
                if (!status.IsOk())
                  return fail(status);
                correction =
                    ObservedCorrectionMetrics(before, candidate, h, length.Value());
                return StepResult::Success(std::nullopt);
              },
              100U);
          const Status solved = iteration_step.Solve(algorithm);
          if (!solved.IsOk())
            return Result<SolidDynamicAcceptedFrameState>::Failure(solved);
          if (!iteration_step.HasCandidate())
            return Result<SolidDynamicAcceptedFrameState>::Failure(
                fail(Failure(domain, "solid-dynamic-nonconvergence",
                             "No complete paired endpoint was accepted.")));
          return Result<SolidDynamicAcceptedFrameState>::Success(
              iteration_step.TakeCandidate());
        },
        [&](SolidDynamicAcceptedFrameState&& accepted_candidate) -> Status {
          status = state.CommitAcceptedFrame(accepted_candidate);
          if (!status.IsOk())
            return fail(status);
          record.stage = "accepted";
          record.outcome = "increment";
          state.RecordIteration(record);
          return Status::Ok();
        });
    if (!status.IsOk()) return status;
  }
  return Status::Ok();
}
} // namespace fesa