% synchFreqNCO - demonstrate phase and Frequency Synchronization
%
% There is a phase and frequency offset betwen the received signal (fc,
% phi) and the LO (f0, theta). Demonstrate that suitable adaptation of
% theta leads to phase and frequency synchronization.
%
% Phase adjustments for the n-th update are given by:
%
% mu*(phaseError(n) + af*SUM_k=0^n phaseError(k))
%
% where phaseError is computed as the derivative of LPF{sp(t)*cos(2pif0t+theta)}
% with respect to theta.
%
% This demonstration assumes that the input signal sp(t) contains ONLY the
% carrier, no message.

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

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

Mavg = 100; % number of samples between NCO updates

Nupdates = 5000; % number of updates
mu = 0.02;
af = 0.005; % set af=0 for phase-only synchronization.

% initial sum over phase errors
intPhase = 0;

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

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

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);
    
    % phase update
    dJPhase = sum(sp_ss)/Mavg;
    
    % running sum of phase updates
    intPhase = intPhase + dJPhase;
    
    
    % update phase - just for keeping track
    if nn > 1
        theta(nn) = theta(nn-1) + mu*(dJPhase + af*intPhase);
    else
        theta(nn) = mu*(dJPhase + af*intPhase);
    end
    
    % update NCO to reflect adjustment
    NCOstate.firstPhase = NCOstate.firstPhase + mu*(dJPhase + af*intPhase);
end

figure(1)
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)')

figure(2)
plot(1:Nupdates, (theta)/pi*180 - 180/pi*(phi + 2*pi*(fc-f0)*(1:Nupdates)*Mavg/fs) ,'b');
grid on
xlabel('Number of Iterations')
ylabel('Phase Error')