#include "fesa/analysis/solid_frequency_procedure.h"

#include <algorithm>
#include <cmath>
#include <limits>
#include <memory>
#include <new>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include "fesa/analysis/analysis_model.h"
#include "fesa/assembly/parallel_for.h"
#include "fesa/assembly/sparse_assembler.h"
#include "fesa/constraints/essential_constraint_policy.h"
#include "fesa/elements/element_factory.h"
#include "fesa/elements/hex8_element.h"
#include "fesa/fem/dof_manager.h"
#include "fesa/math/sparse_matrix.h"
#include "fesa/math/vector.h"
#include "fesa/model/domain.h"

namespace fesa {
namespace {
/// @brief Reports a frequency failure without publishing partial state.
Status Failure(const std::string& code, const std::string& message) {
  return Status::Failure(
      FailureCategory::kSolver,
      {{Severity::kError, code, {}, "FREQUENCY", {}, message}});
}
/// @brief Rounds nonnegative arithmetic toward conservative bounds.
double Up(double value) {
  return std::nextafter(value, (std::numeric_limits<double>::infinity)());
}
/// @brief Rounds a lower bound downward, including underflow uncertainty.
double Down(double value) {
  return std::nextafter(value, -(std::numeric_limits<double>::infinity)());
}
/// @brief Uses robust Frobenius accumulation on original CSR storage.
double Frobenius(const SparseMatrix& matrix) {
  double norm = 0.0;
  for (const double value : matrix.Values()) norm = std::hypot(norm, value);
  return norm;
}
/// @brief Evaluates exact-zero denominator semantics for modal checks.
double Ratio(double numerator, double denominator) {
  if (denominator == 0.0)
    return numerator == 0.0 ? 0.0 : (std::numeric_limits<double>::infinity)();
  return numerator / denominator;
}
/// @brief Builds an immutable candidate; all returns precede public commit.
Result<SolidFrequencyState> ExecuteCandidate(const Domain& domain) {
  const auto* frequency = domain.Frequency();
  if (!domain.IsSolid() || frequency == nullptr ||
      frequency->requested_mode_count <= 0)
    return Result<SolidFrequencyState>::Failure(Failure(
        "invalid-solid-frequency-procedure", "Solid frequency step required."));
  auto model = AnalysisModel::Create(domain);
  if (!model.HasValue())
    return Result<SolidFrequencyState>::Failure(model.GetStatus());
  if (!model.Value().Step().Loads().empty() ||
      !model.Value().ActiveSurfacePressureLoads().empty())
    return Result<SolidFrequencyState>::Failure(
        Failure("loaded-solid-frequency",
                "Frequency requires unloaded reference state."));
  std::vector<double> density(domain.Materials().Size(), 0.0);
  for (const auto& item : domain.Densities()) {
    if (item.material_index >= density.size() ||
        density[item.material_index] != 0.0 || !std::isfinite(item.rho) ||
        !(item.rho > 0.0))
      return Result<SolidFrequencyState>::Failure(
          Failure("invalid-material-density",
                  "Density must be unique finite positive."));
    density[item.material_index] = item.rho;
  }
  ElementFactory factory;
  std::vector<std::unique_ptr<Element>> owned;
  ElementView view;
  for (const auto index : model.Value().ActiveElements()) {
    const auto& definition = domain.Elements()[index];
    if (definition.MaterialIndex() >= density.size() ||
        !(density[definition.MaterialIndex()] > 0.0))
      return Result<SolidFrequencyState>::Failure(
          Failure("missing-frequency-density",
                  "Every Solid material requires density."));
    auto element = factory.Create(definition, domain);
    if (!element.HasValue())
      return Result<SolidFrequencyState>::Failure(element.GetStatus());
    if (dynamic_cast<const Hex8Element*>(element.Value().get()) == nullptr)
      return Result<SolidFrequencyState>::Failure(
          Failure("unsupported-frequency-element", "Only C3D8 is supported."));
    view.emplace_back(*element.Value());
    owned.push_back(std::move(element.Value()));
  }
  DofManager dofs;
  const Status mapping = dofs.Build(model.Value(), view);
  if (!mapping.IsOk()) return Result<SolidFrequencyState>::Failure(mapping);
  if (dofs.Profile() != DofProfile::kSolidStructural ||
      dofs.ComponentsPerNode() != 3U)
    return Result<SolidFrequencyState>::Failure(
        Failure("invalid-solid-frequency-map",
                "Frequency requires actual 3N mapping."));
  for (std::size_t i = 0; i < dofs.PrescribedValues().Size(); ++i)
    if (dofs.PrescribedValues()[i] != 0.0)
      return Result<SolidFrequencyState>::Failure(Failure(
          "unsupported-eigen-boundary", "Frequency requires homogeneous BC."));
  const std::size_t requested =
      static_cast<std::size_t>(frequency->requested_mode_count);
  if (dofs.FreeDofCount() == 0U || requested > dofs.FreeDofCount())
    return Result<SolidFrequencyState>::Failure(
        Failure("insufficient-solid-frequency-modes",
                "Requested modes exceed free dimension."));
  const SerialParallelFor serial;
  auto stiffness = SparseAssembler::Assemble(view, dofs, serial);
  if (!stiffness.HasValue())
    return Result<SolidFrequencyState>::Failure(stiffness.GetStatus());
  std::vector<CooContribution> mass_coo;
  std::vector<double> lower_by_dof(dofs.FullDofCount(), 0.0);
  std::vector<double> magnitude_by_dof(dofs.FullDofCount(), 0.0);
  std::vector<std::size_t> additions_by_dof(dofs.FullDofCount(), 0U);
  constexpr double eps = std::numeric_limits<double>::epsilon();
  for (std::size_t e = 0; e < owned.size(); ++e) {
    const auto& hex = static_cast<const Hex8Element&>(*owned[e]);
    auto mass = hex.ComputeMass();
    if (!mass.HasValue())
      return Result<SolidFrequencyState>::Failure(mass.GetStatus());
    auto scatter = dofs.ElementScatter(hex.DofLayout());
    if (!scatter.HasValue())
      return Result<SolidFrequencyState>::Failure(scatter.GetStatus());
    const auto weights = hex.ReferenceWeights();
    const double min_weight = *std::min_element(weights.begin(), weights.end());
    const auto element_index = model.Value().ActiveElements()[e];
    const double rho =
        density[domain.Elements()[element_index].MaterialIndex()];
    const double local_lower = Down(Down(rho * min_weight) / 27.0);
    // Relative gamma bounds require normal nonzero products. Fixed Gauss
    // shapes exceed 1/128, so rho/16384 bounds the smallest density product.
    if (!std::isfinite(local_lower) ||
        local_lower < std::numeric_limits<double>::min() ||
        rho / 16384.0 < std::numeric_limits<double>::min() ||
        min_weight < std::numeric_limits<double>::min())
      return Result<SolidFrequencyState>::Failure(
          Failure("solid-mass-bound-unavailable",
                  "Local reference mass bound is unrepresentable."));
    // The eight fixed trilinear Gauss shapes have sigma_min(H)^2=1/27.
    // 512 eps bounds their fixed evaluation, products and eight-term sums.
    // Every mass term is positive; stable incidence scatter preserves the
    // bound.
    for (std::size_t i = 0; i < 24U; ++i) {
      const auto row = scatter.Value()[i];
      lower_by_dof[row] = Down(lower_by_dof[row] + local_lower);
      double row_sum = 0.0;
      for (std::size_t j = 0; j < 24U; ++j) {
        const double value = mass.Value().values(i, j);
        if (value != 0.0 &&
            std::abs(value) < std::numeric_limits<double>::min())
          return Result<SolidFrequencyState>::Failure(Failure(
              "solid-mass-bound-unavailable",
              "Subnormal local mass invalidates relative roundoff bounds."));
        row_sum = Up(row_sum + std::abs(value));
        mass_coo.push_back({row, scatter.Value()[j], value, e, i * 24U + j});
      }
      magnitude_by_dof[row] = Up(magnitude_by_dof[row] + row_sum);
      ++additions_by_dof[row];
    }
  }
  double ideal_lower = (std::numeric_limits<double>::infinity)();
  double guard = 0.0;
  for (const auto row : dofs.FreeDofs()) {
    ideal_lower = (std::min)(ideal_lower, lower_by_dof[row]);
    const double count_eps =
        Up((512.0 + static_cast<double>(additions_by_dof[row])) * eps);
    if (!(count_eps < 0.5))
      return Result<SolidFrequencyState>::Failure(
          Failure("solid-mass-bound-unavailable",
                  "Mass reduction exceeds roundoff budget."));
    const double gamma = Up(count_eps / (1.0 - count_eps));
    // Convert the forward bound relative to the exact sum to an error bound
    // relative to the available computed magnitude. Row infinity bounds also
    // bound the symmetric perturbation's spectral norm under restriction.
    const double computed_gamma = Up(gamma / (1.0 - gamma));
    guard = (std::max)(guard, Up(computed_gamma * magnitude_by_dof[row]));
  }
  const double mass_lower = Down(ideal_lower - guard);
  if (!std::isfinite(mass_lower) || !(mass_lower > 0.0) ||
      !(guard < 0.01 * ideal_lower))
    return Result<SolidFrequencyState>::Failure(
        Failure("solid-mass-bound-unavailable",
                "Roundoff prevents a useful SPD mass enclosure."));
  auto mass =
      SparseMatrix::FromCoo(dofs.FullDofCount(), dofs.FullDofCount(),
                            std::move(mass_coo), dofs.GetSparsePattern());
  if (!mass.HasValue())
    return Result<SolidFrequencyState>::Failure(mass.GetStatus());
  const EssentialConstraintPolicy partition;
  auto k_blocks = partition.Partition(stiffness.Value(), dofs);
  auto m_blocks = partition.Partition(mass.Value(), dofs);
  if (!k_blocks.HasValue())
    return Result<SolidFrequencyState>::Failure(k_blocks.GetStatus());
  if (!m_blocks.HasValue())
    return Result<SolidFrequencyState>::Failure(m_blocks.GetStatus());
  const auto& k = k_blocks.Value().k_ff;
  const auto& m = m_blocks.Value().k_ff;
  double scale = 0.0;
  for (std::size_t row = 0; row < k.Rows(); ++row) {
    double kd = 0.0;
    double md = 0.0;
    for (std::size_t p = k.RowOffsets()[row]; p < k.RowOffsets()[row + 1U]; ++p)
      if (k.ColumnIndices()[p] == row) kd = k.Values()[p];
    for (std::size_t p = m.RowOffsets()[row]; p < m.RowOffsets()[row + 1U]; ++p)
      if (m.ColumnIndices()[p] == row) md = m.Values()[p];
    if (!(md > 0.0) || !std::isfinite(std::abs(kd) / md))
      return Result<SolidFrequencyState>::Failure(
          Failure("invalid-solid-eigen-scale",
                  "Mass diagonal or classification scale is invalid."));
    scale = (std::max)(scale, std::abs(kd) / md);
  }
  if (!(scale > 0.0) || !std::isfinite(scale))
    return Result<SolidFrequencyState>::Failure(
        Failure("invalid-solid-eigen-scale",
                "A positive finite classification scale is required."));
  auto backend = MklFeastEigenSolver{}.SolveGeneralized(k, m, requested,
                                                        mass_lower, scale);
  if (!backend.HasValue())
    return Result<SolidFrequencyState>::Failure(backend.GetStatus());
  SolidFrequencyState state(std::move(backend.Value()));
  state.full_dof_count = dofs.FullDofCount();
  state.free_dof_count = dofs.FreeDofCount();
  state.classification_scale = scale;
  state.mass_ideal_lower_bound = ideal_lower;
  state.mass_roundoff_guard = guard;
  state.mass_lower_bound = mass_lower;
  state.constraint_mask.assign(dofs.FullDofCount(), 0U);
  state.prescribed_displacement.assign(dofs.FullDofCount(), 0.0);
  for (const auto row : dofs.ConstrainedDofs()) state.constraint_mask[row] = 1U;
  const std::size_t count = state.backend.eigenvalues.size();
  if (count < requested || state.backend.eigenvectors.Rows() != k.Rows() ||
      state.backend.eigenvectors.Columns() != count)
    return Result<SolidFrequencyState>::Failure(
        Failure("insufficient-solid-frequency-pairs",
                "Backend returned insufficient complete pairs."));
  const double norm_k = Frobenius(k);
  const double norm_m = Frobenius(m);
  std::vector<Vector> phi;
  std::vector<Vector> mass_phi;
  phi.reserve(count);
  mass_phi.reserve(count);
  for (std::size_t c = 0; c < count; ++c) {
    Vector vector(k.Rows());
    for (std::size_t row = 0; row < k.Rows(); ++row) {
      vector[row] = state.backend.eigenvectors(row, c);
      if (!std::isfinite(vector[row]))
        return Result<SolidFrequencyState>::Failure(
            Failure("solid-eigenpair-validation-failed",
                    "Modal mass is not finite positive."));
    }
    Vector mv = m.Multiply(vector);
    const double raw_mass = vector.Dot(mv);
    if (!(raw_mass > 0.0) || !std::isfinite(raw_mass))
      return Result<SolidFrequencyState>::Failure(
          Failure("solid-eigenpair-validation-failed",
                  "Modal mass is not finite positive."));
    vector.Scale(1.0 / std::sqrt(raw_mass));
    for (std::size_t row = 0; row < vector.Size(); ++row) {
      if (!std::isfinite(vector[row]))
        return Result<SolidFrequencyState>::Failure(
            Failure("solid-eigenpair-validation-failed",
                    "Normalized modal vector is not finite."));
    }
    mv = m.Multiply(vector);
    const Vector kv = k.Multiply(vector);
    const double lambda = state.backend.eigenvalues[c];
    const double modal_mass = vector.Dot(mv);
    const double energy = vector.Dot(kv);
    Vector residual = kv;
    residual.Axpy(-lambda, mv);
    const double norm_phi = vector.Norm();
    const double residual_norm = residual.Norm();
    const double lambda_mass = lambda * modal_mass;
    const double energy_error = std::abs(energy - lambda_mass);
    const double matrix_scale = norm_k + std::abs(lambda) * norm_m;
    const double denominator = matrix_scale * norm_phi;
    const double energy_denominator = denominator * norm_phi;
    // Finite / infinity is zero in IEEE arithmetic but cannot certify a pair.
    // Validate both original-pencil scales before either division, preserving
    // the exact-zero branch only for representable finite numerator/scale.
    if (!std::isfinite(norm_k) || !std::isfinite(norm_m) ||
        !std::isfinite(norm_phi) || !std::isfinite(residual_norm) ||
        !std::isfinite(lambda_mass) || !std::isfinite(energy_error) ||
        !std::isfinite(matrix_scale) || !std::isfinite(denominator) ||
        !std::isfinite(energy_denominator))
      return Result<SolidFrequencyState>::Failure(
          Failure("solid-eigenpair-validation-failed",
                  "A nonfinite original K/M validation numerator or scale "
                  "cannot certify a pair."));
    const double eta_r = Ratio(residual_norm, denominator);
    const double eta_norm = std::abs(modal_mass - 1.0);
    const double eta_energy = Ratio(energy_error, energy_denominator);
    if (!std::isfinite(lambda) || !std::isfinite(eta_r) || eta_r > 1e-10 ||
        !std::isfinite(eta_norm) || eta_norm > 1e-10 ||
        !std::isfinite(eta_energy) || eta_energy > 1e-10 ||
        !std::isfinite(energy / modal_mass) || lambda / scale < -1e-9)
      return Result<SolidFrequencyState>::Failure(
          Failure("solid-eigenpair-validation-failed",
                  "Original K/M pair checks failed."));
    state.eigenpair_checks.push_back({c + 1U, state.backend.column_ids[c],
                                      lambda, modal_mass, energy / modal_mass,
                                      eta_r, eta_norm, eta_energy});
    phi.push_back(std::move(vector));
    mass_phi.push_back(std::move(mv));
  }
  for (std::size_t a = 0; a < count; ++a)
    for (std::size_t b = 0; b < count; ++b) {
      const double dot = phi[a].Dot(mass_phi[b]);
      state.eta_gram = std::hypot(state.eta_gram, dot - (a == b ? 1.0 : 0.0));
      if (a != b)
        state.max_eta_orth =
            (std::max)(state.max_eta_orth,
                       std::abs(dot) /
                           std::sqrt(state.eigenpair_checks[a].modal_mass *
                                     state.eigenpair_checks[b].modal_mass));
    }
  if (!std::isfinite(state.eta_gram) || state.eta_gram > 1e-10 ||
      !std::isfinite(state.max_eta_orth) || state.max_eta_orth > 1e-10)
    return Result<SolidFrequencyState>::Failure(
        Failure("solid-eigenpair-validation-failed",
                "All-pair mass Gram checks failed."));
  state.vectors = Matrix(requested, dofs.FullDofCount());
  std::size_t cluster = 1U;
  for (std::size_t c = 0; c < requested; ++c) {
    const double lambda = state.backend.eigenvalues[c];
    if (c > 0 && (lambda - state.backend.eigenvalues[c - 1U]) / scale > 1e-8)
      ++cluster;
    std::optional<double> frequency_value;
    if (lambda >= 0.0)
      frequency_value = std::sqrt(lambda) / (2.0 * std::acos(-1.0));
    state.modes.push_back(
        {c + 1U, lambda,
         static_cast<std::uint8_t>(std::abs(lambda / scale) <= 1e-9 ? 0U : 1U),
         cluster, frequency_value});
    // Strict > retains the first source/component entry for equal magnitudes.
    std::size_t pivot = 0U;
    for (std::size_t row = 1; row < phi[c].Size(); ++row)
      if (std::abs(phi[c][row]) > std::abs(phi[c][pivot])) pivot = row;
    const double sign = phi[c][pivot] < 0.0 ? -1.0 : 1.0;
    for (std::size_t row = 0; row < phi[c].Size(); ++row)
      state.vectors(c, dofs.FreeDofs()[row]) = sign * phi[c][row];
  }
  state.backend.eigenvectors = Matrix(0U, 0U);
  return Result<SolidFrequencyState>::Success(std::move(state));
}
}  // namespace

Result<SolidFrequencyState> SolidFrequencyProcedureExecutor::Execute(
    const Domain& domain) const {
  try {
    auto result = ExecuteCandidate(domain);
    if (result.HasValue()) return result;
    auto diagnostics = result.GetStatus().Diagnostics();
    for (auto& diagnostic : diagnostics)
      if (diagnostic.location.file.empty())
        diagnostic.location = domain.Frequency() != nullptr
                                  ? domain.Frequency()->frequency_location
                                  : SourceLocation{domain.SourcePath(), 0U};
    return Result<SolidFrequencyState>::Failure(Status::Failure(
        result.GetStatus().Category().value_or(FailureCategory::kSolver),
        std::move(diagnostics)));
  } catch (const std::bad_alloc&) {
    return Result<SolidFrequencyState>::Failure(Failure(
        "eigen-allocation-failed", "Solid modal candidate allocation failed."));
  } catch (const std::length_error&) {
    return Result<SolidFrequencyState>::Failure(
        Failure("eigen-allocation-failed",
                "Solid modal workspace exceeds addressable storage."));
  } catch (const std::runtime_error&) {
    return Result<SolidFrequencyState>::Failure(
        Failure("eigen-multiplication-failed",
                "Solid modal sparse multiplication failed."));
  }
}
}  // namespace fesa