% synchCos - synchronize phase of cos

%% parameters
fc = 100;
Noversample = 50;
fs = Noversample*fc;
phi = 0.4*pi;
f0 = 100;
sigma = 0.1;

N = 2000;
mu = 0.08;
theta = zeros(1,N);
theta(1)=0*pi;

m_out = zeros(1,N);

%% theta will be updated once per cycle (every 1/100)
% this allows us to store just one cycle of samples
tt = (0:Noversample-1)/fs;
sp_cycle = cos(2*pi*fc*tt + phi);

for nn = 1:N-1
    sp = sp_cycle + sigma*randn(size(sp_cycle));
    
    cos_lo_cycle = cos(2*pi*f0*tt + theta(nn));
    sin_lo_cycle = sin(2*pi*f0*tt + theta(nn));
    
    % compute the output signal - use Noversample-point average
    m_out(nn) = 1/Noversample * sum(sp .* cos_lo_cycle);
    
    % compute derivative of objective function
    dJ = 1/Noversample * sum(sp .* sin_lo_cycle);
    
    % update theta
    theta(nn+1) = theta(nn) - mu*dJ;
end

% compute last output sample
cos_lo_cycle = cos(2*pi*fc*tt + theta(N));
m_out(N) = 1/Noversample * sum(sp_cycle .* cos_lo_cycle);

%% plots
% plot theta
figure(1)
plot(0:N-1, theta/pi*180, 'b', [0,N-1], phi/pi*180*ones(1,2), 'r--')
grid on
xlabel('Iteration')
ylabel('Estimated Phase (degrees)')

% plot demodulator output
figure(2)
plot(0:N-1, m_out, 'b', [0,N-1], 0.5*ones(1,2), 'r--')
grid on
xlabel('Iteration')
ylabel('Demodulator Output')
