diff --git a/src/CoveragePathPlanner/CoveragePathPlanner.cs b/src/CoveragePathPlanner/CoveragePathPlanner.cs
index 5a7908d..af08788 100644
--- a/src/CoveragePathPlanner/CoveragePathPlanner.cs
+++ b/src/CoveragePathPlanner/CoveragePathPlanner.cs
@@ -39,21 +39,17 @@ public class CoveragePathPlanner
     var queue = new PriorityQueue<Cell, double>();
     queue.Enqueue(startCell, 0);
 
+    List<Point> path = new List<Point>();
+
     while (queue.Count > 0)
     {
       // Dequeue the cell with the lowest cost
       Cell currentCell = queue.Dequeue();
 
-      // If the cell is the goal, construct the path
-      if (currentCell.X == _width - 1 && currentCell.Y == _height - 1)
+      // If the cell has not been visited before, add it to the path
+      if (!currentCell.IsVisited)
       {
-        List<Point> path = new List<Point>();
-        while (currentCell != null)
-        {
-          path.Add(new Point(currentCell.X, currentCell.Y));
-          currentCell = currentCell.Parent;
-        }
-        return path;
+        path.Add(new Point(currentCell.X, currentCell.Y));
       }
 
       // Mark the cell as visited
@@ -70,14 +66,13 @@ public class CoveragePathPlanner
           {
             neighbor.Cost = cost;
             neighbor.Parent = currentCell;
-            queue.Enqueue(neighbor, cost + Heuristic(neighbor));
+            queue.Enqueue(neighbor, cost);
           }
         }
       }
     }
 
-    // If no path is found, return an empty list
-    return new List<Point>();
+    return path;
   }
 
   private List<Cell> GetNeighbors(Cell cell)
@@ -102,12 +97,6 @@ public class CoveragePathPlanner
 
     return neighbors;
   }
-
-  private double Heuristic(Cell cell)
-  {
-    // Manhattan distance heuristic
-    return Math.Abs(cell.X - (_width - 1)) + Math.Abs(cell.Y - (_height - 1));
-  }
 }
 
 public class Cell
diff --git a/src/CoveragePathPlanner/Test.cs b/src/CoveragePathPlanner/Test.cs
new file mode 100644
index 0000000..d1c2507
--- /dev/null
+++ b/src/CoveragePathPlanner/Test.cs
@@ -0,0 +1,66 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+
+class Program
+{
+  static void Main(string[] args)
+  {
+    // Create a 20x20 grid
+    CoveragePathPlanner planner = new CoveragePathPlanner(20, 20);
+
+    // Add some obstacles to the grid
+    List<Point> obstacles = new List<Point>();
+
+    // Add a 2x2 obstacle at (4, 4)
+    for (int x = 4; x < 6; x++)
+    {
+      for (int y = 4; y < 6; y++)
+      {
+        obstacles.Add(new Point(x, y));
+      }
+    }
+
+    // Add a 3x3 obstacle at (10, 10)
+    for (int x = 10; x < 13; x++)
+    {
+      for (int y = 10; y < 13; y++)
+      {
+        obstacles.Add(new Point(x, y));
+      }
+    }
+
+    // Plan the path
+    List<Point> path = planner.PlanPath(obstacles);
+
+    // Print the grid
+    for (int y = 0; y < 20; y++)
+    {
+      for (int x = 0; x < 20; x++)
+      {
+        if (obstacles.Contains(new Point(x, y)))
+        {
+          Console.Write("X "); // Obstacle
+        }
+        else if (path.Contains(new Point(x, y)))
+        {
+          Console.Write("P "); // Path
+        }
+        else
+        {
+          Console.Write(". "); // Empty cell
+        }
+      }
+      Console.WriteLine();
+    }
+
+    // Print the path
+    Console.WriteLine("Path:");
+    foreach (Point point in path)
+    {
+      Console.WriteLine($"({point.X}, {point.Y})");
+    }
+
+    Console.ReadLine();
+  }
+}
diff --git a/src/Server/RobotControlServer/Routes/RoutePlan.cs b/src/Server/RobotControlServer/Routes/RoutePlan.cs
index 1cbb325..a26e255 100644
--- a/src/Server/RobotControlServer/Routes/RoutePlan.cs
+++ b/src/Server/RobotControlServer/Routes/RoutePlan.cs
@@ -26,13 +26,16 @@ public class RoutePlan: IRoute {
       // New instance of CPP with a 500x500 grid || where 0,0 = top-left and 499, 499 = bottom-right
       CoveragePathPlanner planner = new CoveragePathPlanner(500, 500);
 
-      // Add obstacles cell by cell
+      // Create obstacle cell list
+      List<Point> obstacles = new List<Point>();
+
+      // Add obstacle cells
       foreach (var obj in $"[{parsedBody?.objects}]".FromJson<int[][]>())
       {
-        planner.AddObstacleCell(obj[0], obj[1]); // Get x and y for every coordinate in the parsedbody array
+        obstacles.Add(new Point(obj[0], obj[1])); // Get x and y for every coordinate in the parsedbody array
       }
 
-      List<Point> PlanPath = planner.PlanPath();
+      List<Point> PlanPath = planner.PlanPath(obstacles);
       // Don't know if this is needed here or just do List<Point> path = planner.PlanPath(obstacles);?
       string path = JsonConvert.SerializeObject(PlanPath);