skills-win/simulink-rtl-redev/measure_k.m

function [K, R] = measure_k(ref, dut, varargin)
%MEASURE_K  Measure the integer delay K that <dut> lags <ref>, by TWO INDEPENDENT
% methods that must agree (Task16-3 section 5: "延迟必须实测,不能按设计意图推算.
% 用独立微台、至少两种互不依赖的方法测出一致值").
%
%   K = measure_k(ref, dut)
%   [K, R] = measure_k(ref, dut, 'Name', value, ...)
%
% ref, dut : real or COMPLEX vectors, same sample rate.  dut is the signal that
%            comes out K samples later than ref.
%
% Options
%   'MaxLag'   (default 4096)   search window, in samples
%   'OnsetTol' (default 1e-3)   onset threshold, RELATIVE to each signal's own
%                               RMS -- so the two signals may differ in gain
%   'FracTol'  (default 0.05)   how far the interpolated correlation peak may sit
%                               from an integer before K is declared non-integer
%   'Verbose'  (default true)
%
% Returns
%   K : the agreed integer delay, or NaN if the methods disagree
%   R : struct with per-method results and the verdict --
%       .k_corr      argmax of the normalised cross-correlation
%       .k_onset     difference of first-significant-sample indices
%       .k_frac      parabolic-interpolated peak (fractional)
%       .frac_resid  |k_frac - round(k_frac)|  -- large => not a pure integer shift
%       .rho         |correlation| at the peak, 0..1
%       .agree       k_corr == k_onset
%       .pass        agree && frac_resid <= FracTol
%
% WHY TWO METHODS: a correlation argmax alone will happily return a number for
% two unrelated signals.  Onset detection is a structurally different estimator
% (amplitude threshold, not inner product), so agreement between them is real
% evidence.  The fractional peak is the third check: it catches the case where
% the change did NOT introduce a clean integer pipeline delay (e.g. it altered a
% fractional interpolator), which an integer-only estimator would silently round.
%
% Base MATLAB only -- no Signal Processing Toolbox (no xcorr/finddelay).

p = inputParser;
p.addParameter('MaxLag',   4096, @(x) isnumeric(x) && isscalar(x) && x > 0);
p.addParameter('OnsetTol', 1e-3, @(x) isnumeric(x) && isscalar(x) && x > 0);
p.addParameter('FracTol',  0.05, @(x) isnumeric(x) && isscalar(x) && x > 0);
p.addParameter('Verbose',  true, @(x) islogical(x) || isnumeric(x));
p.parse(varargin{:});
o = p.Results;

ref = ref(:);  dut = dut(:);
n = min(numel(ref), numel(dut));
if n < 16
    error('measure_k: need at least 16 samples, got %d', n);
end
ref = ref(1:n);  dut = dut(1:n);

maxlag = min(o.MaxLag, n - 1);

% ---- method 1: normalised cross-correlation, argmax over 0..maxlag -----------
% FFT-based so it stays base MATLAB and is fast for long vectors.
nfft = 2^nextpow2(2*n);
Rxy  = ifft(fft(dut, nfft) .* conj(fft(ref, nfft)));   % dut delayed by k => peak at k
c    = abs(Rxy(1:maxlag+1));
den  = norm(ref) * norm(dut);
if den == 0
    error('measure_k: one of the signals is all zeros');
end
c = c / den;
[rho, idx] = max(c);
k_corr = idx - 1;

% ---- method 3 (on the same curve): parabolic interpolation of the peak -------
% Done here because it needs c; it is reported separately and is NOT one of the
% "two independent methods" -- it only tests integer-ness.
if idx > 1 && idx < numel(c)
    y0 = c(idx-1); y1 = c(idx); y2 = c(idx+1);
    d  = (y0 - y2) / (2*(y0 - 2*y1 + y2));
    if ~isfinite(d), d = 0; end
    k_frac = k_corr + d;
else
    k_frac = k_corr;
end
frac_resid = abs(k_frac - round(k_frac));

% ---- method 2: cross-spectral phase slope (FREQUENCY domain, always available)
% A pure delay of K samples makes angle(Y.*conj(X)) a straight line of slope -K
% against normalised frequency.  Fitting that slope uses no argmax at all, so it
% is a genuinely different estimator from method 1 -- and it is fractional, so it
% doubles as an integer-ness check that cannot be fooled by rounding.
k_phase = local_phase_slope(ref, dut, nfft);

% ---- method 3: onset of the first significant sample (only if one exists) ----
% Threshold is a fraction of each signal's own PEAK envelope, not its RMS: a
% noise/quantisation floor in the DUT during the reference's silent lead-in must
% not trigger it.  Returns NaN for continuously excited signals -- that is not a
% failure, it just means this method does not apply.
i_ref = local_onset(ref, o.OnsetTol);
i_dut = local_onset(dut, o.OnsetTol);
if isnan(i_ref) || isnan(i_dut)
    k_onset = NaN;
else
    k_onset = i_dut - i_ref;
end

% ---- adjudication -----------------------------------------------------------
% Require methods 1 and 2 (always available, different domains) to agree, and
% method 3 to agree as well WHEN it applies.
agree_phase = isfinite(k_phase) && abs(k_phase - k_corr) <= o.FracTol;
agree_onset = isnan(k_onset) || (k_onset == k_corr);   % NaN = not applicable
n_methods   = 2 + double(~isnan(k_onset));
agree = agree_phase && agree_onset;
pass  = agree && (frac_resid <= o.FracTol);
if pass, K = k_corr; else, K = NaN; end

R = struct('k_corr', k_corr, 'k_phase', k_phase, 'k_onset', k_onset, ...
           'k_frac', k_frac, 'frac_resid', frac_resid, 'rho', rho, ...
           'agree_phase', agree_phase, 'agree_onset', agree_onset, ...
           'n_methods', n_methods, 'agree', agree, 'pass', pass, ...
           'n', n, 'maxlag', maxlag);

if o.Verbose
    fprintf('measure_k: n=%d maxlag=%d\n', n, maxlag);
    fprintf('  method 1  xcorr argmax   (time domain)  K = %d      |rho| = %.6f\n', k_corr, rho);
    if isfinite(k_phase)
        fprintf('  method 2  phase slope    (freq domain)  K = %.3f\n', k_phase);
    else
        fprintf('  method 2  phase slope    (freq domain)  K = n/a   (not enough usable bins)\n');
    end
    if isnan(k_onset)
        fprintf('  method 3  onset diff     (amplitude)    K = n/a   (no clear onset -- continuous signal)\n');
    else
        fprintf('  method 3  onset diff     (amplitude)    K = %d\n', k_onset);
    end
    fprintf('  check     interpolated peak              %.3f   (residual %.4f, tol %.4f)\n', ...
            k_frac, frac_resid, o.FracTol);
    if pass
        fprintf('  VERDICT: PASS -- %d independent methods agree, K = %d\n', n_methods, K);
    else
        fprintf('  VERDICT: FAIL -- K NOT established\n');
        if ~agree_phase
            fprintf('           methods 1 and 2 disagree (%d vs %.3f).  Different domains giving\n', ...
                    k_corr, k_phase);
            fprintf('           different answers means these are not a pure delayed copy of each\n');
            fprintf('           other -- check you compared the right taps.\n');
        end
        if ~agree_onset
            fprintf('           method 3 disagrees (%d vs %d).  A noise/quantisation floor in the\n', ...
                    k_corr, k_onset);
            fprintf('           DUT during the reference lead-in can do this: raise OnsetTol.\n');
        end
        if frac_resid > o.FracTol
            fprintf('           peak sits %.3f off an integer: the change did not introduce a\n', frac_resid);
            fprintf('           clean integer pipeline delay (fractional/resampling path?).\n');
        end
        fprintf('           Do NOT fall back to the design-intent value (Task16-3 section 5).\n');
    end
end
end


function kp = local_phase_slope(ref, dut, nfft)
%LOCAL_PHASE_SLOPE  Delay from the slope of the cross-spectrum phase.
% Only bins where the reference has real energy are used, and the phase is
% unwrapped over those bins before a weighted least-squares line fit.
X = fft(ref, nfft);  Y = fft(dut, nfft);
half = floor(nfft/2);
X = X(2:half);  Y = Y(2:half);                 % drop DC and Nyquist
w  = abs(X);
if isempty(w) || max(w) == 0, kp = NaN; return, end
keep = w > 0.10 * max(w);                      % usable bins only
if nnz(keep) < 16, kp = NaN; return, end
k    = (1:numel(X)).';
ph   = unwrap(angle(Y(keep) .* conj(X(keep))));
kk   = k(keep);
ww   = w(keep);
% weighted least squares: ph ~ a + b*(2*pi*kk/nfft),  delay = -b
x  = 2*pi*kk/nfft;
sw = sum(ww);
mx = sum(ww.*x)/sw;  my = sum(ww.*ph)/sw;
b  = sum(ww.*(x-mx).*(ph-my)) / sum(ww.*(x-mx).^2);
kp = -b;
end


function i0 = local_onset(x, tol)
%LOCAL_ONSET  First index where the SMOOTHED envelope exceeds tol * its own PEAK.
% Threshold is relative to the peak, not the RMS, so a noise or quantisation
% floor well below the signal does not trigger it.  Returns NaN when the signal
% is already at level from the first sample (continuously excited) -- that means
% "this method does not apply", not "failure".
a = abs(x);
w = 8;                                     % short moving average
e = filter(ones(w,1)/w, 1, a);
pk = max(e);
if pk <= 0, i0 = NaN; return, end
thr = tol * pk;
i0 = find(e > thr, 1, 'first');
if isempty(i0), i0 = NaN; return, end
% already hot at the very start => no usable onset
if i0 <= w, i0 = NaN; end
end