algorithms/a_star.py

@@ -4,6 +4,10 @@ from dataclasses import dataclass, field
 from enum import Enum, unique
 from typing import Tuple, Optional, List
 
+from common.constants import ROWS, COLUMNS
+
+FREE_FIELD = ' '
+
 
 @unique
 class Direction(Enum):
@@ -54,21 +58,57 @@ def child_node(node: Node, action: Action) -> Node:
     return Node(state=next_state, parent=node, action=action)
 
 
-def actions(state: State) -> List[Action]:
+def next_position(current_position: Tuple[int, int], direction: Direction) -> Tuple[int, int]:
-    pass
+    x1, y1 = direction.value
+    x2, y2 = current_position
+    return x1 + x2, y1 + y2
 
 
-def result(state: State, action: Action) -> State:
+def valid_move(position: Tuple[int, int], grid: List[List[str]]) -> bool:
+    row, col = position
+    return grid[row][col] == FREE_FIELD
+
+
+def actions(state: State, grid: List[List[str]]) -> List[Action]:
+    possible_actions = [Action.FORWARD, Action.TURN_LEFT, Action.TURN_RIGHT]
+    row, col = state.position
+    direction = state.direction
+
+    if direction == Direction.UP and row == 0:
+        remove_forward(possible_actions)
+    if direction == Direction.DOWN and row == ROWS - 1:
+        remove_forward(possible_actions)
+    if direction == Direction.LEFT and col == 0:
+        remove_forward(possible_actions)
+    if direction == Direction.RIGHT and col == COLUMNS - 1:
+        remove_forward(possible_actions)
+
+    if not valid_move(next_position(state.position, direction), grid):
+        remove_forward(possible_actions)
+
+    return possible_actions
+
+
+def remove_forward(possible_actions: List[Action]) -> None:
+    if Action.FORWARD in possible_actions:
+        possible_actions.remove(Action.FORWARD)
+
+
+def result(state: State, action: Action, grid: List[List[str]]) -> State:
     pass
 
 
 def goal_test(state: State, goal_list: List[Tuple[int, int]]) -> bool:
-    pass
+    return state.position in goal_list
 
 
-def h(state: State) -> int:
+def h(state: State, goal: Tuple[int, int]) -> int:
-    pass
+    """heuristics that calculates Manhattan distance between current position and goal"""
+    x1, y1 = state.position
+    x2, y2 = goal
+    return abs(x1 - x2) + abs(y1 - y2)  # Manhattan distance
 
 
-def f(state: State) -> int:
+def f(current_node: Node, goal: Tuple[int, int]) -> int:
-    pass
+    """f(n) = g(n) + h(n), g stands for current cost, h for heuristics"""
+    return current_node.cost + h(state=current_node.state, goal=goal)