diff --git a/src/utils/pathfinding.rs b/src/utils/pathfinding.rs
index 8f660d2..6b6d56f 100644
--- a/src/utils/pathfinding.rs
+++ b/src/utils/pathfinding.rs
@@ -27,7 +27,7 @@ impl PartialOrd for Node {
 }
 
 pub fn manhattan_distance(x1: u32, y1: u32, x2: u32, y2: u32) -> u32 {
-    (x1 as i32 - x2 as i32).abs() as u32 + (y1 as i32 - y2 as i32).abs() as u32
+    x1.abs_diff(x2) + y1.abs_diff(y2)
 }
 
 pub fn find_path(
@@ -37,8 +37,9 @@ pub fn find_path(
     tile_query: &Query<&TileState>,
 ) -> Option<VecDeque<(u32, u32)>> {
     let target_entity = grid.get_tile(end).ok()?;
-    let state = tile_query.get(target_entity).unwrap();
-    if (*state).is_blocking() {
+    let target_state = tile_query.get(target_entity).ok()?;
+
+    if target_state.is_blocking() {
         return None;
     }
 
@@ -68,6 +69,12 @@ pub fn find_path(
             return Some(path);
         }
 
+        if let Some(&best_g) = g_score.get(&current_pos) {
+            if current.cost > best_g {
+                continue;
+            }
+        }
+
         let neighbors = [
             (current.x as i32 + 1, current.y as i32),
             (current.x as i32 - 1, current.y as i32),
@@ -81,9 +88,12 @@ pub fn find_path(
             }
             let next_pos = (nx as u32, ny as u32);
 
-            let tile_entity = grid.get_tile(next_pos).ok()?;
-            let state = tile_query.get(tile_entity).unwrap();
-            if (*state).is_blocking() {
+            let tile_entity = match grid.get_tile(next_pos) {
+                Ok(e) => e,
+                Err(_) => continue,
+            };
+            let state = tile_query.get(tile_entity).ok()?;
+            if state.is_blocking() {
                 continue;
             }
 
@@ -91,6 +101,7 @@ pub fn find_path(
             if tentative_g_score < *g_score.get(&next_pos).unwrap_or(&u32::MAX) {
                 came_from.insert(next_pos, current_pos);
                 g_score.insert(next_pos, tentative_g_score);
+
                 open_set.push(Node {
                     x: next_pos.0,
                     y: next_pos.1,