function scenario = init_swarm_scenario()
%INIT_SWARM_SCENARIO Initial setup for the swarm off-road scenario.
% Phase 2 goal: create the environment and initialise three vehicle agents.
%
% The vehicle model used in this phase is intentionally simple:
%   state = [x, y, psi, v]
% The 8DOF Simulink model should be connected only after the swarm logic is
% validated at a higher level.

    rng(7); % reproducibility
    scenario.vehicle8.velstart=5; %lo metto subito qui perché mi serve subito a definire le cond iniziali
    %% Map parameters
    mapParams.semantic_image_path = 'semantic_map.png';
    mapParams.long_side_m         = 550;
    mapParams.res_m               = 1.0;

    %% Generate terrain map
    map = generate_terrain_map(mapParams);

    %% Mission setup
    %coordinate iniziali
    scenario.map = map;
    scenario.start = [ ...
        90, 20, deg2rad(35), scenario.vehicle8.velstart;   % vehicle 1: x, y, psi, v
        80 , 15, deg2rad(35), scenario.vehicle8.velstart;   % vehicle 2
        70, 5, deg2rad(35), scenario.vehicle8.velstart];  % vehicle 3
    %CASO1
    scenario.goal = [30, 310];
    %CASO 2
    %scenario.goal=[500,300];
    %% Vehicle footprint and simplified dynamics obsoleto, variabili non usate
    scenario.vehicle.length_m = 4.5;
    scenario.vehicle.width_m  = 2.0;
    %scenario.vehicle.wheelbase_m = 2.8;
    scenario.vehicle.track_update_width_m = 2.6;
    scenario.vehicle.track_update_length_m = 5.0;
    %scenario.vehicle.steering_ratio = 13;


    %% parametri aggiuntivi 8dof
    
    % Geometria (dai tuoi valori: a=0.6, b=0.9, carreggiata=1.5m, Hg=0.4m)
    scenario.vehicle8.a        = 0.6;       % [m] distanza CG-asse anteriore
    scenario.vehicle8.b        = 0.9;       % [m] distanza CG-asse posteriore
    scenario.vehicle8.L        = 1.5;       % [m] passo = a+b
    scenario.vehicle8.Tf       = 1.5;       % [m] carreggiata anteriore
    scenario.vehicle8.Tr       = 1.5;       % [m] carreggiata posteriore
    scenario.vehicle8.Hg       = 0.4;       % [m] altezza baricentro da terra
    scenario.vehicle8.m        = 1300;      % [kg] massa veicolo
    scenario.vehicle8.Jz       = 1800;      % [kg*m^2] inerzia imbardata
                                             %   (proporzionale a m: ~1.4*m)
    scenario.vehicle8.f0 = 0.0041;           % [Nm/N] = [m] coefficiente costante della resistenza a rotolamento dello pneumatico 
    scenario.vehicle8.f2 = 2.051e-7;         % [Nm/(Ns^2)]= [m/s^2] coefficiente quadratico nella velocità della resistenza a rotolamento dello pneumatico 

    % Centro di rollio
    scenario.vehicle8.Hroll_f  = 0.08;     % [m] altezza centro rollio ant
    scenario.vehicle8.Hroll_r  = 0.12;     % [m] altezza centro rollio post
    % Distanza CG-centro rollio (calcolata)
    Hroll = (scenario.vehicle8.b*scenario.vehicle8.Hroll_f + ...
             scenario.vehicle8.a*scenario.vehicle8.Hroll_r) / scenario.vehicle8.L;
    scenario.vehicle8.Hroll_CG = scenario.vehicle8.Hg - Hroll; % dist CG-CR
    
    % Dinamica di rollio (molle/smorzatori lasciati come nel 8DOF originale)
    % K_roll totale da Dati_veicolo_manovra.m default: 130000 Nm/rad
    % Distribuito: 40% ARB, 50% front per ARB
    K_roll_tot = 130000;        % [Nm/rad]
    scenario.vehicle8.K = K_roll_tot;
    perc_ARB   = 0.40;
    perc_ARBf  = 0.50;
    Ks         = (1-perc_ARB)*K_roll_tot;
    scenario.vehicle8.Ks_f     = Ks/2;
    scenario.vehicle8.Ks_r     = Ks/2; %ripartito 50/50
    scenario.vehicle8.Karb_F   = perc_ARBf*(K_roll_tot - Ks);%anti roll bar
    scenario.vehicle8.Karb_R   = K_roll_tot - scenario.vehicle8.Karb_F ...
                                 - scenario.vehicle8.Ks_f - scenario.vehicle8.Ks_r;
    scenario.vehicle8.Ix       = 450;       % [kg*m^2] inerzia rollio massa sospesa
                                             %   (ridotto rispetto a 600 perché m=1300)
    G_roll     = 15000;         % [Nms/rad] smorzamento rollio
    scenario.vehicle8.Groll    = G_roll;
    
    % Aerodinamica
    scenario.vehicle8.Af       = 1.8;       % [m^2] sezione frontale
    scenario.vehicle8.Cx       = 0.28;      % [-] Cd
    scenario.vehicle8.rho      = 1.3;       % [kg/m^3]
    
    % Pneumatico (Pirelli 225/50/17 da Tyre225_50_17_Comb.m)
    scenario.vehicle8.R0       = 0.2;     % [m] raggio ruota a riposo ERA 0.317
                                             %   calcolato: (17/2*25.4 + 225*50/100)*1e-3
    scenario.vehicle8.Jns      = 0.8;       % [kg*m^2] inerzia ruota (da file)
    scenario.vehicle8.Kv       = 2.90e5;    % [N/m] rigidezza verticale pneumatico
    scenario.vehicle8.Cp       = 500;       % [Ns/m] smorzamento pneumatico
    scenario.vehicle8.Lril_L   = .055;%ERA 0.055     % [m] lunghezza rilassamento longitudinale
    scenario.vehicle8.Lril_T   = .5; % ERA 0.5      % [m] lunghezza rilassamento trasversale
    scenario.vehicle8.KdF = -6.98*10^(-6)*0;        % [rad/N] gradiente di deformabilità tra angolo di convergenza e forza laterale anteriore
    scenario.vehicle8.KdR = 1.74*10^(-6)*0;         % [rad/N] gradiente di deformabilità tra angolo di convergenza e forza laterale posteriore
    
    scenario.vehicle8.velstart=5; %i veicoli partono quasi fermi nella simulazione
    scenario.vehicle8.Ha = 0.4;          % [m] altezza da terra centro di pressione aerodinamico
    % Convergenza e camber statici (dal file tyre)
    scenario.vehicle8.toe_F    = -0.1*pi/180;    % [rad]
    scenario.vehicle8.toe_R    =  0.1*pi/180;    % [rad]
    scenario.vehicle8.camb_0F  =  0.0*pi/180;   % [rad]
    scenario.vehicle8.camb_0R  = -0.39*pi/180;  % [rad]
    
    % Sterzo
    scenario.vehicle8.Rs       = 13;        % [-] rapporto sterzo ant
    scenario.vehicle8.Rr       = 0;         % [-] rapporto sterzo post (nessuno)
    
    % Condizioni iniziali ruote (a riposo: wr0 = v0 / (0.97 * R0))
    % Vengono ricalcolate nella demo in base a v0 dei veicoli
    scenario.vehicle8.wr0_scale = 1/(0.97 * 0.317);  % [1/m] scala per calcolo IC ruote
    
    % Vettore Pacejka 96 (estratto da Tyre225_50_17_Comb.m)
    % Esegui la funzione del pneumatico per ottenere Pacn
    [~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,~,Pacejka,~,~,~,~] = Tyre225_50_17_Comb();
    pacn = struct2cell(Pacejka);
    Pace = zeros(1, numel(pacn));
    for ii = 1:numel(pacn)
        Pace(ii) = pacn{ii};
    end
    scenario.vehicle8.Pacn = Pace';   % vettore colonna coefficienti Pacejka
    %da cambiare successivamente
    %% Terrain compaction parameters
    scenario.terrain.compaction_factor = 1.20;
    scenario.terrain.mu_max = 0.90;

    %% Safety constraints
    scenario.safety.d_safe_base_m = 2;  % FIX 2026-06-01: era 8m, ora 2m (buffer oltre distanza di frenata; dead zone nel checker gestisce il resto)
    scenario.safety.d_safe_max_m  = 25;

    %% Formation setup
    % Offsets are expressed in the leader body frame: [behind, lateral].
    % Negative x offset = behind the leader.
    % scenario.formation.name = 'triangle';
    % scenario.formation.offsets_body = [ ...
    %      0,   0;     % vehicle 1: leader
    %     -20, -12;     % vehicle 2: left/back follower
    %     -20,  12];    % vehicle 3: right/back follower
    % scenario.formation.max_offset_tracking_error_m = 5;
%% Formation setup
%questa sezione dovrà essere riattivata in futuro manualmente per scegliere
%la formazione della simulazione, o utopisticamente in maniera automatica
%lo sciame sceglie qual è la formazione migliore per se stesso. per adesso
%non può funzionare per come è scritto il blocco ranking di simulink: se i
%follower 2 e 3 si scambiano posizione in classifica, allora nella
%formazione devono invertirsi il ruolo, ma non sono uno dietro l'altro,
%sono uno a sinistra e uno a destra. se si ha uno sciame con ad esempio 21
%elementi (1 follower, 10 elementi a dx e 10 a sx in formazione diamante)
%allora si dovrebbe fare una classifica per ogni ramo.
% fprintf('\nScegli formazione swarm:\n');
% fprintf('  1) line     | follower in fila dietro al leader\n');
% fprintf('  2) triangle | follower dietro, uno a sx e uno a dx\n');
% form_id = input('Formazione [1=line, 2=triangle] = ');
% if isempty(form_id), form_id = 1; end
form_id = 1;   % formazione fissa in linea (triangle disabilitata: non funziona)
switch form_id
case 1
        scenario.formation.name        = 'line';
        scenario.formation.offsets_body = [ ...
             0,   0;    % vehicle 1: leader
           -10,   0;    % vehicle 2: 10 m dietro
           -20,   0];   % vehicle 3: 20 m dietro
        scenario.formation.max_offset_tracking_error_m = 5;
case 2
        scenario.formation.name        = 'triangle';
        scenario.formation.offsets_body = [ ...
             0,   0;    % vehicle 1: leader
           -20, -12;    % vehicle 2: 20 m dietro, 12 m a sx
           -20,  12];   % vehicle 3: 20 m dietro, 12 m a dx
        scenario.formation.max_offset_tracking_error_m = 6.0;
otherwise
        warning('[init_swarm_scenario] Formazione non valida, uso line.');
        scenario.formation.name        = 'line';
        scenario.formation.offsets_body = [ ...
             0,   0;
           -10,   0;
           -20,   0];
        scenario.formation.max_offset_tracking_error_m = 6.0;
end
fprintf('[init] Formazione selezionata: %s\n', scenario.formation.name);    %% Low-level kinematic controller parameters
 
    scenario.control.v_max_mps = 8;
    scenario.control.v_min_mps = 0;
    scenario.control.max_brake_mps2 = 3.5;
    scenario.control.max_steer_rad = deg2rad(32); %32 gradi di delta a terra, non volante
    scenario.control.goal_slowdown_radius_m = 30;
    scenario.control.goal_reached_radius_m = 5;
    scenario.control.local_lookahead_m = 22;
    scenario.control.ray_sample_step_m = 2;
    %% per il raggio di visibilità
    %questo aiuta tanto il tuning di astar
    scenario.sensor.vision_radius_m = 40;
    %% Simulation parameters
    scenario.sim.dt8dof = 0.002;
    scenario.sim.dtcontrol = 0.03;
    scenario.sim.dtastar = 0.1;%deve essere multiplo di dt8dof
    scenario.sim.t_end = 70;
%% PARAMETRI DI PATH PLANNING
scenario.planning.grid_res_m         = 4;       % risoluzione downsampling A* [m]
scenario.planning.cost_obstacle      = 1e6;     % costo astronomico per ostacoli noti
scenario.planning.cost_unknown       = 1e3;     % costo per celle non scoperte
                                                  % (>1: pianifica preferendo aree note;
                                                  %  <1: incentiva esplorazione)

scenario.planning.cost_slope_w       = 0.5;     % peso penalita' pendenza
                                                  % cost += w * max(0, slope_deg - 8)
scenario.planning.max_iter           = 50000;   % limite iterazioni A* (safety)

% ─────────────────────────────────────────────────────────────────────────
% COSTI PER CLASSE SEMANTICA
% ─────────────────────────────────────────────────────────────────────────
% Indici devono corrispondere a default_classes() in generate_terrain_map.m:
%   1 unknown   2 hard_surface   3 bare_soil   4 grass
%   5 gravel_rock   6 water (obs)   7 vegetation_obstacle (obs)   8 obstacle (obs)
%
% I costi sono unita' relative: il costo "vero" di una cella sara'
%   cost_per_meter * cell_size + penalita' (slope, unknown, ecc)
%
% Filosofia di tuning:
%   - hard_surface: ottimale, normalizzato a 1
%   - bare_soil:    leggermente peggio (~1.5x)
%   - gravel_rock:  rigido ma irregolare (~2x)
%   - grass corto:  attrito medio, drenante (~2.5x)
%   - sand/fango:   pessimo (~6-8x)  — qui sotto sono nelle classi semantiche

scenario.planning.cost_per_class = [...
    100.0;   % 1 unknown            (default ottimistico, simile a grass)
    1.0;   % 2 hard_surface       (asfalto/cemento - ottimo)
    10.0;   % 3 bare_soil          (terra battuta)
    50.0;   % 4 grass              (erba bassa)
    30.0;   % 5 gravel_rock        (ghiaia/pietrisco)
    1e6;   % 6 water              ostacolo
    200;   % 7 vegetation_obstacle ostacolo
    1e6 ]; % 8 obstacle           ostacolo
%% ─────────────────────────────────────────────────────────────────────────
% COST MAP COMPLETA PRECALCOLATA
% ─────────────────────────────────────────────────────────────────────────
% build_cost_map costruisce la mappa completa:
%   - costo semantico
%   - gradiente ai confini semantici
%   - penalità slope
%   - ostacoli a costo pl.cost_obstacle
%
% NON gestisce map.known.
% La mappa non nota viene gestita dinamicamente dentro swarm_planner_helper2.

scenario.planning.semantic_gradient_radius_cells = 4;  % usa 3 o 4 celle

map.cost_all = build_cost_map(map, scenario);

% Controllo dimensionale di sicurezza
assert(isequal(size(map.cost_all), size(map.X)), ...
    'Errore: map.cost_all deve avere la stessa dimensione di map.X');

% Aggiorna scenario.map con la mappa completa
scenario.map = map;

end