//! Sequential Substitution (Picard iteration) solver implementation.
//!
//! Provides [`PicardConfig`] which implements Picard iteration for solving
//! systems of non-linear equations. Slower than Newton-Raphson but more robust.

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

use nalgebra::{DMatrix, DVector};

use crate::criteria::ConvergenceCriteria;
use crate::metadata::SimulationMetadata;
use crate::solver::{
    dominant_residual, ConvergedState, ConvergenceDiagnostics, ConvergenceStatus,
    IterationDiagnostics, Solver, SolverError, SolverType, TimeoutConfig, VerboseConfig,
};
use crate::system::System;

/// Configuration for the Sequential Substitution (Picard iteration) solver.
///
/// Solves x = G(x) by iterating: x_{k+1} = (1-ω)·x_k + ω·G(x_k)
/// where ω ∈ (0,1] is the relaxation factor.
#[derive(Debug, Clone, PartialEq)]
pub struct PicardConfig {
    /// Maximum iterations. Default: 100.
    pub max_iterations: usize,
    /// Convergence tolerance (L2 norm). Default: 1e-6.
    pub tolerance: f64,
    /// Relaxation factor ω ∈ (0,1]. Default: 0.5.
    pub relaxation_factor: f64,
    /// Optional time budget.
    pub timeout: Option<Duration>,
    /// Divergence threshold. Default: 1e10.
    pub divergence_threshold: f64,
    /// Consecutive increases before divergence. Default: 5.
    pub divergence_patience: usize,
    /// Timeout behavior configuration.
    pub timeout_config: TimeoutConfig,
    /// Previous state for ZOH fallback.
    pub previous_state: Option<Vec<f64>>,
    /// Residual for previous_state.
    pub previous_residual: Option<f64>,
    /// Smart initial state for cold-start.
    pub initial_state: Option<Vec<f64>>,
    /// Multi-circuit convergence criteria.
    pub convergence_criteria: Option<ConvergenceCriteria>,
    /// Verbose mode configuration for diagnostics.
    pub verbose_config: VerboseConfig,
    /// Anderson acceleration depth `m` (history window). `0` disables acceleration
    /// and the solver behaves as plain relaxed Picard (default). Typical useful
    /// values are 3–5. See [`PicardConfig::with_anderson`].
    pub anderson_depth: usize,
    /// Tikhonov regularization added to the Anderson least-squares normal matrix
    /// for numerical stability. Default: 1e-10. Only used when `anderson_depth > 0`.
    pub anderson_regularization: f64,
}

impl Default for PicardConfig {
    fn default() -> Self {
        Self {
            max_iterations: 100,
            tolerance: 1e-6,
            relaxation_factor: 0.5,
            timeout: None,
            divergence_threshold: 1e10,
            divergence_patience: 5,
            timeout_config: TimeoutConfig::default(),
            previous_state: None,
            previous_residual: None,
            initial_state: None,
            convergence_criteria: None,
            verbose_config: VerboseConfig::default(),
            anderson_depth: 0,
            anderson_regularization: 1e-10,
        }
    }
}

impl PicardConfig {
    /// Sets the initial state for cold-start solving (Story 4.6 — builder pattern).
    ///
    /// The solver will start from `state` instead of the zero vector.
    /// Use [`SmartInitializer::populate_state`] to generate a physically reasonable
    /// initial guess.
    pub fn with_initial_state(mut self, state: Vec<f64>) -> Self {
        self.initial_state = Some(state);
        self
    }

    /// Sets multi-circuit convergence criteria (Story 4.7 — builder pattern).
    ///
    /// When set, the solver uses [`ConvergenceCriteria::check()`] instead of the
    /// raw L2-norm `tolerance` check.
    pub fn with_convergence_criteria(mut self, criteria: ConvergenceCriteria) -> Self {
        self.convergence_criteria = Some(criteria);
        self
    }

    /// Enables verbose mode for diagnostics.
    pub fn with_verbose(mut self, config: VerboseConfig) -> Self {
        self.verbose_config = config;
        self
    }

    /// Enables Anderson acceleration with history depth `m` (Story: solver speed).
    ///
    /// Anderson acceleration (Walker & Ni, 2011) turns the linearly-convergent
    /// relaxed Picard fixed-point iteration into a super-linearly convergent one by
    /// extrapolating from the last `m` residual/map-value pairs via a small
    /// least-squares problem. `m = 0` disables it (plain relaxed Picard). Values of
    /// 3–5 typically cut the iteration count by 2–3× on stiff refrigeration cycles
    /// while adding only an `O(m² · n)` least-squares solve per iteration.
    ///
    /// # Reference
    /// Walker, H.F., Ni, P. (2011). "Anderson acceleration for fixed-point
    /// iterations." *SIAM J. Numerical Analysis*, 49(4):1715–1735.
    pub fn with_anderson(mut self, depth: usize) -> Self {
        self.anderson_depth = depth;
        self
    }

    /// Computes the residual norm (L2 norm of the residual vector).
    fn residual_norm(residuals: &[f64]) -> f64 {
        residuals.iter().map(|r| r * r).sum::<f64>().sqrt()
    }

    /// Handles timeout based on configuration (Story 4.5).
    ///
    /// Returns either:
    /// - `Ok(ConvergedState)` with `TimedOutWithBestState` status (default)
    /// - `Err(SolverError::Timeout)` if `return_best_state_on_timeout` is false
    /// - Previous state (ZOH) if `zoh_fallback` is true and previous state available
    fn handle_timeout(
        &self,
        best_state: &[f64],
        best_residual: f64,
        iterations: usize,
        timeout: Duration,
        system: &System,
    ) -> Result<ConvergedState, SolverError> {
        // If configured to return error on timeout
        if !self.timeout_config.return_best_state_on_timeout {
            return Err(SolverError::Timeout {
                timeout_ms: timeout.as_millis() as u64,
            });
        }

        // If ZOH fallback is enabled and previous state is available
        if self.timeout_config.zoh_fallback {
            if let Some(ref prev_state) = self.previous_state {
                let residual = self.previous_residual.unwrap_or(best_residual);
                tracing::info!(
                    iterations = iterations,
                    residual = residual,
                    "Returning previous state (ZOH fallback) on timeout"
                );
                return Ok(ConvergedState::new(
                    prev_state.clone(),
                    iterations,
                    residual,
                    ConvergenceStatus::TimedOutWithBestState,
                    SimulationMetadata::new(system.input_hash()),
                ));
            }
        }

        // Default: return best state encountered during iteration
        tracing::info!(
            iterations = iterations,
            best_residual = best_residual,
            "Returning best state on timeout"
        );
        Ok(ConvergedState::new(
            best_state.to_vec(),
            iterations,
            best_residual,
            ConvergenceStatus::TimedOutWithBestState,
            SimulationMetadata::new(system.input_hash()),
        ))
    }

    /// Checks for divergence based on residual growth pattern.
    ///
    /// Returns `Some(SolverError::Divergence)` if:
    /// - Residual norm exceeds `divergence_threshold`, or
    /// - Residual has increased for `divergence_patience`+ consecutive iterations
    fn check_divergence(
        &self,
        current_norm: f64,
        previous_norm: f64,
        divergence_count: &mut usize,
    ) -> Option<SolverError> {
        // Check absolute threshold
        if current_norm > self.divergence_threshold {
            return Some(SolverError::Divergence {
                reason: format!(
                    "Residual norm {} exceeds threshold {}",
                    current_norm, self.divergence_threshold
                ),
            });
        }

        // Check consecutive increases
        if current_norm > previous_norm {
            *divergence_count += 1;
            if *divergence_count >= self.divergence_patience {
                return Some(SolverError::Divergence {
                    reason: format!(
                        "Residual increased for {} consecutive iterations: {:.6e} → {:.6e}",
                        self.divergence_patience, previous_norm, current_norm
                    ),
                });
            }
        } else {
            *divergence_count = 0;
        }

        None
    }

    /// Applies relaxation to the state update.
    ///
    /// Update formula: x_new = x_old - omega * residual
    /// where residual = F(x_k) represents the equation residuals.
    ///
    /// This is the standard Picard iteration: x_{k+1} = x_k - ω·F(x_k)
    fn apply_relaxation(state: &mut [f64], residuals: &[f64], omega: f64) {
        for (x, &r) in state.iter_mut().zip(residuals.iter()) {
            *x -= omega * r;
        }
    }

    fn finalize_failure_diagnostics(
        &self,
        mut diagnostics: Option<ConvergenceDiagnostics>,
        iterations: usize,
        final_residual: f64,
        best_residual: f64,
        elapsed_ms: u64,
        final_state: Option<Vec<f64>>,
    ) -> Option<ConvergenceDiagnostics> {
        if let Some(ref mut diag) = diagnostics {
            diag.iterations = iterations;
            diag.final_residual = final_residual;
            diag.best_residual = best_residual;
            diag.converged = false;
            diag.timing_ms = elapsed_ms;
            diag.final_solver = Some(SolverType::SequentialSubstitution);

            if self.verbose_config.dump_final_state {
                diag.final_state = final_state;
                let json_output = diag.dump_diagnostics(self.verbose_config.output_format);
                tracing::warn!(
                    iterations,
                    final_residual,
                    "Non-convergence diagnostics:\n{}",
                    json_output
                );
            }
        }
        diagnostics
    }
}

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

        // Initialize diagnostics collection if verbose mode enabled
        let verbose_enabled = self.verbose_config.enabled && self.verbose_config.is_any_enabled();
        let mut diagnostics = if verbose_enabled {
            Some(ConvergenceDiagnostics::with_capacity(self.max_iterations))
        } else {
            None
        };

        tracing::info!(
            max_iterations = self.max_iterations,
            tolerance = self.tolerance,
            relaxation_factor = self.relaxation_factor,
            divergence_threshold = self.divergence_threshold,
            divergence_patience = self.divergence_patience,
            verbose = verbose_enabled,
            "Sequential Substitution (Picard) solver starting"
        );

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

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

        // Validate state/equation dimensions
        if n_state != n_equations {
            return Err(SolverError::InvalidSystem {
                message: format!(
                    "State dimension ({}) does not match equation count ({})",
                    n_state, n_equations
                ),
            });
        }

        // Pre-allocate all buffers (AC: #6 - no heap allocation in iteration loop)
        // Story 4.6 - AC: #8: Use initial_state if provided, else start from zeros.
        // A mismatched length is a hard error (zero-panic; no silent zeros fallback
        // that would solve a different problem) — consistent with Newton/Homotopy.
        let mut state: Vec<f64> = match self.initial_state.as_ref() {
            Some(s) if s.len() == n_state => s.clone(),
            Some(s) => {
                return Err(SolverError::InvalidSystem {
                    message: format!(
                        "initial_state length {} does not match system state length {}",
                        s.len(),
                        n_state
                    ),
                });
            }
            None => vec![0.0; n_state],
        };
        let mut prev_iteration_state: Vec<f64> = vec![0.0; n_state]; // For convergence delta check
        let mut residuals: Vec<f64> = vec![0.0; n_equations];
        let mut divergence_count: usize = 0;
        let mut previous_norm: f64;

        // Pre-allocate best-state tracking buffer (Story 4.5 - AC: #5)
        let mut best_state: Vec<f64> = vec![0.0; n_state];
        let mut best_residual: f64;

        // Initial residual computation
        system
            .compute_residuals(&state, &mut residuals)
            .map_err(|e| SolverError::InvalidSystem {
                message: format!("Failed to compute initial residuals: {:?}", e),
            })?;

        let mut current_norm = Self::residual_norm(&residuals);

        // Initialize best state tracking with initial state
        best_state.copy_from_slice(&state);
        best_residual = current_norm;

        tracing::debug!(iteration = 0, residual_norm = current_norm, "Initial state");

        // Check if already converged
        if current_norm < self.tolerance {
            tracing::info!(
                iterations = 0,
                final_residual = current_norm,
                "System already converged at initial state"
            );
            return Ok(ConvergedState::new(
                state,
                0,
                current_norm,
                ConvergenceStatus::Converged,
                SimulationMetadata::new(system.input_hash()),
            ));
        }

        // Optional Anderson accelerator (disabled when depth == 0).
        let mut anderson = if self.anderson_depth > 0 {
            Some(AndersonAccelerator::new(
                self.anderson_depth,
                self.anderson_regularization,
            ))
        } else {
            None
        };

        // Main Picard iteration loop
        for iteration in 1..=self.max_iterations {
            // Save state before step for convergence criteria delta checks
            prev_iteration_state.copy_from_slice(&state);

            // Check timeout at iteration start (Story 4.5 - AC: #1)
            if let Some(timeout) = self.timeout {
                if start_time.elapsed() > timeout {
                    tracing::info!(
                        iteration = iteration,
                        elapsed_ms = start_time.elapsed().as_millis(),
                        timeout_ms = timeout.as_millis(),
                        best_residual = best_residual,
                        "Solver timed out"
                    );

                    // Story 4.5 - AC: #2, #6: Return best state or error based on config
                    let failure_diagnostics = self.finalize_failure_diagnostics(
                        diagnostics.take(),
                        iteration - 1,
                        current_norm,
                        best_residual,
                        start_time.elapsed().as_millis() as u64,
                        Some(state.clone()),
                    );
                    return self
                        .handle_timeout(&best_state, best_residual, iteration - 1, timeout, system)
                        .map_err(|err| err.with_optional_diagnostics(failure_diagnostics));
                }
            }

            // Apply update. With Anderson acceleration enabled, extrapolate from the
            // residual/map-value history; otherwise use plain relaxed Picard.
            // Both share the same underlying fixed-point map G(x) = x - ω·F(x).
            if let Some(acc) = anderson.as_mut() {
                acc.next_state_into(&mut state, &residuals, self.relaxation_factor);
            } else {
                Self::apply_relaxation(&mut state, &residuals, self.relaxation_factor);
            }

            // Compute new residuals
            system
                .compute_residuals(&state, &mut residuals)
                .map_err(|e| SolverError::InvalidSystem {
                    message: format!("Failed to compute residuals: {:?}", e),
                })?;

            previous_norm = current_norm;
            current_norm = Self::residual_norm(&residuals);

            // Compute delta norm for diagnostics
            let delta_norm: f64 = state
                .iter()
                .zip(prev_iteration_state.iter())
                .map(|(s, p)| (s - p).powi(2))
                .sum::<f64>()
                .sqrt();

            // Update best state if residual improved (Story 4.5 - AC: #2)
            if current_norm < best_residual {
                best_state.copy_from_slice(&state);
                best_residual = current_norm;
                tracing::debug!(
                    iteration = iteration,
                    best_residual = best_residual,
                    "Best state updated"
                );
            }

            // Verbose mode: Log iteration residuals
            if verbose_enabled && self.verbose_config.log_residuals {
                tracing::info!(
                    iteration,
                    residual_norm = current_norm,
                    delta_norm = delta_norm,
                    relaxation_factor = self.relaxation_factor,
                    "Picard iteration"
                );
            }

            // Collect iteration diagnostics
            if let Some(ref mut diag) = diagnostics {
                let (max_residual_index, max_residual) = dominant_residual(&residuals);
                diag.push_iteration(IterationDiagnostics {
                    iteration,
                    residual_norm: current_norm,
                    delta_norm,
                    alpha: None,              // Picard doesn't use line search
                    jacobian_frozen: false,   // Picard doesn't use Jacobian
                    jacobian_condition: None, // No Jacobian in Picard
                    max_residual_index,
                    max_residual,
                });
            }

            tracing::debug!(
                iteration = iteration,
                residual_norm = current_norm,
                relaxation_factor = self.relaxation_factor,
                "Picard iteration complete"
            );

            // Check convergence (AC: #1, Story 4.7 — criteria-aware)
            let converged = if let Some(ref criteria) = self.convergence_criteria {
                let report =
                    criteria.check(&state, Some(&prev_iteration_state), &residuals, system);
                if report.is_globally_converged() {
                    // Finalize diagnostics
                    if let Some(ref mut diag) = diagnostics {
                        diag.iterations = iteration;
                        diag.final_residual = current_norm;
                        diag.best_residual = best_residual;
                        diag.converged = true;
                        diag.timing_ms = start_time.elapsed().as_millis() as u64;
                        diag.final_solver = Some(SolverType::SequentialSubstitution);

                        if self.verbose_config.log_residuals {
                            tracing::info!("{}", diag.summary());
                        }
                    }

                    tracing::info!(
                        iterations = iteration,
                        final_residual = current_norm,
                        relaxation_factor = self.relaxation_factor,
                        "Sequential Substitution converged (criteria)"
                    );
                    let result = ConvergedState::with_report(
                        state,
                        iteration,
                        current_norm,
                        ConvergenceStatus::Converged,
                        report,
                        SimulationMetadata::new(system.input_hash()),
                    );
                    return Ok(if let Some(d) = diagnostics {
                        ConvergedState {
                            diagnostics: Some(d),
                            ..result
                        }
                    } else {
                        result
                    });
                }
                false
            } else {
                current_norm < self.tolerance
            };

            if converged {
                // Finalize diagnostics
                if let Some(ref mut diag) = diagnostics {
                    diag.iterations = iteration;
                    diag.final_residual = current_norm;
                    diag.best_residual = best_residual;
                    diag.converged = true;
                    diag.timing_ms = start_time.elapsed().as_millis() as u64;
                    diag.final_solver = Some(SolverType::SequentialSubstitution);

                    if self.verbose_config.log_residuals {
                        tracing::info!("{}", diag.summary());
                    }
                }

                tracing::info!(
                    iterations = iteration,
                    final_residual = current_norm,
                    relaxation_factor = self.relaxation_factor,
                    "Sequential Substitution converged"
                );
                let result = ConvergedState::new(
                    state,
                    iteration,
                    current_norm,
                    ConvergenceStatus::Converged,
                    SimulationMetadata::new(system.input_hash()),
                );
                return Ok(if let Some(d) = diagnostics {
                    ConvergedState {
                        diagnostics: Some(d),
                        ..result
                    }
                } else {
                    result
                });
            }

            // Check divergence (AC: #5)
            if let Some(err) =
                self.check_divergence(current_norm, previous_norm, &mut divergence_count)
            {
                tracing::warn!(
                    iteration = iteration,
                    residual_norm = current_norm,
                    "Divergence detected"
                );
                let failure_diagnostics = self.finalize_failure_diagnostics(
                    diagnostics.take(),
                    iteration,
                    current_norm,
                    best_residual,
                    start_time.elapsed().as_millis() as u64,
                    Some(state.clone()),
                );
                return Err(err.with_optional_diagnostics(failure_diagnostics));
            }
        }

        // Non-convergence: dump diagnostics if enabled
        let failure_diagnostics = self.finalize_failure_diagnostics(
            diagnostics.take(),
            self.max_iterations,
            current_norm,
            best_residual,
            start_time.elapsed().as_millis() as u64,
            Some(state.clone()),
        );

        // Max iterations exceeded
        tracing::warn!(
            max_iterations = self.max_iterations,
            final_residual = current_norm,
            "Sequential Substitution did not converge"
        );
        Err(SolverError::NonConvergence {
            iterations: self.max_iterations,
            final_residual: current_norm,
        }
        .with_optional_diagnostics(failure_diagnostics))
    }

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

/// Anderson acceleration state for the relaxed Picard fixed-point iteration.
///
/// The underlying fixed-point map is `G(x) = x - ω·F(x)` where `F` is the residual
/// vector and `ω` the relaxation factor. Define the map residual `f(x) = G(x) - x =
/// -ω·F(x)`. Anderson acceleration maintains the last `m` differences of `f` and `G`
/// and, each iteration, solves the small least-squares problem
/// `min_γ ‖f_k - ΔF·γ‖` then sets `x_{k+1} = G_k - ΔG·γ` (Walker & Ni, 2011,
/// following H. Walker's reference `anderson.m`). With `m = 0` (empty history) it
/// reduces exactly to the plain step `x_{k+1} = G_k`.
struct AndersonAccelerator {
    depth: usize,
    regularization: f64,
    /// Previous map-residual f = G(x) - x.
    f_prev: Option<Vec<f64>>,
    /// Previous map value G(x).
    g_prev: Option<Vec<f64>>,
    /// History of Δf columns (most-recent at back), capped at `depth`.
    df: VecDeque<Vec<f64>>,
    /// History of ΔG columns (most-recent at back), capped at `depth`.
    dg: VecDeque<Vec<f64>>,
}

impl AndersonAccelerator {
    fn new(depth: usize, regularization: f64) -> Self {
        Self {
            depth,
            regularization,
            f_prev: None,
            g_prev: None,
            df: VecDeque::with_capacity(depth),
            dg: VecDeque::with_capacity(depth),
        }
    }

    /// Advances `state` in place from `x_k` to the accelerated `x_{k+1}`, given the
    /// current residual vector `F(x_k)` and relaxation factor `ω`.
    fn next_state_into(&mut self, state: &mut [f64], residual: &[f64], omega: f64) {
        let n = state.len();
        // Map residual f = -ω·F and fixed-point map value G = x + f.
        let fval: Vec<f64> = residual.iter().map(|r| -omega * r).collect();
        let gval: Vec<f64> = state.iter().zip(&fval).map(|(x, f)| x + f).collect();

        // Push newest history differences.
        if let (Some(fp), Some(gp)) = (self.f_prev.as_ref(), self.g_prev.as_ref()) {
            let df_col: Vec<f64> = fval.iter().zip(fp).map(|(a, b)| a - b).collect();
            let dg_col: Vec<f64> = gval.iter().zip(gp).map(|(a, b)| a - b).collect();
            self.df.push_back(df_col);
            self.dg.push_back(dg_col);
            while self.df.len() > self.depth {
                self.df.pop_front();
                self.dg.pop_front();
            }
        }
        self.f_prev = Some(fval.clone());
        self.g_prev = Some(gval.clone());

        let m = self.df.len();
        if m == 0 {
            // No history yet — plain relaxed step.
            state.copy_from_slice(&gval);
            return;
        }

        // Solve the small least-squares problem for γ via regularized normal
        // equations: (ΔFᵀΔF + λI)·γ = ΔFᵀ·f_k. `m` is at most `depth` (small).
        let mut ata = DMatrix::<f64>::zeros(m, m);
        let mut atb = DVector::<f64>::zeros(m);
        for i in 0..m {
            for j in i..m {
                let mut s = 0.0;
                for k in 0..n {
                    s += self.df[i][k] * self.df[j][k];
                }
                ata[(i, j)] = s;
                ata[(j, i)] = s;
            }
            ata[(i, i)] += self.regularization;
            let mut s = 0.0;
            for k in 0..n {
                s += self.df[i][k] * fval[k];
            }
            atb[i] = s;
        }

        let gamma = match ata.clone().lu().solve(&atb) {
            Some(g) => g,
            None => {
                // Singular even with regularization — fall back to plain step.
                state.copy_from_slice(&gval);
                return;
            }
        };

        // x_{k+1} = G_k - ΔG·γ.
        for k in 0..n {
            let mut acc = gval[k];
            for (i, g) in gamma.iter().enumerate() {
                acc -= g * self.dg[i][k];
            }
            state[k] = acc;
        }
    }
}

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

    #[test]
    fn test_picard_config_with_timeout() {
        let timeout = Duration::from_millis(250);
        let cfg = PicardConfig::default().with_timeout(timeout);
        assert_eq!(cfg.timeout, Some(timeout));
    }

    #[test]
    fn test_picard_config_default_sensible() {
        let cfg = PicardConfig::default();
        assert_eq!(cfg.max_iterations, 100);
        assert!(cfg.tolerance > 0.0 && cfg.tolerance < 1e-3);
        assert!(cfg.relaxation_factor > 0.0 && cfg.relaxation_factor <= 1.0);
    }

    #[test]
    fn test_picard_apply_relaxation_formula() {
        let mut state = vec![10.0, 20.0];
        let residuals = vec![1.0, 2.0];
        PicardConfig::apply_relaxation(&mut state, &residuals, 0.5);
        assert!((state[0] - 9.5).abs() < 1e-15);
        assert!((state[1] - 19.0).abs() < 1e-15);
    }

    #[test]
    fn test_picard_residual_norm() {
        let residuals = vec![3.0, 4.0];
        let norm = PicardConfig::residual_norm(&residuals);
        assert!((norm - 5.0).abs() < 1e-15);
    }

    #[test]
    fn test_picard_solver_trait_object() {
        let mut boxed: Box<dyn Solver> = Box::new(PicardConfig::default());
        let mut system = System::new();
        system.finalize().unwrap();
        assert!(boxed.solve(&mut system).is_err());
    }

    // ── Anderson acceleration ────────────────────────────────────────────────

    /// Reference linear residual F(x) = A·x - b. Its unique root is x* = A⁻¹·b.
    /// The relaxed Picard map is x_{k+1} = x_k - ω·(A·x_k - b).
    fn linear_residual(a: &[[f64; 2]; 2], b: &[f64; 2], x: &[f64]) -> Vec<f64> {
        vec![
            a[0][0] * x[0] + a[0][1] * x[1] - b[0],
            a[1][0] * x[0] + a[1][1] * x[1] - b[1],
        ]
    }

    fn residual_norm2(r: &[f64]) -> f64 {
        r.iter().map(|v| v * v).sum::<f64>().sqrt()
    }

    #[test]
    fn test_anderson_depth_zero_matches_plain_relaxation() {
        // With no history, next_state_into must equal x - ω·F(x).
        let mut acc = AndersonAccelerator::new(0, 1e-10);
        let mut state = vec![10.0, 20.0];
        let residuals = vec![1.0, 2.0];
        acc.next_state_into(&mut state, &residuals, 0.5);
        assert!((state[0] - 9.5).abs() < 1e-15);
        assert!((state[1] - 19.0).abs() < 1e-15);
    }

    #[test]
    fn test_anderson_converges_faster_than_plain_picard() {
        // Stiff-ish SPD system where plain relaxed Picard converges slowly.
        let a = [[8.0, 1.0], [1.0, 3.0]];
        let b = [9.0, 4.0]; // exact root x* = [1, 1]
        let omega = 0.12; // deliberately small → slow plain Picard
        let tol = 1e-9;
        let max_iter = 2000;

        let count_iters = |depth: usize| -> (usize, Vec<f64>) {
            let mut state = vec![0.0, 0.0];
            let mut acc = AndersonAccelerator::new(depth, 1e-12);
            for it in 1..=max_iter {
                let r = linear_residual(&a, &b, &state);
                if residual_norm2(&r) < tol {
                    return (it - 1, state);
                }
                acc.next_state_into(&mut state, &r, omega);
            }
            (max_iter, state)
        };

        let (plain_iters, _) = count_iters(0);
        let (anderson_iters, sol) = count_iters(3);

        // Anderson must converge, land on the true root, and use far fewer steps.
        assert!(anderson_iters < max_iter, "Anderson did not converge");
        assert!((sol[0] - 1.0).abs() < 1e-6 && (sol[1] - 1.0).abs() < 1e-6);
        assert!(
            anderson_iters * 3 < plain_iters,
            "Anderson ({}) should be much faster than plain Picard ({})",
            anderson_iters,
            plain_iters
        );
    }

    #[test]
    fn test_anderson_solves_where_plain_diverges_marginally() {
        // Anderson should still hit the exact root of a well-posed linear system.
        let a = [[4.0, 1.0], [2.0, 5.0]];
        let b = [6.0, 9.0];
        // exact root: solve → x=[1, 1.4? ] compute: 4x+y=6, 2x+5y=9
        // From first: y = 6-4x; sub: 2x+5(6-4x)=9 → 2x+30-20x=9 → -18x=-21 → x=7/6
        // y = 6-4*7/6 = 6-28/6 = 8/6 = 4/3
        let omega = 0.15;
        let mut state = vec![0.0, 0.0];
        let mut acc = AndersonAccelerator::new(4, 1e-12);
        let mut converged = false;
        for _ in 0..5000 {
            let r = linear_residual(&a, &b, &state);
            if residual_norm2(&r) < 1e-9 {
                converged = true;
                break;
            }
            acc.next_state_into(&mut state, &r, omega);
        }
        assert!(converged);
        assert!((state[0] - 7.0 / 6.0).abs() < 1e-6);
        assert!((state[1] - 4.0 / 3.0).abs() < 1e-6);
    }

    #[test]
    fn test_with_anderson_builder_sets_depth() {
        let cfg = PicardConfig::default().with_anderson(5);
        assert_eq!(cfg.anderson_depth, 5);
        // Default remains disabled.
        assert_eq!(PicardConfig::default().anderson_depth, 0);
    }
}