%% parameters
% signal parameter
fc = 100.0;
Noversample = 50;
fs = Noversample*fc;
phi = 0.4*pi;

% synchronization parameters
f0 = 100;
NCOstate = struct('freq',f0, 'firstPhase', 0);

Mavg = 66; % number of samples between NCO updates

Nupdates = 5000; % number of updates
mu = 0.01;

% LPF used by receiver
hLPF = firpm(50, [0 100 200 fs/2]/(fs/2), [1 1 0 0]);
Zin = zeros(1, 50);

% create a "message" signal, carrier only
tt = (0:Nupdates*Mavg-1)/fs;
sp = cos(2*pi*fc*tt + phi);%.*cos(2*pi*0.5*tt);

out = zeros(size(sp));

theta = zeros(1, Nupdates);

%% iterative synchronization
for nn=1:Nupdates
    % get cos and sin from NCO
    [cc, ss, NCOstate] = NCO(Mavg, fs, NCOstate);
    
    % compute the output signal, make sure this is continuous across
    % invocations
    [out((nn-1)*Mavg+1:nn*Mavg), Zin] = filter(hLPF, 1, cc.*sp((nn-1)*Mavg+1:nn*Mavg), Zin);
    
    % control loop
    sp_ss = -ss.*sp((nn-1)*Mavg+1:nn*Mavg);
    
    dJPhase = sum(sp_ss)/Mavg;
    
    % update phase - just for keeping track
    if nn > 1
        theta(nn) = theta(nn-1) + mu*dJPhase;
    else
        theta(nn) = mu*dJPhase;
    end
    
    % update NCO to reflect adjustment
    NCOstate.firstPhase = NCOstate.firstPhase + mu*dJPhase;
end

plot(1:Nupdates, theta/pi*180,'b',[1,Nupdates], 180/pi*(phi + 2*pi*(fc-f0)*[0 tt(end)]) ,'r--')
grid on
xlabel('Number of Iterations')
ylabel('Phase (degrees)')