diff --git a/demo/include/utils/maze.hpp b/demo/include/utils/maze.hpp
index 8758ce11..ca81f646 100644
--- a/demo/include/utils/maze.hpp
+++ b/demo/include/utils/maze.hpp
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <algorithm>
 #include <optional>
 #include <utility>
 
@@ -56,7 +57,7 @@ class Maze {
         if (isPassableCell(wrappedPosition)) {
             return wrappedPosition;
         }
-        return position;
+        return {std::clamp(position.x, 0, width() - 1), std::clamp(position.y, 0, height() - 1)};
     }
 
     bool isDot(const Position& position) const {
@@ -87,7 +88,7 @@ struct BaseCell {
     using TileType = demo::TileType;
     using Position = demo::Position;
 
-    BaseCell(const Position& position, const TileType& type) : position(position), type(type) {};
+    BaseCell(const Position& position, const TileType& type) : position(position), type(type){};
 
     double manhattanDistance(const Position& other) const {
         return std::abs(position.x - other.x) + std::abs(position.y - other.y);
@@ -112,7 +113,7 @@ class MazeAdapter {
     using MazeStateConstPtr = std::shared_ptr<const MazeState>;
     using Position = demo::Position;
 
-    explicit MazeAdapter(Maze::ConstPtr maze) : maze_(std::move(maze)), cells_({maze_->width(), maze_->height()}) {};
+    explicit MazeAdapter(Maze::ConstPtr maze) : maze_(std::move(maze)), cells_({maze_->width(), maze_->height()}){};
 
     CellT& cell(const Position& position) const {
         if (!cells_[{position.x, position.y}]) {
diff --git a/demo/test/maze.cpp b/demo/test/maze.cpp
new file mode 100644
index 00000000..71b2b636
--- /dev/null
+++ b/demo/test/maze.cpp
@@ -0,0 +1,37 @@
+#include "utils/maze.hpp"
+
+#include <gtest/gtest.h>
+
+namespace utils::a_star {
+
+using namespace demo;
+
+TEST(MazeState, positionConsideringTunnel) {
+    const char str[] = {"### #"
+                        "#    "
+                        "### #"};
+    Maze maze{makeCustomMazeState({5, 3}, str)};
+
+    // Regular position -> no wrapping
+    EXPECT_EQ(maze.positionConsideringTunnel({1, 1}).x, 1);
+    EXPECT_EQ(maze.positionConsideringTunnel({1, 1}).y, 1);
+
+    // Regular position -> no wrapping
+    EXPECT_EQ(maze.positionConsideringTunnel({4, 1}).x, 4);
+    EXPECT_EQ(maze.positionConsideringTunnel({4, 1}).y, 1);
+
+    // Position out of maze, without tunnel -> clamping
+    EXPECT_EQ(maze.positionConsideringTunnel({5, 1}).x, 4);
+    EXPECT_EQ(maze.positionConsideringTunnel({5, 1}).y, 1);
+
+    // Regular position -> no wrapping
+    EXPECT_EQ(maze.positionConsideringTunnel({3, 0}).x, 3);
+    EXPECT_EQ(maze.positionConsideringTunnel({3, 0}).y, 0);
+
+    // Position out of maze, with tunnel -> wrapping
+    EXPECT_EQ(maze.positionConsideringTunnel({3, -1}).x, 3);
+    EXPECT_EQ(maze.positionConsideringTunnel({3, -1}).y, 2);
+}
+
+
+} // namespace utils::a_star
\ No newline at end of file