use std::{cmp::Ordering, convert::Infallible, error::Error, marker::PhantomData};

use eoa_lib::{binary_string::BinaryString, crossover::Crossover, fitness::FitnessFunction, initializer::Initializer, perturbation::PerturbationOperator, replacement::Population};
use itertools::Itertools;
use nalgebra::{allocator::Allocator, distance, Const, DefaultAllocator, Dim, Dyn, OMatrix, OVector, Point, U1};
use plotters::prelude::*;
use rand::{seq::{IteratorRandom, SliceRandom}, Rng, RngCore};
use thiserror::Error;

use crate::graph::Edge;

#[derive(PartialEq, Clone, Debug)]
pub struct TSPCity {
    point: Point<f64, 2>
}

#[derive(PartialEq, Clone, Debug)]
pub struct NodePermutation<D: Dim>
where
    DefaultAllocator: Allocator<D>
{
    permutation: OVector<usize, D>
}

/// An instance of TSP, a fully connected graph
/// with cities that connect to each other.
/// The D parameter represents the number of cities.
#[derive(PartialEq, Clone, Debug)]
pub struct TSPInstance<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>
{
    cities: Vec<TSPCity>,
    distances: OMatrix<f64, D, D>
}

impl TSPInstance<Dyn>
where
{
    pub fn new_dyn(cities: Vec<(f64, f64)>) -> Self {
        let dim = Dyn(cities.len());

        let cities = OMatrix::<f64, Dyn, Const<2>>::from_fn_generic(dim, Const::<2>, |i, j| if j == 0 { cities[i].0 } else { cities[i].1 });
        TSPInstance::new(cities)
    }
}

impl<const D: usize> TSPInstance<Const<D>>
where
{
    pub fn new_const(cities: Vec<(f64, f64)>) -> Self {
        let cities = OMatrix::<f64, Const<D>, Const<2>>::from_fn(|i, j| if j == 0 { cities[i].0 } else { cities[i].1 });
        TSPInstance::new(cities)
    }
}

impl<D> TSPInstance<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
    DefaultAllocator: Allocator<D>,
    DefaultAllocator: Allocator<D, Const<2>>,
{
    pub fn new(cities: OMatrix<f64, D, Const<2>>) -> Self {
        let dim = cities.shape_generic().0;

        let cities = cities.row_iter()
                .map(|position|
                     TSPCity { point: Point::<f64, 2>::new(position[0], position[1])  }
                )
                .collect::<Vec<_>>();

        let distances = OMatrix::from_fn_generic(
            dim,
            dim,
            |i, j| distance(&cities[i].point, &cities[j].point)
        );

        Self {
            cities,
            distances
        }
    }
}

impl<D> TSPInstance<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
    DefaultAllocator: Allocator<D>,
{
    pub fn dimension(&self) -> D {
        self.distances.shape_generic().0
    }

    pub fn verify_solution(solution: &NodePermutation<D>) -> bool {
        let mut seen_vertices = OVector::from_element_generic(
            solution.permutation.shape_generic().0,
            solution.permutation.shape_generic().1,
            false
        );

        for &vertex in solution.permutation.iter() {
            // This vertex index is out of bounds
            if vertex >= solution.permutation.len() {
                return false;
            }

            // A node is repeating
            if seen_vertices[vertex] {
                return false;
            }

            seen_vertices[vertex] = true;
        }

        true
    }

    pub fn solution_cost(&self, solution: &NodePermutation<D>) -> f64 {
        solution.permutation
            .iter()
            .circular_tuple_windows()
            .map(|(&node1, &node2): (&usize, &usize)| self.distances(node1, node2))
            .sum()
    }

    pub fn distances(&self, city_a: usize, city_b: usize) -> f64 {
        self.distances[(city_a, city_b)]
    }

    fn plot_internal(&self, solution: Option<&NodePermutation<D>>, filename: &str) -> Result<(), Box<dyn std::error::Error>> {
        let root = BitMapBackend::new(filename, (800, 600)).into_drawing_area();
        root.fill(&WHITE)?;

        let x_coords: Vec<f64> = self.cities.iter().map(|city| city.point.x).collect();
        let y_coords: Vec<f64> = self.cities.iter().map(|city| city.point.y).collect();

        let x_min = x_coords.iter().fold(f64::INFINITY, |a, &b| a.min(b));
        let x_max = x_coords.iter().fold(f64::NEG_INFINITY, |a, &b| a.max(b));
        let y_min = y_coords.iter().fold(f64::INFINITY, |a, &b| a.min(b));
        let y_max = y_coords.iter().fold(f64::NEG_INFINITY, |a, &b| a.max(b));

        let x_padding = (x_max - x_min) * 0.1;
        let y_padding = (y_max - y_min) * 0.1;

        let x_range = (x_min - x_padding)..(x_max + x_padding);
        let y_range = (y_min - y_padding)..(y_max + y_padding);

        let title = if let Some(sol) = solution {
            format!("TSP Solution (Cost: {:.2})", self.solution_cost(sol))
        } else {
            "TSP Instance".to_string()
        };

        let mut chart = ChartBuilder::on(&root)
            .caption(&title, ("sans-serif", 40))
            .margin(10)
            .x_label_area_size(40)
            .y_label_area_size(40)
            .build_cartesian_2d(x_range, y_range)?;

        chart.configure_mesh().draw()?;

        if let Some(sol) = solution {
            chart.draw_series(
                sol.permutation.iter().circular_tuple_windows().map(|(&city1_idx, &city2_idx)| {
                    let city1 = &self.cities[city1_idx];
                    let city2 = &self.cities[city2_idx];
                    PathElement::new(vec![(city1.point.x, city1.point.y), (city2.point.x, city2.point.y)], BLUE)
                })
            )?;
        }

        chart.draw_series(
            self.cities.iter().map(|city| {
                Circle::new((city.point.x, city.point.y), 5, RED.filled())
            })
        )?;

        chart.draw_series(
            self.cities.iter().enumerate().map(|(i, city)| {
                Text::new(format!("{}", i), (city.point.x + 0.1, city.point.y + 0.1), ("sans-serif", 15))
            })
        )?;

        root.present()?;
        Ok(())
    }

    pub fn plot(&self, filename: &str) -> Result<(), Box<dyn std::error::Error>> {
        self.plot_internal(None, filename)
    }

    pub fn draw_solution(&self, solution: &NodePermutation<D>, filename: &str) -> Result<(), Box<dyn std::error::Error>> {
        self.plot_internal(Some(solution), filename)
    }
}

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

    fn fit(self: &Self, inp: &Self::In) -> Result<Self::Out, Self::Err> {
        Ok(self.solution_cost(inp))
    }
}

pub struct TSPRandomInitializer<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
{
    _phantom: PhantomData<D>
}

impl<D> TSPRandomInitializer<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
{
    pub fn new() -> Self {
        Self { _phantom: PhantomData }
    }
}

impl<D> Initializer<D, NodePermutation<D>> for TSPRandomInitializer<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
    DefaultAllocator: Allocator<D>,
{
    fn initialize_single(&self, size: D, rng: &mut dyn RngCore) -> NodePermutation<D> {
        let len = size.value();
        let mut indices = OVector::<usize, D>::from_iterator_generic(size, U1, 0..len);
        indices.as_mut_slice().shuffle(rng);

        NodePermutation { permutation: indices }
    }
}

pub struct SwapPerturbation<D> {
    _phantom: PhantomData<D>
}

impl<D> SwapPerturbation<D> {
    pub fn new() -> Self {
        Self { _phantom: PhantomData }
    }
}

impl<D> PerturbationOperator for SwapPerturbation<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
    DefaultAllocator: Allocator<D>,
{
    type Chromosome = NodePermutation<D>;

    fn perturb(&self, chromosome: &mut Self::Chromosome, rng: &mut dyn RngCore) {
        let first = rng.random_range(0..chromosome.permutation.len());
        let second = rng.random_range(0..chromosome.permutation.len());
        chromosome.permutation.swap_rows(first, second);
    }
}

pub struct ReverseSubsequencePerturbation<D> {
    _phantom: PhantomData<D>
}

impl<D> ReverseSubsequencePerturbation<D> {
    pub fn new() -> Self {
        Self { _phantom: PhantomData }
    }
}

impl<D> PerturbationOperator for ReverseSubsequencePerturbation<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
    DefaultAllocator: Allocator<D>,
{
    type Chromosome = NodePermutation<D>;

    fn perturb(&self, chromosome: &mut Self::Chromosome, rng: &mut dyn RngCore) {
        let first = rng.random_range(0..chromosome.permutation.len());
        let second = rng.random_range(0..chromosome.permutation.len());

        let start = first.min(second);
        let end = first.max(second);

        // Reverse the subsequence between start and end (inclusive)
        let mut left = start;
        let mut right = end;

        while left < right {
            chromosome.permutation.swap_rows(left, right);
            left += 1;
            right -= 1;
        }
    }
}

pub struct EdgeRecombinationCrossover<D> {
    _phantom: PhantomData<D>
}

impl<D> EdgeRecombinationCrossover<D> {
    pub fn new() -> Self {
        Self { _phantom: PhantomData }
    }
}

impl<D> Crossover<2> for EdgeRecombinationCrossover<D>
where
    D: Dim,
    DefaultAllocator: Allocator<D, D>,
    DefaultAllocator: Allocator<D>,
    DefaultAllocator: nalgebra::allocator::Allocator<D, Const<4>>
{
    type Chromosome = NodePermutation<D>;
    type Out = f64;

    fn crossover(
        &self,
        parents: &eoa_lib::replacement::EvaluatedPopulation<Self::Chromosome, Self::Out>,
        pairs: impl Iterator<Item = eoa_lib::pairing::ParentPairing<2>>,
        rng: &mut dyn RngCore
    ) -> eoa_lib::replacement::Population<Self::Chromosome> {
        let mut offsprings = vec![];

        let permutation = &parents.population[0].chromosome.permutation;
        let len = permutation.len();
        let mut adjacency_lists = OMatrix::from_element_generic(
            permutation.shape_generic().0,
            Const::<4>,
            None);
        let mut used_nodes = OVector::from_element_generic(
            permutation.shape_generic().0,
            Const::<1>,
            false
        );

        let mut neighbors_count = OVector::from_element_generic(
            permutation.shape_generic().0,
            Const::<1>,
            2usize
        );

        for pair in pairs {
            let parent1 = &parents.population[pair.x].chromosome;
            let parent2 = &parents.population[pair.y].chromosome;

            used_nodes.apply(|n| *n = false);

            // 1. Populate adjacency lists
            for (&c1, &n, &c2) in parent1.permutation.iter().circular_tuple_windows() {
                adjacency_lists[(n, 0)] = Some(c1);
                adjacency_lists[(n, 1)] = Some(c2);
                neighbors_count[n] = 2;
            }

            for (&c1, &n, &c2) in parent2.permutation.iter().circular_tuple_windows() {
                // Not duplicit?
                if adjacency_lists[(n, 0)].unwrap() != c1 && adjacency_lists[(n, 1)].unwrap() != c1 {
                    neighbors_count[n] += 1;
                    adjacency_lists[(n, 2)] = Some(c1);
                } else { // Duplicit
                    adjacency_lists[(n, 2)] = None;
                }

                // Not duplicit
                if adjacency_lists[(n, 0)].unwrap() != c2 && adjacency_lists[(n, 1)].unwrap() != c2 {
                    neighbors_count[n] += 1;
                    adjacency_lists[(n, 3)] = Some(c2);
                } else { // Duplicit
                    adjacency_lists[(n, 3)] = None;
                }
            }

            let chosen_parent = if rng.random_bool(0.5) {
                &parent1
            } else {
                &parent2
            };

            let mut offspring = OVector::from_element_generic(permutation.shape_generic().0, Const::<1>, 0);

            let mut current_node = chosen_parent.permutation[0];

            for i in 0..len-1 {
                offspring[i] = current_node;
                used_nodes[current_node] = true;

                for neighbor in adjacency_lists.row(current_node) {
                    if let Some(neighbor) = neighbor {
                        neighbors_count[*neighbor] -= 1;
                    }
                }

                let min_neighbors = adjacency_lists.row(current_node)
                    .iter()
                    .flatten()
                    .filter(|&&neighbor| !used_nodes[neighbor])
                    .map(|&neighbor| neighbors_count[neighbor])
                    .min();

                let neighbor = if let Some(min_neighbors) = min_neighbors {
                    adjacency_lists.row(current_node)
                        .iter()
                        .flatten()
                        .copied()
                        .filter(|&neighbor| !used_nodes[neighbor] && neighbors_count[neighbor] == min_neighbors)
                        .choose(rng)
                } else {
                    None
                };

                current_node = if let Some(neighbor) = neighbor {
                    neighbor
                } else {
                    (0..len).filter(|&node| !used_nodes[node])
                    .choose(rng)
                    .unwrap()
                };
            }

            offspring[len - 1] = current_node;

            offsprings.push(NodePermutation { permutation: offspring });
        }

        Population::from_vec(offsprings)
    }
}

pub struct BinaryStringToNodePermutation<DIn: Dim, DOut: Dim> {
    dim_in: DIn,
    dim_out: DOut,
}

impl<DIn: Dim, DOut: Dim> BinaryStringToNodePermutation<DIn, DOut> {
    pub fn new(dim_in: DIn, dim_out: DOut) -> Self {
        Self {
            dim_in,
            dim_out
        }
    }
}

#[derive(Error, Debug)]
pub enum DimensionMismatch {
    #[error("The input dimension should be equal to half matrix NxN where the output is N")]
    Mismatch
}

impl<DIn: Dim, DOut: Dim> FitnessFunction for BinaryStringToNodePermutation<DIn, DOut>
where
    DefaultAllocator: Allocator<DIn>,
    DefaultAllocator: Allocator<DOut>,
    DefaultAllocator: Allocator<DOut, DOut>,
{
    type In = BinaryString<DIn>;
    type Out = NodePermutation<DOut>;
    type Err = DimensionMismatch;

    fn fit(self: &Self, inp: &Self::In) -> Result<Self::Out, Self::Err> {
        let nodes = self.dim_out.value();
        let mut orderings =
            OMatrix::<Ordering, DOut, DOut>::from_element_generic(self.dim_out, self.dim_out, Ordering::Equal);

        let mut in_index = 0usize;
        for i in 0..self.dim_out.value() {
            for j in i+1..self.dim_out.value() {
                orderings[(i, j)] = if inp.vec[in_index] == 1 {
                    Ordering::Greater
                } else {
                    Ordering::Less
                };

                orderings[(j, i)] = if inp.vec[in_index] == 1 {
                    Ordering::Less
                } else {
                    Ordering::Greater
                };

                in_index += 1;
            }
        }

        let mut result =
            OVector::from_iterator_generic(self.dim_out, U1, 0..nodes);

        for i in 0..nodes {
            for j in i+1..nodes {
                let node1 = result[i];
                let node2 = result[j];

                if orderings[(node1, node2)] == Ordering::Greater {
                    result.swap_rows(i, j);
                }
            }
        }

        Ok(NodePermutation { permutation: result })
    }
}

#[cfg(test)]
mod tests {
    use std::convert::Infallible;

    use eoa_lib::{binary_string::BinaryString, crossover::Crossover, fitness::FitnessFunction, initializer::Initializer, pairing::{AdjacentPairing, Pairing}, replacement::Population};
    use nalgebra::{Const, SVector, U15, U6};
    use rand::{rngs::StdRng, RngCore, SeedableRng};

    use crate::tsp::TSPInstance;

    use super::{BinaryStringToNodePermutation, EdgeRecombinationCrossover, NodePermutation, ReverseSubsequencePerturbation, TSPRandomInitializer};
    use eoa_lib::perturbation::PerturbationOperator;

    struct MockRng;
    impl RngCore for MockRng {
        fn next_u32(&mut self) -> u32 {
            0
        }

        fn next_u64(&mut self) -> u64 {
            0
        }

        fn fill_bytes(&mut self, _: &mut [u8]) {
            panic!()
        }
    }

    struct ZeroFitness<const LEN: usize>;
    impl<const LEN: usize> FitnessFunction for ZeroFitness<LEN> {
        type In = NodePermutation<Const<LEN>>;
        type Out = f64;
        type Err = Infallible;

        fn fit(self: &Self, _: &Self::In) -> Result<Self::Out, Self::Err> {
            Ok(0.0)
        }
    }

    #[test]
    fn test_binary_string_representation() {
        // x 0 1 2 3 4 5
        // 0 0 0 0 0 0 0
        // 1 1 0 0 0 0 0
        // 2 1 1 0 0 0 0
        // 3 1 1 1 0 0 0
        // 4 1 1 1 1 0 0
        // 5 1 1 1 1 1 0

        // x 0 1 2 3 4 5
        // 0   0 0 0 0 0
        // 1     0 0 0 0
        // 2       0 0 0
        // 3         0 0
        // 4           0
        // 5

        // 6 nodes
        // length of binary string: 5 + 4 + 3 + 2 + 1 = 15

        let converter = BinaryStringToNodePermutation::new(
            U15,
            U6
        );

        let binary_string_ordering = BinaryString::<U15>::new(vec![1; 15]);

        let mut expected_permutation = vec![0, 1, 2, 3, 4, 5];
        expected_permutation.reverse();

        let mut permutation = converter.fit(&binary_string_ordering)
            .unwrap();

        assert_eq!(
            expected_permutation,
            permutation.permutation.as_mut_slice().to_vec()
        );

        let binary_string_ordering = BinaryString::<U15>::new(vec![0; 15]);
        expected_permutation.reverse();

        let mut permutation = converter.fit(&binary_string_ordering)
            .unwrap();

        assert_eq!(
            expected_permutation,
            permutation.permutation.as_mut_slice().to_vec()
        )
    }
    #[test]
    fn test_nontrivial_binary_string_representation() {
        // x 0 1 2 3 4 5
        // 0 0 1 0 0 0 0
        // 1 0 0 0 0 0 0
        // 2 1 1 0 0 0 1
        // 3 1 1 1 0 0 0
        // 4 1 1 1 1 0 0
        // 5 1 1 0 1 1 0

        // x 0 1 2 3 4 5
        // 0   0 0 0 0 0
        // 1     0 0 0 0
        // 2       1 1 1
        // 3         0 0
        // 4           1
        // 5

        // 6 nodes
        // length of binary string: 5 + 4 + 3 + 2 + 1 = 15

        let converter = BinaryStringToNodePermutation::new(
            U15,
            U6
        );

        let mut binary_string_ordering = BinaryString::<U15>::new(vec![0; 15]);
        binary_string_ordering.vec[9] = 1;
        binary_string_ordering.vec[10] = 1;
        binary_string_ordering.vec[11] = 1;
        binary_string_ordering.vec[14] = 1;

        let expected_permutation = vec![0, 1, 3, 5, 4, 2];

        let mut permutation = converter.fit(&binary_string_ordering)
            .unwrap();

        assert_eq!(
            expected_permutation,
            permutation.permutation.as_mut_slice().to_vec()
        );
    }

    #[test]
    fn test_edge_recombination_properties() {
        let crossover = EdgeRecombinationCrossover::<Const<10>>::new();
        let initializer = TSPRandomInitializer::<Const<10>>::new();
        let adjacency_pairing = AdjacentPairing::new();

        let mut rng = StdRng::seed_from_u64(0);
        for _ in 0..100 {
            let parents = Population::from_vec(initializer.initialize(Const::<10>, 10, &mut rng));
            let parents = parents.evaluate(&ZeroFitness).unwrap();

            let pairs = adjacency_pairing.pair(&parents, 0..10);
            let result = crossover.crossover(&parents, pairs, &mut rng);

            // Test invariants that should always hold:
            for chromosome in result.into_iter() {
                assert!(TSPInstance::verify_solution(&chromosome));
            }
        }
    }

    #[test]
    fn test_edge_recombination_specific_case() {
        let parent1: Vec<usize> = vec![0, 1, 2, 4, 5, 3];
        let parent2: Vec<usize> = vec![2, 0, 1, 3, 4, 5];

        let parent1 = NodePermutation::<U6> { permutation: SVector::<usize, 6>::from_vec(parent1) };
        let parent2 = NodePermutation::<U6> { permutation: SVector::<usize, 6>::from_vec(parent2) };

        let pairing = SVector::<usize, 2>::new(0, 1);
        let pairings = vec![pairing].into_iter();

        let parents = Population::from_vec(vec![parent1, parent2]).evaluate(&ZeroFitness).unwrap();

        let crossover = EdgeRecombinationCrossover::<U6>::new();

        let offsprings = crossover.crossover(&parents, pairings, &mut MockRng);
        let offspring = offsprings.into_iter().next().unwrap();

        // NOTE: this sort of relies on the implementation of the algorithm (when there are multiple possibilities
        // currently the algorithm always chooses last). It's possible this test will break due to valid changes to the algorithm.
        assert_eq!(vec![0usize, 1, 3, 4, 5, 2], offspring.permutation.into_iter().copied().collect::<Vec<_>>())
    }


    #[test]
    fn test_reverse_subsequence_perturbation_behavior() {
        let perturbation = ReverseSubsequencePerturbation::<Const<6>>::new();

        // Test multiple specific seeds to get predictable behavior
        // We'll try different seeds until we find ones that give us the patterns we want to test

        // Test case 1: Try to find a seed that reverses a middle subsequence
        let mut found_middle_reverse = false;
        for seed in 0..1000 {
            let mut rng = StdRng::seed_from_u64(seed);
            let mut chromosome = NodePermutation::<Const<6>> {
                permutation: SVector::<usize, 6>::from_vec(vec![0, 1, 2, 3, 4, 5])
            };
            let original = chromosome.clone();

            perturbation.perturb(&mut chromosome, &mut rng);

            // Check if it's a valid reverse pattern and not the whole array or single element
            let result: Vec<usize> = chromosome.permutation.into_iter().copied().collect();
            if result != vec![0, 1, 2, 3, 4, 5] && // Changed
               result != vec![5, 4, 3, 2, 1, 0] && // Not whole array reverse
               TSPInstance::verify_solution(&chromosome) {
                found_middle_reverse = true;
                break;
            }
        }
        assert!(found_middle_reverse, "Should find at least one case of partial subsequence reversal");
    }

    #[test]
    fn test_reverse_subsequence_perturbation_deterministic_seed() {
        let perturbation = ReverseSubsequencePerturbation::<Const<6>>::new();

        // Use a specific seed that we know produces a certain result
        let mut rng1 = StdRng::seed_from_u64(42);
        let mut chromosome1 = NodePermutation::<Const<6>> {
            permutation: SVector::<usize, 6>::from_vec(vec![0, 1, 2, 3, 4, 5])
        };
        perturbation.perturb(&mut chromosome1, &mut rng1);

        // Same seed should produce same result
        let mut rng2 = StdRng::seed_from_u64(42);
        let mut chromosome2 = NodePermutation::<Const<6>> {
            permutation: SVector::<usize, 6>::from_vec(vec![0, 1, 2, 3, 4, 5])
        };
        perturbation.perturb(&mut chromosome2, &mut rng2);

        assert_eq!(chromosome1.permutation, chromosome2.permutation);
        assert!(TSPInstance::verify_solution(&chromosome1));
        assert!(TSPInstance::verify_solution(&chromosome2));
    }

    #[test]
    fn test_reverse_subsequence_perturbation_different_initial_permutations() {
        let perturbation = ReverseSubsequencePerturbation::<Const<5>>::new();

        // Test with a non-sequential initial permutation
        let mut rng = StdRng::seed_from_u64(123);
        let mut chromosome = NodePermutation::<Const<5>> {
            permutation: SVector::<usize, 5>::from_vec(vec![2, 0, 4, 1, 3])
        };
        let original_elements: std::collections::HashSet<usize> =
            chromosome.permutation.iter().copied().collect();

        perturbation.perturb(&mut chromosome, &mut rng);

        // Verify all original elements are still present
        let new_elements: std::collections::HashSet<usize> =
            chromosome.permutation.iter().copied().collect();
        assert_eq!(original_elements, new_elements);

        // Verify it's still a valid permutation
        assert!(TSPInstance::verify_solution(&chromosome));
    }

    #[test]
    fn test_reverse_subsequence_perturbation_edge_cases() {
        let perturbation = ReverseSubsequencePerturbation::<Const<2>>::new();

        // Test with minimum size permutation (2 elements)
        let mut rng = StdRng::seed_from_u64(456);
        let mut chromosome = NodePermutation::<Const<2>> {
            permutation: SVector::<usize, 2>::from_vec(vec![0, 1])
        };

        perturbation.perturb(&mut chromosome, &mut rng);

        let result: Vec<usize> = chromosome.permutation.into_iter().copied().collect();
        // With 2 elements, it should either stay [0,1] or become [1,0]
        assert!(result == vec![0, 1] || result == vec![1, 0]);
        assert!(TSPInstance::verify_solution(&chromosome));
    }

    #[test]
    fn test_reverse_subsequence_perturbation_is_reversible() {
        let perturbation = ReverseSubsequencePerturbation::<Const<6>>::new();

        // Any sequence of reversals should be reversible
        let mut rng = StdRng::seed_from_u64(789);
        let original = NodePermutation::<Const<6>> {
            permutation: SVector::<usize, 6>::from_vec(vec![0, 1, 2, 3, 4, 5])
        };
        let mut chromosome = original.clone();

        // Apply perturbation twice with same seed (reset RNG)
        perturbation.perturb(&mut chromosome, &mut rng);
        let after_first = chromosome.clone();

        // Since we can't easily reverse the exact operation, at least verify
        // that multiple applications maintain the permutation property
        for _ in 0..10 {
            perturbation.perturb(&mut chromosome, &mut rng);
            assert!(TSPInstance::verify_solution(&chromosome));
        }
    }

    #[test]
    fn test_reverse_subsequence_perturbation_preserves_elements() {
        let perturbation = ReverseSubsequencePerturbation::<Const<10>>::new();
        let initializer = TSPRandomInitializer::<Const<10>>::new();

        let mut rng = StdRng::seed_from_u64(42);

        // Test with multiple random permutations
        for _ in 0..50 {
            let mut chromosome = initializer.initialize_single(Const::<10>, &mut rng);
            let original_elements: std::collections::HashSet<usize> = chromosome.permutation.iter().copied().collect();

            perturbation.perturb(&mut chromosome, &mut rng);

            // Verify all elements are still present
            let new_elements: std::collections::HashSet<usize> = chromosome.permutation.iter().copied().collect();
            assert_eq!(original_elements, new_elements);

            // Verify it's still a valid permutation
            assert!(TSPInstance::verify_solution(&chromosome));
        }
    }

    #[test]
    fn test_reverse_subsequence_perturbation_actually_changes_permutation() {
        let perturbation = ReverseSubsequencePerturbation::<Const<8>>::new();
        let mut rng = StdRng::seed_from_u64(12345);

        // Test that the perturbation actually changes the permutation (with high probability)
        let mut changes_detected = 0;
        let total_tests = 100;

        for _ in 0..total_tests {
            let mut chromosome = NodePermutation::<Const<8>> {
                permutation: SVector::<usize, 8>::from_vec(vec![0, 1, 2, 3, 4, 5, 6, 7])
            };
            let original = chromosome.clone();

            perturbation.perturb(&mut chromosome, &mut rng);

            if chromosome.permutation != original.permutation {
                changes_detected += 1;
            }

            // Always verify it's still a valid permutation
            assert!(TSPInstance::verify_solution(&chromosome));
        }

        // We expect at least 85% of random perturbations to actually change the permutation
        // (only fails if start == end randomly, which should be rare)
        assert!(changes_detected >= 85,
            "Expected at least 85 changes out of {} tests, but got {}",
            total_tests, changes_detected);
    }

}