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

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

Mavg = 100; % number of samples between NCO updates

Nupdates = 10000; % number of updates
mu = 0.001;

% 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));

theta1 = zeros(1, Nupdates);
theta2 = zeros(1, Nupdates);

%% iterative synchronization
for nn=1:Nupdates
    % get cos and sin from NCO
    [cc1, ss1, NCO1state] = NCO(Mavg, fs, NCO1state);
    [cc2, ss2, NCO2state] = NCO(Mavg, fs, NCO2state);
    
    % compute the output signal, make sure this is continuous across
    % invocations
    [out((nn-1)*Mavg+1:nn*Mavg), Zin] = filter(hLPF, 1, cc2.*sp((nn-1)*Mavg+1:nn*Mavg), Zin);
    
    % control loop: first loop tracks frequency
    sp_ss1 = -ss1.*sp((nn-1)*Mavg+1:nn*Mavg);
    
    dJPhase1 = sum(sp_ss1)/Mavg;
    
    % update phase - just for keeping track
    if nn > 1
        theta1(nn) = theta1(nn-1) + mu*dJPhase1;
    else
        theta1(nn) = mu*dJPhase1;
    end
    
    % update NCO to reflect adjustment
    NCO1state.firstPhase = NCO1state.firstPhase + mu*dJPhase;
    
    % second loop for residual phase
    sp_ss2 = -ss2.*sp((nn-1)*Mavg+1:nn*Mavg);
    
    dJPhase2 = sum(sp_ss2)/Mavg;
    
    % update phase - just for keeping track
    if nn > 1
        theta2(nn) = theta2(nn-1) + mu*dJPhase2 + mu*dJPhase1;
    else
        theta2(nn) = mu*dJPhase2 + mu*dJPhase1;
    end
    
    % update NCO to reflect adjustment
    NCO2state.firstPhase = NCO2state.firstPhase + mu*dJPhase2 + mu*dJPhase1;
end

% plot(1:Nupdates, theta2/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)')

plot(tt(47:Mavg:end),out(47:Mavg:end),'b',[tt(1), tt(end)], [0.5, 0.5], 'r--');
grid on
xlabel('Time (s)')
ylabel('Demodulator Output')