function to search the path search for a path on a grid avoiding obstacles grid 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1 0 init 0 0 goal lengrid 1 lengrid0 1 cost 1 heuristic 0 lengrid0 for in rangelengrid heuristic 0 for row in rangelengrid0 for col in rangelengrid for i in rangelengrid for j in rangelengrid0 heuristicij absi goal0 absj goal1 if gridij 1 heuristicij 99 path action searchgrid init goal cost heuristic path doctest normalizewhitespace 0 0 1 0 2 0 3 0 4 0 4 1 4 2 4 3 3 3 2 3 2 4 2 5 3 5 4 5 action doctest normalizewhitespace 0 0 0 0 0 0 2 0 0 0 0 0 2 0 0 0 3 3 2 0 0 0 0 2 2 3 3 3 0 2 all coordinates are given in format y x the cost map which pushes the path closer to the goal added extra penalty in the heuristic map left down right up function to search the path search for a path on a grid avoiding obstacles grid 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1 0 init 0 0 goal len grid 1 len grid 0 1 cost 1 heuristic 0 len grid 0 for _ in range len grid heuristic 0 for row in range len grid 0 for col in range len grid for i in range len grid for j in range len grid 0 heuristic i j abs i goal 0 abs j goal 1 if grid i j 1 heuristic i j 99 path action search grid init goal cost heuristic path doctest normalize_whitespace 0 0 1 0 2 0 3 0 4 0 4 1 4 2 4 3 3 3 2 3 2 4 2 5 3 5 4 5 action doctest normalize_whitespace 0 0 0 0 0 0 2 0 0 0 0 0 2 0 0 0 3 3 2 0 0 0 0 2 2 3 3 3 0 2 the reference grid the action grid cost from starting cell to destination cell flag that is set when search is complete flag set if we can t find expand to choose the least costliest action so as to move closer to the goal to try out different valid actions we get the reverse path from here 0 are free path whereas 1 s are obstacles all coordinates are given in format y x the cost map which pushes the path closer to the goal added extra penalty in the heuristic map
from __future__ import annotations

DIRECTIONS = [
    [-1, 0],  
    [0, -1],  
    [1, 0],  
    [0, 1],  
]



def search(
    grid: list[list[int]],
    init: list[int],
    goal: list[int],
    cost: int,
    heuristic: list[list[int]],
) -> tuple[list[list[int]], list[list[int]]]:
    
    closed = [
        [0 for col in range(len(grid[0]))] for row in range(len(grid))
    ]  
    closed[init[0]][init[1]] = 1
    action = [
        [0 for col in range(len(grid[0]))] for row in range(len(grid))
    ]  

    x = init[0]
    y = init[1]
    g = 0
    f = g + heuristic[x][y]  
    cell = [[f, g, x, y]]

    found = False  
    resign = False  

    while not found and not resign:
        if len(cell) == 0:
            raise ValueError("Algorithm is unable to find solution")
        else:  
            cell.sort()
            cell.reverse()
            next_cell = cell.pop()
            x = next_cell[2]
            y = next_cell[3]
            g = next_cell[1]

            if x == goal[0] and y == goal[1]:
                found = True
            else:
                for i in range(len(DIRECTIONS)):  
                    x2 = x + DIRECTIONS[i][0]
                    y2 = y + DIRECTIONS[i][1]
                    if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]):
                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:
                            g2 = g + cost
                            f2 = g2 + heuristic[x2][y2]
                            cell.append([f2, g2, x2, y2])
                            closed[x2][y2] = 1
                            action[x2][y2] = i
    invpath = []
    x = goal[0]
    y = goal[1]
    invpath.append([x, y])  
    while x != init[0] or y != init[1]:
        x2 = x - DIRECTIONS[action[x][y]][0]
        y2 = y - DIRECTIONS[action[x][y]][1]
        x = x2
        y = y2
        invpath.append([x, y])

    path = []
    for i in range(len(invpath)):
        path.append(invpath[len(invpath) - 1 - i])
    return path, action


if __name__ == "__main__":
    grid = [
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],  
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 1, 0],
        [0, 0, 0, 0, 1, 0],
    ]

    init = [0, 0]
    
    goal = [len(grid) - 1, len(grid[0]) - 1]
    cost = 1

    
    heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))]
    for i in range(len(grid)):
        for j in range(len(grid[0])):
            heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1])
            if grid[i][j] == 1:
                
                heuristic[i][j] = 99

    path, action = search(grid, init, goal, cost, heuristic)

    print("ACTION MAP")
    for i in range(len(action)):
        print(action[i])

    for i in range(len(path)):
        print(path[i])