mc_multi_raim.m

%% MC_MULTI_RAIM
% Monte Carlo evaluation of multi-robot cooperative RAIM with voting.
%
% Compares 4 configs + max-residual baseline:
%   1. AHC      / largest
%   2. DBSCAN   / combined
%   3. DBSCAN   / largest
%   4. Max-residual voting (each detector votes using max-residual only)
%
% What varies per trial:
%   - fault_bot    uniform draw from {1..nRobots}
%   - fault_mag    uniform draw from [0.5, 20] m  (step bias)
%   - fault_dir    random unit vector
%   - startPhases  random path start positions for all robots
%
% Metrics per trial:
%   - Detection rate      (correct robot flagged, fault-active steps)
%   - False alarm rate    (any robot wrongly flagged, healthy steps)
%   - False isolation     (wrong robot flagged when fault active)
%
% Outputs:
%   - Summary table
%   - Sensitivity: detection/false-alarm rate vs fault magnitude (binned)

clear; clc; close all;
t_start = tic;   % start total timer

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

% =========================================================================
%% PARAMETERS
% =========================================================================

N_TRIALS    = 1000;
T           = 300;
arenaSize   = 30;
nRobots     = 6;
stateDim    = 2;

sigma_range = 0.2;
sigma_pos   = 0.15;
fault_time  = 60;
alpha       = 1e-3;
voteThresh  = 2;

fault_mag_range = [0.5, 20.0];

% Fault types evaluated (loop over all)
fault_types    = {'step','drift','ramp'};
drift_duration = 150;     % steps to full magnitude (drift)
sigma_drift    = 0.15;    % per-step noise std dev (ramp)

fault_steps_per_trial = T - fault_time + 1;

% --- 4 configs ---
methods  = {'ahc',    'dbscan',   'dbscan',   'maxresid'};
criteria = {'largest','combined', 'largest',  'n/a'};
nConfigs = numel(methods);

configNames = {'AHC / largest', 'DBSCAN / combined', ...
               'DBSCAN / largest', 'Max Residual (voting)'};

% =========================================================================
%% PRE-GENERATE TRIAL DRAWS
% =========================================================================

fprintf('Pre-generating %d trial draws...\n', N_TRIALS);

trial_fault_bot   = zeros(N_TRIALS, 1);
trial_fault_mag   = zeros(N_TRIALS, 1);
trial_fault_vec   = zeros(N_TRIALS, 2);
trial_startPhases = zeros(N_TRIALS, nRobots);

for tr = 1:N_TRIALS
    rng(tr);
    trial_fault_bot(tr)     = randi(nRobots);
    fm = fault_mag_range(1) + diff(fault_mag_range)*rand();
    trial_fault_mag(tr)     = fm;
    fd = randn(1,2); fd = fd/norm(fd);
    trial_fault_vec(tr,:)   = fm * fd;
    trial_startPhases(tr,:) = rand(1, nRobots);
end

fprintf('Fault mag range: [%.2f, %.2f] m\n', min(trial_fault_mag), max(trial_fault_mag));

% =========================================================================
%% PREALLOCATE
% =========================================================================

nFaultTypes = numel(fault_types);
% Per-trial metrics [N_TRIALS x nConfigs x nFaultTypes]
res_detRate  = zeros(N_TRIALS, nConfigs, nFaultTypes);
res_faRate   = zeros(N_TRIALS, nConfigs, nFaultTypes);
res_isoRate  = zeros(N_TRIALS, nConfigs, nFaultTypes);   % correct flag rate (fault steps)

covPos = sigma_pos^2 * eye(2);

% =========================================================================
%% MAIN LOOP
% =========================================================================

fprintf('Running %d trials x %d configs x %d fault types...\n\n', N_TRIALS, nConfigs, nFaultTypes);

for fi = 1:nFaultTypes
fault_type = fault_types{fi};
fprintf('\n--- Fault type: %s ---\n', fault_type);

for tr = 1:N_TRIALS

    if mod(tr,10)==0
        fprintf('  Trial %3d / %d   fault_bot=R%d   fault_mag=%.2fm\n', ...
                tr, N_TRIALS, trial_fault_bot(tr), trial_fault_mag(tr));
    end

    fault_bot = trial_fault_bot(tr);
    fault_vec = trial_fault_vec(tr,:);
    fault_mag = trial_fault_mag(tr) ;
    sp        = trial_startPhases(tr,:)';

    paths = generate_paths_multi(T, arenaSize, sp);

    % Pre-compute full bias trajectory for this trial (no persistent state)
    bias_traj  = zeros(T, 2);
    ramp_st_mc = zeros(1, 2);
    for t_pre = fault_time:T
        switch lower(fault_type)
            case 'step'
                bias_traj(t_pre,:) = fault_vec;
            case 'drift'
                frac = min((t_pre-fault_time)/max(drift_duration,1), 1.0);
                bias_traj(t_pre,:) = frac * fault_vec;
            case 'ramp'
                N_ramp_steps = T - fault_time + 1;
                sigma_d_trial = fault_mag / sqrt(pi * N_ramp_steps / 2);
                ramp_st_mc = ramp_st_mc + sigma_d_trial * randn(1,2);
                bias_traj(t_pre,:) = ramp_st_mc;
                % ramp_st_mc = ramp_st_mc + sigma_drift * randn(1,2);
                % bias_traj(t_pre,:) = ramp_st_mc;
        end
    end

    % Noise seed separate from draw seed
    rng(tr + N_TRIALS*1000);

    % Per-step buffers — shared across configs
    step_faultActive = false(T, 1);
    step_pEst        = zeros(nRobots, 2, T);
    step_ranges      = zeros(nRobots, nRobots, T);

    % Pre-simulate all steps (generate measurements once)
    for t = 1:T
        pTrue = squeeze(paths.robots(t,:,:))';
        pEst  = pTrue + sigma_pos * randn(nRobots, 2);

        faultActive = (t >= fault_time);
        step_faultActive(t) = faultActive;
        if faultActive
            pEst(fault_bot,:) = pEst(fault_bot,:) + bias_traj(t,:);
        end

        r_all = zeros(nRobots, nRobots);
        for i = 1:nRobots
            for j = 1:nRobots
                if i ~= j
                    r_all(i,j) = norm(pTrue(i,:)-pTrue(j,:)) + sigma_range*randn();
                end
            end
        end

        step_pEst(:,:,t)   = pEst;
        step_ranges(:,:,t) = r_all;
    end

    fault_steps   = find(step_faultActive);
    healthy_steps = find(~step_faultActive);

    % Run each config on the same measurements
    for c = 1:nConfigs
        correct_flags = false(T, 1);
        false_alarms  = false(T, 1);
        false_isos    = false(T, 1);

        isMaxResid = strcmp(methods{c}, 'maxresid');

        for t = 1:T
            pEst  = step_pEst(:,:,t);
            r_all = step_ranges(:,:,t);

            if isMaxResid
                % Pure max-residual voting: each detector votes using
                % identify_fault_maxresidual only (no clustering)
                voteTally = zeros(nRobots, 1);
                for d = 1:nRobots
                    hIdx   = setdiff(1:nRobots, d);
                    pH     = pEst(hIdx,:);
                    rH     = r_all(d, hIdx)';
                    [xF, vRes, ~, W] = wls_position(pH, rH, covPos, sigma_range);
                    [det, ~, ~]      = chi2_detect(vRes, W, stateDim, alpha);
                    if ~det, continue; end
                    [fLocal, ~] = identify_fault_maxresidual(pH, rH, covPos, sigma_range);
                    if fLocal >= 1 && fLocal <= numel(hIdx)
                        voteTally(hIdx(fLocal)) = voteTally(hIdx(fLocal)) + 1;
                    end
                end
                % Apply tie-breaking (max normalised residual)
                maxV = max(voteTally);
                faultFlagged = false(nRobots,1);
                if maxV >= voteThresh
                    tied = find(voteTally == maxV);
                    if isscalar(tied)
                        faultFlagged(tied) = true;
                    else
                        % Tiebreaker: mean normalised residual across detectors
                        scores = zeros(numel(tied),1);
                        for ti = 1:numel(tied)
                            r = tied(ti);
                            dets = setdiff(1:nRobots,r);
                            nr = zeros(numel(dets),1);
                            for di = 1:numel(dets)
                                d2   = dets(di);
                                hIdx2 = setdiff(1:nRobots,d2);
                                pH2   = pEst(hIdx2,:);
                                rH2   = r_all(d2,hIdx2)';
                                [~,vR2,~,W2] = wls_position(pH2,rH2,covPos,sigma_range);
                                localIdx = find(hIdx2==r);
                                if ~isempty(localIdx)
                                    nr(di) = abs(vR2(localIdx))*sqrt(W2(localIdx,localIdx));
                                end
                            end
                            scores(ti) = mean(nr);
                        end
                        [~,wi] = max(scores);
                        faultFlagged(tied(wi)) = true;
                    end
                end
            else
                % Clustering config
                [~, faultFlagged] = run_cooperative_raim( ...
                    pEst, r_all, covPos, sigma_range, ...
                    'alpha',      alpha,          ...
                    'method',     methods{c},     ...
                    'criterion',  criteria{c},    ...
                    'voteThresh', voteThresh,     ...
                    'stateDim',   stateDim,       ...
                    'verbose',    false);
            end

            if step_faultActive(t)
                correct_flags(t) = faultFlagged(fault_bot);
                false_isos(t)    = any(faultFlagged(setdiff(1:nRobots,fault_bot)));
            else
                false_alarms(t)  = any(faultFlagged);
            end
        end

        if ~isempty(fault_steps)
            res_detRate(tr,c,fi)  = mean(correct_flags(fault_steps));
            res_isoRate(tr,c,fi)  = mean(false_isos(fault_steps));
        end
        if ~isempty(healthy_steps)
            res_faRate(tr,c,fi)   = mean(false_alarms(healthy_steps));
        end
    end
end

end   % tr
% end   % fi

fprintf('\nDone.\n\n');
t_mc = toc(t_start);
fprintf('Monte Carlo runtime: %s\n\n', format_duration(t_mc));

% Save workspace results for standalone plotting
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');
fprintf('Results saved to mc_multi_raim_results.mat\n\n');


% =========================================================================
%% SUMMARY TABLE
% =========================================================================

for fi = 1:nFaultTypes
fprintf('==========================================================================\n');
fprintf('  Fault type: %-8s | %d trials | fault ~ U[%.1f,%.1f]m\n', ...
        fault_types{fi}, N_TRIALS, fault_mag_range(1), fault_mag_range(2));
fprintf('%-22s  %12s  %12s  %12s\n','Config','Det rate (%)','FA rate (%)','False iso (%)');
fprintf('%s\n', repmat('-',1,64));
for c = 1:nConfigs
    fprintf('%-22s  %11.1f%%  %11.2f%%  %11.1f%%\n', configNames{c}, ...
            mean(res_detRate(:,c,fi))*100, ...
            mean(res_faRate(:,c,fi))*100,  ...
            mean(res_isoRate(:,c,fi))*100);
end
fprintf('==========================================================================\n');
end

% =========================================================================
%% FIGURE 1 — Grouped bar chart: all fault types × all configs × all metrics
% =========================================================================
% Three subplots (detection, false alarm, false isolation).
% X-axis groups: step | drift | ramp.
% Within each group: 4 bars, one per config.
% =========================================================================

figTitles = {'Step fault','Drift fault','Ramp fault'};

cCols = [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

mTitles  = {'Detection rate (%)','False alarm rate (%)','False isolation rate (%)'};
metrics  = {res_detRate, res_faRate, res_isoRate};

nFT = nFaultTypes;   % 3
nC  = nConfigs;      % 4
groupWidth = 0.72;
barWidth   = groupWidth / nC;

figure('Name','MC Summary — all fault types','Position',[50 50 1300 440]);

for m = 1:3
    ax = subplot(1,3,m); hold on; grid on;

    for fi = 1:nFT
        vals = mean(metrics{m}(:,:,fi), 1) * 100;    % [1 x nC]
        errs = std(metrics{m}(:,:,fi),  0, 1) * 100; % [1 x nC]

        groupCentre = fi;
        for c = 1:nC
            xc = groupCentre + (c - (nC+1)/2) * barWidth;
            bar(xc, vals(c), barWidth*0.92, ...
                'FaceColor', cCols(c,:), 'EdgeColor','none', ...
                'HandleVisibility', ternary(fi==1,'on','off'), ...
                'DisplayName', configNames{c});
            errorbar(xc, vals(c), errs(c), 'k.', 'LineWidth',1, ...
                     'HandleVisibility','off');
        end
    end

    % Group labels
    ax.XTick = 1:nFT;
    ax.XTickLabel = figTitles;
    ax.XLim = [0.5  nFT+0.5];
    ylabel('%');
    title(mTitles{m});

    % Y-axis: auto-scale detection, fix others at [0 8] to show near-zero
    if m == 1
        ylim([0 105]);
    else
        ylim([0 8]);
    end

    if m == 1
        legend('Location','southeast','FontSize',8,'NumColumns',2);
    end
end

sgtitle(sprintf('Cooperative RAIM — all fault types  |  %d trials  |  %d robots  |  threshold=%d', ...
        N_TRIALS, nRobots, voteThresh), 'FontSize',12,'FontWeight','bold');
save_figure(gcf, imgDir, 'Fig1_MC_summary_all_fault_types');

% =========================================================================
%% FIGURE 2 — Sensitivity: DBSCAN/combined vs Max Residual, all fault types
% =========================================================================
% Three rows (metrics) × three columns (fault types).
% Two lines per panel: proposed (DBSCAN/combined) and baseline (Max Residual).
% Line style encodes fault type when panels are collapsed; here fault type
% has its own column so line style distinguishes the two methods.
% =========================================================================

nBins      = 8;
edges      = linspace(fault_mag_range(1), fault_mag_range(2), nBins+1);
binCentres = (edges(1:end-1) + edges(2:end)) / 2;

% Index of DBSCAN/combined in configNames
bestC = find(strcmp(methods,'dbscan') & strcmp(criteria,'combined'), 1);

% Colour: proposed = teal, baseline = red
propCol = [0.13 0.55 0.33];
baseCol = [0.85 0.25 0.25];

figure('Name','Sensitivity — proposed vs baseline','Position',[60 60 1300 680]);

for fi = 1:nFT
    % Bin data for this fault type
    binProp = nan(nBins,1); binPropStd = nan(nBins,1);
    binBase = nan(nBins,1); binBaseStd = nan(nBins,1);
    binDet  = nan(nBins,1);
    binFa   = nan(nBins,1);
    binIso  = nan(nBins,1);

    for b = 1:nBins
        if b==nBins
            mask = trial_fault_mag>=edges(b) & trial_fault_mag<=edges(b+1);
        else
            mask = trial_fault_mag>=edges(b) & trial_fault_mag< edges(b+1);
        end
        if sum(mask)<2, continue; end
        binDet(b) = mean(res_detRate(mask, bestC, fi))*100;
        binFa(b)  = mean(res_faRate( mask, bestC, fi))*100;
        binIso(b) = mean(res_isoRate(mask, bestC, fi))*100;
    end

    % Row 1: Detection rate
    subplot(3, nFT, fi); hold on; grid on;
    v = ~isnan(binDet);
    plot(binCentres(v), binDet(v), '-o', 'Color',propCol,'LineWidth',2,'MarkerSize',4, ...
         'DisplayName','DBSCAN / combined');
    % Max-residual detection (same for all configs — use config 4)
    binMaxDet = nan(nBins,1);
    for b = 1:nBins
        if b==nBins; mask2 = trial_fault_mag>=edges(b) & trial_fault_mag<=edges(b+1);
        else;        mask2 = trial_fault_mag>=edges(b) & trial_fault_mag< edges(b+1); end
        if sum(mask2)<2, continue; end
        binMaxDet(b) = mean(res_detRate(mask2, end, fi))*100;
    end
    vm = ~isnan(binMaxDet);
    plot(binCentres(vm), binMaxDet(vm), '--s','Color',baseCol,'LineWidth',2,'MarkerSize',4, ...
         'DisplayName','Max Residual');
    ylabel('Detection rate (%)'); ylim([0 105]);
    xlim([fault_mag_range(1) fault_mag_range(2)]);
    title(figTitles{fi});
    if fi==1; legend('Location','southeast','FontSize',8); end

    % Row 2: False alarm rate
    subplot(3, nFT, nFT+fi); hold on; grid on;
    v2 = ~isnan(binFa);
    plot(binCentres(v2), binFa(v2), '-o','Color',propCol,'LineWidth',2,'MarkerSize',4, ...
         'HandleVisibility','off');
    binMaxFa = nan(nBins,1);
    for b = 1:nBins
        if b==nBins; mask2 = trial_fault_mag>=edges(b) & trial_fault_mag<=edges(b+1);
        else;        mask2 = trial_fault_mag>=edges(b) & trial_fault_mag< edges(b+1); end
        if sum(mask2)<2, continue; end
        binMaxFa(b) = mean(res_faRate(mask2, end, fi))*100;
    end
    vm2 = ~isnan(binMaxFa);
    plot(binCentres(vm2), binMaxFa(vm2),'--s','Color',baseCol,'LineWidth',2,'MarkerSize',4, ...
         'HandleVisibility','off');
    ylabel('False alarm rate (%)'); ylim([0 5]);
    xlim([fault_mag_range(1) fault_mag_range(2)]);

    % Row 3: False isolation rate
    subplot(3, nFT, 2*nFT+fi); hold on; grid on;
    v3 = ~isnan(binIso);
    plot(binCentres(v3), binIso(v3), '-o','Color',propCol,'LineWidth',2,'MarkerSize',4, ...
         'HandleVisibility','off');
    binMaxIso = nan(nBins,1);
    for b = 1:nBins
        if b==nBins; mask2 = trial_fault_mag>=edges(b) & trial_fault_mag<=edges(b+1);
        else;        mask2 = trial_fault_mag>=edges(b) & trial_fault_mag< edges(b+1); end
        if sum(mask2)<2, continue; end
        binMaxIso(b) = mean(res_isoRate(mask2, end, fi))*100;
    end
    vm3 = ~isnan(binMaxIso);
    plot(binCentres(vm3), binMaxIso(vm3),'--s','Color',baseCol,'LineWidth',2,'MarkerSize',4, ...
         'HandleVisibility','off');
    ylabel('False isolation rate (%)'); ylim([0 5]);
    xlim([fault_mag_range(1) fault_mag_range(2)]);
    xlabel('Fault magnitude (m)');
end

sgtitle(sprintf('Sensitivity: DBSCAN/combined vs Max Residual  |  %d trials  |  %d bins', ...
        N_TRIALS, nBins), 'FontSize',12,'FontWeight','bold');
save_figure(gcf, imgDir, 'Fig2_sensitivity_proposed_vs_baseline');

% =========================================================================
%% TOTAL TIME
% =========================================================================

t_total = toc(t_start);
fprintf('\n========================================\n');
fprintf('  Total runtime: %s\n', format_duration(t_total));
fprintf('========================================\n');
save mc_multi_raim_results_new_ramp.mat
% =========================================================================
%% LOCAL HELPERS
% =========================================================================

function save_figure(fh, imgDir, baseName)
% Save figure as PNG, PDF and EPS into imgDir.
    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  (.png / .pdf / .eps)\n', baseName);
end

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

function s = format_duration(sec)
% Format a duration in seconds as a human-readable string.
    if sec < 60
        s = sprintf('%.1f s', sec);
    elseif sec < 3600
        s = sprintf('%dm %ds', floor(sec/60), mod(round(sec),60));
    else
        s = sprintf('%dh %dm %ds', floor(sec/3600), ...
                    floor(mod(sec,3600)/60), mod(round(sec),60));
    end
end