function map = generate_terrain_map(params)
%questo è il codice nuovo per generare la mappa casuale
%per recuperare il vecchio aprire lo zip
%GENERATE_TERRAIN_MAP  Mappa 2.5D off-road con orografia random e
%                      classificazione semantica letta da immagine.
%
%  ELEVAZIONE (z) = z_tilt(piano inclinato) + dune/valli random + rugosita'
%
%  CLASSIFICAZIONE: ogni cella riceve una classe semantica letta da
%  un'immagine RGB (mappa semantica generata da AI esterna). Da ogni
%  classe deriva:
%    - mu (coefficiente di attrito tipico)
%    - obstacle flag
%
%  DIMENSIONAMENTO: la griglia MATLAB ha le stesse dimensioni dell'immagine
%  semantica, scalata in modo che il lato maggiore corrisponda a
%  params.long_side_m metri.
%
%  PARAMETRI RICHIESTI in `params`:
%    semantic_image_path   path alla mappa semantica RGB (PNG/JPG)
%    long_side_m           lunghezza in metri del lato piu' lungo
%    res_m                 risoluzione griglia MATLAB [m/cella]
%
%  PARAMETRI OPZIONALI (con default sensati):
%    max_tilt_deg          inclinazione massima piano base [deg]      (def 6)
%    n_dunes               numero dune positive random                (def 5)
%    n_valleys             numero valli negative random               (def 4)
%    dune_amp_range        [min max] ampiezza dune [m]                (def [4 14])
%    valley_amp_range      [min max] profondita' valli [m]            (def [3 10])
%    dune_sigma_range      [min max] sigma dune [m]                   (def [15 35])
%    roughness_sigma       deviazione rugosita' naturale [m]          (def 0.3)
%    seed                  random seed (per riproducibilita')         (def random)
%    classes               struct delle classi (vedi default_classes) (def auto)
%    max_traversable_slope_deg  pendenza limite per traversability    (def 25)
%
%  OUTPUT (campi compatibili con il resto del codice):
%    map.X, map.Y, map.x, map.y     coordinate griglia [m]
%    map.z                          quota terreno [m]
%    map.mu, map.mu_base            attrito (-1 dove ostacolo)
%    map.obstacle                   maschera logica ostacoli
%    map.traversable                maschera logica traversabilita'
%    map.slope_x, slope_y, slope_mag, slope_deg
%    map.semantic_class             matrice uint8 con id classe per cella
%    map.semantic_rgb               immagine RGB della mappa semantica (HxWx3)
%    map.classes                    tabella id->name->mu->obstacle
%    map.tilt_dir_rad               direzione del piano inclinato
%    map.tilt_angle_deg             inclinazione del piano
%    map.params                     parametri usati
%    map.known                      maschera celle scoperte (per fasi vision)
%    map.visited_count              contatore passaggi per cella

    %% ── 1. Parametri e default ──────────────────────────────────────────
    if ~isfield(params, 'semantic_image_path')
        error('generate_terrain_map:missingPath', ...
              'params.semantic_image_path e'' obbligatorio.');
    end
    if ~isfield(params, 'long_side_m')
        error('generate_terrain_map:missingSize', ...
              'params.long_side_m e'' obbligatorio (lato maggiore in metri).');
    end
    if ~isfield(params, 'res_m'), params.res_m = 1.0; end

    if ~isfield(params, 'max_tilt_deg'),       params.max_tilt_deg = 6;        end
    if ~isfield(params, 'n_dunes'),            params.n_dunes = 5;             end
    if ~isfield(params, 'n_valleys'),          params.n_valleys = 4;           end
    if ~isfield(params, 'dune_amp_range'),     params.dune_amp_range = [4 14]; end
    if ~isfield(params, 'valley_amp_range'),   params.valley_amp_range = [3 10]; end
    if ~isfield(params, 'dune_sigma_range'),   params.dune_sigma_range = [15 35]; end
    if ~isfield(params, 'roughness_sigma'),    params.roughness_sigma = 0.3;   end
    if ~isfield(params, 'max_traversable_slope_deg')
        params.max_traversable_slope_deg = 25;
    end

    if isfield(params, 'seed') && ~isempty(params.seed)
        rng(params.seed);
    end

    if ~isfield(params, 'classes')
        params.classes = default_classes();
    end

    %% ── 2. Lettura immagine semantica ───────────────────────────────────
    img_rgb = imread(params.semantic_image_path);
    if size(img_rgb, 3) ~= 3
        img_rgb = repmat(img_rgb, [1 1 3]);
    end
    [H_img, W_img, ~] = size(img_rgb);

    % Calcolo dimensioni mondo dal lato maggiore
    if W_img >= H_img
        length_m = params.long_side_m;
        width_m  = params.long_side_m * H_img / W_img;
    else
        width_m  = params.long_side_m;
        length_m = params.long_side_m * W_img / H_img;
    end

    %% ── 3. Costruzione griglia ──────────────────────────────────────────
    x = 0 : params.res_m : length_m;
    y = 0 : params.res_m : width_m;
    [X, Y] = meshgrid(x, y);
    sz = size(X);

    % Resize dell'immagine alla griglia MATLAB
    img_grid = imresize(img_rgb, sz, 'nearest');

    %% ── 4. Classificazione semantica per distanza colore minima ─────────
    [class_id, mu_class, is_obstacle_class] = classify_pixels(img_grid, params.classes);

    %% ── 5. Quota base: piano inclinato in direzione random ─────────────
    tilt_angle_deg = params.max_tilt_deg * (0.4 + 0.6*rand());     % 40-100% del max
    tilt_dir_rad   = 2*pi * rand();
    slope_xy       = tand(tilt_angle_deg);

    % Centro della mappa come riferimento (z=0 al centro, simmetrico)
    Xc = X - length_m/2;
    Yc = Y - width_m/2;

    z_tilt = slope_xy * (Xc * cos(tilt_dir_rad) + Yc * sin(tilt_dir_rad));

    %% ── 6. Dune e valli random sopra il piano inclinato ─────────────────
    z_features = zeros(sz);

    % Dune positive
    for k = 1 : params.n_dunes
        cx  = length_m * (0.1 + 0.8*rand());
        cy  = width_m  * (0.1 + 0.8*rand());
        amp = rand_in(params.dune_amp_range);
        sx  = rand_in(params.dune_sigma_range);
        sy  = rand_in(params.dune_sigma_range);
        z_features = z_features + amp * exp( ...
            -(((X-cx).^2)/(2*sx^2) + ((Y-cy).^2)/(2*sy^2)));
    end

    % Valli negative
    for k = 1 : params.n_valleys
        cx  = length_m * (0.1 + 0.8*rand());
        cy  = width_m  * (0.1 + 0.8*rand());
        amp = -rand_in(params.valley_amp_range);
        sx  = rand_in(params.dune_sigma_range);
        sy  = rand_in(params.dune_sigma_range);
        z_features = z_features + amp * exp( ...
            -(((X-cx).^2)/(2*sx^2) + ((Y-cy).^2)/(2*sy^2)));
    end

    % Rugosita' naturale, però bisogna implementarla bene, dovrebbe essere 
    %sentita dalla ruota nel suo punto, non dal G del veicolo
    %z_noise = params.roughness_sigma * randn(sz);

    z = z_tilt + z_features;% + z_noise;

    %% ── 7. Slope da gradiente della quota totale ────────────────────────
    [dz_dy, dz_dx] = gradient(z, params.res_m, params.res_m);
    slope_mag = hypot(dz_dx, dz_dy);
    slope_deg = atan(slope_mag) * 180/pi;

    %% ── 8. Friction map: parte dal mu di classe, modulato dalla pendenza ─
    mu = mu_class;                        % gia' assegnato per classe
    mu(is_obstacle_class) = -1;           % flag ostacolo

    % Pendenza riduce mu (dove non e' ostacolo)
    not_obs = ~is_obstacle_class;
    mu(not_obs) = mu(not_obs) - 0.005 * slope_deg(not_obs);

    % Rumore naturale (omogeneizza zone della stessa classe)
    mu(not_obs) = mu(not_obs) + 0.02 * randn(nnz(not_obs), 1);
    mu(not_obs) = min(max(mu(not_obs), 0.15), 0.90);

    %% ── 9. Maschere logiche ─────────────────────────────────────────────
    obstacle = is_obstacle_class;
    too_steep = slope_deg > params.max_traversable_slope_deg;
    traversable = ~obstacle & ~too_steep;

    %% ── 10. Output struct ───────────────────────────────────────────────
    map.x = x;
    map.y = y;
    map.X = X;
    map.Y = Y;
    map.z = z;
    map.mu = mu;
    map.mu_base = mu;
    map.obstacle = obstacle;
    map.traversable = traversable;
    map.slope_x = dz_dx;
    map.slope_y = dz_dy;
    map.slope_mag = slope_mag;
    map.slope_deg = slope_deg;
    map.semantic_class = class_id;
    map.semantic_rgb   = img_grid;
    map.classes        = params.classes;
    map.tilt_dir_rad   = tilt_dir_rad;
    map.tilt_angle_deg = tilt_angle_deg;
    map.known          = false(sz);
    map.visited_count  = zeros(sz);
    map.params         = params;
    map.params.length_m = length_m;
    map.params.width_m  = width_m;
end

% =========================================================================
function v = rand_in(range)
    v = range(1) + (range(2) - range(1)) * rand();
end

% =========================================================================
function [class_id, mu_per_pixel, is_obs] = classify_pixels(img_rgb, classes)
%CLASSIFY_PIXELS  Per ogni pixel assegna la classe a colore RGB piu' vicino.
%
%   img_rgb   : HxWx3 uint8
%   classes   : array di struct con campi {name, color_rgb (1x3 uint8),
%                                          mu, obstacle}
%
%   class_id     : HxW uint8 (id classe = indice in `classes`)
%   mu_per_pixel : HxW double (mu della classe assegnata)
%   is_obs       : HxW logical (true se classe = ostacolo)

    [H, W, ~] = size(img_rgb);
    pixels = double(reshape(img_rgb, [], 3));        % [H*W, 3]

    n_classes = numel(classes);
    palette = zeros(n_classes, 3);
    for c = 1:n_classes
        palette(c, :) = double(classes(c).color_rgb);
    end

    % Distanza euclidea pixel <-> palette (broadcasting)
    % d2 = sum((pixels - palette).^2, 2) per ogni classe
    d2 = zeros(size(pixels, 1), n_classes);
    for c = 1:n_classes
        d2(:, c) = sum((pixels - palette(c, :)).^2, 2);
    end
    [~, id_flat] = min(d2, [], 2);
    class_id = uint8(reshape(id_flat, [H, W]));

    % Map id -> mu / is_obstacle
    mu_lookup  = arrayfun(@(c) c.mu,       classes);
    obs_lookup = arrayfun(@(c) c.obstacle, classes);

    mu_per_pixel = mu_lookup(class_id);
    is_obs       = logical(obs_lookup(class_id));
end

% =========================================================================
function classes = default_classes()
%DEFAULT_CLASSES  Classi semantiche allineate alla legenda fornita.
%
%   Colori RGB approssimativi (l'AI puo' produrre variazioni di tono;
%   il classificatore usa distanza minima per essere robusto).
%   I valori di mu sono tipici da letteratura per terreni off-road.
%
%   id  name                  mu     obstacle
%   --  --------------------  -----  ---------
%   1   unknown               0.50   no   (default ottimistico per "non classificato")
%   2   hard_surface          0.85   no   (asfalto/cemento, alto attrito)
%   3   bare_soil             0.55   no   (terra)
%   4   grass                 0.45   no   (erba/prato)
%   5   gravel_rock           0.60   no   (ghiaia/pietrisco)
%   6   water                 0.10   YES  (intransitabile)
%   7   vegetation_obstacle   0.30   YES  (cespugli/alberi densi)
%   8   obstacle              0.00   YES  (ostacoli rigidi)

    classes(1) = struct('name', 'unknown',             'color_rgb', [  0,   0,   0], 'mu', 0.50, 'obstacle', false);
    classes(2) = struct('name', 'hard_surface',        'color_rgb', [150,  80, 160], 'mu', 0.85, 'obstacle', false);
    classes(3) = struct('name', 'bare_soil',           'color_rgb', [180, 100,  30], 'mu', 0.55, 'obstacle', false);
    classes(4) = struct('name', 'grass',               'color_rgb', [ 30, 110,  30], 'mu', 0.45, 'obstacle', false);
    classes(5) = struct('name', 'gravel_rock',         'color_rgb', [120, 115,  90], 'mu', 0.60, 'obstacle', false);
    classes(6) = struct('name', 'water',               'color_rgb', [ 30,  60, 200], 'mu', 0.10, 'obstacle', true);
    classes(7) = struct('name', 'vegetation_obstacle', 'color_rgb', [140, 165,  50], 'mu', 0.30, 'obstacle', false);
    classes(8) = struct('name', 'obstacle',            'color_rgb', [240,  40, 100], 'mu', 0.00, 'obstacle', true);
end