function cost = build_cost_map(map, scenario)
%BUILD_COST_MAP  Cost map completa per A* basata su semantica e pendenza.
%
% Questa funzione costruisce la mappa di costo completa una sola volta.
%
% Include:
%   1) costo base da classe semantica;
%   2) gradiente logaritmico vicino ai confini tra classi;
%   3) penalità slope;
%   4) override finale degli ostacoli a scenario.planning.cost_obstacle.
%
% NON include:
%   - logica map.known;
%   - sostituzione delle celle unknown;
%   - penalità mu.
%
% La sostituzione delle celle non note con cost_unknown deve essere fatta
% nel planner helper, creando una copia locale di map.cost_all.

    pl = scenario.planning;
    sz = size(map.X);

    %% ── 1. Costo base per classe semantica ──────────────────────────────

    if isfield(map, 'semantic_class') && ~isempty(map.semantic_class)

        class_ids = double(map.semantic_class);

        % Safety: clip nel range valido di cost_per_class
        class_ids = max(1, min(numel(pl.cost_per_class), class_ids));

        cost_per_class = double(pl.cost_per_class(:));

        % cost_base(i,j) = cost_per_class(class_ids(i,j))
        cost_base = cost_per_class(class_ids);

    else
        warning('build_cost_map:noSemantic', ...
            'map.semantic_class non trovata, uso costo uniforme.');

        class_ids = ones(sz);
        cost_per_class = double(pl.cost_per_class(:));
        cost_base = 2.0 * ones(sz);
    end

    cost = cost_base;

    %% ── 2. Maschera ostacoli ────────────────────────────────────────────

    obstacle_mask = false(sz);

    if isfield(map, 'obstacle') && ~isempty(map.obstacle)
        obstacle_mask = obstacle_mask | logical(map.obstacle);
    end

    % Doppia sicurezza: se una classe ha costo altissimo, trattala come ostacolo
    obstacle_mask = obstacle_mask | (cost_base >= 0.5 * pl.cost_obstacle);

    %% ── 3. Gradiente logaritmico ai confini semantici ───────────────────
    %
    % Obiettivo:
    %   - evitare cambi netti di costo tra classi;
    %   - far stare il veicolo più al centro dei corridoi buoni;
    %   - creare una penalità esterna agli ostacoli;
    %   - NON ridurre mai il costo dentro gli ostacoli.
    %
    % Il gradiente è calcolato in log-space perché i costi possono passare
    % da 1 a 1e6.

    if isfield(pl, 'semantic_gradient_radius_cells')
        R_grad = pl.semantic_gradient_radius_cells;
    else
        R_grad = 4;
    end

    if R_grad > 0
        cost_blended = semantic_log_blend(class_ids, cost_per_class, R_grad, sz);

        % Lo tratto come "costo di gradiente" aggiunto al costo base.
        % Può essere positivo o negativo:
        %   - positivo sul lato buono vicino a una classe costosa;
        %   - negativo sul lato costoso vicino a una classe economica.
        semantic_gradient_cost = cost_blended - cost_base;

        cost = cost + semantic_gradient_cost;
    end

    %% ── 4. Penalità pendenza ────────────────────────────────────────────

    if isfield(map, 'slope_deg') && ~isempty(map.slope_deg)

        if isfield(pl, 'slope_threshold_deg')
            slope_thr = pl.slope_threshold_deg;
        else
            slope_thr = 8.0;
        end

        slope_pen = zeros(sz);

        valid = isfinite(map.slope_deg) & ~obstacle_mask;

        slope_pen(valid) = ...
            pl.cost_slope_w * max(0, map.slope_deg(valid) - slope_thr);

        cost = cost + slope_pen;
    end

    %% ── 5. Override finale ostacoli ─────────────────────────────────────
    %
    % Fondamentale:
    %   - fuori dall'ostacolo il gradiente deve esistere;
    %   - dentro l'ostacolo il costo deve rimanere esattamente cost_obstacle.

    cost(obstacle_mask) = pl.cost_obstacle;

    %% ── 6. Sicurezza numerica ───────────────────────────────────────────

    cost = max(cost, 1.0);

end


%% =========================================================================
%  Helper: blend logaritmico tra classi vicine
%% =========================================================================
function cost_blended = semantic_log_blend(class_ids, cost_per_class, R_grad, sz)

    n_classes = numel(cost_per_class);

    log_cost_per_class = log(max(cost_per_class, 1.0));

    num = zeros(sz);
    den = zeros(sz);

    for c = 1:n_classes

        mask_c = class_ids == c;

        if ~any(mask_c(:))
            continue;
        end

        % Distanza in celle dalla classe c
        d = bwdist(mask_c);

        % Celle entro il raggio di gradiente
        band = d <= R_grad;

        if ~any(band(:))
            continue;
        end

        % Peso decrescente con la distanza.
        % d = 0      → peso massimo
        % d = R_grad → peso minimo positivo
        w = zeros(sz);
        w(band) = (R_grad + 1 - d(band)) ./ (R_grad + 1);

        num = num + w * log_cost_per_class(c);
        den = den + w;
    end

    den = max(den, eps);

    % Ritorno dallo spazio logaritmico allo spazio normale
    cost_blended = exp(num ./ den);

end