1 files changed, 100 insertions, 0 deletions
diff --git a/include/algs.lua b/include/algs.lua
new file mode 100644
index 0000000..ad0016a
--- /dev/null
+++ b/include/algs.lua
@@ -0,0 +1,100 @@
+require "include.constants"
+local Queue = require "include.queue"
+
+local function can_move(x, y, map)
+    return map[y][x] == MAP_SPACE
+end
+
+---@param x number
+---@param y number
+---@param origin table
+---@param map table
+---@param queue table
+local function push_moves(x, y, origin, map, queue)
+    -- UP
+    if can_move(x, y -1, map) then
+        queue:push({
+            x = x,
+            y = y - 1,
+            origin = origin
+        })
+        map[y-1][x] = MAP_VISITED
+    end
+    -- DOWN
+    if can_move(x, y +1, map) then
+        queue:push({
+            x = x,
+            y = y + 1,
+            origin = origin
+        })
+        map[y+1][x] = MAP_VISITED
+    end
+    -- LEFT
+    if can_move(x - 1, y, map) then
+        queue:push({
+            x = x - 1,
+            y = y,
+            origin = origin
+        })
+        map[y][x - 1] = MAP_VISITED
+    end
+    -- RIGHT
+    if can_move(x + 1, y, map) then
+        queue:push({
+            x = x + 1,
+            y = y,
+            origin = origin
+        })
+        map[y][x+1] = MAP_VISITED
+    end
+end
+
+---
+---@param start_pos table [x, y]
+---@param target_pos table [x, y]
+---@param map table 2D map array
+---@return table best move to target [x, y]
+---
+local function pathfind(start_pos, target_pos, map)
+    local queue = Queue:new()
+    local visit_map = {}
+    for k, v in ipairs(map) do
+        row = {}
+        for ik, iv in ipairs(v) do
+            row[ik] = iv
+        end
+        visit_map[k] = row
+    end
+
+    push_moves(start_pos.x, start_pos.y, nil, visit_map, queue)
+    while queue:empty() ~= true do
+        local pos = queue:pop()
+        if (pos.x == target_pos.x and pos.y == target_pos.y) then
+            return { dx = pos.origin.x - start_pos.x, dy = pos.origin.y - start_pos.y }
+        end
+        origin = pos.origin or { x = pos.x, y = pos.y }
+        push_moves(pos.x, pos.y, origin, map, queue)
+    end
+    return { dx = 0, dy = 0 }
+end
+
+---@param map table
+local function print_map(map)
+    for i = 1, #map do
+        row = map[i]
+        line = ""
+        for j = 1, #row do
+            if row[j] == MAP_WALL then
+                line = line .. "# "
+            else
+                line = line .. "  "
+            end
+        end
+        print(line)
+    end
+end
+
+return {
+    pathfind = pathfind,
+    print_map = print_map,
+}