use std::{convert::Infallible, f64::consts::PI, marker::PhantomData};

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

use super::FitnessFunction;

pub struct Linear<const LEN: usize> {
    a0: f64,
    a: SVector<f64, LEN>,
}

impl<const LEN: usize> Linear<LEN> {
    pub fn new(a0: f64, a: SVector<f64, LEN>) -> Self {
        Self {
            a0,
            a
        }
    }
}

impl<const LEN: usize> FitnessFunction for Linear<LEN> {
    type In = SVector<f64, LEN>;
    type Out = f64;
    type Err = Infallible;

    fn fit(self: &Self, inp: &Self::In) -> Result<Self::Out, Self::Err> {
        Ok(self.a0 + (self.a.transpose() * inp).x)
    }
}

pub struct Step<const LEN: usize> {
    a0: f64,
    a: SVector<f64, LEN>,
}

impl<const LEN: usize> Step<LEN> {
    pub fn new(a0: f64, a: SVector<f64, LEN>) -> Self {
        Self {
            a0,
            a
        }
    }
}

impl<const LEN: usize> FitnessFunction for Step<LEN> {
    type In = SVector<f64, LEN>;
    type Out = f64;
    type Err = Infallible;

    fn fit(self: &Self, inp: &Self::In) -> Result<Self::Out, Self::Err> {
        Ok(self.a0 + inp.component_mul(&self.a).map(|x| x.floor()).sum())
    }
}

pub struct Rastrigin<TDim: Dim> {
    _phantom: PhantomData<TDim>
}

impl<TDim: Dim> Rastrigin<TDim> {
    pub fn new() -> Self {
        Self {
            _phantom: PhantomData
        }
    }
}

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

    fn fit(self: &Self, inp: &Self::In) -> Result<Self::Out, Self::Err> {
        let dim = inp.len() as f64;
        Ok(10.0 * dim + inp.iter()
           .map(|x| x.powi(2) - 10.0 * (2.0 * PI * x).cos())
           .sum::<f64>())
    }
}

pub struct Griewank<TDim: Dim> {
    _phantom: PhantomData<TDim>
}

impl<TDim: Dim> Griewank<TDim> {
    pub fn new() -> Self {
        Self {
            _phantom: PhantomData
        }
    }
}

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

    fn fit(self: &Self, inp: &Self::In) -> Result<Self::Out, Self::Err> {
        Ok(1.0 + inp.map(|x| x.powi(2)).sum() / 4000.0 - inp
           .iter()
           .enumerate()
           .map(|(i, x)| (x / ((i + 1) as f64).sqrt()).cos())
           .product::<f64>())
    }
}

pub struct Schwefel<const LEN: usize>;

impl<const LEN: usize> Schwefel<LEN> {
    pub fn new() -> Self {
        Self
    }
}

impl<const LEN: usize> FitnessFunction for Schwefel<LEN> {
    type In = SVector<f64, LEN>;
    type Out = f64;
    type Err = Infallible;

    fn fit(self: &Self, inp: &Self::In) -> Result<Self::Out, Self::Err> {
        Ok(-inp
            .map(|x| x * x.abs().sqrt().sin())
            .sum())
    }
}

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

    use crate::{fitness::{real::{Linear, Rastrigin, Schwefel, Step}, FitnessFunction}, test_infra::load_test_file};

    use super::Griewank;

    #[test]
    fn test_linear_1() {
        const MAX_LEN: usize = 10;

        let data = load_test_file::<f64, f64>("tests/linear_1.txt");
        let offset = SVector::repeat(1.0);
        let linear = Linear::new(1.0, offset);

        for test in data {
            let filled = test.inp.iter()
                .chain(vec![0f64; MAX_LEN - test.inp.len()].iter())
                .map(|x| *x)
                .collect::<Vec<_>>();
            let inp = SVector::<f64, MAX_LEN>::from_vec(filled);

            assert_eq!(
                linear.fit(&inp).unwrap(),
                test.out
            );
        }
    }

    #[test]
    fn test_linear_2() {
        const MAX_LEN: usize = 10;

        let data = load_test_file::<f64, f64>("tests/linear_2.txt");
        let offset = SVector::from_vec((2..=11).map(|x| x as f64).collect());
        let linear = Linear::new(1.0, offset);

        for test in data {
            let filled = test.inp.iter()
                .chain(vec![0f64; MAX_LEN - test.inp.len()].iter())
                .map(|x| *x)
                .collect::<Vec<_>>();
            let inp = SVector::<f64, MAX_LEN>::from_vec(filled);

            assert_eq!(
                linear.fit(&inp).unwrap(),
                test.out
            );
        }
    }

    #[test]
    fn test_step_1() {
        const MAX_LEN: usize = 10;

        let data = load_test_file::<f64, f64>("tests/step_1.txt");
        let offset = SVector::repeat(1.0);
        let linear = Step::new(1.0, offset);

        for test in data {
            let filled = test.inp.iter()
                .chain(vec![0f64; MAX_LEN - test.inp.len()].iter())
                .map(|x| *x)
                .collect::<Vec<_>>();
            let inp = SVector::<f64, MAX_LEN>::from_vec(filled);

            assert_eq!(
                linear.fit(&inp).unwrap(),
                test.out
            );
        }
    }

    #[test]
    fn test_step_2() {
        const MAX_LEN: usize = 10;

        let data = load_test_file::<f64, f64>("tests/step_2.txt");
        let offset = SVector::from_vec((2..=11).map(|x| x as f64).collect());
        let linear = Step::new(1.0, offset);

        for test in data {
            let filled = test.inp.iter()
                .chain(vec![0f64; MAX_LEN - test.inp.len()].iter())
                .map(|x| *x)
                .collect::<Vec<_>>();
            let inp = SVector::<f64, MAX_LEN>::from_vec(filled);

            assert_eq!(
                linear.fit(&inp).unwrap(),
                test.out
            );
        }
    }

    #[test]
    fn test_rastrigin() {
        let data = load_test_file::<f64, f64>("tests/rastrigin.txt");
        let linear = Rastrigin::<Dyn>::new();

        for test in data {
            let inp = OVector::<f64, Dyn>::from_vec(test.inp);

            assert_eq!(
                linear.fit(&inp).unwrap(),
                test.out
            );
        }
    }

    #[test]
    fn test_griewank() {
        let data = load_test_file::<f64, f64>("tests/griewank.txt");
        let griewank = Griewank::<Dyn>::new();

        for test in data {
            let inp = OVector::<f64, Dyn>::from_vec(test.inp);

            assert_eq!(
                griewank.fit(&inp).unwrap(),
                test.out
            );
        }
    }

    #[test]
    fn test_schwefel() {
        const MAX_LEN: usize = 10;

        let data = load_test_file::<f64, f64>("tests/schwefel.txt");
        let schwefel = Schwefel::new();

        for test in data {
            let filled = test.inp.iter()
                .chain(vec![0f64; MAX_LEN - test.inp.len()].iter())
                .map(|x| *x)
                .collect::<Vec<_>>();
            let inp = SVector::<f64, MAX_LEN>::from_vec(filled);

            assert_eq!(
                schwefel.fit(&inp).unwrap(),
                test.out
            );
        }
    }
}