scirs2-integrate 0.4.2

//! BDF Methods for DAE Systems
//!
//! This module provides implementations of Backward Differentiation Formula (BDF)
//! methods specifically tailored for solving Differential-Algebraic Equations (DAEs).
//! These methods are particularly effective for stiff and index-1 DAE systems.

use crate::common::IntegrateFloat;
use crate::dae::types::{DAEIndex, DAEOptions, DAEResult, DAEType};
use crate::error::{IntegrateError, IntegrateResult};
use crate::ode::ODEMethod;
use scirs2_core::ndarray::{Array1, Array2, ArrayView1};

/// Helper to convert f64 constants to generic Float type
#[inline(always)]
fn const_f64<F: IntegrateFloat>(value: f64) -> F {
    F::from_f64(value).expect("Failed to convert constant to target float type")
}

/// BDF method for semi-explicit DAE systems
///
/// Implements a specialized BDF method tailored for semi-explicit DAE systems
/// of the form x' = f(x, y, t), 0 = g(x, y, t)
#[allow(dead_code)]
pub fn bdf_semi_explicit_dae<F, FFunc, GFunc>(
    f: FFunc,
    g: GFunc,
    t_span: [F; 2],
    x0: Array1<F>,
    y0: Array1<F>,
    options: DAEOptions<F>,
) -> IntegrateResult<DAEResult<F>>
where
    F: IntegrateFloat,
    FFunc: Fn(F, ArrayView1<F>, ArrayView1<F>) -> Array1<F>,
    GFunc: Fn(F, ArrayView1<F>, ArrayView1<F>) -> Array1<F>,
{
    // Get dimensions
    let n_x = x0.len();
    let n_y = y0.len();
    let n_total = n_x + n_y;

    // Storage for solution
    let mut t_values = vec![t_span[0]];
    let mut x_values = vec![x0.clone()];
    let mut y_values = vec![y0.clone()];

    // Current values
    let mut t_current = t_span[0];
    let mut _x_current = x0.clone();
    let mut _y_current = y0.clone();

    // Initial step size
    let mut h = options.h0.unwrap_or_else(|| {
        let _span = t_span[1] - t_span[0];
        _span * const_f64::<F>(0.01) // 1% of interval
    });

    // Step limits
    let min_step = options.min_step.unwrap_or_else(|| {
        let _span = t_span[1] - t_span[0];
        _span * const_f64::<F>(1e-6) // Very small relative to interval
    });

    let max_step = options.max_step.unwrap_or_else(|| {
        let _span = t_span[1] - t_span[0];
        _span * const_f64::<F>(0.1) // 10% of interval
    });

    // Maximum BDF order
    let max_order = options.max_order.unwrap_or(5).min(5);

    // Tolerances
    let rtol = options.rtol;
    let atol = options.atol;

    // Counters for statistics
    let mut n_steps = 0;
    let mut n_accepted = 0;
    let mut n_rejected = 0;
    let mut n_f_evals = 0;
    let mut n_g_evals = 0;
    let mut n_jac_evals = 0;
    let mut n_lu_decomps = 0;

    // BDF method coefficients for various orders
    // These are the coefficients in the BDF formula:
    // Σ α_j * y_{n+1-j} = h * β * f(t_{n+1}, y_{n+1})
    // where α_0 = 1 for all orders

    // For order 1 (Backward Euler): y_{n+1} - y_n = h * f(t_{n+1}, y_{n+1})
    // For higher orders, the coefficients provide higher-order accuracy

    // Alpha coefficients (exclude α_0 = 1)
    let alpha_coeffs = [
        // Order 1 (Backward Euler)
        vec![const_f64::<F>(-1.0)],
        // Order 2
        vec![
            F::from_f64(-4.0 / 3.0).expect("Failed to convert to float"),
            F::from_f64(1.0 / 3.0).expect("Failed to convert to float"),
        ],
        // Order 3
        vec![
            F::from_f64(-18.0 / 11.0).expect("Failed to convert to float"),
            F::from_f64(9.0 / 11.0).expect("Failed to convert to float"),
            F::from_f64(-2.0 / 11.0).expect("Failed to convert to float"),
        ],
        // Order 4
        vec![
            F::from_f64(-48.0 / 25.0).expect("Failed to convert to float"),
            F::from_f64(36.0 / 25.0).expect("Failed to convert to float"),
            F::from_f64(-16.0 / 25.0).expect("Failed to convert to float"),
            F::from_f64(3.0 / 25.0).expect("Failed to convert to float"),
        ],
        // Order 5
        vec![
            F::from_f64(-300.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(300.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(-200.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(75.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(-12.0 / 137.0).expect("Failed to convert to float"),
        ],
    ];

    // Beta coefficients (multiplier for the RHS function)
    let beta_coeffs = [
        const_f64::<F>(1.0),                                            // Order 1
        F::from_f64(2.0 / 3.0).expect("Failed to convert to float"),    // Order 2
        F::from_f64(6.0 / 11.0).expect("Failed to convert to float"),   // Order 3
        F::from_f64(12.0 / 25.0).expect("Failed to convert to float"),  // Order 4
        F::from_f64(60.0 / 137.0).expect("Failed to convert to float"), // Order 5
    ];

    // Current order (start with order 1 and build up)
    let mut order = 1;

    // Main integration loop
    while t_current < t_span[1] && n_steps < options.max_steps {
        // Adjust step size to hit the end point exactly if needed
        if t_current + h > t_span[1] {
            h = t_span[1] - t_current;
        }

        // New time point
        let t_new = t_current + h;

        // Predict step: extrapolate from previous points
        let (x_pred, y_pred) = predict_step(
            &x_values,
            &y_values,
            order,
            h,
            t_current,
            t_values.as_slice(),
        );

        // Store original predictions for error estimation
        let x_pred_orig = x_pred.clone();
        let y_pred_orig = y_pred.clone();

        // Evaluate constraint at the predictor point
        let _g_pred = g(t_new, x_pred.view(), y_pred.view());
        n_g_evals += 1;

        // Compute the Jacobian of g with respect to y
        let _g_y = compute_jacobian_y(
            &g,
            t_new,
            x_pred.view(),
            y_pred.view(),
            const_f64::<F>(1e-8),
        );
        n_jac_evals += 1;

        // Get the alpha coefficient for this order
        let alpha = alpha_coeffs[order - 1].clone();
        let beta = beta_coeffs[order - 1];

        // Create a history array for x
        let mut x_history = Vec::with_capacity(order);
        for i in 0..order {
            let idx = x_values.len() - 1 - i;
            x_history.push(x_values[idx].clone());
        }

        // Initialize corrector iteration (Newton's method)
        let mut x_corr = x_pred.clone();
        let mut y_corr = y_pred.clone();

        // Maximum Newton iterations
        let max_newton_iter = options.max_newton_iterations;

        // Newton iteration for corrector
        let mut converged = false;
        for _iter in 0..max_newton_iter {
            // Evaluate f at the current corrector value
            let f_val = f(t_new, x_corr.view(), y_corr.view());
            n_f_evals += 1;

            // Evaluate g at the current corrector value
            let g_val = g(t_new, x_corr.view(), y_corr.view());
            n_g_evals += 1;

            // Compute the residuals for the BDF formula and constraint
            let mut residual_x = Array1::zeros(n_x);

            // Compute the BDF formula residual: x_corr - Σ α_j * x_{n-j} - h * β * f(t_new, x_corr, y_corr)
            for i in 0..n_x {
                // First term: x_corr
                residual_x[i] = x_corr[i];

                // Historical terms: - Σ α_j * x_{n-j}
                for j in 0..order {
                    residual_x[i] += alpha[j] * x_history[j][i];
                }

                // Function term: - h * β * f
                residual_x[i] -= h * beta * f_val[i];
            }

            // Constraint residual is simply g_val
            let residual_g = g_val;

            // Compute the full residual norm
            let res_x_norm = residual_x
                .iter()
                .fold(F::zero(), |acc, &val| acc + val * val)
                .sqrt();
            let res_g_norm = residual_g
                .iter()
                .fold(F::zero(), |acc, &val| acc + val * val)
                .sqrt();

            let residual_norm = (res_x_norm * res_x_norm + res_g_norm * res_g_norm).sqrt();

            // Check convergence
            if residual_norm < options.newton_tol {
                converged = true;
                break;
            }

            // Compute the Jacobian of f with respect to x and y
            let f_x = compute_jacobian_x(
                &f,
                t_new,
                x_corr.view(),
                y_corr.view(),
                const_f64::<F>(1e-8),
            );
            let f_y = compute_jacobian_y(
                &f,
                t_new,
                x_corr.view(),
                y_corr.view(),
                const_f64::<F>(1e-8),
            );
            n_jac_evals += 2;

            // Jacobian of the residual with respect to x and y
            // For the x-residual:
            // ∂residual_x/∂x = I - h * β * ∂f/∂x
            // ∂residual_x/∂y = -h * β * ∂f/∂y

            // For the constraint residual:
            // ∂residual_g/∂x = ∂g/∂x
            // ∂residual_g/∂y = ∂g/∂y

            // Compute ∂g/∂x
            let g_x = compute_jacobian_x(
                &g,
                t_new,
                x_corr.view(),
                y_corr.view(),
                const_f64::<F>(1e-8),
            );
            n_jac_evals += 1;

            // Compute ∂g/∂y
            let g_y = compute_jacobian_y(
                &g,
                t_new,
                x_corr.view(),
                y_corr.view(),
                const_f64::<F>(1e-8),
            );
            n_jac_evals += 1;

            // Construct the full Jacobian matrix
            // [ ∂residual_x/∂x  ∂residual_x/∂y ]
            // [ ∂residual_g/∂x  ∂residual_g/∂y ]
            let mut full_jacobian = Array2::<F>::zeros((n_total, n_total));

            // Fill the x-x block: I - h * β * ∂f/∂x
            for i in 0..n_x {
                for j in 0..n_x {
                    if i == j {
                        full_jacobian[[i, j]] = F::one() - h * beta * f_x[[i, j]];
                    } else {
                        full_jacobian[[i, j]] = -h * beta * f_x[[i, j]];
                    }
                }
            }

            // Fill the x-y block: -h * β * ∂f/∂y
            for i in 0..n_x {
                for j in 0..n_y {
                    full_jacobian[[i, n_x + j]] = -h * beta * f_y[[i, j]];
                }
            }

            // Fill the g-x block: ∂g/∂x
            for i in 0..n_y {
                for j in 0..n_x {
                    full_jacobian[[n_x + i, j]] = g_x[[i, j]];
                }
            }

            // Fill the g-y block: ∂g/∂y
            for i in 0..n_y {
                for j in 0..n_y {
                    full_jacobian[[n_x + i, n_x + j]] = g_y[[i, j]];
                }
            }

            // Construct the full residual vector
            let mut full_residual = Array1::<F>::zeros(n_total);
            for i in 0..n_x {
                full_residual[i] = residual_x[i];
            }
            for i in 0..n_y {
                full_residual[n_x + i] = residual_g[i];
            }

            // Negate the residual for solving J * Δz = -residual
            let neg_residual = full_residual.mapv(|x| -x);

            // Solve the linear system for the Newton step
            let delta_z = match solve_linear_system(&full_jacobian, &neg_residual) {
                Ok(dz) => dz,
                Err(_e) => {
                    // If the linear solve fails, try with a smaller step
                    // and terminate this Newton iteration
                    h *= const_f64::<F>(0.5);
                    break;
                }
            };
            n_lu_decomps += 1;

            // Extract the x and y components of the solution
            let delta_x = delta_z.slice(scirs2_core::ndarray::s![0..n_x]).to_owned();
            let delta_y = delta_z.slice(scirs2_core::ndarray::s![n_x..]).to_owned();

            // Apply the Newton step with damping if needed
            let mut alpha_damp = F::one();
            let min_alpha = const_f64::<F>(0.1);

            // Damped Newton iteration to improve convergence
            while alpha_damp >= min_alpha {
                // Apply the damped step
                let x_new = &x_corr + &(&delta_x * alpha_damp);
                let y_new = &y_corr + &(&delta_y * alpha_damp);

                // Evaluate the residual at the new point
                let f_new = f(t_new, x_new.view(), y_new.view());
                let g_new = g(t_new, x_new.view(), y_new.view());
                n_f_evals += 1;
                n_g_evals += 1;

                // Compute the new residuals
                let mut residual_x_new = Array1::zeros(n_x);
                for i in 0..n_x {
                    residual_x_new[i] = x_new[i];
                    for j in 0..order {
                        residual_x_new[i] += alpha[j] * x_history[j][i];
                    }
                    residual_x_new[i] -= h * beta * f_new[i];
                }

                let res_x_new_norm = residual_x_new
                    .iter()
                    .fold(F::zero(), |acc, &val| acc + val * val)
                    .sqrt();
                let res_g_new_norm = g_new
                    .iter()
                    .fold(F::zero(), |acc, &val| acc + val * val)
                    .sqrt();

                let residual_new_norm =
                    (res_x_new_norm * res_x_new_norm + res_g_new_norm * res_g_new_norm).sqrt();

                // Accept if the residual is reduced
                if residual_new_norm < residual_norm {
                    x_corr = x_new;
                    y_corr = y_new;
                    break;
                }

                // Reduce damping factor
                alpha_damp *= const_f64::<F>(0.5);
            }

            // If damping factor got too small, the Newton iteration is not converging
            if alpha_damp < min_alpha {
                // Reduce step size and try again
                h *= const_f64::<F>(0.5);
                break;
            }
        }

        // Check for convergence of the Newton iteration
        if !converged {
            // If not converged, reduce step size and try again
            h *= const_f64::<F>(0.5);

            // If step size gets too small, the problem might be too stiff
            if h < min_step {
                return Err(IntegrateError::ComputationError(format!(
                    "Failed to converge at t = {t_current}. Step size too small."
                )));
            }

            n_rejected += 1;
            continue;
        }

        // Step accepted, update step count
        n_accepted += 1;

        // Estimate local error for step size control
        // For BDF methods, we can use the difference between the predictor and corrector
        let error_x = (&x_corr - &x_pred_orig).mapv(|x| x.abs());
        let error_y = (&y_corr - &y_pred_orig).mapv(|x| x.abs());

        // Compute scaled error norm
        let mut error_norm_x = F::zero();
        for i in 0..n_x {
            let scale = atol + rtol * x_corr[i].abs();
            error_norm_x += (error_x[i] / scale).powi(2);
        }
        error_norm_x = (error_norm_x / F::from_usize(n_x).expect("Operation failed")).sqrt();

        let mut error_norm_y = F::zero();
        for i in 0..n_y {
            let scale = atol + rtol * y_corr[i].abs();
            error_norm_y += (error_y[i] / scale).powi(2);
        }
        error_norm_y = (error_norm_y / F::from_usize(n_y).expect("Operation failed")).sqrt();

        // Take the maximum of the two error norms
        let error_norm = error_norm_x.max(error_norm_y);

        // Adjust the step size based on the error estimate
        let error_order = order as i32;
        let safety = const_f64::<F>(0.9); // Safety factor

        // Calculate the optimal step size
        let h_new = if error_norm > F::zero() {
            h * safety
                * (F::one() / error_norm)
                    .powf(F::one() / F::from_i32(error_order).expect("Operation failed"))
        } else {
            h * const_f64::<F>(2.0) // Double the step size if error is 0
        };

        // Limit step size increase and decrease
        let max_increase = const_f64::<F>(2.0);
        let max_decrease = const_f64::<F>(0.1);

        let h_new = (h_new / h).max(max_decrease).min(max_increase) * h;
        let h_new = h_new.min(max_step).max(min_step);

        // Update step size for next iteration
        h = h_new;

        // Store the solution
        t_values.push(t_new);
        x_values.push(x_corr.clone());
        y_values.push(y_corr.clone());

        // Update current values
        t_current = t_new;
        _x_current = x_corr;
        _y_current = y_corr;

        // Increment step counter
        n_steps += 1;

        // Adjust the order based on history
        if n_steps >= 5 {
            // At this point, we have enough history to consider increasing the order
            if order < max_order {
                order = (order + 1).min(max_order);
            }
        }
    }

    // Check if we reached the end time
    let success = t_current >= t_span[1];

    // Create result
    let result = DAEResult {
        t: t_values,
        x: x_values,
        y: y_values,
        success,
        message: if success {
            Some(format!("Successful integration. {n_steps} steps taken."))
        } else {
            Some(format!(
                "Integration did not reach end time. {n_steps} steps taken."
            ))
        },
        n_eval: n_f_evals,
        n_constraint_eval: n_g_evals,
        n_steps,
        n_accepted,
        n_rejected,
        n_lu: n_lu_decomps,
        n_jac: n_jac_evals,
        method: ODEMethod::Bdf,
        dae_type: DAEType::SemiExplicit,
        index: DAEIndex::Index1,
    };

    Ok(result)
}

/// BDF method for fully implicit DAE systems
///
/// Implements a specialized BDF method tailored for fully implicit DAE systems
/// of the form F(t, y, y') = 0
#[allow(dead_code)]
pub fn bdf_implicit_dae<F, FFunc>(
    f: FFunc,
    t_span: [F; 2],
    y0: Array1<F>,
    y_prime0: Array1<F>,
    options: DAEOptions<F>,
) -> IntegrateResult<DAEResult<F>>
where
    F: IntegrateFloat,
    FFunc: Fn(F, ArrayView1<F>, ArrayView1<F>) -> Array1<F>,
{
    // Get dimensions
    let n = y0.len();

    // Storage for solution
    let mut t_values = vec![t_span[0]];
    let mut y_values = vec![y0.clone()];
    let mut y_prime_values = vec![y_prime0.clone()];

    // Current values
    let mut t_current = t_span[0];
    let mut y_current = y0.clone();
    let mut _y_prime_current = y_prime0.clone();

    // Initial step size
    let mut h = options.h0.unwrap_or_else(|| {
        let _span = t_span[1] - t_span[0];
        _span * const_f64::<F>(0.01) // 1% of interval
    });

    // Step limits
    let min_step = options.min_step.unwrap_or_else(|| {
        let _span = t_span[1] - t_span[0];
        _span * const_f64::<F>(1e-6) // Very small relative to interval
    });

    let max_step = options.max_step.unwrap_or_else(|| {
        let _span = t_span[1] - t_span[0];
        _span * const_f64::<F>(0.1) // 10% of interval
    });

    // Maximum BDF order
    let max_order = options.max_order.unwrap_or(5).min(5);

    // Tolerances
    let rtol = options.rtol;
    let atol = options.atol;

    // Counters for statistics
    let mut n_steps = 0;
    let mut n_accepted = 0;
    let mut n_rejected = 0;
    let mut n_f_evals = 0;
    let mut n_jac_evals = 0;
    let mut n_lu_decomps = 0;

    // BDF method coefficients for various orders
    // Alpha coefficients (exclude α_0 = 1)
    let alpha_coeffs = [
        // Order 1 (Backward Euler)
        vec![const_f64::<F>(-1.0)],
        // Order 2
        vec![
            F::from_f64(-4.0 / 3.0).expect("Failed to convert to float"),
            F::from_f64(1.0 / 3.0).expect("Failed to convert to float"),
        ],
        // Order 3
        vec![
            F::from_f64(-18.0 / 11.0).expect("Failed to convert to float"),
            F::from_f64(9.0 / 11.0).expect("Failed to convert to float"),
            F::from_f64(-2.0 / 11.0).expect("Failed to convert to float"),
        ],
        // Order 4
        vec![
            F::from_f64(-48.0 / 25.0).expect("Failed to convert to float"),
            F::from_f64(36.0 / 25.0).expect("Failed to convert to float"),
            F::from_f64(-16.0 / 25.0).expect("Failed to convert to float"),
            F::from_f64(3.0 / 25.0).expect("Failed to convert to float"),
        ],
        // Order 5
        vec![
            F::from_f64(-300.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(300.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(-200.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(75.0 / 137.0).expect("Failed to convert to float"),
            F::from_f64(-12.0 / 137.0).expect("Failed to convert to float"),
        ],
    ];

    // Beta coefficients (for derivative approximation)
    let beta_coeffs = [
        const_f64::<F>(1.0),                                            // Order 1
        F::from_f64(2.0 / 3.0).expect("Failed to convert to float"),    // Order 2
        F::from_f64(6.0 / 11.0).expect("Failed to convert to float"),   // Order 3
        F::from_f64(12.0 / 25.0).expect("Failed to convert to float"),  // Order 4
        F::from_f64(60.0 / 137.0).expect("Failed to convert to float"), // Order 5
    ];

    // Current order (start with order 1 and build up)
    let mut order = 1;

    // Main integration loop
    while t_current < t_span[1] && n_steps < options.max_steps {
        // Adjust step size to hit the end point exactly if needed
        if t_current + h > t_span[1] {
            h = t_span[1] - t_current;
        }

        // New time point
        let t_new = t_current + h;

        // Build up history
        let history_start = if y_values.len() >= order {
            y_values.len() - order
        } else {
            0
        };

        let y_history = &y_values[history_start..];

        // Predict step using extrapolation
        let y_pred = predict_fully_implicit(y_history, order, h);

        // For the predictor's derivative, we use the BDF formula
        let y_prime_pred = if order == 1 {
            // For first-order, just use backward Euler
            (&y_pred - &y_current) / h
        } else {
            // For higher orders, compute using the BDF formula
            // y_prime = (Σ α_j * y_{n+1-j}) / (h * β)
            let mut y_prime = Array1::zeros(n);

            // Get coefficients for this order
            let alpha = &alpha_coeffs[order - 1];
            let beta = beta_coeffs[order - 1];

            // Compute the derivative approximation
            for i in 0..n {
                // First term: y_pred
                y_prime[i] = y_pred[i];

                // Historical terms: + Σ α_j * y_{n-j}
                for (j, &alpha_j) in alpha.iter().enumerate().take(order) {
                    let idx = y_values.len() - 1 - j;
                    if idx < y_values.len() {
                        y_prime[i] += alpha_j * y_values[idx][i];
                    }
                }

                // Scale by the beta coefficient
                y_prime[i] /= h * beta;
            }

            y_prime
        };

        // Store original predictions for error estimation
        let y_pred_orig = y_pred.clone();
        let _y_prime_pred_orig = y_prime_pred.clone();

        // Initialize corrector values
        let mut y_corr = y_pred;
        let mut y_prime_corr = y_prime_pred;

        // Maximum Newton iterations
        let max_newton_iter = options.max_newton_iterations;

        // Newton iteration for corrector
        let mut converged = false;
        for _iter in 0..max_newton_iter {
            // Evaluate the residual function
            let residual = f(t_new, y_corr.view(), y_prime_corr.view());
            n_f_evals += 1;

            // Compute residual norm
            let residual_norm = residual
                .iter()
                .fold(F::zero(), |acc, &val| acc + val * val)
                .sqrt();

            // Check convergence
            if residual_norm < options.newton_tol {
                converged = true;
                break;
            }

            // Compute the Jacobians of the residual function
            // We need ∂F/∂y and ∂F/∂y'

            // Jacobian with respect to y
            let jac_y = compute_jacobian_y_implicit(
                &f,
                t_new,
                y_corr.view(),
                y_prime_corr.view(),
                const_f64::<F>(1e-8),
            );

            // Jacobian with respect to y'
            let jac_y_prime = compute_jacobian_yprime_implicit(
                &f,
                t_new,
                y_corr.view(),
                y_prime_corr.view(),
                const_f64::<F>(1e-8),
            );
            n_jac_evals += 2;

            // For the Newton iteration, we need to solve:
            // [∂F/∂y + (∂F/∂y') * (∂y'/∂y)] * Δy = -F

            // For BDF, the derivative approximation is:
            // y' = (y - Σ α_j * y_{n-j}) / (h * β)
            // So ∂y'/∂y = 1 / (h * β)

            // Compute the combined Jacobian
            let beta = beta_coeffs[order - 1];
            let scale = F::one() / (h * beta);

            let mut combined_jac = jac_y.clone();
            for i in 0..n {
                for j in 0..n {
                    combined_jac[[i, j]] += jac_y_prime[[i, j]] * scale;
                }
            }

            // Negate the residual for the Newton step
            let neg_residual = residual.mapv(|x| -x);

            // Solve the linear system
            let delta_y = match solve_linear_system(&combined_jac, &neg_residual) {
                Ok(dy) => dy,
                Err(_e) => {
                    // If linear solve fails, reduce step size and try again
                    h *= const_f64::<F>(0.5);
                    break;
                }
            };
            n_lu_decomps += 1;

            // Apply the Newton step with damping if needed
            let mut alpha_damp = F::one();
            let min_alpha = const_f64::<F>(0.1);

            // Damped Newton iteration
            while alpha_damp >= min_alpha {
                // Apply the damped step
                let y_new = &y_corr + &(&delta_y * alpha_damp);

                // Compute the new derivative using the BDF formula
                let mut y_prime_new = Array1::zeros(n);

                // Get coefficients for this order
                let alpha = &alpha_coeffs[order - 1];
                let beta = beta_coeffs[order - 1];

                // Compute the derivative approximation
                for i in 0..n {
                    // First term: y_new
                    y_prime_new[i] = y_new[i];

                    // Historical terms: + Σ α_j * y_{n-j}
                    for j in 0..order {
                        if j < y_history.len() {
                            y_prime_new[i] += alpha[j] * y_history[y_history.len() - 1 - j][i];
                        }
                    }

                    // Scale by the beta coefficient
                    y_prime_new[i] /= h * beta;
                }

                // Evaluate the residual at the new point
                let residual_new = f(t_new, y_new.view(), y_prime_new.view());
                n_f_evals += 1;

                // Compute the new residual norm
                let residual_new_norm = residual_new
                    .iter()
                    .fold(F::zero(), |acc, &val| acc + val * val)
                    .sqrt();

                // Accept if the residual is reduced
                if residual_new_norm < residual_norm {
                    y_corr = y_new;
                    y_prime_corr = y_prime_new;
                    break;
                }

                // Reduce damping factor
                alpha_damp *= const_f64::<F>(0.5);
            }

            // If damping factor got too small, the Newton iteration is not converging
            if alpha_damp < min_alpha {
                // Reduce step size and try again
                h *= const_f64::<F>(0.5);
                break;
            }
        }

        // Check for convergence of the Newton iteration
        if !converged {
            // If not converged, reduce step size and try again
            h *= const_f64::<F>(0.5);

            // If step size gets too small, the problem might be too stiff
            if h < min_step {
                return Err(IntegrateError::ComputationError(format!(
                    "Failed to converge at t = {t_current}. Step size too small."
                )));
            }

            n_rejected += 1;
            continue;
        }

        // Step accepted, update step count
        n_accepted += 1;

        // Estimate local error for step size control
        // For BDF methods, we can use the difference between the predictor and corrector
        let error = (&y_corr - &y_pred_orig).mapv(|x| x.abs());

        // Compute scaled error norm
        let mut error_norm = F::zero();
        for i in 0..n {
            let scale = atol + rtol * y_corr[i].abs();
            error_norm += (error[i] / scale).powi(2);
        }
        error_norm = (error_norm / F::from_usize(n).expect("Operation failed")).sqrt();

        // Adjust the step size based on the error estimate
        let error_order = order as i32;
        let safety = const_f64::<F>(0.9); // Safety factor

        // Calculate the optimal step size
        let h_new = if error_norm > F::zero() {
            h * safety
                * (F::one() / error_norm)
                    .powf(F::one() / F::from_i32(error_order).expect("Operation failed"))
        } else {
            h * const_f64::<F>(2.0) // Double the step size if error is 0
        };

        // Limit step size increase and decrease
        let max_increase = const_f64::<F>(2.0);
        let max_decrease = const_f64::<F>(0.1);

        let h_new = (h_new / h).max(max_decrease).min(max_increase) * h;
        let h_new = h_new.min(max_step).max(min_step);

        // Update step size for next iteration
        h = h_new;

        // Store the solution
        t_values.push(t_new);
        y_values.push(y_corr.clone());
        y_prime_values.push(y_prime_corr.clone());

        // Update current values
        t_current = t_new;
        y_current = y_corr;
        _y_prime_current = y_prime_corr;

        // Increment step counter
        n_steps += 1;

        // Adjust the order based on history
        if n_steps >= 5 {
            // At this point, we have enough history to consider increasing the order
            if order < max_order {
                order = (order + 1).min(max_order);
            }
        }
    }

    // Check if we reached the end time
    let success = t_current >= t_span[1];

    // Create the empty array for algebraic variables
    // (in the fully implicit form, we don't separate differential and algebraic variables)
    let empty_array = Array1::<F>::zeros(0);
    let empty_y = vec![empty_array; t_values.len()];

    // Create result
    let result = DAEResult {
        t: t_values,
        x: y_values,
        y: empty_y,
        success,
        message: if success {
            Some(format!("Successful integration. {n_steps} steps taken."))
        } else {
            Some(format!(
                "Integration did not reach end time. {n_steps} steps taken."
            ))
        },
        n_eval: n_f_evals,
        n_constraint_eval: 0, // No explicit constraints in fully implicit form
        n_steps,
        n_accepted,
        n_rejected,
        n_lu: n_lu_decomps,
        n_jac: n_jac_evals,
        method: ODEMethod::Bdf,
        dae_type: DAEType::FullyImplicit,
        index: DAEIndex::Index1,
    };

    Ok(result)
}

/// Predict the next state for semi-explicit DAE using extrapolation
#[allow(dead_code)]
fn predict_step<F>(
    x_history: &[Array1<F>],
    y_history: &[Array1<F>],
    order: usize,
    h: F,
    t_current: F,
    t_history: &[F],
) -> (Array1<F>, Array1<F>)
where
    F: IntegrateFloat,
{
    let n_x = x_history[0].len();
    let n_y = y_history[0].len();

    let history_len = x_history.len();

    if history_len < 2 || order == 1 {
        // For first step or first-order method, just use constant extrapolation
        return (
            x_history[history_len - 1].clone(),
            y_history[history_len - 1].clone(),
        );
    }

    // For higher-order extrapolation, use polynomial interpolation
    let order_to_use = order.min(history_len - 1);

    // Start with the most recent point
    let mut x_pred = x_history[history_len - 1].clone();
    let mut y_pred = y_history[history_len - 1].clone();

    // Time for prediction
    let _t_pred = t_current + h;

    // Simple linear extrapolation for order 2
    if order_to_use == 1 {
        let dt = t_history[history_len - 1] - t_history[history_len - 2];
        let t_ratio = h / dt;

        // Linear extrapolation
        for i in 0..n_x {
            x_pred[i] = x_history[history_len - 1][i]
                + (x_history[history_len - 1][i] - x_history[history_len - 2][i]) * t_ratio;
        }

        for i in 0..n_y {
            y_pred[i] = y_history[history_len - 1][i]
                + (y_history[history_len - 1][i] - y_history[history_len - 2][i]) * t_ratio;
        }

        return (x_pred, y_pred);
    }

    // For higher orders, use a more sophisticated extrapolation
    // This could be improved with actual polynomial interpolation
    // For now, we'll just use a simple scaling of the trend

    // Use the most recent trend and amplify it based on the step size
    let dt_recent = t_history[history_len - 1] - t_history[history_len - 2];
    let t_ratio = h / dt_recent;

    // Scale by order factor to improve prediction for higher orders
    let scaling = F::from_f64(1.0 + 0.3 * order_to_use as f64).expect("Failed to convert to float");

    // Extrapolate
    for i in 0..n_x {
        x_pred[i] = x_history[history_len - 1][i]
            + (x_history[history_len - 1][i] - x_history[history_len - 2][i]) * t_ratio * scaling;
    }

    for i in 0..n_y {
        y_pred[i] = y_history[history_len - 1][i]
            + (y_history[history_len - 1][i] - y_history[history_len - 2][i]) * t_ratio * scaling;
    }

    (x_pred, y_pred)
}

/// Predict the next state for fully implicit DAE
#[allow(dead_code)]
fn predict_fully_implicit<F>(y_history: &[Array1<F>], order: usize, h: F) -> Array1<F>
where
    F: IntegrateFloat,
{
    let n = y_history[0].len();
    let history_len = y_history.len();

    if history_len < 2 || order == 1 {
        // For first step or first-order method, just use constant extrapolation
        return y_history[history_len - 1].clone();
    }

    // For higher-order extrapolation, we'll use a simple polynomial predictor
    // For simplicity, we'll just use linear extrapolation here
    // In a full implementation, higher-order predictors would be used

    let mut y_pred = y_history[history_len - 1].clone();

    // Simple linear extrapolation
    for i in 0..n {
        y_pred[i] = y_history[history_len - 1][i]
            + (y_history[history_len - 1][i] - y_history[history_len - 2][i]);
    }

    y_pred
}

/// Compute the Jacobian of a function with respect to x variables
#[allow(dead_code)]
fn compute_jacobian_x<F, Func>(
    f: &Func,
    t: F,
    x: ArrayView1<F>,
    y: ArrayView1<F>,
    epsilon: F,
) -> Array2<F>
where
    F: IntegrateFloat,
    Func: Fn(F, ArrayView1<F>, ArrayView1<F>) -> Array1<F>,
{
    let n_x = x.len();
    let n_f = f(t, x, y).len();
    let mut jacobian = Array2::<F>::zeros((n_f, n_x));

    // Base function value
    let f_base = f(t, x, y);

    // Compute the Jacobian using finite differences
    let mut x_perturbed = x.to_owned();

    for j in 0..n_x {
        // Compute the perturbation size based on the variable magnitude
        let h = epsilon.max(x[j].abs() * epsilon);

        // Perturb the jth component
        x_perturbed[j] = x[j] + h;

        // Evaluate the function with the perturbed variable
        let f_perturbed = f(t, x_perturbed.view(), y);

        // Reset the perturbation
        x_perturbed[j] = x[j];

        // Compute the finite difference approximation
        let col_j = (f_perturbed - &f_base) / h;

        // Store in the Jacobian
        for i in 0..n_f {
            jacobian[[i, j]] = col_j[i];
        }
    }

    jacobian
}

/// Compute the Jacobian of a function with respect to y variables
#[allow(dead_code)]
fn compute_jacobian_y<F, Func>(
    f: &Func,
    t: F,
    x: ArrayView1<F>,
    y: ArrayView1<F>,
    epsilon: F,
) -> Array2<F>
where
    F: IntegrateFloat,
    Func: Fn(F, ArrayView1<F>, ArrayView1<F>) -> Array1<F>,
{
    let n_y = y.len();
    let n_f = f(t, x, y).len();
    let mut jacobian = Array2::<F>::zeros((n_f, n_y));

    // Base function value
    let f_base = f(t, x, y);

    // Compute the Jacobian using finite differences
    let mut y_perturbed = y.to_owned();

    for j in 0..n_y {
        // Compute the perturbation size based on the variable magnitude
        let h = epsilon.max(y[j].abs() * epsilon);

        // Perturb the jth component
        y_perturbed[j] = y[j] + h;

        // Evaluate the function with the perturbed variable
        let f_perturbed = f(t, x, y_perturbed.view());

        // Reset the perturbation
        y_perturbed[j] = y[j];

        // Compute the finite difference approximation
        let col_j = (f_perturbed - &f_base) / h;

        // Store in the Jacobian
        for i in 0..n_f {
            jacobian[[i, j]] = col_j[i];
        }
    }

    jacobian
}

/// Compute the Jacobian of a function with respect to y for implicit DAE
#[allow(dead_code)]
fn compute_jacobian_y_implicit<F, Func>(
    f: &Func,
    t: F,
    y: ArrayView1<F>,
    y_prime: ArrayView1<F>,
    epsilon: F,
) -> Array2<F>
where
    F: IntegrateFloat,
    Func: Fn(F, ArrayView1<F>, ArrayView1<F>) -> Array1<F>,
{
    let n = y.len();
    let n_f = f(t, y, y_prime).len();
    let mut jacobian = Array2::<F>::zeros((n_f, n));

    // Base function value
    let f_base = f(t, y, y_prime);

    // Compute the Jacobian using finite differences
    let mut y_perturbed = y.to_owned();

    for j in 0..n {
        // Compute the perturbation size based on the variable magnitude
        let h = epsilon.max(y[j].abs() * epsilon);

        // Perturb the jth component
        y_perturbed[j] = y[j] + h;

        // Evaluate the function with the perturbed variable
        let f_perturbed = f(t, y_perturbed.view(), y_prime);

        // Reset the perturbation
        y_perturbed[j] = y[j];

        // Compute the finite difference approximation
        let col_j = (f_perturbed - &f_base) / h;

        // Store in the Jacobian
        for i in 0..n_f {
            jacobian[[i, j]] = col_j[i];
        }
    }

    jacobian
}

/// Compute the Jacobian of a function with respect to y' for implicit DAE
#[allow(dead_code)]
fn compute_jacobian_yprime_implicit<F, Func>(
    f: &Func,
    t: F,
    y: ArrayView1<F>,
    y_prime: ArrayView1<F>,
    epsilon: F,
) -> Array2<F>
where
    F: IntegrateFloat,
    Func: Fn(F, ArrayView1<F>, ArrayView1<F>) -> Array1<F>,
{
    let n = y_prime.len();
    let n_f = f(t, y, y_prime).len();
    let mut jacobian = Array2::<F>::zeros((n_f, n));

    // Base function value
    let f_base = f(t, y, y_prime);

    // Compute the Jacobian using finite differences
    let mut y_prime_perturbed = y_prime.to_owned();

    for j in 0..n {
        // Compute the perturbation size based on the variable magnitude
        let h = epsilon.max(y_prime[j].abs() * epsilon);

        // Perturb the jth component
        y_prime_perturbed[j] = y_prime[j] + h;

        // Evaluate the function with the perturbed variable
        let f_perturbed = f(t, y, y_prime_perturbed.view());

        // Reset the perturbation
        y_prime_perturbed[j] = y_prime[j];

        // Compute the finite difference approximation
        let col_j = (f_perturbed - &f_base) / h;

        // Store in the Jacobian
        for i in 0..n_f {
            jacobian[[i, j]] = col_j[i];
        }
    }

    jacobian
}

/// Solve a linear system using Gaussian elimination with partial pivoting
#[allow(dead_code)]
fn solve_linear_system<F>(a: &Array2<F>, b: &Array1<F>) -> IntegrateResult<Array1<F>>
where
    F: IntegrateFloat,
{
    let n = a.shape()[0];
    if n != a.shape()[1] || n != b.len() {
        return Err(IntegrateError::DimensionMismatch(format!(
            "Matrix dimensions don't match: A is {}x{}, b is {}",
            a.shape()[0],
            a.shape()[1],
            b.len()
        )));
    }

    // Create copies of A and b for in-place operations
    let mut a_copy = a.clone();
    let mut b_copy = b.clone();

    // Gaussian elimination with partial pivoting
    for k in 0..n - 1 {
        // Find pivot
        let mut p = k;
        for i in k + 1..n {
            if a_copy[[i, k]].abs() > a_copy[[p, k]].abs() {
                p = i;
            }
        }

        // Swap rows if needed
        if p != k {
            for j in k..n {
                let temp = a_copy[[k, j]];
                a_copy[[k, j]] = a_copy[[p, j]];
                a_copy[[p, j]] = temp;
            }
            let temp = b_copy[k];
            b_copy[k] = b_copy[p];
            b_copy[p] = temp;
        }

        // Check for singularity
        if a_copy[[k, k]].abs() < const_f64::<F>(1e-10) {
            return Err(IntegrateError::ComputationError(format!(
                "Matrix is singular at row {k}"
            )));
        }

        // Elimination
        for i in k + 1..n {
            let factor = a_copy[[i, k]] / a_copy[[k, k]];
            b_copy[i] = b_copy[i] - factor * b_copy[k];
            for j in k..n {
                a_copy[[i, j]] = a_copy[[i, j]] - factor * a_copy[[k, j]];
            }
        }
    }

    // Check the last pivot
    if a_copy[[n - 1, n - 1]].abs() < const_f64::<F>(1e-10) {
        return Err(IntegrateError::ComputationError(
            "Matrix is singular at the last row".to_string(),
        ));
    }

    // Back substitution
    let mut x = Array1::<F>::zeros(n);
    x[n - 1] = b_copy[n - 1] / a_copy[[n - 1, n - 1]];
    for i in (0..n - 1).rev() {
        let mut sum = F::zero();
        for j in i + 1..n {
            sum += a_copy[[i, j]] * x[j];
        }
        x[i] = (b_copy[i] - sum) / a_copy[[i, i]];
    }

    Ok(x)
}