function [amp, freq, phi] = analyzeSinusoid_accurate_slow(sig, fs)
% analyzeSinusoid - determine amplitude, frequency, and phase from samples
%
% Given a signal (sig) sampled at rate fs, determine its amplitude,
% frequency, and phase. The first sample of the signal is taken at time
% t=0, the second at 1/fs, etc.
%
% The objective of this function is to determine parameters amp, freq, and
% phi such that a sinusoid generated from these parameters is as similar as
% possible to the samples in sig. Specifically, the estimated parameters
% will be evaluated as follows (see evalFunc):
%
% % generate a sinusoid from parameters
% tt = (0:length(sig)-1)/fs;        % make a time axis
% xx = amp*cos(2*pi*freq*tt + phi); % construct sinusoid
%
% % measure the power of the difference between xx and sig
% score = var(xx-sig);              % try to make this as small as you can
%
% Your function must have the following inputs and outputs:
%
% Inputs:
%   sig - samples of a (approximately) sinusoidal signal
%   fs  - sampling rate (number of samples per second)
%
% Outputs:
%   amp  - estimated amplitude
%   freq - estimated frequency (in Hz!)
%   phi  - esimated phase (in radians)
%
% Usage:
%   [amp, freq, phi] = analyzeSinusoid(sig, fs)
%
% See also:
%   evalFunc

%%%%%%%%% MAKE NO CHANGES ABOVE THIS LINE %%%%%%%%%%%%%%%%%%
%%%%%%%%% YOU MUST STORE YOUR FILE AS: analyzeSinusoid.m %%%

DEBUG = true; % turns on plotting of intermediate results

%% initialization
N = length(sig);
tt = (0:N-1)/fs;
ff = (0:N/2-1)*fs/N;

score = zeros(size(ff));
amps = zeros(size(ff));
phases = zeros(size(ff));

for kk = 1:length(ff)
    % use function bestPhasor to find the best amp/phase for each frequency
    [amps(kk), phases(kk), score(kk)] = bestPhasor(sig, ff(kk), tt);   
end

% find the best frequency and associated estimates
[min_score,ind_min] = min(score);

amp = amps(ind_min);
phi = phases(ind_min);
freq = ff(ind_min);

if DEBUG
    figure(1)
    plot(ff, score, '-', ff(ind_min), score(ind_min), 'ro');
    grid on
    xlabel('Frequency (Hz)')
    ylabel('Score')
    title(sprintf('After coarse search: score = %4.3f (A=%4.3f, phi=%4.3f, f=%4.3f)', min_score, amp, phi, freq))
    
    pause
end

% refine estimate by computing on finer grid near minimum
fine_ff = linspace(ff(max(1,ind_min-1)), ff(min(ind_min+1, length(ff))), 100);

fine_score = zeros(size(fine_ff));
fine_amps = zeros(size(fine_ff));
fine_phases = zeros(size(fine_ff));

for kk = 1:length(fine_ff)
    % use function bestPhasor to find the best amp/phase for each frequency
    [fine_amps(kk), fine_phases(kk), fine_score(kk)] = bestPhasor(sig, fine_ff(kk), tt);   
end

% find the best frequency and associated estimates
[min_score,ind_min] = min(fine_score);

amp = fine_amps(ind_min);
phi = fine_phases(ind_min);
freq = fine_ff(ind_min);

if DEBUG
    figure(2)
    plot(fine_ff, fine_score, '-', fine_ff(ind_min), fine_score(ind_min), 'ro');
    grid on
    xlabel('Frequency (Hz)')
    ylabel('Score')
    title(sprintf('After fine search: score = %4.3f (A=%4.3f, phi=%4.3f, f=%4.3f)', min_score, amp, phi, freq))
    
end

if DEBUG
   figure(3)
   N3 = min(length(sig), round(3*fs/freq)); % samples for 3 cycles
   plot(tt(1:N3), sig(1:N3), 'b-', ...
       tt(1:N3), amp*cos(2*pi*freq*tt(1:N3)+phi), 'r-')
   grid on
   xlabel('Time (s)')
   
   disp('Enter to continue')
   pause
end


end

%%% helper function best amp and phase
function [amp, phi, score] = bestPhasor(sig, freq, tt)
% bestPhasor - find the best phasor for a given frequency

% special case zero frequency; the matrix below is not full rank
if freq == 0
    amp = mean(sig);
    phi = 0;
    score = 1;
    
    return
end

% make up cos and sin for each frequency
cos_sig = cos(2*pi*freq*tt);
sin_sig = sin(2*pi*freq*tt);

% compute the correlations
cc = sum(cos_sig.^2);
ss = sum(sin_sig.^2);
cs = sum(cos_sig .* sin_sig);

xc = sum(sig .* cos_sig);
xs = sum(sig .* sin_sig);

% solve for real and imaginary parts
A = [cc -cs; -cs ss];
x_vec = [xc; -xs];

two_amps = A\x_vec;

a = two_amps(1); % this is A*cos(phi)
b = two_amps(2); % this is -A*sin(phi)

amp = sqrt(a^2 + b^2);
phi = atan2(b, a);  % the -sign stems from -A*sin(phi)

score = var(sig - a*cos_sig + b*sin_sig)/var(sig);
end

    
    
    
    