%la demo è già stata runnata sulla mappa presentata. Volendo si può
%semplicemente caricare la soluzione caricando il workspace SOLUZIONE.mat.
%In caso contrario, per runnare la demo non c'è bisogno di nessun
%requisito. i Bus di simulink, l'MPC, il motore eccetera vengono creati
%direttamente dentro questa demo, che chiama le relative funzioni. Dopo
%aver lanciato la demo le prime volte si consiglia di commentare le righe
%che caricano motore, MPC, Buses eccetera così da alleggerirla. è
%importante tenere il clear functions, utile per pulire i 'persistent'
%dalle funzioni, che altrimenti rimarrebbero in memoria nonostante il
%clear.
%per cambiare a proprio piacimento la simulazione basta inserire un nuovo
%file denominato 'semantic_map.png' all'interno di questa cartella.
%è possibile cambiare le coordinate di partenza (x,y,psi) dei veicoli così
%come le coordinate di arrivo e altri dati nella funzione
%init_swarm_scenario(). è anche possibile cambiare le caratteristiche
%fisiche del veicolo. nel caso invece si volesse estendere la simulazione
%ad uno sciame più grande basterà fare copia e incolla dei blocchi agents
%nel simulink e inserire altre coordinate di partenza in scenario.start nella
% funzione init_swarm_scenario() così come altre coordinate di riferimento
% al leader in scenario.formation.offsets_body. attualmente la funzione
% define_swarm_buses(N_VEH,1000) viene chiamata con lunghezza massima
% vettore del percorso programmato = 1000, per limitare il calcolo in caso
% di problemi. qualora si decidesse di inserire una mappa per la quale le
% distanze sono molto maggiori a quella corrente, si consiglia di aumentare
% questo numero anche di 100-1000 volte se necessario, è soltanto un limite
% massimo.
% si consiglia inoltre di leggere la documentazione Simulink sulle Argument
% Variables, sui son/child workspaces e sul funzionamento e la definizione
% dei Bus nel caso si volesse modificare il file Simulink.

%per quanto riguarda il codice di riconoscimento AI delle immagini
%satellitari, per farlo funzionare bisogna avere almeno python 3.14, e
%runnare il file predict.py. per cambiare l'immagine da convertire in
%sementic mask dovete cliccare su edit configurations e inserire il nome
%del file in Parameters, in questo modo: --image "C:\vostro_percorso\AI TRAINING\inputs\immagine.png" --mode resized
%inserire successivamente la foto (qualsiasi formato va bene) in AI TRAINING/inputs. il
%risultato sarà disponibile in AI TRAINING/outputs/predictions. è stato
%trainato sul drone semantic dataset della graz university
%(https://ivc.tugraz.at/research-project/semantic-drone-dataset/), che non
%è studiato esattamente per questo tipo di foto, infatti non funziona
%benissimo; non ho approfondito e perfezionato questo aspetto perché non
%era argomento di tesi. vi consiglio di fare più tentativi cambiando
%soprattutto le tonalità colore della vostra foto satellitare di input, o
%in alternativa potete usare qualche altro metodo. Io ho fatto generare
%delle immagini satellitari a chatgpt, perché mentre quelle prese da Google
%Earth non contenevano un alto grado di dettaglio, quelle del dataset erano
%scattate a pochi metri dal suolo (troppo piccole). reputo molto difficile
%trovare un'immagine che fa al caso nostro su internet.


%% DEMO_SWARM_PHASE4
clear; close all; clc;
MDL     = 'swarm_phase_5';
AGENT   = 'Agent';
VEHICLE = 'vehicle';

%% Reset Simulink
% try
%     if bdIsLoaded(MDL)
%         set_param(MDL, 'SimulationCommand', 'stop');
%     end
%     if bdIsLoaded(AGENT)
%         set_param(AGENT, 'SimulationCommand', 'stop');
%     end   
%     if bdIsLoaded(VEHICLE)
%         set_param(VEHICLE, 'SimulationCommand', 'stop');
%     end
% catch
% end
% bdclose('all');
clear functions;
%% ── 2. Carica motor map nel base workspace ──────────────────────────────
load_motor_to_workspace();

%% ── 3. Scenario, mappa, veicoli ─────────────────────────────────────────
fprintf('[setup] Inizializzazione scenario...\n');
scenario = init_swarm_scenario();
map      = scenario.map;
map_truth= scenario.map; %conservo una mappa completa, mi serve per stimulare
visualize_terrain_map(map, scenario) %diamo un'occhiata a cosa viene generato
dt8dof=scenario.sim.dt8dof;
dtcontrol=scenario.sim.dtcontrol;
dtastar=scenario.sim.dtastar;
% la misurazione di mu e pendenza dai veicoli
vehicles = init_vehicle_agents(scenario);
N_VEH = size(scenario.start, 1);   % nel tuo caso 3
make_vref_filter()
% Inizializza map.known a tutto false (nulla scoperto all'inizio)
if ~isfield(map, 'known') || ~isequal(size(map.known), size(map.X))
    map.known = false(size(map.X));
end

% Condividi al base workspace per swarm_planner_helper
assignin('base', 'map',   map);
assignin('base', 'map_truth', map_truth);
assignin('base', 'scenario', scenario);
assignin('base', 'vehicles', vehicles);
%% Bus Simulink
define_swarm_buses(N_VEH,1000);
%3 è il numero di veicoli mentre
% 1000 è la lunghezza vettore path, la si trova in
% swarm_phase_5/Agent_1 (Agent)/SwarmPlanner
% in caso di mappe più grandi è necessario aumentare questo numero,
% che rappresenta la lunghezza del vettore di posizioni calcolate dal
% planner. Se non necessario è bene che sia un numero relativamente
% contenuto: è bene avere la lunghezza dei vettori con cui si lavora non
% eccessiva
assignin('base', 'known_history', struct('t', {}, 'mask', {}));
assignin('base', 'paths_raw_history',    struct('t', {}, 'path', {}));
assignin('base', 'paths_smooth_history', struct('t', {}, 'path', {}));
%% ── 4. Carica modello e popola Model Workspace (solo SCALARI) ──────────
load_system(MDL);
load_system(AGENT);
load_system(VEHICLE);
mws = get_param(MDL, 'ModelWorkspace');
mws_medium = get_param(AGENT, 'ModelWorkspace');
mws_child = get_param(VEHICLE, 'ModelWorkspace');
%%%%%%%% qui assegno le cond iniziali che cambiano per ogni veicolo,
%%%%%%%% verranno sovrascritte dopo
model_args = {
    'x0',           0.0;
    'y0',           0.0;
    'psi0',         0.0;
    'velstart',     5.0;
    'wr0_FL',       3.15;
    'wr0_FR',       3.15;
    'wr0_RL',       3.15;
    'wr0_RR',       3.15;
    'teta0',        0.0;
    'tetapunto0',   0.0;
    'theta_road0',  0.0;
    'phi_road0',    0.0;
    'slope_x_body0',0.0;
    'slope_y_body0',0.0;
};
arg_names_list = {'x0','y0','psi0','velstart', ...
                  'wr0_FL','wr0_FR','wr0_RL','wr0_RR', ...
                  'teta0','tetapunto0','theta_road0','phi_road0', ...
                  'slope_x_body0','slope_y_body0'};
for k = 1:size(model_args, 1)
    mws_child.assignin(model_args{k,1}, model_args{k,2});
    mws_medium.assignin(model_args{k,1}, model_args{k,2});
end
set_param(VEHICLE, 'ParameterArgumentNames', strjoin(arg_names_list, ', '));
mws_medium.assignin('id', 1); % default, verrà sovrascritto dalle istanze
agent_arg_names = [{'id'}, arg_names_list];
set_param(AGENT, ...
    'ParameterArgumentNames', ...
    strjoin(agent_arg_names, ', '));
% i valori fittizi di cond iniziali sono stati assegnati come Argument, per
% dubbi su come funzionano le Argument Variables controllare la
% documentazione MatLab
vehBlocksInAgent = find_system(AGENT, ...
    'BlockType', 'ModelReference', ...
    'ModelName', VEHICLE);

if isempty(vehBlocksInAgent)
    error('Non trovo nessun Model Reference al modello "%s" dentro "%s".', VEHICLE, AGENT);
end

% Dentro Agent.slx il blocco Vehicle deve ricevere i parametri generici:
% x0, y0, psi0, velstart, wr0_FL, ...
set_param(vehBlocksInAgent{1}, ...
    'ParameterArgumentValues', ...
    strjoin(arg_names_list, ', '));
%%%%%%%%
% Geometria/dinamica veicolo (dovrebbero combaciare con scenario.vehicle)
%mws.assignin('L_wb',          scenario.vehicle.wheelbase_m);
if isfield(scenario.vehicle, 'r_wheel_m')
    mws.assignin('r_wheel',   scenario.vehicle.r_wheel_m);
else
    mws.assignin('r_wheel',  0.317);
end
if isfield(scenario.vehicle, 'gear_ratio')
    mws.assignin('gear_ratio', scenario.vehicle.gear_ratio);
else
    mws.assignin('gear_ratio', 10);
end
if isfield(scenario.vehicle, 'mass_kg')
    mws.assignin('m_veh', scenario.vehicle.mass_kg);
else
    mws.assignin('m_veh',   1300); %ho già usato 'm' da qualche parte
end

% Limiti velocita'
mws.assignin('v_max_leader',  scenario.control.v_max_mps);  % usa quello dello scenario
if isfield(scenario.control, 'v_max_follower_mps')
    mws.assignin('v_max_follower', scenario.control.v_max_follower_mps);
else
    mws.assignin('v_max_follower', scenario.control.v_max_mps * 1.15); % follower 15% più veloci
end
mws.assignin('v_min', scenario.control.v_min_mps);
mws.assignin('d_max', scenario.control.max_steer_rad);
mws_child.assignin('d_max', scenario.control.max_steer_rad);

%% ── 5. Terrain truth lookup tables per modello 8DOF ───────────────────────
%servono soltanto nel modello 8dof
mws_child.assignin('terrain_truth_x_bp', map_truth.x);
mws_child.assignin('terrain_truth_y_bp', map_truth.y);
mws_child.assignin('terrain_truth_mu_table', map_truth.mu);
mws_child.assignin('terrain_truth_slope_x_table', map_truth.slope_x);
mws_child.assignin('terrain_truth_slope_y_table', map_truth.slope_y);
mws_child.assignin('terrain_truth_slope_deg_table', map_truth.slope_deg);
mws_child.assignin('terrain_truth_obstacle_table', double(map_truth.obstacle));
mws_child.assignin('terrain_truth_traversable_table', double(map_truth.traversable));

%% ── 6. Initial lateral slope in vehicle frame ─────────────────────────────
% veramente troppe variabili e troppi calcoli inutili qui dentro
% Calcola la pendenza laterale iniziale nel sistema di riferimento veicolo.
% Serve per inizializzare teta0 del modello di rollio.

map_truth = scenario.map;   % mappa fisica reale, non aggiornata dal planner
slope_x_body0 = zeros(N_VEH,1);   % pendenza longitudinale iniziale veicolo
slope_y_body0 = zeros(N_VEH,1);   % pendenza laterale iniziale veicolo
theta_road0   = zeros(N_VEH,1);   % angolo strada longitudinale iniziale [rad]
phi_road0     = zeros(N_VEH,1);   % angolo strada laterale iniziale [rad]
teta0         = zeros(N_VEH,1);   % IC roll angle [rad]
tetapunto0    = zeros(N_VEH,1);   % IC roll rate [rad/s]

for i = 1:N_VEH
    % Stato iniziale veicolo
    x0_i   = vehicles(i).state.x;
    y0_i   = vehicles(i).state.y;
    psi0_i = vehicles(i).state.psi;
    vs     = scenario.vehicle8.velstart;
    wr0    = vs / (0.97 * scenario.vehicle8.R0);
    % Terrain locale nel punto iniziale
    local0 = get_local_terrain(map_truth, x0_i, y0_i);
    % Slope globale nel punto veicolo
    sx_g = local0.slope_x;   % dz/dx globale
    sy_g = local0.slope_y;   % dz/dy globale
    % Safety: se per qualche motivo il veicolo parte fuori mappa
    if ~isfinite(sx_g)
        sx_g = 0.0;
    end
    if ~isfinite(sy_g)
        sy_g = 0.0;
    end
    % Rotazione global frame → vehicle body frame
    c = cos(psi0_i);
    s = sin(psi0_i);
    sx_b =  sx_g*c + sy_g*s;   % lungo x_body/u: avanti veicolo
    sy_b = -sx_g*s + sy_g*c;   % lungo y_body: laterale veicolo
    % Salvataggio
    slope_x_body0(i) = sx_b;
    slope_y_body0(i) = sy_b;
    % Angoli strada iniziali (non li uso per ora)
    % theta_road0(i) = atan(sx_b);
    % phi_road0(i)   = atan(sy_b);
    mws.assignin(sprintf('id%d', i), i);
    mws.assignin(sprintf('x0_%d',          i), x0_i);
    mws.assignin(sprintf('y0_%d',          i), y0_i);
    mws.assignin(sprintf('psi0_%d',        i), psi0_i);
    mws.assignin(sprintf('velstart_%d',    i), vs);
    mws.assignin(sprintf('wr0_FL_%d',      i), wr0);
    mws.assignin(sprintf('wr0_FR_%d',      i), wr0);
    mws.assignin(sprintf('wr0_RL_%d',      i), wr0);
    mws.assignin(sprintf('wr0_RR_%d',      i), wr0);
    mws.assignin(sprintf('teta0_%d',       i), atan(sy_b));
    mws.assignin(sprintf('tetapunto0_%d',  i), 0.0);
    mws.assignin(sprintf('theta_road0_%d', i), atan(sx_b));
    mws.assignin(sprintf('phi_road0_%d',   i), atan(sy_b));
    mws.assignin(sprintf('slope_x_body0_%d', i), sx_b);
    mws.assignin(sprintf('slope_y_body0_%d', i), sy_b);
    % mws.assignin(sprintf('theta_road0_%d', i), theta_road0(i));
    % mws.assignin(sprintf('phi_road0_%d',   i), phi_road0(i));
    agent_arg_vals = [{num2str(i)}, ...
                  cellfun(@(n) sprintf('%s_%d', n, i), ...
                  arg_names_list, 'UniformOutput', false)];
    set_param(sprintf('%s/Agent_%d', MDL, i), ...
    'ParameterArgumentValues', ...
    strjoin(agent_arg_vals, ', '));
end

%% ── 7. TRACKER ───────────────────────────────────────────────────────────────

mws_child.assignin('K_y',   0.06);
mws_child.assignin('K_psi', 0.48);
% Frenata costante (placeholder)
if isfield(scenario.control, 'a_brake_mps2')
    mws_child.assignin('a_brake',    -abs(scenario.control.a_brake_mps2));
else
    mws_child.assignin('a_brake',    -3.0);
end

% Path tracking
mws_child.assignin('r_goal',  scenario.control.goal_reached_radius_m);
if isfield(scenario.control, 'goal_slowdown_radius_m')
    mws_child.assignin('r_slow', scenario.control.goal_slowdown_radius_m);
else
    mws_child.assignin('r_slow',  30.0);
end
mws_child.assignin('MAX_PATH_LEN', 1000);   % deve coincidere con L nel SwarmPlanner code
mws_child.assignin('replan_steps',   20);     % deve coincidere con N_REPLAN nel SwarmPlanner code
mws_child.assignin('dt8dof', dt8dof);
mws_child.assignin('dtcontrol', dtcontrol);
mws_child.assignin('dtastar', dtastar);
mws_medium.assignin('dt8dof', dt8dof);
mws_medium.assignin('dtcontrol', dtcontrol);
mws_medium.assignin('dtastar', dtastar);
mws.assignin('dt8dof', dt8dof);
mws.assignin('dtcontrol', dtcontrol);
mws.assignin('dtastar', dtastar);
mws.assignin('t_end',scenario.sim.t_end);
mws_medium.assignin('t_end', scenario.sim.t_end);
mws_child.assignin('t_end', scenario.sim.t_end);

% Guadagni PID
%da togliere
mws_child.assignin('Kp_h', getfield_or(scenario, {'control','Kp_h'}, 1.5));
mws_child.assignin('Ki_h', getfield_or(scenario, {'control','Ki_h'}, 0));
mws_child.assignin('Kd_h', getfield_or(scenario, {'control','Kd_h'}, 0));
mws_child.assignin('Kp_y', getfield_or(scenario, {'control','Kp_y'}, 0.10));
mws_child.assignin('Ki_y', getfield_or(scenario, {'control','Ki_y'}, 0.0));
mws_child.assignin('Kd_y', getfield_or(scenario, {'control','Kd_y'}, 0.0));
mws_child.assignin('Kp_v', getfield_or(scenario, {'control','Kp_v'}, 1.5));
mws_child.assignin('Ki_v', getfield_or(scenario, {'control','Ki_v'}, 0.10));
mws_child.assignin('Kd_v', getfield_or(scenario, {'control','Kd_v'}, 0.0));

% Motor map: forza il refresh del Model Workspace
mws_child.assignin('mot_n_bp',     evalin('base', 'mot_n_bp'));
mws_child.assignin('mot_alpha_bp', evalin('base', 'mot_alpha_bp'));
mws_child.assignin('mot_T_table',  evalin('base', 'mot_T_table'));
% --- MPC velocità (sostituisce PID_Speed) ---
mpc_speed = design_speed_mpc(scenario);
% Rendi l'oggetto disponibile nel Model Workspace del figlio
mws_child.assignin('mpc_speed', mpc_speed);

%% ── 8. Parametri 8DOF nel Model Workspace ─────────────────────────────────
v8 = scenario.vehicle8;
mws_child.assignin('g', 9.81); %acceler gravità
% Geometria
mws_child.assignin('a',       v8.a);
mws_child.assignin('b',       v8.b);
mws_child.assignin('L',     v8.L);
mws_child.assignin('Tf',         v8.Tf);
mws_child.assignin('Tr',         v8.Tr);
mws_child.assignin('Hg',         v8.Hg);
mws_child.assignin('Ha',         v8.Ha);
mws_child.assignin('m',          v8.m);         % 8DOF usa 'm', non 'm_veh'
mws_child.assignin('Jz',         v8.Jz);

% Rollio
mws_child.assignin('Hroll',   v8.Hroll_CG); %distanza CG CR
mws_child.assignin('Hroll_f',    v8.Hroll_f);
mws_child.assignin('Hroll_r',    v8.Hroll_r);
mws_child.assignin('Ks_f',       v8.Ks_f);
mws_child.assignin('Ks_r',       v8.Ks_r);
mws_child.assignin('Karb_F',     v8.Karb_F);
mws_child.assignin('Karb_R',     v8.Karb_R);
mws_child.assignin('Ix',         v8.Ix);
mws_child.assignin('G',      v8.Groll);
mws_child.assignin('K',         v8.K);
% Aerodinamica
mws_child.assignin('Af',         v8.Af);
mws_child.assignin('Cx',         v8.Cx);
mws_child.assignin('rho',       v8.rho); %rho aria
%mws_child.assignin('velstart',       5); %si può prendere questa
% Pneumatico
mws_child.assignin('R0',         v8.R0);
mws_child.assignin('Rfront',         v8.R0);
mws_child.assignin('Rrear',         v8.R0); %ruote tutte uguali, sto prendendo quelle del
%modello delle lezioni, ma devo mettere dei raggi più piccoli. all'inizio
%lo avevo fatto, poi ho cambiato, potrei aver lasciato in giro i raggi più
%piccoli per sbaglio
mws_child.assignin('Ir',         v8.Jns); %inerzia ruota modello Tota, non mio
mws_child.assignin('Kv',         v8.Kv);
mws_child.assignin('Cp_tyre',    v8.Cp);
mws_child.assignin('Lril_L',     v8.Lril_L);
mws_child.assignin('Lril_T',     v8.Lril_T);
mws_child.assignin('toe_F',      v8.toe_F);
mws_child.assignin('toe_R',      v8.toe_R);
mws_child.assignin('camb_0F',    v8.camb_0F);
mws_child.assignin('camb_0R',    v8.camb_0R);
mws_child.assignin('Pacn',       v8.Pacn);
mws_child.assignin('KdF',        v8.KdF);
mws_child.assignin('KdR',        v8.KdR);
mws_child.assignin('f0',         v8.f0);
mws_child.assignin('f2',         v8.f2); %resistenze rotolamento e aerodyn
% Sterzo
mws_child.assignin('Rs',         v8.Rs);
mws_child.assignin('Rr_sterzo',  v8.Rr);

%% ── 9. sim() ───────────────────────────────────────────────────
fprintf('\n=== Phase 3 single-sim run (StopTime = %.1f s) ===\n', scenario.sim.t_end);

simIn = Simulink.SimulationInput(MDL);
simIn = simIn.setModelParameter( ...
    'StartTime',  '0', ...
    'StopTime',   num2str(scenario.sim.t_end), ...
    'SolverType', 'Fixed-step', ...
    'FixedStep',  num2str(dt8dof), ...
    'SaveOutput', 'on', ...
    'SaveFormat', 'Dataset');
        % 'Solver',     'ode45', ...

% Inizializza log traiettoria (aggiornato ad ogni step da swarm_planner_helper)
assignin('base', 'traj_log', struct('x', zeros(0,3), 'y', zeros(0,3), 't', zeros(0,1)));

% simOut = sim(simIn);
 try
     simOut = sim(simIn);
     sim_crashed = false;
 catch ME
     fprintf('\n[!] Simulazione fermata: %s\n', ME.message);
     sim_crashed = true;
 end
%Plotta il path percorso fino allo stop (funziona anche se sim ha
%crashato), di controllo
% i colori di questo plot sono riferiti al ranking, non all'id veicolo (è
% giusto un debug provvisorio, non ho bisogno che sia preciso)
%se vedete che i colori vengono scambiati tra un veicolo e l'altro, e di
%conseguenza anche le loro traiettorie vengono incrociate con linee in giro
%per la mappa, sappiate che i veicoli si stanno scambiando di posizione nel
%ranking
if evalin('base', 'exist(''traj_log'',''var'')')
    tl = evalin('base', 'traj_log');
    if size(tl.x, 1) > 1
        figure('Name', 'Traiettoria fino allo stop', 'Color', 'w');
        hold on;
        if isfield(map, 'semantic_rgb') && ~isempty(map.semantic_rgb)
            rgb = double(map.semantic_rgb);
            if max(rgb(:)) > 1.5, rgb = rgb/255; end
            imagesc(map.x, map.y, rgb);
        end
        set(gca, 'YDir', 'normal'); axis equal tight;
        colors_p = {'r', 'g', 'b'};
        for i = 1:3
            plot(tl.x(:,i), tl.y(:,i), '-', 'Color', colors_p{i}, 'LineWidth', 2);
            plot(tl.x(end,i), tl.y(end,i), 'o', 'MarkerSize', 10, ...
                'MarkerFaceColor', colors_p{i}, 'MarkerEdgeColor', 'k');
        end
        plot(scenario.goal(1), scenario.goal(2), 'yp', 'MarkerSize', 18, ...
            'MarkerFaceColor', 'y', 'MarkerEdgeColor', 'k');
        drawnow;
    end
end

if sim_crashed
    fprintf('Plot del path parziale completato. SIMULAZIONE crashata o stoppata.\n');
    return;   % programma si ferma
end

%% ── 10. Estrai log da bus PlotGlobal ───────────────────────────────────
yout = simOut.get('yout');

% Adesso yout ha UNA sola outport: il bus PlotGlobal
plotGlobal = yout{1}.Values;

N_VEH = size(scenario.start, 1);   % nel tuo caso 3

% Tempo: lo prendiamo dal primo segnale scalare disponibile
t_log = getBusTime(plotGlobal, 'pl_1.plotveicolo.values.x');
N     = numel(t_log);
%% ── 10.a Estrai stati, comandi e WheelData da PlotGlobal ───────────────
x     = zeros(N, N_VEH);
y     = zeros(N, N_VEH);
psi   = zeros(N, N_VEH);
v     = zeros(N, N_VEH);
alpha = zeros(N, N_VEH);
delta = zeros(N, N_VEH);

wheelFields = { ...
    'slip_vect', ...
    'w_r_vect', ...
    'alpha_wheel', ...
    'delta_vect', ...
    'u_cr', ...
    'v_cr', ...
    'Fx', ...
    'Fy', ...
    'Fz'};

wheelLog = repmat(struct( ...
    'slip_vect',   [], ...
    'w_r_vect',    [], ...
    'alpha_wheel', [], ...
    'delta_vect',  [], ...
    'u_cr',        [], ...
    'v_cr',        [], ...
    'Fx',          [], ...
    'Fy',          [], ...
    'Fz',          []), 1, N_VEH);

for i = 1:N_VEH
    basePath = sprintf('pl_%d.plotveicolo', i);

    % Stato veicolo
    x(:,i)     = getBusData(plotGlobal, [basePath '.values.x']);
    y(:,i)     = getBusData(plotGlobal, [basePath '.values.y']);
    psi(:,i)   = getBusData(plotGlobal, [basePath '.values.psi']);
    v(:,i)     = getBusData(plotGlobal, [basePath '.values.vel_totale']);

    % Comandi globali
    alpha(:,i) = getBusData(plotGlobal, [basePath '.AccelSterzo.Out_alpha']);
    delta(:,i) = getBusData(plotGlobal, [basePath '.AccelSterzo.delta_sterzo']);

    % WheelData: vettori [FL FR RL RR]
    wheelPath = [basePath '.WheelData'];

    for jf = 1:numel(wheelFields)
        fname = wheelFields{jf};
        rawData = getBusData(plotGlobal, [wheelPath '.' fname]);

        wheelLog(i).(fname) = forceNBy4(rawData, N, ...
            sprintf('vehicle %d - WheelData.%s', i, fname));
    end
end

%% ── Debug WheelData ───────────────────────────────────────────────────
disp('[debug] WheelData leader sizes:');
disp(size(wheelLog(1).delta_vect));
disp(size(wheelLog(1).Fx));
disp(size(wheelLog(1).Fy));
disp(size(wheelLog(1).Fz));

fprintf('[debug] Leader wheel sample k=1:\n');
fprintf('delta = [%s]\n', num2str(wheelLog(1).delta_vect(1,:)));
fprintf('Fx    = [%s]\n', num2str(wheelLog(1).Fx(1,:)));
fprintf('Fy    = [%s]\n', num2str(wheelLog(1).Fy(1,:)));
fprintf('Fz    = [%s]\n', num2str(wheelLog(1).Fz(1,:)));
% pct_known è dentro ogni PlotSingle.
% Se è uguale per tutti, usa pl_1.
pct_known_log = getBusData(plotGlobal, 'pl_1.pct_known');
%% ── 11. Aggiorna vehicles con history ────────────────────────────────────
for i = 1:N_VEH
    vehicles(i).history.t     = t_log;
    vehicles(i).history.x     = x(:,i);
    vehicles(i).history.y     = y(:,i);
    vehicles(i).history.psi   = psi(:,i);
    vehicles(i).history.v     = v(:,i);
    vehicles(i).history.alpha = alpha(:,i);
    vehicles(i).history.delta = delta(:,i);
    vehicles(i).state.x   = x(end,i);
    vehicles(i).state.y   = y(end,i);
    vehicles(i).state.psi = psi(end,i);
    vehicles(i).state.v   = v(end,i);
end

map_final = evalin('base', 'map');

%% ── 12. Risultati ────────────────────────────────────────────────────────
final_dist = hypot(scenario.goal(1) - x(end,1), scenario.goal(2) - y(end,1));

fprintf('\n=== Risultati ===\n');
fprintf('  Tempo simulato      : %.2f s\n', t_log(end));
%per questi qui sotto bisogna mettere tic toc
%fprintf('  Tempo wallclock     : %.2f s\n', elapsed);
%fprintf('  Speed-up vs realtime: %.1fx\n', t_log(end)/max(elapsed, eps));
fprintf('  Step totali         : %d\n', N);
fprintf('  Mappa scoperta      : %.1f%%\n', 100*pct_known_log(end));
fprintf('  Distanza leader-goal: %.2f m\n', final_dist);
if final_dist < scenario.control.goal_reached_radius_m
    fprintf('  STATUS: GOAL RAGGIUNTO\n');
else
    fprintf('  STATUS: timeout senza goal\n');
end

% close_system(MDL, 0);
% close_system(AGENT, 0);
% close_system(VEHICLE, 0); % chiudo Simulink, 0 vuol dire senza salvare modifiche
% così posso modificarlo da solo in manuale e lanciare questa demo
% contemporaneamente con altri parametri
visualize_swarm_phase4(map_final, scenario, t_log, x, y, psi, v, alpha, delta, pct_known_log, wheelLog);
% Static time plots of every signal entering Outport 1 (PlotGlobal bus),
% one set of figures per vehicle, saved in latex_tesi_claude\Images\signal_plots
plot_swarm_signals(plotGlobal, scenario);
plot_mu_compaction(map_final, scenario);
fprintf('\nDone.\n');