From c45f3edcc4826d4e6e1273bfeb14977b587e3b92 Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Mon, 17 Nov 2025 04:49:30 +0000
Subject: [PATCH 01/16] Field generation

---
 src/test/path_plan_test.cc | 44 ++++++++++++++++++++++++++++++++++++++
 1 file changed, 44 insertions(+)
 create mode 100644 src/test/path_plan_test.cc

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
new file mode 100644
index 0000000..aa69ead
--- /dev/null
+++ b/src/test/path_plan_test.cc
@@ -0,0 +1,44 @@
+#include <vector>
+#include <iostream>
+
+const int WIDTH = 100;
+const int HEIGHT = 60;
+
+std::vector<std::vector<int>> createField() {
+    return std::vector<std::vector<int>>(HEIGHT, std::vector<int>(WIDTH, 0));
+}
+
+void addCircle(std::vector<std::vector<int>>& field, int cx, int cy, int radius) {
+    for (int i = 0; i < HEIGHT; i++) {
+        for (int j = 0; j < WIDTH; j++) {
+            int dx = j - cx;
+            int dy = i - cy;
+            if (dx*dx + dy*dy <= radius*radius) {
+                field[i][j] = 1;
+            }
+        }
+    }
+}
+
+void printField(std::vector<std::vector<int>>& field) {
+    for (auto row : field) {
+        for (int cell : row) {
+            if (cell == 0) {
+                std::cout << ".";
+            } else if (cell == 1) {
+                std::cout << "#";
+            }
+        }
+        std::cout << "\n";
+    }
+}
+
+int main() {
+    auto field = createField();
+
+    addCircle(field, 50, 30, 8);
+
+    printField(field);
+
+    return 0;   
+}
\ No newline at end of file

From b7e8661a912b3ed905089717290e0462d3a37651 Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Sat, 27 Dec 2025 23:32:21 +0000
Subject: [PATCH 02/16] Add bfs

---
 src/test/CMakeLists.txt    |   3 +
 src/test/path_plan_test.cc | 121 +++++++++++++++++++++++++++++--------
 2 files changed, 99 insertions(+), 25 deletions(-)

diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 6fc7f64..46179cf 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -18,3 +18,6 @@ target_link_libraries(timer_test PRIVATE utils)
 
 add_executable(stress_test stress_test.cc)
 target_link_libraries(stress_test PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo)
+
+add_executable(path_plan_test path_plan_test.cc)
+target_link_libraries(path_plan_test PRIVATE ${OpenCV_LIBS})
diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index aa69ead..f4a4634 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -1,44 +1,115 @@
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/opencv.hpp>
 #include <vector>
-#include <iostream>
+#include <queue>
+#include <cmath>
+#include <algorithm>
 
-const int WIDTH = 100;
-const int HEIGHT = 60;
+const int GRID_W = 10;
+const int GRID_H = 10;
+const int CELL_SIZE = 50;
 
-std::vector<std::vector<int>> createField() {
-    return std::vector<std::vector<int>>(HEIGHT, std::vector<int>(WIDTH, 0));
+class Node {
+public:
+    int x = 0;
+    int y = 0;
+    bool walkable = true;
+    Node* parent = nullptr;
+    float g = INFINITY;
+    cv::Scalar color = { 200, 200, 200 };
+};
+
+cv::Rect nodeRect(const Node& n) {
+    return cv::Rect(n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE);
 }
 
-void addCircle(std::vector<std::vector<int>>& field, int cx, int cy, int radius) {
-    for (int i = 0; i < HEIGHT; i++) {
-        for (int j = 0; j < WIDTH; j++) {
-            int dx = j - cx;
-            int dy = i - cy;
-            if (dx*dx + dy*dy <= radius*radius) {
-                field[i][j] = 1;
-            }
+void drawGrid(cv::Mat& canvas, const std::vector<std::vector<Node>>& grid) {
+    for (int y = 0; y < GRID_H; ++y) {
+        for (int x = 0; x < GRID_W; ++x) {
+            const Node& n = grid[y][x];
+            cv::rectangle(canvas, nodeRect(n), n.color, cv::FILLED);
+            cv::rectangle(canvas, nodeRect(n), { 50, 50, 50 }, 1);
         }
     }
 }
 
-void printField(std::vector<std::vector<int>>& field) {
-    for (auto row : field) {
-        for (int cell : row) {
-            if (cell == 0) {
-                std::cout << ".";
-            } else if (cell == 1) {
-                std::cout << "#";
+void makeObstacle(std::vector<std::vector<Node>>& grid, int row, int col) {
+    grid[row][col].walkable = false;
+    grid[row][col].color = { 0, 0, 0 };
+}
+
+std::vector<Node*> reconstructPath(Node* target) {
+    std::vector<Node*> path;
+    Node* current = target;
+    while (current != nullptr) {
+        path.push_back(current);
+        current = current->parent;
+    }
+    std::reverse(path.begin(), path.end());
+    return path;
+}
+
+void dijkstra(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
+    std::priority_queue<std::pair<float, Node*>, std::vector<std::pair<float, Node*>>, std::greater<std::pair<float, Node*>>> openList;
+    start->g = 0.0f;
+    start->color = { 0, 255, 0 };
+    target->color = { 0, 0, 255 };
+    openList.push(std::make_pair(start->g, start));
+
+    std::vector<std::pair<int, int>> dirs = { {0,-1},{0,1},{-1,0},{1,0},{-1,-1},{-1,1},{1,-1},{1,1} };
+
+    while (!openList.empty()) {
+        std::pair<float, Node*> currentPair = openList.top();
+        openList.pop();
+        Node* current = currentPair.second;
+        if (current == target) return;
+
+        for (const std::pair<int, int>& d : dirs) {
+            int nx = current->x + d.first;
+            int ny = current->y + d.second;
+            if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H) continue;
+            Node* neighbor = &grid[ny][nx];
+            if (!neighbor->walkable) continue;
+            float dx = static_cast<float>(d.first);
+            float dy = static_cast<float>(d.second);
+            float cost = std::sqrt(dx * dx + dy * dy);
+            float tentativeG = current->g + cost;
+            if (tentativeG < neighbor->g) {
+                neighbor->g = tentativeG;
+                neighbor->parent = current;
+                openList.push(std::make_pair(neighbor->g, neighbor));
             }
         }
-        std::cout << "\n";
     }
 }
 
 int main() {
-    auto field = createField();
+    std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
+    for (int y = 0; y < GRID_H; ++y) {
+        for (int x = 0; x < GRID_W; ++x) {
+            grid[y][x].x = x;
+            grid[y][x].y = y;
+        }
+    }
+
+    makeObstacle(grid, 2, 3);
+    makeObstacle(grid, 5, 6);
+    makeObstacle(grid, 7, 1);
+
+    Node* start = &grid[0][0];
+    Node* target = &grid[9][9];
 
-    addCircle(field, 50, 30, 8);
+    dijkstra(grid, start, target);
 
-    printField(field);
+    std::vector<Node*> path = reconstructPath(target);
+    for (Node* n : path)
+        if (n != start && n != target)
+            n->color = { 0, 0, 255 };
 
-    return 0;   
+    cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3, { 255, 255, 255 });
+    drawGrid(canvas, grid);
+    cv::imshow("DFS", canvas);
+    cv::waitKey(0);
+    return 0;
 }
\ No newline at end of file

From e437f1e01cccaa6a824a595cd6d0fe4c59f7df70 Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Sat, 3 Jan 2026 22:32:55 +0000
Subject: [PATCH 03/16] WPI path plan test

---
 src/test/path_plan_test.cc | 33 ++++++++++++++++++++++++++-------
 1 file changed, 26 insertions(+), 7 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index f4a4634..716e190 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -1,5 +1,5 @@
 #include <opencv2/core.hpp>
-#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
 #include <opencv2/opencv.hpp>
 #include <vector>
 #include <queue>
@@ -50,11 +50,11 @@ std::vector<Node*> reconstructPath(Node* target) {
     return path;
 }
 
-void dijkstra(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
+void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
     std::priority_queue<std::pair<float, Node*>, std::vector<std::pair<float, Node*>>, std::greater<std::pair<float, Node*>>> openList;
     start->g = 0.0f;
     start->color = { 0, 255, 0 };
-    target->color = { 0, 0, 255 };
+    target->color = { 255, 0, 0 };
     openList.push(std::make_pair(start->g, start));
 
     std::vector<std::pair<int, int>> dirs = { {0,-1},{0,1},{-1,0},{1,0},{-1,-1},{-1,1},{1,-1},{1,1} };
@@ -84,7 +84,20 @@ void dijkstra(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
     }
 }
 
+void constructLinePath(cv::Mat& canvas, std::vector<Node*> path) {
+    for (size_t i = 0; i < path.size() - 1; ++i) {
+        int x1 = path[i]->x * CELL_SIZE + CELL_SIZE / 2;
+        int y1 = path[i]->y * CELL_SIZE + CELL_SIZE / 2;
+        int x2 = path[i + 1]->x * CELL_SIZE + CELL_SIZE / 2;
+        int y2 = path[i + 1]->y * CELL_SIZE + CELL_SIZE / 2;
+        cv::line(canvas, cv::Point(x1, y1), cv::Point(x2, y2), {0, 0, 0}, 3);
+        
+    }
+}
+
 int main() {
+    std::cout << "hi" << std::endl;
+    
     std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
     for (int y = 0; y < GRID_H; ++y) {
         for (int x = 0; x < GRID_W; ++x) {
@@ -100,16 +113,22 @@ int main() {
     Node* start = &grid[0][0];
     Node* target = &grid[9][9];
 
-    dijkstra(grid, start, target);
+    BFS(grid, start, target);
 
     std::vector<Node*> path = reconstructPath(target);
-    for (Node* n : path)
-        if (n != start && n != target)
+    for (Node* n : path) {
+        if (n != start && n != target) {
             n->color = { 0, 0, 255 };
+        }
+    }
 
+    std::cout << "test" << std::endl;
+    std::cout.flush();
+    
     cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3, { 255, 255, 255 });
     drawGrid(canvas, grid);
-    cv::imshow("DFS", canvas);
+    constructLinePath(canvas, path);
+    cv::imshow("BFS", canvas);
     cv::waitKey(0);
     return 0;
 }
\ No newline at end of file

From 640bc0950d1d0c584d0eae4f96153416b7556d59 Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Sat, 3 Jan 2026 23:23:42 +0000
Subject: [PATCH 04/16] WIP definatly dont work

Signed-off-by: Nano <nanoticity@gmail.com>
---
 src/test/path_plan_test.cc | 23 ++++++++++++++++++-----
 1 file changed, 18 insertions(+), 5 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 716e190..585e35d 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -96,7 +96,6 @@ void constructLinePath(cv::Mat& canvas, std::vector<Node*> path) {
 }
 
 int main() {
-    std::cout << "hi" << std::endl;
     
     std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
     for (int y = 0; y < GRID_H; ++y) {
@@ -106,9 +105,26 @@ int main() {
         }
     }
 
+    makeObstacle(grid, 1, 3);
     makeObstacle(grid, 2, 3);
-    makeObstacle(grid, 5, 6);
+    makeObstacle(grid, 3, 3);
+    makeObstacle(grid, 4, 3);
+    makeObstacle(grid, 5, 3);
     makeObstacle(grid, 7, 1);
+    makeObstacle(grid, 7, 2);
+    makeObstacle(grid, 7, 3);
+    makeObstacle(grid, 7, 4);
+    makeObstacle(grid, 7, 5);
+    makeObstacle(grid, 7, 6);
+    makeObstacle(grid, 3, 6);
+    makeObstacle(grid, 4, 6);
+    makeObstacle(grid, 5, 6);
+    makeObstacle(grid, 6, 6);
+    makeObstacle(grid, 1, 8);
+    makeObstacle(grid, 2, 8);
+    makeObstacle(grid, 3, 8);
+    makeObstacle(grid, 5, 0);
+    makeObstacle(grid, 5, 1);
 
     Node* start = &grid[0][0];
     Node* target = &grid[9][9];
@@ -122,9 +138,6 @@ int main() {
         }
     }
 
-    std::cout << "test" << std::endl;
-    std::cout.flush();
-    
     cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3, { 255, 255, 255 });
     drawGrid(canvas, grid);
     constructLinePath(canvas, path);

From 7fb4d826a446e323d56ced81731cf7dfe911feaa Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Sun, 4 Jan 2026 15:49:35 +0000
Subject: [PATCH 05/16] first b-spline fit, I still have to add the iterative
 collision avoidance.

Signed-off-by: Nano <nanoticity@gmail.com>
---
 constants/navgrid.json     |   1 +
 src/test/CMakeLists.txt    |   2 +-
 src/test/path_plan_test.cc | 167 +++++++++++++++++++++++++++----------
 3 files changed, 124 insertions(+), 46 deletions(-)
 create mode 100644 constants/navgrid.json

diff --git a/constants/navgrid.json b/constants/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/constants/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 46179cf..d1e968c 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -20,4 +20,4 @@ add_executable(stress_test stress_test.cc)
 target_link_libraries(stress_test PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo)
 
 add_executable(path_plan_test path_plan_test.cc)
-target_link_libraries(path_plan_test PRIVATE ${OpenCV_LIBS})
+target_link_libraries(path_plan_test PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json)
diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 585e35d..0fcba55 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -1,25 +1,33 @@
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/opencv.hpp>
+#include <ostream>
 #include <vector>
 #include <queue>
 #include <cmath>
 #include <algorithm>
-
-const int GRID_W = 10;
-const int GRID_H = 10;
-const int CELL_SIZE = 50;
+#include <fstream>
+#include <nlohmann/json.hpp>
 
 class Node {
-public:
+    public:
     int x = 0;
     int y = 0;
     bool walkable = true;
     Node* parent = nullptr;
-    float g = INFINITY;
+    double g = INFINITY;
     cv::Scalar color = { 200, 200, 200 };
 };
 
+using json = nlohmann::json;
+std::ifstream file("/root/bos/constants/navgrid.json");
+json data = json::parse(file);
+
+const int GRID_W = data["grid"][0].size();
+const int GRID_H = data["grid"].size();
+const int CELL_SIZE = 20;
+std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
+
 cv::Rect nodeRect(const Node& n) {
     return cv::Rect(n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE);
 }
@@ -51,7 +59,7 @@ std::vector<Node*> reconstructPath(Node* target) {
 }
 
 void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
-    std::priority_queue<std::pair<float, Node*>, std::vector<std::pair<float, Node*>>, std::greater<std::pair<float, Node*>>> openList;
+    std::priority_queue<std::pair<double, Node*>, std::vector<std::pair<double, Node*>>, std::greater<std::pair<double, Node*>>> openList;
     start->g = 0.0f;
     start->color = { 0, 255, 0 };
     target->color = { 255, 0, 0 };
@@ -60,7 +68,7 @@ void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
     std::vector<std::pair<int, int>> dirs = { {0,-1},{0,1},{-1,0},{1,0},{-1,-1},{-1,1},{1,-1},{1,1} };
 
     while (!openList.empty()) {
-        std::pair<float, Node*> currentPair = openList.top();
+        std::pair<double, Node*> currentPair = openList.top();
         openList.pop();
         Node* current = currentPair.second;
         if (current == target) return;
@@ -71,10 +79,10 @@ void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
             if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H) continue;
             Node* neighbor = &grid[ny][nx];
             if (!neighbor->walkable) continue;
-            float dx = static_cast<float>(d.first);
-            float dy = static_cast<float>(d.second);
-            float cost = std::sqrt(dx * dx + dy * dy);
-            float tentativeG = current->g + cost;
+            double dx = static_cast<double>(d.first);
+            double dy = static_cast<double>(d.second);
+            double cost = std::sqrt(dx * dx + dy * dy);
+            double tentativeG = current->g + cost;
             if (tentativeG < neighbor->g) {
                 neighbor->g = tentativeG;
                 neighbor->parent = current;
@@ -84,50 +92,117 @@ void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
     }
 }
 
-void constructLinePath(cv::Mat& canvas, std::vector<Node*> path) {
-    for (size_t i = 0; i < path.size() - 1; ++i) {
-        int x1 = path[i]->x * CELL_SIZE + CELL_SIZE / 2;
-        int y1 = path[i]->y * CELL_SIZE + CELL_SIZE / 2;
-        int x2 = path[i + 1]->x * CELL_SIZE + CELL_SIZE / 2;
-        int y2 = path[i + 1]->y * CELL_SIZE + CELL_SIZE / 2;
-        cv::line(canvas, cv::Point(x1, y1), cv::Point(x2, y2), {0, 0, 0}, 3);
+std::vector<std::pair<int, int>> constructLinePath(cv::Mat& canvas, std::vector<Node*> path) {
+    std::vector<std::pair<int, int>> controlPoints;
+    for (size_t i = 0; i < path.size(); ++i) {
+        int px = path[i]->x * CELL_SIZE + (CELL_SIZE / 2);
+        int py = path[i]->y * CELL_SIZE + (CELL_SIZE / 2);
         
+        controlPoints.push_back({px, py});
+
+        if (i > 0) {
+            int prevX = path[i-1]->x * CELL_SIZE + (CELL_SIZE / 2);
+            int prevY = path[i-1]->y * CELL_SIZE + (CELL_SIZE / 2);
+            cv::line(canvas, cv::Point(prevX, prevY), cv::Point(px, py), {0, 0, 0}, 2);
+        }
     }
+    return controlPoints;
 }
 
-int main() {
+
+std::vector<double> clampedUniformKnotVector(double k, double p) {
+    std::vector<double> knots;
+    int n = (int)k; 
+    int d = (int)p;
+
+    for (int i = 0; i <= d; ++i) {
+        knots.push_back(0.0);
+    }
+
+    int middle = n - d - 1;
+    for (int i = 1; i <= middle; ++i) {
+        knots.push_back((double)i / (middle + 1));
+    }
+
+    for (int i = 0; i <= d; ++i) {
+        knots.push_back(1.0);
+    }
+
+    return knots;
+}
+
+
+double basisFunction(double i, double p, double t, const std::vector<double>& knots) {
+    int idx = static_cast<int>(i);
+    int deg = static_cast<int>(p);
     
-    std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
+    if (deg == 0) {
+        if (knots[idx] <= t && t < knots[idx + 1]) {
+            return 1.0;
+        }
+        if (t == 1.0 && idx == (int) knots.size() - deg - 2){
+            return 1.0;
+        }
+        
+        return 0.0;
+    }
+
+    double weight = 0.0;
+
+    double denom1 = knots[idx + deg] - knots[idx];
+    if (denom1 != 0) {
+        weight += ((t - knots[idx]) / denom1) * basisFunction(i, p - 1, t, knots);
+    }
+    double denom2 = knots[idx + deg + 1] - knots[idx + 1];
+    if (denom2 != 0) {
+        weight += ((knots[idx + deg + 1] - t) / denom2) * basisFunction(i + 1, p - 1, t, knots);
+    }
+    return weight;
+}
+
+std::pair<double, double> getSplinePoint(double t, const std::vector<std::pair<int, int>>& points, const std::vector<double>& knots, int p) {
+    double px = 0.0, py = 0.0;
+    for (size_t i = 0; i < points.size(); ++i) {
+        double weight = basisFunction((double)i, (double)p, t, knots);
+        px += points[i].first * weight;
+        py += points[i].second * weight;
+    }
+    return {px, py};
+}
+
+void drawSpline(cv::Mat& canvas, const std::vector<std::pair<int, int>>& points, std::vector<double>& knots, int p) { 
+    int res = 200;
+    std::pair<double, double> prevPoint = {static_cast<double>(points[0].first), static_cast<double>(points[0].second)};
+
+    for (int i = 1; i < res; ++i) {
+        double t = (double)i / res;
+        std::pair<double, double> currPoint = getSplinePoint(t, points, knots, p);
+
+
+        cv::line(canvas, cv::Point(prevPoint.first, prevPoint.second), cv::Point(currPoint.first, currPoint.second), cv::Scalar(255, 0, 0), 2);
+
+        
+        prevPoint = currPoint;
+    }
+
+}
+
+int main() {
+    std::vector<std::vector<bool>> navgrid = data["grid"];
+
     for (int y = 0; y < GRID_H; ++y) {
         for (int x = 0; x < GRID_W; ++x) {
             grid[y][x].x = x;
             grid[y][x].y = y;
+            if (static_cast<bool>(navgrid[y][x])) {
+                makeObstacle(grid, y, x);
+            }
         }
     }
 
-    makeObstacle(grid, 1, 3);
-    makeObstacle(grid, 2, 3);
-    makeObstacle(grid, 3, 3);
-    makeObstacle(grid, 4, 3);
-    makeObstacle(grid, 5, 3);
-    makeObstacle(grid, 7, 1);
-    makeObstacle(grid, 7, 2);
-    makeObstacle(grid, 7, 3);
-    makeObstacle(grid, 7, 4);
-    makeObstacle(grid, 7, 5);
-    makeObstacle(grid, 7, 6);
-    makeObstacle(grid, 3, 6);
-    makeObstacle(grid, 4, 6);
-    makeObstacle(grid, 5, 6);
-    makeObstacle(grid, 6, 6);
-    makeObstacle(grid, 1, 8);
-    makeObstacle(grid, 2, 8);
-    makeObstacle(grid, 3, 8);
-    makeObstacle(grid, 5, 0);
-    makeObstacle(grid, 5, 1);
-
-    Node* start = &grid[0][0];
-    Node* target = &grid[9][9];
+
+    Node* start = &grid[std::min(13, GRID_H - 1)][std::min(5, GRID_W - 1)];
+    Node* target = &grid[std::min(13, GRID_H - 1)][std::min(25, GRID_W - 1)];
 
     BFS(grid, start, target);
 
@@ -140,7 +215,9 @@ int main() {
 
     cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3, { 255, 255, 255 });
     drawGrid(canvas, grid);
-    constructLinePath(canvas, path);
+    std::vector<std::pair<int, int>> controlPoints = constructLinePath(canvas, path);
+    std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3);
+    drawSpline(canvas, controlPoints, knots, 3);
     cv::imshow("BFS", canvas);
     cv::waitKey(0);
     return 0;

From a94faaa649e0dafdafb21e3c66b940b28bf9435c Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Sun, 4 Jan 2026 16:48:33 +0000
Subject: [PATCH 06/16] add some more logic

Signed-off-by: Nano <nanoticity@gmail.com>
---
 src/test/path_plan_test.cc | 21 +++++++++++++--------
 1 file changed, 13 insertions(+), 8 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 0fcba55..959d64c 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -137,14 +137,19 @@ double basisFunction(double i, double p, double t, const std::vector<double>& kn
     int deg = static_cast<int>(p);
     
     if (deg == 0) {
-        if (knots[idx] <= t && t < knots[idx + 1]) {
-            return 1.0;
-        }
-        if (t == 1.0 && idx == (int) knots.size() - deg - 2){
-            return 1.0;
-        }
-        
-        return 0.0;
+        if (p == 0) {
+            if (knots[i] <= t && t < knots[i + 1]) return 1.0;
+            
+            if (t == 0.0 && knots[i] == 0.0 && knots[i+1] > 0.0) {
+                return 1.0;
+            }
+
+            if (t == 1.0 && knots[i+1] == 1.0 && knots[i] < 1.0) {
+                return 1.0;
+            }
+
+            return 0.0;
+}
     }
 
     double weight = 0.0;

From 14e5787fc52502fcd28aad22e6e160be1fb74e9d Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Sun, 4 Jan 2026 18:57:25 +0000
Subject: [PATCH 07/16] tidy the code with clang-tidy

Signed-off-by: Nano <nanoticity@gmail.com>
---
 src/test/path_plan_test.cc | 24 ++++++++++++------------
 1 file changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 959d64c..275eda5 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -28,8 +28,8 @@ const int GRID_H = data["grid"].size();
 const int CELL_SIZE = 20;
 std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
 
-cv::Rect nodeRect(const Node& n) {
-    return cv::Rect(n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE);
+auto nodeRect(const Node& n) -> cv::Rect {
+    return {n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE};
 }
 
 void drawGrid(cv::Mat& canvas, const std::vector<std::vector<Node>>& grid) {
@@ -47,7 +47,7 @@ void makeObstacle(std::vector<std::vector<Node>>& grid, int row, int col) {
     grid[row][col].color = { 0, 0, 0 };
 }
 
-std::vector<Node*> reconstructPath(Node* target) {
+auto reconstructPath(Node* target) -> std::vector<Node*> {
     std::vector<Node*> path;
     Node* current = target;
     while (current != nullptr) {
@@ -59,7 +59,7 @@ std::vector<Node*> reconstructPath(Node* target) {
 }
 
 void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
-    std::priority_queue<std::pair<double, Node*>, std::vector<std::pair<double, Node*>>, std::greater<std::pair<double, Node*>>> openList;
+    std::priority_queue<std::pair<double, Node*>, std::vector<std::pair<double, Node*>>, std::greater<>> openList;
     start->g = 0.0f;
     start->color = { 0, 255, 0 };
     target->color = { 255, 0, 0 };
@@ -79,8 +79,8 @@ void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
             if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H) continue;
             Node* neighbor = &grid[ny][nx];
             if (!neighbor->walkable) continue;
-            double dx = static_cast<double>(d.first);
-            double dy = static_cast<double>(d.second);
+            auto dx = static_cast<double>(d.first);
+            auto dy = static_cast<double>(d.second);
             double cost = std::sqrt(dx * dx + dy * dy);
             double tentativeG = current->g + cost;
             if (tentativeG < neighbor->g) {
@@ -92,13 +92,13 @@ void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
     }
 }
 
-std::vector<std::pair<int, int>> constructLinePath(cv::Mat& canvas, std::vector<Node*> path) {
+auto constructLinePath(cv::Mat& canvas, std::vector<Node*> path) -> std::vector<std::pair<int, int>> {
     std::vector<std::pair<int, int>> controlPoints;
     for (size_t i = 0; i < path.size(); ++i) {
         int px = path[i]->x * CELL_SIZE + (CELL_SIZE / 2);
         int py = path[i]->y * CELL_SIZE + (CELL_SIZE / 2);
         
-        controlPoints.push_back({px, py});
+        controlPoints.emplace_back(px, py);
 
         if (i > 0) {
             int prevX = path[i-1]->x * CELL_SIZE + (CELL_SIZE / 2);
@@ -110,7 +110,7 @@ std::vector<std::pair<int, int>> constructLinePath(cv::Mat& canvas, std::vector<
 }
 
 
-std::vector<double> clampedUniformKnotVector(double k, double p) {
+auto clampedUniformKnotVector(double k, double p) -> std::vector<double> {
     std::vector<double> knots;
     int n = (int)k; 
     int d = (int)p;
@@ -132,7 +132,7 @@ std::vector<double> clampedUniformKnotVector(double k, double p) {
 }
 
 
-double basisFunction(double i, double p, double t, const std::vector<double>& knots) {
+auto basisFunction(double i, double p, double t, const std::vector<double>& knots) -> double {
     int idx = static_cast<int>(i);
     int deg = static_cast<int>(p);
     
@@ -165,7 +165,7 @@ double basisFunction(double i, double p, double t, const std::vector<double>& kn
     return weight;
 }
 
-std::pair<double, double> getSplinePoint(double t, const std::vector<std::pair<int, int>>& points, const std::vector<double>& knots, int p) {
+auto getSplinePoint(double t, const std::vector<std::pair<int, int>>& points, const std::vector<double>& knots, int p) -> std::pair<double, double> {
     double px = 0.0, py = 0.0;
     for (size_t i = 0; i < points.size(); ++i) {
         double weight = basisFunction((double)i, (double)p, t, knots);
@@ -192,7 +192,7 @@ void drawSpline(cv::Mat& canvas, const std::vector<std::pair<int, int>>& points,
 
 }
 
-int main() {
+auto main() -> int {
     std::vector<std::vector<bool>> navgrid = data["grid"];
 
     for (int y = 0; y < GRID_H; ++y) {

From 47ec637d6cbd1b2ab7fe1e1c4f79fb936cddeb91 Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Sun, 4 Jan 2026 21:02:35 +0000
Subject: [PATCH 08/16] Format document nicely

---
 src/test/path_plan_test.cc | 339 +++++++++++++++++++------------------
 1 file changed, 176 insertions(+), 163 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 275eda5..a7b7b99 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -1,22 +1,22 @@
+#include <algorithm>
+#include <cmath>
+#include <fstream>
+#include <nlohmann/json.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/opencv.hpp>
 #include <ostream>
-#include <vector>
 #include <queue>
-#include <cmath>
-#include <algorithm>
-#include <fstream>
-#include <nlohmann/json.hpp>
+#include <vector>
 
 class Node {
-    public:
-    int x = 0;
-    int y = 0;
-    bool walkable = true;
-    Node* parent = nullptr;
-    double g = INFINITY;
-    cv::Scalar color = { 200, 200, 200 };
+ public:
+  int x = 0;
+  int y = 0;
+  bool walkable = true;
+  Node* parent = nullptr;
+  double g = INFINITY;
+  cv::Scalar color = {200, 200, 200};
 };
 
 using json = nlohmann::json;
@@ -29,201 +29,214 @@ const int CELL_SIZE = 20;
 std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
 
 auto nodeRect(const Node& n) -> cv::Rect {
-    return {n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE};
+  return {n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE};
 }
 
 void drawGrid(cv::Mat& canvas, const std::vector<std::vector<Node>>& grid) {
-    for (int y = 0; y < GRID_H; ++y) {
-        for (int x = 0; x < GRID_W; ++x) {
-            const Node& n = grid[y][x];
-            cv::rectangle(canvas, nodeRect(n), n.color, cv::FILLED);
-            cv::rectangle(canvas, nodeRect(n), { 50, 50, 50 }, 1);
-        }
+  for (int y = 0; y < GRID_H; ++y) {
+    for (int x = 0; x < GRID_W; ++x) {
+      const Node& n = grid[y][x];
+      cv::rectangle(canvas, nodeRect(n), n.color, cv::FILLED);
+      cv::rectangle(canvas, nodeRect(n), {50, 50, 50}, 1);
     }
+  }
 }
 
 void makeObstacle(std::vector<std::vector<Node>>& grid, int row, int col) {
-    grid[row][col].walkable = false;
-    grid[row][col].color = { 0, 0, 0 };
+  grid[row][col].walkable = false;
+  grid[row][col].color = {0, 0, 0};
 }
 
 auto reconstructPath(Node* target) -> std::vector<Node*> {
-    std::vector<Node*> path;
-    Node* current = target;
-    while (current != nullptr) {
-        path.push_back(current);
-        current = current->parent;
-    }
-    std::reverse(path.begin(), path.end());
-    return path;
+  std::vector<Node*> path;
+  Node* current = target;
+  while (current != nullptr) {
+    path.push_back(current);
+    current = current->parent;
+  }
+  std::reverse(path.begin(), path.end());
+  return path;
 }
 
 void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
-    std::priority_queue<std::pair<double, Node*>, std::vector<std::pair<double, Node*>>, std::greater<>> openList;
-    start->g = 0.0f;
-    start->color = { 0, 255, 0 };
-    target->color = { 255, 0, 0 };
-    openList.push(std::make_pair(start->g, start));
-
-    std::vector<std::pair<int, int>> dirs = { {0,-1},{0,1},{-1,0},{1,0},{-1,-1},{-1,1},{1,-1},{1,1} };
-
-    while (!openList.empty()) {
-        std::pair<double, Node*> currentPair = openList.top();
-        openList.pop();
-        Node* current = currentPair.second;
-        if (current == target) return;
-
-        for (const std::pair<int, int>& d : dirs) {
-            int nx = current->x + d.first;
-            int ny = current->y + d.second;
-            if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H) continue;
-            Node* neighbor = &grid[ny][nx];
-            if (!neighbor->walkable) continue;
-            auto dx = static_cast<double>(d.first);
-            auto dy = static_cast<double>(d.second);
-            double cost = std::sqrt(dx * dx + dy * dy);
-            double tentativeG = current->g + cost;
-            if (tentativeG < neighbor->g) {
-                neighbor->g = tentativeG;
-                neighbor->parent = current;
-                openList.push(std::make_pair(neighbor->g, neighbor));
-            }
-        }
+  std::priority_queue<std::pair<double, Node*>,
+                      std::vector<std::pair<double, Node*>>, std::greater<>>
+      openList;
+  start->g = 0.0f;
+  start->color = {0, 255, 0};
+  target->color = {255, 0, 0};
+  openList.push(std::make_pair(start->g, start));
+
+  std::vector<std::pair<int, int>> dirs = {{0, -1},  {0, 1},  {-1, 0}, {1, 0},
+                                           {-1, -1}, {-1, 1}, {1, -1}, {1, 1}};
+
+  while (!openList.empty()) {
+    std::pair<double, Node*> currentPair = openList.top();
+    openList.pop();
+    Node* current = currentPair.second;
+    if (current == target)
+      return;
+
+    for (const std::pair<int, int>& d : dirs) {
+      int nx = current->x + d.first;
+      int ny = current->y + d.second;
+      if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H)
+        continue;
+      Node* neighbor = &grid[ny][nx];
+      if (!neighbor->walkable)
+        continue;
+      auto dx = static_cast<double>(d.first);
+      auto dy = static_cast<double>(d.second);
+      double cost = std::sqrt(dx * dx + dy * dy);
+      double tentativeG = current->g + cost;
+      if (tentativeG < neighbor->g) {
+        neighbor->g = tentativeG;
+        neighbor->parent = current;
+        openList.push(std::make_pair(neighbor->g, neighbor));
+      }
     }
+  }
 }
 
-auto constructLinePath(cv::Mat& canvas, std::vector<Node*> path) -> std::vector<std::pair<int, int>> {
-    std::vector<std::pair<int, int>> controlPoints;
-    for (size_t i = 0; i < path.size(); ++i) {
-        int px = path[i]->x * CELL_SIZE + (CELL_SIZE / 2);
-        int py = path[i]->y * CELL_SIZE + (CELL_SIZE / 2);
-        
-        controlPoints.emplace_back(px, py);
-
-        if (i > 0) {
-            int prevX = path[i-1]->x * CELL_SIZE + (CELL_SIZE / 2);
-            int prevY = path[i-1]->y * CELL_SIZE + (CELL_SIZE / 2);
-            cv::line(canvas, cv::Point(prevX, prevY), cv::Point(px, py), {0, 0, 0}, 2);
-        }
+auto constructLinePath(cv::Mat& canvas, std::vector<Node*> path)
+    -> std::vector<std::pair<int, int>> {
+  std::vector<std::pair<int, int>> controlPoints;
+  for (size_t i = 0; i < path.size(); ++i) {
+    int px = path[i]->x * CELL_SIZE + (CELL_SIZE / 2);
+    int py = path[i]->y * CELL_SIZE + (CELL_SIZE / 2);
+
+    controlPoints.emplace_back(px, py);
+
+    if (i > 0) {
+      int prevX = path[i - 1]->x * CELL_SIZE + (CELL_SIZE / 2);
+      int prevY = path[i - 1]->y * CELL_SIZE + (CELL_SIZE / 2);
+      cv::line(canvas, cv::Point(prevX, prevY), cv::Point(px, py), {0, 0, 0},
+               2);
     }
-    return controlPoints;
+  }
+  return controlPoints;
 }
 
-
 auto clampedUniformKnotVector(double k, double p) -> std::vector<double> {
-    std::vector<double> knots;
-    int n = (int)k; 
-    int d = (int)p;
+  std::vector<double> knots;
+  int n = (int)k;
+  int d = (int)p;
 
-    for (int i = 0; i <= d; ++i) {
-        knots.push_back(0.0);
-    }
+  for (int i = 0; i <= d; ++i) {
+    knots.push_back(0.0);
+  }
 
-    int middle = n - d - 1;
-    for (int i = 1; i <= middle; ++i) {
-        knots.push_back((double)i / (middle + 1));
-    }
+  int middle = n - d - 1;
+  for (int i = 1; i <= middle; ++i) {
+    knots.push_back((double)i / (middle + 1));
+  }
 
-    for (int i = 0; i <= d; ++i) {
-        knots.push_back(1.0);
-    }
+  for (int i = 0; i <= d; ++i) {
+    knots.push_back(1.0);
+  }
 
-    return knots;
+  return knots;
 }
 
+auto basisFunction(double i, double p, double t,
+                   const std::vector<double>& knots) -> double {
+  int idx = static_cast<int>(i);
+  int deg = static_cast<int>(p);
 
-auto basisFunction(double i, double p, double t, const std::vector<double>& knots) -> double {
-    int idx = static_cast<int>(i);
-    int deg = static_cast<int>(p);
-    
-    if (deg == 0) {
-        if (p == 0) {
-            if (knots[i] <= t && t < knots[i + 1]) return 1.0;
-            
-            if (t == 0.0 && knots[i] == 0.0 && knots[i+1] > 0.0) {
-                return 1.0;
-            }
+  if (deg == 0) {
+    if (p == 0) {
+      if (knots[i] <= t && t < knots[i + 1])
+        return 1.0;
 
-            if (t == 1.0 && knots[i+1] == 1.0 && knots[i] < 1.0) {
-                return 1.0;
-            }
+      if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
+        return 1.0;
+      }
 
-            return 0.0;
-}
-    }
-
-    double weight = 0.0;
+      if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
+        return 1.0;
+      }
 
-    double denom1 = knots[idx + deg] - knots[idx];
-    if (denom1 != 0) {
-        weight += ((t - knots[idx]) / denom1) * basisFunction(i, p - 1, t, knots);
-    }
-    double denom2 = knots[idx + deg + 1] - knots[idx + 1];
-    if (denom2 != 0) {
-        weight += ((knots[idx + deg + 1] - t) / denom2) * basisFunction(i + 1, p - 1, t, knots);
+      return 0.0;
     }
-    return weight;
+  }
+
+  double weight = 0.0;
+
+  double denom1 = knots[idx + deg] - knots[idx];
+  if (denom1 != 0) {
+    weight += ((t - knots[idx]) / denom1) * basisFunction(i, p - 1, t, knots);
+  }
+  double denom2 = knots[idx + deg + 1] - knots[idx + 1];
+  if (denom2 != 0) {
+    weight += ((knots[idx + deg + 1] - t) / denom2) *
+              basisFunction(i + 1, p - 1, t, knots);
+  }
+  return weight;
 }
 
-auto getSplinePoint(double t, const std::vector<std::pair<int, int>>& points, const std::vector<double>& knots, int p) -> std::pair<double, double> {
-    double px = 0.0, py = 0.0;
-    for (size_t i = 0; i < points.size(); ++i) {
-        double weight = basisFunction((double)i, (double)p, t, knots);
-        px += points[i].first * weight;
-        py += points[i].second * weight;
-    }
-    return {px, py};
+auto getSplinePoint(double t, const std::vector<std::pair<int, int>>& points,
+                    const std::vector<double>& knots, int p)
+    -> std::pair<double, double> {
+  double px = 0.0, py = 0.0;
+  for (size_t i = 0; i < points.size(); ++i) {
+    double weight = basisFunction((double)i, (double)p, t, knots);
+    px += points[i].first * weight;
+    py += points[i].second * weight;
+  }
+  return {px, py};
 }
 
-void drawSpline(cv::Mat& canvas, const std::vector<std::pair<int, int>>& points, std::vector<double>& knots, int p) { 
-    int res = 200;
-    std::pair<double, double> prevPoint = {static_cast<double>(points[0].first), static_cast<double>(points[0].second)};
-
-    for (int i = 1; i < res; ++i) {
-        double t = (double)i / res;
-        std::pair<double, double> currPoint = getSplinePoint(t, points, knots, p);
-
+void drawSpline(cv::Mat& canvas, const std::vector<std::pair<int, int>>& points,
+                std::vector<double>& knots, int p) {
+  int res = 200;
+  std::pair<double, double> prevPoint = {static_cast<double>(points[0].first),
+                                         static_cast<double>(points[0].second)};
 
-        cv::line(canvas, cv::Point(prevPoint.first, prevPoint.second), cv::Point(currPoint.first, currPoint.second), cv::Scalar(255, 0, 0), 2);
+  for (int i = 1; i < res; ++i) {
+    double t = (double)i / res;
+    std::pair<double, double> currPoint = getSplinePoint(t, points, knots, p);
 
-        
-        prevPoint = currPoint;
-    }
+    cv::line(canvas, cv::Point(prevPoint.first, prevPoint.second),
+             cv::Point(currPoint.first, currPoint.second),
+             cv::Scalar(255, 0, 0), 2);
 
+    prevPoint = currPoint;
+  }
 }
 
 auto main() -> int {
-    std::vector<std::vector<bool>> navgrid = data["grid"];
-
-    for (int y = 0; y < GRID_H; ++y) {
-        for (int x = 0; x < GRID_W; ++x) {
-            grid[y][x].x = x;
-            grid[y][x].y = y;
-            if (static_cast<bool>(navgrid[y][x])) {
-                makeObstacle(grid, y, x);
-            }
-        }
+  std::vector<std::vector<bool>> navgrid = data["grid"];
+
+  for (int y = 0; y < GRID_H; ++y) {
+    for (int x = 0; x < GRID_W; ++x) {
+      grid[y][x].x = x;
+      grid[y][x].y = y;
+      if (static_cast<bool>(navgrid[y][x])) {
+        makeObstacle(grid, y, x);
+      }
     }
+  }
 
+  Node* start = &grid[std::min(13, GRID_H - 1)][std::min(5, GRID_W - 1)];
+  Node* target = &grid[std::min(13, GRID_H - 1)][std::min(25, GRID_W - 1)];
 
-    Node* start = &grid[std::min(13, GRID_H - 1)][std::min(5, GRID_W - 1)];
-    Node* target = &grid[std::min(13, GRID_H - 1)][std::min(25, GRID_W - 1)];
-
-    BFS(grid, start, target);
+  BFS(grid, start, target);
 
-    std::vector<Node*> path = reconstructPath(target);
-    for (Node* n : path) {
-        if (n != start && n != target) {
-            n->color = { 0, 0, 255 };
-        }
+  std::vector<Node*> path = reconstructPath(target);
+  for (Node* n : path) {
+    if (n != start && n != target) {
+      n->color = {0, 0, 255};
     }
-
-    cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3, { 255, 255, 255 });
-    drawGrid(canvas, grid);
-    std::vector<std::pair<int, int>> controlPoints = constructLinePath(canvas, path);
-    std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3);
-    drawSpline(canvas, controlPoints, knots, 3);
-    cv::imshow("BFS", canvas);
-    cv::waitKey(0);
-    return 0;
+  }
+
+  cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3,
+                 {255, 255, 255});
+  drawGrid(canvas, grid);
+  std::vector<std::pair<int, int>> controlPoints =
+      constructLinePath(canvas, path);
+  std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3);
+  drawSpline(canvas, controlPoints, knots, 3);
+  cv::imshow("BFS", canvas);
+  cv::waitKey(0);
+  return 0;
 }
\ No newline at end of file

From ded78d197a74fc373e5f20de4a4486356d8857fd Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Mon, 5 Jan 2026 23:11:53 +0000
Subject: [PATCH 09/16] Add logging and advantagescope support

---
 src/test/CMakeLists.txt    |  2 +-
 src/test/path_plan_test.cc | 44 +++++++++++++++++++++++++++++++++-----
 2 files changed, 40 insertions(+), 6 deletions(-)

diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index f387035..8046f2f 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -20,7 +20,7 @@ add_executable(stress_test stress_test.cc)
 target_link_libraries(stress_test PRIVATE ${OpenCV_LIBS} apriltag ntcore camera nlohmann_json::nlohmann_json Eigen3::Eigen localization utils yolo)
 
 add_executable(path_plan_test path_plan_test.cc)
-target_link_libraries(path_plan_test PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json)
+target_link_libraries(path_plan_test PRIVATE ${OpenCV_LIBS} nlohmann_json::nlohmann_json ntcore wpilibc wpimath)
 
 add_executable(gamepiece_test gamepiece_test.cc)
 target_link_libraries(gamepiece_test PRIVATE gamepiece yolo camera utils localization)
diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index a7b7b99..6f8516f 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -1,3 +1,4 @@
+#include <iostream>
 #include <algorithm>
 #include <cmath>
 #include <fstream>
@@ -8,6 +9,12 @@
 #include <ostream>
 #include <queue>
 #include <vector>
+#include <frc/DataLogManager.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructArrayTopic.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <frc/geometry/Pose2d.h>
 
 class Node {
  public:
@@ -19,13 +26,12 @@ class Node {
   cv::Scalar color = {200, 200, 200};
 };
 
-using json = nlohmann::json;
-std::ifstream file("/root/bos/constants/navgrid.json");
-json data = json::parse(file);
+std::ifstream file("/bos/constants/navgrid.json");
+nlohmann::json data = nlohmann::json::parse(file);
 
 const int GRID_W = data["grid"][0].size();
 const int GRID_H = data["grid"].size();
-const int CELL_SIZE = 20;
+const int CELL_SIZE = 20; 
 std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
 
 auto nodeRect(const Node& n) -> cv::Rect {
@@ -205,6 +211,13 @@ void drawSpline(cv::Mat& canvas, const std::vector<std::pair<int, int>>& points,
 }
 
 auto main() -> int {
+  nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+  inst.StopClient();
+  inst.StopLocal();
+  inst.StartClient4("path_planning");
+  inst.SetServerTeam(971);
+  frc::DataLogManager::Start("/bos/logs/");
+
   std::vector<std::vector<bool>> navgrid = data["grid"];
 
   for (int y = 0; y < GRID_H; ++y) {
@@ -236,7 +249,28 @@ auto main() -> int {
       constructLinePath(canvas, path);
   std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3);
   drawSpline(canvas, controlPoints, knots, 3);
-  cv::imshow("BFS", canvas);
+
+  std::shared_ptr<nt::NetworkTable> table = inst.GetTable("PathPlanning");
+  nt::StructArrayTopic<frc::Pose2d> trajectory_topic =
+      table->GetStructArrayTopic<frc::Pose2d>("Trajectory");
+  nt::StructArrayPublisher<frc::Pose2d> trajectory_publisher =
+      trajectory_topic.Publish();
+
+  std::vector<frc::Pose2d> trajectory;
+  int resolution = 200;
+  for (int i = 0; i < resolution; ++i) {
+    double t = (double)i / resolution;
+    std::pair<double, double> point = getSplinePoint(t, controlPoints, knots, 3);
+    double px = point.first / CELL_SIZE;
+    double py = point.second / CELL_SIZE;
+    trajectory.push_back(
+        frc::Pose2d(units::meter_t{px}, units::meter_t{py}, units::radian_t{0.0}));
+  }
+  trajectory_publisher.Set(trajectory);
+  inst.Flush();
+
+  cv::imshow("Path Planning", canvas);
   cv::waitKey(0);
+
   return 0;
 }
\ No newline at end of file

From a5920baa7571cfb6c95a993ae5344ef84b41c635 Mon Sep 17 00:00:00 2001
From: Ari <66433127+nanoticity@users.noreply.github.com>
Date: Mon, 5 Jan 2026 19:40:49 -0800
Subject: [PATCH 10/16] add file close

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
---
 src/test/path_plan_test.cc | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 6f8516f..4d9fd46 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -26,8 +26,12 @@ class Node {
   cv::Scalar color = {200, 200, 200};
 };
 
-std::ifstream file("/bos/constants/navgrid.json");
-nlohmann::json data = nlohmann::json::parse(file);
+nlohmann::json data = []() {
+  std::ifstream file("/bos/constants/navgrid.json");  // Open file in a local scope.
+  nlohmann::json j = nlohmann::json::parse(file);     // Parse JSON from the file.
+  file.close();                                       // Explicitly close the file after parsing.
+  return j;
+}();
 
 const int GRID_W = data["grid"][0].size();
 const int GRID_H = data["grid"].size();

From 363939fd658058727aa0c383e0f914be0e23a1d6 Mon Sep 17 00:00:00 2001
From: Ari <66433127+nanoticity@users.noreply.github.com>
Date: Mon, 5 Jan 2026 19:41:19 -0800
Subject: [PATCH 11/16] remove redundant condition

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
---
 src/test/path_plan_test.cc | 20 +++++++++-----------
 1 file changed, 9 insertions(+), 11 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 4d9fd46..b7b4499 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -154,20 +154,18 @@ auto basisFunction(double i, double p, double t,
   int deg = static_cast<int>(p);
 
   if (deg == 0) {
-    if (p == 0) {
-      if (knots[i] <= t && t < knots[i + 1])
-        return 1.0;
+    if (knots[i] <= t && t < knots[i + 1])
+      return 1.0;
 
-      if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
-        return 1.0;
-      }
-
-      if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
-        return 1.0;
-      }
+    if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
+      return 1.0;
+    }
 
-      return 0.0;
+    if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
+      return 1.0;
     }
+
+    return 0.0;
   }
 
   double weight = 0.0;

From 644e7f76fc6900e46c0c3e02159fd890527ec4a5 Mon Sep 17 00:00:00 2001
From: Ari <66433127+nanoticity@users.noreply.github.com>
Date: Mon, 5 Jan 2026 19:42:44 -0800
Subject: [PATCH 12/16] add better variable names

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
---
 src/test/path_plan_test.cc | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index b7b4499..0b0c070 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -127,21 +127,22 @@ auto constructLinePath(cv::Mat& canvas, std::vector<Node*> path)
   return controlPoints;
 }
 
-auto clampedUniformKnotVector(double k, double p) -> std::vector<double> {
+auto clampedUniformKnotVector(double numControlPoints, double degree)
+    -> std::vector<double> {
   std::vector<double> knots;
-  int n = (int)k;
-  int d = (int)p;
+  int numControlPointsInt = static_cast<int>(numControlPoints);
+  int degreeInt = static_cast<int>(degree);
 
-  for (int i = 0; i <= d; ++i) {
+  for (int i = 0; i <= degreeInt; ++i) {
     knots.push_back(0.0);
   }
 
-  int middle = n - d - 1;
+  int middle = numControlPointsInt - degreeInt - 1;
   for (int i = 1; i <= middle; ++i) {
-    knots.push_back((double)i / (middle + 1));
+    knots.push_back(static_cast<double>(i) / (middle + 1));
   }
 
-  for (int i = 0; i <= d; ++i) {
+  for (int i = 0; i <= degreeInt; ++i) {
     knots.push_back(1.0);
   }
 

From 77a5a5ce676edf7d0cf0cd66bcf9c118648955da Mon Sep 17 00:00:00 2001
From: Ari <66433127+nanoticity@users.noreply.github.com>
Date: Mon, 5 Jan 2026 19:43:10 -0800
Subject: [PATCH 13/16] better path reconstruction

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Signed-off-by: Ari <66433127+nanoticity@users.noreply.github.com>
---
 src/test/path_plan_test.cc | 64 +++++++++++++++++++++-----------------
 1 file changed, 35 insertions(+), 29 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 0b0c070..14009c0 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -238,39 +238,45 @@ auto main() -> int {
 
   BFS(grid, start, target);
 
-  std::vector<Node*> path = reconstructPath(target);
-  for (Node* n : path) {
-    if (n != start && n != target) {
-      n->color = {0, 0, 255};
-    }
-  }
-
   cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3,
                  {255, 255, 255});
   drawGrid(canvas, grid);
-  std::vector<std::pair<int, int>> controlPoints =
-      constructLinePath(canvas, path);
-  std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3);
-  drawSpline(canvas, controlPoints, knots, 3);
-
-  std::shared_ptr<nt::NetworkTable> table = inst.GetTable("PathPlanning");
-  nt::StructArrayTopic<frc::Pose2d> trajectory_topic =
-      table->GetStructArrayTopic<frc::Pose2d>("Trajectory");
-  nt::StructArrayPublisher<frc::Pose2d> trajectory_publisher =
-      trajectory_topic.Publish();
-
-  std::vector<frc::Pose2d> trajectory;
-  int resolution = 200;
-  for (int i = 0; i < resolution; ++i) {
-    double t = (double)i / resolution;
-    std::pair<double, double> point = getSplinePoint(t, controlPoints, knots, 3);
-    double px = point.first / CELL_SIZE;
-    double py = point.second / CELL_SIZE;
-    trajectory.push_back(
-        frc::Pose2d(units::meter_t{px}, units::meter_t{py}, units::radian_t{0.0}));
+
+  if (!std::isinf(target->g)) {  // Only reconstruct and visualize a path if BFS found one
+    std::vector<Node*> path = reconstructPath(target);
+    for (Node* n : path) {
+      if (n != start && n != target) {
+        n->color = {0, 0, 255};
+      }
+    }
+
+    std::vector<std::pair<int, int>> controlPoints =
+        constructLinePath(canvas, path);
+    std::vector<double> knots =
+        clampedUniformKnotVector(controlPoints.size(), 3);
+    drawSpline(canvas, controlPoints, knots, 3);
+
+    std::shared_ptr<nt::NetworkTable> table = inst.GetTable("PathPlanning");
+    nt::StructArrayTopic<frc::Pose2d> trajectory_topic =
+        table->GetStructArrayTopic<frc::Pose2d>("Trajectory");
+    nt::StructArrayPublisher<frc::Pose2d> trajectory_publisher =
+        trajectory_topic.Publish();
+
+    std::vector<frc::Pose2d> trajectory;
+    int resolution = 200;
+    for (int i = 0; i < resolution; ++i) {
+      double t = (double)i / resolution;
+      std::pair<double, double> point =
+          getSplinePoint(t, controlPoints, knots, 3);
+      double px = point.first / CELL_SIZE;
+      double py = point.second / CELL_SIZE;
+      trajectory.push_back(frc::Pose2d(units::meter_t{px},
+                                       units::meter_t{py},
+                                       units::radian_t{0.0}));
+    }
+    trajectory_publisher.Set(trajectory);
+    inst.Flush();
   }
-  trajectory_publisher.Set(trajectory);
-  inst.Flush();
 
   cv::imshow("Path Planning", canvas);
   cv::waitKey(0);

From f71c3a996113279bf0fc705418b6ef8605c6285b Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Tue, 6 Jan 2026 04:54:58 +0000
Subject: [PATCH 14/16] Fix everything finally!

---
 src/test/path_plan_test.cc | 108 +++++++++++++++++--------------------
 1 file changed, 50 insertions(+), 58 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 14009c0..22213e5 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -26,15 +26,12 @@ class Node {
   cv::Scalar color = {200, 200, 200};
 };
 
-nlohmann::json data = []() {
-  std::ifstream file("/bos/constants/navgrid.json");  // Open file in a local scope.
-  nlohmann::json j = nlohmann::json::parse(file);     // Parse JSON from the file.
-  file.close();                                       // Explicitly close the file after parsing.
-  return j;
-}();
+std::ifstream file("/bos/constants/navgrid.json");
+nlohmann::json data = nlohmann::json::parse(file);
 
 const int GRID_W = data["grid"][0].size();
 const int GRID_H = data["grid"].size();
+double nodeSizeMeters = data["nodeSizeMeters"];
 const int CELL_SIZE = 20; 
 std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
 
@@ -127,22 +124,21 @@ auto constructLinePath(cv::Mat& canvas, std::vector<Node*> path)
   return controlPoints;
 }
 
-auto clampedUniformKnotVector(double numControlPoints, double degree)
-    -> std::vector<double> {
+auto clampedUniformKnotVector(double k, double p) -> std::vector<double> {
   std::vector<double> knots;
-  int numControlPointsInt = static_cast<int>(numControlPoints);
-  int degreeInt = static_cast<int>(degree);
+  int n = (int)k;
+  int d = (int)p;
 
-  for (int i = 0; i <= degreeInt; ++i) {
+  for (int i = 0; i <= d; ++i) {
     knots.push_back(0.0);
   }
 
-  int middle = numControlPointsInt - degreeInt - 1;
+  int middle = n - d - 1;
   for (int i = 1; i <= middle; ++i) {
-    knots.push_back(static_cast<double>(i) / (middle + 1));
+    knots.push_back((double)i / (middle + 1));
   }
 
-  for (int i = 0; i <= degreeInt; ++i) {
+  for (int i = 0; i <= d; ++i) {
     knots.push_back(1.0);
   }
 
@@ -155,18 +151,20 @@ auto basisFunction(double i, double p, double t,
   int deg = static_cast<int>(p);
 
   if (deg == 0) {
-    if (knots[i] <= t && t < knots[i + 1])
-      return 1.0;
+    if (p == 0) {
+      if (knots[i] <= t && t < knots[i + 1])
+        return 1.0;
 
-    if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
-      return 1.0;
-    }
+      if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
+        return 1.0;
+      }
 
-    if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
-      return 1.0;
-    }
+      if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
+        return 1.0;
+      }
 
-    return 0.0;
+      return 0.0;
+    }
   }
 
   double weight = 0.0;
@@ -238,45 +236,39 @@ auto main() -> int {
 
   BFS(grid, start, target);
 
+  std::vector<Node*> path = reconstructPath(target);
+  for (Node* n : path) {
+    if (n != start && n != target) {
+      n->color = {0, 0, 255};
+    }
+  }
+
   cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3,
                  {255, 255, 255});
   drawGrid(canvas, grid);
-
-  if (!std::isinf(target->g)) {  // Only reconstruct and visualize a path if BFS found one
-    std::vector<Node*> path = reconstructPath(target);
-    for (Node* n : path) {
-      if (n != start && n != target) {
-        n->color = {0, 0, 255};
-      }
-    }
-
-    std::vector<std::pair<int, int>> controlPoints =
-        constructLinePath(canvas, path);
-    std::vector<double> knots =
-        clampedUniformKnotVector(controlPoints.size(), 3);
-    drawSpline(canvas, controlPoints, knots, 3);
-
-    std::shared_ptr<nt::NetworkTable> table = inst.GetTable("PathPlanning");
-    nt::StructArrayTopic<frc::Pose2d> trajectory_topic =
-        table->GetStructArrayTopic<frc::Pose2d>("Trajectory");
-    nt::StructArrayPublisher<frc::Pose2d> trajectory_publisher =
-        trajectory_topic.Publish();
-
-    std::vector<frc::Pose2d> trajectory;
-    int resolution = 200;
-    for (int i = 0; i < resolution; ++i) {
-      double t = (double)i / resolution;
-      std::pair<double, double> point =
-          getSplinePoint(t, controlPoints, knots, 3);
-      double px = point.first / CELL_SIZE;
-      double py = point.second / CELL_SIZE;
-      trajectory.push_back(frc::Pose2d(units::meter_t{px},
-                                       units::meter_t{py},
-                                       units::radian_t{0.0}));
-    }
-    trajectory_publisher.Set(trajectory);
-    inst.Flush();
+  std::vector<std::pair<int, int>> controlPoints =
+      constructLinePath(canvas, path);
+  std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3);
+  drawSpline(canvas, controlPoints, knots, 3);
+
+  std::shared_ptr<nt::NetworkTable> table = inst.GetTable("PathPlanning");
+  nt::StructArrayTopic<frc::Pose2d> trajectory_topic =
+      table->GetStructArrayTopic<frc::Pose2d>("Trajectory");
+  nt::StructArrayPublisher<frc::Pose2d> trajectory_publisher =
+      trajectory_topic.Publish();
+
+  std::vector<frc::Pose2d> trajectory;
+  int resolution = 200;
+  for (int i = 0; i < resolution; ++i) {
+    double t = (double)i / resolution;
+    std::pair<double, double> point = getSplinePoint(t, controlPoints, knots, 3);
+    double px = point.first / CELL_SIZE;
+    double py = point.second / CELL_SIZE;
+    trajectory.push_back(
+    frc::Pose2d(units::meter_t{px * nodeSizeMeters}, units::meter_t{py * nodeSizeMeters}, units::radian_t{0.0}));
   }
+  trajectory_publisher.Set(trajectory);
+  inst.Flush();
 
   cv::imshow("Path Planning", canvas);
   cv::waitKey(0);

From 2c65aef318532a34054461af37e85f8560bed369 Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Wed, 7 Jan 2026 05:20:25 +0000
Subject: [PATCH 15/16] Add a one block buffer to navgrid

---
 constants/navgrid.json | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/constants/navgrid.json b/constants/navgrid.json
index 23e0db9..175cb01 100644
--- a/constants/navgrid.json
+++ b/constants/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

From 73f514d9f9ce6ebd3c4fd0171d8774356c88533c Mon Sep 17 00:00:00 2001
From: Nano <nanoticity@gmail.com>
Date: Thu, 8 Jan 2026 04:50:59 +0000
Subject: [PATCH 16/16] Make better tests, should be ready for review

Signed-off-by: Nano <nanoticity@gmail.com>
---
 src/test/path_plan_test.cc | 461 ++++++++++++++++++++-----------------
 1 file changed, 247 insertions(+), 214 deletions(-)

diff --git a/src/test/path_plan_test.cc b/src/test/path_plan_test.cc
index 22213e5..0048207 100644
--- a/src/test/path_plan_test.cc
+++ b/src/test/path_plan_test.cc
@@ -1,277 +1,310 @@
-#include <iostream>
+#include <frc/DataLogManager.h>
+#include <frc/geometry/Pose2d.h>
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/StructArrayTopic.h>
+#include <units/angle.h>
+#include <units/length.h>
 #include <algorithm>
-#include <cmath>
 #include <fstream>
+#include <iostream>
+#include <queue>
+#include <vector>
 #include <nlohmann/json.hpp>
 #include <opencv2/core.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/opencv.hpp>
-#include <ostream>
-#include <queue>
-#include <vector>
-#include <frc/DataLogManager.h>
-#include <networktables/NetworkTableInstance.h>
-#include <networktables/StructArrayTopic.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <frc/geometry/Pose2d.h>
+#include <cmath>
+#include <utility>
+#include <random>
 
 class Node {
- public:
-  int x = 0;
-  int y = 0;
-  bool walkable = true;
-  Node* parent = nullptr;
-  double g = INFINITY;
-  cv::Scalar color = {200, 200, 200};
+public:
+    int x = 0;
+    int y = 0;
+    bool walkable = true;
+    Node* parent = nullptr;
+    double g = INFINITY;
+    cv::Scalar color = {200, 200, 200};
 };
 
-std::ifstream file("/bos/constants/navgrid.json");
+std::ifstream file("/root/bos/constants/navgrid.json");
 nlohmann::json data = nlohmann::json::parse(file);
 
 const int GRID_W = data["grid"][0].size();
 const int GRID_H = data["grid"].size();
 double nodeSizeMeters = data["nodeSizeMeters"];
-const int CELL_SIZE = 20; 
+const int CELL_SIZE = 20;
 std::vector<std::vector<Node>> grid(GRID_H, std::vector<Node>(GRID_W));
 
 auto nodeRect(const Node& n) -> cv::Rect {
-  return {n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE};
+    return {n.x * CELL_SIZE, n.y * CELL_SIZE, CELL_SIZE, CELL_SIZE};
 }
 
 void drawGrid(cv::Mat& canvas, const std::vector<std::vector<Node>>& grid) {
-  for (int y = 0; y < GRID_H; ++y) {
-    for (int x = 0; x < GRID_W; ++x) {
-      const Node& n = grid[y][x];
-      cv::rectangle(canvas, nodeRect(n), n.color, cv::FILLED);
-      cv::rectangle(canvas, nodeRect(n), {50, 50, 50}, 1);
+    for (int y = 0; y < GRID_H; ++y) {
+        for (int x = 0; x < GRID_W; ++x) {
+            const Node& n = grid[y][x];
+            cv::rectangle(canvas, nodeRect(n), n.color, cv::FILLED);
+            cv::rectangle(canvas, nodeRect(n), {50, 50, 50}, 1);
+        }
     }
-  }
 }
 
 void makeObstacle(std::vector<std::vector<Node>>& grid, int row, int col) {
-  grid[row][col].walkable = false;
-  grid[row][col].color = {0, 0, 0};
+    grid[row][col].walkable = false;
+    grid[row][col].color = {0, 0, 0};
 }
 
-auto reconstructPath(Node* target) -> std::vector<Node*> {
-  std::vector<Node*> path;
-  Node* current = target;
-  while (current != nullptr) {
-    path.push_back(current);
-    current = current->parent;
-  }
-  std::reverse(path.begin(), path.end());
-  return path;
+void resetGrid(std::vector<std::vector<Node>>& grid) {
+    for (int y = 0; y < GRID_H; ++y) {
+        for (int x = 0; x < GRID_W; ++x) {
+            if (grid[y][x].walkable) {
+                grid[y][x].color = {200, 200, 200};
+            }
+            grid[y][x].parent = nullptr;
+            grid[y][x].g = INFINITY;
+        }
+    }
 }
 
-void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
-  std::priority_queue<std::pair<double, Node*>,
-                      std::vector<std::pair<double, Node*>>, std::greater<>>
-      openList;
-  start->g = 0.0f;
-  start->color = {0, 255, 0};
-  target->color = {255, 0, 0};
-  openList.push(std::make_pair(start->g, start));
-
-  std::vector<std::pair<int, int>> dirs = {{0, -1},  {0, 1},  {-1, 0}, {1, 0},
-                                           {-1, -1}, {-1, 1}, {1, -1}, {1, 1}};
-
-  while (!openList.empty()) {
-    std::pair<double, Node*> currentPair = openList.top();
-    openList.pop();
-    Node* current = currentPair.second;
-    if (current == target)
-      return;
-
-    for (const std::pair<int, int>& d : dirs) {
-      int nx = current->x + d.first;
-      int ny = current->y + d.second;
-      if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H)
-        continue;
-      Node* neighbor = &grid[ny][nx];
-      if (!neighbor->walkable)
-        continue;
-      auto dx = static_cast<double>(d.first);
-      auto dy = static_cast<double>(d.second);
-      double cost = std::sqrt(dx * dx + dy * dy);
-      double tentativeG = current->g + cost;
-      if (tentativeG < neighbor->g) {
-        neighbor->g = tentativeG;
-        neighbor->parent = current;
-        openList.push(std::make_pair(neighbor->g, neighbor));
-      }
+auto getRandomWalkableNode(const std::vector<std::vector<Node>>& grid, std::mt19937& rng) -> Node* {
+    std::vector<Node*> walkableNodes;
+    for (int y = 0; y < GRID_H; ++y) {
+        for (int x = 0; x < GRID_W; ++x) {
+            if (grid[y][x].walkable) {
+                walkableNodes.push_back(const_cast<Node*>(&grid[y][x]));
+            }
+        }
     }
-  }
+    
+    if (walkableNodes.empty()) return nullptr;
+    
+    std::uniform_int_distribution<> dist(0, walkableNodes.size() - 1);
+    return walkableNodes[dist(rng)];
 }
 
-auto constructLinePath(cv::Mat& canvas, std::vector<Node*> path)
-    -> std::vector<std::pair<int, int>> {
-  std::vector<std::pair<int, int>> controlPoints;
-  for (size_t i = 0; i < path.size(); ++i) {
-    int px = path[i]->x * CELL_SIZE + (CELL_SIZE / 2);
-    int py = path[i]->y * CELL_SIZE + (CELL_SIZE / 2);
+auto reconstructPath(Node* target) -> std::vector<Node*> {
+    std::vector<Node*> path;
+    Node* current = target;
+    while (current != nullptr) {
+        path.push_back(current);
+        current = current->parent;
+    }
+    std::reverse(path.begin(), path.end());
+    return path;
+}
 
-    controlPoints.emplace_back(px, py);
+void BFS(std::vector<std::vector<Node>>& grid, Node* start, Node* target) {
+    std::priority_queue<std::pair<double, Node*>, std::vector<std::pair<double, Node*>>, std::greater<>> openList;
+    start->g = 0.0f;
+    start->color = {0, 255, 0};
+    target->color = {255, 0, 0};
+    openList.push(std::make_pair(start->g, start));
+
+    std::vector<std::pair<int, int>> dirs = {{0, -1}, {0, 1}, {-1, 0}, {1, 0}, {-1, -1}, {-1, 1}, {1, -1}, {1, 1}};
+
+    while (!openList.empty()) {
+        std::pair<double, Node*> currentPair = openList.top();
+        openList.pop();
+        Node* current = currentPair.second;
+        if (current == target) return;
+
+        for (const std::pair<int, int>& d : dirs) {
+            int nx = current->x + d.first;
+            int ny = current->y + d.second;
+            if (nx < 0 || nx >= GRID_W || ny < 0 || ny >= GRID_H)
+                continue;
+            Node* neighbor = &grid[ny][nx];
+            if (!neighbor->walkable)
+                continue;
+            auto dx = static_cast<double>(d.first);
+            auto dy = static_cast<double>(d.second);
+            double cost = std::sqrt(dx * dx + dy * dy);
+            double tentativeG = current->g + cost;
+            if (tentativeG < neighbor->g) {
+                neighbor->g = tentativeG;
+                neighbor->parent = current;
+                openList.push(std::make_pair(neighbor->g, neighbor));
+            }
+        }
+    }
+}
 
-    if (i > 0) {
-      int prevX = path[i - 1]->x * CELL_SIZE + (CELL_SIZE / 2);
-      int prevY = path[i - 1]->y * CELL_SIZE + (CELL_SIZE / 2);
-      cv::line(canvas, cv::Point(prevX, prevY), cv::Point(px, py), {0, 0, 0},
-               2);
+auto constructLinePath(cv::Mat& canvas, std::vector<Node*> path) -> std::vector<std::pair<int, int>> {
+    std::vector<std::pair<int, int>> controlPoints;
+    for (size_t i = 0; i < path.size(); ++i) {
+        int px = path[i]->x * CELL_SIZE + (CELL_SIZE / 2);
+        int py = path[i]->y * CELL_SIZE + (CELL_SIZE / 2);
+        controlPoints.emplace_back(px, py);
+
+        if (i > 0) {
+            int prevX = path[i - 1]->x * CELL_SIZE + (CELL_SIZE / 2);
+            int prevY = path[i - 1]->y * CELL_SIZE + (CELL_SIZE / 2);
+            cv::line(canvas, cv::Point(prevX, prevY), cv::Point(px, py), {0, 0, 0}, 2);
+        }
     }
-  }
-  return controlPoints;
+    return controlPoints;
 }
 
 auto clampedUniformKnotVector(double k, double p) -> std::vector<double> {
-  std::vector<double> knots;
-  int n = (int)k;
-  int d = (int)p;
+    std::vector<double> knots;
+    int n = (int)k;
+    int d = (int)p;
 
-  for (int i = 0; i <= d; ++i) {
-    knots.push_back(0.0);
-  }
+    for (int i = 0; i <= d; ++i) {
+        knots.push_back(0.0);
+    }
 
-  int middle = n - d - 1;
-  for (int i = 1; i <= middle; ++i) {
-    knots.push_back((double)i / (middle + 1));
-  }
+    int middle = n - d - 1;
+    for (int i = 1; i <= middle; ++i) {
+        knots.push_back((double)i / (middle + 1));
+    }
 
-  for (int i = 0; i <= d; ++i) {
-    knots.push_back(1.0);
-  }
+    for (int i = 0; i <= d; ++i) {
+        knots.push_back(1.0);
+    }
 
-  return knots;
+    return knots;
 }
 
-auto basisFunction(double i, double p, double t,
-                   const std::vector<double>& knots) -> double {
-  int idx = static_cast<int>(i);
-  int deg = static_cast<int>(p);
+auto basisFunction(double i, double p, double t, const std::vector<double>& knots) -> double {
+    int idx = static_cast<int>(i);
+    int deg = static_cast<int>(p);
+
+    if (deg == 0) {
+        if (p == 0) {
+            if (knots[i] <= t && t < knots[i + 1]) return 1.0;
 
-  if (deg == 0) {
-    if (p == 0) {
-      if (knots[i] <= t && t < knots[i + 1])
-        return 1.0;
+            if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
+                return 1.0;
+            }
 
-      if (t == 0.0 && knots[i] == 0.0 && knots[i + 1] > 0.0) {
-        return 1.0;
-      }
+            if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
+                return 1.0;
+            }
 
-      if (t == 1.0 && knots[i + 1] == 1.0 && knots[i] < 1.0) {
-        return 1.0;
-      }
+            return 0.0;
+        }
+    }
 
-      return 0.0;
+    double weight = 0.0;
+
+    double denom1 = knots[idx + deg] - knots[idx];
+    if (denom1 != 0) {
+        weight += ((t - knots[idx]) / denom1) * basisFunction(i, p - 1, t, knots);
+    }
+    double denom2 = knots[idx + deg + 1] - knots[idx + 1];
+    if (denom2 != 0) {
+        weight += ((knots[idx + deg + 1] - t) / denom2) * basisFunction(i + 1, p - 1, t, knots);
     }
-  }
-
-  double weight = 0.0;
-
-  double denom1 = knots[idx + deg] - knots[idx];
-  if (denom1 != 0) {
-    weight += ((t - knots[idx]) / denom1) * basisFunction(i, p - 1, t, knots);
-  }
-  double denom2 = knots[idx + deg + 1] - knots[idx + 1];
-  if (denom2 != 0) {
-    weight += ((knots[idx + deg + 1] - t) / denom2) *
-              basisFunction(i + 1, p - 1, t, knots);
-  }
-  return weight;
+    return weight;
 }
 
-auto getSplinePoint(double t, const std::vector<std::pair<int, int>>& points,
-                    const std::vector<double>& knots, int p)
-    -> std::pair<double, double> {
-  double px = 0.0, py = 0.0;
-  for (size_t i = 0; i < points.size(); ++i) {
-    double weight = basisFunction((double)i, (double)p, t, knots);
-    px += points[i].first * weight;
-    py += points[i].second * weight;
-  }
-  return {px, py};
+auto getSplinePoint(double t, const std::vector<std::pair<int, int>>& points, const std::vector<double>& knots, int p) -> std::pair<double, double> {
+    double px = 0.0, py = 0.0;
+    for (size_t i = 0; i < points.size(); ++i) {
+        double weight = basisFunction((double)i, (double)p, t, knots);
+        px += points[i].first * weight;
+        py += points[i].second * weight;
+    }
+    return {px, py};
 }
 
-void drawSpline(cv::Mat& canvas, const std::vector<std::pair<int, int>>& points,
-                std::vector<double>& knots, int p) {
-  int res = 200;
-  std::pair<double, double> prevPoint = {static_cast<double>(points[0].first),
-                                         static_cast<double>(points[0].second)};
+void drawSpline(cv::Mat& canvas, const std::vector<std::pair<int, int>>& points, std::vector<double>& knots, int p) {
+    int res = 200;
+    std::pair<double, double> prevPoint = {static_cast<double>(points[0].first), static_cast<double>(points[0].second)};
 
-  for (int i = 1; i < res; ++i) {
-    double t = (double)i / res;
-    std::pair<double, double> currPoint = getSplinePoint(t, points, knots, p);
+    for (int i = 1; i < res; ++i) {
+        double t = (double)i / res;
+        std::pair<double, double> currPoint = getSplinePoint(t, points, knots, p);
 
-    cv::line(canvas, cv::Point(prevPoint.first, prevPoint.second),
-             cv::Point(currPoint.first, currPoint.second),
-             cv::Scalar(255, 0, 0), 2);
+        cv::line(canvas, cv::Point(prevPoint.first, prevPoint.second),
+                 cv::Point(currPoint.first, currPoint.second),
+                 cv::Scalar(255, 0, 0), 2);
 
-    prevPoint = currPoint;
-  }
+        prevPoint = currPoint;
+    }
 }
 
 auto main() -> int {
-  nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
-  inst.StopClient();
-  inst.StopLocal();
-  inst.StartClient4("path_planning");
-  inst.SetServerTeam(971);
-  frc::DataLogManager::Start("/bos/logs/");
-
-  std::vector<std::vector<bool>> navgrid = data["grid"];
-
-  for (int y = 0; y < GRID_H; ++y) {
-    for (int x = 0; x < GRID_W; ++x) {
-      grid[y][x].x = x;
-      grid[y][x].y = y;
-      if (static_cast<bool>(navgrid[y][x])) {
-        makeObstacle(grid, y, x);
-      }
+    nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+    inst.StopClient();
+    inst.StopLocal();
+    inst.StartClient4("path_planning");
+    inst.SetServerTeam(971);
+    frc::DataLogManager::Start("/bos/logs/");
+
+    std::vector<std::vector<int>> navgrid = data["grid"];
+    std::random_device rd;
+    std::mt19937 rng(rd());
+
+    for (int y = 0; y < GRID_H; ++y) {
+        for (int x = 0; x < GRID_W; ++x) {
+            grid[y][x].x = x;
+            grid[y][x].y = y;
+            if (static_cast<bool>(navgrid[y][x])) {
+                makeObstacle(grid, y, x);
+            }
+        }
     }
-  }
-
-  Node* start = &grid[std::min(13, GRID_H - 1)][std::min(5, GRID_W - 1)];
-  Node* target = &grid[std::min(13, GRID_H - 1)][std::min(25, GRID_W - 1)];
 
-  BFS(grid, start, target);
-
-  std::vector<Node*> path = reconstructPath(target);
-  for (Node* n : path) {
-    if (n != start && n != target) {
-      n->color = {0, 0, 255};
+    std::shared_ptr<nt::NetworkTable> table = inst.GetTable("PathPlanning");
+    nt::StructArrayTopic<frc::Pose2d> trajectory_topic = table->GetStructArrayTopic<frc::Pose2d>("Trajectory");
+    nt::StructArrayPublisher<frc::Pose2d> trajectory_publisher = trajectory_topic.Publish();
+
+    while (true) {
+        resetGrid(grid);
+        
+        Node* start = getRandomWalkableNode(grid, rng);
+        Node* target = getRandomWalkableNode(grid, rng);
+        
+        if (!start || !target || start == target) {
+            std::cout << "Failed to find valid start/target nodes" << std::endl;
+            continue;
+        }
+
+        std::cout << "Start: (" << start->x << ", " << start->y << ") Target: (" << target->x << ", " << target->y << ")" << std::endl;
+
+        BFS(grid, start, target);
+
+        std::vector<Node*> path = reconstructPath(target);
+        if (path.size() <= 1) {
+            std::cout << "No path found, trying again..." << std::endl;
+            continue;
+        }
+
+        for (Node* n : path) {
+            if (n != start && n != target) {
+                n->color = {0, 0, 255};
+            }
+        }
+
+        cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3, {255, 255, 255});
+        drawGrid(canvas, grid);
+        
+        std::vector<std::pair<int, int>> controlPoints = constructLinePath(canvas, path);
+        std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3.7);
+        drawSpline(canvas, controlPoints, knots, 3);
+
+        std::vector<frc::Pose2d> trajectory;
+        int resolution = 200;
+        for (int i = 0; i < resolution; ++i) {
+            double t = (double)i / resolution;
+            std::pair<double, double> point = getSplinePoint(t, controlPoints, knots, 3.7);
+            double px = point.first / CELL_SIZE;
+            double py = point.second / CELL_SIZE;
+            trajectory.push_back(frc::Pose2d(units::meter_t{px * nodeSizeMeters}, units::meter_t{py * nodeSizeMeters}, units::radian_t{0.0}));
+        }
+        trajectory_publisher.Set(trajectory);
+        inst.Flush();
+
+        cv::imshow("Path Planning", canvas);
+        
+        int key = cv::waitKey(0);
+        if (key == 27) {
+            break;
+        }
     }
-  }
-
-  cv::Mat canvas(GRID_H * CELL_SIZE, GRID_W * CELL_SIZE, CV_8UC3,
-                 {255, 255, 255});
-  drawGrid(canvas, grid);
-  std::vector<std::pair<int, int>> controlPoints =
-      constructLinePath(canvas, path);
-  std::vector<double> knots = clampedUniformKnotVector(controlPoints.size(), 3);
-  drawSpline(canvas, controlPoints, knots, 3);
-
-  std::shared_ptr<nt::NetworkTable> table = inst.GetTable("PathPlanning");
-  nt::StructArrayTopic<frc::Pose2d> trajectory_topic =
-      table->GetStructArrayTopic<frc::Pose2d>("Trajectory");
-  nt::StructArrayPublisher<frc::Pose2d> trajectory_publisher =
-      trajectory_topic.Publish();
-
-  std::vector<frc::Pose2d> trajectory;
-  int resolution = 200;
-  for (int i = 0; i < resolution; ++i) {
-    double t = (double)i / resolution;
-    std::pair<double, double> point = getSplinePoint(t, controlPoints, knots, 3);
-    double px = point.first / CELL_SIZE;
-    double py = point.second / CELL_SIZE;
-    trajectory.push_back(
-    frc::Pose2d(units::meter_t{px * nodeSizeMeters}, units::meter_t{py * nodeSizeMeters}, units::radian_t{0.0}));
-  }
-  trajectory_publisher.Set(trajectory);
-  inst.Flush();
-
-  cv::imshow("Path Planning", canvas);
-  cv::waitKey(0);
-
-  return 0;
+
+    return 0;
 }
\ No newline at end of file