function visualize_terrain_map(map, scenario)
%VISUALIZE_TERRAIN_MAP Basic visualization for Phase 1.
%   Each plot is shown in a separate figure (no subplots) and is also
%   exported as PNG + PDF in
%       <project>\..\latex_tesi_claude\Images\terrain_maps
%   so the figures can be inserted individually in the LaTeX document.
%
%   Note: the traversability-mask figure was removed because the A*
%   planner does not use map.traversable (astar_path.m and build_cost_map.m
%   work on the cost map only). map.traversable is still produced and used
%   by update_friction_map.m and get_local_terrain.m.

    % Output folder for the figures
    this_dir = fileparts(mfilename('fullpath'));
    out_dir  = fullfile(this_dir, '..', 'latex_tesi_claude', 'Images', ...
                        'terrain_maps');
    if ~exist(out_dir, 'dir')
        mkdir(out_dir);
    end
    fprintf('[terrain] Terrain map figures (PNG+PDF) will be saved in:\n    %s\n', out_dir);

    %% Figure 1 - Terrain elevation (3D surface)
    fig1 = figure('Name', 'Phase 1 - Terrain elevation', ...
        'NumberTitle', 'off', 'Color', 'w');
    surf(map.X, map.Y, map.z, 'EdgeColor', 'none');
    view(45, 45);
    axis tight;
    xlabel('X [m]'); ylabel('Y [m]'); zlabel('Elevation z [m]');
    title('Terrain elevation: dunes and valleys');
    hold on;
    plot3(scenario.start(:,1), scenario.start(:,2), 2 + interp2(map.X, map.Y, map.z, scenario.start(:,1), scenario.start(:,2)), 'ko', 'MarkerFaceColor', 'k');
    plot3(scenario.goal(1), scenario.goal(2), 2 + interp2(map.X, map.Y, map.z, scenario.goal(1), scenario.goal(2)), 'kp', 'MarkerFaceColor', 'y', 'MarkerSize', 12);
    save_terrain_fig(fig1, 'terrain_elevation', out_dir);

    %% Figure 2 - Friction coefficient
    fig2 = figure('Name', 'Phase 1 - Friction coefficient', ...
        'NumberTitle', 'off', 'Color', 'w');
    imagesc(map.x, map.y, map.mu);
    axis xy equal tight;
    colorbar;
    xlabel('X [m]'); ylabel('Y [m]');
    title('Friction coefficient \mu');
    hold on;
    contour(map.X, map.Y, map.obstacle, [1 1], 'k', 'LineWidth', 1.5);
    plot(scenario.start(:,1), scenario.start(:,2), 'ko', 'MarkerFaceColor', 'k');
    plot(scenario.goal(1), scenario.goal(2), 'kp', 'MarkerFaceColor', 'y', 'MarkerSize', 12);
    save_terrain_fig(fig2, 'friction_coefficient', out_dir);

    %% Figure 3 - Slope angle
    fig3 = figure('Name', 'Phase 1 - Slope angle', ...
        'NumberTitle', 'off', 'Color', 'w');
    imagesc(map.x, map.y, map.slope_deg);
    axis xy equal tight;
    colorbar;
    xlabel('X [m]'); ylabel('Y [m]');
    title('Slope angle [deg]');
    hold on;
    contour(map.X, map.Y, map.obstacle, [1 1], 'k', 'LineWidth', 1.5);
    save_terrain_fig(fig3, 'slope_angle', out_dir);

%% ── Total cost map visualization (full known) ──────────────────────────
% Builds the cost map as if the map were fully discovered, so that the
% "global" structure used by A* can be inspected.

if exist('scenario', 'var') && exist('build_cost_map', 'file')
    % Force all cells as known (full visibility)
    map_full = map;
    map_full.known = true(size(map.X));

    cost_full = build_cost_map(map_full, scenario);

    %% Figure 4 - Total cost map (full known)
    fig4 = figure('Name', 'Phase 1 - Total cost map (full known)', ...
        'NumberTitle', 'off', 'Color', 'w');
    imagesc(map.x, map.y, log10(cost_full));
    axis image; set(gca, 'YDir', 'normal');
    colorbar;
    title('log_{10}(cost) — full known');
    xlabel('x [m]'); ylabel('y [m]');
    save_terrain_fig(fig4, 'cost_map_full_known', out_dir);

    %% Figure 5 - Semantic map
    fig5 = figure('Name', 'Phase 1 - Semantic map', ...
        'NumberTitle', 'off', 'Color', 'w');
    % Reference semantic map for visual comparison
    if isfield(map, 'semantic_rgb') && ~isempty(map.semantic_rgb)
        rgb = double(map.semantic_rgb);
        if max(rgb(:)) > 1.5, rgb = rgb / 255; end
        image(map.x, map.y, rgb);
    else
        imagesc(map.x, map.y, map.mu);
        colormap(gca, parula); colorbar;
    end
    set(gca, 'YDir', 'normal'); axis image;
    title('Semantic map');
    xlabel('x [m]'); ylabel('y [m]');
    % Note: the per-cell black-dot obstacle overlay was removed. With ~100k
    % obstacle cells it produced 100k vector markers, making the PDF heavy
    % and slow; obstacles are already visible through the semantic colour.
    save_terrain_fig(fig5, 'semantic_map', out_dir);
end
end


% =========================================================================
function save_terrain_fig(fig, basename, out_dir)
%SAVE_TERRAIN_FIG  Export a figure as PNG and PDF.
%   ContentType 'auto': vector axes and text, raster for the heavy
%   surface/image content -> light files.
try
    exportgraphics(fig, fullfile(out_dir, [basename '.png']), 'Resolution', 200);
    exportgraphics(fig, fullfile(out_dir, [basename '.pdf']));
catch ME
    warning('visualize_terrain_map:saveFig', ...
        'Figure "%s" not saved: %s', basename, ME.message);
end
end
