apex-solver 1.5.0

//! Robust loss functions for outlier rejection in nonlinear least squares optimization.
//!
//! Loss functions (also called robust cost functions or M-estimators) reduce the influence of
//! outlier measurements on the optimization result. In standard least squares, the cost is
//! the squared norm of residuals: `cost = Σ ||r_i||²`. With a robust loss function ρ(s), the
//! cost becomes: `cost = Σ ρ(||r_i||²)`.
//!
//! # Mathematical Formulation
//!
//! Each loss function implements the `Loss` trait, which evaluates:
//! - **ρ(s)**: The robust cost value
//! - **ρ'(s)**: First derivative (weight function)
//! - **ρ''(s)**: Second derivative (for corrector algorithm)
//!
//! The input `s = ||r||²` is the squared norm of the residual vector.
//!
//! # Usage in Optimization
//!
//! Loss functions are applied via the `Corrector` algorithm (see `corrector.rs`), which
//! modifies the residuals and Jacobians to account for the robust weighting. The optimization
//! then proceeds as if solving a reweighted least squares problem.
//!
//! # Available Loss Functions
//!
//! ## Basic Loss Functions
//! - [`L2Loss`]: Standard least squares (no robustness)
//! - [`L1Loss`]: Absolute error (simple robust baseline)
//!
//! ## Moderate Robustness
//! - [`HuberLoss`]: Quadratic for inliers, linear for outliers (recommended for general use)
//! - [`FairLoss`]: Smooth transition with continuous derivatives
//! - [`CauchyLoss`]: Heavier suppression of large residuals
//!
//! ## Strong Robustness
//! - [`GemanMcClureLoss`]: Very strong outlier rejection
//! - [`DcsLoss`]: Dynamic covariance scaling for loop closures (g2o-compatible)
//! - [`WelschLoss`]: Exponential downweighting
//! - [`TukeyBiweightLoss`]: Complete outlier suppression (redescending)
//!
//! ## Specialized Functions
//! - [`AndrewsWaveLoss`]: Sine-based redescending M-estimator
//! - [`RamsayEaLoss`]: Exponential decay weighting
//! - [`TrimmedMeanLoss`]: Hard threshold cutoff
//! - [`LpNormLoss`]: Generalized Lp norm (flexible p parameter)
//!
//! ## Modern Adaptive
//! - [`BarronGeneralLoss`]: Unified framework encompassing many loss functions (CVPR 2019)
//!
//! # Loss Function Selection Guide
//!
//! | Use Case | Recommended Loss | Tuning Constant |
//! |----------|-----------------|-----------------|
//! | Clean data, no outliers | `L2Loss` | N/A |
//! | Few outliers (<5%) | `HuberLoss` | c = 1.345 |
//! | Moderate outliers (5-10%) | `FairLoss` or `CauchyLoss` | c = 1.3998 / 2.3849 |
//! | Many outliers (>10%) | `WelschLoss` or `TukeyBiweightLoss` | c = 2.9846 / 4.6851 |
//! | Severe outliers | `GemanMcClureLoss` | c = 1.0-2.0 |
//! | Adaptive/unknown | `BarronGeneralLoss` | α adaptive |
//!
//! # Example
//!
//! ```
//! use apex_solver::core::loss_functions::{LossFunction, HuberLoss};
//! # use apex_solver::core::CoreResult;
//! # fn example() -> CoreResult<()> {
//!
//! let huber = HuberLoss::new(1.345)?;
//!
//! // Evaluate for an inlier (small residual)
//! let s_inlier = 0.5;
//! let [rho, rho_prime, rho_double_prime] = huber.evaluate(s_inlier);
//! assert_eq!(rho, s_inlier); // Quadratic cost in inlier region
//! assert_eq!(rho_prime, 1.0); // Full weight
//!
//! // Evaluate for an outlier (large residual)
//! let s_outlier = 10.0;
//! let [rho, rho_prime, rho_double_prime] = huber.evaluate(s_outlier);
//! // rho grows linearly instead of quadratically
//! // rho_prime < 1.0, downweighting the outlier
//! # Ok(())
//! # }
//! # example().unwrap();
//! ```

use crate::core::{CoreError, CoreResult};

/// Validate a scale/shape parameter used in division, multiplication, a
/// threshold, or a logarithm: it must be finite and strictly positive.
///
/// A bare `value <= 0.0` guard lets `NaN` through silently (every IEEE-754
/// comparison with `NaN` is `false`), and `+infinity` also passes such a
/// guard while producing indeterminate expressions like `infinity * 0`
/// downstream. `DcsLoss` was the only constructor in this module to already
/// guard against both (ISSUE-0007); every other parameterized loss now uses
/// this shared validator instead of repeating the incomplete check.
fn finite_positive(name: &str, value: f64) -> CoreResult<f64> {
    if !value.is_finite() || value <= 0.0 {
        return Err(CoreError::InvalidInput(format!(
            "{name} must be finite and positive, got {value}"
        )));
    }
    Ok(value)
}

/// Validate a shape parameter (e.g. Barron's `alpha`) that is used in
/// `powf`/exponentiation but is not required to be positive — only finite.
fn finite(name: &str, value: f64) -> CoreResult<f64> {
    if !value.is_finite() {
        return Err(CoreError::InvalidInput(format!(
            "{name} must be finite, got {value}"
        )));
    }
    Ok(value)
}

/// Trait for robust loss functions used in nonlinear least squares optimization.
///
/// A loss function transforms the squared residual `s = ||r||²` into a robust cost `ρ(s)`
/// that reduces the influence of outliers. The trait provides the cost value and its first
/// two derivatives, which are used by the `Corrector` to modify the optimization problem.
///
/// # Returns
///
/// The `evaluate` method returns a 3-element array: `[ρ(s), ρ'(s), ρ''(s)]`
/// - `ρ(s)`: Robust cost value
/// - `ρ'(s)`: First derivative (weight function)
/// - `ρ''(s)`: Second derivative
///
/// # Implementation Notes
///
/// - Loss functions should be smooth (at least C²) for optimization stability
/// - Typically ρ(0) = 0, ρ'(0) = 1, ρ''(0) = 0 (behaves like standard least squares near zero)
/// - For outliers, ρ'(s) should decrease to downweight large residuals
pub trait LossFunction: Send + Sync {
    /// Evaluate the loss function and its first two derivatives at squared residual `s`.
    ///
    /// # Arguments
    ///
    /// * `s` - The squared norm of the residual: `s = ||r||²` (always non-negative)
    ///
    /// # Returns
    ///
    /// Array `[ρ(s), ρ'(s), ρ''(s)]` containing the cost, first derivative, and second derivative
    fn evaluate(&self, s: f64) -> [f64; 3];
}

/// L2 loss function (standard least squares, no robustness).
///
/// The L2 loss is the standard squared error used in ordinary least squares optimization.
/// It provides no outlier robustness and is optimal only when residuals follow a Gaussian
/// distribution.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = s
/// ρ'(s) = 1
/// ρ''(s) = 0
/// ```
///
/// where `s = ||r||²` is the squared residual norm.
///
/// # Properties
///
/// - **Convex**: Globally optimal solution
/// - **Not robust**: Outliers have full influence (squared!)
/// - **Optimal**: For Gaussian noise without outliers
/// - **Fast**: Simplest to compute
///
/// # Use Cases
///
/// - Clean data with known Gaussian noise
/// - Baseline comparison for robust methods
/// - When outliers are already filtered
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, L2Loss};
///
/// let l2 = L2Loss::new();
///
/// let [rho, rho_prime, rho_double_prime] = l2.evaluate(4.0);
/// assert_eq!(rho, 4.0);  // ρ(s) = s
/// assert_eq!(rho_prime, 1.0);  // Full weight
/// assert_eq!(rho_double_prime, 0.0);
/// ```
#[derive(Debug, Clone, Copy)]
pub struct L2Loss;

impl L2Loss {
    /// Create a new L2 loss function (no parameters needed).
    pub fn new() -> Self {
        L2Loss
    }
}

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

impl LossFunction for L2Loss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        [s, 1.0, 0.0]
    }
}

/// L1 loss function (absolute error, simple robust baseline).
///
/// The L1 loss uses absolute error instead of squared error, providing basic robustness
/// to outliers. It is optimal for Laplacian noise distributions.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = 2(√(s+ε) - √ε)
/// ρ'(s) = 1/√(s+ε)
/// ρ''(s) = -1/(2(s+ε)^(3/2))
/// ```
///
/// where `s = ||r||²` is the squared residual norm and `ε` is a small fixed
/// smoothing constant so the curve is smooth (C∞) at `s = 0`, matching true
/// L1 (`ρ(s) = 2√s`) for `s ≫ ε` (ISSUE-0011).
///
/// # Properties
///
/// - **Convex**: Globally optimal solution
/// - **Moderately robust**: Linear growth vs quadratic
/// - **Smooth at zero**: True L1's infinite slope at s=0 is replaced by a
///   large but finite derivative, `1/√ε`
/// - **Median estimator**: Minimizes to median instead of mean
///
/// # Use Cases
///
/// - Simple outlier rejection
/// - When median is preferred over mean
/// - Sparse optimization problems
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, L1Loss};
///
/// let l1 = L1Loss::new();
///
/// let [rho, rho_prime, _] = l1.evaluate(4.0);
/// // Smoothed near the origin (ISSUE-0011), so this matches true L1
/// // (ρ(4) = 2√4 = 4) only up to the smoothing scale, not to machine precision.
/// assert!((rho - 4.0).abs() < 1e-5);
/// assert!((rho_prime - 0.5).abs() < 1e-10);  // ρ'(4) = 1/√4 = 0.5
/// ```
#[derive(Debug, Clone, Copy)]
pub struct L1Loss;

impl L1Loss {
    /// Create a new L1 loss function (no parameters needed).
    pub fn new() -> Self {
        L1Loss
    }
}

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

/// Smoothing scale for [`L1Loss`]/[`LpNormLoss`] near `s = 0`, in squared-
/// residual units — see the "ISSUE-0011" note on `L1Loss::evaluate`.
const ORIGIN_SMOOTHING_EPS: f64 = 1e-12;

impl LossFunction for L1Loss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        // ISSUE-0011: the previous `if s < f64::EPSILON { [s,1.0,0.0] }`
        // guard spliced in the *unrelated* L2 branch, whose value/derivative
        // don't match `2√s`/`1/√s` just outside the guard — an ~8-order-of-
        // magnitude jump right at the boundary, well inside the residual
        // range a converging solve passes through. A single Charbonnier-
        // style shifted formula, smooth for every `s ≥ 0` with no branch to
        // mismatch, replaces it: `ρ(s) = 2(√(s+ε) - √ε)` is exactly `2√s`
        // for `s ≫ ε` and finite (not the true L1's infinite slope) at
        // `s = 0`, where `ε` only keeps the curve C∞.
        let shifted = (s + ORIGIN_SMOOTHING_EPS).sqrt();
        [
            2.0 * (shifted - ORIGIN_SMOOTHING_EPS.sqrt()), // ρ(s) = 2(√(s+ε) - √ε)
            1.0 / shifted,                                 // ρ'(s) = 1/√(s+ε)
            -0.5 / (shifted * shifted * shifted),          // ρ''(s) = -1/(2(s+ε)^1.5)
        ]
    }
}

/// Huber loss function for moderate outlier rejection.
///
/// The Huber loss is quadratic for small residuals (inliers) and linear for large residuals
/// (outliers), providing a good balance between robustness and efficiency.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = {  s                           if s ≤ δ²
///        {  2δ√s - δ²                  if s > δ²
///
/// ρ'(s) = {  1                          if s ≤ δ²
///         {  δ / √s                    if s > δ²
///
/// ρ''(s) = {  0                         if s ≤ δ²
///          {  -δ / (2s^(3/2))          if s > δ²
/// ```
///
/// where `δ` is the scale parameter (threshold), and `s = ||r||²` is the squared residual norm.
///
/// # Properties
///
/// - **Inlier region** (s ≤ δ²): Behaves like standard least squares (quadratic cost)
/// - **Outlier region** (s > δ²): Cost grows linearly, limiting outlier influence
/// - **Transition point**: At s = δ², the function switches from quadratic to linear
///
/// # Scale Parameter Selection
///
/// Common choices for the scale parameter `δ`:
/// - **1.345**: Approximately 95% efficiency on Gaussian data (most common)
/// - **0.5-1.0**: More aggressive outlier rejection
/// - **2.0-3.0**: More lenient, closer to standard least squares
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, HuberLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// // Create Huber loss with scale = 1.345 (standard choice)
/// let huber = HuberLoss::new(1.345)?;
///
/// // Small residual (inlier): ||r||² = 0.5
/// let [rho, rho_prime, rho_double_prime] = huber.evaluate(0.5);
/// assert_eq!(rho, 0.5);           // Quadratic: ρ(s) = s
/// assert_eq!(rho_prime, 1.0);     // Full weight
/// assert_eq!(rho_double_prime, 0.0);
///
/// // Large residual (outlier): ||r||² = 10.0
/// let [rho, rho_prime, rho_double_prime] = huber.evaluate(10.0);
/// // ρ(10) ≈ 6.69, grows linearly not quadratically
/// // ρ'(10) ≈ 0.425, downweighted to ~42.5% of original
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct HuberLoss {
    /// Scale parameter δ
    scale: f64,
    /// Cached value δ² for efficient computation
    scale2: f64,
}

impl HuberLoss {
    /// Create a new Huber loss function with the given scale parameter.
    ///
    /// # Arguments
    ///
    /// * `scale` - The threshold δ that separates inliers from outliers (must be positive)
    ///
    /// # Returns
    ///
    /// `Ok(HuberLoss)` if scale > 0, otherwise an error
    ///
    /// # Example
    ///
    /// ```
    /// use apex_solver::core::loss_functions::HuberLoss;
    /// # use apex_solver::core::CoreResult;
    /// # fn example() -> CoreResult<()> {
    ///
    /// let huber = HuberLoss::new(1.345)?;
    /// # Ok(())
    /// # }
    /// # example().unwrap();
    /// ```
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        Ok(HuberLoss {
            scale,
            scale2: scale * scale,
        })
    }
}

impl LossFunction for HuberLoss {
    /// Evaluate Huber loss function: ρ(s), ρ'(s), ρ''(s).
    ///
    /// # Arguments
    ///
    /// * `s` - Squared residual norm: s = ||r||²
    ///
    /// # Returns
    ///
    /// `[ρ(s), ρ'(s), ρ''(s)]` - Cost, first derivative, second derivative
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        if s > self.scale2 {
            // Outlier region: s > δ²
            // Linear cost: ρ(s) = 2δ√s - δ²
            let r = s.sqrt(); // r = √s = ||r||
            let rho1 = (self.scale / r).max(f64::MIN); // ρ'(s) = δ / √s
            [
                2.0 * self.scale * r - self.scale2, // ρ(s)
                rho1,                               // ρ'(s)
                -rho1 / (2.0 * s),                  // ρ''(s) = -δ / (2s√s)
            ]
        } else {
            // Inlier region: s ≤ δ²
            // Quadratic cost: ρ(s) = s, ρ'(s) = 1, ρ''(s) = 0
            [s, 1.0, 0.0]
        }
    }
}

/// Cauchy loss function for aggressive outlier rejection.
///
/// The Cauchy loss (also called Lorentzian loss) provides stronger suppression of outliers
/// than Huber loss. It never fully rejects outliers but reduces their weight significantly.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = δ² * log(1 + s/δ²)
///
/// ρ'(s) = 1 / (1 + s/δ²)
///
/// ρ''(s) = -1 / (δ² * (1 + s/δ²)²)
/// ```
///
/// where `δ` is the scale parameter, and `s = ||r||²` is the squared residual norm.
///
/// # Properties
///
/// - **Smooth transition**: No sharp boundary between inliers and outliers
/// - **Logarithmic growth**: Cost grows very slowly for large residuals
/// - **Strong downweighting**: Large outliers receive very small weights
/// - **Non-convex**: Can have multiple local minima (harder to optimize than Huber)
///
/// # Scale Parameter Selection
///
/// Typical values:
/// - **2.3849**: Approximately 95% efficiency on Gaussian data
/// - **1.0-2.0**: More aggressive outlier rejection
/// - **3.0-5.0**: More lenient
///
/// # Comparison to Huber Loss
///
/// - **Cauchy**: Stronger outlier rejection, smoother, but non-convex (may converge to local minimum)
/// - **Huber**: Weaker outlier rejection, convex, more predictable convergence
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, CauchyLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// // Create Cauchy loss with scale = 2.3849 (standard choice)
/// let cauchy = CauchyLoss::new(2.3849)?;
///
/// // Small residual: ||r||² = 0.5
/// let [rho, rho_prime, _] = cauchy.evaluate(0.5);
/// // ρ ≈ 0.48, slightly less than 0.5 (mild downweighting)
/// // ρ' ≈ 0.92, close to 1.0 (near full weight)
///
/// // Large residual: ||r||² = 100.0
/// let [rho, rho_prime, _] = cauchy.evaluate(100.0);
/// // ρ ≈ 16.6, logarithmic growth (much less than 100)
/// // ρ' ≈ 0.05, heavily downweighted (5% of original)
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
pub struct CauchyLoss {
    /// Cached value δ² (scale squared)
    scale2: f64,
    /// Cached value 1/δ² for efficient computation
    c: f64,
}

impl CauchyLoss {
    /// Create a new Cauchy loss function with the given scale parameter.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter δ (must be positive)
    ///
    /// # Returns
    ///
    /// `Ok(CauchyLoss)` if scale > 0, otherwise an error
    ///
    /// # Example
    ///
    /// ```
    /// use apex_solver::core::loss_functions::CauchyLoss;
    /// # use apex_solver::core::CoreResult;
    /// # fn example() -> CoreResult<()> {
    ///
    /// let cauchy = CauchyLoss::new(2.3849)?;
    /// # Ok(())
    /// # }
    /// # example().unwrap();
    /// ```
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        let scale2 = scale * scale;
        Ok(CauchyLoss {
            scale2,
            c: 1.0 / scale2,
        })
    }
}

impl LossFunction for CauchyLoss {
    /// Evaluate Cauchy loss function: ρ(s), ρ'(s), ρ''(s).
    ///
    /// # Arguments
    ///
    /// * `s` - Squared residual norm: s = ||r||²
    ///
    /// # Returns
    ///
    /// `[ρ(s), ρ'(s), ρ''(s)]` - Cost, first derivative, second derivative
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        let sum = 1.0 + s * self.c; // 1 + s/δ²
        let inv = 1.0 / sum; // 1 / (1 + s/δ²)

        // Note: sum and inv are always positive, assuming s ≥ 0
        //
        // ISSUE-0005 / GH-57: ρ must be the antiderivative of the returned
        // ρ'/ρ'' — the standard Ceres/g2o Cauchy normalization is
        // ρ(s) = δ²·ln(1+s/δ²) (no extra 1/2), which is exactly what
        // differentiating [inv, -c·inv²] below already assumes. The previous
        // `/2.0` here made ρ half of what ρ'/ρ'' implied, so the corrector's
        // reported cost was internally inconsistent (a global rescale of ρ
        // does not change the Gauss-Newton step, so this only cost the
        // solver extra iterations rather than a wrong optimum).
        [
            self.scale2 * sum.ln(), // ρ(s) = δ² * ln(1 + s/δ²)
            inv.max(f64::MIN),      // ρ'(s) = 1 / (1 + s/δ²)
            -self.c * (inv * inv),  // ρ''(s) = -1 / (δ² * (1 + s/δ²)²)
        ]
    }
}

/// Fair loss function with continuous smooth derivatives.
///
/// The Fair loss provides a good balance between robustness and stability with everywhere-defined
/// continuous derivatives up to third order. It yields a unique solution and is recommended
/// for general use when you need guaranteed smoothness.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = c² * (|x|/c - ln(1 + |x|/c))
/// ρ'(s) = c / (2(c + |x|))
/// ρ''(s) = -c / (4|x|(c + |x|)²)
/// ```
///
/// where `c` is the scale parameter, `s = ||r||²`, and `x = √s = ||r||`.
///
/// # Properties
///
/// - **Smooth**: Continuous derivatives of first three orders
/// - **Unique solution**: Strictly convex near origin
/// - **Moderate robustness**: Between Huber and Cauchy
/// - **Stable**: No discontinuities in optimization
///
/// # Scale Parameter Selection
///
/// - **1.3998**: Approximately 95% efficiency on Gaussian data (recommended)
/// - **0.8-1.2**: More aggressive outlier rejection
/// - **2.0-3.0**: More lenient
///
/// # Comparison
///
/// - Smoother than Huber (no kink at threshold)
/// - Less aggressive than Cauchy
/// - Better behaved numerically than many redescending M-estimators
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, FairLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let fair = FairLoss::new(1.3998)?;
///
/// let [rho, rho_prime, _] = fair.evaluate(4.0);
/// // Smooth transition, no sharp corners
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct FairLoss {
    scale: f64,
}

impl FairLoss {
    /// Create a new Fair loss function with the given scale parameter.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter c (must be positive)
    ///
    /// # Returns
    ///
    /// `Ok(FairLoss)` if scale > 0, otherwise an error
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        Ok(FairLoss { scale })
    }
}

impl LossFunction for FairLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        if s < f64::EPSILON {
            return [s, 1.0, 0.0];
        }

        let x = s.sqrt(); // ||r|| = √s
        let abs_x = x.abs();
        let c_plus_x = self.scale + abs_x;

        // ρ(s) = c² * (|x|/c - ln(1 + |x|/c))
        let rho = self.scale * self.scale * (abs_x / self.scale - (1.0 + abs_x / self.scale).ln());

        // dρ/ds = dρ/dx * dx/ds, x = √s: ρ'(s) = c / (2(c + |x|))
        let rho_prime = self.scale / (2.0 * c_plus_x);

        // ρ''(s) = -c / (4|x|(c + |x|)²)
        let rho_double_prime = -self.scale / (4.0 * abs_x * c_plus_x * c_plus_x);

        [rho, rho_prime, rho_double_prime]
    }
}

/// Geman-McClure loss function for very strong outlier rejection.
///
/// The Geman-McClure loss provides one of the strongest forms of outlier suppression,
/// with weights that decay rapidly for large residuals.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = s / (1 + s/c²)
/// ρ'(s) = 1 / (1 + s/c²)²
/// ρ''(s) = -2 / (c² * (1 + s/c²)³)
/// ```
///
/// where `c` is the scale parameter and `s = ||r||²`.
///
/// # Properties
///
/// - **Very strong rejection**: Weights decay as O(1/s²) for large s
/// - **Non-convex**: Multiple local minima possible
/// - **No unique solution**: Requires good initialization
/// - **Aggressive**: Use when outliers are severe
///
/// # Scale Parameter Selection
///
/// - **1.0-2.0**: Typical range
/// - **0.5-1.0**: Very aggressive (use with care)
/// - **2.0-4.0**: More lenient
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, GemanMcClureLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let geman = GemanMcClureLoss::new(1.0)?;
///
/// let [rho, rho_prime, _] = geman.evaluate(100.0);
/// // Very small weight for large outliers
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct GemanMcClureLoss {
    c: f64, // 1/scale²
}

impl GemanMcClureLoss {
    /// Create a new Geman-McClure loss function.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter c (must be positive)
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        let scale2 = scale * scale;
        Ok(GemanMcClureLoss { c: 1.0 / scale2 })
    }
}

impl LossFunction for GemanMcClureLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        let denom = 1.0 + s * self.c; // 1 + s/c²
        let inv = 1.0 / denom;
        let inv2 = inv * inv;

        [
            s * inv,                    // ρ(s) = s / (1 + s/c²)
            inv2,                       // ρ'(s) = 1 / (1 + s/c²)²
            -2.0 * self.c * inv2 * inv, // ρ''(s) = -2 / (c²(1 + s/c²)³)
        ]
    }
}

/// Dynamic Covariance Scaling (DCS) loss for switchable-constraint loop closures.
///
/// DCS (Agarwal et al., ICRA 2013) downweights loop-closure constraints with
/// large errors by scaling their information matrix with `s²`, where the
/// scaling factor has the closed form `s = min(1, 2Φ/(Φ+χ²))` and `Φ` is a
/// free parameter (1.0 in the paper and in g2o).
///
/// # Mathematical Definition
///
/// With `s = ||r||²`:
///
/// ```text
/// s ≤ Φ:  ρ(s) = s,    ρ'(s) = 1,  ρ''(s) = 0     (inlier: plain LS)
/// s > Φ:  ρ(s) = Φ,    ρ'(s) = 0,  ρ''(s) = 0      (outlier: saturating plateau)
/// ```
///
/// # Why this differs from g2o's `RobustKernelDCS`
///
/// g2o's own DCS term is `ρ(s) = scale²·s` with `scale = 2Φ/(Φ+s)`, which
/// keeps *declining* toward 0 past `s = Φ` rather than saturating — but g2o
/// applies it through its own explicit information-matrix reweighting, not
/// through a generic square-root [`Corrector`](crate::core::corrector::Corrector).
/// Run through this crate's Ceres/Triggs-style corrector, `ρ'(s) < 0` past
/// `s = Φ` gets clamped to zero (the corrected residual and Jacobian both
/// become exactly zero, the same "no gradient" treatment as Tukey's flat
/// outlier region) — but g2o's declining `ρ(s)` would keep changing the
/// *reported* cost with state while contributing zero to the model the
/// optimizer actually descends, so actual and predicted reduction disagree
/// past the threshold. Freezing `ρ` at its peak value `Φ` once `ρ'` would go
/// negative keeps the reported cost consistent with the zero-gradient model:
/// a genuine saturating plateau, matching Tukey/Geman-McClure in kind, not a
/// cost that silently keeps moving with no corresponding gradient.
///
/// # Scale Parameter Selection
///
/// - **1.0**: Paper and g2o default; wide working range on standard datasets
/// - **0.1–1.0**: Tighter gating; below 0.1 convergence degrades (notably Sphere)
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, DcsLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let dcs = DcsLoss::new(1.0)?;
///
/// // Inlier region behaves like least squares
/// let [rho, rho_prime, _] = dcs.evaluate(0.5);
/// assert_eq!(rho, 0.5);
/// assert_eq!(rho_prime, 1.0);
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct DcsLoss {
    phi: f64,
}

impl DcsLoss {
    /// Create a new DCS loss function.
    ///
    /// # Arguments
    ///
    /// * `phi` - Free parameter Φ (must be positive; 1.0 is the paper default)
    pub fn new(phi: f64) -> CoreResult<Self> {
        let phi = finite_positive("phi", phi)?;
        Ok(DcsLoss { phi })
    }
}

impl LossFunction for DcsLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        if s <= self.phi {
            // s ≤ Φ: unscaled least squares.
            [s, 1.0, 0.0]
        } else {
            // s > Φ: saturating plateau at the inlier/outlier boundary value
            // — see "Why this differs from g2o's RobustKernelDCS" above.
            [self.phi, 0.0, 0.0]
        }
    }
}

/// Welsch loss function with exponential downweighting.
///
/// The Welsch loss (also called Leclerc loss) uses exponential decay to strongly
/// suppress outliers while maintaining smoothness. It completely suppresses very
/// large outliers (weight → 0 as s → ∞).
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = c²/2 * (1 - exp(-s/c²))
/// ρ'(s) = (1/2) * exp(-s/c²)
/// ρ''(s) = -(1/2c²) * exp(-s/c²)
/// ```
///
/// where `c` is the scale parameter and `s = ||r||²`.
///
/// # Properties
///
/// - **Redescending**: Weights decrease to zero for large residuals
/// - **Smooth**: Infinitely differentiable
/// - **Strong suppression**: Exponential decay
/// - **Non-convex**: Requires good initialization
///
/// # Scale Parameter Selection
///
/// - **2.9846**: Approximately 95% efficiency on Gaussian data
/// - **2.0-2.5**: More aggressive
/// - **3.5-4.5**: More lenient
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, WelschLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let welsch = WelschLoss::new(2.9846)?;
///
/// let [rho, rho_prime, _] = welsch.evaluate(50.0);
/// // Weight approaches zero for large residuals
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct WelschLoss {
    scale2: f64,
    inv_scale2: f64,
}

impl WelschLoss {
    /// Create a new Welsch loss function.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter c (must be positive)
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        let scale2 = scale * scale;
        Ok(WelschLoss {
            scale2,
            inv_scale2: 1.0 / scale2,
        })
    }
}

impl LossFunction for WelschLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        let exp_term = (-s * self.inv_scale2).exp();

        [
            (self.scale2 / 2.0) * (1.0 - exp_term), // ρ(s) = c²/2 * (1 - exp(-s/c²))
            0.5 * exp_term,                         // ρ'(s) = (1/2) * exp(-s/c²)
            -0.5 * self.inv_scale2 * exp_term,      // ρ''(s) = -(1/2c²) * exp(-s/c²)
        ]
    }
}

/// Tukey biweight loss function with complete outlier suppression.
///
/// The Tukey biweight (bisquare) loss completely suppresses outliers beyond a threshold,
/// setting their weight to exactly zero. This is a "redescending" M-estimator.
///
/// # Mathematical Definition
///
/// For |x| ≤ c:
/// ```text
/// ρ(s) = c²/6 * (1 - (1 - (x/c)²)³)
/// ρ'(s) = (1/2) * (1 - (x/c)²)²
/// ρ''(s) = -(1 - (x/c)²) / c²
/// ```
///
/// For |x| > c:
/// ```text
/// ρ(s) = c²/6
/// ρ'(s) = 0
/// ρ''(s) = 0
/// ```
///
/// where `c` is the scale parameter, `x = √s`, and `s = ||r||²`.
///
/// # Properties
///
/// - **Complete suppression**: Outliers have exactly zero weight
/// - **Redescending**: Weight goes to zero beyond threshold
/// - **Non-convex**: Multiple local minima
/// - **Aggressive**: Best for severe outlier contamination
///
/// # Scale Parameter Selection
///
/// - **4.6851**: Approximately 95% efficiency on Gaussian data
/// - **3.5-4.0**: More aggressive
/// - **5.5-6.5**: More lenient
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, TukeyBiweightLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let tukey = TukeyBiweightLoss::new(4.6851)?;
///
/// let [rho, rho_prime, _] = tukey.evaluate(25.0); // |x| = 5 > 4.6851
/// assert_eq!(rho_prime, 0.0); // Complete suppression
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct TukeyBiweightLoss {
    scale: f64,
    scale2: f64,
}

impl TukeyBiweightLoss {
    /// Create a new Tukey biweight loss function.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter c (must be positive)
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        Ok(TukeyBiweightLoss {
            scale,
            scale2: scale * scale,
        })
    }
}

impl LossFunction for TukeyBiweightLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        let x = s.sqrt();

        if x > self.scale {
            // Complete outlier suppression
            [self.scale2 / 6.0, 0.0, 0.0]
        } else {
            let ratio = x / self.scale;
            let ratio2 = ratio * ratio;
            let one_minus_ratio2 = 1.0 - ratio2;
            let one_minus_ratio2_sq = one_minus_ratio2 * one_minus_ratio2;

            [
                (self.scale2 / 6.0) * (1.0 - one_minus_ratio2 * one_minus_ratio2_sq), // ρ(s)
                0.5 * one_minus_ratio2_sq,                                            // ρ'(s)
                -one_minus_ratio2 / self.scale2, // ρ''(s) = -(1-s/c²)/c²
            ]
        }
    }
}

/// Andrews sine wave loss function (redescending M-estimator).
///
/// The Andrews sine wave loss uses a periodic sine function to create a redescending
/// M-estimator that completely suppresses outliers beyond π*c.
///
/// # Mathematical Definition
///
/// For |x| ≤ πc:
/// ```text
/// ρ(s) = c² * (1 - cos(x/c))
/// ρ'(s) = c * sin(x/c) / (2x)
/// ρ''(s) = (x*cos(x/c) - c*sin(x/c)) / (4x³)
/// ```
/// with the analytic limits `ρ'(0) = 1/2` and `ρ''(0) = -1/(12c²)` at `x = 0`.
///
/// For |x| > πc:
/// ```text
/// ρ(s) = 2c²
/// ρ'(s) = 0
/// ρ''(s) = 0
/// ```
///
/// where `c` is the scale parameter, `x = √s`, and `s = ||r||²`.
///
/// # Properties
///
/// - **Periodic structure**: Sine-based weighting
/// - **Complete suppression**: Zero weight beyond πc
/// - **Redescending**: Smooth transition to zero
/// - **Non-convex**: Requires careful initialization
///
/// # Scale Parameter Selection
///
/// - **1.339**: Standard tuning constant
/// - **1.0-1.2**: More aggressive
/// - **1.5-2.0**: More lenient
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, AndrewsWaveLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let andrews = AndrewsWaveLoss::new(1.339)?;
///
/// let [rho, rho_prime, _] = andrews.evaluate(20.0);
/// // Weight is zero for large outliers
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct AndrewsWaveLoss {
    scale: f64,
    scale2: f64,
    threshold: f64, // π * scale
}

impl AndrewsWaveLoss {
    /// Create a new Andrews sine wave loss function.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter c (must be positive)
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        Ok(AndrewsWaveLoss {
            scale,
            scale2: scale * scale,
            threshold: std::f64::consts::PI * scale,
        })
    }
}

impl LossFunction for AndrewsWaveLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        let x = s.sqrt();

        if x > self.threshold {
            // Complete suppression beyond π*c
            [2.0 * self.scale2, 0.0, 0.0]
        } else if s < 1e-6 {
            // ρ'(s) = c·sin(x/c)/(2x) and ρ''(s) = (x·cos(x/c)-c·sin(x/c))/(4x³)
            // both hit 0/0 as x→0 and lose precision to cancellation well
            // before that (two O(x) terms agreeing to O(x³)); use the Taylor
            // series in s near the origin instead. Matches the closed form's
            // own analytic limit: ρ'(0) = 1/2, not the 0 the removable
            // singularity naively returns.
            let rho = self.scale2 * (1.0 - (x / self.scale).cos());
            let rho_prime = 0.5 - s / (12.0 * self.scale2);
            let rho_double_prime =
                -1.0 / (12.0 * self.scale2) + s / (120.0 * self.scale2 * self.scale2);
            [rho, rho_prime, rho_double_prime]
        } else {
            let arg = x / self.scale;
            let sin_val = arg.sin();
            let cos_val = arg.cos();

            [
                self.scale2 * (1.0 - cos_val),                            // ρ(s)
                self.scale * sin_val / (2.0 * x),                         // ρ'(s) = c·sin(x/c)/(2x)
                (x * cos_val - self.scale * sin_val) / (4.0 * x * x * x), // ρ''(s)
            ]
        }
    }
}

/// Ramsay Ea loss function with exponential decay.
///
/// The Ramsay Ea loss uses exponential weighting to provide smooth, strong
/// downweighting of outliers.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = a⁻² * (1 - exp(-a|x|) * (1 + a|x|))
/// ```
///
/// where `a` is the scale parameter, `x = √s`, and `s = ||r||²`.
///
/// # Properties
///
/// - **Exponential decay**: Smooth weight reduction
/// - **Strongly robust**: Good for heavy outliers
/// - **Smooth**: Continuous derivatives
/// - **Non-convex**: Needs good initialization
///
/// # Scale Parameter Selection
///
/// - **0.3**: Standard tuning constant
/// - **0.2-0.25**: More aggressive
/// - **0.35-0.5**: More lenient
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, RamsayEaLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let ramsay = RamsayEaLoss::new(0.3)?;
///
/// let [rho, rho_prime, _] = ramsay.evaluate(10.0);
/// // Exponential downweighting
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct RamsayEaLoss {
    scale: f64,
    inv_scale2: f64,
}

impl RamsayEaLoss {
    /// Create a new Ramsay Ea loss function.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter a (must be positive)
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        Ok(RamsayEaLoss {
            scale,
            inv_scale2: 1.0 / (scale * scale),
        })
    }
}

impl LossFunction for RamsayEaLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        let x = s.sqrt();
        let ax = self.scale * x;
        let exp_term = (-ax).exp();

        // ρ(s) = a⁻² * (1 - exp(-a|x|) * (1 + a|x|))
        let rho = self.inv_scale2 * (1.0 - exp_term * (1.0 + ax));

        // ρ'(s) = (1/2) * exp(-a|x|)
        let rho_prime = 0.5 * exp_term;

        // ρ''(s) = -(a/4|x|) * exp(-a|x|)
        let rho_double_prime = -(self.scale / (4.0 * x.max(f64::EPSILON))) * exp_term;

        [rho, rho_prime, rho_double_prime]
    }
}

/// Trimmed mean loss function with hard threshold.
///
/// The trimmed mean loss is the simplest redescending estimator, applying a hard
/// cutoff at a threshold. Residuals below the threshold use L2 loss, those above
/// contribute a constant.
///
/// # Mathematical Definition
///
/// For s ≤ c²:
/// ```text
/// ρ(s) = s/2
/// ρ'(s) = 1/2
/// ρ''(s) = 0
/// ```
///
/// For s > c²:
/// ```text
/// ρ(s) = c²/2
/// ρ'(s) = 0
/// ρ''(s) = 0
/// ```
///
/// where `c` is the scale parameter and `s = ||r||²`.
///
/// # Properties
///
/// - **Simple**: Easiest to understand and implement
/// - **Hard cutoff**: Discontinuous weight function
/// - **Robust**: Completely ignores large outliers
/// - **Unstable**: Discontinuity can cause optimization issues
///
/// # Scale Parameter Selection
///
/// - **2.0**: Standard tuning constant
/// - **1.5**: More aggressive
/// - **3.0**: More lenient
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, TrimmedMeanLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let trimmed = TrimmedMeanLoss::new(2.0)?;
///
/// let [rho, rho_prime, _] = trimmed.evaluate(5.0);
/// assert_eq!(rho_prime, 0.0); // Beyond threshold
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct TrimmedMeanLoss {
    scale2: f64,
}

impl TrimmedMeanLoss {
    /// Create a new trimmed mean loss function.
    ///
    /// # Arguments
    ///
    /// * `scale` - The scale parameter c (must be positive)
    pub fn new(scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        Ok(TrimmedMeanLoss {
            scale2: scale * scale,
        })
    }
}

impl LossFunction for TrimmedMeanLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        if s <= self.scale2 {
            [s / 2.0, 0.5, 0.0]
        } else {
            [self.scale2 / 2.0, 0.0, 0.0]
        }
    }
}

/// Generalized Lp norm loss function.
///
/// The Lp norm loss allows flexible control over robustness through the p parameter.
/// It interpolates between L1 (p=1) and L2 (p=2) losses.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = (s+ε)^(p/2) - ε^(p/2)
/// ρ'(s) = (p/2) * (s+ε)^(p/2-1)
/// ρ''(s) = (p/2) * (p/2-1) * (s+ε)^(p/2-2)
/// ```
///
/// where `p` is the norm parameter, `s = ||r||²`, and `ε` is a small fixed
/// smoothing constant so the curve is smooth (C∞) at `s = 0` for every `p`,
/// matching true `|x|^p` for `s ≫ ε` (ISSUE-0011: the previous `s < f64::EPSILON`
/// guard spliced in an unrelated L2 branch with a large value/derivative
/// jump right at the boundary).
///
/// # Properties
///
/// - **Flexible**: Tune robustness with p parameter
/// - **p=2**: L2 norm (standard least squares)
/// - **p=1**: L1 norm (robust median estimator)
/// - **p<1**: Very robust (non-convex)
/// - **1<p<2**: Compromise between L1 and L2
///
/// # Parameter Selection
///
/// - **p = 2.0**: No robustness (L2 loss)
/// - **p = 1.5**: Moderate robustness
/// - **p = 1.2**: Strong robustness
/// - **p = 1.0**: L1 loss (median)
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, LpNormLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let lp = LpNormLoss::new(1.5)?;
///
/// let [rho, rho_prime, _] = lp.evaluate(4.0);
/// // Between L1 and L2 behavior
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct LpNormLoss {
    p: f64,
}

impl LpNormLoss {
    /// Create a new Lp norm loss function.
    ///
    /// # Arguments
    ///
    /// * `p` - The norm parameter (0 < p ≤ 2 for practical use)
    pub fn new(p: f64) -> CoreResult<Self> {
        let p = finite_positive("p", p)?;
        Ok(LpNormLoss { p })
    }
}

impl LossFunction for LpNormLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        // ISSUE-0011: same fix as `L1Loss` (its `p=1` special case) — a
        // single shifted power, smooth for every `s ≥ 0`, replacing the old
        // `s < f64::EPSILON` splice into the unrelated `[s,1,0]` L2 branch.
        let shifted = s + ORIGIN_SMOOTHING_EPS;
        let exp_rho = self.p / 2.0;
        let exp_rho_prime = exp_rho - 1.0;
        let exp_rho_double_prime = exp_rho_prime - 1.0;

        [
            shifted.powf(exp_rho) - ORIGIN_SMOOTHING_EPS.powf(exp_rho), // ρ(s) = (s+ε)^(p/2) - ε^(p/2)
            exp_rho * shifted.powf(exp_rho_prime), // ρ'(s) = (p/2) * (s+ε)^(p/2-1)
            exp_rho * exp_rho_prime * shifted.powf(exp_rho_double_prime), // ρ''(s)
        ]
    }
}

/// Barron's general and adaptive robust loss function (CVPR 2019).
///
/// The Barron loss is a unified framework that encompasses many classic loss functions
/// (L2, Charbonnier, Cauchy, Geman-McClure, Welsch) through a single shape parameter α.
/// It can also adapt α automatically during optimization.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s, α, c) = (|α|/c²) * (|(x/c)² * |α|/2 + 1|^(α/2) - 1)
/// ```
///
/// where `α` controls robustness, `c` is scale, `x = √s`, and `s = ||r||²`.
///
/// # Special Cases (by α value)
///
/// - **α = 2**: L2 loss (no robustness)
/// - **α = 1**: Charbonnier/Pseudo-Huber loss
/// - **α = 0**: Cauchy loss
/// - **α = -1**: Welsch loss
/// - **α = -2**: Geman-McClure loss
/// - **α → -∞**: L0 "norm" (binary)
///
/// # Properties
///
/// - **Unified**: Single framework for many loss functions
/// - **Adaptive**: Can learn optimal α during training
/// - **Smooth**: Continuously differentiable in α
/// - **Modern**: State-of-the-art from computer vision research
///
/// # Parameter Selection
///
/// **Fixed α (manual tuning):**
/// - α = 2.0: Clean data, no outliers
/// - α = 0.0 to 1.0: Moderate outliers
/// - α = -2.0 to 0.0: Heavy outliers
///
/// **Scale c:**
/// - c = 1.0: Standard choice
/// - Adjust based on expected residual magnitude
///
/// # References
///
/// Barron, J. T. (2019). A general and adaptive robust loss function.
/// IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR).
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, BarronGeneralLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// // Cauchy-like behavior
/// let barron = BarronGeneralLoss::new(0.0, 1.0)?;
///
/// let [rho, rho_prime, _] = barron.evaluate(4.0);
/// // Behaves like Cauchy loss
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct BarronGeneralLoss {
    alpha: f64,
    scale2: f64,
}

impl BarronGeneralLoss {
    /// Create a new Barron general robust loss function.
    ///
    /// # Arguments
    ///
    /// * `alpha` - The shape parameter (controls robustness)
    /// * `scale` - The scale parameter c (must be positive)
    pub fn new(alpha: f64, scale: f64) -> CoreResult<Self> {
        let scale = finite_positive("scale", scale)?;
        let alpha = finite("alpha", alpha)?;
        Ok(BarronGeneralLoss {
            alpha,
            scale2: scale * scale,
        })
    }
}

impl LossFunction for BarronGeneralLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        let c2 = self.scale2;

        // α → 0 limit (Barron 2019, eq. 13): ρ(s) = ln(s/(2c²) + 1).
        // This is the α → 0 limit of the general branch below, taken via
        // L'Hôpital as `(b/α)(inner^(α/2)-1) → ln(inner)`; evaluating the
        // general formula directly at α = 0 is a 0/0 indeterminate form.
        if self.alpha.abs() < 1e-6 {
            let inner = s / (2.0 * c2) + 1.0;
            let rho = inner.ln();
            let rho_prime = 1.0 / (2.0 * c2 * inner);
            let rho_double_prime = -1.0 / (4.0 * c2 * c2 * inner * inner);
            return [rho, rho_prime, rho_double_prime];
        }

        // α → 2 limit: ordinary (scaled) L2, ρ(s) = s/(2c²). The general
        // branch's `b = |α-2|` denominator vanishes here, so this must be
        // taken as a limit rather than evaluated directly.
        if (self.alpha - 2.0).abs() < 1e-6 {
            return [s / (2.0 * c2), 1.0 / (2.0 * c2), 0.0];
        }

        // General case (Barron 2019, eq. 8, x² = s):
        //   b = |α - 2|,  inner(s) = s/(c²·b) + 1
        //   ρ(s)   = (b/α) · (inner^(α/2) - 1)
        //   ρ'(s)  = inner^(α/2 - 1) / (2c²)
        //   ρ''(s) = sign(α - 2) · inner^(α/2 - 2) / (4c⁴)
        // (ρ'/ρ'' are the exact derivatives of ρ above w.r.t. s — the `b`
        // factor cancels in ρ'; nonnegative and finite for every α, unlike
        // the previous `|α|`-based normalization, which could go negative
        // for α < 0.)
        let b = (self.alpha - 2.0).abs();
        let inner = s / (c2 * b) + 1.0;
        let half_alpha = self.alpha / 2.0;

        let rho = (b / self.alpha) * (inner.powf(half_alpha) - 1.0);
        let rho_prime = inner.powf(half_alpha - 1.0) / (2.0 * c2);
        let sign = if self.alpha > 2.0 { 1.0 } else { -1.0 };
        let rho_double_prime = sign * inner.powf(half_alpha - 2.0) / (4.0 * c2 * c2);

        [rho, rho_prime, rho_double_prime]
    }
}

/// Student's t-distribution loss function (robust M-estimator).
///
/// The t-distribution loss is derived from the negative log-likelihood of Student's
/// t-distribution. It provides heavy tails for robustness against outliers, with the
/// degrees of freedom parameter ν controlling the tail heaviness.
///
/// # Mathematical Definition
///
/// ```text
/// ρ(s) = (ν + 1)/2 · log(1 + s/ν)
/// ρ'(s) = (ν + 1)/(2(ν + s))
/// ρ''(s) = -(ν + 1)/(2(ν + s)²)
/// ```
///
/// where `ν` is the degrees of freedom and `s = ||r||²` is the squared residual norm.
///
/// # Properties
///
/// - **Heavy tails**: Provides robustness through heavier tails than Gaussian
/// - **Parameter control**: Small ν → heavy tails (more robust), large ν → Gaussian (less robust)
/// - **Well-founded**: Based on maximum likelihood estimation with t-distribution
/// - **Smooth**: Continuous derivatives for all s > 0
///
/// # Degrees of Freedom Selection
///
/// - **ν = 3-4**: Very robust, heavy outlier suppression
/// - **ν = 5**: Recommended default, good balance
/// - **ν = 10**: Moderate robustness
/// - **ν → ∞**: Converges to Gaussian (L2 loss)
///
/// # Use Cases
///
/// - Robust regression with unknown outlier distribution
/// - SLAM and pose graph optimization with loop closure outliers
/// - Bundle adjustment with incorrect feature matches
/// - Any optimization problem with heavy-tailed noise
///
/// # References
///
/// - Student's t-distribution is widely used in robust statistics
/// - Applied in robust SLAM (e.g., Chebrolu et al. 2021, Agarwal et al.)
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, TDistributionLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// let t_loss = TDistributionLoss::new(5.0)?;
///
/// let [rho, rho_prime, _] = t_loss.evaluate(4.0);
/// // Robust to outliers with heavy tails
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct TDistributionLoss {
    nu: f64,             // Degrees of freedom
    half_nu_plus_1: f64, // (ν + 1)/2 (cached)
}

impl TDistributionLoss {
    /// Create a new Student's t-distribution loss function.
    ///
    /// # Arguments
    ///
    /// * `nu` - Degrees of freedom (must be positive)
    ///
    /// # Recommended Values
    ///
    /// - ν = 5.0: Default, good balance between robustness and efficiency
    /// - ν = 3.0-4.0: More robust to outliers
    /// - ν = 10.0: Less aggressive, closer to Gaussian
    pub fn new(nu: f64) -> CoreResult<Self> {
        let nu = finite_positive("nu", nu)?;
        Ok(TDistributionLoss {
            nu,
            half_nu_plus_1: (nu + 1.0) / 2.0,
        })
    }
}

impl LossFunction for TDistributionLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        // ρ(s) = (ν + 1)/2 · log(1 + s/ν)
        let inner = 1.0 + s / self.nu;
        let rho = self.half_nu_plus_1 * inner.ln();

        // ρ'(s) = (ν + 1)/(2(ν + s))
        let denom = self.nu + s;
        let rho_prime = self.half_nu_plus_1 / denom;

        // ρ''(s) = -(ν + 1)/(2(ν + s)²)
        let rho_double_prime = -self.half_nu_plus_1 / (denom * denom);

        [rho, rho_prime, rho_double_prime]
    }
}

/// Adaptive Barron loss function (simplified version).
///
/// This is a convenience wrapper around `BarronGeneralLoss` with recommended default
/// parameters for adaptive robust optimization. Based on Chebrolu et al. (2021) RAL paper
/// "Adaptive Robust Kernels for Non-Linear Least Squares Problems".
///
/// # Mathematical Definition
///
/// For α ≠ 0:
/// ```text
/// ρ(s) = |α - 2|/α · ((s/c² + 1)^(α/2) - 1)
/// ```
///
/// For α = 0 (Cauchy-like):
/// ```text
/// ρ(s) = log(s/(2c²) + 1)
/// ```
///
/// where `α` is the shape parameter and `c` is the scale parameter.
///
/// # Properties
///
/// - **Adaptive**: Can approximate many M-estimators (Huber, Cauchy, Geman-McClure, etc.)
/// - **Shape parameter α**: Controls the robustness level
/// - **Scale parameter c**: Controls the transition point
/// - **Unified framework**: Single loss function family
///
/// # Parameter Selection
///
/// **Default (α = 0.0, c = 1.0):**
/// - Cauchy-like behavior
/// - Good general-purpose robust loss
/// - Suitable for moderate to heavy outliers
///
/// **Other values:**
/// - α = 2.0: L2 loss (no robustness)
/// - α = 1.0: Pseudo-Huber/Charbonnier-like
/// - α = -2.0: Geman-McClure-like (very robust)
///
/// # Note on Adaptivity
///
/// This simplified version uses fixed parameters. The full adaptive version from
/// Chebrolu et al. requires iterative estimation of α based on residual distribution,
/// which would require integration into the optimizer's main loop.
///
/// # References
///
/// Chebrolu, N., Läbe, T., Vysotska, O., Behley, J., & Stachniss, C. (2021).
/// Adaptive robust kernels for non-linear least squares problems.
/// IEEE Robotics and Automation Letters, 6(2), 2240-2247.
///
/// Barron, J. T. (2019). A general and adaptive robust loss function.
/// IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR).
///
/// # Example
///
/// ```
/// use apex_solver::core::loss_functions::{LossFunction, AdaptiveBarronLoss};
/// # use apex_solver::core::CoreResult;
/// # fn example() -> CoreResult<()> {
///
/// // Default Cauchy-like behavior
/// let adaptive = AdaptiveBarronLoss::new(0.0, 1.0)?;
///
/// let [rho, rho_prime, _] = adaptive.evaluate(4.0);
/// // Adaptive robust behavior
/// # Ok(())
/// # }
/// # example().unwrap();
/// ```
#[derive(Debug, Clone)]
pub struct AdaptiveBarronLoss {
    inner: BarronGeneralLoss,
}

impl AdaptiveBarronLoss {
    /// Create a new adaptive Barron loss function.
    ///
    /// # Arguments
    ///
    /// * `alpha` - Shape parameter (default: 0.0 for Cauchy-like)
    /// * `scale` - Scale parameter c (must be positive)
    ///
    /// # Recommended Defaults
    ///
    /// - α = 0.0, c = 1.0: General-purpose robust loss
    pub fn new(alpha: f64, scale: f64) -> CoreResult<Self> {
        Ok(AdaptiveBarronLoss {
            inner: BarronGeneralLoss::new(alpha, scale)?,
        })
    }

    /// Create default instance without validation (alpha=0.0, scale=1.0).
    ///
    /// This is safe because the default parameters are mathematically valid.
    const fn new_default() -> Self {
        AdaptiveBarronLoss {
            inner: BarronGeneralLoss {
                alpha: 0.0,
                scale2: 1.0,
            },
        }
    }
}

impl LossFunction for AdaptiveBarronLoss {
    #[inline]
    fn evaluate(&self, s: f64) -> [f64; 3] {
        self.inner.evaluate(s)
    }
}

impl Default for AdaptiveBarronLoss {
    /// Creates default AdaptiveBarronLoss with validated parameters (alpha=0.0, scale=1.0).
    fn default() -> Self {
        Self::new_default()
    }
}

/// Single source of truth mapping loss names to constructors.
///
/// CLI tools and examples must resolve names through
/// [`loss_from_name`] instead of keeping their own match tables, so a new
/// kernel becomes available everywhere (and documented in
/// [`loss_canonical_names`]) the moment it is added here. Matching is
/// case-insensitive; every entry lists all accepted aliases first.
pub fn loss_from_name(
    name: &str,
    scale: Option<f64>,
) -> CoreResult<Box<dyn LossFunction + Send + Sync>> {
    // Each arm picks its own default scale, so there is no second table to
    // drift out of sync. `scale` overrides the default where the loss takes
    // one; L2 and L1 have no scale parameter.
    let s = |default: f64| scale.unwrap_or(default);
    let loss: Box<dyn LossFunction + Send + Sync> = match name.to_lowercase().as_str() {
        "l2" => Box::new(L2Loss),
        "l1" => Box::new(L1Loss),
        "huber" => Box::new(HuberLoss::new(s(1.345))?),
        "cauchy" => Box::new(CauchyLoss::new(s(2.3849))?),
        "fair" => Box::new(FairLoss::new(s(1.3999))?),
        "welsch" => Box::new(WelschLoss::new(s(2.9846))?),
        "tukey" => Box::new(TukeyBiweightLoss::new(s(4.6851))?),
        "geman" | "gemanmcclure" => Box::new(GemanMcClureLoss::new(s(1.0))?),
        "dcs" => Box::new(DcsLoss::new(s(1.0))?),
        "andrews" => Box::new(AndrewsWaveLoss::new(s(1.339))?),
        "ramsay" => Box::new(RamsayEaLoss::new(s(0.3))?),
        "trimmed" | "trimmedmean" => Box::new(TrimmedMeanLoss::new(s(2.0))?),
        "lp" => Box::new(LpNormLoss::new(s(1.5))?),
        "barron0" => Box::new(BarronGeneralLoss::new(0.0, s(1.0))?),
        "barron1" => Box::new(BarronGeneralLoss::new(1.0, s(1.0))?),
        "barron-2" => Box::new(BarronGeneralLoss::new(-2.0, s(1.0))?),
        "t-distribution" | "tdistribution" => Box::new(TDistributionLoss::new(s(5.0))?),
        "adaptive-barron" | "adaptivebarron" => Box::new(AdaptiveBarronLoss::new(0.0, s(1.0))?),
        other => {
            return Err(CoreError::InvalidInput(format!(
                "unknown loss function: {other}. Valid options: {}",
                loss_canonical_names().join(", ")
            )));
        }
    };
    Ok(loss)
}

/// Canonical (first-alias) name of every registered loss, for help text and
/// parity tests. Keep in the same order as [`loss_from_name`].
pub fn loss_canonical_names() -> Vec<&'static str> {
    vec![
        "l2",
        "l1",
        "huber",
        "cauchy",
        "fair",
        "welsch",
        "tukey",
        "geman",
        "dcs",
        "andrews",
        "ramsay",
        "trimmed",
        "lp",
        "barron0",
        "barron1",
        "barron-2",
        "t-distribution",
        "adaptive-barron",
    ]
}

#[cfg(test)]
mod tests {
    use super::*;

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

    const EPSILON: f64 = 1e-6;

    /// Helper function to test derivatives numerically
    fn numerical_derivative(loss: &dyn LossFunction, s: f64, h: f64) -> (f64, f64) {
        let [rho_plus, _, _] = loss.evaluate(s + h);
        let [rho_minus, _, _] = loss.evaluate(s - h);
        let [rho, _, _] = loss.evaluate(s);

        // First derivative: f'(x) ≈ (f(x+h) - f(x-h)) / (2h)
        let rho_prime_numerical = (rho_plus - rho_minus) / (2.0 * h);

        // Second derivative: f''(x) ≈ (f(x+h) - 2f(x) + f(x-h)) / h²
        let rho_double_prime_numerical = (rho_plus - 2.0 * rho + rho_minus) / (h * h);

        (rho_prime_numerical, rho_double_prime_numerical)
    }

    #[test]
    fn test_l2_loss() -> TestResult {
        let loss = L2Loss;

        // Test at s = 0
        let [rho, rho_prime, rho_double_prime] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert_eq!(rho_prime, 1.0);
        assert_eq!(rho_double_prime, 0.0);

        // Test at s = 4.0
        let [rho, rho_prime, rho_double_prime] = loss.evaluate(4.0);
        assert_eq!(rho, 4.0);
        assert_eq!(rho_prime, 1.0);
        assert_eq!(rho_double_prime, 0.0);

        Ok(())
    }

    #[test]
    fn test_l1_loss() -> TestResult {
        let loss = L1Loss;

        // Test at s = 0 (should handle gracefully)
        let [rho, rho_prime, rho_double_prime] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert!(rho_prime.is_finite());
        assert!(rho_double_prime.is_finite());

        // Test at s = 4.0 (√s = 2.0, ρ(s) = 2√s = 4.0). ISSUE-0011: rho is
        // now smoothed near the origin, so it matches true L1 only up to
        // the smoothing scale, not to `EPSILON`, even at s=4.
        let [rho, rho_prime, _] = loss.evaluate(4.0);
        assert!((rho - 4.0).abs() < 1e-5); // ρ(s) = 2√s = 2*2 = 4
        assert!((rho_prime - 0.5).abs() < EPSILON); // ρ'(s) = 1/√s = 1/2 = 0.5

        Ok(())
    }

    /// Squared-residual sample grid for the contract sweeps: from the
    /// inlier region, through each redescending kernel's threshold, out
    /// five decades into the saturated outlier tail.
    ///
    /// `1.0` and `4.0` land exactly on DCS's Φ and the trimmed mean's `c²`
    /// (both default scales) — deliberately, so the kink handling below is
    /// exercised rather than dodged by a lucky grid.
    const CONTRACT_S: &[f64] = &[
        0.0, 1e-3, 0.05, 0.1, 0.3, 0.7, 1.0, 2.0, 4.0, 10.0, 25.0, 50.0, 200.0, 1e4,
    ];

    /// Radius around a declared derivative cliff within which the analytic
    /// `[ρ, ρ′, ρ″]` is one-sided and therefore not comparable to a
    /// straddling central difference. Finiteness is still required there.
    const KINK_RADIUS: f64 = 1e-3;

    /// Derivative cliffs of each registered kernel **at the default scale
    /// `loss_from_name` uses** — keep them in sync with that function.
    ///
    /// The values are `s`, not `√s`: a kernel branches on `x = √s`, so the
    /// switch point in `s` is that threshold squared. An *undeclared* cliff
    /// is not silently tolerated: the derivative contract below fails at
    /// that sample and the cliff gets added here.
    fn derivative_kinks(name: &str) -> Vec<f64> {
        match name {
            // ρ′ = 1 inside `c²`, ρ″ = −1/(2c²) outside; ρ′ continuous.
            "huber" => vec![1.345f64 * 1.345],
            // ρ′ = 1 for `s ≤ Φ`, ρ′ = 0 above — a value cliff.
            "dcs" => vec![1.0],
            // ρ′ → 0 continuously at `πc`, ρ″ jumps to the plateau's 0.
            "andrews" => {
                let t = std::f64::consts::PI * 1.339;
                vec![t * t]
            }
            // ρ′ = 1/2 inside `c²`, ρ′ = 0 above — a value cliff.
            "trimmed" => vec![4.0],
            _ => Vec::new(),
        }
    }

    /// Is `s` too close to a declared cliff for a central difference?
    fn near_kink(name: &str, s: f64) -> bool {
        derivative_kinks(name)
            .iter()
            .any(|k| (s - k).abs() < KINK_RADIUS)
    }

    /// ISSUE-0005 / GH-57, widened by the Round-2 audit (F5): `rho'`/`rho''`
    /// must be the actual derivatives of the loss's own returned `rho`, for
    /// **every** kernel in the registry — not just the Cauchy/Fair/Tukey/
    /// Andrews quartet the original table covered — table-driven against
    /// central differences over [`CONTRACT_S`], which runs five decades past
    /// each kernel's inlier region into its outlier regime.
    ///
    /// Two samples are excluded from the derivative comparison, both
    /// deliberately:
    ///
    /// * `s < 10h`: the stencil would reach `s − h < 0` and take a square
    ///   root of it. The kernels are only defined for `s ≥ 0`, and the
    ///   origin-smoothing branch is covered by its own continuity tests
    ///   (ISSUE-0011).
    /// * [`near_kink`] — a declared derivative cliff, where the analytic
    ///   value is one-sided by definition, so comparing it to a straddling
    ///   difference would assert a falsehood. Finiteness is still required
    ///   there.
    ///
    /// On top of finiteness and the derivative identity, the sweep pins two
    /// contracts the `Corrector` leans on:
    ///
    /// * `ρ ≥ 0` — a negative robust cost would make "better" mean "more
    ///   negative cost", inverting step acceptance.
    /// * `ρ′ ≥ 0` — `Corrector::new` clamps a negative weight to zero, so
    ///   a declining kernel would suppress the block's gradient while
    ///   `robust_cost()` kept moving: the ISSUE-0010 split, housed in the
    ///   kernel instead of the corrector. (All redescending kernels here
    ///   go *flat* rather than declining, which is why they pass.)
    ///
    /// Every kernel must leave at least 10 of the 14 grid samples eligible
    /// for the derivative comparison (L2 leaves 13; the cliff-bearers leave
    /// 12), so a grid or cliff list that drifts into skipping most of the
    /// sweep fails instead of passing vacuously.
    #[test]
    fn test_robust_loss_derivative_contract_table() -> TestResult {
        const H: f64 = 1e-5;
        let names = loss_canonical_names();
        assert!(
            names.len() >= 16,
            "registry shrank to {} kernels — the contract table only means \
             something if it covers them all",
            names.len()
        );

        for name in &names {
            let loss = loss_from_name(name, None)?;
            let kinks = derivative_kinks(name);
            let mut derivative_samples = 0usize;

            for &s in CONTRACT_S {
                let [rho, rho_prime, rho_double_prime] = loss.evaluate(s);
                assert!(
                    rho.is_finite() && rho_prime.is_finite() && rho_double_prime.is_finite(),
                    "{name} s={s}: non-finite output [{rho}, {rho_prime}, {rho_double_prime}]"
                );
                assert!(rho >= 0.0, "{name} s={s}: negative robust cost rho={rho}");
                assert!(
                    rho_prime >= 0.0,
                    "{name} s={s}: negative weight rho'={rho_prime} — Corrector would clamp \
                     it to zero and the block would stop moving while the cost still did"
                );

                if s < 10.0 * H || near_kink(name, s) {
                    continue;
                }
                derivative_samples += 1;

                let (num_prime, num_double_prime) = numerical_derivative(loss.as_ref(), s, H);
                let prime_err = (rho_prime - num_prime).abs();
                let double_prime_err = (rho_double_prime - num_double_prime).abs();
                assert!(
                    prime_err < 1e-3 * (1.0 + rho_prime.abs()),
                    "{name} s={s}: rho'={rho_prime} vs numeric {num_prime} (err={prime_err})"
                );
                assert!(
                    double_prime_err < 1e-2 * (1.0 + rho_double_prime.abs()),
                    "{name} s={s}: rho''={rho_double_prime} vs numeric {num_double_prime} \
                     (err={double_prime_err})"
                );
            }

            assert!(
                derivative_samples >= 10,
                "{name}: only {derivative_samples} admissible derivative samples — \
                 too close to skipped samples for the sweep to mean anything"
            );

            // The cliff list is only meaningful if the grid actually walks
            // up to and past each declared cliff, so a kernel whose cliff
            // drifted off the grid (e.g. a changed default scale) is caught
            // rather than silently skipped forever.
            for k in &kinks {
                assert!(
                    CONTRACT_S.iter().any(|&s| (s - k).abs() < 10.0),
                    "{name}: declared cliff at s={k} is nowhere near the sample grid"
                );
            }
        }
        Ok(())
    }

    /// F5: the `Corrector` contract over **every** registered kernel, in
    /// each of the regimes its branchy `new()` can take.
    ///
    /// `Corrector::new` has three exits:
    ///
    /// * `sq_norm == 0 || ρ″ ≤ 0 || ρ′ ≤ 0` → early return, `α = 0`.
    ///   The `ρ″ ≤ 0` half is the *common* case — every classic kernel has
    ///   non-positive curvature — and `ρ′ ≤ 0` is the suppression path a
    ///   redescending kernel reaches past its threshold.
    /// * `ρ′ > 0 && ρ″ > 0` → the general `α` path, which **no registered
    ///   kernel reaches at its default scale**: the sweep appends two
    ///   configurations (`LpNorm` with `p = 3`, Barron with `α = 3`, both
    ///   of which have positive curvature) precisely so the third branch is
    ///   not dead code in a green run.
    ///
    /// The sweep therefore asserts *branch coverage* alongside the
    /// property, because a green run that never entered a branch says
    /// nothing about it. The property itself generalizes ISSUE-0010 to
    /// every kernel: whatever branch is taken, `J̃ᵀr̃` — the Gauss–Newton
    /// gradient the solver actually descends — must equal the true
    /// derivative of `robust_cost()` w.r.t. state, and every corrected
    /// quantity must stay finite.
    ///
    /// The residual is two-dimensional with a non-collinear Jacobian
    /// column, so the `u_c = ⟨r, col⟩` term inside the α correction is
    /// actually exercised rather than collapsing to a scalar identity.
    ///
    /// Measured on [`CONTRACT_S`] across the 20 configurations: 21 samples
    /// took the `ρ′ ≤ 0` suppression path, 26 took the `ρ″ > 0` α path, and
    /// 235 of 280 produced a cost change large enough to differentiate
    /// numerically (the rest are exact plateaus or tails sitting below the
    /// cost's own rounding error) — the three coverage assertions below
    /// keep those from quietly going to zero.
    #[test]
    fn test_corrector_gradient_consistency_for_every_loss() -> TestResult {
        use crate::core::corrector::Corrector;

        const H: f64 = 1e-6;
        /// ∂C/∂x at `x = 0` for `r(x) = r₀ + J·x`, `C = 0.5·ρ(‖r‖²)`.
        fn true_gradient(rho_prime: f64, r0: &[f64; 2], j: &[f64; 2]) -> f64 {
            rho_prime * (r0[0] * j[0] + r0[1] * j[1])
        }

        let mut configs: Vec<(String, Box<dyn LossFunction + Send + Sync>)> = Vec::new();
        for name in loss_canonical_names() {
            configs.push((name.to_string(), loss_from_name(name, None)?));
        }
        configs.push((
            "lp p=3 (rho''>0)".to_string(),
            Box::new(LpNormLoss::new(3.0)?),
        ));
        configs.push((
            "barron alpha=3 (rho''>0)".to_string(),
            Box::new(BarronGeneralLoss::new(3.0, 1.0)?),
        ));

        // Fixed Jacobian column, deliberately not aligned with the residual
        // so the α correction's ⟨r, J⟩ projection is non-trivial.
        let j = [1.0, -0.3];
        let mut suppressed_cases = 0usize;
        let mut curved_cases = 0usize;
        let mut numeric_checks = 0usize;
        let mut samples = 0usize;

        for (name, loss) in &configs {
            for &s in CONTRACT_S {
                samples += 1;
                let rho = loss.evaluate(s);

                // Branch bookkeeping: this is `Corrector::new`'s dispatch
                // condition, read off the public derivatives.
                if rho[1] <= 0.0 {
                    suppressed_cases += 1;
                } else if rho[2] > 0.0 && s > 0.0 {
                    curved_cases += 1;
                }

                // Two rows whose squared norm is exactly `s`.
                let half = (s / 2.0).sqrt();
                let r0 = [half, half];

                let corrector = Corrector::new(loss.as_ref(), s);
                assert!(
                    corrector.robust_cost().is_finite(),
                    "{name} s={s}: non-finite robust cost"
                );
                assert_eq!(
                    corrector.robust_cost(),
                    0.5 * rho[0],
                    "{name} s={s}: robust_cost must be 0.5*rho, not 0.5*||r_tilde||^2"
                );

                // Modeled gradient: the corrected Jacobian sees the *original*
                // residual (that is the order both call sites use), the
                // corrected residual is what it is paired with.
                let mut modeled_residual = r0;
                let mut modeled_jacobian = j;
                corrector.correct_jacobian_in_place(&modeled_residual, &mut modeled_jacobian, 2, 1);
                corrector.correct_residual_in_place(&mut modeled_residual);
                let modeled = modeled_jacobian[0] * modeled_residual[0]
                    + modeled_jacobian[1] * modeled_residual[1];

                assert!(
                    modeled.is_finite() && modeled_residual.iter().all(|v| v.is_finite()),
                    "{name} s={s}: non-finite corrected residual/Jacobian [{modeled:?}]"
                );

                // True gradient of robust_cost() under a linear residual
                // model r(x) = r0 + J·x, by central difference.
                let cost = |x: f64| {
                    let rx = [r0[0] + j[0] * x, r0[1] + j[1] * x];
                    let sx = rx[0] * rx[0] + rx[1] * rx[1];
                    Corrector::new(loss.as_ref(), sx).robust_cost()
                };
                let expected = true_gradient(rho[1], &r0, &j);
                assert!(
                    (modeled - expected).abs() <= 1e-6 * (1.0 + expected.abs()),
                    "{name} s={s}: modeled gradient {modeled} vs analytic \
                     rho'*(r.J) = {expected} — the correction algebra does not \
                     reproduce the loss's own weight"
                );

                // The cost-based check needs a *resolvable* difference: a
                // change below the cost's own rounding error is pure noise.
                // Ramsay at s=1e4 is the extreme case — its cost still
                // moves by ~1e-18 per step while sitting at ~5.6 — and a
                // flat plateau (Tukey/trimmed/DCS past their thresholds)
                // is exactly constant. Both are covered by the analytic
                // check above instead.
                let delta = cost(H) - cost(-H);
                if near_kink(name, s) || delta.abs() <= 1e-14 * (1.0 + cost(0.0).abs()) {
                    continue;
                }
                numeric_checks += 1;

                let numeric = delta / (2.0 * H);
                assert!(
                    (modeled - numeric).abs() <= 1e-4 * (1.0 + numeric.abs()),
                    "{name} s={s}: modeled gradient {modeled} vs numeric derivative of \
                     robust_cost {numeric} — the solver would descend a cost it is \
                     not measuring"
                );
            }
        }

        assert!(
            suppressed_cases > 0,
            "no sampled (kernel, s) had rho' <= 0 — the suppression path went untested \
             ({suppressed_cases} suppressed, {curved_cases} curved)"
        );
        assert!(
            curved_cases > 0,
            "no sampled (kernel, s) had rho'' > 0 — the general alpha path went untested \
             ({suppressed_cases} suppressed, {curved_cases} curved)"
        );
        assert!(
            numeric_checks * 4 >= samples,
            "only {numeric_checks}/{samples} samples had a resolvable cost change — \
             the numeric half of the property went largely unexercised"
        );
        Ok(())
    }

    /// Andrews' removable singularity at `s=0`: the code's own small-s
    /// series branch must agree with the closed-form branch just outside its
    /// threshold, and both must agree with the analytic limits `rho'(0)=1/2`,
    /// `rho''(0)=-1/(12c²)`.
    #[test]
    fn test_andrews_wave_loss_small_s_continuity() -> TestResult {
        let scale = 1.339;
        let loss = AndrewsWaveLoss::new(scale)?;

        let [_, rho_prime_0, rho_double_prime_0] = loss.evaluate(0.0);
        assert!((rho_prime_0 - 0.5).abs() < 1e-12);
        assert!((rho_double_prime_0 - (-1.0 / (12.0 * scale * scale))).abs() < 1e-12);

        for &s in &[1e-7, 1e-6, 1e-5, 1e-4] {
            let [_, rho_prime, rho_double_prime] = loss.evaluate(s);
            assert!(
                (rho_prime - 0.5).abs() < 1e-4,
                "s={s}: rho'={rho_prime} far from the s=0 limit 0.5"
            );
            assert!(
                rho_double_prime.is_finite(),
                "s={s}: rho'' must stay finite near the branch boundary"
            );
        }
        Ok(())
    }

    #[test]
    fn test_fair_loss() -> TestResult {
        let loss = FairLoss::new(1.3999)?;

        // Test at s = 0 (special case handling)
        let [rho, rho_prime, rho_double_prime] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert_eq!(rho_prime, 1.0);
        assert_eq!(rho_double_prime, 0.0);

        // Test inlier region (s = 1.0)
        let [_, rho_prime, _] = loss.evaluate(1.0);
        assert!(rho_prime > 0.28 && rho_prime < 0.30); // ρ'(s) = c/(2(c+|x|)) where x=1, c≈1.4 → ~0.292

        // Test outlier region (s = 100.0)
        let [_, rho_prime_outlier, _] = loss.evaluate(100.0);
        assert!(rho_prime_outlier < rho_prime); // Weight should decrease

        // Test that derivatives are finite and reasonable
        let [_, rho_prime_4, rho_double_prime_4] = loss.evaluate(4.0);
        assert!(rho_prime_4.is_finite() && rho_prime_4 > 0.0);
        assert!(rho_double_prime_4.is_finite() && rho_double_prime_4 < 0.0); // Convex near origin

        Ok(())
    }

    #[test]
    fn test_geman_mcclure_loss() -> TestResult {
        let loss = GemanMcClureLoss::new(1.0)?;

        // Test at s = 0
        let [rho, rho_prime, _] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert!((rho_prime - 1.0).abs() < EPSILON);

        // Test outlier suppression
        let [_, rho_prime_small, _] = loss.evaluate(1.0);
        let [_, rho_prime_large, _] = loss.evaluate(100.0);
        assert!(rho_prime_large < rho_prime_small);
        assert!(rho_prime_large < 0.1); // Strong suppression

        // Verify derivatives
        let s = 2.0;
        let [_, rho_prime, rho_double_prime] = loss.evaluate(s);
        let (rho_prime_num, rho_double_prime_num) = numerical_derivative(&loss, s, 1e-5);
        assert!((rho_prime - rho_prime_num).abs() < 1e-4);
        assert!((rho_double_prime - rho_double_prime_num).abs() < 1e-3);

        Ok(())
    }

    #[test]
    fn test_welsch_loss() -> TestResult {
        let loss = WelschLoss::new(2.9846)?;

        // Test at s = 0: ρ'(0) = 0.5 * exp(0) = 0.5
        let [rho, rho_prime, _] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert!((rho_prime - 0.5).abs() < EPSILON); // ρ'(s) = 0.5 * exp(-s/c²), at s=0: 0.5

        // Test redescending behavior
        let [_, rho_prime_10, _] = loss.evaluate(10.0);
        let [_, rho_prime_100, _] = loss.evaluate(100.0);
        assert!(rho_prime_100 < rho_prime_10);
        assert!(rho_prime_100 < 0.01); // Nearly zero for large outliers

        // Verify derivatives
        let s = 5.0;
        let [_, rho_prime, rho_double_prime] = loss.evaluate(s);
        let (rho_prime_num, rho_double_prime_num) = numerical_derivative(&loss, s, 1e-5);
        assert!((rho_prime - rho_prime_num).abs() < 1e-4);
        assert!((rho_double_prime - rho_double_prime_num).abs() < 1e-3);

        Ok(())
    }

    #[test]
    fn test_tukey_biweight_loss() -> TestResult {
        let loss = TukeyBiweightLoss::new(4.6851)?;

        // Test at s = 0: ρ'(0) = 0.5 * (1-0)^2 = 0.5
        let [rho, rho_prime, _] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert!((rho_prime - 0.5).abs() < EPSILON); // ρ'(s) = 0.5 * (1 - ratio²)², at s=0: 0.5

        // Test within threshold
        let scale2 = 4.6851 * 4.6851;
        let [_, rho_prime_in, _] = loss.evaluate(scale2 * 0.5);
        assert!(rho_prime_in > 0.05);

        // Test beyond threshold (complete suppression)
        let [_, rho_prime_out, _] = loss.evaluate(scale2 * 1.5);
        assert_eq!(rho_prime_out, 0.0);

        // Test that derivatives are finite and reasonable
        let [_, rho_prime_5, rho_double_prime_5] = loss.evaluate(5.0);
        assert!(rho_prime_5.is_finite() && rho_prime_5 > 0.0);
        assert!(rho_double_prime_5.is_finite() && rho_double_prime_5 < 0.0);

        Ok(())
    }

    #[test]
    fn test_andrews_wave_loss() -> TestResult {
        let loss = AndrewsWaveLoss::new(1.339)?;

        // Test at s = 0: ρ'(s) = c·sin(x/c)/(2x) → 1/2 as x→0 (analytic limit
        // of ρ(s) = c²(1-cos(x/c)) itself, not the 0 a naive 0/0 gives).
        let [rho, rho_prime, _] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert!((rho_prime - 0.5).abs() < EPSILON);

        // Test within threshold (small s where sin(x/c) gives moderate weight)
        let [_, rho_prime_in, _] = loss.evaluate(1.0);
        assert!(rho_prime_in > 0.44 && rho_prime_in < 0.47); // ~0.455

        // Test beyond threshold
        let scale = 1.339;
        let [_, rho_prime_out, _] = loss.evaluate((scale * std::f64::consts::PI + 0.1).powi(2));
        assert!(rho_prime_out.abs() < 0.01);

        // Test that derivatives are finite
        let [_, rho_prime_1, rho_double_prime_1] = loss.evaluate(1.0);
        assert!(rho_prime_1.is_finite() && rho_prime_1 > 0.0);
        assert!(rho_double_prime_1.is_finite());

        Ok(())
    }

    #[test]
    fn test_ramsay_ea_loss() -> TestResult {
        let loss = RamsayEaLoss::new(0.3)?;

        // Test at s = 0 (should handle gracefully)
        let [rho, _, _] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);

        // Test exponential decay behavior
        let [_, rho_prime_small, _] = loss.evaluate(1.0);
        let [_, rho_prime_large, _] = loss.evaluate(100.0);
        assert!(rho_prime_large < rho_prime_small);

        // Verify derivatives
        let s = 4.0;
        let [_, rho_prime, rho_double_prime] = loss.evaluate(s);
        let (rho_prime_num, rho_double_prime_num) = numerical_derivative(&loss, s, 1e-5);
        assert!((rho_prime - rho_prime_num).abs() < 1e-4);
        assert!((rho_double_prime - rho_double_prime_num).abs() < 1e-3);

        Ok(())
    }

    #[test]
    fn test_trimmed_mean_loss() -> TestResult {
        let loss = TrimmedMeanLoss::new(2.0)?;
        let scale2 = 4.0;

        // Test below threshold (L2 behavior)
        let [rho, rho_prime, rho_double_prime] = loss.evaluate(2.0);
        assert!((rho - 1.0).abs() < EPSILON);
        assert!((rho_prime - 0.5).abs() < EPSILON);
        assert_eq!(rho_double_prime, 0.0);

        // Test above threshold (constant)
        let [rho_out, rho_prime_out, rho_double_prime_out] = loss.evaluate(10.0);
        assert!((rho_out - scale2 / 2.0).abs() < EPSILON);
        assert_eq!(rho_prime_out, 0.0);
        assert_eq!(rho_double_prime_out, 0.0);

        Ok(())
    }

    #[test]
    fn test_lp_norm_loss() -> TestResult {
        // Test L1 (p = 1). ISSUE-0011: smoothed near the origin, so this
        // matches true L1 (||r||₁ = 2) only up to the smoothing scale.
        let l1 = LpNormLoss::new(1.0)?;
        let [rho_l1, _, _] = l1.evaluate(4.0);
        assert!((rho_l1 - 2.0).abs() < 1e-5);

        // Test L2 (p = 2)
        let l2 = LpNormLoss::new(2.0)?;
        let [rho_l2, rho_prime_l2, rho_double_prime_l2] = l2.evaluate(4.0);
        assert!((rho_l2 - 4.0).abs() < EPSILON);
        assert!((rho_prime_l2 - 1.0).abs() < EPSILON);
        assert_eq!(rho_double_prime_l2, 0.0);

        // Test fractional p (p = 0.5)
        let l05 = LpNormLoss::new(0.5)?;
        let [_, rho_prime_05, _] = l05.evaluate(4.0);
        assert!(rho_prime_05 < 1.0); // Robust behavior

        // Verify derivatives for p = 1.5
        let loss = LpNormLoss::new(1.5)?;
        let s = 4.0;
        let [_, rho_prime, rho_double_prime] = loss.evaluate(s);
        let (rho_prime_num, rho_double_prime_num) = numerical_derivative(&loss, s, 1e-5);
        assert!((rho_prime - rho_prime_num).abs() < 1e-4);
        assert!((rho_double_prime - rho_double_prime_num).abs() < 1e-3);

        Ok(())
    }

    /// ISSUE-0011 regression: value and derivative must stay continuous
    /// across the *old* `s < f64::EPSILON` guard boundary — the previous
    /// splice into the unrelated L2 branch jumped by orders of magnitude
    /// right there, for every `p` including L1's own `p=1` special case.
    #[test]
    fn test_l1_lp_continuous_across_old_guard_boundary() -> TestResult {
        // The bug was an ~8-order-of-magnitude *relative* jump right at the
        // old branch (e.g. L1's rho' went 1.0 -> 6.7e7); check the new
        // single-formula value/derivative agree within a small relative
        // tolerance across that same boundary, rather than an absolute one
        // that would be miscalibrated for small p's steeper local slope.
        let eps = f64::EPSILON;
        for p in [0.5, 1.0, 1.5, 2.0] {
            let loss = LpNormLoss::new(p)?;
            let below = loss.evaluate(eps / 2.0);
            let above = loss.evaluate(eps * 2.0);
            let prime_scale = 1.0 + below[1].abs().max(above[1].abs());
            assert!(
                (above[1] - below[1]).abs() < 1e-3 * prime_scale,
                "p={p}: rho' jumps from {} to {} across the old guard boundary",
                below[1],
                above[1]
            );
        }

        let l1 = L1Loss::new();
        let below = l1.evaluate(eps / 2.0);
        let above = l1.evaluate(eps * 2.0);
        let prime_scale = 1.0 + below[1].abs().max(above[1].abs());
        assert!((above[1] - below[1]).abs() < 1e-3 * prime_scale);

        Ok(())
    }

    /// Sweep `s` geometrically from the origin through the old guard's
    /// transition region and confirm both value and derivative vary
    /// smoothly — no discontinuity anywhere — for `p ∈ {0.5, 1, 1.5, 2}`.
    #[test]
    fn test_lp_norm_smooth_sweep_through_origin() -> TestResult {
        for p in [0.5, 1.0, 1.5, 2.0] {
            let loss = LpNormLoss::new(p)?;
            let mut prev: Option<[f64; 3]> = None;
            let mut s = 1e-20;
            while s < 1.0 {
                let cur = loss.evaluate(s);
                assert!(
                    cur.iter().all(|v| v.is_finite()),
                    "p={p} s={s}: non-finite output {cur:?}"
                );
                if let Some(prev_vals) = prev {
                    let d_rho = (cur[0] - prev_vals[0]).abs();
                    assert!(
                        d_rho < 0.5,
                        "p={p} s={s}: rho jumped by {d_rho} between geometrically \
                         adjacent samples (10x apart in s)"
                    );
                }
                prev = Some(cur);
                s *= 10.0;
            }
        }
        Ok(())
    }

    /// One-dimensional LM/DogLeg-style check: `Corrector::robust_cost()` is
    /// exactly what step acceptance compares, so as a residual's magnitude
    /// crosses the old guard boundary, it must vary continuously with state
    /// — not jump, the way the previous L2 splice made it.
    #[test]
    fn test_l1_robust_cost_continuous_across_guard_boundary() {
        use crate::core::corrector::Corrector;

        // Fine (20%) steps confined to the immediate neighborhood of the OLD
        // `s < f64::EPSILON` guard — where the bug actually was — rather
        // than a coarse sweep across many decades, where cost legitimately
        // changes a lot per step and isn't a "boundary jump" at all.
        let loss = L1Loss::new();
        let eps = f64::EPSILON;
        let mut prev_cost: Option<f64> = None;
        let mut s = eps / 8.0;
        while s < eps * 8.0 {
            let cost = Corrector::new(&loss, s).robust_cost();
            if let Some(p) = prev_cost {
                assert!(
                    (cost - p).abs() < 1e-8,
                    "s={s}: robust_cost jumped from {p} to {cost}"
                );
            }
            prev_cost = Some(cost);
            s *= 1.2;
        }
    }

    #[test]
    fn test_barron_general_loss_special_cases() -> TestResult {
        // α = 0 (Cauchy-like)
        let cauchy = BarronGeneralLoss::new(0.0, 1.0)?;
        let [_, rho_prime_small, _] = cauchy.evaluate(1.0);
        let [_, rho_prime_large, _] = cauchy.evaluate(100.0);
        assert!(rho_prime_large < rho_prime_small);

        // α = 2 (L2, scaled): ρ(s) = s/(2c²)
        let l2 = BarronGeneralLoss::new(2.0, 1.0)?;
        let [rho, rho_prime, rho_double_prime] = l2.evaluate(4.0);
        assert!((rho - 2.0).abs() < EPSILON);
        assert!((rho_prime - 0.5).abs() < EPSILON);
        assert!(rho_double_prime.abs() < EPSILON);

        // α = 1 (Charbonnier-like)
        let charbonnier = BarronGeneralLoss::new(1.0, 1.0)?;
        let [_, rho_prime_char, _] = charbonnier.evaluate(4.0);
        assert!(rho_prime_char > 0.0 && rho_prime_char < 1.0);

        // Test α = -2 (Geman-McClure-like) - strong outlier suppression
        let gm = BarronGeneralLoss::new(-2.0, 1.0)?;
        let [_, rho_prime_small, _] = gm.evaluate(1.0);
        let [_, rho_prime_large, _] = gm.evaluate(100.0);
        assert!(rho_prime_large < rho_prime_small); // Redescending behavior
        assert!(rho_prime_large < 0.1); // Strong suppression

        Ok(())
    }

    /// ISSUE-0006 regression: `BarronGeneralLoss`'s returned `rho'`/`rho''`
    /// must be the actual derivatives of its own returned `rho`, `rho(0)=0`,
    /// and cost must stay nonnegative for every alpha — including negative
    /// alpha, where the previous `|α|`-normalized formula went negative.
    #[test]
    fn test_barron_general_loss_derivative_contract() -> TestResult {
        for alpha in [2.0, 1.0, 0.5, 0.0, -1.0, -2.0, -10.0, -100.0] {
            let loss = BarronGeneralLoss::new(alpha, 1.0)?;

            let [rho0, _, _] = loss.evaluate(0.0);
            assert!(
                rho0.abs() < 1e-9,
                "alpha={alpha}: rho(0) = {rho0}, expected 0"
            );

            for &s in &[1e-3, 0.5, 1.0, 4.0, 25.0, 100.0] {
                let [rho, rho_prime, rho_double_prime] = loss.evaluate(s);
                assert!(rho.is_finite() && rho_prime.is_finite() && rho_double_prime.is_finite());
                assert!(
                    rho >= -1e-9,
                    "alpha={alpha} s={s}: rho={rho} is negative (ISSUE-0006 reproduction)"
                );

                let (num_prime, num_double_prime) = numerical_derivative(&loss, s, 1e-4);
                let prime_err = (rho_prime - num_prime).abs();
                let double_prime_err = (rho_double_prime - num_double_prime).abs();
                assert!(
                    prime_err < 1e-3 * (1.0 + rho_prime.abs()),
                    "alpha={alpha} s={s}: rho'={rho_prime} vs numeric {num_prime}"
                );
                assert!(
                    double_prime_err < 1e-2 * (1.0 + rho_double_prime.abs()),
                    "alpha={alpha} s={s}: rho''={rho_double_prime} vs numeric {num_double_prime}"
                );
            }
        }
        Ok(())
    }

    /// The general branch must agree with the special-cased α→0 and α→2
    /// limits just outside the 1e-6 switch-over threshold, in both value and
    /// first derivative.
    #[test]
    fn test_barron_general_loss_continuous_at_shape_limits() -> TestResult {
        let scale = 1.7;
        let s = 3.0;

        for &(alpha, name) in &[(0.0, "alpha=0"), (2.0, "alpha=2")] {
            let at_limit = BarronGeneralLoss::new(alpha, scale)?.evaluate(s);
            for delta in [1e-5, 1e-4, 1e-3] {
                let near = BarronGeneralLoss::new(alpha + delta, scale)?.evaluate(s);
                let rho_err = (at_limit[0] - near[0]).abs();
                let prime_err = (at_limit[1] - near[1]).abs();
                assert!(
                    rho_err < 10.0 * delta,
                    "{name}: rho discontinuous approaching from +{delta}: {rho_err}"
                );
                assert!(
                    prime_err < 10.0 * delta,
                    "{name}: rho' discontinuous approaching from +{delta}: {prime_err}"
                );

                let near_below = BarronGeneralLoss::new(alpha - delta, scale)?.evaluate(s);
                let rho_err_below = (at_limit[0] - near_below[0]).abs();
                assert!(
                    rho_err_below < 10.0 * delta,
                    "{name}: rho discontinuous approaching from -{delta}: {rho_err_below}"
                );
            }
        }
        Ok(())
    }

    #[test]
    fn test_constructor_validation() -> TestResult {
        // Test that negative or zero scale parameters are rejected
        assert!(FairLoss::new(0.0).is_err());
        assert!(FairLoss::new(-1.0).is_err());
        assert!(GemanMcClureLoss::new(0.0).is_err());
        assert!(WelschLoss::new(-1.0).is_err());
        assert!(TukeyBiweightLoss::new(0.0).is_err());
        assert!(AndrewsWaveLoss::new(-1.0).is_err());
        assert!(RamsayEaLoss::new(0.0).is_err());
        assert!(TrimmedMeanLoss::new(-1.0).is_err());
        assert!(BarronGeneralLoss::new(0.0, 0.0).is_err());
        assert!(BarronGeneralLoss::new(1.0, -1.0).is_err());

        // Test that p ≤ 0 is rejected for LpNormLoss
        assert!(LpNormLoss::new(0.0).is_err());
        assert!(LpNormLoss::new(-1.0).is_err());

        // Test valid constructors
        assert!(FairLoss::new(1.0).is_ok());
        assert!(LpNormLoss::new(1.5).is_ok());
        assert!(BarronGeneralLoss::new(1.0, 1.0).is_ok());

        Ok(())
    }

    /// ISSUE-0007 regression: every parameterized-loss constructor must
    /// reject `NaN` and `+infinity` (not just non-positive finite values) —
    /// a bare `<= 0.0` guard lets both through, since every comparison with
    /// `NaN` is `false`. `-infinity` is finite-checked the same way as any
    /// other negative value (rejected by the positivity check).
    #[test]
    fn test_constructor_rejects_nonfinite_scale_parameters() {
        for bad in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY, -0.5, 0.0] {
            assert!(HuberLoss::new(bad).is_err(), "Huber({bad})");
            assert!(CauchyLoss::new(bad).is_err(), "Cauchy({bad})");
            assert!(FairLoss::new(bad).is_err(), "Fair({bad})");
            assert!(GemanMcClureLoss::new(bad).is_err(), "GemanMcClure({bad})");
            assert!(DcsLoss::new(bad).is_err(), "Dcs({bad})");
            assert!(WelschLoss::new(bad).is_err(), "Welsch({bad})");
            assert!(TukeyBiweightLoss::new(bad).is_err(), "Tukey({bad})");
            assert!(AndrewsWaveLoss::new(bad).is_err(), "Andrews({bad})");
            assert!(RamsayEaLoss::new(bad).is_err(), "Ramsay({bad})");
            assert!(TrimmedMeanLoss::new(bad).is_err(), "TrimmedMean({bad})");
            assert!(LpNormLoss::new(bad).is_err(), "Lp({bad})");
            assert!(TDistributionLoss::new(bad).is_err(), "TDistribution({bad})");
            assert!(
                BarronGeneralLoss::new(1.0, bad).is_err(),
                "Barron(1,{bad}) scale"
            );
            assert!(
                AdaptiveBarronLoss::new(1.0, bad).is_err(),
                "AdaptiveBarron(1,{bad})"
            );
        }
        // Barron's alpha is finite-only (no positivity requirement).
        assert!(BarronGeneralLoss::new(f64::NAN, 1.0).is_err());
        assert!(BarronGeneralLoss::new(f64::INFINITY, 1.0).is_err());
        assert!(BarronGeneralLoss::new(f64::NEG_INFINITY, 1.0).is_err());
        // A negative finite alpha is a valid shape parameter.
        assert!(BarronGeneralLoss::new(-2.0, 1.0).is_ok());

        // Every valid finite parameter must still construct successfully.
        assert!(HuberLoss::new(1.345).is_ok());
        assert!(CauchyLoss::new(2.3849).is_ok());
        assert!(DcsLoss::new(1.0).is_ok());
        assert!(TDistributionLoss::new(5.0).is_ok());
    }

    #[test]
    fn test_loss_comparison() -> TestResult {
        // Compare robustness: L2 vs Huber vs Cauchy at outlier
        let s_outlier = 100.0;

        let l2 = L2Loss;
        let huber = HuberLoss::new(1.345)?;
        let cauchy = CauchyLoss::new(2.3849)?;

        let [_, w_l2, _] = l2.evaluate(s_outlier);
        let [_, w_huber, _] = huber.evaluate(s_outlier);
        let [_, w_cauchy, _] = cauchy.evaluate(s_outlier);

        // L2 should give highest weight (no robustness)
        assert!(w_l2 > w_huber);
        assert!(w_huber > w_cauchy);

        // Cauchy should strongly suppress outliers
        assert!(w_cauchy < 0.1);

        Ok(())
    }

    #[test]
    fn test_t_distribution_loss() -> TestResult {
        let loss = TDistributionLoss::new(5.0)?;

        // Test at s = 0 (should be well-defined)
        let [rho, rho_prime, _] = loss.evaluate(0.0);
        assert_eq!(rho, 0.0);
        assert!((rho_prime - 0.6).abs() < 0.01); // (ν+1)/(2ν) = 6/10 = 0.6

        // Test heavy tail behavior (downweighting)
        let [_, rho_prime_small, _] = loss.evaluate(1.0);
        let [_, rho_prime_large, _] = loss.evaluate(100.0);
        assert!(rho_prime_large < rho_prime_small);

        // Test that large outliers are strongly downweighted
        assert!(rho_prime_large < 0.1);

        // Verify derivatives numerically
        let s = 4.0;
        let [_, rho_prime, rho_double_prime] = loss.evaluate(s);
        let (rho_prime_num, rho_double_prime_num) = numerical_derivative(&loss, s, 1e-5);
        assert!((rho_prime - rho_prime_num).abs() < 1e-4);
        assert!((rho_double_prime - rho_double_prime_num).abs() < 1e-4);

        Ok(())
    }

    #[test]
    fn test_t_distribution_loss_different_nu() -> TestResult {
        // Test that smaller ν is more robust
        let t3 = TDistributionLoss::new(3.0)?;
        let t10 = TDistributionLoss::new(10.0)?;

        let s_outlier = 100.0;
        let [_, w_t3, _] = t3.evaluate(s_outlier);
        let [_, w_t10, _] = t10.evaluate(s_outlier);

        // Smaller ν should downweight more aggressively
        assert!(w_t3 < w_t10);

        Ok(())
    }

    #[test]
    fn test_adaptive_barron_loss() -> TestResult {
        // Test default (Cauchy-like with α = 0)
        let adaptive = AdaptiveBarronLoss::new(0.0, 1.0)?;

        // Test at s = 0
        let [rho, _, _] = adaptive.evaluate(0.0);
        assert!(rho.abs() < EPSILON);

        // Test robustness
        let [_, rho_prime_small, _] = adaptive.evaluate(1.0);
        let [_, rho_prime_large, _] = adaptive.evaluate(100.0);
        assert!(rho_prime_large < rho_prime_small);

        // AdaptiveBarron wraps BarronGeneral which is already tested,
        // so we just verify the wrapper works correctly
        let barron = BarronGeneralLoss::new(0.0, 1.0)?;
        let [rho_a, rho_prime_a, rho_double_prime_a] = adaptive.evaluate(4.0);
        let [rho_b, rho_prime_b, rho_double_prime_b] = barron.evaluate(4.0);

        // Should match the underlying BarronGeneral exactly
        assert!((rho_a - rho_b).abs() < EPSILON);
        assert!((rho_prime_a - rho_prime_b).abs() < EPSILON);
        assert!((rho_double_prime_a - rho_double_prime_b).abs() < EPSILON);

        Ok(())
    }

    #[test]
    fn test_adaptive_barron_default() -> TestResult {
        // Test default constructor
        let adaptive = AdaptiveBarronLoss::default();

        // Should behave like Cauchy
        let [_, rho_prime, _] = adaptive.evaluate(4.0);
        assert!(rho_prime > 0.0 && rho_prime < 1.0);

        Ok(())
    }

    #[test]
    fn test_new_loss_constructor_validation() -> TestResult {
        // T-distribution: reject non-positive degrees of freedom
        assert!(TDistributionLoss::new(0.0).is_err());
        assert!(TDistributionLoss::new(-1.0).is_err());
        assert!(TDistributionLoss::new(5.0).is_ok());

        // Adaptive Barron: reject non-positive scale
        assert!(AdaptiveBarronLoss::new(0.0, 0.0).is_err());
        assert!(AdaptiveBarronLoss::new(1.0, -1.0).is_err());
        assert!(AdaptiveBarronLoss::new(0.0, 1.0).is_ok());

        Ok(())
    }

    #[test]
    fn test_loss_registry_resolves_every_canonical_name() -> TestResult {
        // The registry is the single source CLI tools resolve through: every
        // canonical name (plus its documented aliases, case-insensitively)
        // must construct, and unknowns must error listing the valid set.
        for name in loss_canonical_names() {
            let loss = loss_from_name(name, None)?;
            // Sanity at the origin: zero cost and a sane (unit-or-less,
            // non-negative) weight. Conventions differ per kernel (Welsch
            // uses ρ'(0) = 1/2), so only the bounds are pinned here.
            //
            // L1/Lp (p<2) are the documented exception (ISSUE-0011): their
            // *true* ρ is genuinely singular at s=0 (ρ'(s)=1/√s → ∞), so any
            // faithful smoothing must trade off between a bounded weight at
            // the exact origin and staying close to the true loss for
            // ordinary (non-infinitesimal) residuals. This module keeps the
            // smoothing scale tiny — matching true L1/Lp everywhere but a
            // sliver near s=0 — which is the right choice for a robust loss
            // (its whole point is its shape at moderate/large residuals),
            // at the cost of a large-but-finite ρ'(0) instead of a "sane"
            // one; the origin weight is still finite and positive, never
            // NaN/Inf, which is what the corrector actually depends on.
            let [rho, rho_prime, _] = loss.evaluate(0.0);
            assert_eq!(rho, 0.0, "{name}: ρ(0) must be 0");
            if name == "l1" || name == "lp" {
                assert!(
                    rho_prime.is_finite() && rho_prime > 0.0,
                    "{name}: ρ'(0) must be finite and positive, got {rho_prime}"
                );
            } else {
                assert!(
                    (0.0..=1.0).contains(&rho_prime),
                    "{name}: ρ'(0) must be a sane weight, got {rho_prime}"
                );
            }
            assert_eq!(
                name.to_lowercase(),
                name,
                "{name}: canonical names must already be lowercase"
            );
        }
        // Documented aliases resolve too.
        for alias in [
            "gemanmcclure",
            "trimmedmean",
            "tdistribution",
            "adaptivebarron",
            "HUBER",
        ] {
            loss_from_name(alias, None)?;
        }
        let Err(err) = loss_from_name("not-a-loss", None) else {
            panic!("unknown loss name must not resolve");
        };
        let message = err.to_string();
        for name in loss_canonical_names() {
            assert!(
                message.contains(name),
                "error must list valid names, missing {name}: {message}"
            );
        }
        Ok(())
    }

    #[test]
    fn test_dcs_saturates_past_phi_instead_of_declining() -> TestResult {
        // ISSUE-0010: past s = Φ, ρ must freeze at the peak value Φ (with
        // ρ' = ρ'' = 0) rather than g2o's own declining ρ(s) = scale²·s —
        // the latter changes the reported cost while the corrector's
        // clamped-negative-weight path already contributes zero gradient,
        // so actual and predicted reduction would silently disagree.
        let dcs = DcsLoss::new(1.0)?;
        for s in [1.5, 4.0, 100.0, 1e12] {
            let [rho, rho_prime, rho_double_prime] = dcs.evaluate(s);
            assert!((rho - 1.0).abs() < 1e-12, "ρ({s}) = {rho}, expected Φ=1");
            assert_eq!(rho_prime, 0.0, "ρ'({s})");
            assert_eq!(rho_double_prime, 0.0, "ρ''({s})");
        }
        Ok(())
    }

    #[test]
    fn test_dcs_inlier_identity_and_saturation() -> TestResult {
        let dcs = DcsLoss::new(1.0)?;
        // At and below Φ the kernel is plain least squares.
        for s in [0.0, 0.5, 1.0] {
            let [rho, rho_prime, rho_double_prime] = dcs.evaluate(s);
            assert_eq!(rho, s, "inlier ρ({s})");
            assert_eq!(rho_prime, 1.0, "inlier ρ'({s})");
            assert_eq!(rho_double_prime, 0.0, "inlier ρ''({s})");
        }
        // Far outliers saturate at Φ exactly — never negative, never
        // declining back toward 0, always finite.
        let [rho, _, _] = dcs.evaluate(1e12);
        assert!(rho.is_finite() && rho >= 0.0, "saturated ρ = {rho}");
        assert!(
            (rho - 1.0).abs() < 1e-12,
            "saturated ρ must equal Φ, got {rho}"
        );
        // Rejects non-positive or non-finite Φ like the other scaled losses.
        assert!(DcsLoss::new(0.0).is_err());
        assert!(DcsLoss::new(-1.0).is_err());
        assert!(DcsLoss::new(f64::INFINITY).is_err());
        Ok(())
    }

    #[test]
    fn test_dcs_negative_weight_suppresses_without_nan() -> TestResult {
        use crate::core::corrector::Corrector;

        // Past s = Φ the DCS weight goes negative; the corrector must clamp
        // instead of propagating NaN through sqrt(ρ').
        let dcs = DcsLoss::new(1.0)?;
        for s in [1.5, 4.0, 100.0] {
            let corrector = Corrector::new(&dcs, s);
            let mut residual = vec![s.sqrt(), 0.0];
            corrector.correct_residual_in_place(&mut residual);
            assert!(
                residual.iter().all(|v| v.is_finite()),
                "NaN at s = {s}: {residual:?}"
            );
            assert!(
                residual.iter().all(|&v| v == 0.0),
                "fully suppressed at s = {s}: {residual:?}"
            );
            assert!(corrector.robust_cost().is_finite());
        }
        Ok(())
    }
}