let pathfinding (start: Position) (goal: Position) (grid: Grid<_>) =
    let frontier = [ (start, 0) ]
    let came_from = Map.singleton start None
    let cost_so_far = Map.singleton start 0
    let rec loop (frontier, came_from, cost_so_far) =
        match frontier with
        | [] -> came_from
        | (current, _) :: frontier ->
            if current = goal then came_from else
            getWalkableNeighbord current grid
            |> Array.fold (fun (frontier, came_from, cost_so_far) next ->
                let new_cost = (Map.find current cost_so_far) + costOfMovement current next
                if not (cost_so_far |> Map.containsKey next) || new_cost < (Map.find next cost_so_far) then
                    let cost_so_far = cost_so_far |> Map.add next new_cost
                    let priority = new_cost + distance goal next
                    let frontier = List.Cons ((next, priority), frontier) |> List.sortBy snd
                    let came_from = came_from |> Map.add next (Some current)
                    (frontier, came_from, cost_so_far)
                else
                    (frontier, came_from, cost_so_far)
            ) (frontier, came_from, cost_so_far)
            |> loop
    let came_from = loop (frontier, came_from, cost_so_far)