Bug fixes

examples/layout.rs

@@ -1,47 +1,34 @@
+use clap::Parser;
 use factorio_blueprint::{
     belt_finding::{conflict_avoidance::ConflictAvoidance, Problem},
     common::visualize::Visualize,
-    layout::Layout,
+    layout::{GeneticAlgorithm, Layout, PathLayout},
 };
-use rand::SeedableRng;
+use rand::{rngs::SmallRng, SeedableRng};
+
+#[derive(Debug, Parser)]
+struct Args {
+    #[clap(default_value_t = 0)]
+    seed: u64,
+}
 
 fn main() {
-    // let mut p = Problem::new(Position::new(10, 10));
+    let args = Args::parse();
 
-    // let b1 = p.add_block(Position::new(3, 2));
-    // let b2 = p.add_block(Position::new(5, 2));
-    // let b3 = p.add_block(Position::new(5, 7));
-
-    // p.add_connection(b1, Position::new(1, 0), b2, Position::new(1, 0));
-    // p.add_connection(b2, Position::new(3, 1), b3, Position::new(4, 6));
-
-    let file = std::fs::File::open("layout.yml").unwrap();
+    let file = std::fs::File::open("layout2.yml").unwrap();
 
     let p = serde_yaml::from_reader(file).unwrap();
 
-    for i in 0..1 {
-        let mut rng = rand::rngs::SmallRng::seed_from_u64(5);
+    let mut rng = SmallRng::seed_from_u64(args.seed);
 
-        let l = Layout::new(&p, &mut rng);
+    dbg!(&p);
 
-        // let s = l.score();
-        l.print_visualization();
+    let mut g = GeneticAlgorithm::new(&p, 20, 4, 2, &mut rng);
 
-        let mut p = Problem::from_layout(&l);
-        p.print();
-        p.find_path();
-        p.print();
-        let mut c = ConflictAvoidance::new(p);
-
-        c.remove_all_conflicts();
-
-        c.print();
-        // println!("Seed: {i}, Score {}", s);
-
-        // l.print_visualization();
-        // if s < min {
-        //     min = s;
-        //     min_l = Some(l);
-        // }
+    for i in 0..100 {
+        println!("Generatrion {i}");
+        g.generation(&mut rng);
     }
+
+    // g.output_population();
 }

layout2.yml

@@ -0,0 +1,61 @@
+size:
+  x: 30
+  y: 30
+blocks:
+  - size:
+      x: 3
+      y: 2
+    input:
+      - offset:
+          x: 1
+          y: 1
+        dir: Up
+    output:
+      - offset:
+          x: 1
+          y: 0
+        dir: Up
+  - size:
+      x: 5
+      y: 2
+    input:
+    output:
+      - offset:
+          x: 1
+          y: 1
+        dir: Down
+  - size:
+      x: 5
+      y: 7
+    input:
+      - offset:
+          x: 0
+          y: 1
+        dir: Right
+    output:
+  - size:
+      x: 5
+      y: 5
+    input:
+      - offset:
+          x: 0
+          y: 1
+        dir: Right
+    output:
+      - offset:
+          x: 0
+          y: 2
+        dir: Left
+connections:
+  - startblock: 1
+    startpoint: 0
+    endblock: 0
+    endpoint: 0
+  - startblock: 0
+    startpoint: 0
+    endblock: 3
+    endpoint: 0
+  - startblock: 3
+    startpoint: 0
+    endblock: 2
+    endpoint: 0

src/belt_finding/common.rs

@@ -62,6 +62,17 @@ impl PathField {
             },
         }
     }
+
+    pub fn cost(&self) -> usize {
+        match self {
+            PathField::Belt { pos: _, dir: _ } => 300,
+            PathField::Underground {
+                pos: _,
+                dir: _,
+                len: _,
+            } => 1750,
+        }
+    }
 }
 
 pub fn print_map<F>(width: i32, height: i32, f: F) -> io::Result<()>

src/belt_finding/conflict_avoidance.rs

@@ -196,7 +196,7 @@ impl ConflictAvoidance {
                 if let Some(PathField::Underground { pos, dir, len }) = path.get(end_index + 1) {
                     if xrange.contains(&(pos.x as usize)) && yrange.contains(&(pos.y as usize)) {
                         let p = *pos - offset;
-                        println!("Blocked {:?}", p);
+                        // println!("Blocked {:?}", p);
                         if b.get_blocked(p.x as usize, p.y as usize) {
                             return None;
                         }
@@ -247,8 +247,8 @@ impl ConflictAvoidance {
 
         let mut b = b.build();
 
-        b.print();
-        self.print();
+        // b.print();
+        // self.print();
 
         let mut min_cost = f64::INFINITY;
         let mut solutions = Vec::new();
@@ -317,6 +317,8 @@ impl ConflictAvoidance {
         //     println!();
         // }
 
+        // self.print();
+
         let mut candidates = Vec::new();
 
         for y in 0..self.map.height {
@@ -337,10 +339,17 @@ impl ConflictAvoidance {
         if candidates.is_empty() {
             return false;
         }
+        // dbg!(&candidates);
 
         loop {
             candidates.sort_by_key(|c| -c.area());
-            let c = candidates.pop().unwrap();
+            // dbg!(&candidates);
+            let c = match candidates.pop() {
+                Some(c) => c,
+                None => {
+                    return false;
+                }
+            };
 
             self.range = Some((c.min.x..=c.max.x, c.min.y..=c.max.y));
 
@@ -425,12 +434,27 @@ impl ConflictAvoidance {
             if candidate != c && !candidates.iter().any(|c| c == &candidate) {
                 candidates.push(candidate);
             }
-            // dbg!(&candidates);
         }
     }
 
-    pub fn remove_all_conflicts(&mut self) {
+    pub fn remove_all_conflicts(&mut self) -> bool {
         while self.remove_conflict() {}
+
+        let mut conflicts: Map<bool> = Map::new(self.map.width, self.map.height);
+
+        for x in 0..self.map.width {
+            for y in 0..self.map.height {
+                if self.map.get(x, y).blocked {
+                    if conflicts.get(x, y) == &true {
+                        return false;
+                    } else {
+                        conflicts.set(x, y, true);
+                    }
+                }
+            }
+        }
+
+        true
     }
 
     pub fn print(&self) {

src/belt_finding/mod.rs

@@ -2,6 +2,7 @@ use crate::common::color::COLORS;
 use crate::graph::wheighted_graph::WheightedGraph;
 use crate::layout::Layout;
 use crate::misc::Map;
+use crate::priority_queue::{BinaryHeap, Trace};
 use crate::{
     graph::wheighted_graph::shortest_path::dijkstra, priority_queue::fibonacci_heap::FibonacciHeap,
 };
@@ -225,18 +226,21 @@ impl<'a> WheightedGraph for MapInternal<'a> {
 }
 
 impl Problem {
-    pub fn find_path(&mut self) {
+    pub fn find_path(&mut self) -> bool {
         for i in 0..self.start.len() {
             self.calculate_wheights(i);
             let m = MapInternal {
                 map: &self.map,
                 end: self.end[i],
             };
-            let p = dijkstra::<MapInternal, FibonacciHeap<_>>(&m, self.start[i], self.end[i]);
+            let p = dijkstra::<MapInternal, BinaryHeap<_>>(&m, self.start[i], self.end[i]);
             if let Some(p) = p {
                 self.path[i] = p;
+            } else {
+                return false;
             }
         }
+        true
     }
 }
 

src/common/visualize.rs

@@ -1,181 +0,0 @@
-use std::collections::HashMap;
-
-use std::io::Write;
-use termcolor::{ColorSpec, StandardStream, WriteColor};
-
-use crate::prelude::*;
-
-pub trait Visualize {
-    fn visualize(&self) -> Visualization;
-
-    fn print_visualization(&self) {
-        let v = self.visualize();
-
-        let stdout = &mut StandardStream::stdout(termcolor::ColorChoice::Always);
-
-        let width_digits = (v.size.x - 1).ilog10() + 1;
-        let height_digits = (v.size.y - 1).ilog10() + 1;
-
-        // print header
-        for i in 0..width_digits {
-            let d = width_digits - i - 1;
-
-            //padding
-            for _ in 0..height_digits {
-                write!(stdout, " ").unwrap();
-            }
-
-            for x in 0..v.size.x {
-                let digits = x / (i32::pow(10, d));
-                if digits == 0 && d > 0 {
-                    write!(stdout, " ").unwrap();
-                } else {
-                    write!(
-                        stdout,
-                        "{}",
-                        char::from_u32((digits % 10) as u32 + 48).unwrap()
-                    )
-                    .unwrap();
-                }
-            }
-            writeln!(stdout).unwrap();
-        }
-
-        for y in 0..v.size.y {
-            write!(stdout, "{:1$}", y, height_digits as usize).unwrap();
-
-            for x in 0..v.size.x {
-                if let Some((s, fg, bg)) = v.symbols.get(&Position::new(x, y)) {
-                    let mut c = ColorSpec::new();
-                    c.set_fg(fg.as_ref().map(|c| termcolor::Color::Rgb(c.r, c.g, c.b)));
-                    c.set_bg(bg.as_ref().map(|c| termcolor::Color::Rgb(c.r, c.g, c.b)));
-                    stdout.set_color(&c).unwrap();
-                    write!(stdout, "{:1}", s.get_str()).unwrap();
-                    stdout.reset().unwrap();
-                } else {
-                    write!(stdout, " ").unwrap();
-                }
-            }
-
-            writeln!(stdout, "{:1$}", y, height_digits as usize).unwrap();
-        }
-
-        for i in 0..width_digits {
-            let d = width_digits - i - 1;
-
-            //padding
-            for _ in 0..height_digits {
-                write!(stdout, " ").unwrap();
-            }
-
-            for x in 0..v.size.x {
-                let digits = x / (i32::pow(10, d));
-                if digits == 0 && d > 0 {
-                    write!(stdout, " ").unwrap();
-                } else {
-                    write!(
-                        stdout,
-                        "{}",
-                        char::from_u32((digits % 10) as u32 + 48).unwrap()
-                    )
-                    .unwrap();
-                }
-            }
-            writeln!(stdout).unwrap();
-        }
-    }
-}
-
-pub enum Symbol {
-    Arrow(Direction),
-    ArrowEnter(Direction),
-    ArrowExit(Direction),
-    Char(&'static str),
-    Block,
-    Space,
-}
-
-impl Symbol {
-    fn get_str(&self) -> &'static str {
-        match self {
-            Symbol::Arrow(dir) => match dir {
-                Direction::Up => "↑",
-                Direction::Right => "→",
-                Direction::Down => "↓",
-                Direction::Left => "←",
-            },
-            Symbol::ArrowEnter(dir) => match dir {
-                Direction::Up => "↟",
-                Direction::Right => "↠",
-                Direction::Down => "↡",
-                Direction::Left => "↞",
-            },
-            Symbol::ArrowExit(dir) => match dir {
-                Direction::Up => "↥",
-                Direction::Right => "↦",
-                Direction::Down => "↧",
-                Direction::Left => "↤",
-            },
-            Symbol::Char(c) => c.split_at(1).0,
-            Symbol::Block => "#",
-            Symbol::Space => " ",
-        }
-    }
-
-    fn get_char(&self) -> char {
-        self.get_str().chars().next().unwrap()
-    }
-}
-
-pub struct Visualization {
-    size: Position,
-    symbols: HashMap<Position, (Symbol, Option<Color>, Option<Color>)>,
-}
-
-#[derive(Debug, Clone, Copy)]
-pub struct Color {
-    r: u8,
-    g: u8,
-    b: u8,
-}
-
-impl Color {
-    pub fn new(r: u8, g: u8, b: u8) -> Self {
-        Self { r, g, b }
-    }
-
-    pub fn index(i: usize) -> Self {
-        let c = [
-            Color::new(0xe6, 0x00, 0x49),
-            Color::new(0x0b, 0xb4, 0xff),
-            Color::new(0x50, 0xe9, 0x91),
-            Color::new(0xe6, 0xd8, 0x00),
-            Color::new(0x9b, 0x19, 0xf5),
-            Color::new(0xff, 0xa3, 0x00),
-            Color::new(0xdc, 0x0a, 0xb4),
-            Color::new(0xb3, 0xd4, 0xff),
-            Color::new(0x00, 0xbf, 0xa0),
-        ];
-
-        c[i % c.len()]
-    }
-}
-
-impl Visualization {
-    pub fn new(size: Position) -> Self {
-        Self {
-            size,
-            symbols: HashMap::new(),
-        }
-    }
-
-    pub fn add_symbol(
-        &mut self,
-        pos: Position,
-        symbol: Symbol,
-        fg: Option<Color>,
-        bg: Option<Color>,
-    ) {
-        self.symbols.insert(pos, (symbol, fg, bg));
-    }
-}

src/common/visualize/mod.rs

@@ -0,0 +1,108 @@
+mod print;
+
+use std::collections::HashMap;
+
+use crate::prelude::*;
+
+pub trait Visualize {
+    fn visualize(&self) -> Visualization;
+
+    fn print_visualization(&self) {
+        let v = self.visualize();
+        print::print(v);
+    }
+}
+
+pub enum Symbol {
+    Arrow(Direction),
+    ArrowEnter(Direction),
+    ArrowExit(Direction),
+    Char(&'static str),
+    Block,
+    Space,
+}
+
+impl Symbol {
+    fn get_str(&self) -> &'static str {
+        match self {
+            Symbol::Arrow(dir) => match dir {
+                Direction::Up => "↑",
+                Direction::Right => "→",
+                Direction::Down => "↓",
+                Direction::Left => "←",
+            },
+            Symbol::ArrowEnter(dir) => match dir {
+                Direction::Up => "↟",
+                Direction::Right => "↠",
+                Direction::Down => "↡",
+                Direction::Left => "↞",
+            },
+            Symbol::ArrowExit(dir) => match dir {
+                Direction::Up => "↥",
+                Direction::Right => "↦",
+                Direction::Down => "↧",
+                Direction::Left => "↤",
+            },
+            Symbol::Char(c) => c.split_at(1).0,
+            Symbol::Block => "#",
+            Symbol::Space => " ",
+        }
+    }
+
+    // fn get_char(&self) -> char {
+    //     self.get_str().chars().next().unwrap()
+    // }
+}
+
+pub struct Visualization {
+    size: Position,
+    symbols: HashMap<Position, (Symbol, Option<Color>, Option<Color>)>,
+}
+
+#[derive(Debug, Clone, Copy)]
+pub struct Color {
+    r: u8,
+    g: u8,
+    b: u8,
+}
+
+impl Color {
+    pub fn new(r: u8, g: u8, b: u8) -> Self {
+        Self { r, g, b }
+    }
+
+    pub fn index(i: usize) -> Self {
+        let c = [
+            Color::new(0xe6, 0x00, 0x49),
+            Color::new(0x0b, 0xb4, 0xff),
+            Color::new(0x50, 0xe9, 0x91),
+            Color::new(0xe6, 0xd8, 0x00),
+            Color::new(0x9b, 0x19, 0xf5),
+            Color::new(0xff, 0xa3, 0x00),
+            Color::new(0xdc, 0x0a, 0xb4),
+            Color::new(0xb3, 0xd4, 0xff),
+            Color::new(0x00, 0xbf, 0xa0),
+        ];
+
+        c[i % c.len()]
+    }
+}
+
+impl Visualization {
+    pub fn new(size: Position) -> Self {
+        Self {
+            size,
+            symbols: HashMap::new(),
+        }
+    }
+
+    pub fn add_symbol(
+        &mut self,
+        pos: Position,
+        symbol: Symbol,
+        fg: Option<Color>,
+        bg: Option<Color>,
+    ) {
+        self.symbols.insert(pos, (symbol, fg, bg));
+    }
+}

src/common/visualize/print.rs

@@ -0,0 +1,81 @@
+use super::Visualization;
+
+use std::io::Write;
+use termcolor::{ColorSpec, StandardStream, WriteColor};
+
+use crate::prelude::*;
+
+pub(super) fn print(v: Visualization) {
+    let stdout = &mut StandardStream::stdout(termcolor::ColorChoice::Always);
+
+    let width_digits = (v.size.x - 1).ilog10() + 1;
+    let height_digits = (v.size.y - 1).ilog10() + 1;
+
+    // print header
+    for i in 0..width_digits {
+        let d = width_digits - i - 1;
+
+        //padding
+        for _ in 0..height_digits {
+            write!(stdout, " ").unwrap();
+        }
+
+        for x in 0..v.size.x {
+            let digits = x / (i32::pow(10, d));
+            if digits == 0 && d > 0 {
+                write!(stdout, " ").unwrap();
+            } else {
+                write!(
+                    stdout,
+                    "{}",
+                    char::from_u32((digits % 10) as u32 + 48).unwrap()
+                )
+                .unwrap();
+            }
+        }
+        writeln!(stdout).unwrap();
+    }
+
+    for y in 0..v.size.y {
+        write!(stdout, "{:1$}", y, height_digits as usize).unwrap();
+
+        for x in 0..v.size.x {
+            if let Some((s, fg, bg)) = v.symbols.get(&Position::new(x, y)) {
+                let mut c = ColorSpec::new();
+                c.set_fg(fg.as_ref().map(|c| termcolor::Color::Rgb(c.r, c.g, c.b)));
+                c.set_bg(bg.as_ref().map(|c| termcolor::Color::Rgb(c.r, c.g, c.b)));
+                stdout.set_color(&c).unwrap();
+                write!(stdout, "{:1}", s.get_str()).unwrap();
+                stdout.reset().unwrap();
+            } else {
+                write!(stdout, " ").unwrap();
+            }
+        }
+
+        writeln!(stdout, "{:1$}", y, height_digits as usize).unwrap();
+    }
+
+    for i in 0..width_digits {
+        let d = width_digits - i - 1;
+
+        //padding
+        for _ in 0..height_digits {
+            write!(stdout, " ").unwrap();
+        }
+
+        for x in 0..v.size.x {
+            let digits = x / (i32::pow(10, d));
+            if digits == 0 && d > 0 {
+                write!(stdout, " ").unwrap();
+            } else {
+                write!(
+                    stdout,
+                    "{}",
+                    char::from_u32((digits % 10) as u32 + 48).unwrap()
+                )
+                .unwrap();
+            }
+        }
+        writeln!(stdout).unwrap();
+    }
+}

src/layout/mod.rs

@@ -1,8 +1,82 @@
+use crate::belt_finding::common::PathField;
+use crate::belt_finding::conflict_avoidance::ConflictAvoidance;
 use crate::common::visualize::{Color, Symbol, Visualization, Visualize};
 use crate::prelude::*;
-use rand::Rng;
+use rand::{seq::SliceRandom, Rng};
 use serde::{Deserialize, Serialize};
 
+pub struct GeneticAlgorithm<'a> {
+    problem: &'a Problem,
+    population: Vec<PathLayout<'a>>,
+    population_size: usize,
+    population_keep: usize,
+    population_new: usize,
+}
+
+impl<'a> GeneticAlgorithm<'a> {
+    pub fn new<R: Rng + ?Sized>(
+        problem: &'a Problem,
+        population_size: usize,
+        population_keep: usize,
+        population_new: usize,
+        rng: &mut R,
+    ) -> GeneticAlgorithm<'a> {
+        let mut population = Vec::new();
+
+        while population.len() < population_size {
+            if let Some(p) = PathLayout::new(Layout::new(problem, rng)) {
+                population.push(p);
+            }
+        }
+
+        population.sort_by_cached_key(|p| p.score());
+
+        println!("Best score: {}", population[0].score());
+        population[0].print_visualization();
+
+        GeneticAlgorithm {
+            problem,
+            population,
+            population_size,
+            population_keep,
+            population_new,
+        }
+    }
+
+    pub fn generation<R: Rng + ?Sized>(&mut self, rng: &mut R) {
+        for i in self.population_keep..(self.population_keep + self.population_new) {
+            loop {
+                if let Some(p) = PathLayout::new(Layout::new(self.problem, rng)) {
+                    self.population[i] = p;
+                    break;
+                }
+            }
+        }
+
+        for i in (self.population_keep + self.population_new)..self.population_size {
+            let j = i - (self.population_keep + self.population_new);
+            loop {
+                if let Some(p) = PathLayout::new(self.population[j].layout.mutate(rng)) {
+                    self.population[i] = p;
+                    break;
+                }
+            }
+        }
+
+        self.population.sort_by_cached_key(|p| p.score());
+        println!("Best score: {}", self.population[0].score());
+        self.population[0].print_visualization();
+    }
+
+    pub fn output_population(&self) {
+        println!("Population:");
+        for (i, p) in self.population.iter().enumerate() {
+            println!("{i:3}: {}", p.score());
+            p.print_visualization();
+        }
+    }
+}
+
 #[derive(Debug, Serialize, Deserialize)]
 pub(crate) struct Block {
     pub(crate) size: Position,
@@ -40,6 +114,85 @@ pub struct Layout<'a> {
     pub(crate) blocks: Vec<(Position, Direction)>,
 }
 
+pub struct PathLayout<'a> {
+    layout: Layout<'a>,
+    paths: Vec<Vec<PathField>>,
+    score: usize,
+}
+
+impl<'a> PathLayout<'a> {
+    pub fn new(layout: Layout<'a>) -> Option<PathLayout<'a>> {
+        layout.print_visualization();
+        let mut p = crate::belt_finding::Problem::from_layout(&layout);
+
+        if !p.find_path() {
+            return None;
+        }
+        p.print();
+
+        let mut c = ConflictAvoidance::new(p);
+
+        if !c.remove_all_conflicts() {
+            return None;
+        }
+
+        let paths = c.get_paths().to_vec();
+
+        let score = paths
+            .iter()
+            .map(|path| path.iter().skip(1).map(|p| p.cost()).sum::<usize>())
+            .sum();
+
+        Some(PathLayout {
+            layout,
+            paths,
+            score,
+        })
+    }
+
+    fn score(&self) -> usize {
+        self.score
+    }
+}
+
+impl<'a> Visualize for PathLayout<'a> {
+    fn visualize(&self) -> Visualization {
+        let mut v = self.layout.visualize();
+        let offset = self.layout.blocks.len();
+
+        for (i, path) in self.paths.iter().enumerate() {
+            for p in &path[1..] {
+                match p {
+                    PathField::Belt { pos, dir } => {
+                        v.add_symbol(
+                            *pos,
+                            Symbol::Arrow(*dir),
+                            Some(Color::index(i + offset)),
+                            None,
+                        );
+                    }
+                    PathField::Underground { pos, dir, len } => {
+                        v.add_symbol(
+                            *pos,
+                            Symbol::ArrowEnter(*dir),
+                            Some(Color::index(i + offset)),
+                            None,
+                        );
+                        v.add_symbol(
+                            pos.in_direction(dir, *len as i32),
+                            Symbol::ArrowEnter(*dir),
+                            Some(Color::index(i + offset)),
+                            None,
+                        );
+                    }
+                }
+            }
+        }
+
+        v
+    }
+}
+
 impl Problem {
     pub fn new(size: Position) -> Self {
         Self {
@@ -143,11 +296,115 @@ impl Layout<'_> {
 
     /// Mutate existing layout, creating a valid layout
     pub fn mutate<R: Rng + ?Sized>(&self, rng: &mut R) -> Self {
-        let s = self.clone();
+        let mut s = self.clone();
+
+        #[allow(clippy::type_complexity)]
+        let r: &[(&dyn Fn(&mut Layout, &mut R) -> bool, _)] = &[
+            (&Self::mutate_replace::<R>, 30),
+            (&Self::mutate_flip::<R>, 50),
+            (&Self::mutate_jiggle::<R>, 80),
+        ];
+
+        loop {
+            let p = r.choose_weighted(rng, |i| i.1).unwrap();
+
+            if p.0(&mut s, rng) && rng.gen_bool(0.4) {
+                break;
+            }
+        }
 
         s
     }
 
+    fn mutate_replace<R: Rng + ?Sized>(layout: &mut Layout, rng: &mut R) -> bool {
+        let i = rng.gen_range(0..layout.blocks.len());
+
+        let dir = rng.gen::<Direction>();
+
+        let b = &layout.problem.blocks[i];
+
+        let pos = match dir {
+            Direction::Up => Position::new(
+                rng.gen_range(0..=(layout.problem.size.x - b.size.x)),
+                rng.gen_range(0..=(layout.problem.size.y - b.size.y)),
+            ),
+            Direction::Right => Position::new(
+                rng.gen_range((b.size.y - 1)..layout.problem.size.x),
+                rng.gen_range(0..=(layout.problem.size.y - b.size.x)),
+            ),
+            Direction::Down => Position::new(
+                rng.gen_range((b.size.x - 1)..layout.problem.size.x),
+                rng.gen_range((b.size.y - 1)..layout.problem.size.y),
+            ),
+            Direction::Left => Position::new(
+                rng.gen_range(0..=(layout.problem.size.x - b.size.y)),
+                rng.gen_range((b.size.x - 1)..layout.problem.size.y),
+            ),
+        };
+
+        if layout.blocks.iter().enumerate().all(|(j, (p, d))| {
+            j == i || !Self::collision((&layout.problem.blocks[j], *p, *d), (b, pos, dir))
+        }) {
+            layout.blocks[i] = (pos, dir);
+            true
+        } else {
+            false
+        }
+    }
+
+    fn mutate_flip<R: Rng + ?Sized>(layout: &mut Layout, rng: &mut R) -> bool {
+        let i = rng.gen_range(0..layout.blocks.len());
+        let b = &mut layout.blocks[i];
+        let block = &layout.problem.blocks[i];
+
+        b.0 = match &b.1 {
+            Direction::Up => b.0 + block.size - Position::new(1, 1),
+            Direction::Right => b.0 + Position::new(1 - block.size.y, block.size.x - 1),
+            Direction::Down => b.0 - block.size + Position::new(1, 1),
+            Direction::Left => b.0 + Position::new(block.size.y - 1, 1 - block.size.x),
+        };
+        b.1 = b.1.reverse();
+
+        true
+    }
+
+    fn mutate_jiggle<R: Rng + ?Sized>(layout: &mut Layout, rng: &mut R) -> bool {
+        let i = rng.gen_range(0..layout.blocks.len());
+        let dir = rng.gen::<Direction>();
+        // let step = [(1, 10), (2, 5), (3, 5)]
+        //     .choose_weighted(rng, |i| i.1)
+        //     .unwrap()
+        //     .0;
+        let step = 1;
+
+        let b = &layout.problem.blocks[i];
+
+        let new_pos = layout.blocks[i].0.in_direction(&dir, step);
+
+        let (npos, nsize) = Self::normalize_pos((b, new_pos, layout.blocks[i].1));
+
+        if npos.x < 0
+            || npos.y < 0
+            || npos.x + nsize.x > layout.problem.size.x
+            || npos.y + nsize.y > layout.problem.size.y
+        {
+            return false;
+        }
+
+        if layout.blocks.iter().enumerate().all(|(j, (p, d))| {
+            j == i
+                || !Self::collision(
+                    (&layout.problem.blocks[j], *p, *d),
+                    (b, new_pos, layout.blocks[i].1),
+                )
+        }) {
+            layout.blocks[i].0 = new_pos;
+            true
+        } else {
+            false
+        }
+    }
+
     fn collision(
         block1: (&Block, Position, Direction),
         block2: (&Block, Position, Direction),

src/priority_queue/mod.rs

@@ -1,3 +1,4 @@
+use std::fmt::Debug;
 pub mod fibonacci_heap;
 
 pub trait PriorityQueue<Item>
@@ -17,7 +18,8 @@ where
 
 #[derive(Debug)]
 pub struct BinaryHeap<Item> {
-    data: Vec<Item>,
+    nextfree: usize,
+    data: Vec<(usize, Item)>,
 }
 
 impl<Item> BinaryHeap<Item>
@@ -47,16 +49,16 @@ where
     fn upheap(&mut self, index: usize) {
         if index > 0 {
             let parent = (index - 1) / 2;
-            if self.data[parent] > self.data[index] {
+            if self.data[parent].1 > self.data[index].1 {
                 self.data.swap(parent, index);
                 self.upheap(parent);
             }
         }
     }
 
-    fn search(&self, item: &Item) -> Option<usize> {
+    fn search(&self, id: usize) -> Option<usize> {
         for (i, d) in self.data.iter().enumerate() {
-            if d == item {
+            if d.0 == id {
                 return Some(i);
             }
         }
@@ -68,12 +70,13 @@ impl<Item> PriorityQueue<Item> for BinaryHeap<Item>
 where
     Item: PartialOrd + Clone,
 {
-    type Handle = Item;
+    type Handle = usize;
 
     fn insert(&mut self, item: Item) -> Self::Handle {
-        self.data.push(item.clone());
+        self.data.push((self.nextfree, item.clone()));
         self.upheap(self.data.len() - 1);
-        item
+        self.nextfree += 1;
+        self.nextfree - 1
     }
 
     fn pop_min(&mut self) -> Option<Item> {
@@ -82,19 +85,59 @@ where
         } else {
             let d = self.data.swap_remove(0);
             self.downheap(0);
-            Some(d)
+            Some(d.1)
         }
     }
 
     fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut Item)) {
-        if let Some(index) = self.search(handle) {
-            f(&mut self.data[index]);
+        if let Some(index) = self.search(*handle) {
+            f(&mut self.data[index].1);
             self.upheap(index);
         }
     }
 
     fn new() -> Self {
-        Self { data: Vec::new() }
+        Self {
+            data: Vec::new(),
+            nextfree: 0,
+        }
+    }
+}
+
+#[derive(Debug)]
+pub struct Trace<P> {
+    inner: P,
+}
+
+impl<P, I> PriorityQueue<I> for Trace<P>
+where
+    I: PartialOrd + Clone + Debug,
+    P: PriorityQueue<I>,
+{
+    type Handle = P::Handle;
+
+    fn new() -> Self {
+        println!("New priority queue.");
+        Self { inner: P::new() }
+    }
+
+    fn insert(&mut self, item: I) -> Self::Handle {
+        println!("Insert: {item:?}");
+        self.inner.insert(item)
+    }
+
+    fn pop_min(&mut self) -> Option<I> {
+        let min = self.inner.pop_min();
+        println!("Pop min: {min:?}");
+        min
+    }
+
+    fn decrease_key(&mut self, handle: &Self::Handle, f: impl Fn(&mut I)) {
+        self.inner.decrease_key(handle, |i| {
+            let old_i = i.clone();
+            f(i);
+            println!("Decrease key: {old_i:?} -> {i:?}");
+        })
     }
 }