%% Demonstrate adaptive phase tracking

% do_freq_offset = false; % set in outer script

% target output
A_target = 1;

% adaptive gain
% mu = 0.1; % set in outher script 
% eta = 0; %mu/100; % set in outer script

% signal length
N = 1000;

% noise
% sigma = 0.05; % set in outer script

% input signal - complex envelope
if(do_freq_offset)
    s_in = exp(1j*(2*pi*(1:N)/N + pi/4));
else
    s_in = exp(1j*pi/4)*ones(1,N);
end

s_in = s_in + sigma/sqrt(2)*(randn(1, N) + 1j*randn(1, N));

% adaptive gain
g = 1j*zeros(1, length(s_in));
g(1) = 1 + 0j;

% phase corrected signal
r = zeros(1, length(s_in));
r(1) = s_in(1)*g(1);

sum_err = 0;

for n = 2:N
    % compute gradient
    err = r(n-1)-A_target;
    sum_err = sum_err + err;
    grad = conj(s_in(n-1))*err;
    sum_grad = conj(s_in(n-1))*sum_err;
    
    % update gain - direction of negative gradient
    g(n) = g(n-1) - mu*grad - eta*sum_grad;
    
    % apply gain
    r(n) = s_in(n)*g(n);
end

figure(1)
plot(1:N, angle(s_in)*180/pi, 'b', 1:N, angle(r)*180/pi, 'r')
legend('Input', 'Output')
xlabel('Iteration')
ylabel('Phase (degrees)')
grid()
if(~do_freq_offset)
    axis([0, N, -10, 50])
end

figure(2)
plotyy(1:N, angle(g)*180/pi, 1:N, abs(g))
ylabel('Phase of gain (degrees)'     )
grid()
if(~do_freq_offset)
    axis([0, N, -50, 10])
end


% print results
fprintf('Results (sigma = %4.2f)\n=======\n', sigma)
fprintf('mu = %5.2f\n', mu)
N0 = find(abs(angle(r)) < 0.01, 1, 'first');
fprintf('N0 = %5d\n', N0)
s_theta = std(angle(r(N0:N0+100)));
fprintf('std(phase) = %5.3f\n', s_theta)