% *******************************************************
% Example to determine the location of a signal via FFT with
% (almost) arbitrary accuracy
% Markus Nentwig 2007
% This program is provided WITHOUT ANY WARRANTY
% *******************************************************
% Note:
% - the signal is assumed to be cyclic within the FFT length
% - the pulse is actually infinitely long (a bandlimited pulse
%   cannot be limited in time) This can be seen after time shifting.
close all; clear all;

% *******************************************************
% Main loop: 
% - Sweep through a number of delays
% - delay the test pulse via FFT
% - recover the delay
% - plot the result (deviation to nominal)
% *******************************************************
x=[0:sqrt(2)/100:30];
%x=10.45; %examples
y=[];
for dNom=x

  % *******************************************************
  % Generate the test pulse
  % add some zero padding 
  % *******************************************************
  % sine pulse
  len=150; pulse=cos((0:(len-1))/len*2*pi*5);
  %pulse=randn(1, 150); % random pulse
  pulse=[pulse, zeros(1, 1*length(pulse))];
  s=pulse;
  % s=s+(randn(size(s))-0.5)*0.05; %Add some noise
  n=length(s);
  
  % *******************************************************
  % Calculate the frequency that corresponds to each FFT bin
  % [-0.5..0.5[
  % *******************************************************
  binFreq=(mod(((0:n-1)+floor(n/2)), n)-floor(n/2))/n;

  % *******************************************************
  % Delay the test pulse
  % *******************************************************
  s=fft(s);
  s=s .* exp(-2i*pi*dNom .* binFreq);
  s=real(ifft(s));

  % *******************************************************
  % Delay calculation starts:
  % Convert to frequency domain...
  % *******************************************************
  sig_FD=fft(s);
  ref_FD=fft(pulse, n);

  % *******************************************************
  % ... calculate crosscorrelation between 
  % signal and reference...
  % *******************************************************
  u=sig_FD .* conj(ref_FD);
  if mod(n, 2) == 0
    % for an even sized FFT the center bin represents a signal
    % [-1 1 -1 1 ...]. It cannot be delayed. The frequency component
    % is excluded from the calculation.
    u(length(u)/2+1)=0;
  end
  Xcor=abs(ifft(u));

  % *******************************************************
  % Each bin in Xcor corresponds to a given delay in samples.
  % The bin with the highest absolute value corresponds to
  % the delay where maximum correlation occurs.
  % *******************************************************
  integerDelay=find(Xcor==max(Xcor));
  % in case there are several bitwise identical peaks, use the first one
  % Minus one: Delay 0 appears in bin 1
  integerDelay=integerDelay(1)-1;

  % Fourier transform of a pulse shifted by one sample
  rotN=exp(2i*pi*integerDelay .* binFreq);

  uDelayPhase=-2*pi*binFreq;
  
  % *******************************************************
  % Since the signal was multiplied with the conjugate of the
  % reference, the phase is rotated back to 0 degrees in case
  % of no delay. Delay appears as linear increase in phase, but
  % it has discontinuities.
  % Use the known phase (with +/- 1/2 sample accuracy) to 
  % rotate back the phase. This removes the discontinuities.
  % *******************************************************
  % figure(); plot(angle(u)*180/pi); title('phase of fft(signal) .* conj(reference), incl. integer delay'); xlabel('FFT bin'); ylabel('degrees');
  u=u .* rotN;
  % figure(); hold on; plot(angle(u)*180/pi, 'b'); plot(uDelayPhase*180/pi, 'r'); title('phase of fft(signal) .* conj(reference), excl. integer delay'); xlabel('FFT bin'); ylabel('degrees'); legend('FFT contents', 'phase of unity delay');
  
  % *******************************************************
  % Obtain the delay using linear least mean squares fit
  % The phase is weighted according to the amplitude.
  % This suppresses the error caused by frequencies with
  % little power, that may have radically different phase.
  % *******************************************************
  weight=abs(u); 
  uDelayPhase=uDelayPhase.* weight;
  ang=angle(u).* weight;
  % figure(); hold on; plot(ang*180/pi, 'b'); plot(uDelayPhase*180/pi, 'r'); title('amplitude-weighted phase'); xlabel('FFT bin'); ylabel('degrees'); legend('FFT contents (weighted)', 'phase of unity delay (weighted)');

  fractionalDelay=transpose(uDelayPhase)\transpose(ang); %linear mean square

  % *******************************************************
  % Finally, the total delay is the sum of integer part and
  % fractional part.
  % *******************************************************
  result=integerDelay+fractionalDelay;
  y=[y result-dNom];
end


figure(); hold on; plot(pulse+1.1, 'r'); plot(s-1.1, 'b'); xlabel('sample index'); ylabel('amplitude (offset)'); legend('reference', 'time-shifted signal'); title('signals (example, last run)');
figure(); plot(x, y, '+'); xlabel('actual delay'); ylabel('delay error'); title('accuracy');