"""astar.py -- A* path planner for saltybot social navigation."""

import heapq
import numpy as np


def astar(grid: np.ndarray, start: tuple, goal: tuple,
          obstacle_val: int = 100) -> list | None:
    """
    A* on a 2D occupancy grid (row, col indexing).

    Args:
        grid: 2D numpy array, values 0=free, >=obstacle_val=obstacle
        start: (row, col) start cell
        goal: (row, col) goal cell
        obstacle_val: cells with value >= obstacle_val are blocked

    Returns:
        List of (row, col) tuples from start to goal, or None if no path.
    """
    rows, cols = grid.shape

    def h(a, b):
        return abs(a[0] - b[0]) + abs(a[1] - b[1])  # Manhattan heuristic

    open_set = []
    heapq.heappush(open_set, (h(start, goal), 0, start))
    came_from = {}
    g_score = {start: 0}

    # 8-directional movement
    neighbors_delta = [
        (-1, -1), (-1, 0), (-1, 1),
        (0, -1),           (0, 1),
        (1, -1),  (1, 0),  (1, 1),
    ]

    while open_set:
        _, cost, current = heapq.heappop(open_set)

        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            return list(reversed(path))

        if cost > g_score.get(current, float('inf')):
            continue

        for dr, dc in neighbors_delta:
            nr, nc = current[0] + dr, current[1] + dc
            if not (0 <= nr < rows and 0 <= nc < cols):
                continue
            if grid[nr, nc] >= obstacle_val:
                continue
            move_cost = 1.414 if (dr != 0 and dc != 0) else 1.0
            new_g = g_score[current] + move_cost
            neighbor = (nr, nc)
            if new_g < g_score.get(neighbor, float('inf')):
                g_score[neighbor] = new_g
                f = new_g + h(neighbor, goal)
                came_from[neighbor] = current
                heapq.heappush(open_set, (f, new_g, neighbor))

    return None  # No path found


def inflate_obstacles(grid: np.ndarray, inflation_radius_cells: int) -> np.ndarray:
    """Inflate obstacles for robot footprint safety."""
    from scipy.ndimage import binary_dilation

    obstacle_mask = grid >= 50
    kernel = np.ones(
        (2 * inflation_radius_cells + 1, 2 * inflation_radius_cells + 1),
        dtype=bool,
    )
    inflated = binary_dilation(obstacle_mask, structure=kernel)
    result = grid.copy()
    result[inflated] = 100
    return result
