diff --git a/examples/03_view_map.py b/examples/03_view_map.py
index 7f91782..d054edd 100644
--- a/examples/03_view_map.py
+++ b/examples/03_view_map.py
@@ -42,7 +42,7 @@
     elif isinstance(node, tuple):
         path.append([node[0], node[1], node[2]])
 print(f"path: {path}")
-  
+
 
 # visualize path in open3d
 if USE_OPEN3D:
@@ -51,16 +51,16 @@
     xyz_pt = np.stack(obstacle_indices, axis=-1).astype(float)
     colors = np.zeros((xyz_pt.shape[0], 3))
     colors[:, 2] = obstacle_indices[2] / np.max(obstacle_indices[2])
-    
+
     # Prepare start and end colors
     start_color = np.array([[1.0, 0, 0]])  # Red
-    end_color = np.array([[0, 1.0, 0]])    # Green
+    end_color = np.array([[0, 1.0, 0]])  # Green
     path_colors = np.full((len(path) - 2, 3), [0.7, 0.7, 0.7])  # Grey for the path
 
     # Combine points and colors
     xyz_pt = np.concatenate((xyz_pt, [start_pt], [end_pt], path[1:-1]))
     colors = np.concatenate((colors, start_color, end_color, path_colors))
-    
+
     # Create and visualize the point cloud
     pcd = o3d.geometry.PointCloud()
     pcd.points = o3d.utility.Vector3dVector(xyz_pt)
diff --git a/test/test_connect_grids.py b/test/test_connect_grids.py
index 2c4454d..2f91e26 100644
--- a/test/test_connect_grids.py
+++ b/test/test_connect_grids.py
@@ -4,9 +4,9 @@
 
 PATH = [
     (2, 0, 0, 0),
-    (2, 0, 1, 0),
-    (2, 0, 2, 0),
-    (2, 1, 2, 0),
+    (2, 1, 0, 0),
+    (2, 2, 0, 0),
+    (2, 2, 1, 0),
     (2, 2, 2, 0),
     # move to grid 1
     (2, 2, 2, 1),
diff --git a/test/test_heap.py b/test/test_heap.py
new file mode 100644
index 0000000..6d821c3
--- /dev/null
+++ b/test/test_heap.py
@@ -0,0 +1,26 @@
+from pathfinding3d.core.grid import Grid
+from pathfinding3d.core.heap import SimpleHeap
+
+
+def test_heap():
+    grid = Grid(width=10, height=10, depth=10)
+    start = grid.node(0, 0, 0)
+    open_list = SimpleHeap(start, grid)
+
+    # Test pop
+    assert open_list.pop_node() == start
+    assert len(open_list) == 0
+
+    # Test push
+    open_list.push_node(grid.node(1, 1, 1))
+    open_list.push_node(grid.node(1, 1, 2))
+    open_list.push_node(grid.node(1, 1, 3))
+
+    # Test removal and pop
+    assert len(open_list) == 3
+    open_list.remove_node(grid.node(1, 1, 2), 0)
+    assert len(open_list) == 3
+
+    assert open_list.pop_node() == grid.node(1, 1, 1)
+    assert open_list.pop_node() == grid.node(1, 1, 3)
+    assert len(open_list) == 0
diff --git a/test/test_path.py b/test/test_path.py
index fc838c6..f429883 100644
--- a/test/test_path.py
+++ b/test/test_path.py
@@ -13,7 +13,6 @@
 from pathfinding3d.finder.ida_star import IDAStarFinder
 from pathfinding3d.finder.msp import MinimumSpanningTree
 
-# test scenarios from Pathfinding.JS
 finders = [
     AStarFinder,
     BestFirst,
@@ -31,7 +30,6 @@
     DijkstraFinder,
     MinimumSpanningTree,
 ]
-TIME_LIMIT = 10  # give it a 10 second limit
 
 SIMPLE_MATRIX = np.zeros((5, 5, 5))
 SIMPLE_MATRIX[0, 0, 0] = 1
@@ -60,7 +58,7 @@
 
 def test_path():
     """
-    test scenarios defined in json file
+    test if we can find a path
     """
     grid = Grid(matrix=SIMPLE_MATRIX)
     start = grid.node(0, 0, 0)