sim_compare_raim_g.m

%% SIM_COMPARE_RAIM
% Single-trial comparison of cooperative RAIM in two environments:
%   (A) Open square arena  — 30x30 m,  generate_paths_multi
%   (B) Narrow corridor    —  5x30 m,  generate_paths_corridor
%
% Produces one publication figure:
%   Left  — GDOP over time for both environments (mean across all 6 detectors)
%   Right — Detection / ID accuracy / False alarm bar chart
%
% All RAIM parameters, fault parameters, noise seeds, and start phases
% are IDENTICAL between the two runs. Differences are purely geometric.

clear; clc; close all;

imgDir = fullfile(fileparts(mfilename('fullpath')), 'new_images');
if ~exist(imgDir,'dir'), mkdir(imgDir); end

% =========================================================================
%% SHARED PARAMETERS
% =========================================================================

RNG_SEED       = 42;
rng(RNG_SEED);

nRobots        = 6;
T              = 300;
stateDim       = 2;

sigma_range    = 0.2;
sigma_pos      = 0.15;
covPos         = sigma_pos^2 * eye(2);

fault_bot      = randi(nRobots);
fault_time     = 60;
fault_mag      = 5.0;
fault_type     = 'drift';
drift_duration = 100;
sigma_drift    = 0.3;

alpha          = 1e-3;
method         = 'ahc';
criterion      = 'largest';
voteThresh     = 2;

startPhases    = rand(nRobots, 1);
fault_dir      = randn(1,2); fault_dir = fault_dir / norm(fault_dir);
fault_vec      = fault_mag * fault_dir;

% Pre-compute bias trajectory
bias_traj  = zeros(T, 2);
ramp_state = 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'
            ramp_state = ramp_state + sigma_drift * randn(1,2);
            bias_traj(t_pre,:) = ramp_state;
    end
end

% =========================================================================
%% ENVIRONMENT DEFINITIONS
% =========================================================================

envs(1).name      = 'Open Arena (30\times30 m)';
envs(1).shortname = 'Arena';
envs(1).paths     = generate_paths_multi(T, 30, startPhases);

envs(2).name      = 'Corridor (3\times30 m)';
envs(2).shortname = 'Corridor';
envs(2).paths     = generate_paths_corridor(T, 3, 30, startPhases);

nEnvs = numel(envs);

% =========================================================================
%% GDOP COMPUTATION
% =========================================================================

for e = 1:nEnvs
    gdop = zeros(T, nRobots);
    for t = 1:T
        pTrue = squeeze(envs(e).paths.robots(t,:,:))';
        for d = 1:nRobots
            hIdx  = setdiff(1:nRobots, d);
            pH    = pTrue(hIdx,:);
            pd    = pTrue(d,:);
            diffs = pd - pH;
            dists = vecnorm(diffs, 2, 2);
            if any(dists < 1e-6)
                gdop(t,d) = NaN;
                continue;
            end
            H   = diffs ./ dists;
            HtH = H' * H;
            if rcond(HtH) < 1e-12
                gdop(t,d) = NaN;
            else
                gdop(t,d) = sqrt(trace(inv(HtH)));
            end
        end
    end
    envs(e).gdop      = gdop;
    envs(e).gdop_mean = mean(gdop, 2, 'omitnan');
end

% =========================================================================
%% RAIM SIMULATION
% =========================================================================

for e = 1:nEnvs
    log_voteTally    = zeros(T, nRobots);
    log_faultFlagged = false(T, nRobots);
    log_faultActive  = false(T, 1);
    log_correctFlag  = false(T, 1);
    log_falseFlag    = false(T, 1);
    log_anyFlagged   = false(T, 1);

    rng(RNG_SEED + 1000);   % identical noise seed for both envs

    for t = 1:T
        pTrue = squeeze(envs(e).paths.robots(t,:,:))';
        pEst  = pTrue + sigma_pos * randn(nRobots, 2);

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

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

        [voteTally, faultFlagged] = run_cooperative_raim( ...
            pEst, ranges_all, covPos, sigma_range, ...
            'alpha',      alpha,      ...
            'method',     method,     ...
            'criterion',  criterion,  ...
            'voteThresh', voteThresh, ...
            'stateDim',   stateDim,   ...
            'verbose',    false);

        log_voteTally(t,:)    = voteTally';
        log_faultFlagged(t,:) = faultFlagged';
        log_anyFlagged(t)     = any(faultFlagged);

        if faultActive
            log_correctFlag(t) = faultFlagged(fault_bot);
            log_falseFlag(t)   = any(faultFlagged(setdiff(1:nRobots, fault_bot)));
        end
    end

    fault_steps   = find(log_faultActive);
    healthy_steps = find(~log_faultActive);
    det_steps     = fault_steps(log_anyFlagged(fault_steps));

    envs(e).fault_steps   = fault_steps;
    envs(e).healthy_steps = healthy_steps;
    envs(e).det_steps     = det_steps;

    envs(e).detRate   = 100 * sum(log_correctFlag(fault_steps))             / numel(fault_steps);
    envs(e).faRate    = 100 * sum(any(log_faultFlagged(healthy_steps,:),2)) / numel(healthy_steps);
    if isempty(det_steps)
        envs(e).identRate = 0;
    else
        envs(e).identRate = 100 * sum(log_correctFlag(det_steps)) / numel(det_steps);
    end

    fprintf('%s  |  Det=%.1f%%  ID=%.1f%% (of %d detections)  FA=%.2f%%\n', ...
            envs(e).shortname, envs(e).detRate, envs(e).identRate, ...
            numel(det_steps), envs(e).faRate);
end

% =========================================================================
%% MANUSCRIPT FIGURE
% =========================================================================

envCols = [0.18 0.44 0.76;    % Arena    — blue
           0.85 0.35 0.10];   % Corridor — orange

tAxis = 1:T;

fig = figure('Name','Manuscript Comparison','Position',[60 60 1100 400]);

% ---- Left panel: GDOP over time ----------------------------------------

subplot(1, 2, 1);
hold on; grid on; box on;

gdop_vals_all = [envs(1).gdop_mean; envs(2).gdop_mean];
gdop_finite   = gdop_vals_all(isfinite(gdop_vals_all));
gdop_cap      = prctile(gdop_finite, 97) * 1.25;
ylim_gdop     = [0, gdop_cap];

% Fault-active shading
patch([fault_time T T fault_time], ...
      [ylim_gdop(1) ylim_gdop(1) ylim_gdop(2) ylim_gdop(2)], ...
      [1 0.88 0.88], 'FaceAlpha', 0.45, 'EdgeColor', 'none', ...
      'HandleVisibility', 'off');

for e = 1:nEnvs
    % Individual robot traces (thin, faded)
    for r = 1:nRobots
        plot(tAxis, envs(e).gdop(:,r), '-', ...
             'Color', [envCols(e,:), 0.12], 'LineWidth', 0.5, ...
             'HandleVisibility', 'off');
    end
    % Mean GDOP (thick)
    plot(tAxis, envs(e).gdop_mean, '-', ...
         'Color', envCols(e,:), 'LineWidth', 2.2, ...
         'DisplayName', envs(e).shortname);
end

xline(fault_time, 'k--', 'LineWidth', 1.2, 'HandleVisibility', 'off');
text(fault_time + 3, ylim_gdop(2) * 0.08, ...
     sprintf('t_{fault}=%d', fault_time), ...
     'FontSize', 8, 'Color', [0.55 0 0], 'FontAngle', 'italic');

% Mean GDOP annotations stacked at right edge
mu_vals = [mean(envs(1).gdop_mean,'omitnan'), mean(envs(2).gdop_mean,'omitnan')];
vGap    = ylim_gdop(2) * 0.12;
for e = 1:nEnvs
    text(T - 5, ylim_gdop(2)*0.52 + (nEnvs-e)*vGap, ...
         sprintf('mu(%s)=%.2f', envs(e).shortname, mu_vals(e)), ...
         'Color', envCols(e,:), 'FontSize', 8.5, ...
         'HorizontalAlignment', 'right', 'FontWeight', 'bold');
end

xlabel('Time step', 'FontSize', 10);
ylabel('GDOP', 'FontSize', 10);
% title('(a)  Geometric Dilution of Precision', 'FontSize', 11, 'FontWeight', 'bold');
legend('Location', 'northwest', 'FontSize', 9);
ylim(ylim_gdop); xlim([1 T]);

% ---- Right panel: performance bar chart --------------------------------

subplot(1, 2, 2);
hold on; grid on; box on;

metrics   = [envs(1).detRate,   envs(2).detRate;
             envs(1).identRate, envs(2).identRate;
             envs(1).faRate,    envs(2).faRate];
barLabels = {'Detection', 'ID accuracy', 'False alarm'};
nMetrics  = size(metrics, 1);
groupW    = 0.62;
barW      = groupW / nEnvs;

for g = 1:nMetrics
    for e = 1:nEnvs
        xc = g + (e - (nEnvs+1)/2) * barW;
        bar(xc, metrics(g,e), barW * 0.9, ...
            'FaceColor', envCols(e,:), 'EdgeColor', 'none', ...
            'HandleVisibility', ternary(g==1,'on','off'), ...
            'DisplayName', envs(e).shortname);
        labelY = max(metrics(g,e) + 1.8, 3.5);
        text(xc, labelY, sprintf('%.1f', metrics(g,e)), ...
             'HorizontalAlignment', 'center', 'FontSize', 8.5, ...
             'FontWeight', 'bold', 'Color', envCols(e,:) * 0.72);
    end
end

xticks(1:nMetrics);
xticklabels(barLabels);
ylabel('Rate (%)', 'FontSize', 10);
% title('(b)  FDI Performance Metrics', 'FontSize', 11, 'FontWeight', 'bold');
legend('Location', 'northeast', 'FontSize', 9);
ylim([0 118]); xlim([0.4 nMetrics + 0.6]);

% ID accuracy footnote
% text(2, -13, ...
%      sprintf('ID accuracy conditioned on detection events (Arena: n=%d, Corridor: n=%d)', ...
%              numel(envs(1).det_steps), numel(envs(2).det_steps)), ...
%      'HorizontalAlignment', 'center', 'FontSize', 7.5, 'Color', [0.45 0.45 0.45]);

% ---- Supertitle --------------------------------------------------------

% sgtitle(sprintf(['Arena vs Corridor  |  fault = R%d  |  type = %s  |  ' ...
%                  'mag = %.1f m  |  alpha = %.0e  |  seed = %d'], ...
%         fault_bot, fault_type, fault_mag, alpha, RNG_SEED), ...
%         'FontSize', 10, 'FontWeight', 'bold', 'Interpreter', 'none');

% sgtitle(sprintf(['Arena vs Corridor  |  fault = R%d  |  type = %s  |  ' ...
%                  'mag = %.1f m  |  AHC/largest '], ...
%         fault_bot, fault_type, fault_mag), ...
%         'FontSize', 10, 'FontWeight', 'bold', 'Interpreter', 'none');

% ---- Save --------------------------------------------------------------

base = fullfile(imgDir, 'Fig_ArenaVsCorridor');
exportgraphics(fig, [base '.png'], 'Resolution', 300);
exportgraphics(fig, [base '.pdf'], 'ContentType', 'vector');
exportgraphics(fig, [base '.eps'], 'ContentType', 'vector');
fprintf('\nSaved: Fig_ArenaVsCorridor  (.png / .pdf / .eps)\n');

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

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