use crate::Dual;
use linalg_traits::{Scalar, Vector};

/// Trait to create a vector of dual numbers.
pub trait DualVector<S, V>
where
    S: Scalar,
    V: Vector<S>,
{
    /// Convert this vector of scalars to a vector of dual numbers.
    ///
    /// # Returns
    ///
    /// A copy of this vector with each element converted to a dual number (with dual part `0.0`).
    fn to_dual_vector(self) -> V::VectorT<Dual>;
}

impl<S, V> DualVector<S, V> for V
where
    S: Scalar,
    V: Vector<S>,
{
    fn to_dual_vector(self) -> V::VectorT<Dual> {
        let mut vec_dual = V::VectorT::new_with_length(self.len());
        for i in 0..self.len() {
            vec_dual.vset(i, Dual::from_real(self.vget(i).to_f64().unwrap()));
        }
        vec_dual
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use nalgebra::{SVector, dvector};
    use ndarray::array;

    #[test]
    fn test_vec() {
        let vec = vec![1.0, 2.0, 3.0];
        assert_eq!(
            vec.to_dual_vector(),
            vec![
                Dual::from_real(1.0),
                Dual::from_real(2.0),
                Dual::from_real(3.0)
            ]
        );
    }

    #[test]
    fn test_nalgebra_dvector() {
        let vec = dvector![1.0, 2.0, 3.0];
        assert_eq!(
            vec.to_dual_vector(),
            dvector![
                Dual::from_real(1.0),
                Dual::from_real(2.0),
                Dual::from_real(3.0)
            ]
        );
    }

    #[test]
    fn test_nalgebra_svector() {
        let vec = SVector::<f64, 3>::from_row_slice(&[1.0, 2.0, 3.0]);
        assert_eq!(
            vec.to_dual_vector(),
            SVector::<Dual, 3>::from_row_slice(&[
                Dual::from_real(1.0),
                Dual::from_real(2.0),
                Dual::from_real(3.0)
            ])
        );
    }

    #[test]
    fn test_ndarray_array1() {
        let vec = array![1.0, 2.0, 3.0];
        assert_eq!(
            vec.to_dual_vector(),
            vec![
                Dual::from_real(1.0),
                Dual::from_real(2.0),
                Dual::from_real(3.0)
            ]
        );
    }
}
