function [fn,zeta,A,Hhat,report] = resolve_close_modes_bic_cmif(f,H,fn,zeta,A,varargin)
% RESOLVE_CLOSE_MODES_BIC_CMIF
% Decide if two close peaks are actually one physical mode using:
%  - Local 1-pole vs 2-pole fit with BIC penalty (toolbox-free; fminsearch)
%  - Optional CMIF (singular value rank test) and residue MAC when Ny>1.
%
% Inputs
%   f    : [Nf×1] frequency grid (Hz)
%   H    : [Nf×Ny] complex FRF(s); columns = outputs/DOFs (SISO => Ny=1)
%   fn   : [Nm×1] current natural frequencies (Hz)
%   zeta : [Nm×1] damping ratios
%   A    : residues — Ny×Nm (for SISO you may pass [Nm×1] or [1×Nm])
%
% Name/Value options:
%   'FRF'        : 'acc/force' (default), 'vel/force', 'disp/force', or '' (→ 'acc/force')
%   'NearPct'    : relative separation to test pairs (default 0.01 = 1%)
%   'WinFactor'  : half-window in ~ (zeta*fn) units (default 2.0)
%   'DeltaBIC'   : merge if BIC(2pole)−BIC(1pole) ≥ DeltaBIC (default 2.0)
%   'UseCMIF'    : also require CMIF/MAC evidence when Ny>1 (default true)
%   'CMIFthr'    : median σ2/σ1 threshold (default 0.15 ⇒ one singular value dominates)
%   'MACthr'     : residue MAC threshold (default 0.98)
%   'Delay'      : common delay τ (sec), default 0
%   'DCGain'     : complex DC baseline, default 0
%   'Verbose'    : true/false
%
% Outputs
%   fn,zeta,A : possibly merged set (A returned as Ny×Nm)
%   Hhat      : reconstructed FRF matrix [Nf×Ny] with updated params
%   report    : struct with fields: pairs, dBIC, CMIF, MAC, merged
%
% Notes
% - BIC uses nReal = 2*Nf_loc*Ny (real residuals); parameter counts include
%   pole params and complex residues per output (penalizes 2-pole properly).
% - With one reference and multiple outputs, CMIF rank is still at most 1
%   per frequency; CMIF is only meaningful with multiple references/inputs.
% - This function assumes an additive FRF model for the local decision.
%   For 'FRF'=='' we treat it as 'acc/force' here (works well for 1-vs-2).
%
% Tom-safe: no toolboxes required.

% ---------- options ----------
p = inputParser;
p.addParameter('FRF','acc/force');
p.addParameter('NearPct',0.01);
p.addParameter('WinFactor',2.0);
p.addParameter('DeltaBIC',2.0);
p.addParameter('UseCMIF',true);
p.addParameter('CMIFthr',0.15);
p.addParameter('MACthr',0.98);
p.addParameter('Delay',0);
p.addParameter('DCGain',0);
p.addParameter('Verbose',false);
p.parse(varargin{:});
opt = p.Results;

% ---------- shapes & defaults ----------
f  = f(:);
[Nf,Ny] = size(H);
fn   = fn(:);
zeta = zeta(:);
if isvector(A)
    A = reshape(A,1,[]);    % 1×Nm
end
if size(A,1) ~= Ny
    % Expand/trim to Ny rows if needed
    if size(A,1)==1 && Ny>1
        A = repmat(A,Ny,1);
    else
        A = reshape(A,Ny,[]);
    end
end
Nm = numel(fn);
if isempty(opt.FRF) || strcmp(opt.FRF,'')
    opt.FRF = 'acc/force';
end

% ---------- full-band synthesis (for return) ----------
Hhat = synth_full(f,fn,zeta,A,opt.FRF,opt.DCGain,opt.Delay);

% ---------- report init ----------
report.pairs  = [];
report.dBIC   = [];
report.CMIF   = [];
report.MAC    = [];
report.merged = [];

% ---------- main loop over close neighbors ----------
i = 1;
while i < Nm
    relsep = abs(fn(i+1)-fn(i))/max(fn(i),eps);
    if relsep <= opt.NearPct
        % Local window centered at the pair’s midpoint
        f0  = 0.5*(fn(i)+fn(i+1));
        fBW = max(zeta(i)*fn(i) + zeta(i+1)*fn(i+1), 1e-6*max(f));
        I   = f >= (f0 - opt.WinFactor*fBW) & f <= (f0 + opt.WinFactor*fBW);
        if nnz(I) < 10
            i = i + 1; continue;
        end
        floc = f(I);
        Hloc = H(I,:);

        % --- initial guesses & loose bounds (f,z) ---
        th2_0 = [fn(i) zeta(i) fn(i+1) zeta(i+1)];     % [f1 z1 f2 z2]
        fmin  = 0.8*min(fn(i),fn(i+1));
        fmax  = 1.2*max(fn(i),fn(i+1));
        map2  = make_mapper([fmin 1e-4 fmin 1e-4], [fmax 0.25 fmax 0.25], th2_0);

        % one-pole guess near weighted average
        w = vecnorm(A(:,i)) + vecnorm(A(:,i+1)) + eps;
        f1_0 = (vecnorm(A(:,i))*fn(i) + vecnorm(A(:,i+1))*fn(i+1)) / w;
        z1_0 = max(min((zeta(i)+zeta(i+1))/2, 0.2), 1e-4);
        th1_0 = [f1_0 z1_0];
        map1  = make_mapper([fmin 1e-4], [fmax 0.25], th1_0);

        % --- objectives: global RSS over outputs with residues solved linearly ---
        J1 = @(y) rss_model(floc,Hloc, map1.f(y), opt.FRF,opt.DCGain,opt.Delay);  % 1-pole
        J2 = @(y) rss_model(floc,Hloc, map2.f(y), opt.FRF,opt.DCGain,opt.Delay);  % 2-pole

        % Nelder–Mead (base MATLAB)
        opts = optimset('Display','off','MaxFunEvals',2000,'MaxIter',2000);
        [y1,fval1] = fminsearch(J1, map1.y0, opts);  x1 = map1.f(y1);
        [y2,fval2] = fminsearch(J2, map2.y0, opts);  x2 = map2.f(y2);

        % get fitted residues for diagnostics
        [~,A1loc] = rss_model(floc,Hloc, x1, opt.FRF,opt.DCGain,opt.Delay);  % Ny×1
        [~,A2loc] = rss_model(floc,Hloc, x2, opt.FRF,opt.DCGain,opt.Delay);  % Ny×2

        % --- BIC (penalize extra pole + residues) ---
        nReal = 2*nnz(I)*Ny;
        k1 = 2 + 2*Ny;    % (f,z) + complex residue per output
        k2 = 4 + 4*Ny;    % two poles + 2 complex residues per output
        BIC1 = nReal*log(fval1/max(nReal,1)) + k1*log(max(nReal,1));
        BIC2 = nReal*log(fval2/max(nReal,1)) + k2*log(max(nReal,1));
        dBIC = BIC2 - BIC1;   % >0 favors 1-pole

        % --- CMIF/MAC evidence (only meaningful if Ny>1 and multiple refs) ---
        cmif_ratio = NaN;
        mac12      = NaN;
        if opt.UseCMIF && Ny>1
            cmif_ratio = cmif_s2_over_s1(Hloc);       % median σ2/σ1
            mac12      = mac_complex(A2loc(:,1), A2loc(:,2));
        end

        mergeOK = (dBIC >= opt.DeltaBIC);
        if Ny>1 && opt.UseCMIF
            % Require additional evidence for merging in MIMO
            evidence = (cmif_ratio <= opt.CMIFthr) | (mac12 >= opt.MACthr);
            mergeOK = mergeOK & evidence;
        end

        % record diagnostics
        report.pairs(end+1,:)  = [i i+1];
        report.dBIC(end+1,1)   = dBIC;
        report.CMIF(end+1,1)   = cmif_ratio;
        report.MAC(end+1,1)    = mac12;
        report.merged(end+1,1) = mergeOK;

        if opt.Verbose
            fprintf('Pair [%d %d] f=[%.4g %.4g] Hz: dBIC=%.2f, CMIF=%.3f, MAC=%.3f → %s\n',...
                i,i+1,fn(i),fn(i+1),dBIC,cmif_ratio,mac12, tern(mergeOK,'MERGE','keep'));
        end

        if mergeOK
            % replace pair with the single pole (x1)
            fn(i)   = x1(1);
            zeta(i) = x1(2);
            A(:,i)  = A1loc;       % Ny×1
            % delete second
            fn(i+1)   = [];
            zeta(i+1) = [];
            A(:,i+1)  = [];
            Nm = Nm - 1;
            % do not increment i; new neighbor might also be close
        else
            i = i + 1;
        end
    else
        i = i + 1;
    end
end

% final synthesis with updated params
Hhat = synth_full(f,fn,zeta,A,opt.FRF,opt.DCGain,opt.Delay);
end

% ====================== helpers ======================

function mapper = make_mapper(lb,ub,x0)
% Vectorized bounds transform for fminsearch (no arrayfun pitfalls)
lb = lb(:)'; 
ub = ub(:)'; 
x0 = x0(:)';

to_y = @(x) logit( (x - lb) ./ max(ub - lb, eps) );
to_x = @(y) lb + invlogit(y) .* (ub - lb);

mapper.f  = @(y) to_x(y);
mapper.y0 = to_y( min(max(x0, lb), ub) );
end

function [rss,Ahat] = rss_model(floc,Hloc, x, FRF, DC, tau)
% x is [f1 z1] or [f1 z1 f2 z2]
% Returns RSS over all outputs, solving residues by complex least squares.
w = 2*pi*floc(:); Ny = size(Hloc,2);
K = numel(x)/2;

Phi = zeros(numel(w), K);
for k=1:K
    fk = x(2*k-1);  zk = x(2*k);
    wn = 2*pi*fk;
    Hx = 1 ./ ( (wn^2 - w.^2) + 1i*(2*zk*wn).*w );  % receptance
    switch lower(FRF)
        case 'disp/force', Hk = Hx;
        case 'vel/force',  Hk = 1i*w .* Hx;
        case 'acc/force',  Hk = (1i*w).^2 .* Hx;
        otherwise,         Hk = (1i*w).^2 .* Hx;
    end
    Phi(:,k) = Hk;
end

E = exp(-1i*2*pi*floc(:)*tau);
Phi = E .* Phi;

% subtract DC (known)
Hloc_dc = Hloc - DC;

% residues per output via least squares
Ahat = zeros(Ny,K);
res  = 0;
for j=1:Ny
    a = Phi \ Hloc_dc(:,j);
    r = Hloc_dc(:,j) - Phi*a;
    Ahat(j,:) = a.';
    res = res + sum(abs(r).^2);
end
rss = res;
end

function Hs = synth_full(f,fn,zeta,A,FRF,DC,tau)
w = 2*pi*f(:); Ny = size(A,1); Nm = numel(fn);
Hs = repmat(DC + 0*w, 1, Ny);
E  = exp(-1i*2*pi*f(:)*tau);
for k=1:Nm
    wn = 2*pi*fn(k);
    Hx = 1 ./ ( (wn^2 - w.^2) + 1i*(2*zeta(k)*wn).*w );  % receptance
    switch lower(FRF)
        case 'disp/force', Hk = Hx;
        case 'vel/force',  Hk = 1i*w .* Hx;
        case 'acc/force',  Hk = (1i*w).^2 .* Hx;
        otherwise,         Hk = (1i*w).^2 .* Hx;
    end
    Hs = Hs + (E .* Hk) * A(:,k).';
end
end

function r = cmif_s2_over_s1(Hloc)
% Median ratio of 2nd to 1st singular value over the local window.
% With single reference, rank≤1 per freq ⇒ ratio is typically 0.
[Nf,~] = size(Hloc);
rat = zeros(Nf,1);
for i=1:Nf
    s = svd(Hloc(i,:),'econ');    % row vector → at most one non-zero σ
    if numel(s) >= 2
        rat(i) = s(2)/max(s(1),eps);
    else
        rat(i) = 0;
    end
end
r = median(rat);
end

function m = mac_complex(a,b)
% Modal Assurance Criterion for complex vectors a,b (Ny×1)
m = (abs(a' * b)^2) / (max(a'*a,eps) * max(b'*b,eps));
end

function y = logit(p)
p = max(min(p,1-1e-12),1e-12);
y = log(p./(1-p));
end

function p = invlogit(y)
p = 1./(1+exp(-y));
end

function val = tern(tf,a,b)
if tf, val=a; else, val=b; end
end
