function path_xy = astar_path(start_xy, goal_xy, cost, map, scenario)
%ASTAR_PATH  A* su griglia con costi per cella.
%
%   Input:
%     start_xy : [x, y] coordinate mondo del punto di partenza [m]
%     goal_xy  : [x, y] coordinate mondo del goal [m]
%     cost     : matrice di costo per cella (da build_cost_map)
%     map      : map struct (per coordinate X, Y e res_m)
%     scenario : per parametri planning (max_iter, grid_res_m)
%
%   Output:
%     path_xy : [N x 2] matrice di waypoint in coordinate mondo [m].
%               Se irraggiungibile, ritorna matrice vuota.
%
%   STRATEGIA: per velocita', sotto-campiona la griglia da res_m nativo
%   a planning.grid_res_m. A* gira sulla griglia ridotta. Il path poi
%   torna in coordinate mondo per essere usato dal leader.

    pl = scenario.planning;
    res_native = map.params.res_m;
    step_factor = max(1, round(pl.grid_res_m / res_native));

    %% ── 1. Sotto-campionamento ──────────────────────────────────────────
    Xg = map.X(1:step_factor:end, 1:step_factor:end);
    Yg = map.Y(1:step_factor:end, 1:step_factor:end);
    Cg = cost(1:step_factor:end, 1:step_factor:end);

    [n_rows, n_cols] = size(Xg);
    grid_res = pl.grid_res_m;

    %% ── 2. Conversione start/goal in indici griglia ────────────────────
    [si, sj] = world_to_grid(start_xy(1), start_xy(2), Xg, Yg);
    [gi, gj] = world_to_grid(goal_xy(1),  goal_xy(2),  Xg, Yg);

    if ~in_bounds(si, sj, n_rows, n_cols) || ~in_bounds(gi, gj, n_rows, n_cols)
        warning('astar_path:outOfBounds', 'Start o goal fuori griglia.');
        path_xy = [];
        return;
    end

    %% ── 3. A* setup ─────────────────────────────────────────────────────
    g_score = inf(n_rows, n_cols);
    f_score = inf(n_rows, n_cols);
    came_i  = zeros(n_rows, n_cols);
    came_j  = zeros(n_rows, n_cols);
    closed  = false(n_rows, n_cols);
    open    = false(n_rows, n_cols);

    g_score(si, sj) = 0;
    f_score(si, sj) = heuristic([si sj], [gi gj], grid_res);
    open(si, sj)    = true;

nbr = [-1 -1; -1  0; -1  1; ...   % 8 standard
        0 -1;          0  1; ...
        1 -1;  1  0;  1  1; ...
       -2 -1; -2  1; ...           % knight (±2,±1) → ~26.6°
        2 -1;  2  1; ...
       -1 -2;  1 -2; ...           % knight (±1,±2) → ~63.4°
       -1  2;  1  2; ...
       -3 -1; -3  1; ...           % (±3,±1) → ~18.4°
        3 -1;  3  1; ...
       -1 -3;  1 -3; ...           % (±1,±3) → ~71.6°
       -1  3;  1  3; ...
       -3 -2; -3  2; ...           % (±3,±2) → ~33.7°
        3 -2;  3  2; ...
       -2 -3;  2 -3; ...           % (±2,±3) → ~56.3°
       -2  3;  2  3];
        % il costo è già aclcolato con la sqrt più avanti, quindi qualsiasi
        % sia il salto di celle che si fa non c'è problema
    %% ── 4. Loop principale ──────────────────────────────────────────────
    iter = 0;
    while any(open(:))
        iter = iter + 1;
        if iter > pl.max_iter
            warning('astar_path:maxIter', 'Limite iterazioni A* raggiunto.');
            path_xy = [];
            return;
        end

        % Estrai nodo open con f minimo
        f_open = f_score; f_open(~open) = inf;
        [~, lin] = min(f_open(:));
        [ci, cj] = ind2sub([n_rows, n_cols], lin);

        % Goal raggiunto?
        if ci == gi && cj == gj
            path_xy = reconstruct_path(came_i, came_j, ci, cj, Xg, Yg);
            return;
        end

        open(ci, cj)   = false;
        closed(ci, cj) = true;

        %% Espandi vicini
        for k = 1:size(nbr, 1)
            ni = ci + nbr(k, 1);
            nj = cj + nbr(k, 2);
            if ~in_bounds(ni, nj, n_rows, n_cols), continue; end
            if closed(ni, nj), continue; end

            % Step cost: distanza geometrica * media costi celle (start, end)
            ds   = grid_res * sqrt(nbr(k,1)^2 + nbr(k,2)^2);
            step_cost = ds * 0.5 * (Cg(ci, cj) + Cg(ni, nj));

            tentative_g = g_score(ci, cj) + step_cost;

            if tentative_g < g_score(ni, nj)
                came_i(ni, nj)  = ci;
                came_j(ni, nj)  = cj;
                g_score(ni, nj) = tentative_g;
                f_score(ni, nj) = tentative_g + heuristic([ni nj], [gi gj], grid_res);
                open(ni, nj)    = true;
            end
        end
    end

    % Open list svuotata senza raggiungere il goal: irraggiungibile
    warning('astar_path:unreachable', 'Goal non raggiungibile dalla start.');
    path_xy = [];
end

% =========================================================================
%  Helper functions
% =========================================================================
function h = heuristic(a, b, grid_res)
%   Octile distance: ammissibile per griglia 8-connected
    di = abs(a(1) - b(1));
    dj = abs(a(2) - b(2));
    h = grid_res * (max(di, dj) + (sqrt(2) - 1) * min(di, dj));
end

function [i, j] = world_to_grid(x, y, Xg, Yg)
    [~, j] = min(abs(Xg(1, :) - x));
    [~, i] = min(abs(Yg(:, 1) - y));
end

function tf = in_bounds(i, j, nr, nc)
    tf = i >= 1 && i <= nr && j >= 1 && j <= nc;
end

function path_xy = reconstruct_path(came_i, came_j, i, j, Xg, Yg)
    pts = [];
    while i > 0 && j > 0
        pts(end+1, :) = [Xg(i, j), Yg(i, j)]; %#ok<AGROW>
        ni = came_i(i, j);
        nj = came_j(i, j);
        if ni == 0 && nj == 0, break; end
        i = ni; j = nj;
    end
    path_xy = flipud(pts);
end