function plot_swarm_signals(plotGlobal, scenario)
%PLOT_SWARM_SIGNALS  Static time plots of the signals in Outport 1
%   (the PlotGlobal bus) of swarm_phase_5, one set of figures per vehicle.
%
%   plot_swarm_signals(plotGlobal, scenario)
%
%   Every figure is saved as PNG + PDF in
%       <project>\..\latex_tesi_claude\Images\signal_plots
%   and is left open.
%
%   Changes w.r.t. the first version:
%     - the vehicle x and y position plots are no longer produced;
%     - heading_err and psi_err are drawn as WIDE (rectangular) figures;
%     - psi_err is overlaid with the heading psi and the steering angle;
%     - the speed figure overlays the actual speed with the raw and the
%       filtered speed reference (the standalone v_ref plots are dropped).

    N_VEH = size(scenario.start, 1);
    % -- Font globale per ogni figura (tick, etichette, titolo, legenda) --
    FS = 13;                                   % base assi (era ~10): alza qui
    oldFS = get(groot, 'defaultAxesFontSize');
    set(groot, 'defaultAxesFontSize', FS);
    cleanupFS = onCleanup(@() set(groot, 'defaultAxesFontSize', oldFS)); %#ok<NASGU>
    % -- Output folder ---------------------------------------------------
    this_dir = fileparts(mfilename('fullpath'));
    out_dir  = fullfile(this_dir, '..', 'latex_tesi_claude', 'Images', ...
                        'signal_plots');
    if ~exist(out_dir, 'dir')
        mkdir(out_dir);
    end
    fprintf('[signals] Signal plots will be saved in:\n    %s\n', out_dir);

    % -- 4-wheel plotting style ------------------------------------------
    wheelLeg = {'FL','FR','RL','RR'};
    wheelCol = [0.85 0.10 0.10;    % FL
                0.10 0.45 0.85;    % FR
                0.10 0.60 0.20;    % RL
                0.55 0.30 0.70];   % RR

    posStd  = [90 90 760 460];     % standard figure
    posWide = [70 90 1180 340];    % wide / rectangular figure

    % -- Signal catalogue (single-signal figures) ------------------------
    %   columns: {key, path (after 'pl_i.'), title, kind, y-label, wide}
    %   kind = 'scalar' | 'wheel' | 'rank'
    SIG = {
       'ranking',             'ranking',                               'Ranking',                          'rank',   'rank [-]',          false
       'pct_known',           'pct_known',                             'Discovered map fraction',          'scalar', 'fraction [-]',      false
       'psi',                 'plotveicolo.values.psi',                'Heading angle \psi',               'scalar', '\psi [rad]',        false
       'Out_xi',              'plotveicolo.AccelSterzo.Out_xi',        'Throttle/brake command',           'scalar', '\xi [%]',           false
       'delta_sterzo',        'plotveicolo.AccelSterzo.delta_sterzo',  'Steering command',                 'scalar', '\delta [rad]',      false
       'cross_track_error',   'plotveicolo.heading_err',               'Cross-track error',                'scalar', 'cross-track [m]',   true
       'obstacle_brake_flag', 'plotveicolo.obstacle_brake_flag',       'Obstacle brake flag',              'scalar', 'flag [-]',          false
       'hold_flag',           'plotveicolo.hold_flag',                 'Hold flag',                        'scalar', 'flag [-]',          false
       'mu_next',             'plotveicolo.mu_next',                   'Friction look-ahead \mu_{next}',   'scalar', '\mu [-]',           false
       'unknown_ratio',       'plotveicolo.unknown_ratio',             'Unknown-area ratio',               'scalar', 'ratio [-]',         false
       'slip_vect',           'plotveicolo.WheelData.slip_vect',       'Wheel longitudinal slip',          'wheel',  'slip [-]',          false
       'w_r_vect',            'plotveicolo.WheelData.w_r_vect',        'Wheel angular speed \omega',       'wheel',  '\omega [rad/s]',    false
       'alpha_wheel',         'plotveicolo.WheelData.alpha_wheel',     'Wheel slip angle \alpha_w',        'wheel',  '\alpha_w [rad]',    false
       'delta_vect',          'plotveicolo.WheelData.delta_vect',      'Wheel steering angle \delta',      'wheel',  '\delta [rad]',      false
       'u_cr',                'plotveicolo.WheelData.u_cr',            'Wheel-centre longitudinal speed u','wheel',  'u [m/s]',           false
       'v_cr',                'plotveicolo.WheelData.v_cr',            'Wheel-centre lateral speed v',     'wheel',  'v [m/s]',           false
       'Fx',                  'plotveicolo.WheelData.Fx',              'Tyre longitudinal force F_x',      'wheel',  'F_x [N]',           false
       'Fy',                  'plotveicolo.WheelData.Fy',              'Tyre lateral force F_y',           'wheel',  'F_y [N]',           false
       'Cm',                  'plotveicolo.WheelData.Cm',              'Engine Torque C_m',                'wheel',  'C_m [Nm]',          true
       'Fz',                  'plotveicolo.WheelData.Fz',              'Tyre vertical force F_z',          'wheel',  'F_z [N]',           false
       'ABS_flag',            'plotveicolo.ABS_flag',                  'ABS flag',                         'wheel',  'ABS flag [-]',      false
    
    };

    n_sig  = size(SIG, 1);
    n_made = 0;
    n_fail = 0;

    for i = 1:N_VEH
        base    = sprintf('pl_%d', i);
        vehName = sprintf('ID%d', i);

        % Time vector from a signal that always exists
        try
            t = getBusTime(plotGlobal, [base '.plotveicolo.values.x']);
        catch
            warning('plot_swarm_signals:noTime', ...
                'Cannot read the time vector for %s: vehicle skipped.', vehName);
            continue;
        end
        N = numel(t);

        % ---- single-signal figures -------------------------------------
        for s = 1:n_sig
            key  = SIG{s,1};
            sigp = [base '.' SIG{s,2}];
            ttl  = SIG{s,3};
            kind = SIG{s,4};
            ylab = SIG{s,5};
            wide = SIG{s,6};

            try
                raw = getBusData(plotGlobal, sigp);
            catch ME
                warning('plot_swarm_signals:missingSignal', ...
                    'Signal "%s" not found (%s): skipped.', sigp, ME.message);
                n_fail = n_fail + 1;
                continue;
            end

            if wide, pos = posWide; else, pos = posStd; end
            fig = figure('Name', sprintf('%s | %s', vehName, ttl), ...
                'NumberTitle', 'off', 'Color', 'w', 'Visible', 'off', 'Position', pos);
            ax = axes('Parent', fig); %#ok<LAXES>
            hold(ax, 'on'); grid(ax, 'on');

            switch kind
                case 'wheel'
                    W = forceNBy4(double(raw), N, key);
                    for c = 1:4
                        plot(ax, t, W(:,c), '-', ...
                            'LineWidth', 1.5, 'Color', wheelCol(c,:));
                    end
                    legend(ax, wheelLeg, 'Location', 'best');

                case 'rank'
                    R = double(raw);
                    if size(R,1) ~= N && size(R,2) == N
                        R = R.';
                    end
                    if isvector(R)
                        R = R(:);
                    end
                    nc  = size(R,2);
                    lbl = cell(1, nc);
                    for c = 1:nc
                        plot(ax, t(1:size(R,1)), R(:,c), '-', 'LineWidth', 1.5);
                        lbl{c} = sprintf('component %d', c);
                    end
                    legend(ax, lbl, 'Location', 'best');

                otherwise   % scalar
                    d = double(raw(:));
                    n = min(numel(d), N);
                    plot(ax, t(1:n), d(1:n), '-', ...
                        'LineWidth', 1.6, 'Color', [0.10 0.25 0.75]);
            end

            xlabel(ax, 't [s]');
            ylabel(ax, ylab);
            % title(ax, sprintf('%s — %s', vehName, ttl), 'FontWeight',
            % 'bold'); titolo tolto perché già nella tesi
            if t(end) > t(1)
                xlim(ax, [t(1) t(end)]);
            end

            save_one(fig, sprintf('signal_%s_%s', key, vehName), out_dir);
            n_made = n_made + 1;
        end

        % ---- composite figure: psi error + psi + steering (WIDE) -------
        items_pe = { ...
            'plotveicolo.psi_err',                  '\psi error'
            'plotveicolo.values.psi',               '\psi'
            'plotveicolo.AccelSterzo.delta_sterzo', 'steering angle' };
        if make_overlay(plotGlobal, base, vehName, t, N, ...
                sprintf('signal_psi_err_%s', vehName), ...
                'Heading tracking error, heading and steering', ...
                'angle [rad]', items_pe, true, posWide, posStd, out_dir)
            n_made = n_made + 1;
        end

        % ---- composite figure: actual speed + raw/filtered reference ---
        items_v = { ...
            'plotveicolo.values.vel_totale', 'actual speed'
            'plotveicolo.v_ref_raw',         'reference (raw)'
            'plotveicolo.v_ref_filtered',    'reference (filtered)' };
        if make_overlay(plotGlobal, base, vehName, t, N, ...
                sprintf('signal_vel_totale_%s', vehName), ...
                'Speed: actual and references', ...
                'v [m/s]', items_v, false, posWide, posStd, out_dir)
            n_made = n_made + 1;
        end
    end

    fprintf(['[signals] Done: %d figures created and saved (PNG+PDF), ', ...
             '%d signals skipped.\n'], n_made, n_fail);
end


% =========================================================================
function ok = make_overlay(plotGlobal, base, vehName, t, N, ...
        basename, ttl, ylab, items, wide, posWide, posStd, out_dir)
%MAKE_OVERLAY  Plot several scalar signals on the same axes and save it.
    cols = [0.10 0.25 0.75;    % signal 1
            0.85 0.10 0.10;    % signal 2
            0.10 0.60 0.20];   % signal 3
    if wide, pos = posWide; else, pos = posStd; end
    fig = figure('Name', sprintf('%s | %s', vehName, ttl), ...
        'NumberTitle', 'off', 'Color', 'w', 'Visible', 'off', 'Position', pos);
    ax = axes('Parent', fig); %#ok<LAXES>
    hold(ax, 'on'); grid(ax, 'on');
    lbl = {};
    for k = 1:size(items,1)
        try
            d = double(getBusData(plotGlobal, [base '.' items{k,1}]));
        catch ME
            warning('plot_swarm_signals:overlay', ...
                'Signal "%s" not found (%s): skipped.', items{k,1}, ME.message);
            continue;
        end
        d = d(:);
        n = min(numel(d), N);
        plot(ax, t(1:n), d(1:n), '-', 'LineWidth', 1.6, ...
            'Color', cols(min(k,size(cols,1)),:));
        lbl{end+1} = items{k,2}; %#ok<AGROW>
    end
    xlabel(ax, 't [s]');
    ylabel(ax, ylab);
    %title(ax, sprintf('%s — %s', vehName, ttl), 'FontWeight',
    %'bold');commento perché già c'è il titolo dentro la tesi, poi ci sono
    %troppi titoli se no
    if numel(lbl) >= 1
        legend(ax, lbl, 'Location', 'best');
    end
    if t(end) > t(1)
        xlim(ax, [t(1) t(end)]);
    end
    ok = ~isempty(lbl);
    save_one(fig, basename, out_dir);
end


% =========================================================================
function save_one(fig, basename, out_dir)
%SAVE_ONE  Save a figure as PNG + PDF in out_dir (robust: retries, then
%   closes the figure so windows do not pile up and destabilise export).
    saved = false;
    for attempt = 1:3
        try
            drawnow;
            exportgraphics(fig, fullfile(out_dir, [basename '.png']), ...
                'Resolution', 200);
            exportgraphics(fig, fullfile(out_dir, [basename '.pdf']), ...
                'ContentType', 'vector');
            saved = true;
            break;
        catch ME
            if attempt < 3
                pause(0.5); drawnow;
            else
                warning('plot_swarm_signals:save', ...
                    'Figure "%s" not saved after 3 attempts: %s', ...
                    basename, ME.message);
            end
        end
    end
    if isgraphics(fig)
        close(fig);
    end
end
