apex_solver/optimizer/

gauss_newton.rs

//! Gauss-Newton optimization algorithm implementation.
//!
//! The Gauss-Newton method is a fundamental iterative algorithm for solving nonlinear least squares problems
//! of the form:
//!
//! ```text
//! min f(x) = ½||r(x)||² = ½Σᵢ rᵢ(x)²
//! ```
//!
//! where `r: ℝⁿ → ℝᵐ` is the residual vector function.
//!
//! # Algorithm Overview
//!
//! The Gauss-Newton method solves the normal equations at each iteration:
//!
//! ```text
//! J^T·J·h = -J^T·r
//! ```
//!
//! where:
//! - `J` is the Jacobian matrix (m × n) of partial derivatives ∂rᵢ/∂xⱼ
//! - `r` is the residual vector (m × 1)
//! - `h` is the step vector (n × 1)
//!
//! The approximated Hessian `H ≈ J^T·J` replaces the true Hessian `∇²f = J^T·J + Σᵢ rᵢ·∇²rᵢ`,
//! which works well when residuals are small or nearly linear.
//!
//! ## Convergence Properties
//!
//! - **Quadratic convergence** near the solution when the Gauss-Newton approximation is valid
//! - **May diverge** if the initial guess is far from the optimum or the problem is ill-conditioned
//! - **No step size control** - always takes the full Newton step without damping
//!
//! ## When to Use
//!
//! Gauss-Newton is most effective when:
//! - The problem is well-conditioned with `J^T·J` having good numerical properties
//! - The initial parameter guess is close to the solution
//! - Fast convergence is prioritized over robustness
//! - Residuals at the solution are expected to be small
//!
//! For ill-conditioned problems or poor initial guesses, consider:
//! - [`LevenbergMarquardt`](crate::optimizer::LevenbergMarquardt) for adaptive damping
//! - [`DogLeg`](crate::optimizer::DogLeg) for trust region control
//!
//! # Implementation Features
//!
//! - **Sparse matrix support**: Efficient handling of large-scale problems via `faer` sparse library
//! - **Robust linear solvers**: Choice between Cholesky (fast) and QR (stable) factorizations
//! - **Jacobi scaling**: Optional diagonal preconditioning to improve conditioning
//! - **Manifold operations**: Support for optimization on Lie groups (SE2, SE3, SO2, SO3)
//! - **Comprehensive diagnostics**: Detailed convergence and performance summaries
//!
//! # Mathematical Background
//!
//! At each iteration k, the algorithm:
//!
//! 1. **Linearizes** the problem around current estimate xₖ: `r(xₖ + h) ≈ r(xₖ) + J(xₖ)·h`
//! 2. **Solves** the normal equations for step h: `J^T·J·h = -J^T·r`
//! 3. **Updates** parameters: `xₖ₊₁ = xₖ ⊕ h` (using manifold plus operation)
//! 4. **Checks** convergence criteria (cost, gradient, parameter change)
//!
//! The method terminates when cost change, gradient norm, or parameter update fall below
//! specified tolerances, or when maximum iterations are reached.
//!
//! # Examples
//!
//! ## Basic usage
//!
//! ```no_run
//! use apex_solver::optimizer::GaussNewton;
//! use apex_solver::core::problem::Problem;
//! use apex_solver::JacobianMode;
//!
//! # type TestResult = Result<(), Box<dyn std::error::Error>>;
//! # fn main() -> TestResult {
//! // Create optimization problem
//! let mut problem = Problem::new(JacobianMode::Sparse);
//! // ... add residual blocks (factors) to problem ...
//!
//! // Create solver with default configuration
//! let mut solver = GaussNewton::new();
//!
//! // Run optimization
//! let result = solver.optimize(&mut problem)?;
//! # Ok(())
//! # }
//! ```
//!
//! ## Advanced configuration
//!
//! ```no_run
//! use apex_solver::optimizer::gauss_newton::{GaussNewtonConfig, GaussNewton};
//! use apex_solver::linalg::LinearSolverType;
//!
//! # fn main() {
//! let config = GaussNewtonConfig::new()
//!     .with_max_iterations(100)
//!     .with_cost_tolerance(1e-8)
//!     .with_parameter_tolerance(1e-8)
//!     .with_gradient_tolerance(1e-10)
//!     .with_linear_solver_type(LinearSolverType::SparseQR)  // More stable
//!     .with_jacobi_scaling(true);  // Improve conditioning
//!
//! let mut solver = GaussNewton::with_config(config);
//! # }
//! ```
//!
//! # References
//!
//! - Nocedal, J. & Wright, S. (2006). *Numerical Optimization* (2nd ed.). Springer. Chapter 10.
//! - Madsen, K., Nielsen, H. B., & Tingleff, O. (2004). *Methods for Non-Linear Least Squares Problems* (2nd ed.).
//! - Björck, Å. (1996). *Numerical Methods for Least Squares Problems*. SIAM.

use crate::error::ErrorLogging;
use crate::{core::problem, error, linalg, optimizer};
use std::time;
use tracing::debug;

use crate::linalg::{
    CovarianceOptions, Damping, DenseCholeskySolver, DenseMode, DenseQRSolver, ExplicitDenseSchur,
    ExplicitSchurVariant, ExplicitSparseSchur, ImplicitSparseSchur, JacobianMode, LinearSolver,
    LinearSolverType, SchurPreconditioner, SparseCholeskySolver, SparseMode, SparseQRSolver,
    StructureAware,
};
use crate::optimizer::{AssemblyBackend, IterationStats};

/// Configuration parameters for the Gauss-Newton optimizer.
///
/// Controls the behavior of the Gauss-Newton algorithm including convergence criteria,
/// linear solver selection, and numerical stability enhancements.
///
/// # Builder Pattern
///
/// All configuration options can be set using the builder pattern:
///
/// ```
/// use apex_solver::optimizer::gauss_newton::GaussNewtonConfig;
/// use apex_solver::linalg::LinearSolverType;
///
/// let config = GaussNewtonConfig::new()
///     .with_max_iterations(50)
///     .with_cost_tolerance(1e-6)
///     .with_linear_solver_type(LinearSolverType::SparseQR);
/// ```
///
/// # Convergence Criteria
///
/// The optimizer terminates when ANY of the following conditions is met:
///
/// - **Cost tolerance**: `|cost_k - cost_{k-1}| < cost_tolerance`
/// - **Parameter tolerance**: `||step|| < parameter_tolerance`
/// - **Gradient tolerance**: `||J^T·r|| < gradient_tolerance`
/// - **Maximum iterations**: `iteration >= max_iterations`
/// - **Timeout**: `elapsed_time >= timeout`
///
/// # See Also
///
/// - [`GaussNewton`] - The solver that uses this configuration
/// - [`LevenbergMarquardtConfig`](crate::optimizer::levenberg_marquardt::LevenbergMarquardtConfig) - For adaptive damping
/// - [`DogLegConfig`](crate::optimizer::dog_leg::DogLegConfig) - For trust region methods
#[derive(Clone)]
pub struct GaussNewtonConfig {
    /// Type of linear solver for the linear systems
    pub linear_solver_type: linalg::LinearSolverType,
    /// Maximum number of iterations
    pub max_iterations: usize,
    /// Convergence tolerance for cost function
    pub cost_tolerance: f64,
    /// Convergence tolerance for parameter updates
    pub parameter_tolerance: f64,
    /// Convergence tolerance for gradient norm
    pub gradient_tolerance: f64,
    /// Timeout duration
    pub timeout: Option<time::Duration>,
    /// Use Jacobi column scaling (preconditioning)
    ///
    /// When enabled, normalizes Jacobian columns by their L2 norm before solving.
    /// This can improve convergence for problems with mixed parameter scales
    /// (e.g., positions in meters + angles in radians) but adds ~5-10% overhead.
    ///
    /// Default: false (Gauss-Newton is typically used on well-conditioned problems)
    pub use_jacobi_scaling: bool,
    /// Uniform diagonal regularization added to `JᵀJ` before each solve.
    ///
    /// Pure Gauss-Newton (λ=0) fails outright when `JᵀJ` is singular. Solving
    /// `(JᵀJ + min_diagonal·I)·Δx = −Jᵀr` instead keeps the factorization well
    /// posed at a cost that is invisible against any real curvature. Set to
    /// `0.0` for the un-regularized normal equations.
    ///
    /// Default: 1e-10 (very small, practically identical to pure Gauss-Newton)
    pub min_diagonal: f64,

    /// Minimum objective function cutoff (optional early termination)
    ///
    /// If set, optimization terminates when cost falls below this threshold.
    /// Useful for early stopping when a "good enough" solution is acceptable.
    ///
    /// Default: None (disabled)
    pub min_cost_threshold: Option<f64>,

    /// Maximum condition number for Jacobian matrix (optional check)
    ///
    /// If set, the optimizer checks if condition_number(J^T*J) exceeds this
    /// threshold and terminates with IllConditionedJacobian status.
    /// Note: Computing condition number is expensive, so this is disabled by default.
    ///
    /// Default: None (disabled)
    pub max_condition_number: Option<f64>,

    /// Compute per-variable covariance matrices (uncertainty estimation)
    ///
    /// When enabled, computes covariance by re-linearizing the problem at the
    /// solution and inverting the Gauss-Newton Hessian `H = JᵀJ` after
    /// convergence. The full covariance matrix is extracted into per-variable
    /// blocks stored in both Variable structs and optimier::SolverResult.
    ///
    /// Scaled (σ̂²·H⁻¹) versus unscaled (H⁻¹) covariance, the factorization
    /// algorithm, and the pseudo-inverse cutoff are configured through
    /// [`covariance_options`](Self::covariance_options).
    ///
    /// Default: false (to avoid performance overhead)
    pub compute_covariances: bool,

    /// Options for covariance estimation when `compute_covariances` is enabled.
    ///
    /// Defaults to unscaled `H⁻¹` via sparse Cholesky — the correct choice when
    /// residuals are whitened by their measurement information matrix. Set
    /// `apply_variance_scaling` for unweighted least squares, where the noise
    /// scale `σ̂² = 2·cost/(m−n)` must be estimated from the fit. See
    /// [`CovarianceOptions`](crate::linalg::covariance::CovarianceOptions).
    pub covariance_options: CovarianceOptions,

    /// [`ExplicitSparseSchur`] sub-variant (for
    /// [`LinearSolverType::ExplicitSparseSchur`]).
    ///
    /// - [`ExplicitSchurVariant::Sparse`]: form S, sparse Cholesky. Most
    ///   accurate; memory is O(kept_dof²).
    /// - [`ExplicitSchurVariant::Chunked`]: form S straight from `J`, never
    ///   materializing `JᵀJ`. Same algebra, far less memory on large problems.
    /// - [`ExplicitSchurVariant::Iterative`]: form S, then PCG instead of
    ///   Cholesky.
    ///
    /// To never form `S` at all, use
    /// [`LinearSolverType::ImplicitSparseSchur`] instead — a different
    /// solver, not a variant of this one.
    ///
    /// Default: `Sparse`
    pub schur_variant: ExplicitSchurVariant,
    /// Preconditioner for the PCG used by the iterative Schur variants.
    ///
    /// - [`SchurPreconditioner::None`]: unconditioned CG.
    /// - [`SchurPreconditioner::BlockDiagonal`]: block diagonal of `H_kk`.
    /// - [`SchurPreconditioner::SchurJacobi`]: block diagonal of `S` itself —
    ///   Ceres's `SCHUR_JACOBI`, and usually the best convergence.
    ///
    /// Ignored by the direct variants, which factorize instead of iterating.
    ///
    /// Default: `SchurJacobi`
    pub schur_preconditioner: SchurPreconditioner,
    /// Maximum PCG iterations per linear solve, for the iterative variants.
    ///
    /// Default: 200 (Ceres's default)
    pub schur_cg_max_iterations: usize,
    /// Relative residual tolerance ending a PCG solve, for the iterative
    /// variants.
    ///
    /// Default: 1e-6
    pub schur_cg_tolerance: f64,
    /// Forcing-sequence parameter η for the Schur PCG paths (Ceres's
    /// `Solver::Options::eta`).
    ///
    /// PCG stops once the quadratic model's relative improvement per iteration
    /// falls below `η/i`, which is what makes early Newton steps cheap. `0.0`
    /// disables the rule, leaving only `schur_cg_tolerance` and the iteration
    /// cap — ask for that when you want an exact linear solve.
    ///
    /// Default: 0.1
    pub schur_cg_q_tolerance: f64,

    /// Deprecated: never read. Visualization goes through the observer pattern.
    ///
    /// Setting this has no effect. Attach an observer to the solver instead:
    /// `solver.add_observer(RerunObserver::new(true)?)`, which is how
    /// Levenberg-Marquardt has always done it and works for every optimizer.
    ///
    /// Default: false
    #[cfg(feature = "visualization")]
    #[deprecated(
        since = "1.6.0",
        note = "never read; attach a `RerunObserver` with `add_observer` instead"
    )]
    pub enable_visualization: bool,
}

impl Default for GaussNewtonConfig {
    fn default() -> Self {
        Self {
            linear_solver_type: linalg::LinearSolverType::default(),
            // Ceres Solver default: 50 (changed from 100 for compatibility)
            max_iterations: 50,
            // Ceres Solver default: 1e-6 (changed from 1e-8 for compatibility)
            cost_tolerance: 1e-6,
            // Ceres Solver default: 1e-8 (unchanged)
            parameter_tolerance: 1e-8,
            // Ceres Solver default: 1e-10 (changed from 1e-8 for compatibility)
            gradient_tolerance: 1e-10,
            timeout: None,
            use_jacobi_scaling: false,
            min_diagonal: 1e-10,
            // New Ceres-compatible termination parameters
            min_cost_threshold: None,
            max_condition_number: None,
            compute_covariances: false,
            covariance_options: CovarianceOptions::default(),
            schur_variant: ExplicitSchurVariant::default(),
            schur_preconditioner: SchurPreconditioner::default(),
            schur_cg_max_iterations: 200,
            schur_cg_tolerance: 1e-6,
            schur_cg_q_tolerance: crate::linalg::schur::DEFAULT_ETA,
            #[cfg(feature = "visualization")]
            #[allow(deprecated)]
            enable_visualization: false,
        }
    }
}

impl GaussNewtonConfig {
    /// Create a new Gauss-Newton configuration with default values.
    pub fn new() -> Self {
        Self::default()
    }

    /// Set the linear solver type
    pub fn with_linear_solver_type(mut self, linear_solver_type: linalg::LinearSolverType) -> Self {
        self.linear_solver_type = linear_solver_type;
        self
    }

    /// Set the maximum number of iterations
    pub fn with_max_iterations(mut self, max_iterations: usize) -> Self {
        self.max_iterations = max_iterations;
        self
    }

    /// Set the cost tolerance
    pub fn with_cost_tolerance(mut self, cost_tolerance: f64) -> Self {
        self.cost_tolerance = cost_tolerance;
        self
    }

    /// Set the parameter tolerance
    pub fn with_parameter_tolerance(mut self, parameter_tolerance: f64) -> Self {
        self.parameter_tolerance = parameter_tolerance;
        self
    }

    /// Set the gradient tolerance
    pub fn with_gradient_tolerance(mut self, gradient_tolerance: f64) -> Self {
        self.gradient_tolerance = gradient_tolerance;
        self
    }

    /// Set the timeout duration
    pub fn with_timeout(mut self, timeout: time::Duration) -> Self {
        self.timeout = Some(timeout);
        self
    }

    /// Enable or disable Jacobi column scaling (preconditioning).
    ///
    /// When enabled, normalizes Jacobian columns by their L2 norm before solving.
    /// Can improve convergence for mixed-scale problems but adds ~5-10% overhead.
    pub fn with_jacobi_scaling(mut self, use_jacobi_scaling: bool) -> Self {
        self.use_jacobi_scaling = use_jacobi_scaling;
        self
    }

    /// Set the minimum diagonal regularization for numerical stability.
    ///
    /// A small value (e.g., 1e-10) ensures J^T·J is positive definite while
    /// maintaining the fast convergence of pure Gauss-Newton.
    pub fn with_min_diagonal(mut self, min_diagonal: f64) -> Self {
        self.min_diagonal = min_diagonal;
        self
    }

    /// Set minimum objective function cutoff for early termination.
    ///
    /// When set, optimization terminates with MinCostThresholdReached status
    /// if the cost falls below this threshold. Useful for early stopping when
    /// a "good enough" solution is acceptable.
    pub fn with_min_cost_threshold(mut self, min_cost: f64) -> Self {
        self.min_cost_threshold = Some(min_cost);
        self
    }

    /// Set maximum condition number for Jacobian matrix.
    ///
    /// If set, the optimizer checks if condition_number(J^T*J) exceeds this
    /// threshold and terminates with IllConditionedJacobian status.
    /// Note: Computing condition number is expensive, disabled by default.
    pub fn with_max_condition_number(mut self, max_cond: f64) -> Self {
        self.max_condition_number = Some(max_cond);
        self
    }

    /// Enable or disable covariance computation (uncertainty estimation).
    ///
    /// When enabled, computes the full covariance matrix by inverting the Hessian
    /// after convergence, then extracts per-variable covariance blocks.
    pub fn with_compute_covariances(mut self, compute_covariances: bool) -> Self {
        self.compute_covariances = compute_covariances;
        self
    }

    /// Set the options used for covariance estimation.
    ///
    /// Controls scaled (`σ̂²·H⁻¹`) versus unscaled (`H⁻¹`) covariance, the
    /// factorization algorithm (sparse Cholesky or dense SVD pseudo-inverse),
    /// and the singular-value cutoff. Only takes effect when
    /// `compute_covariances` is enabled.
    pub fn with_covariance_options(mut self, options: CovarianceOptions) -> Self {
        self.covariance_options = options;
        self
    }

    /// Set the [`ExplicitSparseSchur`] sub-variant.
    ///
    /// Takes effect when `linear_solver_type` is
    /// [`LinearSolverType::ExplicitSparseSchur`].
    pub fn with_schur_variant(mut self, variant: ExplicitSchurVariant) -> Self {
        self.schur_variant = variant;
        self
    }

    /// Set Schur complement preconditioner for the iterative variants
    pub fn with_schur_preconditioner(mut self, preconditioner: SchurPreconditioner) -> Self {
        self.schur_preconditioner = preconditioner;
        self
    }

    /// PCG budget for the iterative Schur variants.
    pub fn with_schur_cg_params(mut self, max_iterations: usize, tolerance: f64) -> Self {
        self.schur_cg_max_iterations = max_iterations;
        self.schur_cg_tolerance = tolerance;
        self
    }

    /// Set the PCG forcing-sequence parameter η. `0.0` disables the
    /// quadratic-model stopping rule; see [`Self::schur_cg_q_tolerance`].
    pub fn with_schur_cg_q_tolerance(mut self, q_tolerance: f64) -> Self {
        self.schur_cg_q_tolerance = q_tolerance;
        self
    }

    /// Deprecated: no-op. See [`enable_visualization`](Self::enable_visualization).
    #[cfg(feature = "visualization")]
    #[deprecated(
        since = "1.6.0",
        note = "no-op; attach a `RerunObserver` with `add_observer` instead"
    )]
    #[allow(deprecated)]
    pub fn with_visualization(mut self, enable: bool) -> Self {
        self.enable_visualization = enable;
        self
    }

    /// Print configuration parameters (info level logging)
    pub fn print_configuration(&self) {
        debug!(
            "\nConfiguration:\n  Solver:        Gauss-Newton\n  Linear solver: {:?}\n  Schur variant: {:?}\n  Schur preconditioner: {:?}\n  Schur CG budget:     {} iters @ {:.2e}\n  Convergence Criteria:\n  Max iterations:      {}\n  Cost tolerance:      {:.2e}\n  Parameter tolerance: {:.2e}\n  Gradient tolerance:  {:.2e}\n  Timeout:             {:?}\n  Numerical Settings:\n  Jacobi scaling:      {}\n  Compute covariances: {}",
            self.linear_solver_type,
            self.schur_variant,
            self.schur_preconditioner,
            self.schur_cg_max_iterations,
            self.schur_cg_tolerance,
            self.max_iterations,
            self.cost_tolerance,
            self.parameter_tolerance,
            self.gradient_tolerance,
            self.timeout,
            if self.use_jacobi_scaling {
                "enabled"
            } else {
                "disabled"
            },
            if self.compute_covariances {
                "enabled"
            } else {
                "disabled"
            }
        );
    }
}

/// Result from step computation
struct StepResult {
    step: faer::Mat<f64>,
    gradient_norm: f64,
}

/// Result from cost evaluation
struct CostEvaluation {
    new_cost: f64,
    cost_reduction: f64,
}

/// Gauss-Newton solver for nonlinear least squares optimization.
///
/// Implements the classical Gauss-Newton algorithm which solves `J^T·J·h = -J^T·r` at each
/// iteration to find the step `h`. This provides fast quadratic convergence near the solution
/// but may diverge for poor initial guesses or ill-conditioned problems.
///
/// # Algorithm
///
/// At each iteration k:
/// 1. Compute residual `r(xₖ)` and Jacobian `J(xₖ)`
/// 2. Form normal equations: `(J^T·J)·h = -J^T·r`
/// 3. Solve for step `h` using Cholesky or QR factorization
/// 4. Update parameters: `xₖ₊₁ = xₖ ⊕ h` (manifold plus operation)
/// 5. Check convergence criteria
///
/// # Examples
///
/// ```no_run
/// use apex_solver::optimizer::GaussNewton;
/// use apex_solver::core::problem::Problem;
/// use apex_solver::JacobianMode;
///
/// # type TestResult = Result<(), Box<dyn std::error::Error>>;
/// # fn main() -> TestResult {
/// let mut problem = Problem::new(JacobianMode::Sparse);
/// // ... add factors to problem ...
///
/// let mut solver = GaussNewton::new();
/// let result = solver.optimize(&mut problem)?;
/// # Ok(())
/// # }
/// ```
///
/// # See Also
///
/// - [`GaussNewtonConfig`] - Configuration options
/// - [`LevenbergMarquardt`](crate::optimizer::LevenbergMarquardt) - For adaptive damping
/// - [`DogLeg`](crate::optimizer::DogLeg) - For trust region control
pub struct GaussNewton {
    config: GaussNewtonConfig,
    jacobi_scaling: Option<Vec<f64>>,
    observers: optimizer::OptObserverVec,
}

impl Default for GaussNewton {
    fn default() -> Self {
        Self::new()
    }
}

impl GaussNewton {
    /// Create a new Gauss-Newton solver with default configuration.
    pub fn new() -> Self {
        Self::with_config(GaussNewtonConfig::default())
    }

    /// Create a new Gauss-Newton solver with the given configuration.
    pub fn with_config(config: GaussNewtonConfig) -> Self {
        Self {
            config,
            jacobi_scaling: None,
            observers: optimizer::OptObserverVec::new(),
        }
    }

    /// Add an observer to the solver.
    ///
    /// Observers are notified at each iteration with the current variable values.
    /// This enables real-time visualization, logging, metrics collection, etc.
    ///
    /// # Examples
    ///
    /// ```no_run
    /// use apex_solver::optimizer::GaussNewton;
    /// # use apex_solver::optimizer::OptObserver;
    /// # use apex_solver::core::VarKey;
    /// # use apex_solver::core::variable::ManifoldVariable;
    /// # use slotmap::SlotMap;
    ///
    /// # struct MyObserver;
    /// # impl OptObserver for MyObserver {
    /// #     fn on_step(&self, _: &SlotMap<VarKey, Box<dyn ManifoldVariable>>, _: usize) {}
    /// # }
    /// let mut solver = GaussNewton::new();
    /// solver.add_observer(MyObserver);
    /// ```
    pub fn add_observer(&mut self, observer: impl optimizer::OptObserver + 'static) {
        self.observers.add(observer);
    }

    /// Compute Gauss-Newton step by solving the normal equations (generic over assembly mode).
    fn compute_step_generic<M: AssemblyBackend>(
        &self,
        residuals: &faer::Mat<f64>,
        scaled_jacobian: &M::Jacobian,
        linear_solver: &mut dyn LinearSolver<M>,
    ) -> Result<StepResult, optimizer::OptimizerError> {
        // Solve the Gauss-Newton equation: (JᵀJ + min_diagonal·I)·Δx = −Jᵀr.
        //
        // The regularizer is uniform λI, not the Marquardt diagonal: it is a
        // guard against an exactly singular JᵀJ, not a trust region, and at the
        // default of 1e-10 it is numerically invisible next to any real
        // curvature. `min_diagonal = 0` selects the un-regularized normal
        // equations, i.e. textbook Gauss-Newton.
        let scaled_step = if self.config.min_diagonal > 0.0 {
            let damping = Damping::identity(self.config.min_diagonal);
            linear_solver.solve_augmented_equation(residuals, scaled_jacobian, &damping)
        } else {
            linear_solver.solve_normal_equation(residuals, scaled_jacobian)
        }
        .map_err(|e| {
            optimizer::OptimizerError::LinearSolveFailed(e.to_string()).log_with_source(e)
        })?;

        // Get gradient from the solver (J^T * r) — scaled when Jacobi scaling
        // is enabled, since `scaled_jacobian` is what the solver last saw.
        let gradient = linear_solver.get_gradient().ok_or_else(|| {
            optimizer::OptimizerError::NumericalInstability("Gradient not available".into()).log()
        })?;

        // Apply inverse Jacobi scaling to get final step (if enabled), and
        // report `gradient_tolerance`'s convergence check against the same
        // un-scaled gradient in both cases (GitHub #57).
        let (step, gradient_norm) = if self.config.use_jacobi_scaling {
            let scaling = self
                .jacobi_scaling
                .as_ref()
                .ok_or_else(|| optimizer::OptimizerError::JacobiScalingNotInitialized.log())?;
            (
                M::apply_inverse_scaling(&scaled_step, scaling),
                optimizer::unscaled_gradient_norm(gradient, scaling),
            )
        } else {
            (scaled_step, gradient.norm_l2())
        };

        Ok(StepResult {
            step,
            gradient_norm,
        })
    }

    /// Apply step to parameters and evaluate new cost
    fn apply_step_and_evaluate_cost(
        &self,
        step_result: &StepResult,
        state: &mut optimizer::InitializedState,
        problem: &problem::Problem,
    ) -> error::ApexSolverResult<CostEvaluation> {
        // Apply parameter updates using manifold operations
        let _step_norm = optimizer::apply_parameter_step(
            &mut state.variables,
            step_result.step.as_ref(),
            &state.sorted_vars,
        );

        // Compute new cost (residual only, no Jacobian needed for step evaluation).
        // Reuses the solve workspace instead of allocating one per iteration,
        // like Levenberg-Marquardt does.
        let (_new_residual, new_cost) = problem.compute_residual_and_cost_sparse_with_workspace(
            &state.variables,
            &mut state.workspace,
        )?;

        // Compute cost reduction
        let cost_reduction = state.current_cost - new_cost;

        // Update current cost
        state.current_cost = new_cost;

        Ok(CostEvaluation {
            new_cost,
            cost_reduction,
        })
    }

    /// Run optimization using the specified assembly mode and linear solver.
    ///
    /// `state` is built once by [`Self::optimize`] and handed over here —
    /// see `LevenbergMarquardt::optimize_with_mode` for why: the Schur path
    /// needs the variable index map to size its block structure, and rebuilding
    /// it here would repeat the symbolic construction and cost evaluation.
    fn optimize_with_mode<M: AssemblyBackend>(
        &mut self,
        problem: &mut problem::Problem,
        linear_solver: &mut dyn LinearSolver<M>,
        mut state: optimizer::InitializedState,
    ) -> optimizer::OptimizeResult {
        let start_time = time::Instant::now();
        let mut iteration = 0;
        let mut cost_evaluations = 1; // Initial cost evaluation
        let mut jacobian_evaluations = 0;

        // Initialize summary tracking variables
        let mut max_gradient_norm: f64 = 0.0;
        let mut max_parameter_update_norm: f64 = 0.0;
        let mut total_cost_reduction = 0.0;
        let mut final_gradient_norm;
        let mut final_parameter_update_norm;

        // Initialize iteration statistics tracking
        let mut iteration_stats = Vec::with_capacity(self.config.max_iterations);
        let mut previous_cost = state.current_cost;

        // Print configuration and header if debug level is enabled
        if tracing::enabled!(tracing::Level::DEBUG) {
            self.config.print_configuration();
            IterationStats::print_header();
        }

        // Main optimization loop
        loop {
            let iter_start = time::Instant::now();

            // Shared preamble: assemble, conditioning check, Jacobi scaling
            let (residuals, scaled_jacobian) = match optimizer::iteration_preamble::<M>(
                problem,
                &mut state,
                &mut self.jacobi_scaling,
                self.config.use_jacobi_scaling,
                iteration,
                self.config.max_condition_number,
                &mut jacobian_evaluations,
            )? {
                optimizer::IterationPreamble::Proceed {
                    residuals,
                    scaled_jacobian,
                } => (residuals, scaled_jacobian),
                optimizer::IterationPreamble::EarlyExit(status) => {
                    let elapsed = start_time.elapsed();
                    self.observers.notify_complete(&state.variables, iteration);
                    return Ok(optimizer::build_solver_result(
                        status,
                        iteration,
                        state,
                        elapsed,
                        0.0,
                        0.0,
                        cost_evaluations,
                        jacobian_evaluations,
                        None,
                    ));
                }
            };

            // Compute Gauss-Newton step
            let step_result =
                self.compute_step_generic::<M>(&residuals, &scaled_jacobian, linear_solver)?;

            // Update tracking variables
            max_gradient_norm = max_gradient_norm.max(step_result.gradient_norm);
            final_gradient_norm = step_result.gradient_norm;
            let step_norm = step_result.step.norm_l2();
            max_parameter_update_norm = max_parameter_update_norm.max(step_norm);
            final_parameter_update_norm = step_norm;

            // Capture cost before applying step (for convergence check)
            let cost_before_step = state.current_cost;

            // Apply step and evaluate new cost
            let cost_eval = self.apply_step_and_evaluate_cost(&step_result, &mut state, problem)?;
            cost_evaluations += 1;
            total_cost_reduction += cost_eval.cost_reduction;

            // OPTIMIZATION: Only collect iteration statistics if debug level is enabled
            if tracing::enabled!(tracing::Level::DEBUG) {
                let iter_elapsed_ms = iter_start.elapsed().as_secs_f64() * 1000.0;
                let total_elapsed_ms = start_time.elapsed().as_secs_f64() * 1000.0;

                let stats = IterationStats {
                    iteration,
                    cost: state.current_cost,
                    cost_change: previous_cost - state.current_cost,
                    gradient_norm: step_result.gradient_norm,
                    step_norm,
                    tr_ratio: 0.0,  // Not used in Gauss-Newton
                    tr_radius: 0.0, // Not used in Gauss-Newton
                    ls_iter: 0,     // Direct solver (Cholesky) has no iterations
                    iter_time_ms: iter_elapsed_ms,
                    total_time_ms: total_elapsed_ms,
                    accepted: true, // Gauss-Newton always accepts steps
                };

                iteration_stats.push(stats.clone());
                stats.print_line();
            }

            previous_cost = state.current_cost;

            // Notify all observers with current state
            optimizer::notify_observers_generic::<M>(
                &mut self.observers,
                &state.variables,
                iteration,
                state.current_cost,
                step_result.gradient_norm,
                None, // Gauss-Newton doesn't use damping
                step_norm,
                None, // Gauss-Newton doesn't use step quality
                linear_solver,
            );

            // Compute parameter norm for convergence check
            let parameter_norm = optimizer::compute_parameter_norm(&state.variables);

            // Check convergence using comprehensive termination criteria
            let elapsed = start_time.elapsed();
            if let Some(status) = optimizer::check_convergence(&optimizer::ConvergenceParams {
                iteration,
                current_cost: cost_before_step,
                new_cost: cost_eval.new_cost,
                parameter_norm,
                parameter_update_norm: step_norm,
                gradient_norm: step_result.gradient_norm,
                elapsed,
                step_accepted: true, // GN always accepts
                max_iterations: self.config.max_iterations,
                gradient_tolerance: self.config.gradient_tolerance,
                parameter_tolerance: self.config.parameter_tolerance,
                cost_tolerance: self.config.cost_tolerance,
                min_cost_threshold: self.config.min_cost_threshold,
                timeout: self.config.timeout,
                trust_region_radius: None,
                min_trust_region_radius: None,
            }) {
                // Print summary only if debug level is enabled
                if tracing::enabled!(tracing::Level::DEBUG) {
                    let summary = optimizer::create_optimizer_summary(
                        "Gauss-Newton",
                        state.initial_cost,
                        state.current_cost,
                        iteration + 1,
                        None,
                        None,
                        max_gradient_norm,
                        final_gradient_norm,
                        max_parameter_update_norm,
                        final_parameter_update_norm,
                        total_cost_reduction,
                        elapsed,
                        iteration_stats.clone(),
                        status.clone(),
                        None,
                        None,
                        None,
                    );
                    debug!("{}", summary);
                }

                // Compute covariances if enabled
                let covariances = if self.config.compute_covariances {
                    problem.compute_and_set_covariances(
                        &mut state.variables,
                        self.config.covariance_options,
                    )
                } else {
                    None
                };

                return Ok(optimizer::build_solver_result(
                    status,
                    iteration + 1,
                    state,
                    elapsed,
                    final_gradient_norm,
                    final_parameter_update_norm,
                    cost_evaluations,
                    jacobian_evaluations,
                    covariances,
                ));
            }

            iteration += 1;
        }
    }

    /// Run optimization, automatically selecting sparse or dense path based on config.
    ///
    /// Only solver/mode combinations that match are dispatched; anything else
    /// returns an error rather than silently substituting a different solver.
    /// `ExplicitSparseSchur`/`ImplicitSparseSchur` group residual rows by
    /// eliminated variable and initialize the Schur block structure from the
    /// shared initial state, exactly like Levenberg-Marquardt.
    pub fn optimize(&mut self, problem: &mut problem::Problem) -> optimizer::OptimizeResult {
        // Chunk-wise elimination sweeps rows in chunk order, so each eliminated
        // variable's rows must be adjacent. Regroup before any structure is
        // built from the current layout — row order is a labelling, so this
        // leaves the problem and its cost unchanged.
        if matches!(
            self.config.linear_solver_type,
            LinearSolverType::ExplicitSparseSchur | LinearSolverType::ImplicitSparseSchur
        ) {
            problem.group_rows_for_elimination();
        }

        // Built once here and moved into the chosen arm (see
        // `optimize_with_mode`).
        let state = optimizer::initialize_optimization_state(problem)?;

        match problem.jacobian_mode {
            JacobianMode::Dense => match self.config.linear_solver_type {
                LinearSolverType::DenseQR => {
                    let mut solver = DenseQRSolver::new();
                    self.optimize_with_mode::<DenseMode>(problem, &mut solver, state)
                }
                LinearSolverType::DenseCholesky => {
                    let mut solver = DenseCholeskySolver::new();
                    self.optimize_with_mode::<DenseMode>(problem, &mut solver, state)
                }
                LinearSolverType::ExplicitDenseSchur => {
                    let init = |e: linalg::LinAlgError| {
                        optimizer::OptimizerError::LinearSolveFailed(format!(
                            "Failed to initialize Schur solver: {e}"
                        ))
                        .log()
                    };
                    let mut solver = ExplicitDenseSchur::new();
                    solver
                        .initialize_structure(
                            &state.variables,
                            &state.variable_index_map,
                            &problem.schur_landmark_keys,
                        )
                        .map_err(init)?;
                    self.optimize_with_mode::<DenseMode>(problem, &mut solver, state)
                }
                other => Err(optimizer::OptimizerError::InvalidParameters(format!(
                    "Gauss-Newton in dense Jacobian mode supports DenseCholesky, DenseQR and \
                     ExplicitDenseSchur only; requested {other}"
                ))
                .into()),
            },
            JacobianMode::Sparse => match self.config.linear_solver_type {
                linalg::LinearSolverType::SparseQR => {
                    let mut solver = SparseQRSolver::new();
                    self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
                }
                linalg::LinearSolverType::SparseCholesky => {
                    let mut solver = SparseCholeskySolver::new();
                    self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
                }
                LinearSolverType::ExplicitSparseSchur => {
                    let init = |e: linalg::LinAlgError| {
                        optimizer::OptimizerError::LinearSolveFailed(format!(
                            "Failed to initialize Schur solver: {e}"
                        ))
                        .log()
                    };
                    let mut solver = ExplicitSparseSchur::new()
                        .with_variant(self.config.schur_variant)
                        .with_preconditioner(self.config.schur_preconditioner)
                        .with_cg_params(
                            self.config.schur_cg_max_iterations,
                            self.config.schur_cg_tolerance,
                        )
                        .with_cg_q_tolerance(self.config.schur_cg_q_tolerance);
                    solver
                        .initialize_structure(
                            &state.variables,
                            &state.variable_index_map,
                            &problem.schur_landmark_keys,
                        )
                        .map_err(init)?;
                    self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
                }
                LinearSolverType::ImplicitSparseSchur => {
                    // Never forms S, so it is a different solver type rather
                    // than a mode of the explicit one — this is what keeps
                    // both the O(kept_dof²) buffer and `JᵀJ` itself out of
                    // the picture entirely.
                    let init = |e: linalg::LinAlgError| {
                        optimizer::OptimizerError::LinearSolveFailed(format!(
                            "Failed to initialize Schur solver: {e}"
                        ))
                        .log()
                    };
                    let mut solver = ImplicitSparseSchur::with_config(
                        self.config.schur_cg_max_iterations,
                        self.config.schur_cg_tolerance,
                        self.config.schur_preconditioner,
                    )
                    .with_cg_q_tolerance(self.config.schur_cg_q_tolerance);
                    solver
                        .initialize_structure(
                            &state.variables,
                            &state.variable_index_map,
                            &problem.schur_landmark_keys,
                        )
                        .map_err(init)?;
                    self.optimize_with_mode::<SparseMode>(problem, &mut solver, state)
                }
                other => Err(optimizer::OptimizerError::InvalidParameters(format!(
                    "Gauss-Newton in sparse Jacobian mode supports SparseCholesky, SparseQR, \
                     ExplicitSparseSchur and ImplicitSparseSchur only; requested {other}"
                ))
                .into()),
            },
        }
    }
}

impl optimizer::Optimizer for GaussNewton {
    fn optimize(&mut self, problem: &mut problem::Problem) -> optimizer::OptimizeResult {
        self.optimize(problem)
    }
}

#[cfg(test)]
mod tests {
    use super::{GaussNewton, GaussNewtonConfig};
    use crate::{core::problem, factors, linalg::JacobianMode, optimizer};
    use apex_manifolds as manifold;
    use faer::prelude::ReborrowMut;
    use nalgebra::dvector;

    type TestResult = Result<(), Box<dyn std::error::Error>>;

    /// Custom Rosenbrock Factor 1: r1 = 10(x2 - x1²)
    /// Demonstrates extensibility - custom factors can be defined outside of factors.rs
    #[derive(Debug, Clone)]
    struct RosenbrockFactor1;

    impl factors::Factor for RosenbrockFactor1 {
        fn linearize(
            &self,
            params: &[&[f64]],
            residual: &mut [f64],
            jacobian: Option<faer::mat::MatMut<'_, f64>>,
        ) {
            let x1 = params[0][0];
            let x2 = params[1][0];
            residual[0] = 10.0 * (x2 - x1 * x1);
            if let Some(mut jac) = jacobian {
                *jac.rb_mut().get_mut(0, 0) = -20.0 * x1;
                *jac.rb_mut().get_mut(0, 1) = 10.0;
            }
        }
        fn residual_dim(&self) -> usize {
            1
        }
        fn jacobian_shape(&self) -> (usize, usize) {
            (1, 2)
        }
    }

    /// Custom Rosenbrock Factor 2: r2 = 1 - x1
    /// Demonstrates extensibility - custom factors can be defined outside of factors.rs
    #[derive(Debug, Clone)]
    struct RosenbrockFactor2;

    impl factors::Factor for RosenbrockFactor2 {
        fn linearize(
            &self,
            params: &[&[f64]],
            residual: &mut [f64],
            jacobian: Option<faer::mat::MatMut<'_, f64>>,
        ) {
            residual[0] = 1.0 - params[0][0];
            if let Some(mut jac) = jacobian {
                *jac.rb_mut().get_mut(0, 0) = -1.0;
            }
        }
        fn residual_dim(&self) -> usize {
            1
        }
        fn jacobian_shape(&self) -> (usize, usize) {
            (1, 1)
        }
    }

    #[test]
    fn test_rosenbrock_optimization() -> TestResult {
        // Rosenbrock function test:
        // Minimize: r1² + r2² where
        //   r1 = 10(x2 - x1²)
        //   r2 = 1 - x1
        // Starting point: [-1.2, 1.0]
        // Expected minimum: [1.0, 1.0]

        let mut problem = problem::Problem::new(JacobianMode::Sparse);
        let x1 = problem.add_variable(manifold::ManifoldType::RN, dvector![-1.2]);
        let x2 = problem.add_variable(manifold::ManifoldType::RN, dvector![1.0]);

        // Add custom factors (demonstrates extensibility!)
        problem.add_residual_block(&[x1, x2], Box::new(RosenbrockFactor1), None);
        problem.add_residual_block(&[x1], Box::new(RosenbrockFactor2), None);

        // Configure Gauss-Newton optimizer
        let config = GaussNewtonConfig::new()
            .with_max_iterations(100)
            .with_cost_tolerance(1e-8)
            .with_parameter_tolerance(1e-8)
            .with_gradient_tolerance(1e-10);

        let mut solver = optimizer::GaussNewton::with_config(config);
        let result = solver.optimize(&mut problem)?;

        // Extract final values
        let x1_final = result.parameters[x1].as_param_slice()[0];
        let x2_final = result.parameters[x2].as_param_slice()[0];

        // Verify convergence to [1.0, 1.0]
        assert!(
            matches!(
                result.status,
                optimizer::OptimizationStatus::Converged
                    | optimizer::OptimizationStatus::CostToleranceReached
                    | optimizer::OptimizationStatus::ParameterToleranceReached
                    | optimizer::OptimizationStatus::GradientToleranceReached
            ),
            "Optimization should converge"
        );
        assert!(
            (x1_final - 1.0).abs() < 1e-4,
            "x1 should converge to 1.0, got {}",
            x1_final
        );
        assert!(
            (x2_final - 1.0).abs() < 1e-4,
            "x2 should converge to 1.0, got {}",
            x2_final
        );
        assert!(
            result.final_cost < 1e-6,
            "Final cost should be near zero, got {}",
            result.final_cost
        );
        Ok(())
    }

    /// Trivial factor: r = x - target, J = [[1.0]]
    struct LinearFactor {
        target: f64,
    }

    impl factors::Factor for LinearFactor {
        fn linearize(
            &self,
            params: &[&[f64]],
            residual: &mut [f64],
            jacobian: Option<faer::mat::MatMut<'_, f64>>,
        ) {
            residual[0] = params[0][0] - self.target;
            if let Some(mut jac) = jacobian {
                *jac.rb_mut().get_mut(0, 0) = 1.0;
            }
        }
        fn residual_dim(&self) -> usize {
            1
        }
        fn jacobian_shape(&self) -> (usize, usize) {
            (1, 1)
        }
    }

    fn rosenbrock_problem() -> problem::Problem {
        let mut prob = problem::Problem::new(JacobianMode::Sparse);
        let x1 = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![-1.2]);
        let x2 = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![1.0]);
        prob.add_residual_block(&[x1, x2], Box::new(RosenbrockFactor1), None);
        prob.add_residual_block(&[x1], Box::new(RosenbrockFactor2), None);
        prob
    }

    fn linear_problem(start: f64) -> problem::Problem {
        let mut prob = problem::Problem::new(JacobianMode::Sparse);
        let x = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![start]);
        prob.add_residual_block(&[x], Box::new(LinearFactor { target: 0.0 }), None);
        prob
    }

    // -------------------------------------------------------------------------
    // Ceres-compatibility config fields
    // -------------------------------------------------------------------------

    /// A rank-deficient `JᵀJ` is solvable with the regularizer and not without.
    ///
    /// Two variables enter the residual only through their sum, so `JᵀJ` is
    /// exactly singular: the un-regularized normal equations have no unique
    /// solution. `min_diagonal` is what the doc-comment always promised and what
    /// the solver never applied.
    #[test]
    fn gauss_newton_min_diagonal_regularizes_a_singular_hessian() -> TestResult {
        /// r = (x + y) - 1, so ∂r/∂x = ∂r/∂y = 1 and JᵀJ = [[1,1],[1,1]].
        #[derive(Debug)]
        struct SumFactor;
        impl crate::factors::Factor for SumFactor {
            fn linearize(
                &self,
                params: &[&[f64]],
                residual: &mut [f64],
                jacobian: Option<faer::mat::MatMut<'_, f64>>,
            ) {
                residual[0] = params[0][0] + params[1][0] - 1.0;
                if let Some(mut jac) = jacobian {
                    use faer::prelude::ReborrowMut;
                    *jac.rb_mut().get_mut(0, 0) = 1.0;
                    *jac.rb_mut().get_mut(0, 1) = 1.0;
                }
            }
            fn residual_dim(&self) -> usize {
                1
            }
            fn jacobian_shape(&self) -> (usize, usize) {
                (1, 2)
            }
        }

        let build = || {
            let mut prob = problem::Problem::new(JacobianMode::Sparse);
            let x = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![0.0]);
            let y = prob.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![0.0]);
            prob.add_residual_block(&[x, y], Box::new(SumFactor), None);
            prob
        };

        let mut regularized_problem = build();
        let regularized = GaussNewton::with_config(
            GaussNewtonConfig::new()
                .with_max_iterations(20)
                .with_min_diagonal(1e-10),
        )
        .optimize(&mut regularized_problem)?;
        assert!(
            regularized.final_cost < 1e-12,
            "with regularization the singular system should still reach the \
             minimum-norm solution, got cost {:.3e}",
            regularized.final_cost
        );

        let mut unregularized_problem = build();
        let unregularized = GaussNewton::with_config(
            GaussNewtonConfig::new()
                .with_max_iterations(20)
                .with_min_diagonal(0.0),
        )
        .optimize(&mut unregularized_problem);
        assert!(
            unregularized.is_err(),
            "without regularization the exactly singular JᵀJ should fail to \
             factorize, but the solve returned Ok — min_diagonal is not being read"
        );
        Ok(())
    }

    /// `max_condition_number` detects a variable no residual constrains.
    #[test]
    fn gauss_newton_max_condition_number_detects_an_unconstrained_variable() -> TestResult {
        let mut problem = rosenbrock_problem();
        let _orphan = problem.add_variable(manifold::ManifoldType::RN, nalgebra::dvector![0.0]);

        let result = GaussNewton::with_config(
            GaussNewtonConfig::new()
                .with_max_iterations(10)
                .with_max_condition_number(1e12),
        )
        .optimize(&mut problem)?;

        assert_eq!(
            result.status,
            optimizer::OptimizationStatus::IllConditionedJacobian
        );
        Ok(())
    }

    // -------------------------------------------------------------------------
    // GaussNewtonConfig builder tests
    // -------------------------------------------------------------------------

    #[test]
    fn test_gn_config_default() {
        let cfg = optimizer::gauss_newton::GaussNewtonConfig::default();
        assert_eq!(cfg.max_iterations, 50);
        assert!((cfg.cost_tolerance - 1e-6).abs() < 1e-15);
        assert!(!cfg.use_jacobi_scaling);
        assert!(!cfg.compute_covariances);
    }

    #[test]
    fn test_gn_config_builders() {
        use crate::linalg::LinearSolverType;
        let cfg = GaussNewtonConfig::new()
            .with_max_iterations(15)
            .with_cost_tolerance(1e-5)
            .with_parameter_tolerance(1e-6)
            .with_gradient_tolerance(1e-7)
            .with_jacobi_scaling(true)
            .with_min_diagonal(1e-8)
            .with_min_cost_threshold(1e-10)
            .with_compute_covariances(true)
            .with_linear_solver_type(LinearSolverType::SparseQR);
        assert_eq!(cfg.max_iterations, 15);
        assert!((cfg.cost_tolerance - 1e-5).abs() < 1e-20);
        assert!(cfg.use_jacobi_scaling);
        assert!(cfg.min_cost_threshold.is_some());
        assert!(cfg.compute_covariances);
        assert!(matches!(cfg.linear_solver_type, LinearSolverType::SparseQR));
    }

    #[test]
    fn test_gn_print_configuration_no_panic() {
        optimizer::gauss_newton::GaussNewtonConfig::default().print_configuration();
    }

    #[test]
    fn test_gn_default_equals_new() {
        let _a = optimizer::GaussNewton::new();
        let _b = optimizer::GaussNewton::default();
    }

    #[test]
    fn test_gn_with_config_method() {
        let cfg = GaussNewtonConfig::new().with_max_iterations(3);
        let _solver = GaussNewton::with_config(cfg);
    }

    // -------------------------------------------------------------------------
    // Convergence termination paths
    // -------------------------------------------------------------------------

    #[test]
    fn test_gn_max_iterations_termination() -> TestResult {
        let mut problem = rosenbrock_problem();
        let cfg = GaussNewtonConfig::new().with_max_iterations(2);
        let mut solver = GaussNewton::with_config(cfg);
        let result = solver.optimize(&mut problem)?;
        assert_eq!(
            result.status,
            optimizer::OptimizationStatus::MaxIterationsReached
        );
        Ok(())
    }

    #[test]
    fn test_gn_gradient_tolerance_convergence() -> TestResult {
        let mut problem = linear_problem(1.0);
        let cfg = GaussNewtonConfig::new()
            .with_gradient_tolerance(1e3)
            .with_cost_tolerance(1e-20)
            .with_parameter_tolerance(1e-20);
        let mut solver = GaussNewton::with_config(cfg);
        let result = solver.optimize(&mut problem)?;
        assert_eq!(
            result.status,
            optimizer::OptimizationStatus::GradientToleranceReached
        );
        Ok(())
    }

    #[test]
    fn test_gn_cost_tolerance_convergence() -> TestResult {
        let mut problem = rosenbrock_problem();
        let cfg = GaussNewtonConfig::new()
            .with_cost_tolerance(1e2) // very loose
            .with_gradient_tolerance(1e-20)
            .with_parameter_tolerance(1e-20);
        let mut solver = GaussNewton::with_config(cfg);
        let result = solver.optimize(&mut problem)?;
        assert!(matches!(
            result.status,
            optimizer::OptimizationStatus::CostToleranceReached
                | optimizer::OptimizationStatus::GradientToleranceReached
                | optimizer::OptimizationStatus::ParameterToleranceReached
                | optimizer::OptimizationStatus::Converged
        ));
        Ok(())
    }

    #[test]
    fn test_gn_qr_solver() -> TestResult {
        use crate::linalg::LinearSolverType;
        let mut problem = rosenbrock_problem();
        let cfg = GaussNewtonConfig::new()
            .with_linear_solver_type(LinearSolverType::SparseQR)
            .with_max_iterations(100);
        let mut solver = GaussNewton::with_config(cfg);
        let result = solver.optimize(&mut problem)?;
        assert!(result.final_cost < 1e-6);
        Ok(())
    }

    #[test]
    fn test_gn_jacobi_scaling_enabled() -> TestResult {
        let mut problem = rosenbrock_problem();
        let cfg = GaussNewtonConfig::new()
            .with_jacobi_scaling(true)
            .with_max_iterations(100);
        let mut solver = GaussNewton::with_config(cfg);
        let result = solver.optimize(&mut problem)?;
        assert!(result.final_cost < 1e-6);
        Ok(())
    }

    #[test]
    fn test_gn_min_cost_threshold() -> TestResult {
        let mut problem = rosenbrock_problem();
        let cfg = GaussNewtonConfig::new()
            .with_min_cost_threshold(1e10)
            .with_cost_tolerance(1e-20)
            .with_gradient_tolerance(1e-20)
            .with_parameter_tolerance(1e-20);
        let mut solver = GaussNewton::with_config(cfg);
        let result = solver.optimize(&mut problem)?;
        assert_eq!(
            result.status,
            optimizer::OptimizationStatus::MinCostThresholdReached
        );
        Ok(())
    }

    #[test]
    fn test_gn_result_fields() -> TestResult {
        let mut problem = rosenbrock_problem();
        let mut solver = optimizer::GaussNewton::new();
        let result = solver.optimize(&mut problem)?;
        assert!(result.initial_cost > result.final_cost);
        assert!(result.iterations > 0);
        Ok(())
    }

    #[test]
    fn test_gn_convergence_info_populated() -> TestResult {
        let mut problem = rosenbrock_problem();
        let mut solver = optimizer::GaussNewton::new();
        let result = solver.optimize(&mut problem)?;
        assert!(result.convergence_info.is_some());
        Ok(())
    }

    #[test]
    fn test_gn_timeout_config() {
        let cfg = GaussNewtonConfig::new().with_timeout(std::time::Duration::from_secs(60));
        assert!(cfg.timeout.is_some());
    }
}