diff --git a/src/belt_finding/conflict_avoidance.rs b/src/belt_finding/conflict_avoidance.rs
index 5c52247..b6af824 100644
--- a/src/belt_finding/conflict_avoidance.rs
+++ b/src/belt_finding/conflict_avoidance.rs
@@ -71,6 +71,11 @@ impl ConflictAvoidance {
         for i in 0..problem.path.len() {
             let mut p = Vec::new();
 
+            p.push(PathField::Belt {
+                pos: problem.start[i].0,
+                dir: problem.start[i].1,
+            });
+
             for j in 0..problem.path[i].len() - 1 {
                 p.push(PathField::Belt {
                     pos: problem.path[i][j],
@@ -80,7 +85,12 @@ impl ConflictAvoidance {
 
             p.push(PathField::Belt {
                 pos: *problem.path[i].last().unwrap(),
-                dir: *p.last().unwrap().dir(),
+                dir: problem.end[i].1,
+            });
+
+            p.push(PathField::Belt {
+                pos: problem.end[i].0,
+                dir: problem.end[i].1,
             });
 
             belts.push(p);
@@ -109,7 +119,7 @@ impl ConflictAvoidance {
         }
 
         for path in &self.belts {
-            for p in path {
+            for p in &path[1..path.len() - 1] {
                 match p {
                     PathField::Belt { pos, dir: _ } => {
                         *conflicts.get_mut(pos.x as usize, pos.y as usize) += 1
@@ -426,7 +436,7 @@ impl ConflictAvoidance {
         }
 
         for path in &self.belts {
-            for p in path {
+            for p in &path[1..path.len() - 1] {
                 match p {
                     PathField::Belt { pos, dir: _ } => {
                         *conflicts.get_mut(pos.x as usize, pos.y as usize) += 1
diff --git a/src/belt_finding/mod.rs b/src/belt_finding/mod.rs
index 1663152..38bf46a 100644
--- a/src/belt_finding/mod.rs
+++ b/src/belt_finding/mod.rs
@@ -7,7 +7,7 @@ use crate::{
 use std::ops::Index;
 use termcolor::{Color, ColorSpec};
 
-use self::common::{print_map, PathField, Position, PositionType};
+use self::common::{print_map, Direction, PathField, Position, PositionType};
 
 pub mod brute_force;
 pub mod common;
@@ -21,8 +21,8 @@ pub struct Field {
 
 pub struct Problem {
     map: Map<Field>,
-    start: Vec<Position>,
-    end: Vec<Position>,
+    start: Vec<(Position, Direction)>,
+    end: Vec<(Position, Direction)>,
     path: Vec<Vec<Position>>,
 }
 
@@ -36,7 +36,7 @@ impl Problem {
         }
     }
 
-    pub fn add_connection(&mut self, start: Position, end: Position) {
+    pub fn add_connection(&mut self, start: (Position, Direction), end: (Position, Direction)) {
         self.start.push(start);
         self.end.push(end);
         self.path.push(Vec::new());
@@ -80,11 +80,11 @@ impl Problem {
         }
 
         for p in &self.start {
-            self.map.get_mut(p.x as usize, p.y as usize).wheight += 200.0;
+            self.map.get_mut(p.0.x as usize, p.0.y as usize).wheight = f64::INFINITY;
         }
 
         for p in &self.end {
-            self.map.get_mut(p.x as usize, p.y as usize).wheight += 200.0;
+            self.map.get_mut(p.0.x as usize, p.0.y as usize).wheight = f64::INFINITY;
         }
     }
 
@@ -94,14 +94,14 @@ impl Problem {
             if let Some(i) = self
                 .start
                 .iter()
-                .position(|p| p == &Position::new(x as PositionType, y as PositionType))
+                .position(|p| p.0 == Position::new(x as PositionType, y as PositionType))
             {
                 color.set_fg(Some(COLORS[i]));
                 (color, "s")
             } else if let Some(i) = self
                 .end
                 .iter()
-                .position(|p| p == &Position::new(x as PositionType, y as PositionType))
+                .position(|p| p.0 == Position::new(x as PositionType, y as PositionType))
             {
                 color.set_fg(Some(COLORS[i]));
                 (color, "t")
@@ -111,13 +111,15 @@ impl Problem {
                     .map(|j| (i, j))
             }) {
                 color.set_fg(Some(COLORS[i]));
-                if self.path[i][p].x < self.path[i][p + 1].x {
+                let c = &self.path[i][p];
+                let n = self.path[i].get(p + 1).unwrap_or(&self.end[i].0);
+                if c.x < n.x {
                     (color, "→")
-                } else if self.path[i][p].x > self.path[i][p + 1].x {
+                } else if c.x > n.x {
                     (color, "←")
-                } else if self.path[i][p].y < self.path[i][p + 1].y {
+                } else if c.y < n.y {
                     (color, "↓")
-                } else if self.path[i][p].y > self.path[i][p + 1].y {
+                } else if c.y > n.y {
                     (color, "↑")
                 } else {
                     unreachable!()
@@ -210,7 +212,11 @@ impl Problem {
         for i in 0..self.start.len() {
             self.calculate_wheights(i);
             let m = MapInternal { map: &self.map };
-            let p = dijkstra::<MapInternal, FibonacciHeap<_>>(&m, self.start[i], self.end[i]);
+            let p = dijkstra::<MapInternal, FibonacciHeap<_>>(
+                &m,
+                self.start[i].0.in_direction(&self.start[i].1, 1),
+                self.end[i].0.in_direction(&self.end[i].1, -1),
+            );
 
             if let Some(p) = p {
                 self.path[i] = p;
@@ -220,7 +226,10 @@ impl Problem {
 }
 
 pub mod problems {
-    use super::{common::Position, Problem};
+    use super::{
+        common::{Direction, Position},
+        Problem,
+    };
 
     pub fn belt_madness_level_1() -> Problem {
         let mut p = Problem::new(17, 13);
@@ -253,11 +262,23 @@ pub mod problems {
 
         p.set_blocked(16, 9, true);
 
-        p.add_connection(Position::new(3, 7), Position::new(13, 4));
-        p.add_connection(Position::new(3, 8), Position::new(13, 5));
+        p.add_connection(
+            (Position::new(2, 7), Direction::Right),
+            (Position::new(14, 4), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 8), Direction::Right),
+            (Position::new(14, 5), Direction::Right),
+        );
 
-        p.add_connection(Position::new(3, 4), Position::new(13, 8));
-        p.add_connection(Position::new(3, 5), Position::new(13, 7));
+        p.add_connection(
+            (Position::new(2, 4), Direction::Right),
+            (Position::new(14, 8), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 5), Direction::Right),
+            (Position::new(14, 7), Direction::Right),
+        );
 
         p
     }
@@ -278,15 +299,39 @@ pub mod problems {
 
         p.set_blocked(16, 9, true);
 
-        p.add_connection(Position::new(3, 4), Position::new(13, 2));
-        p.add_connection(Position::new(3, 7), Position::new(13, 3));
-        p.add_connection(Position::new(3, 10), Position::new(13, 4));
-        p.add_connection(Position::new(3, 9), Position::new(13, 5));
+        p.add_connection(
+            (Position::new(2, 4), Direction::Right),
+            (Position::new(14, 2), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 7), Direction::Right),
+            (Position::new(14, 3), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 10), Direction::Right),
+            (Position::new(14, 4), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 9), Direction::Right),
+            (Position::new(14, 5), Direction::Right),
+        );
 
-        p.add_connection(Position::new(3, 2), Position::new(13, 7));
-        p.add_connection(Position::new(3, 3), Position::new(13, 8));
-        p.add_connection(Position::new(3, 5), Position::new(13, 9));
-        p.add_connection(Position::new(3, 8), Position::new(13, 10));
+        p.add_connection(
+            (Position::new(2, 2), Direction::Right),
+            (Position::new(14, 7), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 3), Direction::Right),
+            (Position::new(14, 8), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 5), Direction::Right),
+            (Position::new(14, 9), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 8), Direction::Right),
+            (Position::new(14, 10), Direction::Right),
+        );
 
         p
     }
@@ -309,13 +354,31 @@ pub mod problems {
         p.set_blocked(32, 3, true);
         p.set_blocked(32, 9, true);
 
-        p.add_connection(Position::new(3, 3), Position::new(29, 7));
-        p.add_connection(Position::new(3, 4), Position::new(29, 9));
-        p.add_connection(Position::new(3, 5), Position::new(29, 8));
+        p.add_connection(
+            (Position::new(2, 3), Direction::Right),
+            (Position::new(29, 7), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 4), Direction::Right),
+            (Position::new(29, 9), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 5), Direction::Right),
+            (Position::new(29, 8), Direction::Right),
+        );
 
-        p.add_connection(Position::new(3, 7), Position::new(29, 3));
-        p.add_connection(Position::new(3, 8), Position::new(29, 5));
-        p.add_connection(Position::new(3, 9), Position::new(29, 4));
+        p.add_connection(
+            (Position::new(2, 7), Direction::Right),
+            (Position::new(29, 3), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 8), Direction::Right),
+            (Position::new(29, 5), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 9), Direction::Right),
+            (Position::new(29, 4), Direction::Right),
+        );
 
         p
     }
@@ -408,20 +471,62 @@ pub mod problems {
         p.set_blocked_range(28, 17, 29, 17, true);
 
         // Path
-        p.add_connection(Position::new(4, 3), Position::new(26, 25));
-        p.add_connection(Position::new(12, 3), Position::new(18, 25));
-        p.add_connection(Position::new(14, 3), Position::new(3, 26));
-        p.add_connection(Position::new(16, 3), Position::new(14, 25));
-        p.add_connection(Position::new(3, 4), Position::new(27, 24));
-        p.add_connection(Position::new(3, 11), Position::new(27, 17));
-        p.add_connection(Position::new(3, 13), Position::new(27, 26));
-        p.add_connection(Position::new(3, 15), Position::new(27, 13));
-        p.add_connection(Position::new(27, 15), Position::new(2, 3));
-        p.add_connection(Position::new(3, 17), Position::new(18, 3));
-        p.add_connection(Position::new(3, 24), Position::new(26, 3));
-        p.add_connection(Position::new(4, 25), Position::new(27, 4));
-        p.add_connection(Position::new(12, 25), Position::new(27, 11));
-        p.add_connection(Position::new(16, 25), Position::new(28, 3));
+        p.add_connection(
+            (Position::new(4, 2), Direction::Down),
+            (Position::new(26, 26), Direction::Down),
+        );
+        p.add_connection(
+            (Position::new(12, 2), Direction::Down),
+            (Position::new(18, 26), Direction::Down),
+        );
+        p.add_connection(
+            (Position::new(14, 2), Direction::Down),
+            (Position::new(2, 26), Direction::Left),
+        );
+        p.add_connection(
+            (Position::new(16, 2), Direction::Down),
+            (Position::new(14, 26), Direction::Down),
+        );
+        p.add_connection(
+            (Position::new(2, 4), Direction::Right),
+            (Position::new(28, 24), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 11), Direction::Right),
+            (Position::new(28, 17), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 13), Direction::Right),
+            (Position::new(28, 26), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(2, 15), Direction::Right),
+            (Position::new(28, 13), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(28, 15), Direction::Left),
+            (Position::new(2, 2), Direction::Up),
+        );
+        p.add_connection(
+            (Position::new(2, 17), Direction::Right),
+            (Position::new(18, 2), Direction::Up),
+        );
+        p.add_connection(
+            (Position::new(2, 24), Direction::Right),
+            (Position::new(26, 2), Direction::Up),
+        );
+        p.add_connection(
+            (Position::new(4, 26), Direction::Up),
+            (Position::new(28, 4), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(12, 26), Direction::Up),
+            (Position::new(28, 11), Direction::Right),
+        );
+        p.add_connection(
+            (Position::new(16, 26), Direction::Up),
+            (Position::new(28, 2), Direction::Up),
+        );
 
         p
     }