%
% Fundamentals of Signal Processing
%
% L.EEC025             2022/2023
% U.Porto-DEEC
%
% Author: Prof. Anibal Ferreira (AJF)
% Week 11, P2P Problem 2 (any other utilization requires prior written
% consent from Prof. Anibal Ferreira)
%
%

FS=8000;
N=64; N2=N/2;
n=[0:N-1]; k=n;
npts=256;

resolution=FS/N;

accerror = [];

for freq=500:2.5:1600
    
    OMEGA0=2*pi*freq;
    omega0=OMEGA0/FS;
    x=sin(OMEGA0*n/FS);
    
%     x=sawtooth(2*pi*freq*n/FS);
%     x=square(2*pi*freq*n/FS);

    % reference (i.e. "non-sampled") spectrum (half part only)
    w=[0:npts-1]/npts-omega0/pi;
    H=N/2*abs(sinc(w*N/2)./sinc(w/2));
    
    % compute spectrum
    X=fft(x);
    MAG=abs(X);
    
    % find local maximum
    [tmp ell] = max(MAG);
    
    % crude frequency estimation
    estimatedfreq=(ell-1)*resolution;
    
    % make sure ell is the first DFT bin of the two largest DFT lines
    if (MAG(ell-1) > MAG(ell+1)) ell=ell-1; end
    
    fprintf('original  freq (Hz): %4.2f\n', freq);
    
    % now use an interpolation rule to obtain a better frequency estimate
    delta = N/pi*atan((sin(pi/N))/(cos(pi/N)+abs(X(ell)/X(ell+1))));
    % uncommenting the next line overrides line 43 "estimatedfreq=(ell-1)*resolution;"
    %estimatedfreq=(ell-1+delta)*resolution;
    
    fprintf('estimated freq (Hz): %4.2f\n', estimatedfreq);
    
    % current frequency estimation error (in % relative to DFT resolution)
    thiserror=abs(estimatedfreq-freq)/resolution*100;
    fprintf('relative  error : %2.2f (%%)\n', thiserror);
    accerror=[accerror thiserror];

    subplot(1,3,1);
    plot(n/FS*1000,x)
    axis([0 N/FS*1000 -1.2 1.2]);
    title('Sinusoid');
    xlabel('Time (ms)');
    ylabel('Amplitude');
    
    subplot(1,3,2);
    stem([0:N2-1]*FS/N/1000, MAG(1:N2),'filled');
    axis([-0.1 N2*FS/N/1000 0 35]);
    title('Spectrum analysis');
    xlabel('Frequency (kHz)');
    ylabel('|H[k]|');
    hold on
    plot(FS*[0:npts-1]/npts/2/1000,H,'m')
    hold off
    
    subplot(1,3,3);
    plot(accerror)
    xlabel('Estimation number')
    ylabel('Estimation error (%) relative to DFT bin-width') 
    
    pause;
end