plot_results.m

%% PLOT_RESULTS
% Standalone plotting script. Loads saved .mat results from mc_multi_raim
% and mc_config_comparison and produces the two consolidated publication
% figures for each.
%
% Expected .mat files (save from the MC scripts using):
%   save('mc_multi_raim_results.mat',   'res_detRate','res_faRate','res_isoRate', ...
%        'trial_fault_mag','N_TRIALS','nConfigs','nFaultTypes','fault_types', ...
%        'configNames','fault_mag_range','nRobots','voteThresh');
%
%   save('mc_config_results.mat',       'res_idAcc','res_errClean', ...
%        'res_idAcc_maxr','res_errClean_maxr','res_detRate_sr','res_faRate_sr', ...
%        'res_errFull','trial_fault_mag','N_TRIALS','nConfigs','fault_mag_range', ...
%        'methods','criteria','configNames','N');
%
% Edit the file paths below to match where you saved your .mat files.

clear; clc; close all;

% =========================================================================
%% PATHS — edit these
% =========================================================================

mat_multi  = 'mc_multi_raim_results_5.mat';
mat_single = 'mc_config_results.mat';

imgDir = fullfile(fileparts(mfilename('fullpath')), 'new_images');
if ~exist(imgDir,'dir'), mkdir(imgDir); end
fprintf('Output folder: %s\n\n', imgDir);

% =========================================================================
%% LOAD
% =========================================================================

fprintf('Loading multi-robot results from: %s\n', mat_multi);
mr = load(mat_multi);

% Set to false to skip single-robot figures
plot_sr = false;

fprintf('Loading single-robot results from: %s\n\n', mat_single);
if plot_sr
    sr = load(mat_single);
end

% =========================================================================
%% MULTI-ROBOT FIGURE — Identification accuracy across 4 configs × 3 fault types
% =========================================================================
% Identification accuracy = fraction of fault-active steps where the
% correct robot was flagged by the voting system (= res_detRate).
%
% Layout: grouped bar chart
%   X-axis groups : Step | Drift | Ramp
%   Bars per group: one per config (4 bars)
%   Error bars    : ±1 std across trials
%   Overlay       : individual trial points (jittered) for distribution
% =========================================================================

figTitles_mr = {'Step fault','Drift fault','Ramp fault'};
cCols_mr = [0.18 0.44 0.76;   % AHC / largest
            0.13 0.55 0.33;   % DBSCAN / combined
            0.80 0.40 0.10;   % DBSCAN / largest
            0.85 0.25 0.25];  % Max Residual

nFT = mr.nFaultTypes;
nC  = mr.nConfigs;
groupWidth = 0.72;
barWidth   = groupWidth / nC;

% Three subplots side by side — one per fault type
% Each panel: 4 CDF curves, one per config
% X-axis: ID accuracy (0–100%), Y-axis: fraction of trials ≤ x

lineStyles_mr = {'-','--',':','-.'};

figure('Name','MR: ID accuracy CDFs','Position',[50 50 1200 420]);

for fi = 1:nFT
    subplot(1, nFT, fi); hold on; grid on; box on;

    for c = 1:nC
        vals = sort(mr.res_detRate(:, c, fi) * 100);   % sorted ID acc per trial
        % CDF: for n trials, the k-th sorted value has CDF = k/n
        n    = numel(vals);
        cdf  = (1:n)' / n * 100;   % express as percentage of trials

        stairs(cdf, vals, lineStyles_mr{c}, ...
               'Color',  cCols_mr(c,:), ...
               'LineWidth', 2, ...
               'DisplayName', mr.configNames{c});
    end

    % Reference lines (now on swapped axes)
    yline(50,  'Color',[0.7 0.7 0.7],'LineStyle',':','LineWidth',0.8,'HandleVisibility','off');
    yline(90,  'Color',[0.7 0.7 0.7],'LineStyle',':','LineWidth',0.8,'HandleVisibility','off');
    xline(50,  'Color',[0.7 0.7 0.7],'LineStyle',':','LineWidth',0.8,'HandleVisibility','off');

    ylabel('ID accuracy (%)');
    if fi == 1
        xlabel('Trials (%)');
        legend('Location','northwest','FontSize',8);
    else
        xlabel('Trials (%)');
    end
    title(figTitles_mr{fi});
    xlim([0 100]); ylim([0 100]);
    xticks(0:25:100); yticks(0:25:100);
end

sgtitle(sprintf('Identification accuracy CDFs  |  %d trials  |  %d robots  |  threshold=%d', ...
        mr.N_TRIALS, mr.nRobots, mr.voteThresh), 'FontSize',12,'FontWeight','bold');

save_fig(gcf, imgDir, 'MR_ID_accuracy_CDFs');

% =========================================================================
%% MULTI-ROBOT FIGURE 2 — Vote margin CDF
% =========================================================================
% Vote margin = winner votes − runner-up votes at steps where correct robot
% was flagged.  A margin of 1 means the decision was fragile; 5 means
% unanimous.  X-axis = margin (0..nRobots), Y-axis = fraction of trials.

figure('Name','MR: vote margin CDFs','Position',[70 70 1200 420]);

for fi = 1:nFT
    subplot(1, nFT, fi); hold on; grid on; box on;
    for c = 1:nC
        vals = mr.res_voteMargin(:, c, fi);
        vals = vals(~isnan(vals));          % only trials with correct flags
        if isempty(vals), continue; end
        sv  = sort(vals);
        cdf = (1:numel(sv))' / numel(sv) * 100;
        stairs(cdf, sv, lineStyles_mr{c}, ...
               'Color', cCols_mr(c,:), 'LineWidth', 2, ...
               'HandleVisibility', tern(fi==1,'on','off'), ...
               'DisplayName', mr.configNames{c});
    end
    yline(1, 'Color',[0.7 0.7 0.7],'LineStyle',':','LineWidth',0.8,'HandleVisibility','off');
    ylabel('Vote margin (winner − runner-up)');
    xlabel('Trials (%)');
    title(figTitles_mr{fi});
    xlim([0 100]); ylim([0 mr.nRobots]);
    yticks(0:mr.nRobots);
    if fi==1; legend('Location','northwest','FontSize',8); end
end

sgtitle(sprintf('Vote margin CDFs  |  %d trials  |  threshold=%d', ...
        mr.N_TRIALS, mr.voteThresh), 'FontSize',12,'FontWeight','bold');
save_fig(gcf, imgDir, 'MR_vote_margin_CDFs');

% =========================================================================
%% MULTI-ROBOT FIGURE 3 — False isolation rate vs fault magnitude
% =========================================================================

nBins_fi = 8;
edges_fi = linspace(mr.fault_mag_range(1), mr.fault_mag_range(2), nBins_fi+1);
bc_fi    = (edges_fi(1:end-1) + edges_fi(2:end)) / 2;

figure('Name','MR: false isolation vs fault mag','Position',[80 80 1200 420]);

for fi = 1:nFT
    subplot(1, nFT, fi); hold on; grid on; box on;
    for c = 1:nC
        binIso = nan(nBins_fi,1);
        for b = 1:nBins_fi
            if b==nBins_fi; mask = mr.trial_fault_mag>=edges_fi(b) & mr.trial_fault_mag<=edges_fi(b+1);
            else;           mask = mr.trial_fault_mag>=edges_fi(b) & mr.trial_fault_mag< edges_fi(b+1); end
            if sum(mask)<2, continue; end
            binIso(b) = mean(mr.res_isoRate(mask,c,fi))*100;
        end
        v = ~isnan(binIso);
        plot(bc_fi(v), binIso(v), lineStyles_mr{c}, ...
             'Color', cCols_mr(c,:), 'LineWidth', 2, ...
             'HandleVisibility', tern(fi==1,'on','off'), ...
             'DisplayName', mr.configNames{c});
    end
    xlabel('Fault magnitude (m)'); ylabel('False isolation rate (%)');
    title(figTitles_mr{fi});
    xlim([mr.fault_mag_range(1) mr.fault_mag_range(2)]); ylim([0 10]);
    if fi==1; legend('Location','northeast','FontSize',8); end
end

sgtitle(sprintf('False isolation rate vs fault magnitude  |  %d trials', mr.N_TRIALS), ...
        'FontSize',12,'FontWeight','bold');
save_fig(gcf, imgDir, 'MR_false_isolation_vs_mag');

% =========================================================================
%% TABLE 1 — Summary statistics per config × fault type
% =========================================================================

fprintf('\n');
fprintf('%s\n', repmat('=',1,88));
fprintf('  SUMMARY STATISTICS  |  %d trials  |  %d robots  |  threshold=%d\n', ...
        mr.N_TRIALS, mr.nRobots, mr.voteThresh);
fprintf('%s\n', repmat('=',1,88));
fprintf('%-22s  %-8s  %9s  %10s  %10s  %8s  %9s\n', ...
        'Config','Fault','Det (%)','Med ID (%)','90th (%)','FA (%)','FIso (%)');
fprintf('%s\n', repmat('-',1,88));

for fi = 1:nFT
    for c = 1:nC
        det  = mean(mr.res_detRate(:,c,fi))*100;
        fa   = mean(mr.res_faRate(:,c,fi))*100;
        fiso = mean(mr.res_isoRate(:,c,fi))*100;
        % Median and 90th percentile from the trial distribution
        id_vals = mr.res_detRate(:,c,fi)*100;
        med90   = prctile(id_vals, [50 90]);
        fprintf('%-22s  %-8s  %8.1f%%  %9.1f%%  %9.1f%%  %7.2f%%  %8.1f%%\n', ...
                mr.configNames{c}, mr.fault_types{fi}, det, med90(1), med90(2), fa, fiso);
    end
    if fi < nFT; fprintf('%s\n', repmat('-',1,88)); end
end
fprintf('%s\n', repmat('=',1,88));

% =========================================================================
%% TABLE 2 — Pairwise delta vs Max Residual baseline
% =========================================================================

baseC = mr.nConfigs;   % last config = Max Residual

fprintf('\n');
fprintf('%s\n', repmat('=',1,72));
fprintf('  DELTA vs MAX RESIDUAL BASELINE  (positive = better)\n');
fprintf('%s\n', repmat('=',1,72));
fprintf('%-22s  %-8s  %12s  %12s  %12s\n', ...
        'Config','Fault','Δ Det (pp)','Δ Med ID (pp)','Δ FIso (pp)');
fprintf('%s\n', repmat('-',1,72));

for fi = 1:nFT
    base_det  = mean(mr.res_detRate(:,baseC,fi))*100;
    base_fiso = mean(mr.res_isoRate(:,baseC,fi))*100;
    base_med  = median(mr.res_detRate(:,baseC,fi)*100);

    for c = 1:nC-1   % exclude baseline itself
        det  = mean(mr.res_detRate(:,c,fi))*100;
        fiso = mean(mr.res_isoRate(:,c,fi))*100;
        med  = median(mr.res_detRate(:,c,fi)*100);

        d_det  = det  - base_det;
        d_med  = med  - base_med;
        d_fiso = fiso - base_fiso;   % negative = fewer false isolations = better

        % Significance markers
        mk_det  = sig_mark(mr.res_detRate(:,c,fi),  mr.res_detRate(:,baseC,fi));
        mk_fiso = sig_mark(mr.res_isoRate(:,c,fi),  mr.res_isoRate(:,baseC,fi));

        fprintf('%-22s  %-8s  %+10.1fpp%s  %+10.1fpp    %+10.1fpp%s\n', ...
                mr.configNames{c}, mr.fault_types{fi}, ...
                d_det, mk_det, d_med, d_fiso, mk_fiso);
    end
    if fi < nFT; fprintf('%s\n', repmat('-',1,72)); end
end
fprintf('%s\n', repmat('=',1,72));
fprintf('  pp = percentage points.  * p<0.05,  ** p<0.01  (paired Wilcoxon signed-rank)\n');
fprintf('%s\n', repmat('=',1,72));



% =========================================================================
%% SINGLE-ROBOT FIGURE 1
%  3×3 accuracy heatmap
% =========================================================================

methodList    = {'ahc','kmeans','dbscan'};
criterionList = {'largest','minvar','combined'};
meanAcc       = mean(sr.res_idAcc, 1, 'omitnan') * 100;   % [1 x nConfigs]

accGrid = zeros(3,3);
for mi = 1:3
    for ki = 1:3
        for c = 1:sr.nConfigs
            if strcmp(sr.methods{c}, methodList{mi}) && strcmp(sr.criteria{c}, criterionList{ki})
                accGrid(ki,mi) = meanAcc(c);
            end
        end
    end
end

nLevels  = 64;
blueRamp = [linspace(1,0.08,nLevels)', linspace(1,0.38,nLevels)', linspace(1,0.75,nLevels)'];

figure('Name','SR: config accuracy heatmap','Position',[200 200 540 420]);
ax = gca;
imagesc(ax, accGrid);
colormap(ax, blueRamp);
lo = max(0,   min(accGrid(:)) - 5);
hi = min(100, max(accGrid(:)) + 5);
clim(ax, [lo hi]);
colorbar(ax);

for ki = 1:3
    for mi = 1:3
        val = accGrid(ki,mi);
        textcol = 'k'; if val > lo+0.6*(hi-lo); textcol='w'; end
        text(ax, mi, ki, sprintf('%.1f%%', val), ...
             'HorizontalAlignment','center','VerticalAlignment','middle', ...
             'FontSize',13,'FontWeight','bold','Color',textcol);
    end
end

ax.XTick = 1:3; ax.YTick = 1:3;
ax.XTickLabel = {'AHC','K-Means','DBSCAN'};
ax.YTickLabel = {'Largest','Min Var','Combined'};
ax.FontSize = 11;
xlabel('Clustering method'); ylabel('Selection criterion');
title(sprintf('Mean ID accuracy (%%)  |  %d trials  |  fault mag ~ U[%.1f,%.1f]m', ...
      sr.N_TRIALS, sr.fault_mag_range(1), sr.fault_mag_range(2)), 'FontSize',11);
save_fig(gcf, imgDir, 'SR_Fig1_config_heatmap');

% =========================================================================
%% SINGLE-ROBOT FIGURE 2
%  Ranked bar chart: ID accuracy + position error
% =========================================================================

baselineAcc = mean(sr.res_idAcc_maxr,    'omitnan') * 100;
baselineErr = mean(sr.res_errClean_maxr, 'omitnan');

allAcc   = [meanAcc, baselineAcc];
allStd   = [arrayfun(@(c) std(sr.res_idAcc(:,c),'omitnan')*100, 1:sr.nConfigs), ...
             std(sr.res_idAcc_maxr,'omitnan')*100];
allErr   = [arrayfun(@(c) mean(sr.res_errClean(:,c),'omitnan'), 1:sr.nConfigs), baselineErr];
allErrSt = [arrayfun(@(c) std(sr.res_errClean(:,c),'omitnan'), 1:sr.nConfigs), ...
             std(sr.res_errClean_maxr,'omitnan')];
allNames = [sr.configNames; {'Max Residual'}];

[sortedAcc, sIdx] = sort(allAcc,'descend');
sortedStd    = allStd(sIdx);
sortedErr    = allErr(sIdx);
sortedErrStd = allErrSt(sIdx);
sortedNames  = allNames(sIdx);

barCols = repmat([0.3 0.5 0.8], numel(allAcc), 1);
basePos = find(sIdx == sr.nConfigs+1);
if ~isempty(basePos), barCols(basePos,:) = [0.85 0.25 0.25]; end

figure('Name','SR: config ranking','Position',[50 50 1100 520]);

subplot(1,2,1); hold on; grid on; box on;
for i = 1:numel(sortedAcc)
    bar(i, sortedAcc(i), 0.6,'FaceColor',barCols(i,:),'EdgeColor','none');
end
errorbar(1:numel(sortedAcc), sortedAcc, sortedStd,'k.','LineWidth',1.2);
xticks(1:numel(sortedAcc)); xticklabels(sortedNames); xtickangle(35);
ylabel('Mean ID accuracy (%)');
title('Identification accuracy — all configs');
ylim([max(0, min(sortedAcc)-10) 105]);

subplot(1,2,2); hold on; grid on; box on;
for i = 1:numel(sortedErr)
    bar(i, sortedErr(i), 0.6,'FaceColor',barCols(i,:),'EdgeColor','none');
end
errorbar(1:numel(sortedErr), sortedErr, sortedErrStd,'k.','LineWidth',1.2);
yline(mean(sr.res_errFull),'r--','LineWidth',1.5,'DisplayName','Full WLS (biased)');
xticks(1:numel(sortedErr)); xticklabels(sortedNames); xtickangle(35);
ylabel('Mean clean position error (m)');
title('Position error after exclusion');

sgtitle(sprintf('Single-robot config comparison  |  %d trials  |  fault mag ~ U[%.1f,%.1f]m  |  N=%d helpers', ...
        sr.N_TRIALS, sr.fault_mag_range(1), sr.fault_mag_range(2), sr.N), ...
        'FontSize',12,'FontWeight','bold');
save_fig(gcf, imgDir, 'SR_Fig2_config_ranking');

% =========================================================================
%% SINGLE-ROBOT FIGURE 3
%  Sensitivity: ID accuracy vs fault magnitude
% =========================================================================

nBins_sr   = 10;
edges_sr   = linspace(sr.fault_mag_range(1), sr.fault_mag_range(2), nBins_sr+1);
binCentres_sr = (edges_sr(1:end-1) + edges_sr(2:end)) / 2;

methodCols_sr = containers.Map({'ahc','kmeans','dbscan'}, ...
    {[0.18 0.44 0.76],[0.13 0.55 0.33],[0.80 0.40 0.10]});
lineStyles_sr   = {'-','--',':'};
criterionList_s = {'largest','minvar','combined'};

binAcc_c = nan(nBins_sr, sr.nConfigs);
binStd_c = nan(nBins_sr, sr.nConfigs);
binAcc_m = nan(nBins_sr, 1);
binStd_m = nan(nBins_sr, 1);
binDet_s = nan(nBins_sr, 1);

for b = 1:nBins_sr
    if b==nBins_sr
        mask = sr.trial_fault_mag>=edges_sr(b) & sr.trial_fault_mag<=edges_sr(b+1);
    else
        mask = sr.trial_fault_mag>=edges_sr(b) & sr.trial_fault_mag< edges_sr(b+1);
    end
    if sum(mask)<2, continue; end
    for c = 1:sr.nConfigs
        v = sr.res_idAcc(mask,c)*100;
        binAcc_c(b,c) = mean(v,'omitnan');
        binStd_c(b,c) = std(v, 'omitnan');
    end
    vm = sr.res_idAcc_maxr(mask)*100;
    binAcc_m(b) = mean(vm,'omitnan');
    binStd_m(b) = std(vm, 'omitnan');
    if isfield(sr,'res_detRate')
        binDet_s(b) = mean(sr.res_detRate(mask))*100;
    end
end

figure('Name','SR: sensitivity','Position',[80 80 1100 480]);

ax_l = subplot(1,2,1); hold on; grid on; box on;
for c = 1:sr.nConfigs
    col = methodCols_sr(sr.methods{c});
    ki  = find(strcmp(criterionList_s, sr.criteria{c}));
    ls  = lineStyles_sr{ki};
    v   = ~isnan(binAcc_c(:,c));
    plot(binCentres_sr(v), binAcc_c(v,c), ls, 'Color',col,'LineWidth',1.8, ...
         'DisplayName', sr.configNames{c});
    xf = [binCentres_sr(v), fliplr(binCentres_sr(v))];
    yf = [binAcc_c(v,c)'+binStd_c(v,c)', fliplr(binAcc_c(v,c)'-binStd_c(v,c)')];
    fill(xf, max(0,min(100,yf)), col, 'FaceAlpha',0.07,'EdgeColor','none','HandleVisibility','off');
end
vm = ~isnan(binAcc_m);
plot(binCentres_sr(vm), binAcc_m(vm), 'r-','LineWidth',2.5,'DisplayName','Max Residual');
fill([binCentres_sr(vm),fliplr(binCentres_sr(vm))], ...
     max(0,min(100,[binAcc_m(vm)'+binStd_m(vm)', fliplr(binAcc_m(vm)'-binStd_m(vm)')])), ...
     [0.85 0.25 0.25],'FaceAlpha',0.07,'EdgeColor','none','HandleVisibility','off');
xlabel('Fault magnitude (m)'); ylabel('Mean ID accuracy (%)');
title('All 9 configs vs Max Residual  (mean \pm 1 std)');
legend('Location','southeast','FontSize',7,'NumColumns',2);
ylim([0 105]); xlim([sr.fault_mag_range(1) sr.fault_mag_range(2)]);

ax_r = subplot(1,2,2); hold on; grid on; box on;
if any(~isnan(binDet_s))
    vd = ~isnan(binDet_s);
    plot(binCentres_sr(vd), binDet_s(vd), 'k-o','LineWidth',2,'MarkerSize',5, ...
         'DisplayName','Detection rate');
end
yline(100*(1-1e-3),'r--','LineWidth',1.5, ...
      'DisplayName','Target (1-\alpha) = 99.9%');
xlabel('Fault magnitude (m)'); ylabel('%');
title('Detection rate vs fault magnitude');
legend('Location','southeast'); ylim([0 105]);
xlim([sr.fault_mag_range(1) sr.fault_mag_range(2)]);

sgtitle(sprintf('Single-robot sensitivity  |  %d trials  |  %d bins', ...
        sr.N_TRIALS, nBins_sr), 'FontSize',12,'FontWeight','bold');
save_fig(gcf, imgDir, 'SR_Fig3_sensitivity');

% end  % plot_sr

% =========================================================================
%% DONE
% =========================================================================

fprintf('\nAll figures saved to: %s\n', imgDir);
fprintf('Files produced:\n');
saved = {'MR_Fig1_summary_all_fault_types', 'MR_Fig2_sensitivity_proposed_vs_baseline', ...
         'SR_Fig1_config_heatmap', 'SR_Fig2_config_ranking', 'SR_Fig3_sensitivity'};
for k = 1:numel(saved)
    fprintf('  %s  (.png / .pdf / .eps)\n', saved{k});
end

% =========================================================================
%% LOCAL HELPERS
% =========================================================================

function save_fig(fh, imgDir, baseName)
    base = fullfile(imgDir, baseName);
    exportgraphics(fh, [base '.png'], 'Resolution', 300);
    exportgraphics(fh, [base '.pdf'], 'ContentType', 'vector');
    exportgraphics(fh, [base '.eps'], 'ContentType', 'vector');
    fprintf('  Saved: %s\n', baseName);
end

function out = tern(cond, a, b)
    if cond; out = a; else; out = b; end
end

function mk = sig_mark(a, b)
% Paired Wilcoxon signed-rank test; returns '**', '*', or '' marker.
    try
        p = signrank(a(:), b(:));
        if     p < 0.01; mk = '**';
        elseif p < 0.05; mk = ' *';
        else;            mk = '  ';
        end
    catch
        mk = '  ';
    end
end