% *******************************************************
% delay-matching between two signals (complex/real-valued)
% M. Nentwig
%
% signal and ref are two sampled signals, same number of points.
% both are treated as cyclic (use zero-padding, if needed)
% 
%
% * output:
%   => coeff: complex scaling factor that scales 'ref' into 'signal'
%   => delay 'deltaN' samples (with subsample resolution)
%   => 'shiftedRef': a shifted and scaled version of 'ref' that 
%      matches 'signal' 
%   => (signal - shiftedRef) gives the residual (vector error)
% norm(signal - shiftedRef) is minimized.
% 
% Properly used, the timing can be resolved with an accuracy of a small 
% fraction of the sample duration, even nano-sample accuracy can be
% achieved.
% *******************************************************
function [coeff, shiftedRef, deltaN] = fitSignal_120708(signal, ref)
 
    signal = signal(:) .'; % force row vector
    ref = ref(:) .';
    n = numel(signal);
    assert(n == numel(ref), 'length mismatch');
    doPlot = false;
    enableCoarseCorr = true;
    enableUnwrap = false; % may fail for example if DC term is removed
    enableSanityCheck = false;

    % *******************************************************
    % Calculate the normalized frequency for each FFT bin
    % [-0.5..0.5[
    % *******************************************************
    binFreq=(mod(((0:n-1)+floor(n/2)), n)-floor(n/2))/n;
    
    % *******************************************************
    % Convert signals to frequency domain
    % *******************************************************
    sig_FD = fft(signal);
    ref_FD = fft(ref);
    
    % rotate each bin backwards with the phase of the reference
    u = sig_FD .* conj(ref_FD);
    
    negateCoeff = false;
    if enableCoarseCorr
        
        % *******************************************************
        % Coarse correction (+/- 1 sample) using crosscorrelation between 
        % signal and reference...
        % *******************************************************
        NyqBin = [];
        if mod(n, 2) == 0
            % for an even-sized FFT, it is a matter of interpretation, whether the center
            % bin should be treated as a positive or a negative frequency. 
            % Without that information, it cannot be delayed. 
            % In a real-valued signal, it cannot be delayed at all, as it is lacking its
            % negative frequency counterpart (time domain: [1 -1 1 -1 1 -1 1 ...])
            % Disregard it.
            u(n/2 + 1) = 0;
        end
        Xcor = ifft(u);
        
        % *******************************************************
        % Each bin in Xcor corresponds to a delay in samples.
        % The bin with the highest absolute value corresponds to
        % the delay where maximum correlation occurs.
        % *******************************************************
        ix = find(abs(Xcor) == max(abs(Xcor)));
        ix = ix(1); % in case there are several bitwise-identical peaks
        
        % *******************************************************
        % A negative peak implies a sign change: signal matches -ref.
        % *******************************************************
        if real(Xcor(ix)) > 0
            % positive correlation peak
        else
            % negative correlation peak
            % This would result in a 180 degree baseline phase in the 
            % LS solver, which creates ambiguity problems 
            % Solution: Negate the signal (and all dependent expressions), 
            % and undo by negating coeff at the end. 
            signal = -signal; 
            sig_FD = -sig_FD;
            u = -u;  
            Xcor = -Xcor;
            negateCoeff = true;
        end

        % the location of the peak denotes the delay, with a resolution of 
        % one sample. Delay 0 appears in bin 1 etc.
        integerDelay = ix - 1; 
        
        if doPlot
            figure(); hold on; 
            v = abs(Xcor);
            plot(v, 'b');
            h = plot(integerDelay+1, v(integerDelay+1), 'r+');
            set(h, 'lineWidth', 3);
            legend('crosscorrelation', 'coarse (integer) delay estimate');
            title('coarse delay estimation via crosscorrelation');
        end
    else
        integerDelay = 0;
    end
    
    % *******************************************************
    % un-delay the signal by the estimated coarse delay.
    % causality is not an issue, as the signal is cyclic (it
    % repeats continuously over time = -inf..inf)
    % *******************************************************
    % exp(-2i pi f) is the Fourier transform of a unity delay
    % exp(2i pi f) is a negative delay.
    rotN = exp(2i*pi*integerDelay .* binFreq);
    
    if doPlot
        tref = ifft(fft(ref) .* conj(rotN)); % conj => sign change of delay
        figure(); hold on;
        plot(real(ref), 'b');
        plot(real(signal), 'k');
        plot(real(tref), 'r');
        legend('original reference', 'signal', 'reference, coarse aligned');
    end
    
    if false && doPlot
        figure(); hold on;
        plot(fftshift(binFreq), fftshift(20*log10(abs(fft(ref)) + eps)), 'k');
        plot(fftshift(binFreq), fftshift(20*log10(abs(fft(signal)) + eps)), 'b');
        legend('reference', 'signal');
        xlabel('normalized frequency');
        ylabel('dB');
        title('power spectrum');
    end

    % *******************************************************
    % Find the exact delay using linear least mean squares fit
    % on the phase.
    % 
    % u is the phase difference on any frequency bin. 
    % Delay appears as linear increase in phase, but unwrapping
    % the phase (2 pi ambiguity) across gaps in the power spectrum 
    % is not robust enough.
    % Therefore, use the coarse delay (with +/- 1/2 sample accuracy) to 
    % rotate back the phase. This removes most of the 2 pi steps.
    % Note this is method is not completely "foolproof" either but 
    % usually quite robust. 
    % In case of failure, try oversampling (fft;zero pad; ifft) on 
    % both signal and reference.
    % *******************************************************
    angCorr = angle(u .* rotN);
    ang = angCorr;

    if enableUnwrap
        ang = fftPhaseUnwrap(ang);
    end
    if false && doPlot
        angRaw = angle(u);
        figure(); grid on; hold on;
        h = plot(angRaw, 'k'); set(h, 'lineWidth', 2);
        h = plot(angCorr, 'b'); set(h, 'lineWidth', 2);
        h = plot(ang, 'g'); set(h, 'lineWidth', 1);
        xlabel('FFT bin index');
        ylabel('phase / radians');
        title('phase');
        legend('raw phase', 'phase corrected with coarse estimate', 'corrected and unwrapped');
    end
    
    % *******************************************************
    % For the least-squares fit, the phase is weighted according to
    % the product of amplitudes from signal and reference. 
    % As intuitive explanation: any bin is disregarded, if either
    % reference signal or received signal contain too little energy, 
    % as the phase would be meaningless and vary wildly as result of 
    % added noise etc.
    % *******************************************************
    weight = abs(u);
    % normalize for plotting (doesn't affect the result)
    weight = weight / max(weight);
    
    % Phase shift of a complex-valued scaling factor
    % It rotates all frequencies by the same amount.
    % Apply per-frequency weighting. 
    constRotPhase = 1 .* weight;
    
    % Phase shift of a unit delay
    % Apply per-frequency weighting
    uDelayPhase = -2*pi*binFreq .* weight;    
    
    % The observed phase difference between signal and reference
    % (corrected by the coarse estimate), and weighted.
    ang = ang .* weight;
    
    if false
        % discard low-value bins (optimization for long, highly oversampled signals)
        chkThr = abs(ang);
        thr = max(chkThr) * 1e-6;
        ixPow = find(chkThr > thr);
        constRotPhase = constRotPhase(ixPow);
        uDelayPhase = uDelayPhase(ixPow);
        ang = ang(ixPow);
    end
    
    % Least-squares equation system that attempts to "explain" the observed
    % phase in terms of
    % - a constant phase shift => r(1)
    % - a unity delay => r(2)
    % base .' * r = ang .'
    % least-squares solve for r:
    base = [constRotPhase; uDelayPhase];
    r = base .' \ ang.'; %linear mean square solution
    
    if doPlot
        figure(); hold on;
        h = plot(ang, 'k'); set(h, 'lineWidth', 2);
        plot(constRotPhase, 'g');
        plot(uDelayPhase, 'b');
        plot(base.' * r, 'r');
        title('phase vectors in least-squares solver');
        legend('signal, weighted', 'constant, weighted', 'unit delay, weighted', 'least-squares fit');
        xlabel('FFT bin index'); ylabel('weighted phase');
    end
        
    % The constant phase rotation r(1) is not used
    % It will be obtained later as the phase of 'coeff'
    
    % least-squares optimal number of unit delays 
    fractionalDelay = r(2);
    if enableCoarseCorr
        assert(abs(fractionalDelay) <= 0.5*1.2345, 'mismatch between coarse- and fine estimation'); 
    end
    
    % *******************************************************
    % Final delay estimate: Restore the coarse estimate that
    % had been removed earlier
    % (returned result)
    % *******************************************************
    deltaN = integerDelay + fractionalDelay;        

    % *******************************************************
    % Convert numbers beyond the midpoint into a negative
    % number (optional)
    % *******************************************************
    deltaN = mod(deltaN-n/2, n) + n/2;
    
    % *******************************************************
    % Delay ref with the final delay estimate
    % *******************************************************    
    ref_FD = ref_FD .* exp(-2i*pi*deltaN .* binFreq);
    shiftedRef = ifft(ref_FD);
    
    % *******************************************************
    % Crosscorrelation with the now time-aligned signal
    % The resulting coeff minimizes norm(signal - coeff * shiftedRef)
    % (returned result)
    % *******************************************************
    coeff = sum(signal .* conj(shiftedRef)) / sum(shiftedRef .* conj(shiftedRef));
    if negateCoeff
        % sign was changed earlier. Change it back.
        coeff = -coeff;
    end
    
    % *******************************************************
    % apply the coefficient to the delayed reference signal. 
    % (returned result)
    % *******************************************************
    shiftedRef = shiftedRef * coeff;

    % *******************************************************
    % Sanity check:
    % calculate the crosscorrelation for all delays. 
    % Assuming we have done a proper job, the peak must be 
    % at zero delay. Otherwise, it would mean that there is a 
    % "better" delay on the tested one-sample grid.
    % *******************************************************
    if enableSanityCheck
        u = sig_FD .* conj(ref_FD);
        Xcor=abs(ifft(u));
        if Xcor(1) ~= max(Xcor)
            figure(); plot(abs(Xcor)); title('sanity check failed');
            error ('not locked to the correct bin');
        end
        assert(real(Xcor(1)) >= 0, 'sign is wrong?!');
    end

    % *******************************************************
    % Report a delay beyond the midpoint as negative
    % *******************************************************
    halfLen = floor(n / 2);
    deltaN = mod(deltaN + halfLen, n) - halfLen;                    
end

% p is a phase of a complex-valued FFT, with the "0 Hz bin" at index 1
% removes phase jumps in excess of pi in positive and negative direction
function p = fftPhaseUnwrap(p)
    n = numel(p);
    lastPosIx = ceil(n/2); % last positive frequency
    
    % positive frequencies
    ixVec = 2:lastPosIx;
    % ixVecAdj points towards the adjacent bin, in direction
    % towards the origin (minus one bin for pos. frequencies)
    ixVecAdj = ixVec - 1;
    delta = p(ixVec) - p(ixVecAdj);
    delta = round(delta / 2 / pi) * 2 * pi;
    delta = cumsum(delta);
    p(ixVec) = p(ixVec) - delta;
    
    % negative frequencies
    % delta processsed in reverse direction, so that cumsum
    % can be used
    ixVec = n:-1:lastPosIx+1;
    % ixVecAdj points towards the adjacent bin, in direction
    % towards the origin (plus one bin for neg. frequencies)
    ixVecAdj = ixVec + 1;
    ixVecAdj(1) = 1; % n+1 wraps around to 1
    delta = p(ixVec) - p(ixVecAdj);
    delta = round(delta / 2 / pi) * 2 * pi;
    delta = cumsum(delta);
    p(ixVec) = p(ixVec) - delta;    
end