/// Get a function that returns the Hessian of the provided multivariate, scalar-valued function.
///
/// The Hessian is computed using forward-mode automatic differentiation with hyper-dual numbers.
///
/// # Arguments
///
/// * `f` - Multivariate, scalar-valued function, $f:\mathbb{R}^{n}\to\mathbb{R}$.
/// * `func_name` - Name of the function that will return the Hessian of $f(\mathbf{x})$ at any
///   point $\mathbf{x}\in\mathbb{R}^{n}$.
/// * `param_type` (optional) - Type of the extra runtime parameter `p` that is passed to `f`.
///   Defaults to `[f64]` (implying that `f` accepts `p: &[f64]`).
///
/// # Defining `f`
///
/// The multivariate, scalar-valued function `f` must have the following function signature:
///
/// ```ignore
/// fn f<S: Scalar, V: Vector<S>>(x: &V, p: &[f64]) -> S {
///     // place function contents here
/// }
/// ```
///
/// For the automatic differentiation to work, `f` must be fully generic over the types of scalars
/// and vectors used.
///
/// # Warning
///
/// `f` cannot be defined as closure. It must be defined as a function.
///
/// # Warning
///
/// This Hessian function generated by this macro will always return a dynamically-sized matrix,
/// even if the function `f` uses statically-sized vectors. This is to avoid needing to pass a const
/// generic to this function to define the size ($n$) of the Hessian. Instead, the dimension is
/// determined at runtime.
///
/// # Note
///
/// The function produced by this macro will perform $\frac{n(n+1)}{2}$ evaluations of
/// $f(\mathbf{x})$ to evaluate its Hessian. This is more efficient than using finite differences,
/// which would require $\frac{n(n+1)}{2} + 1$ evaluations.
///
/// # Examples
///
/// ## Basic Example
///
/// Compute the Hessian of
///
/// $$f(\mathbf{x}) = x_{0}^{5}x_{1}+x_{0}\sin^{3}(x_{1})$$
///
/// at $\mathbf{x} = (5, 8)^T$, and compare the result to the true result of
///
/// $$
/// \begin{aligned}
///     \mathbf{H}\left((5,8)^{T}\right)&=
///     \begin{bmatrix}
///         20x_{0}^{3}x_{1} & 5x_{0}^{4}+3\sin^{2}{x_{1}}\cos{x_{1}} \\\\
///         5x_{0}^{4}+3\sin^{2}{x_{1}}\cos{x_{1}} & 6x_{0}\sin{x_{1}}\cos^{2}{x_{1}}-3x_{0}\sin^{3}{x_{1}}
///     \end{bmatrix}
///     \bigg\rvert_{\mathbf{x}=(5,8)^{T}}
/// \end{aligned}
/// $$
///
/// #### Using standard vectors
///
/// ```
/// use linalg_traits::{Mat, Matrix, Scalar, Vector};
/// use numtest::*;
///
/// use numdiff::{get_shessian, HyperDual, HyperDualVector};
///
/// // Define the function, f(x).
/// fn f<S: Scalar, V: Vector<S>>(x: &V, _p: &[f64]) -> S {
///     x.vget(0).powi(5) * x.vget(1) + x.vget(0) * x.vget(1).sin().powi(3)
/// }
///
/// // Define the evaluation point.
/// let x0 = vec![5.0, 8.0];
///
/// // Parameter vector (empty for this example).
/// let p = [];
///
/// // Autogenerate the function "hess" that can be used to compute the Hessian of f(x) at any point
/// // x.
/// get_shessian!(f, hess);
///
/// // Evaluate the Hessian using "hess".
/// let hess_eval: Mat<f64> = hess(&x0, &p);
///
/// // True Hessian of f(x) at the evaluation point.
/// let hess_true: Mat<f64> = Mat::from_row_slice(
///     2,
///     2,
///     &[
///         20000.0,
///         3125.0 + 3.0 * 8.0_f64.sin().powi(2) * 8.0_f64.cos(),
///         3125.0 + 3.0 * 8.0_f64.sin().powi(2) * 8.0_f64.cos(),
///         30.0 * 8.0_f64.sin() * 8.0_f64.cos().powi(2) - 15.0 * 8.0_f64.sin().powi(3),
///     ],
/// );
///
/// // Verify that the Hessian function obtained using get_shessian! computes the Hessian correctly.
/// assert_arrays_equal_to_decimal!(hess_eval, hess_true, 16);
/// ```
///
/// #### Using other vector types
///
/// The function produced by `get_shessian!` can accept _any_ type for `x0`, as long as it
/// implements the `linalg_traits::Vector` trait.
///
/// ```
/// use faer::Mat;
/// use linalg_traits::{Scalar, Vector};
/// use nalgebra::{dvector, DMatrix, DVector, SMatrix, SVector};
/// use ndarray::{array, Array1, Array2};
///
/// use numdiff::{get_shessian, HyperDual, HyperDualVector};
///
/// // Define the function, f(x).
/// fn f<S: Scalar, V: Vector<S>>(x: &V, _p: &[f64]) -> S {
///     x.vget(0).powi(5) * x.vget(1) + x.vget(0) * x.vget(1).sin().powi(3)
/// }
///
/// // Parameter vector (empty for this example).
/// let p = [];
///
/// // Autogenerate the function "hess" that can be used to compute the Hessian of f(x) at any
/// // point x.
/// get_shessian!(f, hess);
///
/// // nalgebra::DVector
/// let x0: DVector<f64> = dvector![5.0, 8.0];
/// let hess_eval: DMatrix<f64> = hess(&x0, &p);
///
/// // nalgebra::SVector
/// let x0: SVector<f64, 2> = SVector::from_slice(&[5.0, 8.0]);
/// let hess_eval: DMatrix<f64> = hess(&x0, &p);
///
/// // ndarray::Array1
/// let x0: Array1<f64> = array![5.0, 8.0];
/// let hess_eval: Array2<f64> = hess(&x0, &p);
///
/// // faer::Mat
/// let x0: Mat<f64> = Mat::from_slice(&[5.0, 8.0]);
/// let hess_eval: Mat<f64> = hess(&x0, &p);
/// ```
///
/// ## Example Passing Runtime Parameters
///
/// Compute the Hessian of a parameterized function
///
/// $$f(\mathbf{x}) = ax_{0}^{2}x_{1}+bx_{0}x_{1}^{2}+cx_{0}^{2}+dx_{1}^{2}$$
///
/// where $a$, $b$, $c$, and $d$ are runtime parameters. Compare the result against the true
/// Hessian of
///
/// $$
/// \mathbf{H}=
/// \begin{bmatrix}
/// 2ax_{1}+2c & 2ax_{0}+2bx_{1} \\\\
/// 2ax_{0}+2bx_{1} & 2bx_{0}+2d
/// \end{bmatrix}
/// $$
///
/// ```
/// use linalg_traits::{Mat, Matrix, Scalar, Vector};
/// use numtest::*;
///
/// use numdiff::{get_shessian, HyperDual, HyperDualVector};
///
/// // Define the function, f(x).
/// fn f<S: Scalar, V: Vector<S>>(x: &V, p: &[f64]) -> S {
///     let a = S::new(p[0]);
///     let b = S::new(p[1]);
///     let c = S::new(p[2]);
///     let d = S::new(p[3]);
///     a * x.vget(0).powi(2) * x.vget(1) + b * x.vget(0) * x.vget(1).powi(2) + c * x.vget(0).powi(2) + d * x.vget(1).powi(2)
/// }
///
/// // Runtime parameters.
/// let a = 1.5;
/// let b = 2.0;
/// let c = 0.8;
/// let d = 3.0;
/// let p = [a, b, c, d];
///
/// // Evaluation point.
/// let x0 = vec![1.0, -0.5];
///
/// // Autogenerate the Hessian function.
/// get_shessian!(f, hess);
///
/// // True Hessian function.
/// let hess_true = Mat::from_row_slice(2, 2, &[
///     2.0 * a * x0[1] + 2.0 * c, 2.0 * a * x0[0] + 2.0 * b * x0[1],
///     2.0 * a * x0[0] + 2.0 * b * x0[1], 2.0 * b * x0[0] + 2.0 * d
/// ]);
///
/// // Compute the Hessian using both the automatically generated Hessian function and the true
/// // Hessian function, and compare the results.
/// let hess_eval: Mat<f64> = hess(&x0, &p);
/// assert_arrays_equal_to_decimal!(hess_eval, hess_true, 16);
/// ```
///
/// ## Example Passing Custom Parameter Types
///
/// Use a custom parameter struct instead of `f64` values.
///
/// ```
/// use linalg_traits::{Mat, Matrix, Scalar, Vector};
/// use numtest::*;
///
/// use numdiff::{get_shessian, HyperDual, HyperDualVector};
///
/// struct Data {
///     a: f64,
///     b: f64,
///     c: f64,
///     d: f64,
/// }
///
/// // Define the function, f(x).
/// fn f<S: Scalar, V: Vector<S>>(x: &V, p: &Data) -> S {
///     let a = S::new(p.a);
///     let b = S::new(p.b);
///     let c = S::new(p.c);
///     let d = S::new(p.d);
///     a * x.vget(0).powi(2) * x.vget(1) + b * x.vget(0) * x.vget(1).powi(2) + c * x.vget(0).powi(2) + d * x.vget(1).powi(2)
/// }
///
/// // Runtime parameter struct.
/// let p = Data {
///     a: 1.5,
///     b: 2.0,
///     c: 0.8,
///     d: 3.0,
/// };
///
/// // Evaluation point.
/// let x0 = vec![1.0, -0.5];
///
/// // Autogenerate the Hessian function, telling the macro to expect a runtime parameter of type
/// // &Data.
/// get_shessian!(f, hess, Data);
///
/// // True Hessian function.
/// let hess_true = Mat::from_row_slice(2, 2, &[
///     2.0 * p.a * x0[1] + 2.0 * p.c, 2.0 * p.a * x0[0] + 2.0 * p.b * x0[1],
///     2.0 * p.a * x0[0] + 2.0 * p.b * x0[1], 2.0 * p.b * x0[0] + 2.0 * p.d
/// ]);
///
/// // Compute the Hessian using both the automatically generated Hessian function and the true
/// // Hessian function, and compare the results.
/// let hess_eval: Mat<f64> = hess(&x0, &p);
/// assert_arrays_equal_to_decimal!(hess_eval, hess_true, 16);
/// ```
#[macro_export]
macro_rules! get_shessian {
    ($f:ident, $func_name:ident) => {
        get_shessian!($f, $func_name, [f64]);
    };
    ($f:ident, $func_name:ident, $param_type:ty) => {
        /// Hessian of a multivariate, scalar-valued function `f: ℝⁿ → ℝ`.
        ///
        /// This function is generated for a specific function `f` using the
        /// `numdiff::get_shessian!` macro.
        ///
        /// # Arguments
        ///
        /// * `x0` - Evaluation point, `x₀ ∈ ℝⁿ`.
        /// * `p` - Extra runtime parameter. This is a parameter (can be of any arbitrary type)
        ///   defined at runtime that the function may depend on but is not differentiated with
        ///   respect to.
        ///
        /// # Returns
        ///
        /// Hessian of `f` with respect to `x`, evaluated at `x = x₀`.
        ///
        /// `H(x₀) = (∂²f/∂x²)|ₓ₌ₓ₀ ∈ ℝⁿˣⁿ`
        fn $func_name<S, V>(x0: &V, p: &$param_type) -> V::DMatrixMxNf64
        where
            S: Scalar,
            V: Vector<S>,
        {
            // Promote the evaluation point to a vector of hyper-dual numbers.
            let x0_hyper_dual = x0.clone().to_hyper_dual_vector();

            // Determine the size of the Hessian.
            let n = x0.len();

            // Initialize a matrix of zeros to store the Hessian.
            let mut hess = x0.new_dmatrix_m_by_n_f64(n);

            // Variable to store the evaluation point perturbed with hyper-dual directions.
            let mut x_perturbed;

            // Evaluate the Hessian, iterating over the upper triangular elements.
            for i in 0..n {
                for j in i..n {
                    // Make a copy of the hyper-dual evaluation point.
                    x_perturbed = x0_hyper_dual.clone();

                    // Diagonal element (∂²f/∂xᵢ²).
                    if i == j {
                        // Set both ε₁ and ε₂ coefficients for variable i to 1.0.
                        let original = x_perturbed.vget(i);
                        x_perturbed.vset(i, HyperDual::new(original.get_a(), 1.0, 1.0, 0.0));
                    }
                    // Off-diagonal element (∂²f/∂xᵢ∂xⱼ).
                    else {
                        // Set ε₁ coefficient for variable i and ε₂ coefficient for variable j to
                        // 1.0.
                        let original_i = x_perturbed.vget(i);
                        let original_j = x_perturbed.vget(j);
                        x_perturbed.vset(i, HyperDual::new(original_i.get_a(), 1.0, 0.0, 0.0));
                        x_perturbed.vset(j, HyperDual::new(original_j.get_a(), 0.0, 1.0, 0.0));
                    }

                    // Evaluate the function at the perturbed point.
                    let f_result = $f(&x_perturbed, p);

                    // Extract the second-order derivative from the hyper-dual result.
                    let second_derivative = f_result.get_d();

                    // Store the result in both (i,j) and (j,i) positions (Hessian is symmetric).
                    hess[(i, j)] = second_derivative;
                    hess[(j, i)] = second_derivative;
                }
            }

            hess
        }
    };
}

#[cfg(test)]
mod tests {
    use crate::{HyperDual, HyperDualVector};
    use linalg_traits::{Mat, Matrix, Scalar, Vector};
    use nalgebra::{DMatrix, DVector, SMatrix, SVector, dvector};
    use ndarray::{Array1, Array2, array};
    use numtest::*;

    #[test]
    fn test_shessian_1() {
        // Function to take the Hessian of.
        fn f<S: Scalar, V: Vector<S>>(x: &V, _p: &[f64]) -> S {
            x.vget(0).powi(3)
        }

        // Evaluation point.
        let x0 = vec![2.0];

        // Parameter vector (empty for this test).
        let p = [];

        // True Hessian function.
        let hess = |x: &Vec<f64>| Mat::from_row_slice(1, 1, &[6.0 * x[0]]);

        // Hessian function obtained via forward-mode automatic differentation.
        get_shessian!(f, hess_autodiff);

        // Evaluate the Hessian using both functions.
        let hess_eval_autodiff: Mat<f64> = hess_autodiff(&x0, &p);
        let hess_eval: Mat<f64> = hess(&x0);

        // Test autodiff Hessian against true Hessian.
        assert_arrays_equal_to_decimal!(hess_eval_autodiff, hess_eval, 16);
    }

    #[test]
    fn test_shessian_2() {
        // Function to take the Hessian of.
        fn f<S: Scalar, V: Vector<S>>(x: &V, _p: &[f64]) -> S {
            x.vget(0).powi(2) + x.vget(1).powi(3)
        }

        // Evaluation point.
        let x0 = SVector::from_row_slice(&[1.0, 2.0]);

        // Parameter vector (empty for this test).
        let p = [];

        // True Hessian function.
        let hess = |x: &SVector<f64, 2>| {
            SMatrix::<f64, 2, 2>::from_row_slice(&[2.0, 0.0, 0.0, 6.0 * x[1]])
        };

        // Hessian function obtained via forward-mode automatic differentation.
        get_shessian!(f, hess_autodiff);

        // Evaluate the Hessian using both functions.
        let hess_eval_autodiff: DMatrix<f64> = hess_autodiff(&x0, &p);
        let hess_eval: SMatrix<f64, 2, 2> = hess(&x0);

        // Test autodiff Hessian against true Hessian.
        assert_arrays_equal_to_decimal!(hess_eval_autodiff, hess_eval, 16);
    }

    #[test]
    fn test_shessian_3() {
        // Function to take the Hessian of.
        fn f<S: Scalar, V: Vector<S>>(x: &V, _p: &[f64]) -> S {
            x.vget(0).powi(5) * x.vget(1) + x.vget(0) * x.vget(1).sin().powi(3)
        }

        // Evaluation point.
        let x0 = array![1.0, 2.0];

        // Parameter vector (empty for this test).
        let p = [];

        // True Hessian function.
        let hess = |x: &Array1<f64>| {
            Array2::<f64>::from_row_slice(
                2,
                2,
                &[
                    20.0 * x[0].powi(3) * x[1],
                    5.0 * x[0].powi(4) + 3.0 * x[1].sin().powi(2) * x[1].cos(),
                    5.0 * x[0].powi(4) + 3.0 * x[1].sin().powi(2) * x[1].cos(),
                    6.0 * x[0] * x[1].sin() * x[1].cos().powi(2) - 3.0 * x[0] * x[1].sin().powi(3),
                ],
            )
        };

        // Hessian function obtained via forward-mode automatic differentation.
        get_shessian!(f, hess_autodiff);

        // Evaluate the Hessian using both functions.
        let hess_eval_autodiff: Array2<f64> = hess_autodiff(&x0, &p);
        let hess_eval: Array2<f64> = hess(&x0);

        // Test autodiff Hessian against true Hessian.
        assert_arrays_equal_to_decimal!(hess_eval_autodiff, hess_eval, 16);
    }

    #[test]
    fn test_shessian_4() {
        // Function to take the Hessian of.
        #[allow(clippy::many_single_char_names)]
        fn f<S: Scalar, V: Vector<S>>(x: &V, p: &[f64]) -> S {
            let a = S::new(p[0]);
            let b = S::new(p[1]);
            let c = S::new(p[2]);
            let d = S::new(p[3]);
            a * x.vget(0).powi(2) * x.vget(1)
                + b * x.vget(0) * x.vget(1).powi(2)
                + c * x.vget(0).powi(2)
                + d * x.vget(1).powi(2)
        }

        // Parameter vector.
        let p = [1.5, 2.0, 0.8, 3.0];

        // Evaluation point.
        let x0 = dvector![1.0, -0.5];

        // True Hessian function.
        let hess = |x: &DVector<f64>, p: &[f64]| {
            DMatrix::<f64>::from_row_slice(
                2,
                2,
                &[
                    2.0 * p[0] * x[1] + 2.0 * p[2],
                    2.0 * p[0] * x[0] + 2.0 * p[1] * x[1],
                    2.0 * p[0] * x[0] + 2.0 * p[1] * x[1],
                    2.0 * p[1] * x[0] + 2.0 * p[3],
                ],
            )
        };

        // Hessian function obtained via forward-mode automatic differentiation.
        get_shessian!(f, hess_autodiff);

        // Evaluate the Hessian using both methods.
        let hess_eval_autodiff: DMatrix<f64> = hess_autodiff(&x0, &p);
        let hess_eval: DMatrix<f64> = hess(&x0, &p);

        // Test autodiff Hessian against true Hessian.
        assert_arrays_equal_to_decimal!(hess_eval_autodiff, hess_eval, 16);
    }

    #[test]
    fn test_shessian_custom_params() {
        struct Data {
            a: f64,
            b: f64,
            c: f64,
            d: f64,
        }

        // Function to take the Hessian of.
        #[allow(clippy::many_single_char_names)]
        fn f<S: Scalar, V: Vector<S>>(x: &V, p: &Data) -> S {
            let a = S::new(p.a);
            let b = S::new(p.b);
            let c = S::new(p.c);
            let d = S::new(p.d);
            a * x.vget(0).powi(2) * x.vget(1)
                + b * x.vget(0) * x.vget(1).powi(2)
                + c * x.vget(0).powi(2)
                + d * x.vget(1).powi(2)
        }

        // Runtime parameter struct.
        let p = Data {
            a: 1.5,
            b: 2.0,
            c: 0.8,
            d: 3.0,
        };

        // Evaluation point.
        let x0 = vec![1.0, -0.5];

        // Hessian function obtained via forward-mode automatic differentiation.
        get_shessian!(f, hess, Data);

        // True Hessian function.
        let hess_true = Mat::from_row_slice(
            2,
            2,
            &[
                2.0 * p.a * x0[1] + 2.0 * p.c,
                2.0 * p.a * x0[0] + 2.0 * p.b * x0[1],
                2.0 * p.a * x0[0] + 2.0 * p.b * x0[1],
                2.0 * p.b * x0[0] + 2.0 * p.d,
            ],
        );

        // Evaluate the Hessian using both functions.
        let hess_eval: Mat<f64> = hess(&x0, &p);

        // Test autodiff Hessian against true Hessian.
        assert_arrays_equal_to_decimal!(hess_eval, hess_true, 16);
    }
}
