% ESTIMATE_MODES_HALFPOWER_40
% ---------------------------------------------------------------
% Goal
%   Robustly estimate modal parameters {fn (Hz), ζ (damping ratio), A (modal
%   scale)} from a single complex frequency response (FRF) or transmissibility.
%   Designed to work on clean or noisy data and to handle closely-spaced modes.
%
% What it ingests (THM)
%   THM may be:
%     • [f, Re(H), Im(H)]   (preferred)
%     • [f, Hcomplex]
%     • [f, |H|]            (magnitude-only; complex steps degrade gracefully)
%
% What it returns
%   fn   :  [ndof×1] natural frequencies (Hz)
%   zeta :  [ndof×1] damping ratios (dimensionless)
%   A    :  [ndof×1] modal scale factors estimated by local complex LS
%   info :  diagnostics & artifacts (seed indices, f1/f2 half-power points,
%           overlap flags, per-mode tables, RMSE before/after, fitted H, etc.)
%
% Supported FRF types  (Name-Value: 'FRF', value)
%   '' (empty)       → transmissibility (legacy multiplicative model)
%   'acc/force'      → accelerance    H = Ẍ/F      (additive model)
%   'vel/force'      → mobility       H = Ẋ/F      (additive model)
%   'disp/force'     → receptance     H = X/F      (additive model)
%
% Model forms (auto-selected unless overridden with 'Model')
%   • Multiplicative (transmissibility):     H ≈ Gdc · (1 + Σ A_k·H_k)
%   • Additive (FRFs):                       H ≈ Gdc + Σ A_k·H_k
%   Optional constant baseline correction ΔG is fitted for disp/force.
%
% High-level pipeline
%   1) Preprocess & smoothing
%      - Parse THM → frequency f, complex H, magnitude |H|.
%      - Build smoothed log-magnitude Ls (median + Savitzky–Golay) for robust
%        peak detection. Auto-set DC gain Gdc for additive FRFs.
%
%   2) Robust seeding of candidate modes
%      - Combine multiple cues to find peaks:
%          • Strict/relaxed findpeaks on Ls (dB)
%          • Simple local maxima
%          • Phase gradient on log-frequency for low-f sensitivity
%          • Deviation from low-frequency DC (for additive FRFs)
%      - Merge/score candidates, enforce minimal bin spacing, keep top ndof.
%
%   3) Half-power (-3 dB) initialization (per seed)
%      - Refine each peak location via a 3-point parabola on Ls.
%      - Set Lhp = Lpk − 3 dB on Ls and linearly interpolate left/right
%        crossings to obtain f1, f2.
%      - Initialize:
%            fn   = sqrt(f1·f2)
%            ζ    = (f2 − f1) / (2·fn)
%
%   4) Modal scale A via local complex least squares
%      - In a narrow band around each fn, build the single-DOF kernel
%        H_k(ω; ω_n, ζ) appropriate to the chosen FRF:
%           den = (ω_n^2 − ω^2) + j·2ζω_n·ω
%           disp/force:  1/den
%           vel/force:   jω/den
%           acc/force:  −ω^2/den
%           transmiss.:  (ω^2)/den  (legacy probe)
%      - Solve weighted complex LS (optionally constraining A to be real).
%        Weights emphasize the resonance band.
%
%   5) Small-percent refinement (global fit)
%      - Build multi-mode H using current {fn, ζ, A} and compute complex RMSE.
%      - Iterate nitRef times:
%          • Randomly jitter fn (percent) and ζ (absolute percent) with a
%            cooling schedule; enforce minimum relative separation between modes.
%          • Re-solve A (and optional ΔG) by weighted complex LS.
%          • Keep the parameter set that lowers RMSE.
%
%   6) Overlap diagnostics
%      - Flag pairs whose center spacing is smaller than K·(BW1+BW2)/2, where
%        BW_k = 2ζ_k·fn_k (half-power bandwidth). info.suggested_bands marks
%        regions likely to contain coupled/close modes.
%
%   7) Jitter search for additional hidden modes
%      - For each accepted mode, attempt to add one nearby mode:
%          • Propose fn within ±FnPct and ζ with random jitter (ZetaPct or
%            ZetaScale bounds).
%          • Compare 1-mode vs 2-mode weighted LS error in a local window.
%          • Accept the candidate if the relative error drop ≥ ImproveThresh
%            and separation constraints are satisfied. At most MaxAddsPerMode.
%      - If new modes are added, run a short global refinement again.
%
%   8) Monte-Carlo overlap probe (optional)
%      - Randomly “split” a suspected peak into two nearby modes across many
%        trials with small complex noise; if improvement occurs frequently,
%        keep the extra mode(s) and refine.
%
% Key options to know
%   'FRF'           : FRF type (see above)
%   'Model'         : 'auto'|'additive'|'multiplicative'
%   'DCGain'        : Complex DC baseline (auto for additive FRFs)
%   'Areal'         : true → constrain A real, false → complex A
%   'RefineIters'   : iterations in global small-percent refinement
%   'MinRelSep'     : minimal relative spacing enforced between modes
%   'Jitter'        : struct with Enable, FnPct, ZetaPct/ZetaScale,
%                     Ntrials, ImproveThresh, MinSepPct, WindowBW
%   'MC'            : struct controlling Monte-Carlo split test
%
% Practical tips / limitations
%   • Frequency resolution: aim for ≥3–5 points across each half-power BW.
%   • For strongly overlapped modes, the -3 dB step gives seeds only; the
%     refinement and jitter/MC steps do most of the disambiguation.
%   • Magnitude-only THM works, but complex data improves seeding and LS fits.
%   • For disp/force, allow ΔG fitting to absorb static-like bias.
%   • Set 'Areal'=false if phase/asymmetry suggests complex residues.
%   • RNG is fixed (rng(1)) for reproducibility; change or remove as desired.
%
% Typical calls
%   % Transmissibility (legacy multiplicative model):
%   [fn, zeta, info, A] = estimate_modes_halfpower_40(THM, 3);
%
%   % Accelerance FRF (additive model) with plots:
%   [fn, zeta, info, A] = estimate_modes_halfpower_40(THM, 3, ...
%       'FRF','acc/force','plot',true);
%
%   % Receptance with baseline refit and relaxed separation:
%   [fn, zeta, info, A] = estimate_modes_halfpower_40(THM, 4, ...
%       'FRF','disp/force','FitBaseline',true,'MinRelSep',2e-4);


function [fn, zeta, info, A] = estimate_modes_halfpower_40(THM, ndof, varargin)

% Robust half-power (-3 dB) estimator from complex (or magnitude) transmissibility
% or FRFs (acc/force, vel/force, disp/force), then small-percent MDOF refinement.
% DC-aware seeding to catch low-frequency modes.
%
% NEW (this version):
% - Jittering search around each identified mode to add nearby/hidden modes
%   until the requested ndof is reached (if possible).
% - DEFAULT RANGES INCREASED:
%     Jitter.FnPct = 0.10 (±10%), Ntrials = 600, ZetaScale = [0.5 2.0],
%     ImproveThresh = 0.02, WindowBW = 1.5, MinSepPct = 3e-4
%     MC.PctWithin = 0.12 (±12% split range)
%
% THM: [f, Re(H), Im(H)]  OR  [f, Hcomplex]  OR  [f, |H|] (mag-only ok)

% ---------- defaults & options ----------
opts0 = struct( ...
    'zeta_guess' , 0.02, ...
    'plot'       , false, ...
    'overlapK'   , 0.90, ...
    'Areal'      , true, ...
    'Compare'    , true, ...
    'DCGain'     , 1+0i, ...
    'Delay'      , 0, ...
    'RefineIters', 600, ...
    'RefinePct'  , [1.0 0.04], ...
    'MinRelSep'  , 5e-4, ...
    ... % FRF selection ('' keeps legacy transmissibility)
    'FRF'        , '', ...           % '', 'acc/force', 'vel/force', 'disp/force'
    'Model'      , 'auto', ...       % 'auto' | 'additive' | 'multiplicative'
    ... % seeding helpers for low-f modes
    'SeedDCBoost', true, ...
    'DCFrac'     , 0.06,  ...
    'PhaseLogF'  , true, ...
    ... % disp/force bias removal (ΔG fit during refinement)
    'FitBaseline', [], ...           % [] = auto (true for disp/force additive, else false)
    ... % Jitter search (WIDER DEFAULTS)
    'Jitter', struct('Enable',true,'Ntrials',600,'FnPct',0.10, ...       % ±10% around fk
                     'ZetaScale',[0.5 2.0],'ZetaPct',0.15, ...          % << damping jitter
                     'ImproveThresh',0.03, 'MaxAddsPerMode',1, ...
                     'MinSepPct',3e-4,'WindowBW',1.5),...
    ... % Monte-Carlo overlap options (WIDER SPLIT RANGE)
    'MC', struct('Enable', true, 'PctWithin', 0.12, 'Ntrials', 200, ...
                 'NoiseFrac', 0.01, 'ImproveThresh', 0.03) ...
);
opts = parse_opts(opts0, varargin{:});

rng(1);

zeta_guess = opts.zeta_guess;
plotFlag   = opts.plot;
overlapK   = opts.overlapK;
Areal      = opts.Areal;
doCompare  = opts.Compare;
tau        = opts.Delay;
nitRef     = opts.RefineIters;
refPct     = opts.RefinePct;
minRelSep  = opts.MinRelSep;

% Decide model form from FRF
frfType = lower(string(opts.FRF));        % '', 'acc/force', 'vel/force', 'disp/force'
if string(opts.Model) == "auto"
    if frfType == ""
        modelForm = "multiplicative";     % legacy transmissibility
    else
        modelForm = "additive";           % FRFs
    end
else
    modelForm = string(opts.Model);
end

% ---------- auto-DC baseline for additive FRFs (unless user passed DCGain) ----------
userPassedDC = false;
if ~isempty(varargin)
    if numel(varargin)==1 && isstruct(varargin{1})
        userPassedDC = isfield(varargin{1}, 'DCGain');
    else
        names = varargin(1:2:end);
        userPassedDC = any(strcmpi(string(names), 'DCGain'));
    end
end
if ~userPassedDC && modelForm=="additive"
    if frfType=="acc/force" || frfType=="vel/force"
        opts.DCGain = 0;  % accelerance/mobility → H(0)=0
    elseif frfType=="disp/force"
        % Auto-estimate static compliance from low-frequency band
        [f_tmp, Hc_tmp] = thm_to_complex(THM);
        f_tmp = f_tmp(:); Hc_tmp = Hc_tmp(:);
        DCFrac = max(0.01, min(0.20, opts.DCFrac));   % 1–20% clamp
        nDC = max(5, round(DCFrac*numel(f_tmp))); nDC = min(nDC, numel(f_tmp));
        opts.DCGain = median(Hc_tmp(1:nDC), 'omitnan');
        if ~isfinite(opts.DCGain), opts.DCGain = 0; end
    end
end
Gdc = opts.DCGain;

% ---------- choose whether to refit ΔG baseline during refinement ----------
if isempty(opts.FitBaseline)
    fitBaseline = (modelForm=="additive") && (frfType=="disp/force");
else
    fitBaseline = logical(opts.FitBaseline);
end

% --- complex data from THM ---
[f, Hc] = thm_to_complex(THM);
Tmag = abs(Hc);
mask = isfinite(f) & isfinite(Tmag) & (Tmag>0);
f = f(mask); Hc = Hc(mask); Tmag = Tmag(mask);
[fs,ord] = sort(f); f = fs; Hc = Hc(ord); Tmag = Tmag(ord);
w = 2*pi*f;

% --- smooth log-mag for seeding ---
LdB = 20*log10(Tmag);
spanMed = max(3, round(numel(f)/400));
Ls = movmedian(LdB, spanMed);
if exist('sgolayfilt','file')==2 && numel(f)>=11, Ls = sgolayfilt(Ls,3,11); end

df = median(diff(f));
fmid = median(f);
bw_Hz   = max(2*zeta_guess*fmid, 3*df);
minDist = max(round((zeta_guess*fmid)/max(df,eps)),3);
baseline = movmean(Ls, max(11,round(numel(f)/50)));
prom = max( 6*1.4826*mad(Ls - baseline,1), 0.25 ); % dB

% --- ROBUST, DC-AWARE seeding (multi-source union) ---
[locs, seed_diag] = robust_peak_locs_dcaware(f, Ls, ndof, prom, minDist, bw_Hz, Hc, opts.SeedDCBoost, opts.DCFrac, opts.PhaseLogF);
if isempty(locs)
    fn = []; zeta = []; A = [];
    info = struct('peaks_idx',[],'f1',[],'f2',[],'smooth_dB',Ls, ...
                  'overlap_pairs',[],'suggested_bands',[],'A',[], ...
                  'changes_table',table(),'rmse_before',NaN,'rmse_after',NaN, ...
                  'improvement_pct',NaN,'Hfit',Hc,'seed_diag',seed_diag, ...
                  'mc_overlap', struct(),'jitter',struct(), ...
                  'FRF', frfType, 'Model', modelForm);
    if plotFlag, figure('Color','w'); semilogy(f,Tmag,'.'); title('No peaks found'); end
    if doCompare, compare_real_imag_internal(f,Hc,Hc); end
    return;
end
locs = sort(locs(1:min(ndof,numel(locs))));

% --- half-power per peak (seeds) ---
fn   = nan(numel(locs),1);
zeta = nan(numel(locs),1);
f1s  = nan(numel(locs),1);
f2s  = nan(numel(locs),1);

for k = 1:numel(locs)
    i0 = locs(k);
    % 3-pt parabola on Ls
    if i0>1 && i0<numel(f)
        y1=Ls(i0-1); y2=Ls(i0); y3=Ls(i0+1);
        den = (y1 - 2*y2 + y3);
        if den~=0
            delta = 0.5*(y1 - y3)/den;              % samples
            fpk = f(i0) + delta*(f(i0+1)-f(i0));    % Hz
        else
            fpk = f(i0);
        end
    else
        fpk = f(i0);
    end
    Lpk = interp1(f, Ls, fpk, 'linear','extrap');
    Lhp = Lpk - 3;                                % -3 dB

    il = find(f < fpk & Ls <= Lhp, 1, 'last');    % left
    if ~isempty(il) && il>1
        f1 = interp1(Ls(il-1:il), f(il-1:il), Lhp, 'linear','extrap');
    else
        f1 = NaN;
    end

    ir = find(f > fpk & Ls <= Lhp, 1, 'first');   % right
    if ~isempty(ir) && ir<numel(f)
        f2 = interp1(Ls(ir:ir+1), f(ir:ir+1), Lhp, 'linear','extrap');
    else
        f2 = NaN;
    end

    if ~isnan(f1) && ~isnan(f2) && f2>f1
        fn(k)   = sqrt(f1*f2);
        zeta(k) = (f2 - f1) / (2*max(fn(k),eps));
        f1s(k)=f1; f2s(k)=f2;
    end
end

% --- overlap detection (bandwidth overlap heuristic) ---
[overlap_pairs, bands] = detect_overlaps(fn, zeta, overlapK);

% --- per-mode amplitude A via local complex LS (A real by default) ---
A = nan(size(fn));
for k=1:numel(fn)
    if ~isfinite(fn(k)) || ~isfinite(zeta(k)), continue; end
    if isfinite(f1s(k)) && isfinite(f2s(k)) && f2s(k)>f1s(k)
        flo = f1s(k); fhi = f2s(k);
    else
        flo = fn(k)*(1-0.12); fhi = fn(k)*(1+0.12);    % fallback window
    end
    win = (f>=flo & f<=fhi);
    if nnz(win) < 10
        pad = max(0.05*fn(k), 5*df);
        win = (f>=fn(k)-pad & f<=fn(k)+pad);
    end
    if nnz(win) < 5, continue; end

    fw = f(win); ww = 2*pi*fw;
    omegan = 2*pi*fn(k); z = zeta(k);

    Hj = frf_kernel(ww, omegan, z, frfType);

    bw = max( 2*z*fn(k), max(1.0, 0.04*fn(k)) );
    W  = 1 + 4*exp(-((fw - fn(k)).^2)/(2*(bw/2.355)^2));
    Wsqrt = sqrt(W(:));

    if modelForm == "multiplicative"
        y  = (Hc(win)./Gdc) .* exp(1i*ww*tau) - 1;
    else
        y  = (Hc(win) - Gdc) .* exp(1i*ww*tau);
    end

    u = Hj .* Wsqrt; v = y .* Wsqrt;
    if Areal
        denom = sum(abs(u).^2);
        if denom > eps, A(k) = real(sum(conj(u).*v)) / denom; else, A(k)=0; end
    else
        denom = (u' * u);
        if abs(denom) > eps, A(k) = (u' * v) / denom; else, A(k)=0; end
    end
end

% ======================= BEFORE (seed model) =======================
valid_modes = isfinite(fn) & isfinite(zeta) & isfinite(A);
fn_before   = fn(valid_modes);
zeta_before = zeta(valid_modes);
A_before    = A(valid_modes);

if isempty(fn_before)
    Hhat0 = transfer_from_params(f, zeros(0,1), zeros(0,1), zeros(0,1), ...
        'DCGain', Gdc, 'Delay', tau, 'FRF', frfType, 'Model', modelForm);
else
    Hhat0 = transfer_from_params(f, A_before, fn_before, zeta_before, ...
        'DCGain', Gdc, 'Delay', tau, 'FRF', frfType, 'Model', modelForm);
end
rmse_before = complex_rmse(Hc, Hhat0);
if ~isfinite(rmse_before), rmse_before = complex_rmse(Hc, Gdc*ones(size(f))); end

% ======================= REFINEMENT (with optional ΔG baseline) =======================
[fn, zeta, A, Hhat, dGfit] = refine_mdof_smallpct( ...
    f, Hc, Gdc, tau, fn_before, zeta_before, A_before, ...
    Areal, nitRef, refPct, minRelSep, frfType, modelForm, fitBaseline);

rmse_after = complex_rmse(Hc, Hhat);
if ~isfinite(rmse_after), rmse_after = complex_rmse(Hc, Hc*0 + Gdc); end

% ======================= JITTER SEARCH (wider defaults) =======================
jit = struct();
want_extra = max(0, ndof - numel(fn));
if want_extra>0 && opts.Jitter.Enable && any(isfinite(fn))
    jit = jitter_additional_modes( ...
        f, Hc, Gdc, tau, fn, zeta, Areal, frfType, modelForm, opts.Jitter, minRelSep);

    if ~isempty(jit.candidates)
        take = min(want_extra, height(jit.candidates));
        cand  = jit.candidates(1:take,:);
        fn    = [fn;   cand.fn2];
        zeta  = [zeta; cand.z2];
        A     = [A;    cand.A2];
        [fn, zeta, A, Hhat, dGfit] = refine_mdof_smallpct( ...
            f, Hc, Gdc, tau, fn, zeta, A, ...
            Areal, max(60, round(0.5*nitRef)), refPct, minRelSep, frfType, modelForm, fitBaseline);
        rmse_after = complex_rmse(Hc, Hhat); %#ok<NASGU>
    end
end

% ======================= MONTE-CARLO OVERLAP CHECK (wider split range) =======================
mc = struct();
want_extra = max(0, ndof - numel(fn));
if opts.MC.Enable && want_extra > 0 && any(isfinite(fn))
    mc = mc_overlap_check(f, Hc, Gdc, tau, fn, zeta, want_extra, opts.MC, minRelSep);
    if ~isempty(mc.extra_modes)
        fn_all   = [fn(:);   mc.extra_modes.fn(:)];
        zeta_all = [zeta(:); mc.extra_modes.zeta(:)];
        A_all    = [A(:);    mc.extra_modes.A(:)];
        [fn, zeta, A, Hhat, dGfit] = refine_mdof_smallpct( ...
            f, Hc, Gdc, tau, fn_all, zeta_all, A_all, ...
            Areal, max(60, round(0.5*nitRef)), refPct, minRelSep, frfType, modelForm, fitBaseline);
        rmse_after = complex_rmse(Hc, Hhat); %#ok<NASGU>
    end
end

% ======================= CHANGES TABLE =======================
valid_before = isfinite(fn_before) & isfinite(zeta_before);
fn_before   = fn_before(valid_before);
zeta_before = zeta_before(valid_before);

valid_after = isfinite(fn) & isfinite(zeta);
fn          = fn(valid_after);
zeta        = zeta(valid_after);

n0 = numel(fn_before);
n1 = numel(fn);
n  = max(n0, n1);

fnb = nan(n,1); fna = nan(n,1);
zb0 = nan(n,1); zba = nan(n,1);

if n0>0, fnb(1:n0) = fn_before(:); zb0(1:n0) = zeta_before(:); end
if n1>0, fna(1:n1) = fn(:);         zba(1:n1) = zeta(:);        end

dfn      = fna - fnb;                             % Hz
dzeta    = zba - zb0;
rel_dfn  = 100 * (dfn  ./ max(abs(fnb),  eps));   % %
rel_dzet = 100 * (dzeta./ max(abs(zb0),  eps));   % %

changes_table = table(fnb, fna, dfn, rel_dfn, zb0, zba, dzeta, rel_dzet, ...
    'VariableNames', {'fn_before_Hz','fn_after_Hz','dFn_Hz','dFn_pct', ...
                      'zeta_before','zeta_after','dZeta','dZeta_pct'});

if isfinite(rmse_before) && rmse_before>0 && isfinite(rmse_after)
    improvement_pct = 100 * max(0, (rmse_before - rmse_after) / rmse_before);
else
    improvement_pct = NaN;
end

% ---- mode summary table ----
mode_table = table(fn(:), zeta(:), 100*zeta(:), 1./max(2*zeta(:), eps), ...
    'VariableNames', {'fn_Hz','zeta','damping_pct','Q'});
fprintf('\n=== Mode Estimates (half-power + refinement + jitter) ===\n');
disp(mode_table);

% --- pack info ---
info = struct();
info.peaks_idx       = locs;
info.f1              = f1s; 
info.f2              = f2s;
info.smooth_dB       = Ls;
info.overlap_pairs   = overlap_pairs;
info.suggested_bands = bands;
info.A               = A;
info.A_is_real       = Areal;
info.rmse_before     = rmse_before;
info.rmse_after      = rmse_after;
info.improvement_pct = improvement_pct;
info.changes_table   = changes_table;
info.mode_table      = mode_table;
info.Hfit            = Hhat;
info.seed_diag       = seed_diag;
info.mc_overlap      = mc;
info.jitter          = jit;      % diagnostics
info.FRF             = frfType;
info.Model           = modelForm;
info.DC_initial      = Gdc;
info.DC_correction   = dGfit;
info.DC_effective    = Gdc + dGfit;

% --- optional plots ---
if plotFlag
    figure('Color','w','Name','Half-power markers');
    subplot(2,1,1);
    semilogy(f, Tmag, '.', 'DisplayName','|H| data'); hold on;
    for k=1:numel(fn), if ~isnan(fn(k)), xline(fn(k),'--','HandleVisibility','off'); end, end
    grid on; xlabel('Frequency (Hz)'); ylabel('|H|'); title('Half-power markers');
    subplot(2,1,2);
    plot(f, LdB, ':', 'DisplayName','raw dB'); hold on;
    plot(f, Ls , '-', 'DisplayName','smooth dB');
    for k=1:numel(f1s)
        if ~isnan(f1s(k)) && ~isnan(f2s(k))
            plot([f1s(k) f2s(k)], [interp1(f,Ls,f1s(k)) interp1(f,Ls,f2s(k))], ...
                 'ro-','DisplayName','-3 dB');
        end
    end
    legend('Location','best'); grid on; xlabel('Hz'); ylabel('dB');
end

if doCompare
    compare_real_imag_internal(f, Hc, Hhat);
end
end

% ======================================================================
% FRF kernels
% ======================================================================
function Hk = frf_kernel(w, wn, zeta, frfType)
den = (wn.^2 - w.^2) + 1i*(2*zeta*wn).*w;   % dynamic stiffness (m=1 absorbed)
switch lower(char(frfType))
    case 'disp/force'   % receptance
        Hk = 1 ./ den;
    case 'vel/force'    % mobility
        Hk = (1i*w) ./ den;
    case 'acc/force'    % accelerance
        Hk = (-w.^2) ./ den;
    otherwise           % legacy transmissibility kernel
        Hk = (w.^2) ./ den;
end
end

% ======================================================================
% Complex transfer from (A, fn, zeta)
% ======================================================================
function Hhat = transfer_from_params(f_or_THM, A, fn, zeta, varargin)
p = inputParser;
p.addParameter('DCGain',1+0i);
p.addParameter('Delay',0);
p.addParameter('FRF','');           % '', 'acc/force', 'vel/force', 'disp/force'
p.addParameter('Model','auto');     % 'auto' | 'additive' | 'multiplicative'
p.parse(varargin{:});
G=p.Results.DCGain; tau=p.Results.Delay;
frfType = lower(string(p.Results.FRF));
modelForm = string(p.Results.Model);
if modelForm == "auto"
    if frfType == ""; modelForm = "multiplicative"; else; modelForm = "additive"; end
end

if isvector(f_or_THM) && size(f_or_THM,2)==1, f = double(f_or_THM(:));
else, f = double(f_or_THM(:,1)); end
A=A(:); fn=fn(:); zeta=zeta(:);
w=2*pi*f;

Hcols = zeros(numel(w), numel(fn));
for k = 1:numel(fn)
    wn = 2*pi*fn(k); zet = zeta(k);
    Hcols(:,k) = frf_kernel(w, wn, zet, frfType);
end

if isempty(A), Hbody = zeros(size(f)); else, Hbody = Hcols*A; end
if modelForm == "multiplicative", Hhat= G * (1 + Hbody);
else, Hhat= G + Hbody; end
if tau~=0, Hhat = exp(-1i*w*tau).*Hhat; end
end

% ======================================================================
% Refinement: small-% jitter on (fn, zeta), LS on A (and optional ΔG) each step
% ======================================================================
function [fnB, zB, AB, Hbest, dGbest] = refine_mdof_smallpct( ...
    f, Hc, Gdc, tau, fn0, z0, A0, Areal, nit, pct, minRelSep, frfType, modelForm, fitBaseline)

w = 2*pi*f;
if modelForm == "multiplicative"
    Htilde = (Hc./Gdc) .* exp(1i*w*tau);
else
    Htilde = (Hc - Gdc) .* exp(1i*w*tau);
end

amag = abs(Htilde);
W = 1 + (amag./max(amag)).^2;
for k=1:numel(fn0)
    bw = max( 2*z0(k)*fn0(k), max(1.0, 0.04*fn0(k)) );
    W = W + 3*exp(-((f - fn0(k)).^2)/(2*(bw/2.355)^2));
end
W = W / mean(W); Wsqrt = sqrt(W(:));

    function [err, A, dG, HhatEff] = obj(fn,z)
        wn  = 2*pi*fn(:).';  zet = z(:).';
        Hcols = zeros(numel(w), numel(wn));
        for kk = 1:numel(wn), Hcols(:,kk) = frf_kernel(w, wn(kk), zet(kk), frfType); end
        if modelForm == "multiplicative", y  = Htilde - 1; else, y  = Htilde; end

        Xw = Hcols .* Wsqrt;  yw = y .* Wsqrt;
        colN = sqrt(sum(abs(Xw).^2,1)); colN(colN==0)=1;
        Xn = Xw ./ colN;

        dG = 0;
        if modelForm=="additive" && fitBaseline
            Bw = Wsqrt;  normB = sqrt(sum(abs(Bw).^2)); if normB==0, normB=1; end
            Bn = Bw / normB;
        else, Bn=[]; normB=1;
        end

        if Areal
            if isempty(Bn)
                X2 = [real(Xn); imag(Xn)];  y2 = [real(yw); imag(yw)];
            else
                N = numel(w);
                X2 = [real(Xn), Bn, zeros(N,1); ...
                      imag(Xn), zeros(N,1), Bn];
                y2 = [real(yw); imag(yw)];
            end
            if exist('lsqminnorm','file')==2, coef = lsqminnorm(X2, y2);
            else, coef = pinv(X2,1e-8)*y2; end
            m = size(Xn,2);
            A = real(coef(1:m)) ./ colN.';
            if ~isempty(Bn), dGr = coef(m+1); dGi = coef(m+2); dG  = (dGr + 1i*dGi) / normB; end
        else
            if isempty(Bn), Xc=Xn; yc=yw; else, Xc=[Xn, Bn]; yc=yw; end
            if exist('lsqminnorm','file')==2, coef = lsqminnorm(Xc, yc);
            else, coef = pinv(Xc,1e-8)*yc; end
            m=size(Xn,2); A = (coef(1:m) ./ colN.'); if ~isempty(Bn), dG = coef(m+1)/normB; end
        end

        if modelForm == "multiplicative", HhatEff = 1 + Hcols*A;
        else, HhatEff = isempty(Bn) * (Hcols*A) + ~isempty(Bn) * (Hcols*A + dG);
        end

        r = yw - HhatEff .* Wsqrt;
        err = sqrt( mean(abs(r).^2) );
    end

fnB = fn0(:);  zB = z0(:);  AB = A0(:);
[bestErr, AB, dGbest, HbestEff] = obj(fnB, zB);
if modelForm == "multiplicative"
    Hbest = Gdc * HbestEff .* exp(-1i*w*tau);
else
    if fitBaseline, Hbest = (Gdc + dGbest + (HbestEff - dGbest)) .* exp(-1i*w*tau);
    else,            Hbest = (Gdc + HbestEff) .* exp(-1i*w*tau);
    end
end

pf = pct(1); pz = pct(2);
for it = 1:nit
    t = it / max(1,nit);
    sfn   = (0.8 + 0.2*(1-t)) * pf/100;
    szeta = (0.8 + 0.2*(1-t)) * pz;

    fn_try = fnB .* (1 + sfn*randn(size(fnB)));
    z_try  = zB  .* (1 + szeta*randn(size(zB)));
    z_try = min(max(z_try, 1e-4), 0.30);

    [fn_try, ord] = sort(fn_try(:));  z_try = z_try(ord);
    for k = 2:numel(fn_try)
        if fn_try(k) <= fn_try(k-1)*(1+minRelSep)
            fn_try(k) = fn_try(k-1)*(1+minRelSep);
        end
    end

    [err, Atry, dGtry, HtryEff] = obj(fn_try, z_try);
    if err < bestErr
        bestErr = err;
        fnB = fn_try; zB = z_try; AB = Atry; dGbest = dGtry;
        if modelForm == "multiplicative"
            Hbest = Gdc * HtryEff .* exp(-1i*w*tau);
        else
            if fitBaseline, Hbest = (Gdc + dGbest + (HtryEff - dGbest)) .* exp(-1i*w*tau);
            else,            Hbest = (Gdc + HtryEff) .* exp(-1i*w*tau);
            end
        end
    end
end
end

% ======================================================================
% Jittering search around each found mode (defaults widened)
%  (minor change: also jitters damping zeta per trial)
% ======================================================================
function jit = jitter_additional_modes(f, Hc, Gdc, tau, fn, zeta, Areal, frfType, modelForm, J, minRelSep)

w = 2*pi*f;
if modelForm == "multiplicative"
    Htilde = (Hc./Gdc) .* exp(1i*w*tau);  target = Htilde - 1;
else
    Htilde = (Hc - Gdc) .* exp(1i*w*tau); target = Htilde;
end

cands = []; % collect candidate structs

for k = 1:numel(fn)
    fk = fn(k); zk = zeta(k);
    if ~isfinite(fk) || ~isfinite(zk) || fk<=0 || zk<=0, continue; end

    BW  = max( 2*zk*fk, max(1.0, 0.04*fk) );
    pad = J.WindowBW * BW;
    win = (f >= max(f(2), fk - pad)) & (f <= min(f(end-1), fk + pad));
    if nnz(win) < 12, continue; end

    fw = f(win); ww = 2*pi*fw;
    H1 = frf_kernel(ww, 2*pi*fk, zk, frfType);

    % weights centered at fk
    W  = 1 + 3*exp(-((fw - fk).^2)/(2*(BW/2.355)^2));
    yw = target(win) .* sqrt(W(:));

    % best one-mode (re-estimate A1)
    X1 = H1 .* sqrt(W(:));
    if Areal
        a1 = real( (real(X1)'*real(yw) + imag(X1)'*imag(yw)) / ...
                   (real(X1)'*real(X1) + imag(X1)'*imag(X1) + eps) );
    else
        a1 = (X1' * yw) / (X1' * X1 + eps);
    end
    r1 = yw - X1*a1; err1 = sqrt(mean(abs(r1).^2));

    best = struct('fn2',NaN,'z2',NaN,'A2',NaN,'improve',-Inf);

    trials = max(50, J.Ntrials);
    for t = 1:trials
        % ---- jitter proposals ----
        dfPct = (2*rand()-1) * J.FnPct;     % ±FnPct
        fn2 = fk * (1 + dfPct);

        % jitter damping as well
        if isfield(J,'ZetaPct') && J.ZetaPct > 0
            z2  = min(max(zk * (1 + J.ZetaPct*randn()), 1e-4), 0.30);
        else
            z2  = min(max(zk * (J.ZetaScale(1) + (J.ZetaScale(2)-J.ZetaScale(1))*rand()), 1e-4), 0.30);
        end

        % separation constraints
        if any(abs(fn2 - fn) < max(minRelSep, J.MinSepPct).*fn2), continue; end

        % two-mode fit (parent + candidate)
        H2 = frf_kernel(ww, 2*pi*fn2, z2, frfType);
        X2 = [H1, H2] .* sqrt(W(:));

        if Areal
            Xr = [real(X2); imag(X2)]; yr = [real(yw); imag(yw)];
            coef = pinv(Xr,1e-8)*yr; Ahat = real(coef(:));
        else
            Ahat = pinv(X2,1e-8)*yw;
        end
        r2 = yw - X2*Ahat; err2 = sqrt(mean(abs(r2).^2));
        improve = max(0, (err1 - err2)/max(err1,eps));

        if improve > best.improve
            best.fn2 = fn2; best.z2 = z2; best.A2 = Ahat(2); best.improve = improve;
        end
    end

    if best.improve >= J.ImproveThresh
        cands = [cands; best]; %#ok<AGROW>
        if J.MaxAddsPerMode<=1, continue; end
    end
end

% sort candidates by improvement desc and enforce separation among themselves
if isempty(cands)
    jit.candidates = [];
    jit.notes = 'no jitter candidates passed threshold';
    jit.params = J;
    return;
end
[~,ord] = sort([cands.improve],'descend'); cands = cands(ord);

kept = [];
added_fn = [];
for i=1:numel(cands)
    fi = cands(i).fn2;
    if any(abs(fi - [fn; added_fn(:)]) < max(minRelSep, J.MinSepPct).*fi)
        continue;
    end
    kept(end+1) = i; %#ok<AGROW>
    added_fn(end+1) = fi; %#ok<AGROW>
end
cands = cands(kept);

% return as table
jit.candidates = struct2table(cands);
jit.params = J;
end

% ======================================================================
% Measured vs model plots (Re/Im)
% ======================================================================
function compare_real_imag_internal(f, Hmeas, Hhat)
figure('Color','w','Name','Measured vs Model: Real/Imag (refined)');
subplot(2,1,1);
semilogx(f, real(Hmeas), f, real(Hhat), 'LineWidth',1.2);
grid on; xlabel('Frequency (Hz)'); ylabel('Real');
legend('Measured Re{H}','Model Re{H}','Location','best');
subplot(2,1,2);
semilogx(f, imag(Hmeas), f, imag(Hhat), 'LineWidth',1.2);
grid on; xlabel('Frequency (Hz)'); ylabel('Imag');
legend('Measured Im{H}','Model Im{H}','Location','best');
end

% ======================================================================
% DC-aware robust seeding
% ======================================================================
function [locs, diaginfo] = robust_peak_locs_dcaware(f, Ls, ndof, prom, minDist, bw_Hz, Hc, useDC, DCFrac, usePhaseLogF)
cands = [];
tags  = [];
diag  = {};

% 1) strict findpeaks on Ls
try
    if exist('findpeaks','file')==2
        [~, L1] = findpeaks(Ls, 'MinPeakDistance',minDist, ...
                                'MinPeakWidth',   max(round(bw_Hz/max(median(diff(f)),eps)),3), ...
                                'MinPeakProminence', prom, ...
                                'SortStr','descend');
    else
        L1 = simple_findpeaks(Ls);
    end
catch, L1 = []; end
if ~isempty(L1), cands = [cands; L1(:)]; tags = [tags; repmat("pks-strict",numel(L1),1)]; diag{end+1}='pks-strict'; end

% 2) relaxed findpeaks
try
    prom2    = max( 2*1.4826*mad(Ls - movmean(Ls, max(11,round(numel(f)/50))),1), 0.05 );
    minDist2 = max(2, round(0.25*minDist));
    [~, L2] = findpeaks(Ls, 'MinPeakDistance',minDist2, 'MinPeakProminence', prom2);
catch, L2 = []; end
if ~isempty(L2), cands = [cands; L2(:)]; tags = [tags; repmat("pks-relaxed",numel(L2),1)]; diag{end+1}='pks-relaxed'; end

% 3) local maxima
m2 = Ls(2:end-1);
L3 = find((m2 > Ls(1:end-2)) & (m2 >= Ls(3:end))) + 1;
if ~isempty(L3), cands = [cands; L3(:)]; tags = [tags; repmat("localmax",numel(L3),1)]; diag{end+1}='localmax'; end

% 4) phase gradient on log-frequency (helps low-f)
if usePhaseLogF
    phi = unwrap(angle(Hc(:)));
    dphi_dlnf = abs( gradient(phi, log(f)) );  % f * dphi/df
    [~, L4] = maxk(dphi_dlnf, min(max(3,2*ndof), max(1,floor(numel(f)/40))));
    if ~isempty(L4), cands=[cands; L4(:)]; tags=[tags; repmat("phase-logf",numel(L4),1)]; diag{end+1}='phase-logf'; end
else
    L4 = [];
end

% 5) DC deviation |H - Hdc|
if useDC
    nDC   = max(3, round(DCFrac*numel(f)));
    Hdc   = median(Hc(1:nDC));
    dev   = abs(Hc - Hdc);
    devs  = movmedian(dev, max(3, round(numel(f)/300)));
    if exist('sgolayfilt','file')==2 && numel(f)>=11, devs = sgolayfilt(devs,3,11); end
    [~, L5] = findpeaks(devs, 'MinPeakDistance', max(3, round(0.15*minDist)));
    if ~isempty(L5), cands=[cands; L5(:)]; tags=[tags; repmat("dc-dev",numel(L5),1)]; diag{end+1}='dc-dev'; end
end

if isempty(cands)
    locs = round(linspace( max(2,round(0.05*numel(f))), max(3,round(0.95*numel(f))), ndof ));
    locs = unique(locs);
    diaginfo = struct('stage','forced-grid','notes',diag);
    return;
end

% --- merge & score ---
cands = unique(cands);
score = Ls(cands);
if exist('L4','var') && ~isempty(L4), score = score + ismember(cands,L4)*2; end
if useDC && exist('L5','var') && ~isempty(L5), score = score + ismember(cands,L5)*3; end
[~,I] = sort(score,'descend');
cands = cands(I);

% prune near-duplicates
keep = false(size(cands));
minBins = max(3, round(0.5*bw_Hz/max(median(diff(f)),eps)));
last = -Inf;
for i=1:numel(cands)
    if cands(i) - last >= minBins
        keep(i) = true; last = cands(i);
    end
end
locs = cands(keep);
locs = locs(1:min(ndof, numel(locs)));

diaginfo = struct('stage','multi','notes',diag,'num_candidates',numel(cands));
end

% ======================================================================
% Monte-Carlo overlap probe
% ======================================================================
function mc = mc_overlap_check(f, Hc, Gdc, tau, fn, zeta, want_extra, mcopts, minRelSep)
w = 2*pi*f;
Htilde_mult = (Hc./Gdc).*exp(1i*w*tau);
mc = struct('extra_modes',[],'parent_map',[],'stats',[]);
PctWithin = mcopts.PctWithin; Ntrials=mcopts.Ntrials;
NoiseFrac=mcopts.NoiseFrac;  ImpThr=mcopts.ImproveThresh;

per_mode = struct('detect_rate',[],'median_improve',[],'best_split',[]);
extra_fn=[]; extra_zeta=[]; extra_A=[]; parent_idx=[];

for k = 1:numel(fn)
    if ~isfinite(fn(k)) || ~isfinite(zeta(k)), continue; end
    fk = fn(k); zk=zeta(k);

    win_pad = max( 2*zk*fk, max(1.0, 0.05*fk) );
    win = (f>=fk*(1-0.12) & f<=fk*(1+0.12));
    if nnz(win) < 12, win = (f>=fk-win_pad & f<=fk+win_pad); end
    if nnz(win) < 8,  continue; end

    fw = f(win); ww = 2*pi*fw;
    Hj1 = frf_kernel(ww, 2*pi*fk, zk, ""); % probe with legacy-like kernel

    impr = zeros(Ntrials,1);
    bestLocal = struct('f2',[NaN NaN],'z2',[NaN NaN],'A2',[NaN NaN],'improve',-Inf);
    detect_cnt = 0;

    for t = 1:Ntrials
        epsC = (randn(sum(win),1)+1i*randn(sum(win),1))*(NoiseFrac*median(abs(Htilde_mult(win))+eps));
        y = (Htilde_mult(win) + epsC) - 1;

        rel = 0.5*PctWithin * (0.5 + rand());  % random within ±PctWithin
        fk1 = fk*(1 - rel); fk2 = fk*(1 + rel);
        fk1 = max(f(2), fk1); fk2 = min(f(end-1), fk2);
        if fk2 <= fk1*(1+max(minRelSep,1e-4)), fk2 = fk1*(1+max(minRelSep,1e-4))*1.001; end

        z1 = min(max(zk*(1+0.15*randn), 1e-4), 0.30);
        z2 = min(max(zk*(1+0.15*randn), 1e-4), 0.30);

        Hj2a = frf_kernel(ww, 2*pi*fk1, z1, "");
        Hj2b = frf_kernel(ww, 2*pi*fk2, z2, "");

        W = 1 + 3*exp(-((fw - fk).^2)/(2*(win_pad/2.355)^2)); Wsqrt = sqrt(W(:));

        X1 = Hj1 .* Wsqrt; yW = y.*Wsqrt;
        if rank([real(X1),imag(X1)])>=1
            A1 = real( (real(X1)'*real(yW) + imag(X1)'*imag(yW)) / ...
                       (real(X1)'*real(X1)+imag(X1)'*imag(X1)+eps) );
        else, A1 = 0; end
        r1 = yW - X1*A1;  err1 = sqrt(mean(abs(r1).^2));

        X2 = [Hj2a Hj2b] .* Wsqrt;
        colN = sqrt(sum(abs(X2).^2,1)); colN(colN==0)=1; X2n = X2./colN; yW = y.*Wsqrt;
        if rank([real(X2n); imag(X2n)])>=2
            Ahat = pinv([real(X2n); imag(X2n)],1e-8)*[real(yW); imag(yW)];
            A2 = real(Ahat(:)./colN.');
        else
            A2 = [0;0];
        end
        r2 = yW - X2*(A2); err2 = sqrt(mean(abs(r2).^2));

        impr(t) = max(0, (err1 - err2)/max(err1,eps));
        if impr(t) > ImpThr, detect_cnt = detect_cnt + 1; end
        if impr(t) > bestLocal.improve
            bestLocal = struct('f2',[fk1 fk2],'z2',[z1 z2],'A2',A2(:).', 'improve',impr(t));
        end
    end

    per_mode(k).detect_rate   = detect_cnt / Ntrials;
    per_mode(k).median_improve= median(impr);
    per_mode(k).best_split    = bestLocal;

    if per_mode(k).median_improve >= ImpThr && per_mode(k).detect_rate >= 0.25
        [~, idxExtra] = min(abs(bestLocal.f2 - fk));
        f_extra  = bestLocal.f2(idxExtra);
        z_extra  = bestLocal.z2(idxExtra);
        A_extra  = bestLocal.A2(idxExtra);
        if abs(f_extra - fk) <= PctWithin*fk
            extra_fn   = [extra_fn;   f_extra];
            extra_zeta = [extra_zeta; z_extra];
            extra_A    = [extra_A;    A_extra];
            parent_idx = [parent_idx; k];
        end
    end
end

if ~isempty(extra_fn)
    keep = 1:numel(extra_fn);
    if numel(extra_fn) > want_extra
        scores = zeros(numel(extra_fn),1);
        for i=1:numel(extra_fn), scores(i) = per_mode(parent_idx(i)).median_improve; end
        [~,J] = maxk(scores, want_extra);
        keep = sort(J);
    end
    mc.extra_modes = table(extra_fn(:), extra_zeta(:), extra_A(:), parent_idx(:), ...
        'VariableNames', {'fn','zeta','A','parent_idx'});
    mc.parent_map  = parent_idx(:);
else
    mc.extra_modes = [];
    mc.parent_map  = [];
end
mc.stats = per_mode;
end

% ======================================================================
% Misc helpers
% ======================================================================
function rm = complex_rmse(Hc, Hhat)
r = Hc - Hhat; rm = sqrt(mean(abs(r).^2));
end

function [overlap_pairs, bands] = detect_overlaps(fn, zeta, K)
if nargin < 3 || isempty(K), K = 0.90; end
fn = fn(:); zeta = zeta(:);
good = isfinite(fn) & isfinite(zeta) & fn > 0 & zeta > 0;
fn = fn(good); zeta = zeta(good);
[fn, ord] = sort(fn); zeta = zeta(ord);
overlap_pairs = []; bands = zeros(0,3);
if numel(fn) < 2, return; end
BW = 2 .* zeta .* fn;  % half-power bandwidths
for i = 1:numel(fn)-1
    d  = fn(i+1) - fn(i);
    th = K * 0.5 * (BW(i) + BW(i+1));
    if d < th
        overlap_pairs(end+1,:) = [i, i+1]; %#ok<AGROW>
        mid = 0.5*(fn(i) + fn(i+1)); pad = 0.5*(BW(i) + BW(i+1));
        flo = max(0, mid - pad);  fhi = mid + pad;
        bands(end+1,:) = [flo, fhi, 2]; %#ok<AGROW>
    end
end
end

function [f, Hc] = thm_to_complex(THM)
if size(THM,2) < 2, error('THM must be [f, H] or [f, Re, Im].'); end
f = double(THM(:,1));
if size(THM,2) >= 3 && isreal(THM(1,2)) && isreal(THM(1,3))
    Hc = complex(double(THM(:,2)), double(THM(:,3)));
else
    Hc = THM(:,2);
    if ~isa(Hc,'double'), Hc = double(Hc); end
    if isreal(Hc) && all(isfinite(Hc)) && all(Hc >= 0), Hc = complex(Hc, 0); end
end
f = f(:); Hc = Hc(:);
end

function [pk,locs] = simple_findpeaks(y)
pk = []; locs = [];
if numel(y)<3, return; end
m2 = y(2:end-1);
mask = (m2 > y(1:end-2)) & (m2 >= y(3:end));
locs = find(mask) + 1;
pk   = y(locs);
end

function opts = parse_opts(defaults, varargin)
opts = defaults;
if isempty(varargin), return; end
if numel(varargin)==1 && isstruct(varargin{1})
    user = varargin{1}; fns = fieldnames(user);
    for i=1:numel(fns)
        opts.(fns{i}) = user.(fns{i});
    end
    return
end
if mod(numel(varargin),2)~=0, error('Name-value arguments must come in pairs.'); end
for k=1:2:numel(varargin)
    name = varargin{k}; val = varargin{k+1};
    if ~(ischar(name) || isstring(name)), error('Option names must be strings.'); end
    % special-case partial struct overrides for Jitter/MC: shallow-merge
    if strcmpi(char(name),'Jitter') && isstruct(val)
        base = defaults.Jitter;
        fns = fieldnames(val);
        for ii=1:numel(fns), base.(fns{ii}) = val.(fns{ii}); end
        opts.Jitter = base;
    elseif strcmpi(char(name),'MC') && isstruct(val)
        base = defaults.MC;
        fns = fieldnames(val);
        for ii=1:numel(fns), base.(fns{ii}) = val.(fns{ii}); end
        opts.MC = base;
    else
        opts.(char(name)) = val;
    end
end
end
