% ****************************************************************
% find sub-sample delay and scaling factor between two cyclic signals 
% to maximize crosscorrelation
% Markus Nentwig, 120609_v1.1
% ****************************************************************
function iterDelayEstDemo();
    close all;
    n = 1024;    
    % n = 1024 * 256; disp('*** test: long signal enabled ***');

    % ****************************************************************
    % random signal
    % ****************************************************************
    fd = randn(1, n) + 1i * randn(1, n);
    
    % ****************************************************************
    % lowpass filter
    % ****************************************************************
    f = binFreq(n);
    fd(abs(f) > 0.045) = 0;
    s1 = real(ifft(fd)) * sqrt(n);
    
    % ****************************************************************
    % create delayed 2nd signal
    % ****************************************************************
    dTest_samples = 12.3456;
    cTest = 1.23456;
    % cTest = cTest + 1i; disp('*** test: complex coeff enabled ***'); 
    % cTest = -cTest; disp('*** test: negative coeff enabled ***'); 

    s2 = cTest * cs_delay(s1, 1, dTest_samples);
    % s2 = s2 + 0.5*randn(size(s2)); disp('*** test: noise enabled ***');
    
    % ****************************************************************
    % estimate delay
    % ****************************************************************
    [delay_samples, coeff] = iterDelayEst(s1, s2);

    % ****************************************************************
    % correct it
    % ****************************************************************
    s2a = cs_delay(s1, 1, delay_samples);
    s2b = s2a * coeff;

    figure(); hold on;
    h = plot(real(s1), 'k'); set(h, 'lineWidth', 3);
    h = plot(real(s2), 'b'); set(h, 'lineWidth', 3);
    h = plot(real(s2a), 'r'); set(h, 'lineWidth', 1);
    h = plot(real(s2b), 'm'); set(h, 'lineWidth', 1);
    xlim([1, numel(s1)]);
    xlabel('samples');
    legend('s1', 's2', 's2 un-delayed', 's2 un-delayed and scaled');
    title('test signals');
    
    format long;
    disp('nominal delay of s2 relative to s1')';
    dTest_samples
    disp('iterDelayEst() returned:');
    delay_samples
    disp('original scaling factor:');
    cTest
    disp('estimated scaling factor:');
    coeff
end

% ****************************************************************
% estimates delay and scaling factor 
% ****************************************************************
function [delay_samples, coeff] = iterDelayEst(s1, s2)
    
    s1 = s1(:) .'; % force row vectors
    s2 = s2(:) .';
    rflag = isreal(s1) && isreal(s2);
    n = numel(s1);
    halfN = floor(n/2);
    assert(numel(s2) == n, 'signals must have same length');

    % ****************************************************************
    % constants
    % ****************************************************************    
    % exit if uncertainty below threshold
    thr_samples = 1e-7;

    % exit after fixed number of iterations
    nIter = 25;

    % frequency domain representation of signals
    fd1 = fft(s1);
    fd2 = fft(s2);    

    % first round: No delay was applied
    tau = [];
    fd2Tau = fd2; % delayed s2 in freq. domain
    
    % frequency corresponding to each FFT bin -0.5..0.5
    f = binFreq(n);

    % uncertainty plot data
    e = [];

    % normalization factor
    nf = sqrt((fd1 * fd1') * (fd2 * fd2')) / n; % normalizes to 1
    
    % search window: 
    % known maximum and two surrounding points
    x1 = -1;
    x2 = -1;
    x3 = -1;
    y1 = -1;
    y2 = -1;
    y3 = -1;
    
    % ****************************************************************
    % iteration loop
    % ****************************************************************
    for count = 1:nIter
    
        % ****************************************************************
        % crosscorrelation with time-shifted signal
        % ****************************************************************
        xcorr = abs(ifft(fd2Tau .* conj(fd1)))/ nf;

        % ****************************************************************
        % detect peak
        % ****************************************************************
        if isempty(tau)

            % ****************************************************************
            % startup
            % initialize with three adjacent bins around peak
            % ****************************************************************
            ix = find(xcorr == max(xcorr));
            ix = ix(1); % use any, if multiple bitwise identical peaks
            
            % indices of three bins around peak
            ixLow = mod(ix-1-1, n) + 1; % one below
            ixMid = ix;
            ixHigh = mod(ix-1+1, n) + 1; % one above

            % delay corresponding to the three bins
            tauLow = mod(ixLow -1 + halfN, n) - halfN;
            tauMid = mod(ixMid -1 + halfN, n) - halfN;         
            tauHigh = mod(ixHigh -1 + halfN, n) - halfN; 

            % crosscorrelation value for the three bins
            xcLow = xcorr(ixLow);
            xcMid = xcorr(ixMid);
            xcHigh = xcorr(ixHigh);
            
            x1 = tauLow;
            x2 = tauMid;
            x3 = tauHigh;
            y1 = xcLow;
            y2 = xcMid; 
            y3 = xcHigh;
        else
            % ****************************************************************
            % only main peak at first bin is of interest
            % ****************************************************************
            tauMid = tau;
            xcMid = xcorr(1);

            if xcMid > y2
                % ****************************************************************
                % improve midpoint
                % ****************************************************************
                if tauMid > x2
                    % midpoint becomes lower point
                    x1 = x2;
                    y1 = y2;
                else
                    % midpoint becomes upper point
                    x3 = x2;
                    y3 = y2;
                end
                x2 = tauMid;
                y2 = xcMid;
            
            elseif tauMid < x2
                % ****************************************************************
                % improve low point
                % ****************************************************************
                assert(tauMid >= x1); % bitwise identical is OK
                assert(tauMid > x1 || xcMid > y1); % expect improvement
                x1 = tauMid;
                y1 = xcMid;
            elseif tauMid > x2 
                % ****************************************************************
                % improve high point
                % ****************************************************************
                assert(tauMid <= x3); % bitwise identical is OK                
                assert((tauMid < x3) || (xcMid > y3)); % expect improvement
                x3 = tauMid;
                y3 = xcMid;
            else
                assert(false, '?? evaluated for existing tau ??');
            end
        end

        % ****************************************************************
        % calculate uncertainty (window width)
        % ****************************************************************
        eIter = abs(x3 - x1);
        e = [e, eIter];
        if eIter < thr_samples
            % disp('threshold reached, exiting');
            break;
        end

        if y1 == y2 || y2 == y3
            % reached limit of numerical accuracy on one side
            usePoly = 0;
        else
            % ****************************************************************
            % fit 2nd order polynomial and find maximum
            % ****************************************************************
            num = (x2^2-x1^2)*y3+(x1^2-x3^2)*y2+(x3^2-x2^2)*y1;
            denom = (2*x2-2*x1)*y3+(2*x1-2*x3)*y2+(2*x3-2*x2)*y1;
            if denom ~= 0
                tau = num / denom;
                % is the point within [x1, x3]? 
                usePoly = ((tau > x1) && (tau < x3));
            else
                usePoly = 0;
            end            
        end
        if ~usePoly
            % revert to linear interpolation on the side with the
            % less-accurate outer sample 
            % Note: There is no guarantee that the side with the more accurate
            % outer sample is the right one, as the samples aren't 
            % placed on a regular grid!
            % Therefore, iterate to improve the "worse" side, which will
            % eventually become the "better side", and iteration converges.
            
            tauLow = (x1 + x2) / 2;
            tauHigh = (x2 + x3) / 2;
            if y1 < y3
                o = [tauLow, tauHigh];
            else
                o = [tauHigh, tauLow];                
            end
            % don't choose point that is identical to one that is already known
            tau = o(1);
            if tau == x1 || tau == x2 || tau == x3
                tau = o(2);
                if tau == x1 || tau == x2 || tau == x3
                    break;
                end
            end
        end

        % ****************************************************************
        % advance 2nd signal according to location of maximum
        % phase shift in frequency domain - delay in time domain
        % ****************************************************************
        fd2Tau = fd2 .* exp(2i * pi * f * tau);
    end % for

    % ****************************************************************
    % plot the uncertainty (window size) over the number of iterations
    % ****************************************************************
    if true
        figure(); semilogy(e, '+-'); grid on;
        xlabel('iteration');
        title('uncertainty in delay');
    end

    % ****************************************************************
    % the delay estimate is the final location of the delay that 
    % maximized crosscorrelation (center of window).
    % ****************************************************************
    delay_samples = x2;

    % ****************************************************************
    % Coefficient: Turn signal 1 into signal 2
    % ****************************************************************
    coeff = fd2Tau * fd1' ./ (fd1 * fd1');

    % ****************************************************************
    % chop roundoff error, if input signals are known to be 
    % real-valued.
    % ****************************************************************
    if rflag
        coeff = real(coeff);
    end
end

% ****************************************************************
% frequency corresponding to FFT bin
% ****************************************************************
function f = binFreq(n)
    f = (mod(((0:n-1)+floor(n/2)), n)-floor(n/2))/n;
end

% ****************************************************************
% delay by phase shift
% needed only for demo code
% ****************************************************************
function waveform = cs_delay(waveform, rate_Hz, delay_s)
    rflag = isreal(waveform);
    
    n = numel(waveform);
    cycLen_s = n / rate_Hz;
    nCyc = delay_s / cycLen_s();
    
    f = 0:(n - 1);
    f = f + floor(n / 2);
    f = mod(f, n);
    f = f - floor(n / 2);
    
    phase = -2 * pi * f * nCyc;
    rot = exp(1i*phase);
    
    waveform = ifft(fft(waveform) .* rot);
    if rflag
        waveform = real(waveform);
    end
end
