diff --git a/src/GridFillerBench/FunctionalTests.cs b/src/GridFillerBench/FunctionalTests.cs
new file mode 100644
index 0000000..9e7e768
--- /dev/null
+++ b/src/GridFillerBench/FunctionalTests.cs
@@ -0,0 +1,180 @@
+using LargeGridPathfinding;
+using Microsoft.Xna.Framework;
+
+public class GridFillerFunctionalTests
+{
+    public static void Main()
+    {
+        Console.WriteLine("Running GridFiller Functional Tests...\n");
+
+        TestBasicGridFill();
+        TestPartialGridFill();
+        TestObstaclePlacement();
+        TestObstacleRemoval();
+        TestWeightAdjustment();
+        TestRectanglePlacement();
+
+        Console.WriteLine("\n✅ All functional tests passed!");
+    }
+
+    private static void TestBasicGridFill()
+    {
+        Console.WriteLine("Test 1: Basic Grid Fill");
+        var filler = new GridFiller(100, 100);
+        
+        filler.FillGrid(fillAll: true);
+        
+        // Check that grid has been populated with valid labels
+        int filledCells = 0;
+        for (int y = 0; y < 100; y++)
+        {
+            for (int x = 0; x < 100; x++)
+            {
+                if (filler.Grid[y, x] > 0)
+                    filledCells++;
+            }
+        }
+        
+        Console.WriteLine($"  Filled cells: {filledCells}/10000");
+        Assert(filledCells > 0, "Grid should have filled cells");
+        Console.WriteLine("  ✓ Passed\n");
+    }
+
+    private static void TestPartialGridFill()
+    {
+        Console.WriteLine("Test 2: Partial Grid Fill");
+        var filler = new GridFiller(100, 100);
+        
+        filler.FillGrid(0, 0, 50, 50, fillAll: true);
+        
+        // Check that only the partial region has been filled
+        int filledInRegion = 0;
+        int filledOutsideRegion = 0;
+        
+        for (int y = 0; y < 100; y++)
+        {
+            for (int x = 0; x < 100; x++)
+            {
+                if (filler.Grid[y, x] > 0)
+                {
+                    if (x < 50 && y < 50)
+                        filledInRegion++;
+                    else
+                        filledOutsideRegion++;
+                }
+            }
+        }
+        
+        Console.WriteLine($"  Filled in region (0,0,50,50): {filledInRegion}");
+        Console.WriteLine($"  Filled outside region: {filledOutsideRegion}");
+        Assert(filledInRegion > 0, "Region should have filled cells");
+        Assert(filledOutsideRegion == 0, "Outside region should be empty");
+        Console.WriteLine("  ✓ Passed\n");
+    }
+
+    private static void TestObstaclePlacement()
+    {
+        Console.WriteLine("Test 3: Obstacle Placement");
+        var filler = new GridFiller(100, 100);
+        
+        var obstacleRect = new Rectangle(10, 10, 20, 20);
+        filler.PlaceObstacle(obstacleRect);
+        
+        // Check that obstacle has negative labels
+        int obstaclesPlaced = 0;
+        for (int y = 10; y < 30; y++)
+        {
+            for (int x = 10; x < 30; x++)
+            {
+                if (filler.Grid[y, x] < 0)
+                    obstaclesPlaced++;
+            }
+        }
+        
+        Console.WriteLine($"  Obstacles placed: {obstaclesPlaced}/400");
+        Assert(obstaclesPlaced == 400, "Obstacle should cover entire rectangle");
+        Console.WriteLine("  ✓ Passed\n");
+    }
+
+    private static void TestObstacleRemoval()
+    {
+        Console.WriteLine("Test 4: Obstacle Removal");
+        var filler = new GridFiller(100, 100);
+        
+        var obstacleRect = new Rectangle(10, 10, 20, 20);
+        filler.PlaceObstacle(obstacleRect);
+        filler.RemoveObstacle(obstacleRect);
+        
+        // Check that obstacle has been removed
+        int obstaclesRemaining = 0;
+        for (int y = 10; y < 30; y++)
+        {
+            for (int x = 10; x < 30; x++)
+            {
+                if (filler.Grid[y, x] < 0)
+                    obstaclesRemaining++;
+            }
+        }
+        
+        Console.WriteLine($"  Obstacles remaining: {obstaclesRemaining}");
+        Assert(obstaclesRemaining == 0, "Obstacle should be completely removed");
+        Console.WriteLine("  ✓ Passed\n");
+    }
+
+    private static void TestWeightAdjustment()
+    {
+        Console.WriteLine("Test 5: Weight Adjustment");
+        var filler = new GridFiller(100, 100);
+        
+        var points = new List<Point> { new(10, 10), new(11, 11) };
+        filler.SetTileWeights(points, 5);
+        
+        Assert(filler.WeightGrid[10, 10] == 5, "Weight at (10,10) should be 5");
+        Assert(filler.WeightGrid[11, 11] == 5, "Weight at (11,11) should be 5");
+        
+        filler.ResetTileWeights(points);
+        
+        Assert(filler.WeightGrid[10, 10] == 1, "Weight should be reset to 1");
+        Assert(filler.WeightGrid[11, 11] == 1, "Weight should be reset to 1");
+        Console.WriteLine("  ✓ Passed\n");
+    }
+
+    private static void TestRectanglePlacement()
+    {
+        Console.WriteLine("Test 6: Rectangle Placement Tracking");
+        var filler = new GridFiller(100, 100);
+        
+        filler.FillGrid(fillAll: true);
+        
+        // Check that PlacedRectangles collection is populated
+        int placedCount = filler.PlacedRectangles.Count;
+        Console.WriteLine($"  Rectangles placed: {placedCount}");
+        Assert(placedCount > 0, "Should have placed rectangles");
+        
+        // Verify each rectangle in the collection matches the grid
+        foreach (var kvp in filler.PlacedRectangles)
+        {
+            int label = kvp.Key;
+            var rect = kvp.Value;
+            
+            // Check a few cells from this rectangle
+            for (int y = rect.Top; y < Math.Min(rect.Top + 2, rect.Bottom); y++)
+            {
+                for (int x = rect.Left; x < Math.Min(rect.Left + 2, rect.Right); x++)
+                {
+                    Assert(filler.Grid[y, x] == label, $"Grid cell ({x},{y}) should have label {label}");
+                }
+            }
+        }
+        
+        Console.WriteLine("  ✓ Passed\n");
+    }
+
+    private static void Assert(bool condition, string message)
+    {
+        if (!condition)
+        {
+            throw new Exception($"Assertion failed: {message}");
+        }
+    }
+}
diff --git a/src/GridFillerBench/GridFillerBench.csproj b/src/GridFillerBench/GridFillerBench.csproj
new file mode 100644
index 0000000..1479217
--- /dev/null
+++ b/src/GridFillerBench/GridFillerBench.csproj
@@ -0,0 +1,18 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net8.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+
+  <ItemGroup>
+    <PackageReference Include="BenchmarkDotNet" Version="0.13.12" />
+  </ItemGroup>
+
+  <ItemGroup>
+    <ProjectReference Include="..\LargeGridPathfinding\LargeGridPathfinding.csproj" />
+  </ItemGroup>
+
+</Project>
diff --git a/src/GridFillerBench/Program.cs b/src/GridFillerBench/Program.cs
new file mode 100644
index 0000000..85b89a8
--- /dev/null
+++ b/src/GridFillerBench/Program.cs
@@ -0,0 +1,174 @@
+using BenchmarkDotNet.Attributes;
+using BenchmarkDotNet.Running;
+using LargeGridPathfinding;
+using Microsoft.Xna.Framework;
+
+// Check if we should run tests or benchmarks
+if (args.Length > 0 && args[0] == "--test")
+{
+    RunFunctionalTests();
+}
+else
+{
+    BenchmarkRunner.Run<GridFillerBenchmarks>();
+}
+
+static void RunFunctionalTests()
+{
+    Console.WriteLine("Running GridFiller Functional Tests...\n");
+
+    TestBasicGridFill();
+    TestObstaclePlacement();
+    TestObstacleRemoval();
+    TestWeightAdjustment();
+
+    Console.WriteLine("\n✅ All functional tests passed!");
+}
+
+static void TestBasicGridFill()
+{
+    Console.WriteLine("Test 1: Basic Grid Fill");
+    var filler = new GridFiller(100, 100);
+    
+    filler.FillGrid(fillAll: true);
+    
+    int filledCells = 0;
+    for (int y = 0; y < 100; y++)
+    {
+        for (int x = 0; x < 100; x++)
+        {
+            if (filler.Grid[y, x] > 0)
+                filledCells++;
+        }
+    }
+    
+    Console.WriteLine($"  Filled cells: {filledCells}/10000");
+    if (filledCells > 0) Console.WriteLine("  ✓ Passed\n");
+    else throw new Exception("Grid should have filled cells");
+}
+
+static void TestObstaclePlacement()
+{
+    Console.WriteLine("Test 2: Obstacle Placement");
+    var filler = new GridFiller(100, 100);
+    
+    var obstacleRect = new Rectangle(10, 10, 20, 20);
+    filler.PlaceObstacle(obstacleRect);
+    
+    int obstaclesPlaced = 0;
+    for (int y = 10; y < 30; y++)
+    {
+        for (int x = 10; x < 30; x++)
+        {
+            if (filler.Grid[y, x] < 0)
+                obstaclesPlaced++;
+        }
+    }
+    
+    Console.WriteLine($"  Obstacles placed: {obstaclesPlaced}/400");
+    if (obstaclesPlaced == 400) Console.WriteLine("  ✓ Passed\n");
+    else throw new Exception("Obstacle placement failed");
+}
+
+static void TestObstacleRemoval()
+{
+    Console.WriteLine("Test 3: Obstacle Removal");
+    var filler = new GridFiller(100, 100);
+    
+    var obstacleRect = new Rectangle(10, 10, 20, 20);
+    filler.PlaceObstacle(obstacleRect);
+    filler.RemoveObstacle(obstacleRect);
+    
+    int obstaclesRemaining = 0;
+    for (int y = 10; y < 30; y++)
+    {
+        for (int x = 10; x < 30; x++)
+        {
+            if (filler.Grid[y, x] < 0)
+                obstaclesRemaining++;
+        }
+    }
+    
+    Console.WriteLine($"  Obstacles remaining: {obstaclesRemaining}");
+    if (obstaclesRemaining == 0) Console.WriteLine("  ✓ Passed\n");
+    else throw new Exception("Obstacle removal failed");
+}
+
+static void TestWeightAdjustment()
+{
+    Console.WriteLine("Test 4: Weight Adjustment");
+    var filler = new GridFiller(100, 100);
+    
+    var points = new List<Point> { new(10, 10), new(11, 11) };
+    filler.SetTileWeights(points, 5);
+    
+    if (filler.WeightGrid[10, 10] != 5) throw new Exception("Weight at (10,10) should be 5");
+    if (filler.WeightGrid[11, 11] != 5) throw new Exception("Weight at (11,11) should be 5");
+    
+    filler.ResetTileWeights(points);
+    
+    if (filler.WeightGrid[10, 10] != 1) throw new Exception("Weight should be reset to 1");
+    if (filler.WeightGrid[11, 11] != 1) throw new Exception("Weight should be reset to 1");
+    Console.WriteLine("  ✓ Passed\n");
+}
+
+[MemoryDiagnoser]
+public class GridFillerBenchmarks
+{
+    private GridFiller gridFiller = null!;
+    private const int GridSize = 512;
+
+    [GlobalSetup]
+    public void Setup()
+    {
+        gridFiller = new GridFiller(GridSize, GridSize);
+    }
+
+    [Benchmark(Description = "FillGrid - Full grid")]
+    public void FillGridFull()
+    {
+        gridFiller.FillGrid(fillAll: true);
+    }
+
+    [Benchmark(Description = "FillGrid - Partial region")]
+    public void FillGridPartial()
+    {
+        gridFiller.FillGrid(0, 0, GridSize / 2, GridSize / 2);
+    }
+
+    [Benchmark(Description = "PlaceObstacle - Single rectangle")]
+    public void PlaceObstacleSingle()
+    {
+        var rect = new Rectangle(10, 10, 50, 50);
+        gridFiller.PlaceObstacle(rect);
+    }
+
+    [Benchmark(Description = "RemoveObstacle - Single rectangle")]
+    public void RemoveObstacleSingle()
+    {
+        var rect = new Rectangle(10, 10, 50, 50);
+        gridFiller.RemoveObstacle(rect);
+    }
+
+    [Benchmark(Description = "SetTileWeights - 100 tiles")]
+    public void SetTileWeights()
+    {
+        var points = new List<Point>();
+        for (int i = 0; i < 100; i++)
+        {
+            points.Add(new Point(i % 50, i / 50));
+        }
+        gridFiller.SetTileWeights(points, 2);
+    }
+
+    [Benchmark(Description = "ResetTileWeights - 100 tiles")]
+    public void ResetTileWeights()
+    {
+        var points = new List<Point>();
+        for (int i = 0; i < 100; i++)
+        {
+            points.Add(new Point(i % 50, i / 50));
+        }
+        gridFiller.ResetTileWeights(points);
+    }
+}
diff --git a/src/LargeGridPathfinding/GridFiller.cs b/src/LargeGridPathfinding/GridFiller.cs
index d244074..de4306a 100644
--- a/src/LargeGridPathfinding/GridFiller.cs
+++ b/src/LargeGridPathfinding/GridFiller.cs
@@ -9,7 +9,7 @@ using System.Threading.Tasks;
 
 namespace LargeGridPathfinding;
 
-internal class GridFiller
+public class GridFiller
 {
     private const int RecalculationRadius = 24;
     private readonly object mutationLock = new();
@@ -107,7 +107,8 @@ internal class GridFiller
                 Debug.WriteLine($"Candidates: {candidates.Count}");
                 Debug.WriteLine("Sorting candidates...");
 
-                List<(int x, int y, int w, int h, int weight)> sortedCandidates = [.. candidates];
+                var sortedCandidates = new List<(int x, int y, int w, int h, int weight)>(candidates.Count);
+                sortedCandidates.AddRange(candidates);
                 sortedCandidates.Sort((a, b) =>
                 {
                     int areaComparison = (b.w * b.h).CompareTo(a.w * a.h);
@@ -186,7 +187,8 @@ internal class GridFiller
 
             foreach (Rectangle rectangle in clampedRectangles)
             {
-                for (int dy = rectangle.Top; dy < rectangle.Bottom; dy++)
+                int endY = rectangle.Bottom;
+                for (int dy = rectangle.Top; dy < endY; dy++)
                 {
                     for (int dx = rectangle.Left; dx < rectangle.Right; dx++)
                     {
@@ -210,8 +212,9 @@ internal class GridFiller
                     foreach (Rectangle rectangle in clampedRectangles)
                     {
                         int obstacle = currentObstacleLabel--;
+                        int endY = rectangle.Bottom;
 
-                        for (int dy = rectangle.Top; dy < rectangle.Bottom; dy++)
+                        for (int dy = rectangle.Top; dy < endY; dy++)
                         {
                             for (int dx = rectangle.Left; dx < rectangle.Right; dx++)
                             {
@@ -256,7 +259,8 @@ internal class GridFiller
 
             foreach (Rectangle rectangle in clampedRectangles)
             {
-                for (int dy = rectangle.Top; dy < rectangle.Bottom; dy++)
+                int endY = rectangle.Bottom;
+                for (int dy = rectangle.Top; dy < endY; dy++)
                 {
                     for (int dx = rectangle.Left; dx < rectangle.Right; dx++)
                     {
@@ -279,7 +283,8 @@ internal class GridFiller
                 {
                     foreach (Rectangle rectangle in clampedRectangles)
                     {
-                        for (int dy = rectangle.Top; dy < rectangle.Bottom; dy++)
+                        int endY = rectangle.Bottom;
+                        for (int dy = rectangle.Top; dy < endY; dy++)
                         {
                             for (int dx = rectangle.Left; dx < rectangle.Right; dx++)
                             {
@@ -358,15 +363,17 @@ internal class GridFiller
 
     private bool IsAreaFree(int x, int y, int w, int h, int requiredWeight)
     {
+        int gridRow = y;
         for (int dy = 0; dy < h; dy++)
         {
             for (int dx = 0; dx < w; dx++)
             {
-                if (Grid[y + dy, x + dx] != 0 || WeightGrid[y + dy, x + dx] != requiredWeight)
+                if (Grid[gridRow, x + dx] != 0 || WeightGrid[gridRow, x + dx] != requiredWeight)
                 {
                     return false;
                 }
             }
+            gridRow++;
         }
         return true;
     }
@@ -416,6 +423,10 @@ internal class GridFiller
                 bestHeight = h;
                 bestStretch = stretch;
             }
+            else if (area < bestArea && h > bestHeight * 2)
+            {
+                break;
+            }
         }
 
         return (bestWidth, bestHeight);
@@ -424,12 +435,14 @@ internal class GridFiller
     private void PlaceRectangle(Rectangle rectangle)
     {
         int label = currentLabel++;
+        int endY = rectangle.Y + rectangle.Height;
+        int endX = rectangle.X + rectangle.Width;
 
-        for (int dy = 0; dy < rectangle.Height; dy++)
+        for (int dy = rectangle.Y; dy < endY; dy++)
         {
-            for (int dx = 0; dx < rectangle.Width; dx++)
+            for (int dx = rectangle.X; dx < endX; dx++)
             {
-                Grid[rectangle.Y + dy, rectangle.X + dx] = label;
+                Grid[dy, dx] = label;
             }
         }
 
@@ -443,11 +456,14 @@ internal class GridFiller
             return Rectangle.Empty;
         }
 
-        for (int dy = 0; dy < rectangle.Height; dy++)
+        int endY = rectangle.Y + rectangle.Height;
+        int endX = rectangle.X + rectangle.Width;
+
+        for (int dy = rectangle.Y; dy < endY; dy++)
         {
-            for (int dx = 0; dx < rectangle.Width; dx++)
+            for (int dx = rectangle.X; dx < endX; dx++)
             {
-                Grid[rectangle.Y + dy, rectangle.X + dx] = 0;
+                Grid[dy, dx] = 0;
             }
         }
 
diff --git a/src/LargeGridPathfinding/LargeGridPathfindingGame.cs b/src/LargeGridPathfinding/LargeGridPathfindingGame.cs
index 916c2ca..4274a0d 100644
--- a/src/LargeGridPathfinding/LargeGridPathfindingGame.cs
+++ b/src/LargeGridPathfinding/LargeGridPathfindingGame.cs
@@ -626,8 +626,8 @@ public class LargeGridPathfindingGame : Game
         _ = Task.Run(() =>
         {
             // Configuration options
-            int width = 1000;
-            int height = 1000;
+            int width = 10000;
+            int height = 10000;
             int agentCount = 10000;
 
             bool pathRandomization = false; // Randomize path costs to prevent agents from following the same path