diff --git a/cpp/Makefile b/cpp/Makefile
index 5a866eb..bb0b33e 100644
--- a/cpp/Makefile
+++ b/cpp/Makefile
@@ -2,7 +2,7 @@
 # Generated by Kimi K2
 #---------- configurable ----------
 CXX      := g++
-CXXFLAGS := -std=c++23 -Wall -Wextra -Wpedantic -ggdb3
+CXXFLAGS := -Isrc -std=c++23 -Wall -Wextra -Wpedantic -ggdb3
 LDFLAGS  :=
 LDLIBS   := -lSDL3 -lSDL3_image -lGLEW -lGL
 
@@ -11,7 +11,7 @@ BUILD_DIR:= build
 TARGET   := pathfinding
 
 #----------------------------------
-SOURCES := $(wildcard $(SRC_DIR)/*.cpp)
+SOURCES := $(shell find $(SRC_DIR) -name '*.cpp')
 OBJECTS := $(SOURCES:$(SRC_DIR)/%.cpp=$(BUILD_DIR)/%.o)
 
 #----------------------------------
@@ -25,6 +25,7 @@ $(TARGET): $(OBJECTS)
 
 # compile step
 $(BUILD_DIR)/%.o: $(SRC_DIR)/%.cpp | $(BUILD_DIR)
+	@mkdir -p $(dir $@)
 	$(CXX) $(CXXFLAGS) -c -o $@ $<
 
 $(BUILD_DIR):
diff --git a/cpp/src/gameloop.cpp b/cpp/src/gameloop.cpp
index e73d82c..85d825e 100644
--- a/cpp/src/gameloop.cpp
+++ b/cpp/src/gameloop.cpp
@@ -7,7 +7,7 @@
 #include "window.hpp"
 #include "user_input.hpp"
 #include "log.hpp"
-#include "pathfinder.hpp"
+#include "pathfinder/base.hpp"
 #include "math.hpp"
 
 void GameLoop::Run() {
diff --git a/cpp/src/pathfinder.cpp b/cpp/src/pathfinder.cpp
deleted file mode 100644
index 091fd70..0000000
--- a/cpp/src/pathfinder.cpp
+++ /dev/null
@@ -1,273 +0,0 @@
-#include <memory>
-#include <cassert>
-#include <queue>
-
-#include "pathfinder.hpp"
-#include "log.hpp"
-
-namespace pathfinder {
-
-
-PathFinderBase::PathFinderBase(const Map* map) : m_Map(map) {}
-
-
-Path LinearPathFinder::CalculatePath(WorldPos start, WorldPos end)
-{
-  auto path = Path{end};
-  return path;
-}
-
-
-Path BFS::CalculatePath(WorldPos start_world, WorldPos end_world) {
-    if (m_Map == nullptr) return {};
-
-    const TilePos start = m_Map->WorldToTile(start_world);
-    const TilePos end   = m_Map->WorldToTile(end_world);
-
-    if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
-        return {};
-    if (start == end) {
-        return {};
-    }
-    // clear previous run
-    m_CameFrom.clear();
-    m_Distance.clear();
-
-    std::queue<TilePos> frontier;
-    frontier.push(start);
-    m_CameFrom[start] = start;
-    m_Distance[start] = 0.0f;
-
-    // ---------------- build flow-field ----------------
-    bool early_exit = false;
-    while (!frontier.empty() && !early_exit) {
-        TilePos current = frontier.front();
-        frontier.pop();
-
-        for (TilePos next : m_Map->GetNeighbors(current)) {
-            if (m_CameFrom.find(next) == m_CameFrom.end()) {  // not visited
-                frontier.push(next);
-                m_Distance[next] = m_Distance[current] + 1.0f;
-                m_CameFrom[next] = current;
-
-                if (next == end) {          // early exit
-                    early_exit = true;
-                    break;
-                }
-            }
-        }
-    }
-
-    // --------------- reconstruct path -----------------
-    if (m_CameFrom.find(end) == m_CameFrom.end())
-        return {};                        // end not reached
-
-    Path path;
-    TilePos cur = end;
-    path.push_back(m_Map->TileToWorld(cur));
-    while (cur != start) {
-        cur = m_CameFrom[cur];
-        path.push_back(m_Map->TileToWorld(cur));
-    }
-    std::reverse(path.begin(), path.end());
-    return path;
-}
-//
-//Path Dijkstra::CalculatePath(WorldPos start_world, WorldPos end_world) {
-//    if (m_Map == nullptr) return {};
-//
-//    const TilePos start = m_Map->WorldToTile(start_world);
-//    const TilePos end   = m_Map->WorldToTile(end_world);
-//
-//    if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
-//        return {};
-//    if (start == end) {
-//        return {};
-//    }
-//    // clear previous run
-//    m_CameFrom.clear();
-//    m_Distance.clear();
-//
-//    std::queue<TilePos> frontier;
-//    frontier.push(start);
-//    m_CameFrom[start] = start;
-//    m_Distance[start] = 0.0f;
-//
-//    // ---------------- build flow-field ----------------
-//    bool early_exit = false;
-//    while (!frontier.empty() && !early_exit) {
-//        TilePos current = frontier.front();
-//        frontier.pop();
-//
-//        for (TilePos next : m_Map->GetNeighbors(current)) {
-//            if (m_CameFrom.find(next) == m_CameFrom.end()) {  // not visited
-//                frontier.push(next);
-//                m_Distance[next] = m_Distance[current] + 1.0f;
-//                m_CameFrom[next] = current;
-//
-//                if (next == end) {          // early exit
-//                    early_exit = true;
-//                    break;
-//                }
-//            }
-//        }
-//    }
-//
-//    // --------------- reconstruct path -----------------
-//    if (m_CameFrom.find(end) == m_CameFrom.end())
-//        return {};                        // end not reached
-//
-//    Path path;
-//    TilePos cur = end;
-//    path.push_back(m_Map->TileToWorld(cur));
-//    while (cur != start) {
-//        cur = m_CameFrom[cur];
-//        path.push_back(m_Map->TileToWorld(cur));
-//    }
-//    std::reverse(path.begin(), path.end());
-//    return path;
-//}
-
-struct QueueEntry
-{
-    float cost;
-    TilePos tile;
-
-    // min-heap -> smallest cost on top
-    bool operator>(const QueueEntry& o) const noexcept { return cost > o.cost; }
-};
-
-Path Dijkstra::CalculatePath(WorldPos start_world, WorldPos end_world)
-{
-    if (!m_Map) return {};
-
-    const TilePos start = m_Map->WorldToTile(start_world);
-    const TilePos end   = m_Map->WorldToTile(end_world);
-
-    if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
-        return {};
-    if (start == end) return {};
-
-    // clear previous run
-    m_CameFrom.clear();
-    m_Cost.clear();
-
-    std::priority_queue<QueueEntry, std::vector<QueueEntry>, std::greater<>> frontier;
-    frontier.push({0.0f, start});
-    m_CameFrom[start] = start;   // sentinel
-    m_Cost[start]     = 0.0f;
-
-    while (!frontier.empty())
-    {
-        const QueueEntry current = frontier.top();
-        frontier.pop();
-
-        if (current.tile == end)          // early exit
-            break;
-
-        for (TilePos next : m_Map->GetNeighbors(current.tile))
-        {
-            // cost of moving to neighbour (uniform 1.0 matches original BFS)
-            const float newCost = m_Cost[current.tile] + m_Map->GetCost(next);
-
-            if (!m_Cost.count(next) || newCost < m_Cost[next])
-            {
-                m_Cost[next]     = newCost;
-                m_CameFrom[next] = current.tile;
-                frontier.push({newCost, next});
-            }
-        }
-    }
-
-    // reconstruct path
-    if (!m_CameFrom.count(end))
-        return {};                   // goal never reached
-
-    Path path;
-    TilePos cur = end;
-    path.push_back(m_Map->TileToWorld(cur));
-
-    while (cur != start)
-    {
-        cur = m_CameFrom[cur];
-        path.push_back(m_Map->TileToWorld(cur));
-    }
-    std::reverse(path.begin(), path.end());
-    return path;
-}
-
-float GBFS::Heuristic(const TilePos& a, const TilePos& b)
-{
-    return static_cast<float>(std::abs(a.x- b.x) + std::abs(a.y - b.y));
-}
-
-Path GBFS::CalculatePath(WorldPos start_world, WorldPos end_world)
-{
-    if (!m_Map) return {};
-
-    const TilePos start = m_Map->WorldToTile(start_world);
-    const TilePos end   = m_Map->WorldToTile(end_world);
-
-    if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
-        return {};
-    if (start == end) return {};
-
-    m_CameFrom.clear();
-
-    std::priority_queue<QueueEntry, std::vector<QueueEntry>, std::greater<>> frontier;
-    frontier.push({Heuristic(start, end), start});
-    m_CameFrom[start] = start;  // sentinel
-
-    while (!frontier.empty())
-    {
-        const QueueEntry current = frontier.top();
-        frontier.pop();
-
-        if (current.tile == end)  // early exit
-            break;
-
-        for (TilePos next : m_Map->GetNeighbors(current.tile))
-        {
-            if (!m_CameFrom.count(next))  // not visited
-            {
-                m_CameFrom[next] = current.tile;
-                frontier.push({Heuristic(end, next), next});
-            }
-        }
-    }
-
-    // reconstruct path
-    if (!m_CameFrom.count(end))
-        return {};  // goal never reached
-
-    Path path;
-    TilePos cur = end;
-    path.push_back(m_Map->TileToWorld(cur));
-
-    while (cur != start)
-    {
-        cur = m_CameFrom[cur];
-        path.push_back(m_Map->TileToWorld(cur));
-    }
-    std::reverse(path.begin(), path.end());
-    return path;
-}
-
-std::unique_ptr<PathFinderBase> create(PathFinderType type, const Map* map) {
-  switch (type) {
-    case PathFinderType::LINEAR:
-      return std::move(std::make_unique<LinearPathFinder>(map));
-    case PathFinderType::BFS:
-      return std::move(std::make_unique<BFS>(map));
-    case PathFinderType::DIJKSTRA:
-      return std::move(std::make_unique<Dijkstra>(map));
-    case PathFinderType::GBFS:
-      return std::move(std::make_unique<GBFS>(map));
-    case PathFinderType::COUNT:
-      LOG_WARNING("Incorrect pathfinder type");
-      return nullptr;
-  };
-  return nullptr;
-}
-
-} // pathfinder namespace
diff --git a/cpp/src/pathfinder.hpp b/cpp/src/pathfinder.hpp
deleted file mode 100644
index 18bf275..0000000
--- a/cpp/src/pathfinder.hpp
+++ /dev/null
@@ -1,97 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <memory>
-#include <unordered_map>
-
-#include "math.hpp"
-#include "map.hpp"
-
-namespace pathfinder {
-
-using Path = std::vector<WorldPos>;
-
-enum class PathFinderType {
-  LINEAR = 1,
-  BFS,
-  DIJKSTRA,
-  GBFS,
-  COUNT,
-};
-
-class PathFinderBase {
-public:
-  PathFinderBase(const Map* m);
-  ~PathFinderBase() = default;
-
-  PathFinderBase(const PathFinderBase&) = delete;
-  PathFinderBase(PathFinderBase&&) = delete;
-  PathFinderBase& operator=(const PathFinderBase&) = delete;
-  PathFinderBase& operator=(PathFinderBase&&) = delete;
-
-  virtual const std::string_view& GetName() const = 0; 
-  virtual Path CalculatePath(WorldPos start, WorldPos end) = 0;
-
-protected:
-  const Map* m_Map;
-};
-
-
-class LinearPathFinder : public PathFinderBase {
-
-public:
-  LinearPathFinder(const Map* m): PathFinderBase(m) {}
-  Path CalculatePath(WorldPos start, WorldPos end) override;
-  const std::string_view& GetName() const override { return m_Name; }
-
-private:
-  const std::string_view m_Name = "Linear Path";
-};
-
-
-class BFS: public PathFinderBase {
-
-public:
-  BFS(const Map* m): PathFinderBase(m) {}
-  Path CalculatePath(WorldPos start, WorldPos end) override;
-  const std::string_view& GetName() const override { return m_Name; }
-
-private:
-  const std::string_view m_Name = "Breadth First Search";
-  std::unordered_map<TilePos, double, TilePosHash> m_Distance;
-  std::unordered_map<TilePos, TilePos, TilePosHash> m_CameFrom;
-};
-
-class Dijkstra: public PathFinderBase {
-
-public:
-  Dijkstra(const Map* m): PathFinderBase(m) {}
-  Path CalculatePath(WorldPos start, WorldPos end) override;
-  const std::string_view& GetName() const override { return m_Name; }
-
-private:
-  const std::string_view m_Name = "Dijkstra's Algorithm";
-  std::unordered_map<TilePos, double, TilePosHash> m_Cost;
-  std::unordered_map<TilePos, TilePos, TilePosHash> m_CameFrom;
-};
-
-
-class GBFS: public PathFinderBase {
-
-public:
-  GBFS(const Map* m): PathFinderBase(m) {}
-  Path CalculatePath(WorldPos start, WorldPos end) override;
-  const std::string_view& GetName() const override { return m_Name; }
-
-private:
-  static float Heuristic(const TilePos& a, const TilePos& b);
-  const std::string_view m_Name = "Greedy Best First Search";
-  std::unordered_map<TilePos, TilePos, TilePosHash> m_CameFrom;
-};
-
-
-
-std::unique_ptr<PathFinderBase> create(PathFinderType type, const Map* map);
-
-} // pathfinder namespace
-  
diff --git a/cpp/src/pathfinder/astar.cpp b/cpp/src/pathfinder/astar.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/cpp/src/pathfinder/astar.hpp b/cpp/src/pathfinder/astar.hpp
new file mode 100644
index 0000000..e69de29
diff --git a/cpp/src/pathfinder/base.cpp b/cpp/src/pathfinder/base.cpp
new file mode 100644
index 0000000..5468672
--- /dev/null
+++ b/cpp/src/pathfinder/base.cpp
@@ -0,0 +1,22 @@
+#include <memory>
+#include <cassert>
+#include <queue>
+
+#include "pathfinder/base.hpp"
+
+#include "log.hpp"
+#include "math.hpp"
+
+namespace pathfinder {
+
+PathFinderBase::PathFinderBase(const Map* map) : m_Map(map) {}
+
+// LinearPathFinder also lives here, since it is too small to get it's
+// own implementation file
+Path LinearPathFinder::CalculatePath(WorldPos start, WorldPos end)
+{
+  auto path = Path{end};
+  return path;
+}
+
+} // pathfinder namespace
diff --git a/cpp/src/pathfinder/base.hpp b/cpp/src/pathfinder/base.hpp
new file mode 100644
index 0000000..97ce18f
--- /dev/null
+++ b/cpp/src/pathfinder/base.hpp
@@ -0,0 +1,52 @@
+#pragma once
+
+#include <vector>
+#include <memory>
+#include <unordered_map>
+
+#include "math.hpp"
+#include "map.hpp"
+
+namespace pathfinder {
+
+using Path = std::vector<WorldPos>;
+
+enum class PathFinderType {
+  LINEAR = 1,
+  BFS,
+  DIJKSTRA,
+  GBFS,
+  COUNT,
+};
+
+class PathFinderBase {
+public:
+  PathFinderBase(const Map* m);
+  ~PathFinderBase() = default;
+
+  PathFinderBase(const PathFinderBase&) = delete;
+  PathFinderBase(PathFinderBase&&) = delete;
+  PathFinderBase& operator=(const PathFinderBase&) = delete;
+  PathFinderBase& operator=(PathFinderBase&&) = delete;
+
+  virtual const std::string_view& GetName() const = 0; 
+  virtual Path CalculatePath(WorldPos start, WorldPos end) = 0;
+
+protected:
+  const Map* m_Map;
+};
+
+
+class LinearPathFinder : public PathFinderBase {
+
+public:
+  LinearPathFinder(const Map* m): PathFinderBase(m) {}
+  Path CalculatePath(WorldPos start, WorldPos end) override;
+  const std::string_view& GetName() const override { return m_Name; }
+
+private:
+  const std::string_view m_Name = "Linear Path";
+};
+
+} // pathfinder namespace
+  
diff --git a/cpp/src/pathfinder/bfs.cpp b/cpp/src/pathfinder/bfs.cpp
new file mode 100644
index 0000000..747f410
--- /dev/null
+++ b/cpp/src/pathfinder/bfs.cpp
@@ -0,0 +1,66 @@
+#include <queue>
+
+#include "bfs.hpp"
+
+#include "base.hpp"
+#include "map.hpp"
+#include "math.hpp"
+
+namespace pathfinder {
+  
+Path BFS::CalculatePath(WorldPos start_world, WorldPos end_world) {
+    if (m_Map == nullptr) return {};
+
+    const TilePos start = m_Map->WorldToTile(start_world);
+    const TilePos end   = m_Map->WorldToTile(end_world);
+
+    if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
+        return {};
+    if (start == end) {
+        return {};
+    }
+    // clear previous run
+    m_CameFrom.clear();
+    m_Distance.clear();
+
+    std::queue<TilePos> frontier;
+    frontier.push(start);
+    m_CameFrom[start] = start;
+    m_Distance[start] = 0.0f;
+
+    // ---------------- build flow-field ----------------
+    bool early_exit = false;
+    while (!frontier.empty() && !early_exit) {
+        TilePos current = frontier.front();
+        frontier.pop();
+
+        for (TilePos next : m_Map->GetNeighbors(current)) {
+            if (m_CameFrom.find(next) == m_CameFrom.end()) {  // not visited
+                frontier.push(next);
+                m_Distance[next] = m_Distance[current] + 1.0f;
+                m_CameFrom[next] = current;
+
+                if (next == end) {          // early exit
+                    early_exit = true;
+                    break;
+                }
+            }
+        }
+    }
+
+    // --------------- reconstruct path -----------------
+    if (m_CameFrom.find(end) == m_CameFrom.end())
+        return {};                        // end not reached
+
+    Path path;
+    TilePos cur = end;
+    path.push_back(m_Map->TileToWorld(cur));
+    while (cur != start) {
+        cur = m_CameFrom[cur];
+        path.push_back(m_Map->TileToWorld(cur));
+    }
+    std::reverse(path.begin(), path.end());
+    return path;
+}
+
+} // pathfinder namespace
diff --git a/cpp/src/pathfinder/bfs.hpp b/cpp/src/pathfinder/bfs.hpp
new file mode 100644
index 0000000..f42124c
--- /dev/null
+++ b/cpp/src/pathfinder/bfs.hpp
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <string_view>
+#include <unordered_map>
+
+#include "base.hpp"
+
+#include "math.hpp"
+
+namespace pathfinder {
+  
+class BFS: public PathFinderBase {
+
+public:
+  BFS(const Map* m): PathFinderBase(m) {}
+  Path CalculatePath(WorldPos start, WorldPos end) override;
+  const std::string_view& GetName() const override { return m_Name; }
+
+private:
+  const std::string_view m_Name = "Breadth First Search";
+  std::unordered_map<TilePos, double, TilePosHash> m_Distance;
+  std::unordered_map<TilePos, TilePos, TilePosHash> m_CameFrom;
+};
+
+}
diff --git a/cpp/src/pathfinder/dijkstra.cpp b/cpp/src/pathfinder/dijkstra.cpp
new file mode 100644
index 0000000..4c7048f
--- /dev/null
+++ b/cpp/src/pathfinder/dijkstra.cpp
@@ -0,0 +1,73 @@
+#include <queue>
+
+#include "dijkstra.hpp"
+
+#include "base.hpp"
+#include "utils.hpp"
+#include "math.hpp"
+#include "map.hpp"
+
+namespace pathfinder {
+
+Path Dijkstra::CalculatePath(WorldPos start_world, WorldPos end_world)
+{
+  using QueueEntry = utils::QueueEntry;
+
+  if (!m_Map) return {};
+
+  const TilePos start = m_Map->WorldToTile(start_world);
+  const TilePos end   = m_Map->WorldToTile(end_world);
+
+  if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
+      return {};
+  if (start == end) return {};
+
+  // clear previous run
+  m_CameFrom.clear();
+  m_Cost.clear();
+
+  std::priority_queue<QueueEntry, std::vector<QueueEntry>, std::greater<>> frontier;
+  frontier.push({0.0f, start});
+  m_CameFrom[start] = start;   // sentinel
+  m_Cost[start]     = 0.0f;
+
+  while (!frontier.empty())
+  {
+      const QueueEntry current = frontier.top();
+      frontier.pop();
+
+      if (current.tile == end)          // early exit
+          break;
+
+      for (TilePos next : m_Map->GetNeighbors(current.tile))
+      {
+          // cost of moving to neighbour (uniform 1.0 matches original BFS)
+          const float newCost = m_Cost[current.tile] + m_Map->GetCost(next);
+
+          if (!m_Cost.count(next) || newCost < m_Cost[next])
+          {
+              m_Cost[next]     = newCost;
+              m_CameFrom[next] = current.tile;
+              frontier.push({newCost, next});
+          }
+      }
+  }
+
+  // reconstruct path
+  if (!m_CameFrom.count(end))
+      return {};                   // goal never reached
+
+  Path path;
+  TilePos cur = end;
+  path.push_back(m_Map->TileToWorld(cur));
+
+  while (cur != start)
+  {
+      cur = m_CameFrom[cur];
+      path.push_back(m_Map->TileToWorld(cur));
+  }
+  std::reverse(path.begin(), path.end());
+  return path;
+}
+
+} // pathfinder namespace
diff --git a/cpp/src/pathfinder/dijkstra.hpp b/cpp/src/pathfinder/dijkstra.hpp
new file mode 100644
index 0000000..4d867c8
--- /dev/null
+++ b/cpp/src/pathfinder/dijkstra.hpp
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <string_view>
+#include <unordered_map>
+
+#include "base.hpp"
+
+#include "map.hpp"
+#include "math.hpp"
+
+namespace pathfinder {
+
+class Dijkstra: public PathFinderBase {
+
+public:
+  Dijkstra(const Map* m): PathFinderBase(m) {}
+  Path CalculatePath(WorldPos start, WorldPos end) override;
+  const std::string_view& GetName() const override { return m_Name; }
+
+private:
+  const std::string_view m_Name = "Dijkstra's Algorithm";
+  std::unordered_map<TilePos, double, TilePosHash> m_Cost;
+  std::unordered_map<TilePos, TilePos, TilePosHash> m_CameFrom;
+};
+
+} // pathfinder namespace
diff --git a/cpp/src/pathfinder/gbfs.cpp b/cpp/src/pathfinder/gbfs.cpp
new file mode 100644
index 0000000..8b9bbfd
--- /dev/null
+++ b/cpp/src/pathfinder/gbfs.cpp
@@ -0,0 +1,71 @@
+#include <queue>
+
+#include "gbfs.hpp"
+
+#include "base.hpp"
+#include "math.hpp"
+#include "map.hpp"
+#include "pathfinder/utils.hpp"
+
+namespace pathfinder {
+
+float GBFS::Heuristic(const TilePos& a, const TilePos& b)
+{
+    return static_cast<float>(std::abs(a.x- b.x) + std::abs(a.y - b.y));
+}
+
+Path GBFS::CalculatePath(WorldPos start_world, WorldPos end_world)
+{
+  using QueueEntry = pathfinder::utils::QueueEntry;
+
+  if (!m_Map) return {};
+
+  const TilePos start = m_Map->WorldToTile(start_world);
+  const TilePos end   = m_Map->WorldToTile(end_world);
+
+  if (!m_Map->IsTilePosValid(start) || !m_Map->IsTilePosValid(end))
+      return {};
+  if (start == end) return {};
+
+  m_CameFrom.clear();
+
+  std::priority_queue<QueueEntry, std::vector<QueueEntry>, std::greater<>> frontier;
+  frontier.push({Heuristic(start, end), start});
+  m_CameFrom[start] = start;  // sentinel
+
+  while (!frontier.empty())
+  {
+      const QueueEntry current = frontier.top();
+      frontier.pop();
+
+      if (current.tile == end)  // early exit
+          break;
+
+      for (TilePos next : m_Map->GetNeighbors(current.tile))
+      {
+          if (!m_CameFrom.count(next))  // not visited
+          {
+              m_CameFrom[next] = current.tile;
+              frontier.push({Heuristic(end, next), next});
+          }
+      }
+  }
+
+  // reconstruct path
+  if (!m_CameFrom.count(end))
+      return {};  // goal never reached
+
+  Path path;
+  TilePos cur = end;
+  path.push_back(m_Map->TileToWorld(cur));
+
+  while (cur != start)
+  {
+      cur = m_CameFrom[cur];
+      path.push_back(m_Map->TileToWorld(cur));
+  }
+  std::reverse(path.begin(), path.end());
+  return path;
+}
+
+} // pathfinder namespace
diff --git a/cpp/src/pathfinder/gbfs.hpp b/cpp/src/pathfinder/gbfs.hpp
new file mode 100644
index 0000000..dc50585
--- /dev/null
+++ b/cpp/src/pathfinder/gbfs.hpp
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <string_view>
+#include <unordered_map>
+
+#include "base.hpp"
+
+#include "map.hpp"
+#include "math.hpp"
+
+namespace pathfinder {
+
+class GBFS: public PathFinderBase {
+
+public:
+  GBFS(const Map* m): PathFinderBase(m) {}
+  Path CalculatePath(WorldPos start, WorldPos end) override;
+  const std::string_view& GetName() const override { return m_Name; }
+
+private:
+  static float Heuristic(const TilePos& a, const TilePos& b);
+  const std::string_view m_Name = "Greedy Best First Search";
+  std::unordered_map<TilePos, TilePos, TilePosHash> m_CameFrom;
+};
+
+} // pathfinder namespace
diff --git a/cpp/src/pathfinder/linear.cpp b/cpp/src/pathfinder/linear.cpp
new file mode 100644
index 0000000..e69de29
diff --git a/cpp/src/pathfinder/linear.hpp b/cpp/src/pathfinder/linear.hpp
new file mode 100644
index 0000000..e69de29
diff --git a/cpp/src/pathfinder/utils.cpp b/cpp/src/pathfinder/utils.cpp
new file mode 100644
index 0000000..e2bc5a4
--- /dev/null
+++ b/cpp/src/pathfinder/utils.cpp
@@ -0,0 +1,35 @@
+#include <memory>
+
+#include "utils.hpp"
+
+#include "base.hpp"
+#include "map.hpp"
+#include "math.hpp"
+#include "log.hpp"
+#include "pathfinder/bfs.hpp"
+#include "pathfinder/dijkstra.hpp"
+#include "pathfinder/gbfs.hpp"
+
+namespace pathfinder {
+namespace utils {
+
+std::unique_ptr<PathFinderBase> create(PathFinderType type, const Map* map) {
+  using namespace pathfinder;
+  switch (type) {
+    case PathFinderType::LINEAR:
+      return std::move(std::make_unique<LinearPathFinder>(map));
+    case PathFinderType::BFS:
+      return std::move(std::make_unique<BFS>(map));
+    case PathFinderType::DIJKSTRA:
+      return std::move(std::make_unique<Dijkstra>(map));
+    case PathFinderType::GBFS:
+      return std::move(std::make_unique<GBFS>(map));
+    case PathFinderType::COUNT:
+      LOG_WARNING("Incorrect pathfinder type");
+      return nullptr;
+  };
+  return nullptr;
+}
+
+} // utils namespace
+} // pathfinding namespace
diff --git a/cpp/src/pathfinder/utils.hpp b/cpp/src/pathfinder/utils.hpp
new file mode 100644
index 0000000..c40e4fb
--- /dev/null
+++ b/cpp/src/pathfinder/utils.hpp
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <memory>
+
+#include "pathfinder/base.hpp"
+
+#include "map.hpp"
+#include "math.hpp"
+
+namespace pathfinder {
+namespace utils {
+
+struct QueueEntry
+{
+    float cost;
+    TilePos tile;
+
+    // min-heap -> smallest cost on top
+    bool operator>(const QueueEntry& o) const noexcept { return cost > o.cost; }
+};
+
+std::unique_ptr<pathfinder::PathFinderBase> create(pathfinder::PathFinderType type, const Map* map);
+
+} // utils namespace
+} // pathfinding namespace
diff --git a/cpp/src/pathfindingdemo.cpp b/cpp/src/pathfindingdemo.cpp
index f58f9dd..55e7c0a 100644
--- a/cpp/src/pathfindingdemo.cpp
+++ b/cpp/src/pathfindingdemo.cpp
@@ -9,14 +9,15 @@
 #include "log.hpp"
 #include "map.hpp"
 #include "user_input.hpp"
-#include "pathfinder.hpp"
+#include "pathfinder/base.hpp"
+#include "pathfinder/utils.hpp"
 
 PathFindingDemo::PathFindingDemo(int width, int height) :
   m_Map(width, height)
 {
   LOG_DEBUG(".");
   // set default pathfinder method
-  m_PathFinder = pathfinder::create(pathfinder::PathFinderType::LINEAR, (const Map*)&m_Map);
+  m_PathFinder = pathfinder::utils::create(pathfinder::PathFinderType::LINEAR, (const Map*)&m_Map);
 }
 
 PathFindingDemo::~PathFindingDemo() { LOG_DEBUG("."); }
@@ -88,7 +89,7 @@ void PathFindingDemo::HandleActions(const std::vector<UserAction> &actions) {
     } else if (action.type == UserAction::Type::SELECT_PATHFINDER) {
       using namespace pathfinder;
       PathFinderType type = static_cast<PathFinderType>(action.Argument.number);
-      m_PathFinder = pathfinder::create(type, (const Map*)&m_Map);
+      m_PathFinder = pathfinder::utils::create(type, (const Map*)&m_Map);
       LOG_INFO("Switched to path finding method: ", m_PathFinder->GetName());
     }
   };
diff --git a/cpp/src/pathfindingdemo.hpp b/cpp/src/pathfindingdemo.hpp
index 126d1db..20532b3 100644
--- a/cpp/src/pathfindingdemo.hpp
+++ b/cpp/src/pathfindingdemo.hpp
@@ -9,7 +9,7 @@
 #include "log.hpp"
 #include "map.hpp"
 #include "user_input.hpp"
-#include "pathfinder.hpp"
+#include "pathfinder/base.hpp"
 
 class PathFindingDemo {
 public: