//! Powell dogleg trust-region globalization.
//!
//! When Newton-with-line-search stagnates near a local minimum of ‖F‖ — the
//! signature failure of fully-coupled vapor-compression systems with two-phase
//! pressure drops at high EXV opening, where the Newton direction is almost
//! orthogonal to the gradient of ‖F‖ and Armijo backtracking collapses to
//! infinitesimal steps — trust-region globalization escapes by picking the
//! step inside a ball of radius δ that best reduces the linear model
//! m(p) = ½‖r + J·p‖². The classic Powell dogleg blends the Newton step n
//! (J·n = −r) and the Cauchy (steepest-descent) step c inside the trust region;
//! when the Jacobian is singular or ill-conditioned, the step falls back to a
//! Levenberg–Marquardt solve (JᵀJ + λI)·p = −Jᵀr, clipped to the region.
//! The radius δ is grown/shrunk from the gain ratio ρ of actual vs predicted
//! ‖r‖ reduction. The method reuses Entropyk's analytical Jacobian — no finite
//! differences — and the same Ruiz-equilibrated LU path as `NewtonConfig`.
//!
//! References:
//! - Nocedal & Wright, *Numerical Optimization* (2nd ed.), ch. 4.6 (dogleg)
//! - Madsen, Nielsen & Tingleff, *Methods for Non-Linear Least Squares
//!   Problems* (2004) — LM with gain ratio
//! - Dennis & Schnabel, *Numerical Methods for Unconstrained Optimization*
//! - GSL `gsl_multifit_nlinear` hybrid scaled algorithm
//! - gomez crate (MIT): <https://github.com/datamole-ai/gomez>

use std::time::{Duration, Instant};

use crate::jacobian::JacobianMatrix;
use crate::metadata::SimulationMetadata;
use crate::solver::{apply_newton_step, ConvergedState, ConvergenceStatus, Solver, SolverError};
use crate::system::System;
use entropyk_components::JacobianBuilder;
use nalgebra::DVector;

/// Configuration for the Powell dogleg trust-region solver.
///
/// Used as a globalization stage of [`super::FallbackSolver`]: only invoked
/// when the primary Newton/Picard stages fail, so it never perturbs problems
/// that already converge. The defaults mirror the well-tested gomez/GSL
/// parameterization.
#[derive(Debug, Clone, PartialEq)]
pub struct TrustRegionConfig {
    /// Maximum trust-region iterations.
    pub max_iterations: usize,
    /// Convergence tolerance on the residual L2 norm.
    pub tolerance: f64,
    /// Initial trust-region radius δ₀ (in column-scaled variables).
    pub delta_init: f64,
    /// Minimum radius: if δ collapses below this the method has failed.
    pub delta_min: f64,
    /// Maximum radius (effectively unbounded above this).
    pub delta_max: f64,
    /// Gain ratio ρ below which a step is rejected and δ shrinks.
    pub accept_threshold: f64,
    /// ρ below which δ is shrunk even on accepted steps (poor model match).
    pub shrink_threshold: f64,
    /// ρ above which δ is expanded (only if ‖D·p‖ ≈ δ, i.e. the step hit the boundary).
    pub expand_threshold: f64,
    /// δ shrink multiplier (< 1) on rejected steps.
    pub shrink_factor: f64,
    /// δ expand multiplier (> 1) on the boundary with good gain.
    pub expand_factor: f64,
    /// Consecutive rejections before declaring failure at the current point.
    pub max_rejections: usize,
    /// Initial Levenberg–Marquardt λ for the singular-Newton fallback.
    pub lm_lambda_init: f64,
    /// LM λ upper bound (solves become pure gradient descent above this).
    pub lm_lambda_max: f64,
    /// LM λ growth on a rejected LM step.
    pub lm_lambda_grow: f64,
    /// LM λ shrink on an accepted LM step.
    pub lm_lambda_shrink: f64,
    /// Optional initial state (cold start shared with the primary solvers).
    pub initial_state: Option<Vec<f64>>,
    /// Optional timeout.
    pub timeout: Option<Duration>,
}

impl Default for TrustRegionConfig {
    fn default() -> Self {
        Self {
            max_iterations: 200,
            tolerance: 1e-6,
            delta_init: 1.0,
            delta_min: f64::EPSILON.sqrt(), // ≈ 1.5e-8 (gomez)
            delta_max: 1e9,
            accept_threshold: 1e-4,
            shrink_threshold: 0.25,
            expand_threshold: 0.75,
            shrink_factor: 0.5, // gomez / Madsen-Tingleff μ=0.5 on rejection
            expand_factor: 2.0,
            max_rejections: 20,
            lm_lambda_init: 1e-3,
            lm_lambda_max: 1e10,
            lm_lambda_grow: 10.0,
            lm_lambda_shrink: 0.1,
            initial_state: None,
            timeout: None,
        }
    }
}

impl TrustRegionConfig {
    /// L2 residual norm, consistent with the other strategies. Falls back to
    /// `f64::MAX` on non-finite values so the divergence guard fires cleanly
    /// instead of poisoning the gain ratio with NaN.
    fn residual_norm(residuals: &[f64]) -> f64 {
        let norm = residuals.iter().map(|r| r * r).sum::<f64>().sqrt();
        if norm.is_finite() {
            norm
        } else {
            f64::MAX
        }
    }

    /// Column-scaling diagonal `d_j = 1 / ‖col_j(J)‖` (GSL-style). Returns the
    /// vector of `d_j` so the trust-region metric `‖D·p‖` treats every state
    /// variable on the same scale — critical for thermodynamic systems that
    /// mix P (~1e6 Pa), h (~1e5 J/kg) and dimensionless controls (~1) in the
    /// same state vector. Zero columns fall back to `d_j = 1` to avoid div-by-0.
    fn column_scaling(jacobian: &JacobianMatrix) -> Vec<f64> {
        let m = jacobian.as_matrix();
        let ncols = m.ncols();
        let mut d = Vec::with_capacity(ncols);
        for j in 0..ncols {
            let col_norm = m.column(j).norm();
            d.push(if col_norm > f64::EPSILON.sqrt() {
                1.0 / col_norm
            } else {
                1.0
            });
        }
        d
    }

    /// Weighted norm `‖D·v‖` used as the trust-region metric.
    fn scaled_norm(v: &[f64], d: &[f64]) -> f64 {
        let mut acc = 0.0_f64;
        for (vi, di) in v.iter().zip(d.iter()) {
            acc += (vi * di).powi(2);
        }
        acc.sqrt()
    }

    /// Computes the dogleg step blending the Cauchy point `c` and Newton step
    /// `n` so that `‖D·p‖ ≤ δ`. Returns the step in the same coordinates as
    /// the Newton step (i.e. in x-space, not D-scaled).
    ///
    /// Algorithm (Nocedal & Wright, Procedure 4.3, scaled form):
    ///   * If `‖D·n‖ ≤ δ`, return `n` (full Newton step is inside the region).
    ///   * Else if `‖D·c‖ ≥ δ`, clip the Cauchy step: `p = (δ/‖D·c‖)·c`.
    ///   * Else find the unique τ ∈ (0,1] with `‖D·(c + τ(n − c))‖ = δ` by
    ///     solving the quadratic `a·τ² + b·τ + c = 0` and return the linear
    ///     blend on the segment from c to n.
    ///
    /// All inputs are in x-space; only the trust-region *radius* is measured
    /// in D-scaled coordinates.
    #[allow(clippy::too_many_arguments)]
    fn dogleg_step(
        newton: Option<&[f64]>,
        cauchy: &[f64],
        d: &[f64],
        delta: f64,
        buf_dp: &mut [f64],
        buf_dn: &mut [f64],
    ) -> Vec<f64> {
        // Case 1: Newton step valid and inside the region.
        if let Some(n) = newton {
            let norm_dn = {
                for (i, &ni) in n.iter().enumerate() {
                    buf_dn[i] = ni * d[i];
                }
                buf_dn.iter().map(|x| x * x).sum::<f64>().sqrt()
            };
            if norm_dn <= delta {
                return n.to_vec();
            }
        }

        // Case 2: outside Newton — clip Cauchy if needed.
        let norm_dc = {
            for (i, &ci) in cauchy.iter().enumerate() {
                buf_dp[i] = ci * d[i];
            }
            buf_dp.iter().map(|x| x * x).sum::<f64>().sqrt()
        };
        if norm_dc >= delta || newton.is_none() {
            let alpha = if norm_dc > 0.0 { delta / norm_dc } else { 0.0 };
            return cauchy.iter().map(|c| c * alpha).collect();
        }

        // Case 3: dogleg on the segment c → n. Solve the quadratic for τ ≥ 0
        // such that ‖D·(c + τ(n − c))‖² = δ².
        let n = newton.unwrap();
        let mut a = 0.0_f64;
        let mut b = 0.0_f64;
        let mut c_coef = 0.0_f64;
        for j in 0..n.len() {
            let dj = d[j];
            let cj = cauchy[j] * dj;
            let nj = n[j] * dj;
            let mj = nj - cj; // direction of the segment in scaled space
            a += mj * mj;
            b += 2.0 * cj * mj;
            c_coef += cj * cj;
        }
        let rhs = c_coef - delta * delta;
        // Solve a·τ² + b·τ + rhs = 0 for the smallest non-negative root. We
        // want τ ∈ (0,1]: that is the intersection of the segment with the
        // sphere surface. If a is ~0 (n ≈ c numerically) fall back to Cauchy.
        let tau = if a.abs() < f64::EPSILON {
            1.0
        } else {
            let disc = b * b - 4.0 * a * rhs;
            if disc < 0.0 {
                // Quadratic has no real root: the whole segment is inside or
                // outside. Numerically degenerate — fall back to Cauchy clip.
                return cauchy.to_vec();
            }
            let sqrt_disc = disc.sqrt();
            let tau1 = (-b + sqrt_disc) / (2.0 * a);
            let tau2 = (-b - sqrt_disc) / (2.0 * a);
            // Pick the smallest non-negative root in (0,1].
            let mut candidates = [tau1, tau2]
                .iter()
                .copied()
                .filter(|&t| t.is_finite() && t > 0.0 && t <= 1.0 + 1e-12)
                .collect::<Vec<_>>();
            candidates.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
            candidates.first().copied().unwrap_or(1.0).clamp(0.0, 1.0)
        };
        cauchy
            .iter()
            .zip(n.iter())
            .map(|(c, n)| c + tau * (n - c))
            .collect()
    }

    /// Solves the Levenberg–Marquardt normal equations `(JᵀJ + λI)·p = −Jᵀr`
    /// via LU. Returns `None` if the regularized system is still singular.
    fn lm_step(jacobian: &JacobianMatrix, residuals: &[f64], lambda: f64) -> Option<Vec<f64>> {
        let m = jacobian.as_matrix();
        let n = m.ncols();
        let jt = m.transpose();
        let mut regularized = &jt * m;
        // Add λ on the diagonal of the normal-equations matrix. nalgebra
        // exposes `diagonal_mut` only on owned `OMatrix`; we already have one
        // (result of `&jt * m` is owned), so the call works through DMatrix.
        for j in 0..n {
            regularized[(j, j)] += lambda;
        }
        let jtr = DVector::from_iterator(
            n,
            (0..n).map(|j| {
                let col = jt.column(j);
                -(col.dot(&DVector::from_row_slice(residuals)))
            }),
        );
        let lu = regularized.lu();
        lu.solve(&jtr)
            .map(|v| {
                let p: Vec<f64> = v.iter().copied().collect();
                if p.iter().all(|x| x.is_finite()) {
                    Some(p)
                } else {
                    None
                }
            })
            .flatten()
    }
}

impl Solver for TrustRegionConfig {
    fn solve(&mut self, system: &mut System) -> Result<ConvergedState, SolverError> {
        let start_time = Instant::now();

        let n_state = system.full_state_vector_len();
        let n_equations: usize = system
            .traverse_for_jacobian()
            .map(|(_, c, _)| c.n_equations())
            .sum::<usize>()
            + system.constraints().count()
            + system.coupling_residual_count()
            + 2 * system.saturated_controller_count()
            + system.mass_flow_closure_count();

        if n_state == 0 || n_equations == 0 {
            return Err(SolverError::InvalidSystem {
                message: "Empty system has no state variables or equations".to_string(),
            });
        }

        let mut state: Vec<f64> = match self.initial_state.as_ref() {
            Some(s) if s.len() == n_state => s.clone(),
            Some(s) => {
                return Err(SolverError::InvalidSystem {
                    message: format!(
                        "initial_state length {} does not match system state length {}",
                        s.len(),
                        n_state
                    ),
                });
            }
            None => vec![0.0; n_state],
        };
        let mut residuals: Vec<f64> = vec![0.0; n_equations];
        let mut saved_state: Vec<f64> = vec![0.0; n_state];
        let mut trial_state: Vec<f64> = vec![0.0; n_state];
        let mut trial_residuals: Vec<f64> = vec![0.0; n_equations];
        let mut jacobian_builder = JacobianBuilder::new();
        let mut jacobian_matrix = JacobianMatrix::zeros(n_equations, n_state);

        // Scratch buffers for the dogleg computation — sized to n_state.
        let mut buf_dp = vec![0.0; n_state];
        let mut buf_dn = vec![0.0; n_state];

        let clipping_mask: Vec<Option<(f64, f64)>> = (0..n_state)
            .map(|i| system.get_solver_bounds_for_state_index(i))
            .collect();

        system
            .compute_residuals(&state, &mut residuals)
            .map_err(|e| SolverError::InvalidSystem {
                message: format!("Failed to compute initial residuals: {:?}", e),
            })?;
        let mut current_norm = Self::residual_norm(&residuals);

        tracing::info!(
            max_iterations = self.max_iterations,
            tolerance = self.tolerance,
            delta_init = self.delta_init,
            residual_norm = current_norm,
            "Trust-region (Powell dogleg + LM) starting"
        );

        if current_norm < self.tolerance {
            return Ok(ConvergedState::new(
                state,
                0,
                current_norm,
                ConvergenceStatus::Converged,
                SimulationMetadata::new(system.input_hash()),
            ));
        }

        let mut delta = self.delta_init;
        let mut lm_lambda = self.lm_lambda_init;
        let mut consecutive_rejections = 0usize;

        for iteration in 1..=self.max_iterations {
            if let Some(timeout) = self.timeout {
                if start_time.elapsed() > timeout {
                    tracing::info!(iteration, "Trust-region timed out");
                    return Err(SolverError::Timeout {
                        timeout_ms: timeout.as_millis() as u64,
                    });
                }
            }

            // Fresh analytical Jacobian every iteration — the trust-region
            // accepts/rejects based on actual residuals, so a stale Jacobian
            // would mis-predict the gain ratio.
            jacobian_builder.clear();
            system
                .assemble_jacobian(&state, &mut jacobian_builder)
                .map_err(|e| SolverError::InvalidSystem {
                    message: format!("Failed to assemble Jacobian: {:?}", e),
                })?;
            jacobian_matrix.update_from_builder(jacobian_builder.entries());

            // Column scaling diagonal D (constant within an iteration).
            let d = Self::column_scaling(&jacobian_matrix);

            // Gradient of ½‖r‖²: g = Jᵀ·r.
            let m = jacobian_matrix.as_matrix().clone();
            let r_vec = DVector::from_row_slice(&residuals);
            let grad = m.transpose() * &r_vec;

            // ─── Newton step ──────────────────────────────────────────────
            let newton_step = jacobian_matrix.solve(&residuals);

            // ─── Cauchy (steepest-descent) step ───────────────────────────
            // α = (g·g) / (g·JᵀJ·g) = ‖g‖² / ‖J·g‖². For F(x)=0 we use the
            // convention that the descent direction is −g.
            let grad_norm_sq = grad.dot(&grad);
            let j_grad = &m * &grad; // J·g  (n_eq-vector)
            let j_grad_norm_sq = j_grad.dot(&j_grad);
            let cauchy_step: Vec<f64> = if j_grad_norm_sq > f64::EPSILON {
                let alpha = grad_norm_sq / j_grad_norm_sq;
                grad.iter().map(|gi| -alpha * gi).collect()
            } else {
                // Degenerate (gradient ≈ 0): no descent direction available.
                grad.iter().map(|_| 0.0).collect()
            };

            // ─── Pick the trust-region step ───────────────────────────────
            // Try the dogleg first; if the Newton step is missing (singular J),
            // fall back to a Levenberg–Marquardt solve clipped to the region.
            let step = if newton_step.is_some() || j_grad_norm_sq > f64::EPSILON {
                Self::dogleg_step(
                    newton_step.as_deref(),
                    &cauchy_step,
                    &d,
                    delta,
                    &mut buf_dp,
                    &mut buf_dn,
                )
            } else {
                // Both Newton and Cauchy are unusable — LM is the only hope.
                match Self::lm_step(&jacobian_matrix, &residuals, lm_lambda) {
                    Some(p) => {
                        let norm_dp = Self::scaled_norm(&p, &d);
                        if norm_dp > delta && norm_dp > 0.0 {
                            p.iter().map(|pi| pi * (delta / norm_dp)).collect()
                        } else {
                            p
                        }
                    }
                    None => {
                        // Even LM failed: shrink δ and retry without consuming
                        // an iteration of state progress.
                        delta *= self.shrink_factor;
                        if delta < self.delta_min {
                            return Err(SolverError::NonConvergence {
                                iterations: iteration - 1,
                                final_residual: current_norm,
                            });
                        }
                        continue;
                    }
                }
            };

            if step.iter().any(|s| !s.is_finite()) {
                // Poisoned step: shrink and retry.
                delta *= self.shrink_factor;
                consecutive_rejections += 1;
                if delta < self.delta_min || consecutive_rejections > self.max_rejections {
                    return Err(SolverError::NonConvergence {
                        iterations: iteration - 1,
                        final_residual: current_norm,
                    });
                }
                continue;
            }

            // ─── Trial state ──────────────────────────────────────────────
            saved_state.copy_from_slice(&state);
            trial_state.copy_from_slice(&state);
            apply_newton_step(&mut trial_state, &step, &clipping_mask, 1.0);

            if let Err(e) = system.compute_residuals(&trial_state, &mut trial_residuals) {
                if !e.is_recoverable() {
                    // Fatal evaluation error: abort immediately, do not burn
                    // the shrink budget on it.
                    return Err(SolverError::InvalidSystem {
                        message: format!("Failed to compute residuals: {:?}", e),
                    });
                }
                // Recoverable domain violation (KINSOL > 0): reject and shrink.
                delta *= self.shrink_factor;
                consecutive_rejections += 1;
                if delta < self.delta_min || consecutive_rejections > self.max_rejections {
                    return Err(SolverError::NonConvergence {
                        iterations: iteration - 1,
                        final_residual: current_norm,
                    });
                }
                continue;
            }
            let trial_norm = Self::residual_norm(&trial_residuals);

            // ─── Gain ratio ρ ─────────────────────────────────────────────
            //   ρ = (½‖r‖² − ½‖r_trial‖²) / (½‖r‖² − ½‖r + J·p‖²)
            // The denominator is the linear-model reduction; for square systems
            // with an exact Newton step it equals ½‖r‖².
            let actual_reduction = 0.5 * (current_norm * current_norm - trial_norm * trial_norm);
            let predicted_reduction = {
                let j_step = &m * DVector::from_row_slice(&step);
                let predicted: DVector<f64> = &r_vec + &j_step;
                0.5 * (current_norm * current_norm - predicted.norm().powi(2))
            };
            let rho = if predicted_reduction.abs() > f64::EPSILON {
                actual_reduction / predicted_reduction
            } else if actual_reduction > 0.0 {
                // Model and reality both flat-ish; accept progress but do not
                // expand the region (would lead to runaway δ growth).
                1.0
            } else {
                // No predicted reduction and no actual reduction — reject.
                -1.0
            };

            let step_norm_d = Self::scaled_norm(&step, &d);

            if rho >= self.accept_threshold && trial_norm < current_norm {
                // ─── Accept ───────────────────────────────────────────────
                state.copy_from_slice(&trial_state);
                residuals.copy_from_slice(&trial_residuals);
                current_norm = trial_norm;
                consecutive_rejections = 0;

                // Grow δ on a boundary step with good model agreement. On a
                // *very* good gain (ρ > 0.9) we also re-center δ at its init
                // value when it has collapsed — a strong signal that we just
                // escaped a tough spot and can afford bolder steps again.
                if rho > self.expand_threshold
                    && step_norm_d > 0.9 * delta
                    && step_norm_d < 1.1 * delta
                {
                    delta = (delta * self.expand_factor).min(self.delta_max);
                } else if rho > self.shrink_threshold {
                    // Reasonable gain, no shrink. If δ had collapsed and we
                    // just made real progress, lift it back up so subsequent
                    // iterations don't get stuck at infinitesimal steps.
                    if delta < self.delta_init {
                        delta = self.delta_init.min(self.delta_max);
                    }
                } else {
                    // Accepted but the model poorly predicted the reduction —
                    // tighten the region for the next iteration.
                    delta = (delta * self.shrink_factor).max(self.delta_min);
                }

                // LM λ decay on accepted LM steps.
                if newton_step.is_none() {
                    lm_lambda = (lm_lambda * self.lm_lambda_shrink).max(1e-12);
                }

                tracing::debug!(
                    iteration,
                    residual_norm = current_norm,
                    delta,
                    rho,
                    step_norm_d,
                    "Trust-region accepted step"
                );

                if current_norm < self.tolerance {
                    tracing::info!(
                        iterations = iteration,
                        final_residual = current_norm,
                        "Trust-region converged"
                    );
                    return Ok(ConvergedState::new(
                        state,
                        iteration,
                        current_norm,
                        ConvergenceStatus::Converged,
                        SimulationMetadata::new(system.input_hash()),
                    ));
                }
            } else {
                // ─── Reject ───────────────────────────────────────────────
                consecutive_rejections += 1;
                delta = (delta * self.shrink_factor).max(self.delta_min);

                // Grow LM λ on rejected LM steps so the next iteration is more
                // gradient-descent-like.
                if newton_step.is_none() {
                    lm_lambda = (lm_lambda * self.lm_lambda_grow).min(self.lm_lambda_max);
                }

                tracing::debug!(
                    iteration,
                    residual_norm = current_norm,
                    trial_norm,
                    delta,
                    rho,
                    consecutive_rejections,
                    "Trust-region rejected step"
                );

                if delta <= self.delta_min || consecutive_rejections > self.max_rejections {
                    tracing::warn!(
                        iteration,
                        final_residual = current_norm,
                        delta,
                        consecutive_rejections,
                        "Trust-region failed to make progress"
                    );
                    return Err(SolverError::NonConvergence {
                        iterations: iteration - 1,
                        final_residual: current_norm,
                    });
                }
            }
        }

        tracing::warn!(
            max_iterations = self.max_iterations,
            final_residual = current_norm,
            "Trust-region did not converge"
        );
        Err(SolverError::NonConvergence {
            iterations: self.max_iterations,
            final_residual: current_norm,
        })
    }

    fn with_timeout(mut self, timeout: Duration) -> Self {
        self.timeout = Some(timeout);
        self
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::solver::Solver;
    use crate::system::System;

    #[test]
    fn test_trust_region_defaults() {
        let cfg = TrustRegionConfig::default();
        assert_eq!(cfg.max_iterations, 200);
        assert!(cfg.delta_init > 0.0);
        assert!(cfg.delta_min > 0.0 && cfg.delta_min < cfg.delta_init);
        assert!(cfg.delta_max > cfg.delta_init);
        assert!(cfg.shrink_factor > 0.0 && cfg.shrink_factor < 1.0);
        assert!(cfg.expand_factor > 1.0);
        assert!(cfg.accept_threshold > 0.0 && cfg.accept_threshold < 1.0);
    }

    #[test]
    fn test_trust_region_rejects_empty_system() {
        let mut system = System::new();
        system.finalize().unwrap();
        let mut solver = TrustRegionConfig::default();
        let result = solver.solve(&mut system);
        assert!(matches!(result, Err(SolverError::InvalidSystem { .. })));
    }

    /// Smoke check on the dogleg step: when Newton is inside the region the
    /// returned step must equal the Newton step exactly.
    #[test]
    fn test_dogleg_returns_newton_inside_region() {
        // Newton step of norm 0.5 (in D=I), region radius 1.0 → inside.
        let newton = vec![0.3, 0.4]; // ‖n‖ = 0.5
        let cauchy = vec![0.1, 0.1];
        let d = vec![1.0, 1.0];
        let mut buf_dp = vec![0.0; 2];
        let mut buf_dn = vec![0.0; 2];
        let p = TrustRegionConfig::dogleg_step(
            Some(&newton),
            &cauchy,
            &d,
            1.0,
            &mut buf_dp,
            &mut buf_dn,
        );
        assert!((p[0] - 0.3).abs() < 1e-12);
        assert!((p[1] - 0.4).abs() < 1e-12);
    }

    /// When the Newton step is outside the region but the Cauchy point is
    /// inside, the dogleg must lie on the segment c → n at the boundary.
    #[test]
    fn test_dogleg_blends_to_boundary() {
        // Newton ‖n‖ = 10, Cauchy ‖c‖ = 0.1, δ = 1.0 → τ must place the step
        // on the segment c→n with scaled norm 1.
        let newton = vec![10.0, 0.0];
        let cauchy = vec![-0.1, 0.0];
        let d = vec![1.0, 1.0];
        let mut buf_dp = vec![0.0; 2];
        let mut buf_dn = vec![0.0; 2];
        let p = TrustRegionConfig::dogleg_step(
            Some(&newton),
            &cauchy,
            &d,
            1.0,
            &mut buf_dp,
            &mut buf_dn,
        );
        let norm = (p[0].powi(2) + p[1].powi(2)).sqrt();
        // Step must hit the trust-region boundary (within numerical tolerance).
        assert!(
            (norm - 1.0).abs() < 1e-9,
            "dogleg must hit the boundary, got norm = {}",
            norm
        );
        // Step must lie on the segment c → n (x-component between −0.1 and 10).
        assert!(p[0] >= -0.1 && p[0] <= 10.0);
        assert!((p[1]).abs() < 1e-12);
    }

    /// When Newton is missing (singular Jacobian) and Cauchy is outside the
    /// region, the step is the Cauchy direction clipped to the boundary.
    #[test]
    fn test_dogleg_clips_cauchy_when_newton_missing() {
        let cauchy = vec![3.0, 4.0]; // ‖c‖ = 5
        let d = vec![1.0, 1.0];
        let mut buf_dp = vec![0.0; 2];
        let mut buf_dn = vec![0.0; 2];
        let p = TrustRegionConfig::dogleg_step(None, &cauchy, &d, 1.0, &mut buf_dp, &mut buf_dn);
        let norm = (p[0].powi(2) + p[1].powi(2)).sqrt();
        assert!((norm - 1.0).abs() < 1e-12);
        // Same direction as Cauchy.
        assert!(p[0] > 0.0 && p[1] > 0.0);
    }

    /// Column scaling must assign ~1 to near-zero columns and 1/‖col‖ otherwise.
    #[test]
    fn test_column_scaling_handles_zero_and_unit_columns() {
        // J = [[10, 0], [0, 2]] → d = [1/10, 1/2].
        let entries = vec![(0, 0, 10.0), (1, 1, 2.0)];
        let j = JacobianMatrix::from_builder(&entries, 2, 2);
        let d = TrustRegionConfig::column_scaling(&j);
        assert!((d[0] - 0.1).abs() < 1e-12);
        assert!((d[1] - 0.5).abs() < 1e-12);

        // Zero column → d = 1.
        let entries = vec![(0, 0, 5.0), (1, 0, 5.0)]; // col 1 all zeros
        let j = JacobianMatrix::from_builder(&entries, 2, 2);
        let d = TrustRegionConfig::column_scaling(&j);
        assert!((d[1] - 1.0).abs() < 1e-12);
    }
}
