function desired_heading = select_local_heading(vehicle, targetXY, map, scenario)
%SELECT_LOCAL_HEADING Very simple local planner for Phase 2.
%
% It checks candidate headings around the direct target direction and scores
% each one according to traversability, obstacles, slope and friction.
% This is not yet MPC: it is only a practical placeholder to keep the agents
% moving without immediately driving through obstacles.

    x = vehicle.state.x;
    y = vehicle.state.y;

    direct_heading = atan2(targetXY(2) - y, targetXY(1) - x);
    offsets_deg = [-80, -60, -40, -25, -12, 0, 12, 25, 40, 60, 80];

    bestScore = Inf;
    desired_heading = direct_heading;

    for k = 1:numel(offsets_deg)
        heading = direct_heading + deg2rad(offsets_deg(k));
        score = score_heading_ray(x, y, heading, direct_heading, targetXY, map, scenario);
        if score < bestScore
            bestScore = score;
            desired_heading = heading;
        end
    end
end

function score = score_heading_ray(x, y, heading, direct_heading, targetXY, map, scenario)

    lookahead = scenario.control.local_lookahead_m;
    ds = scenario.control.ray_sample_step_m;
    distances = ds:ds:lookahead;

    obstaclePenalty = 0;
    slopePenalty = 0;
    lowMuPenalty = 0;
    outOfMapPenalty = 0;
    unknownPenalty = 0;

    for r = distances
        xs = x + r * cos(heading);
        ys = y + r * sin(heading);

        local = get_local_terrain(map, xs, ys);

        % Ostacoli / celle non attraversabili: penalità molto forte
        if ~local.traversable || local.obstacle
            obstaclePenalty = obstaclePenalty + 1e6 / max(r, 0.5);
        end

        % Penalità fuori mappa
        if ~isfinite(local.slope_deg)
            outOfMapPenalty = outOfMapPenalty + 1e6 / max(r, 0.5);
            continue;
        end

        % Penalità pendenza
        slopePenalty = slopePenalty + max(0, local.slope_deg - 16)^2 * 0.03;

        % Penalità basso attrito
        lowMuPenalty = lowMuPenalty + max(0, 0.45 - local.mu)^2 * 30;

        % Penalità celle ignote, se map.known esiste
        if isfield(map, 'known') && ~isempty(map.known)
            isKnown = is_known_xy(map, xs, ys);
            if ~isKnown
                unknownPenalty = unknownPenalty + 50 / max(r, 0.5); %NON TROPPO ALTA ALTRIMENTI PENALIZZA I FOLLOWER
            end
        end
    end

    headingDeviation = abs(wrapToPiLocal(heading - direct_heading));
    headingPenalty = 15 * headingDeviation;

    % Reward progresso verso target
    xEnd = x + lookahead * cos(heading);
    yEnd = y + lookahead * sin(heading);

    currentDistance = hypot(targetXY(1) - x, targetXY(2) - y);
    endDistance     = hypot(targetXY(1) - xEnd, targetXY(2) - yEnd);

    progressReward = 2.0 * (currentDistance - endDistance);

    score = obstaclePenalty ...
          + slopePenalty ...
          + lowMuPenalty ...
          + outOfMapPenalty ...
          + unknownPenalty ...
          + headingPenalty ...
          - progressReward;
end
function known = is_known_xy(map, x, y)

    known = true;

    if ~isfield(map, 'known') || isempty(map.known)
        return;
    end

    if ~isfield(map, 'params') || ~isfield(map.params, 'res_m')
        return;
    end

    res = map.params.res_m;
    sz = size(map.known);

    i = floor(y / res) + 1;
    j = floor(x / res) + 1;

    if i < 1 || i > sz(1) || j < 1 || j > sz(2)
        known = false;
        return;
    end

    known = map.known(i, j);
end