diff --git a/lib/dijkstra/src/lib.rs b/lib/dijkstra/src/lib.rs
index f5084ab..16eb93a 100644
--- a/lib/dijkstra/src/lib.rs
+++ b/lib/dijkstra/src/lib.rs
@@ -24,6 +24,7 @@ impl<T> Debug for dyn Matrix<T> {
 
 pub struct Dijkstra<'a, T> {
     matrix: Box<&'a mut dyn Matrix<T>>,
+    has_distance: HashSet<Coordinate>,
     unvisited: HashSet<Coordinate>,
 }
 
@@ -40,12 +41,17 @@ where
     T: PartialOrd + Add<Output = T> + Display + Copy + Debug,
 {
     pub fn new(matrix: Box<&'a mut dyn Matrix<T>>) -> Self {
+        let has_distance = HashSet::from([START]);
         let unvisited = Self::initialize_unvisited_set(*matrix, START);
-        Self { matrix, unvisited }
+        Self {
+            matrix,
+            has_distance,
+            unvisited,
+        }
     }
 
     pub fn solve(&mut self) {
-        while !self.unvisited.is_empty() {
+        while !(self.unvisited.is_empty() && self.has_distance.is_empty()) {
             let coord = self.get_unvisited_coord_with_smallest_distance_from_start();
             let neighbours = self.get_neighbours(&coord);
             let previous_distance = *self.matrix.get(coord).unwrap().get_distance().unwrap();
@@ -57,7 +63,7 @@ where
 
     fn get_unvisited_coord_with_smallest_distance_from_start(&mut self) -> Coordinate {
         let coord = self
-            .unvisited
+            .has_distance
             .iter()
             .filter_map(|c| {
                 let node = self.matrix.get(*c).unwrap();
@@ -66,7 +72,7 @@ where
             .reduce(|(c0, v0), (c1, v1)| if v0 < v1 { (c0, v0) } else { (c1, v1) })
             .map(|(c, _)| *c)
             .unwrap_or((0, 0));
-        self.unvisited.remove(&coord);
+        self.has_distance.remove(&coord);
         coord
     }
 
@@ -88,9 +94,11 @@ where
         if let Some(old_distance) = next_node.get_distance() {
             if new_distance < *old_distance {
                 Self::update_node(next_node, new_distance, previous);
+                self.update_distances_hashset(next);
             }
         } else {
             Self::update_node(next_node, new_distance, previous);
+            self.update_distances_hashset(next);
         }
     }
 
@@ -106,6 +114,11 @@ where
         next_node.set_distance(new_distance);
         next_node.set_previous(previous);
     }
+
+    fn update_distances_hashset(&mut self, next: Coordinate) {
+        self.unvisited.remove(&next);
+        self.has_distance.insert(next);
+    }
 }
 
 pub trait Node<T>