function [A_c, A_s] = phase_detect(phase,noise_pwr,corr_periods)
%function [A_c, A_s] = phase_detect(phase,noise_pwr,corr_periods)
%
% PHASE_DETECT -- Correlates a noisy sinusoid with a sine wave
%                  and a cosine wave
%
% Input Parameters:
%   phase        -- The phase shift of the input sinusoid
%   noise_pwr    -- The power of the noise added to the input
%                   sinusoid (Default: 0)
%   corr_periods -- The number of periods of the sinusoid to
%                   correlate over (Default: 1)
%
% Output Parameters:
%   A_c   -- Correlation of noisy signal with the cosine wave
%   A_s   -- Correlation of noisy signal with the sine wave
%
% The function also plots one period of three signals:
%   1) In blue is the input noisy) signal
%   2) In green is the cosine-phase sinusoid
%   3) In red is the sine-phase sinusoid
%
% Usage examples:
%   phase_detect(pi/4);
%   phase_detect(-pi/6,[],10);
%   phase_detect([],0.75);
%

if ~exist('noise_pwr','var') | isempty(noise_pwr)
    noise_pwr = 0;
end

if ~exist('corr_periods','var') | isempty(corr_periods)
    corr_periods = 1;
end

fs = 100;
frq = 2*pi;
t = 0:1/fs:(corr_periods - 1/fs);

signal = cos(t*frq + phase) + noise_pwr*randn(size(t));

cos_correlator = cos(frq*t);
sin_correlator = sin(frq*t);

A_c = sum(signal.*cos_correlator)/fs/corr_periods;
A_s = sum(signal.*sin_correlator)/fs/corr_periods;

plot(t(1:fs),[signal(1:fs)' cos_correlator(1:fs)' sin_correlator(1:fs)']);