

from __future__ import annotations

UpperCAmelCase__ :     Optional[int]				    =    [
    [-1, 0],  # left
    [0, -1],  # down
    [1, 0],  # right
    [0, 1],  # up
]






def 					__lowercase      (     _A						,    _A						,    _A						,    _A						,    _A						,    )     ->						tuple[list[list[int]], list[list[int]]]:
      SCREAMING_SNAKE_CASE :      List[str]       =   [
          [0 for col in range(len(grid[0]			)			)] for row in range(len(_A			)			)
      ]  # the reference grid
      SCREAMING_SNAKE_CASE :      Any       =   1
      SCREAMING_SNAKE_CASE :      List[str]       =   [
          [0 for col in range(len(grid[0]			)			)] for row in range(len(_A			)			)
      ]  # the action grid

      SCREAMING_SNAKE_CASE :      Tuple       =   init[0]
      SCREAMING_SNAKE_CASE :      str       =   init[1]
      SCREAMING_SNAKE_CASE :      Any       =   0
      SCREAMING_SNAKE_CASE :      str       =   g + heuristic[x][y]  # cost from starting cell to destination cell
      SCREAMING_SNAKE_CASE :      str       =   [[f, g, x, y]]

      SCREAMING_SNAKE_CASE :      List[Any]       =   False  # flag that is set when search is complete
      SCREAMING_SNAKE_CASE :      Tuple       =   False  # flag set if we can't find expand

      while not found and not resign:
            if len(_A			) == 0:
                  raise ValueError("""Algorithm is unable to find solution"""			)
            else:  # to choose the least costliest action so as to move closer to the goal
                  cell.sort()
                  cell.reverse()
                  SCREAMING_SNAKE_CASE :      int       =   cell.pop()
                  SCREAMING_SNAKE_CASE :      Optional[int]       =   next_cell[2]
                  SCREAMING_SNAKE_CASE :      Optional[Any]       =   next_cell[3]
                  SCREAMING_SNAKE_CASE :      List[str]       =   next_cell[1]

                  if x == goal[0] and y == goal[1]:
                        SCREAMING_SNAKE_CASE :      Tuple       =   True
                  else:
                        for i in range(len(_A			)			):  # to try out different valid actions
                              SCREAMING_SNAKE_CASE :      int       =   x + DIRECTIONS[i][0]
                              SCREAMING_SNAKE_CASE :      Optional[int]       =   y + DIRECTIONS[i][1]
                              if xa >= 0 and xa < len(_A			) and ya >= 0 and ya < len(grid[0]			):
                                    if closed[xa][ya] == 0 and grid[xa][ya] == 0:
                                          SCREAMING_SNAKE_CASE :      Union[str, Any]       =   g + cost
                                          SCREAMING_SNAKE_CASE :      str       =   ga + heuristic[xa][ya]
                                          cell.append([fa, ga, xa, ya]			)
                                          SCREAMING_SNAKE_CASE :      List[str]       =   1
                                          SCREAMING_SNAKE_CASE :      Optional[int]       =   i
      SCREAMING_SNAKE_CASE :      int       =   []
      SCREAMING_SNAKE_CASE :      int       =   goal[0]
      SCREAMING_SNAKE_CASE :      Tuple       =   goal[1]
      invpath.append([x, y]			)  # we get the reverse path from here
      while x != init[0] or y != init[1]:
            SCREAMING_SNAKE_CASE :      List[str]       =   x - DIRECTIONS[action[x][y]][0]
            SCREAMING_SNAKE_CASE :      List[str]       =   y - DIRECTIONS[action[x][y]][1]
            SCREAMING_SNAKE_CASE :      List[Any]       =   xa
            SCREAMING_SNAKE_CASE :      Optional[int]       =   ya
            invpath.append([x, y]			)

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


if __name__ == "__main__":
  UpperCAmelCase__ :     Tuple				    =    [
      [0, 1, 0, 0, 0, 0],
      [0, 1, 0, 0, 0, 0],  # 0 are free path whereas 1's are obstacles
      [0, 1, 0, 0, 0, 0],
      [0, 1, 0, 0, 1, 0],
      [0, 0, 0, 0, 1, 0],
  ]

  UpperCAmelCase__ :     List[str]				    =    [0, 0]
  # all coordinates are given in format [y,x]
  UpperCAmelCase__ :     List[str]				    =    [len(grid) - 1, len(grid[0]) - 1]
  UpperCAmelCase__ :     int				    =    1

  # the cost map which pushes the path closer to the goal
  UpperCAmelCase__ :     Optional[Any]				    =    [[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])):
      UpperCAmelCase__ :     Optional[Any]				    =    abs(i - goal[0]) + abs(j - goal[1])
      if grid[i][j] == 1:
        # added extra penalty in the heuristic map
        UpperCAmelCase__ :     Optional[int]				    =    99

  UpperCAmelCase__ :     int				    =    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])







from __future__ import annotations

import numpy as np






def 					__lowercase      (     _A			)     ->						tuple[np.ndarray, np.ndarray]:
      SCREAMING_SNAKE_CASE     ,	SCREAMING_SNAKE_CASE :      Dict       =   np.shape(_A			)
      if rows != columns:
            SCREAMING_SNAKE_CASE :      int       =   (
                """'table' has to be of square shaped array but got a """
                F"{rows}x{columns} array:\n{table}"
            )
            raise ValueError(_A			)

      SCREAMING_SNAKE_CASE :      Union[str, Any]       =   np.zeros((rows, columns)			)
      SCREAMING_SNAKE_CASE :      Optional[Any]       =   np.zeros((rows, columns)			)
      for i in range(_A			):
            for j in range(_A			):
                  SCREAMING_SNAKE_CASE :      Tuple       =   sum(lower[i][k] * upper[k][j] for k in range(_A			)			)
                  if upper[j][j] == 0:
                        raise ArithmeticError("""No LU decomposition exists"""			)
                  SCREAMING_SNAKE_CASE :      Union[str, Any]       =   (table[i][j] - total) / upper[j][j]
            SCREAMING_SNAKE_CASE :      Optional[Any]       =   1
            for j in range(_A						,    _A			):
                  SCREAMING_SNAKE_CASE :      List[str]       =   sum(lower[i][k] * upper[k][j] for k in range(_A			)			)
                  SCREAMING_SNAKE_CASE :      Optional[Any]       =   table[i][j] - total
      return lower, upper


if __name__ == "__main__":
  import doctest

  doctest.testmod()




