pub fn astar(self: *@This(), start: [2]usize, goal: [2]usize, dir: anytype, heuristics: anytype) !T {
var CostGrid = switch (@typeInfo(T)) {
.Int => [_][GRID_COLS]T{[_]T{std.math.maxInt(T)} ** GRID_COLS} ** GRID_ROWS,
.Float => [_][GRID_COLS]T{[_]T{std.math.floatMax(T)} ** GRID_COLS} ** GRID_ROWS,
else => @compileError("unsupported type: " ++ @typeName(T)),
};
CostGrid[start[0]][start[1]] = 0;
var queue = std.PriorityQueue(QueueType, void, comptime lessThan).init(self.arena.allocator(), {});
defer queue.deinit();
try queue.add(.{ .state = start, .cost = 0 });
while (queue.count() > 0) {
const curr = queue.remove();
if (std.meta.eql(curr.state, goal)) {
return curr.cost;
}
for (dir) |d| {
const diffrow = @intCast(isize, curr.state[0]) + d[0];
if (diffrow < 0 or diffrow >= GRID_ROWS) continue;
const dr = @intCast(usize, diffrow);
const diffcol = @intCast(isize, curr.state[1]) + d[1];
if (diffcol < 0 or diffcol >= GRID_COLS) continue;
const dc = @intCast(usize, diffcol);
const cost = CostGrid[curr.state[0]][curr.state[1]] + self.grid[dr][dc];
if (cost < CostGrid[dr][dc]) {
CostGrid[dr][dc] = cost;
try queue.add(
.{ .state = .{ dr, dc }, .cost = cost + try heuristics(.{ dr, dc }, goal) },
);
}
}
}
return error.PathNotFound;
}