use std::convert::Infallible;
use nalgebra::{allocator::Allocator, DefaultAllocator, Dim, OVector, SVector};

use super::FitnessFunction;

pub struct Sphere<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D>
{
    offset: OVector<f64, D>
}

impl<D> Sphere<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D>
{
    pub fn new(offset: OVector<f64, D>) -> Self {
        Sphere {
            offset
        }
    }
}

impl<D> FitnessFunction for Sphere<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D>
{
    type In = OVector<f64, D>;
    type Out = f64;
    type Err = Infallible;

    fn fit(self: &Self, chromosome: &OVector<f64, D>) -> Result<f64, Infallible> {
        Ok(chromosome
           .iter()
           .zip(&self.offset)
           .map(|(x, o)| (x - o).powi(2))
           .sum())
    }
}

#[cfg(test)]
pub mod tests {
    use nalgebra::{Dyn, OVector};

    use crate::{fitness::{sphere::Sphere, FitnessFunction}, test_infra::load_test_file};

    #[test]
    fn test_sphere() {
        let data = load_test_file::<f64, f64>("tests/sphere.txt");

        for test in data {
            assert_eq!(
                Sphere::<Dyn>::new(OVector::<_, Dyn>::from_vec(vec![1.0; test.inp.len()]))
                    .fit(&OVector::<_, Dyn>::from_vec(test.inp)).unwrap(),
                test.out
            )
        }
    }
}