% MATLAB exercise for fundamental frequency estimation
% R. Martin, Ruhr-Universitaet Bochum
% September 12, 2017
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear                      % clear all variables
close all                  % close all windows
f_1 = 110;                 % frequency of first signal
f_2 = 2^(-5/3)*440;        % frequency of second signal

fs=44100;                  % sampling frequency

N_1=fs/f_1;                % number of samples for one period

k=0:5000;                  % Number of signal samples

y_1=0.95.^(k/N_1).*sin(k*2*pi/N_1);    % first signal
 
N_2=fs/f_2;                % Number of signal samples
y_2=0.95.^(k/N_2).*sin(k*2*pi/N_2);    % second signal
 
% odd partials (3rd and 5th ones) of f1
y_1_3=0.95.^(k*(3*f_1)/fs).*sin(k*2*pi*(3*f_1)/fs);
y_1_5=0.95.^(k*(5*f_1)/fs).*sin(k*2*pi*(5*f_1)/fs);
y_1_3_5 = y_1 + y_1_3 + y_1_5;

N=2000;        % length of signal segment for F0 analysis

ysig = y_1;    % define signal to analyse
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure(2)

% plot input signal in first subplot
subplot(2,1,1)
xvec = (1:N)./fs;
plot(xvec, ysig(1:length(xvec)))
axis([0 0.04 -2 2])
xlabel('t / s')
ylabel('y')
title('input signal')
grid

% select F0 method
method_switch = 1;                        % 1: autocorrelation method  ; 0: YIN-like method

if method_switch
  % compute autocorrelation function and lag vector for lags in range N
  %!!!!! [objfun lags] = <...>
  % Find first peak next to 0
  [pks, locs] = findpeaks(objfun);      % get all peaks
else
  % compute an objective function similar to the YIN method
  % prepare empty arrays
  df_y = zeros(ceil(length(ysig)/2),1);
  objfun = zeros(ceil(length(ysig)/2),1);
  
  % compute squared difference function for all lags tau
  for tau = 0:N
    %!!!! df_y(tau+1) = <...>
  end
  lags = 0:N;
  
  % compute normalized difference function
  objfun(1) = 1;
  for tau = 1:N
    %!!!!  objfun(tau+1) = <...>
  end
  % search for minimum by inverting and a search for maximum
  objfun = -objfun;
  % Find first peak next to 0
  [pks, locs] = findpeaks(objfun);             % get all peaks
  pks = pks.';
end

min_range = min(objfun)-1
max_range = max(objfun)+1

figure(2)

% plot objective function
subplot(2, 1, 2)
plot(lags./fs, objfun)
axis([0 0.04 min_range max_range])
xlabel('t / s')
grid on
hold on

% Peak-Detection:
[~, pos] = max(((lags(locs)>0).*pks)); % get the position of the first peak bigger than 0

% plot the peak
first_peak_pos=lags(locs(pos));
first_peak_positions=[first_peak_pos./fs first_peak_pos./fs];
plot(first_peak_positions, [min_range max_range]);
scatter(first_peak_pos./fs, pks(pos), 'r', 'x');

title(['objective function; N=', num2str(N),...
  '; detected frequency: f=', num2str(1/(first_peak_pos/fs)), 'Hz'],'interpreter', 'latex')
   
hold off

