diff --git a/examples/layout.rs b/examples/layout.rs
index 40bc0f0..1250375 100644
--- a/examples/layout.rs
+++ b/examples/layout.rs
@@ -1,64 +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 m = l.mutate(&mut rng);
-        m.print_visualization();
-
-        let m = m.mutate(&mut rng);
-        m.print_visualization();
-        let m = m.mutate(&mut rng);
-        m.print_visualization();
-        let m = m.mutate(&mut rng);
-        m.print_visualization();
-        let m = m.mutate(&mut rng);
-        m.print_visualization();
-        let m = m.mutate(&mut rng);
-        m.print_visualization();
-        let m = m.mutate(&mut rng);
-        m.print_visualization();
-        let m = m.mutate(&mut rng);
-        m.print_visualization();
-        // 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();
 }
diff --git a/layout.yml b/layout.yml
index 24e98dc..4189f8f 100644
--- a/layout.yml
+++ b/layout.yml
@@ -1,6 +1,6 @@
 size:
-  x: 10
-  y: 10
+  x: 15
+  y: 15
 blocks:
   - size:
       x: 3
diff --git a/layout2.yml b/layout2.yml
new file mode 100644
index 0000000..e89adb4
--- /dev/null
+++ b/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
diff --git a/src/belt_finding/common.rs b/src/belt_finding/common.rs
index c58ed1d..be3f43a 100644
--- a/src/belt_finding/common.rs
+++ b/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<()>
diff --git a/src/belt_finding/conflict_avoidance.rs b/src/belt_finding/conflict_avoidance.rs
index ebf44ea..fd4c955 100644
--- a/src/belt_finding/conflict_avoidance.rs
+++ b/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) {
diff --git a/src/belt_finding/mod.rs b/src/belt_finding/mod.rs
index 0648f41..c8e7b9a 100644
--- a/src/belt_finding/mod.rs
+++ b/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
     }
 }
 
diff --git a/src/layout/mod.rs b/src/layout/mod.rs
index 15b2751..5dfc31d 100644
--- a/src/layout/mod.rs
+++ b/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::{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 {
diff --git a/src/priority_queue/mod.rs b/src/priority_queue/mod.rs
index b622c6b..5d2b03a 100644
--- a/src/priority_queue/mod.rs
+++ b/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:?}");
+        })
     }
 }