#include "fesa/analysis/solid_dynamic_analysis_state.h"

#include <algorithm>
#include <cmath>
#include <stdexcept>
#include <string>

#include "fesa/elements/element_factory.h"
#include "fesa/elements/hex8_element.h"
#include "math/high_low_arithmetic.h"

namespace fesa {
namespace {
/// @brief Rejects a candidate without changing observable history.
Status Failure(const std::string& message) {
  return Status::Failure(FailureCategory::kSolver,
                         {{Severity::kError,
                           "solid-dynamic-invalid-state",
                           {},
                           "DYNAMIC",
                           {},
                           message}});
}
/// @brief Checks a physical record without replacing nonfinite quantities.
template <std::size_t N>
bool Finite(const std::array<double, N>& values) {
  return std::all_of(values.begin(), values.end(),
                     [](double x) { return std::isfinite(x); });
}
bool ValidPair(double high, double low) {
  const math_internal::HighLow value(high, low);
  return math_internal::IsFinite(value) && math_internal::IsNormalized(value);
}
bool ValidPair(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)
    if (!ValidPair(high[i], low[i])) return false;
  return true;
}
template <std::size_t N>
bool ValidPair(const std::array<double, N>& high,
               const std::array<double, N>& low) {
  for (std::size_t i = 0; i < N; ++i)
    if (!ValidPair(high[i], low[i])) return false;
  return true;
}
/// @brief Preserves canonical and raw source identity independently of indices.
bool SameId(const SourceEntityId& a, const SourceEntityId& b) {
  return a.instance_name == b.instance_name &&
         a.source_label == b.source_label &&
         a.source_label_text == b.source_label_text;
}
}  // namespace

Result<SolidDynamicAnalysisState> SolidDynamicAnalysisState::Create(
    const Domain& domain, const DofManager& dofs) {
  const auto* step = domain.Dynamic();
  if (!domain.IsSolid() || step == nullptr ||
      dofs.Profile() != DofProfile::kSolidStructural ||
      dofs.FullDofCount() != 3 * domain.Nodes().size() ||
      !dofs.ValidateInvariants().IsOk() || step->increment_count == 0 ||
      !std::isfinite(step->initial_increment) || step->initial_increment <= 0 ||
      !std::isfinite(step->time_period) ||
      static_cast<double>(step->increment_count) * step->initial_increment !=
          step->time_period ||
      step->increment_count > step->maximum_increments) {
    return Result<SolidDynamicAnalysisState>::Failure(
        Failure("Solid HHT requires a valid 3N exact fixed schedule."));
  }
  for (std::size_t i = 0; i < dofs.PrescribedValues().Size(); ++i)
    if (dofs.PrescribedValues()[i] != 0)
      return Result<SolidDynamicAnalysisState>::Failure(
          Failure("Dynamic prescribed translations must be exactly zero."));
  SolidDynamicAnalysisState state;
  state.full_dof_count_ = dofs.FullDofCount();
  state.increment_count_ = step->increment_count;
  state.time_increment_ = step->initial_increment;
  state.constrained_dofs_ = dofs.ConstrainedDofs();
  for (const auto& node : domain.Nodes())
    state.source_ids_.push_back(node.source_id);
  ElementFactory factory;
  for (std::size_t e = 0; e < domain.Elements().Size(); ++e) {
    auto runtime = factory.Create(domain.Elements()[e], domain);
    if (!runtime.HasValue())
      return Result<SolidDynamicAnalysisState>::Failure(runtime.GetStatus());
    const auto* solid = dynamic_cast<const Hex8Element*>(runtime.Value().get());
    if (solid == nullptr)
      return Result<SolidDynamicAnalysisState>::Failure(Failure(
          "Dynamic Solid inventory contains a different element family."));
    auto zero = solid->EvaluateLinear(Vector(24));
    if (!zero.HasValue())
      return Result<SolidDynamicAnalysisState>::Failure(zero.GetStatus());
    state.point_inventory_.insert(state.point_inventory_.end(),
                                  zero.Value().points.begin(),
                                  zero.Value().points.end());
  }
  state.initialized_ = true;
  return Result<SolidDynamicAnalysisState>::Success(std::move(state));
}

Status SolidDynamicAnalysisState::CommitAcceptedFrame(
    const SolidDynamicAcceptedFrameState& f) {
  if (!math_internal::ArithmeticEnvironmentSupported())
    return Failure(
        "Paired state commit requires nearest rounding and gradual "
        "underflow.");
  const std::size_t index = accepted_frames_.size();
  if (!initialized_ || index > increment_count_ || f.frame_index != index ||
      f.increment_index != index || f.is_initial != (index == 0) ||
      f.time != static_cast<double>(index) * time_increment_ ||
      f.iteration_count > 100 || (index == 0 && f.iteration_count != 0) ||
      f.nodal_internal_order.size() != source_ids_.size() ||
      f.nodal_source_ids.size() != source_ids_.size() ||
      f.solid_results.size() != point_inventory_.size()) {
    return Failure(
        "Incomplete or out-of-order Solid endpoint inventory/schedule.");
  }
  for (std::size_t n = 0; n < source_ids_.size(); ++n)
    if (f.nodal_internal_order[n] != n ||
        !SameId(f.nodal_source_ids[n], source_ids_[n]))
      return Failure("Solid nodal source identity differs from its inventory.");
  const std::array<const Vector*, 10> high{
      &f.displacement,   &f.velocity,
      &f.acceleration,   &f.algorithmic_acceleration,
      &f.inertia_force,  &f.internal_force,
      &f.external_force, &f.residual,
      &f.reaction,       &f.residual_scale};
  const std::array<const Vector*, 10> low{
      &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};
  for (std::size_t i = 0; i < high.size(); ++i)
    if (!ValidPair(*high[i], *low[i], full_dof_count_))
      return Failure(
          "Solid endpoint pairs are incomplete, unnormalized or "
          "nonfinite.");
  using namespace math_internal;
  for (std::size_t i = 0; i < full_dof_count_; ++i) {
    const bool constrained = std::binary_search(constrained_dofs_.begin(),
                                                constrained_dofs_.end(), i);
    const HighLow inertia(f.inertia_force[i], f.low.inertia_force[i]);
    const HighLow internal(f.internal_force[i], f.low.internal_force[i]);
    const HighLow external(f.external_force[i], f.low.external_force[i]);
    const HighLow residual(f.residual[i], f.low.residual[i]);
    const HighLow scale(f.residual_scale[i], f.low.residual_scale[i]);
    const HighLow reaction(f.reaction[i], f.low.reaction[i]);
    const auto expected_residual = Subtract(Add(inertia, internal), external);
    const auto expected_scale =
        Add(Add(Abs(inertia), Abs(internal)), Abs(external));
    if (!IsFinite(expected_residual) || !IsFinite(expected_scale) ||
        Compare(residual, expected_residual) != 0 ||
        Compare(scale, expected_scale) != 0 ||
        Compare(reaction, constrained ? residual : HighLow(0)) != 0)
      return Failure(
          "Physical reaction/residual/uncancelled force scale disagree.");
    if (constrained && (f.displacement[i] != 0 || f.low.displacement[i] != 0 ||
                        f.velocity[i] != 0 || f.low.velocity[i] != 0 ||
                        f.acceleration[i] != 0 || f.low.acceleration[i] != 0 ||
                        f.algorithmic_acceleration[i] != 0 ||
                        f.low.algorithmic_acceleration[i] != 0))
      return Failure(
          "Prescribed dynamic translations and rates must be exact zero.");
    if (!constrained) {
      if (Compare(scale, HighLow(0)) < 0)
        return Failure("Negative physical force scale.");
      if (Compare(scale, HighLow(0)) == 0) {
        if (Compare(residual, HighLow(0)) != 0)
          return Failure("Physical zero-scale residual must be exactly zero.");
      } else {
        const auto ratio = Divide(Abs(residual), scale);
        if (!IsFinite(ratio) || !IsNormalized(ratio) ||
            Compare(ratio, HighLow(1e-10)) > 0)
          return Failure("Physical free endpoint equilibrium is incomplete.");
      }
    }
    if (index == 0 &&
        (f.displacement[i] != 0 || f.velocity[i] != 0 ||
         f.low.displacement[i] != 0 || f.low.velocity[i] != 0 ||
         Compare(HighLow(f.acceleration[i], f.low.acceleration[i]),
                 HighLow(f.algorithmic_acceleration[i],
                         f.low.algorithmic_acceleration[i])) != 0))
      return Failure(
          "Initial Solid displacement/velocity must be zero and A=a.");
  }
  for (std::size_t i = 0; i < point_inventory_.size(); ++i) {
    const auto& row = f.solid_results[i];
    const auto& expected = point_inventory_[i];
    if (row.element != expected.element ||
        row.integration_point != expected.integration_point ||
        row.natural_coordinates != expected.natural_coordinates ||
        row.reference_coordinates != expected.reference_coordinates ||
        !Finite(row.cauchy_stress) || !Finite(row.strain))
      return Failure(
          "Solid point recovery has invalid identity or finite values.");
  }
  if (!ValidPair(f.strain_energy, f.low.strain_energy) ||
      !ValidPair(f.kinetic_energy, f.low.kinetic_energy) ||
      !ValidPair(f.external_work, f.low.external_work) ||
      !ValidPair(f.energy_balance_error, f.low.energy_balance_error))
    return Failure("Nonfinite or unnormalized Solid energy/work pair.");
  if ((index == 0 &&
       (f.external_work != 0 || f.low.external_work != 0 ||
        f.energy_balance_error != 0 || f.low.energy_balance_error != 0)) ||
      Compare(HighLow(f.kinetic_energy, f.low.kinetic_energy), HighLow(0)) < 0)
    return Failure(
        "Initial work/error or kinetic energy violates its definition.");
  if (!ValidPair(f.linear_momentum, f.low.linear_momentum) ||
      !ValidPair(f.angular_momentum, f.low.angular_momentum) ||
      !ValidPair(f.linear_momentum_rate, f.low.linear_momentum_rate) ||
      !ValidPair(f.angular_momentum_rate, f.low.angular_momentum_rate) ||
      !ValidPair(f.linearization_current_internal_moment,
                 f.low.linearization_current_internal_moment) ||
      !ValidPair(f.reference_internal_moment,
                 f.low.reference_internal_moment) ||
      !ValidPair(f.current_free_residual_moment,
                 f.low.current_free_residual_moment))
    return Failure("Nonfinite or unnormalized physical momentum/moment pair.");
  if (!ValidPair(f.global_equilibrium, f.low.global_equilibrium) ||
      f.iterations.empty())
    return Failure("Missing finite equilibrium or iteration evidence.");
  for (std::size_t i = 0; i < f.iterations.size(); ++i) {
    const auto& e = f.iterations[i];
    if (e.iteration_index != i || !Finite(e.correction_metrics) ||
        !Finite(e.worst_force_terms) || !Finite(e.worst_weighted_terms))
      return Failure("Incomplete or nonfinite Newton correction evidence.");
    for (double x : {e.residual_numerator, e.residual_scale, e.residual_metric,
                     e.weighted_residual_numerator, e.update_metric,
                     e.old_residual_numerator, e.recombined_residual_numerator,
                     e.weighted_equivalence_metric})
      if (!std::isfinite(x) || x < 0)
        return Failure("Invalid raw Newton residual/update evidence.");
    for (double x : e.correction_metrics)
      if (x < 0) return Failure("Invalid correction scale.");
  }
  const auto& last = f.iterations.back();
  if (f.iterations.size() != static_cast<std::size_t>(f.iteration_count) + 1 ||
      !last.residual_pass || !last.correction_pass || !last.update_pass ||
      !last.zero_scale_exact || last.residual_metric > 1e-10 ||
      last.update_metric > 1e-12 || last.weighted_equivalence_metric > 1e-12 ||
      std::any_of(last.correction_metrics.begin(),
                  last.correction_metrics.end(),
                  [](double x) { return x > 1e-10; }))
    return Failure(
        "Candidate does not carry complete accepted convergence evidence.");
  accepted_frames_.push_back(f);
  return Status::Ok();
}

Result<SolidDynamicAcceptedFrameState>
SolidDynamicAnalysisState::BeginTrialFromCommitted() const {
  if (accepted_frames_.empty())
    return Result<SolidDynamicAcceptedFrameState>::Failure(
        Failure("No accepted Solid endpoint exists."));
  return Result<SolidDynamicAcceptedFrameState>::Success(
      accepted_frames_.back());
}
const std::vector<SolidDynamicAcceptedFrameState>&
SolidDynamicAnalysisState::AcceptedFrames() const noexcept {
  return accepted_frames_;
}
const SolidDynamicAcceptedFrameState&
SolidDynamicAnalysisState::CommittedState() const {
  if (accepted_frames_.empty())
    throw std::logic_error("No accepted Solid endpoint.");
  return accepted_frames_.back();
}
bool SolidDynamicAnalysisState::IsInitialized() const noexcept {
  return initialized_;
}
void SolidDynamicAnalysisState::RecordIteration(
    SolidDynamicIterationRecord record) {
  iteration_records_.push_back(std::move(record));
}
const std::vector<SolidDynamicIterationRecord>&
SolidDynamicAnalysisState::IterationRecords() const noexcept {
  return iteration_records_;
}
}  // namespace fesa