function visualize_swarm_phase4(map_final, scenario, t_log, x, y, psi, v, alpha, delta, pct_known_log, wheelLog)
if nargin < 11
    wheelLog = [];
end
N_fast = length(t_log);
% Use the real log time directly, without the dtastar/dt8dof ratio
if N_fast >= 2
    dt_log = mean(diff(t_log));
else
    if isfield(scenario, 'sim') && isfield(scenario.sim, 'dt8dof')
        dt_log = scenario.sim.dt8dof;
    elseif isfield(scenario, 'sim') && isfield(scenario.sim, 'dt')
        dt_log = scenario.sim.dt;
    else
        dt_log = 0.02;
    end
end
frame_dt = 0.1;   % [s] fine step: smooth video export. Set 1.0 for a quick debug replay.
plot_every_fast = max(1, round(frame_dt / dt_log));
pause_replay = 0;   % no pause while exporting the video; restore 0.1 for interactive use
% vehicle colors
col_leader = [0.85 0.10 0.10];
col_foll1  = [0.10 0.60 0.20];
col_foll2  = [0.10 0.25 0.85];
colors = {col_leader, col_foll1, col_foll2};
names  = {'ID1', 'ID2', 'ID3'};
% path styles (spline + raw A*)
col_smooth = [1.0 0.85 0.0];
col_raw    = [0.05 0.25 0.95];
% Retrieve histories
known_hist = [];
paths_raw  = [];
paths_smo  = [];
if evalin('base', 'exist(''known_history'',''var'')')
    known_hist = evalin('base', 'known_history');
end
if evalin('base', 'exist(''paths_raw_history'',''var'')')
    paths_raw = evalin('base', 'paths_raw_history');
end
if evalin('base', 'exist(''paths_smooth_history'',''var'')')
    paths_smo = evalin('base', 'paths_smooth_history');
end
has_rgb = isfield(map_final, 'semantic_rgb') && ~isempty(map_final.semantic_rgb);
if has_rgb
    rgb_full = double(map_final.semantic_rgb);
    if max(rgb_full(:)) > 1.5
        rgb_full = rgb_full / 255;
    end
else
    rgb_full = [];
end
% ── Evento mappa runtime (apply_map_event): sfondo pre/post evento ──────
% La mappa cambia una volta sola, quindi NON serve la storia per ogni t:
% bastano lo sfondo pre-evento, quello finale e l'istante di commutazione.
rgb_pre = rgb_full;
t_event = inf;
if evalin('base', 'exist(''map_event_t'',''var'')')
    t_event = evalin('base', 'map_event_t');
    if has_rgb && evalin('base', 'exist(''map_event_rgb_pre'',''var'')')
        rgb_pre = double(evalin('base', 'map_event_rgb_pre'));
        if max(rgb_pre(:)) > 1.5
            rgb_pre = rgb_pre / 255;
        end
    end
    fprintf('[animation] Map event at t = %.2f s: background switches pre/post.\n', t_event);
end
L_veh = 2.0;
W_veh = 1.0;
% ── Output folder for the replay snapshots (every 5 s) ───────────────────
this_dir = fileparts(mfilename('fullpath'));
snapshot_dir = fullfile(this_dir, '..', 'latex_tesi_claude', 'Images', ...
    'replay_snapshots');
if ~exist(snapshot_dir, 'dir')
    mkdir(snapshot_dir);
end
fprintf('[animation] Replay snapshots will be saved in:\n    %s\n', snapshot_dir);
next_snap = 0;   % next simulated time [s] at which a snapshot is dumped
fprintf('\n[animation] Starting swarm replay (frame every %.2fs)...\n', plot_every_fast * dt_log);
fig_anim = figure('Name', 'Swarm — Phase 4 Replay', ...
    'NumberTitle', 'off', 'Position', [80 80 1500 900], 'Color', 'w');
% ── Video del pannello "leader path + known map" ─────────────────────────
make_video = true;
if make_video
    v_out = VideoWriter(fullfile(snapshot_dir, 'replay_leader_path.mp4'), 'MPEG-4');
    v_out.FrameRate = 10;   % con frame_dt = 0.1 s -> video in tempo reale
    v_out.Quality   = 95;
    open(v_out);
    fig_video = figure('Name', 'video', 'Color', 'w', 'Visible', 'off', ...
        'Position', [100 100 900 720]);   % NON ridimensionare mai
    fprintf('[animation] Video will be saved in:\n    %s\n', ...
        fullfile(snapshot_dir, 'replay_leader_path.mp4'));
end
fprintf('[debug] known_history: %d snapshots\n', numel(known_hist));
fprintf('[debug] paths_raw_history: %d snapshots\n', numel(paths_raw));
fprintf('[debug] paths_smooth_history: %d snapshots\n', numel(paths_smo));
for k_fast = 1:plot_every_fast:N_fast
    t_current = t_log(k_fast);
    progress  = 100 * k_fast / N_fast;
    % sfondo del frame: mappa originale prima dell'evento, con ostacolo dopo
    if t_current < t_event
        rgb_bg = rgb_pre;
    else
        rgb_bg = rgb_full;
    end
    if ~ishandle(fig_anim)
        fprintf('[animation] Swarm replay interrupted: figure closed.\n');
        return;
    end
    figure(fig_anim);
    clf(fig_anim);
    tl_swarm = tiledlayout(fig_anim, 2, 2, ...
        'Padding', 'compact', ...
        'TileSpacing', 'compact');
    % ─── (1) Full map + trajectories + vehicles ─────────────────────────
    ax1 = nexttile(tl_swarm, 1);
    draw_swarm_panel(ax1, map_final, has_rgb, rgb_bg, x, y, psi, ...
        k_fast, colors, names, L_veh, W_veh, scenario, ...
        sprintf('[%.0f%%]', progress));
    % ─── (2) Progressive known map + vehicle paths ──────────────────────
    ax2 = nexttile(tl_swarm, 2);
    draw_path_panel(ax2, map_final, has_rgb, rgb_bg, known_hist, ...
        t_current, paths_smo, paths_raw, x, y, psi, k_fast, ...
        col_smooth, col_raw, colors, names, L_veh, W_veh, ...
        scenario, pct_known_log, '');
    % ─── (3) Speed ──────────────────────────────────────────────────────
    ax3 = nexttile(tl_swarm, 3); hold(ax3, 'on'); grid(ax3, 'on');
    h_v = gobjects(3, 1);
    for i = 1:3
        h_v(i) = plot(ax3, t_log(1:k_fast), v(1:k_fast,i) * 3.6, '-', ...
            'Color', colors{i}, 'LineWidth', 1.5);
    end
    yline(ax3, scenario.control.v_max_mps * 3.6, '--k', 'LineWidth', 1.2, ...
        'HandleVisibility', 'off');
    xlabel(ax3, 't [s]'); ylabel(ax3, 'v [km/h]');
    title(ax3, 'Speed', 'FontWeight', 'bold');
    legend(ax3, h_v, names, 'Location', 'best', 'FontSize', 9);
    % ─── (4) Throttle/Brake ─────────────────────────────────────────────
    ax4 = nexttile(tl_swarm, 4); hold(ax4, 'on'); grid(ax4, 'on');
    h_a = gobjects(3, 1);
    for i = 1:3
        h_a(i) = plot(ax4, t_log(1:k_fast), alpha(1:k_fast,i), '-', ...
            'Color', colors{i}, 'LineWidth', 1.5);
    end
    yline(ax4, 0, ':k', 'LineWidth', 1, 'HandleVisibility', 'off');
    xlabel(ax4, 't [s]'); ylabel(ax4, '\xi [%]');
    title(ax4, 'Throttle/Brake', 'FontWeight', 'bold');
    ylim(ax4, [-110 110]);
    legend(ax4, h_a, names, 'Location', 'best', 'FontSize', 9);
    sgtitle(tl_swarm, sprintf('Swarm — Replay t = %.2f / %.2f s', ...
        t_current, t_log(end)), 'FontSize', 12, 'FontWeight', 'bold');
    drawnow;
    % ─── Video frame: pannello leader path su figura dedicata ───────────
    if make_video && isgraphics(fig_video)
        clf(fig_video);
        ax_v = axes('Parent', fig_video);
        draw_path_panel(ax_v, map_final, has_rgb, rgb_bg, known_hist, ...
            t_current, paths_smo, paths_raw, x, y, psi, k_fast, ...
            col_smooth, col_raw, colors, names, L_veh, W_veh, ...
            scenario, pct_known_log, sprintf('(t = %.1f s)', t_current));
        im = print(fig_video, '-RGBImage', '-r120');      % render off-screen
        im = im(1:end-mod(size(im,1),2), 1:end-mod(size(im,2),2), :); % lati pari per H.264
        writeVideo(v_out, im);
    end
    % ─── Snapshot every 5 s: two separate figures (plots, not subplots) ──
    if t_current >= next_snap
        tstamp = round(t_current);
        fig_snap_v = figure('Name', ...
            sprintf('Snapshot - map + vehicles  t = %d s', tstamp), ...
            'NumberTitle', 'off', 'Color', 'w', 'Visible', 'off', ...
            'Position', [120 90 780 640]);
        ax_snap_v = axes('Parent', fig_snap_v); %#ok<LAXES>
        draw_swarm_panel(ax_snap_v, map_final, has_rgb, rgb_bg, x, y, psi, ...
            k_fast, colors, names, L_veh, W_veh, scenario, ...
            sprintf('(t = %d s)', tstamp));
        save_snapshot(fig_snap_v, ...
            sprintf('replay_map_vehicles_t%03ds', tstamp), snapshot_dir);
        fig_snap_p = figure('Name', ...
            sprintf('Snapshot - leader path  t = %d s', tstamp), ...
            'NumberTitle', 'off', 'Color', 'w', 'Visible', 'off', ...
            'Position', [160 90 780 640]);
        ax_snap_p = axes('Parent', fig_snap_p); %#ok<LAXES>
        draw_path_panel(ax_snap_p, map_final, has_rgb, rgb_bg, known_hist, ...
            t_current, paths_smo, paths_raw, x, y, psi, k_fast, ...
            col_smooth, col_raw, colors, names, L_veh, W_veh, ...
            scenario, pct_known_log, sprintf('(t = %d s)', tstamp));
        save_snapshot(fig_snap_p, ...
            sprintf('replay_leader_path_t%03ds', tstamp), snapshot_dir);
        next_snap = next_snap + 5;
    end
    pause(pause_replay);
end
% ── Chiusura video ────────────────────────────────────────────────────────
if make_video
    close(v_out);
    if isgraphics(fig_video), close(fig_video); end
    fprintf('[animation] Video saved in %s\n', ...
        fullfile(snapshot_dir, 'replay_leader_path.mp4'));
end
fprintf('[animation] Swarm replay completed. Starting wheel replay...\n');
if ~isempty(wheelLog)
    replay_wheel_forces_all_vehicles( ...
        scenario, ...
        t_log, ...
        psi, ...
        v, ...
        alpha, ...
        delta, ...
        wheelLog, ...
        plot_every_fast, ...
        pause_replay);
else
    warning('visualize_swarm_phase4:noWheelLog', ...
        'wheelLog not available: skipping wheel replay.');
end
fprintf('[animation] Replay completed.\n\n');
end
% =========================================================================
%  Panel 1 - Full map + trajectories + vehicles
% =========================================================================
function draw_swarm_panel(ax, map_final, has_rgb, rgb_full, x, y, psi, ...
    k_fast, colors, names, L_veh, W_veh, scenario, title_suffix)
hold(ax, 'on');
if has_rgb
    imagesc(ax, map_final.x, map_final.y, rgb_full);
else
    imagesc(ax, map_final.x, map_final.y, map_final.slope_deg);
    colormap(ax, parula);
end
set(ax, 'YDir', 'normal');
% Obstacle cells are already visible through the semantic RGB colour.
% The per-cell black-dot overlay was removed: with ~100k obstacle cells
% it produced 100k vector marker objects, making the PDF huge and slow.
h_traj = gobjects(3, 1);
for i = 1:3
    h_traj(i) = plot(ax, x(1:k_fast,i), y(1:k_fast,i), '-', ...
        'Color', colors{i}, 'LineWidth', 1.8);
    draw_vehicle_rect(ax, x(k_fast,i), y(k_fast,i), psi(k_fast,i), ...
        L_veh, W_veh, colors{i});
end
h_goal = plot(ax, scenario.goal(1), scenario.goal(2), 'p', ...
    'MarkerSize', 18, 'MarkerFaceColor', 'y', 'MarkerEdgeColor', 'k', ...
    'LineWidth', 1.5);
axis(ax, 'equal'); axis(ax, 'tight');
xlabel(ax, 'x [m]'); ylabel(ax, 'y [m]');
title(ax, strtrim(sprintf('Full map + trajectories   %s', title_suffix)), ...
    'FontWeight', 'bold');
legend(ax, [h_traj; h_goal], [names, {'Goal'}], ...
    'Location', 'best', 'FontSize', 9);
end
% =========================================================================
%  Panel 2 - Progressive known map + paths (raw A*, spline, ID1/ID2/ID3)
% =========================================================================
function draw_path_panel(ax, map_final, has_rgb, rgb_full, known_hist, ...
    t_current, paths_smo, paths_raw, x, y, psi, k_fast, ...
    col_smooth, col_raw, colors, names, L_veh, W_veh, ...
    scenario, pct_known_log, title_suffix)
hold(ax, 'on');
known_now = false(size(map_final.X));
if ~isempty(known_hist)
    times_k = [known_hist.t];
    idx_kn  = find(times_k <= t_current, 1, 'last');
    if ~isempty(idx_kn)
        known_now = known_hist(idx_kn).mask;
    end
end
if has_rgb
    mask3_now = repmat(known_now, [1 1 3]);
    rgb_now   = rgb_full .* mask3_now;
    imagesc(ax, map_final.x, map_final.y, rgb_now);
else
    imagesc(ax, map_final.x, map_final.y, double(known_now));
    colormap(ax, [0 0 0; 0.7 0.7 0.7]);
end
set(ax, 'YDir', 'normal');
h_leg2  = gobjects(0);
lbl_leg2 = {};
if ~isempty(paths_smo)
    times_s = [paths_smo.t];
    idx_s   = find(times_s <= t_current, 1, 'last');
    if ~isempty(idx_s)
        p_s = paths_smo(idx_s).path;
        h = plot(ax, p_s(:,1), p_s(:,2), '-', ...
            'Color', col_smooth, 'LineWidth', 3.0);
        h_leg2(end+1) = h; %#ok<AGROW>
        lbl_leg2{end+1} = 'Spline'; %#ok<AGROW>
    end
end
% Actual path of each vehicle (ID1, ID2, ID3), same colours as panel 1
for i = 1:3
    h_veh = plot(ax, x(1:k_fast,i), y(1:k_fast,i), '-', ...
        'Color', colors{i}, 'LineWidth', 2.0);
    h_leg2(end+1)   = h_veh;    %#ok<AGROW>
    lbl_leg2{end+1} = names{i}; %#ok<AGROW>
end
if ~isempty(paths_raw)
    times_r = [paths_raw.t];
    idx_r   = find(times_r <= t_current, 1, 'last');
    if ~isempty(idx_r)
        p_r = paths_raw(idx_r).path;
        h = plot(ax, p_r(:,1), p_r(:,2), '--', ...
            'Color', col_raw, 'LineWidth', 1.2);
        h_leg2(end+1) = h; %#ok<AGROW>
        lbl_leg2{end+1} = 'Raw A*'; %#ok<AGROW>
    end
end
% Vehicle bodies for all three agents (same colours as panel 1)
for i = 1:3
    draw_vehicle_rect(ax, x(k_fast,i), y(k_fast,i), psi(k_fast,i), ...
        L_veh, W_veh, colors{i});
end
plot(ax, scenario.goal(1), scenario.goal(2), 'p', ...
    'MarkerSize', 18, 'MarkerFaceColor', 'y', 'MarkerEdgeColor', 'w', ...
    'LineWidth', 1.5, 'HandleVisibility', 'off');
axis(ax, 'equal'); axis(ax, 'tight');
xlabel(ax, 'x [m]'); ylabel(ax, 'y [m]');
title(ax, strtrim(sprintf('Discovered: %.1f%%   |   Leader path   %s', ...
    100 * pct_known_log(min(k_fast, numel(pct_known_log))), title_suffix)), ...
    'FontWeight', 'bold');
if ~isempty(h_leg2)
    legend(ax, h_leg2, lbl_leg2, 'Location', 'best', 'FontSize', 9);
end
end
% =========================================================================
%  Save a figure as PNG + PDF in the snapshot folder
% =========================================================================
function save_snapshot(fig, basename, out_dir)
%SAVE_SNAPSHOT  Save a snapshot figure as PNG + PDF (robust: retries, then
%   closes the figure so windows do not pile up and destabilise export).
png_path = fullfile(out_dir, [basename '.png']);
pdf_path = fullfile(out_dir, [basename '.pdf']);
for attempt = 1:3
    try
        drawnow;
        exportgraphics(fig, png_path, 'Resolution', 200);
        exportgraphics(fig, pdf_path, 'ContentType', 'vector');
        break;
    catch ME
        if attempt < 3
            pause(0.5); drawnow;
        else
            warning('visualize_swarm_phase4:saveSnapshot', ...
                'Snapshot "%s" not saved after 3 attempts: %s', ...
                basename, ME.message);
        end
    end
end
if isgraphics(fig)
    close(fig);
end
end
function replay_wheel_forces_all_vehicles(scenario, t_log, psi, v, alpha, delta, wheelLog, plot_every_fast, pause_replay)
%REPLAY_WHEEL_FORCES_ALL_VEHICLES
% Replay that follows the swarm replay.
% Layout 1 row x 3 columns:
%   column 1 = ID1
%   column 2 = ID2
%   column 3 = ID3
fprintf('[animation] Starting wheel replay for vehicles ID1-ID2-ID3...\n');
N_fast = numel(t_log);
N_VEH  = min(3, numel(wheelLog));
vehColors = { ...
    [0.85 0.10 0.10], ...   % ID1
    [0.10 0.60 0.20], ...   % ID2
    [0.10 0.25 0.85]  ...   % ID3
};
fig_wheel = figure('Name', 'Wheel Forces Top View — ID1 ID2 ID3', ...
    'NumberTitle', 'off', ...
    'Position', [60 120 1800 720], ...
    'Color', 'w');
for k_fast = 1:plot_every_fast:N_fast
    t_current = t_log(k_fast);
    if ~ishandle(fig_wheel)
        fprintf('[animation] Wheel replay interrupted: figure closed.\n');
        return;
    end
    figure(fig_wheel);
    clf(fig_wheel);
    tl = tiledlayout(fig_wheel, 1, 3, ...
        'Padding', 'compact', ...
        'TileSpacing', 'compact');
    for veh = 1:N_VEH
        axW = nexttile(tl, veh);
        hold(axW, 'on');
        grid(axW, 'on');
        draw_vehicle_wheel_forces_topview(axW, ...
            wheelLog(veh), ...
            k_fast, ...
            scenario, ...
            psi(k_fast,veh), ...
            v(k_fast,veh), ...
            alpha(k_fast,veh), ...
            delta(k_fast,veh), ...
            sprintf('ID%d', veh), ...
            vehColors{veh});
        title(axW, sprintf('ID%d | \\psi=%.1f° | v=%.2f m/s | \\xi_{cmd}=%.1f %% | \\delta_{cmd}=%.1f°', ...
            veh, ...
            rad2deg(psi(k_fast,veh)), ...
            v(k_fast,veh), ...
            alpha(k_fast,veh), ...
            rad2deg(delta(k_fast,veh))), ...
            'FontWeight', 'bold', ...
            'FontSize', 9, ...
            'Interpreter', 'tex');
    end
    sgtitle(tl, ...
        sprintf('Wheel-level replay — t = %.2f / %.2f s', t_current, t_log(end)), ...
        'FontSize', 12, ...
        'FontWeight', 'bold');
    drawnow;
    pause(pause_replay);
end
fprintf('[animation] Vehicle wheel replay completed.\n\n');
end
% =========================================================================
% Draw the vehicle as a rectangle on the global map
% =========================================================================
function draw_vehicle_rect(ax, xc, yc, psi_v, L, W, color)
% Draw the vehicle as an oriented rectangle in the global frame.
corners = [ L/2,  W/2;
            L/2, -W/2;
           -L/2, -W/2;
           -L/2,  W/2];
R = [cos(psi_v), -sin(psi_v);
     sin(psi_v),  cos(psi_v)];
rot = (R * corners')';
xpts = rot(:,1) + xc;
ypts = rot(:,2) + yc;
patch(ax, xpts, ypts, color, ...
    'EdgeColor', 'k', ...
    'LineWidth', 1.2, ...
    'FaceAlpha', 0.85, ...
    'HandleVisibility', 'off');
nose_local = [L/2, 0;
              L/2 + L*0.25, 0;
              L/2, 0];
nose_rot = (R * nose_local')';
plot(ax, nose_rot(:,1) + xc, nose_rot(:,2) + yc, ...
    'k-', ...
    'LineWidth', 1.5, ...
    'HandleVisibility', 'off');
end
function draw_vehicle_wheel_forces_topview(ax, wheelVeh, k, scenario, psi_v, v_body, alpha_cmd, delta_cmd, vehName, vehColor)
%DRAW_VEHICLE_WHEEL_FORCES_TOPVIEW
% Top-view of the vehicle in the body frame.
%
% Convention:
%   positive x_body = vehicle forward
%   positive y_body = vehicle left
%
% Order of the WheelData vectors:
%   1 = FL
%   2 = FR
%   3 = RL
%   4 = RR
%
% Arrows:
%   - V starts at the wheel center and points along the local velocity.
%   - Fx has its tip at the wheel center and its tail set back along the tyre axis.
%   - Fy has its tip at a point set back on the wheel and its tail further back.
%   - Fz is drawn outside the vehicle: towards +y on the left wheels,
%     towards -y on the right wheels. It is a graphical projection, not a
%     physical direction in the xy plane.
%
% Graphical note:
%   The Fx, Fy and Fz scales are independent of each other. Each force
%   family is proportional to its own instantaneous maximum, so the arrows
%   stay readable even when the orders of magnitude differ.
%% ── Vehicle geometry ───────────────────────────────────────────────────
if isfield(scenario, 'vehicle8')
    a  = scenario.vehicle8.a;
    b  = scenario.vehicle8.b;
    Tf = scenario.vehicle8.Tf;
    Tr = scenario.vehicle8.Tr;
    R0 = scenario.vehicle8.R0;
    if isfield(scenario.vehicle8, 'm')
        mVeh = scenario.vehicle8.m; %#ok<NASGU>
    end
else
    Ltmp = scenario.vehicle.wheelbase_m;
    a  = Ltmp/2;
    b  = Ltmp/2;
    Tf = scenario.vehicle.width_m;
    Tr = scenario.vehicle.width_m;
    R0 = 0.317;
end
Dwheel = 2 * R0;     % tyre length in top view [m]
Wwheel = 0.225;      % tyre width [m]
%% ── Chassis: slightly longer, slightly narrower ────────────────────────
marginLong = 0.92 * Dwheel + 0.14;
marginLat  = 0.36 * Dwheel + 0.08;
xFrontBody =  a + marginLong;
xRearBody  = -b - marginLong;
halfWBody  = max(Tf, Tr)/2 + marginLat;
chassis = [ ...
    xFrontBody,  halfWBody;
    xFrontBody, -halfWBody;
    xRearBody,  -halfWBody;
    xRearBody,   halfWBody];
patch(ax, chassis(:,1), chassis(:,2), vehColor, ...
    'EdgeColor', 'k', ...
    'LineWidth', 1.5, ...
    'FaceAlpha', 0.18, ...
    'HandleVisibility', 'off');
% Vehicle longitudinal axis
plot(ax, [xRearBody-0.25, xFrontBody+0.35], [0, 0], ...
    'k--', ...
    'LineWidth', 1.0, ...
    'HandleVisibility', 'off');
% Front indicator
plot(ax, [xFrontBody-0.25, xFrontBody+0.25], [0, 0], ...
    'k-', ...
    'LineWidth', 2.0, ...
    'HandleVisibility', 'off');
text(ax, xFrontBody+0.32, 0, 'front', ...
    'FontSize', 9, ...
    'FontWeight', 'bold', ...
    'HorizontalAlignment', 'left', ...
    'VerticalAlignment', 'middle');
% Vehicle label
text(ax, xRearBody+0.12, halfWBody-0.12, vehName, ...
    'FontSize', 12, ...
    'FontWeight', 'bold', ...
    'Color', vehColor, ...
    'BackgroundColor', 'w', ...
    'Margin', 2);
%% ── Wheel positions ────────────────────────────────────────────────────
wheelNames = {'FL','FR','RL','RR'};
wheelPos = [ ...
     a,  Tf/2;   % FL
     a, -Tf/2;   % FR
    -b,  Tr/2;   % RL
    -b, -Tr/2];  % RR
%% ── Wheel data at instant k ────────────────────────────────────────────
delta_vec = safeRow(wheelVeh.delta_vect,  k, 4);
alpha_vec = safeRow(wheelVeh.alpha_wheel, k, 4);
slip_vec  = safeRow(wheelVeh.slip_vect,   k, 4);
wr_vec    = safeRow(wheelVeh.w_r_vect,    k, 4);
u_vec     = safeRow(wheelVeh.u_cr,        k, 4);
v_vec     = safeRow(wheelVeh.v_cr,        k, 4);
Fx_vec    = safeRow(wheelVeh.Fx,          k, 4);
Fy_vec    = safeRow(wheelVeh.Fy,          k, 4);
if isfield(wheelVeh, 'Fz')
    Fz_vec = safeRow(wheelVeh.Fz, k, 4);
else
    Fz_vec = zeros(1,4);
end
%% ── Distinct colors ────────────────────────────────────────────────────
colVel   = [0.00 0.45 0.74];   % blue
colFx    = [0.90 0.25 0.05];   % orange/red
colFy    = [0.55 0.10 0.65];   % purple
colFz    = [0.00 0.55 0.35];   % teal green
colDelta = [0.85 0.10 0.10];   % red
colAlpha = [0.10 0.60 0.20];   % green
%% ── Independent graphical scales ───────────────────────────────────────
velScale = 0.13;
maxVelArrow = 0.95;
maxFxArrow = 1.35;     % about three times the previous version
maxFyArrow = 1.35;
maxFzArrow = 1.05;
FxMaxAbs = max(abs(Fx_vec));
FyMaxAbs = max(abs(Fy_vec));
FzMaxAbs = max(abs(Fz_vec));
if FxMaxAbs > 1e-9
    scaleFx = maxFxArrow / FxMaxAbs;
else
    scaleFx = 0;
end
if FyMaxAbs > 1e-9
    scaleFy = maxFyArrow / FyMaxAbs;
else
    scaleFy = 0;
end
if FzMaxAbs > 1e-9
    scaleFz = maxFzArrow / FzMaxAbs;
else
    scaleFz = 0;
end
%% ── Draw wheels, velocity and forces ───────────────────────────────────
for iw = 1:4
    p  = wheelPos(iw, :);
    d  = delta_vec(iw);
    aw = alpha_vec(iw);
    uLocal = u_vec(iw);
    vLocal = v_vec(iw);
    Fx = Fx_vec(iw);
    Fy = Fy_vec(iw);
    Fz = Fz_vec(iw);
    % Wheel unit vectors in the body frame
    eLong = [cos(d), sin(d)];
    eLat  = [-sin(d), cos(d)];
    % Wheel as a rectangle
    draw_tyre_rect_topview(ax, p(1), p(2), d, Dwheel, Wwheel, wheelNames{iw});
    % Blue line removed: it was the longitudinal axis/centerline of the tyre.
    % The wheel rectangle and the V arrow already make the orientation clear.
    % Delta arc
    draw_angle_arc_local(ax, p, 0, d, 0.22, colDelta, '\delta');
    %% Local velocity: tail at wheel center, tip forward
    Vbody = uLocal * eLong + vLocal * eLat;
    Vdraw = capVector(velScale * Vbody, maxVelArrow);
    quiver(ax, p(1), p(2), Vdraw(1), Vdraw(2), 0, ...
        'Color', colVel, ...
        'LineWidth', 1.9, ...
        'MaxHeadSize', 0.9, ...
        'HandleVisibility', 'off');
    text(ax, p(1)+Vdraw(1), p(2)+Vdraw(2), 'V', ...
        'Color', colVel, ...
        'FontWeight', 'bold', ...
        'FontSize', 11);
    %% Fx: tip at the wheel center, tail set back
    FxDraw = scaleFx * Fx * eLong;
    pFxTip  = p;
    pFxTail = pFxTip - FxDraw;
    quiver(ax, pFxTail(1), pFxTail(2), FxDraw(1), FxDraw(2), 0, ...
        'Color', colFx, ...
        'LineWidth', 4.6, ...
        'MaxHeadSize', 1.35, ...
        'HandleVisibility', 'off');
    text(ax, pFxTail(1), pFxTail(2), 'F_x', ...
        'Color', colFx, ...
        'FontWeight', 'bold', ...
        'FontSize', 22, ...
        'HorizontalAlignment', 'center', ...
        'VerticalAlignment', 'middle');
    %% Fy: tip in the wheel area, tail set back along Fy
    FyDraw = scaleFy * Fy * eLat;
    pFyTip  = p - (Dwheel/3) * eLong;
    pFyTail = pFyTip - FyDraw;
    quiver(ax, pFyTail(1), pFyTail(2), FyDraw(1), FyDraw(2), 0, ...
        'Color', colFy, ...
        'LineWidth', 4.6, ...
        'MaxHeadSize', 1.35, ...
        'HandleVisibility', 'off');
    text(ax, pFyTail(1), pFyTail(2), 'F_y', ...
        'Color', colFy, ...
        'FontWeight', 'bold', ...
        'FontSize', 22, ...
        'HorizontalAlignment', 'center', ...
        'VerticalAlignment', 'middle');
    %% Fz: representation in the plane, outside the vehicle
    % Left wheels: arrow towards +y. Right wheels: arrow towards -y.
    % The scale is independent of Fx and Fy but proportional among the four Fz.
    if p(2) >= 0
        sideSign = 1;
        yTailFz = halfWBody + 0.18;
        vAlignFz = 'bottom';
    else
        sideSign = -1;
        yTailFz = -halfWBody - 0.18;
        vAlignFz = 'top';
    end
    FzLen = scaleFz * abs(Fz);
    pFzTail = [p(1), yTailFz];
    FzDraw = [0, sideSign * FzLen];
    quiver(ax, pFzTail(1), pFzTail(2), FzDraw(1), FzDraw(2), 0, ...
        'Color', colFz, ...
        'LineWidth', 3.4, ...
        'MaxHeadSize', 1.25, ...
        'HandleVisibility', 'off');
    text(ax, pFzTail(1), pFzTail(2) + FzDraw(2), 'F_z', ...
        'Color', colFz, ...
        'FontWeight', 'bold', ...
        'FontSize', 18, ...
        'HorizontalAlignment', 'center', ...
        'VerticalAlignment', vAlignFz);
    %% Alpha arc: from the wheel axis to the local velocity direction
    if norm(Vbody) > 1e-6
        thetaV = atan2(Vbody(2), Vbody(1));
        draw_angle_arc_local(ax, p, d, thetaV, 0.33, colAlpha, '\alpha');
    else
        draw_angle_arc_local(ax, p, d, d + aw, 0.33, colAlpha, '\alpha');
    end
    %% Wheel data text with units
    if iw==1
        txt_x= 1.2;
        txt_y =  1.8;
        hAlign = 'left';
        vAlign = 'bottom';
    elseif iw==2
        txt_x= 1.2;
        txt_y =  - 1.8;
        hAlign = 'left';
        vAlign = 'top';
    elseif iw==3
        txt_x= -1.2;
        txt_y =  1.8;
        hAlign = 'right';
        vAlign = 'bottom';
    else
        txt_x= -1.2;
        txt_y =  -1.8;
        hAlign = 'right';
        vAlign = 'top';
    end
    txt = sprintf(['%s\n' ...
        '\\delta=%.1f° | \\alpha_w=%.1f°\n' ...
        'slip=%.3f | \\omega=%.2f rad/s\n' ...
        'u=%.2f m/s | v=%.2f m/s\n' ...
        'Fx=%.0f N | Fy=%.0f N\n' ...
        'Fz=%.0f N'], ...
        wheelNames{iw}, ...
        rad2deg(d), rad2deg(aw), ...
        slip_vec(iw), wr_vec(iw), ...
        uLocal, vLocal, ...
        Fx, Fy, Fz);
    text(ax, txt_x, txt_y, txt, ...
        'HorizontalAlignment', hAlign, ...
        'VerticalAlignment', vAlign, ...
        'FontSize', 7.5, ...
        'FontWeight', 'bold', ...
        'BackgroundColor', 'w', ...
        'Margin', 2);
end
%% ── Axes and consistent zoom for all subplots ──────────────────────────
xMin = xRearBody  - 0.90;
xMax = xFrontBody + 0.70;
yMin = -halfWBody - 1.75;
yMax =  halfWBody + 1.75;
% ── Fixed axes: must stay identical in every frame ────────────────────
set(ax, 'DataAspectRatio', [1 1 1]);
xlim(ax, [xMin, xMax]);
ylim(ax, [yMin, yMax]);
set(ax, 'XLimMode', 'manual');
set(ax, 'YLimMode', 'manual');
set(ax, 'DataAspectRatioMode', 'manual');
set(ax, 'PlotBoxAspectRatioMode', 'manual');
xlabel(ax, 'x_{body} [m]');
ylabel(ax, 'y_{body} [m]');
end
function draw_tyre_rect_topview(ax, xc, yc, delta, Dwheel, Wwheel, label)
%DRAW_TYRE_RECT_TOPVIEW Draw the tyre as a top-view rectangle.
%
% Dwheel = size along the rolling axis
% Wwheel = tyre width
tyreLocal = [ ...
     Dwheel/2,  Wwheel/2;
     Dwheel/2, -Wwheel/2;
    -Dwheel/2, -Wwheel/2;
    -Dwheel/2,  Wwheel/2];
R = [cos(delta), -sin(delta);
     sin(delta),  cos(delta)];
tyreRot = (R * tyreLocal')';
patch(ax, tyreRot(:,1)+xc, tyreRot(:,2)+yc, [0.18 0.18 0.18], ...
    'EdgeColor', 'k', ...
    'LineWidth', 1.2, ...
    'FaceAlpha', 0.96, ...
    'HandleVisibility', 'off');
text(ax, xc, yc, label, ...
    'Color', 'w', ...
    'FontWeight', 'bold', ...
    'FontSize', 8, ...
    'HorizontalAlignment', 'center', ...
    'VerticalAlignment', 'middle', ...
    'HandleVisibility', 'off');
end
function draw_angle_arc_local(ax, p, ang0, ang1, rArc, colorArc, labelText)
%DRAW_ANGLE_ARC_LOCAL Draw a local angular arc centered at p.
dAng = wrapToPiLocalPlot(ang1 - ang0);
if abs(dAng) < deg2rad(0.5)
    return;
end
th = linspace(ang0, ang0 + dAng, 30);
xArc = p(1) + rArc * cos(th);
yArc = p(2) + rArc * sin(th);
plot(ax, xArc, yArc, '-', ...
    'Color', colorArc, ...
    'LineWidth', 1.3, ...
    'HandleVisibility', 'off');
text(ax, xArc(end), yArc(end), labelText, ...
    'Color', colorArc, ...
    'FontWeight', 'bold', ...
    'FontSize', 8.5, ...
    'HorizontalAlignment', 'center', ...
    'HandleVisibility', 'off');
end
function vOut = capVector(vIn, maxNorm)
%CAPVECTOR Limit the graphical length of a 2D vector.
n = norm(vIn);
if n > maxNorm && n > eps
    vOut = vIn / n * maxNorm;
else
    vOut = vIn;
end
end
function row = safeRow(A, k, nCols)
%SAFEROW Extract A(k,:) with index protection.
if isempty(A)
    row = zeros(1, nCols);
    return;
end
kSafe = min(max(1, k), size(A,1));
row = A(kSafe, :);
if numel(row) ~= nCols
    row = reshape(row, 1, []);
    if numel(row) < nCols
        row = [row, zeros(1, nCols-numel(row))];
    else
        row = row(1:nCols);
    end
end
end
function ang = wrapToPiLocalPlot(ang)
%WRAPTOPILOCALPLOT Wrap an angle to [-pi, pi].
ang = mod(ang + pi, 2*pi) - pi;
end