FESADev/src/fesa/analysis/hht_reduced_internal.h

#ifndef FESA_ANALYSIS_HHT_REDUCED_INTERNAL_H_
#define FESA_ANALYSIS_HHT_REDUCED_INTERNAL_H_

#include <array>
#include <cstddef>
#include <functional>
#include <vector>

#include "analysis/hht_dynamic_internal.h"
#include "fesa/core/status.h"
#include "fesa/math/matrix.h"
#include "fesa/math/vector.h"

namespace fesa {
class TangentSolver;

namespace hht_reduced_internal {

/// @brief One canonical nonzero sparse coefficient in full coordinates.
struct ConstraintEntry {
  std::size_t dof = 0U;
  double value = 0.0;
};

/// @brief One immutable private coordinate and its full-space injection.
struct FixedBasisColumn {
  std::size_t node = 0U;
  std::size_t channel = 0U;  // Translation=0, angular=1.
  bool fixed_plane = false;
  std::vector<ConstraintEntry> support;
  std::vector<double> support_radii;
};

/// @brief Fixed initial-coordinate split q_f=L_p p+L_z z for one HHT run.
/// @note A node whose constrained rotation cuts its analytic null is retained
/// entirely in p.  The split is immutable; chart/rank changes are failures.
struct FixedChart {
  std::size_t full_dof_count = 0U;
  std::vector<std::size_t> p_dofs;
  std::vector<std::size_t> z_dofs;
  std::vector<std::size_t> null_nodes;
  std::vector<std::size_t> pivot_components;
  // p_dofs/z_dofs are canonical labels, not injection indices.
  std::vector<FixedBasisColumn> p_columns;
  std::vector<FixedBasisColumn> z_columns;
  std::vector<std::array<double, 3U>> initial_nulls;
  std::vector<std::size_t> minimum_axes;  // 3 for component charts.
};

/// @brief One canonical nonzero Hessian entry for a scalar constraint.
struct ConstraintHessianEntry {
  std::size_t row = 0U;
  std::size_t column = 0U;
  double value = 0.0;
};

/// @brief Exact sparse derivatives of one nodal drilling constraint.
/// @note Element-local 24-by-24 contributions are reduced in source/scatter
/// order.  This deliberately avoids a null-count by full-DOF squared tensor.
struct ConstraintRowOperators {
  double value = 0.0;
  /// @brief Existing N3 uncancelled position-constraint scale.
  double value_scale = 0.0;
  double time_first = 0.0;
  double time_second = 0.0;
  std::vector<ConstraintEntry> gradient;
  std::vector<ConstraintEntry> time_gradient;
  std::vector<ConstraintEntry> third_directional_gradient;
  std::vector<ConstraintHessianEntry> hessian;
};

/// @brief Exact sparse nodal drilling operators in chart-null order.
struct ConstraintOperators {
  std::vector<ConstraintRowOperators> rows;
};

/// @brief Accepted-branch graph result.  `operators` belongs exactly to q.
struct EmbeddingRoot {
  Vector p{0U};
  Vector z{0U};
  Vector q{0U};
  ConstraintOperators operators;
  double graph_residual_infinity = 0.0;
  double graph_residual_relative = 0.0;
  double graph_correction_infinity = 0.0;
  std::size_t graph_iteration_count = 0U;
};

/// @brief First implicit graph derivatives at one converged embedding root.
struct EmbeddingFirstDerivatives {
  Matrix jacobian{0U, 0U};
  Vector time_derivative{0U};
};

/// @brief Full physical rates reconstructed from private reduced coordinates.
struct ReconstructedRates {
  Vector velocity{0U};
  Vector acceleration{0U};
  Vector z_velocity{0U};
  Vector z_acceleration{0U};
};

/// @brief Private equation ordering for one exact augmented reduced Newton step.
/// The four blocks are `(delta p, delta z, delta vz, delta az)`; no public
/// equation numbers or result fields use this ordering.
struct AugmentedReducedSystem {
  SparseMatrix tangent;
  Vector rhs{0U};
  std::size_t physical_count = 0U;
  std::size_t algebraic_count = 0U;
  Vector row_scale{0U};
  Vector unknown_scale{0U};
};

/// @brief One augmented solve mapped back to full chart correction channels.
/// Only `p` is an integrator correction; z/vz/az are same-trial auxiliary
/// variables for diagnostics and exact full q/v/a correction recovery.
struct AugmentedCorrection {
  Vector p{0U};
  Vector z{0U};
  Vector z_velocity{0U};
  Vector z_acceleration{0U};
  Vector coordinate{0U};
  Vector velocity{0U};
  Vector acceleration{0U};
};

/// @brief Actual Newton operator, retaining its indices in the full system.
struct AugmentedSolveSelection {
  SparseMatrix tangent;
  Vector rhs{0U};
  std::vector<std::size_t> full_indices;
  bool angular_only = false;
};

/// @brief Approved translation-pass/rotation-residual-fail solve policy.
bool ShouldRefineAngular(
    const FixedChart& chart, const std::array<double, 2U>& residual_metrics,
    const std::array<double, 4U>& translation_corrections,
    bool translation_private_algorithmic_pass);

/// @brief Selects angular p and all auxiliary blocks, or the full operator.
Result<AugmentedSolveSelection> SelectAugmentedSolve(
    const AugmentedReducedSystem& full, const FixedChart& chart,
    bool angular_only);

/// @brief Zero-embeds an actual selected solve into the full augmented order.
Result<Vector> EmbedAugmentedSolution(
    const AugmentedSolveSelection& selected, const Vector& solution,
    std::size_t full_size);

/// @brief Evaluates one position-only graph trial without mutating history.
using ConstraintEvaluator =
    std::function<Result<ConstraintOperators>(const Vector&)>;

/// @brief Builds the fixed p/z graph selectors from frame-zero rotations.
Result<FixedChart> BuildFixedChart(
    const DofManager& dofs,
    const std::vector<DynamicRotationKinematics>& rotations);

/// @brief Lifts selector coordinates, retaining exact zero prescribed values.
Result<Vector> Lift(const FixedChart& chart, const Vector& p, const Vector& z);

/// @brief Applies the fixed sparse transpose without reinterpreting labels.
Result<Vector> ProjectPhysical(const FixedChart& chart, const Vector& full);
Result<Vector> ProjectAlgebraic(const FixedChart& chart, const Vector& full);

/// @brief Returns the explicit translation/angular channel for a p column.
std::size_t PhysicalCoordinateChannel(const FixedChart& chart,
                                      std::size_t coordinate);

/// @brief Solves the coupled fixed graph psi(L_p p+L_z z,t)=0.
/// @note `seed` is caller-owned accepted branch data; only the returned root
/// may be committed.  No nodewise solve, pivot change, or full-coordinate
/// fallback is performed here.
Result<EmbeddingRoot> SolveEmbeddingRoot(const FixedChart& chart,
                                         const Vector& p, const Vector& seed,
                                         const ConstraintEvaluator& evaluate,
                                         TangentSolver& solver);

/// @brief Builds Q_p and Q_t from RC-07 using the root's G factorization.
Result<EmbeddingFirstDerivatives> EvaluateEmbeddingFirstDerivatives(
    const FixedChart& chart, const EmbeddingRoot& root, TangentSolver& solver);

/// @brief Applies RC-07/08 directly without forming a production dense J.
Result<ReconstructedRates> ReconstructPhysicalRates(
    const FixedChart& chart, const EmbeddingRoot& root, const Vector& vp,
    const Vector& ap, TangentSolver& solver);

/// @brief Returns H_i[left,right] for all scalar drilling constraints.
Vector ContractConstraintHessian(const ConstraintOperators& operators,
                                 const Vector& left, const Vector& right);

/// @brief Forms RC-10/11 without materializing Q_p or a dense reduced tangent.
/// @param velocity_rate Coefficient gamma/(beta*h).
/// @param acceleration_rate Coefficient 1/((1+alpha)*beta*h*h).
/// @note `assembly` must retain separate DqI+Kint, DvI and M operators.
Result<AugmentedReducedSystem> AssembleAugmentedReducedSystem(
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    const ConstraintOperators& operators, double velocity_rate,
    double acceleration_rate, double reference_length,
    double time_increment);

/// @brief Decodes all augmented blocks using the RC selector substitutions.
Result<AugmentedCorrection> MapAugmentedCorrection(
    const FixedChart& chart, const AugmentedReducedSystem& system,
    const Vector& solution, double velocity_rate, double acceleration_rate);

/// @brief Scatters exact shell drilling scalar operators in canonical order.
/// @param assembly Current full assembly at q/v/a; its rotation inventory is
/// checked against `chart` before any operator is returned.
/// @param force_rate, force_acceleration Private manufactured-load derivatives.
/// Public constant CLOAD callers pass zero vectors.
Result<ConstraintOperators> EvaluateConstraintOperators(
    const Domain& domain, const DofManager& dofs,
    const std::vector<hht_internal::RuntimeShell>& shells,
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    const Vector& force_rate, const Vector& force_acceleration);

/// @brief Solves G x=rhs with the existing nonsymmetric tangent backend.
/// The factorization is checked by the same backward-error rule as HHT.
Result<Vector> SolveGraphSystem(const Matrix& matrix, const Vector& rhs,
                                TangentSolver& solver);

/// @brief Certifies W^-1(psi_q L_z)W^-1 with the existing 2^-32 policy.
/// @return Reciprocal infinity condition; empty charts return exact zero.
Result<double> CheckGraphRegularity(
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    const ConstraintOperators& operators, TangentSolver& solver);

/// @brief Checks the normalized full-root spanning operator separately from G.
Result<double> CheckSelectorTransversality(
    const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
    TangentSolver& solver);

/// @brief Forms G=psi_q L_z without building a dense full selector.
Matrix GraphJacobian(const ConstraintOperators& operators,
                     const FixedChart& chart);

/// @brief Forms psi_q L_p, used by RC-07 graph derivative solves.
Matrix PhysicalJacobian(const ConstraintOperators& operators,
                        const FixedChart& chart);

}  // namespace hht_reduced_internal
}  // namespace fesa

#endif  // FESA_ANALYSIS_HHT_REDUCED_INTERNAL_H_