% E05_2  Adaptive Filters: Noise_Canc      gr_08may20
%
clear all;

if 2==2
    N = 1000;
    n = 1:N;
    s = sin(0.075*pi*n);    % ideal signal
    v = randn(1,N)/2; % white Gaussian noise
else
    nam = 'dsp01.wav';
    % audioinfo(nam),return % Fs=44100, length=165390
    s = audioread(nam);
    s = s(17000:end-26100)';
    N = length(s);
    n = 1:N;
    nam = 'fff.wav';
    % audioinfo(nam),return % Fs=44100, length=122368
    v = audioread(nam);
    v = v(1:N)';
end

 ar = [1, -1/2, 1/4];  % Autoregression coefficients
% ar = [1, 1/2, -1/4];   % Autoregression coefficients
v1 = 10 * filter(1,ar,v);  % colored noise 
d = s + v1;   % signal + noise

ma = [1, -0.8, 0.4 , -0.2];   % Moving average coeffs.
v2 = filter(ma,1,v);    % input signal x (noise correlated to v1)
% v2 = filter(ma,1,v) + randn(size(v))*.01;  % add non-cancellable noise)
% fvtool(1,ar,ma,1); return

L = 7;  % try 3..13
h0 = [1, zeros(1,L-1)];
h = h0;
muLMS = .005;  % .005
muNLMS = .1;   % .02

for k=L:N
    v2k = v2(k:-1:k-L+1);   % acquired version of noise (v2)
    dk = d(k:-1:k-L+1);     % noisy signal
    y(k) = h * v2k';        % estimate of noise v1, obtained from v2
    e(k) = d(k) - y(k);     % recovered signal
    h = h + muLMS * e(k) * dk;    
%    h = h + muNLMS / (dk*dk'+1e-3) * e(k) * dk;
        % note dk too short to provide a stable normalization
end

rqer = sqrt((e-s).^2);
subplot(311); plot(n,d)
subplot(312); plot(n,e,'b',n,s-2,'r') % recovered (blue), ideal (shifted)(red)
subplot(313); plot(n,rqer)
SNR_first100_dB = 20 * log10( std(s) ./ std(rqer(L+1:L+100)) )
SNR_last100_dB = 20 * log10( std(s) ./ std(rqer(N-99:N)) )

return
%%
 Fs1 = 44100 / 1;
 sound(s,Fs1); pause(4); 
 sound(d,Fs1); pause(4);
 sound(e,Fs1);

%% compare to Wiener filter (needs recent dsp toolbox)

bw = firwiener(L-1,v2',d'); % Optimal FIR Wiener filter
bw, h
fvtool(h,1,bw,1);


%% Example from Mathworks (needs recent dsp toolbox)
% https://www.mathworks.com/help/dsp/ug/enhance-a-signal-using-lms-and-normalized-lms-algorithms.html
clear all
 
n = (1:1000)';
s = sin(0.075*pi*n);
v = 0.8*randn(1000,1); % Random noise part.
ar = [1,1/2];          % Autoregression coefficients.
v1 = filter(1,ar,v);   % Noise signal. Applies a 1-D digital filter.
x = s + v1;

ma = [1, -0.8, 0.4 , -0.2];
v2 = filter(ma,1,v); 
  
L = 7;
lms = dsp.LMSFilter(L,'Method','LMS');
nlms = dsp.LMSFilter(L,'Method','Normalized LMS');

[mumaxlms,mumaxmselms]   = maxstep(lms,x)
[mumaxnlms,mumaxmsenlms] = maxstep(nlms,x)

lms.StepSize  = mumaxmselms/30 
nlms.StepSize = mumaxmsenlms/20 

[ylms,elms,wlms] = lms(v2,x);
[ynlms,enlms,wnlms] = nlms(v2,x);

% compare with optimal Wiener filter
bw = firwiener(L-1,v2,x); % Optimal FIR Wiener filter
yw = filter(bw,1,v2);   % Estimate of x using Wiener filter
ew = x - yw;            % Estimate of actual sinusoid

plot(n(900:end),[ew(900:end), elms(900:end),enlms(900:end)]);
legend('Wiener filter denoised sinusoid',...
    'LMS denoised sinusoid','NLMS denoised sinusoid');

xlabel('Time index (n)');
ylabel('Amplitude');

hold on
plot(n(900:end),x(900:end),'k:')
xlabel('Time index (n)');
ylabel('Amplitude');
hold off

[bw.' wlms wnlms]

% [ylms,elms,wlms] = lms(v2,x);   % reset
% [ynlms,enlms,wnlms] = nlms(v2,x);

%% show learning curves

n = (1:5000)';
s = sin(0.075*pi*n);
nr = 25;
v = 0.8*randn(5000,nr);
v1 = filter(1,ar,v);
x = repmat(s,1,nr) + v1;
v2 = filter(ma,1,v);

reset(lms);
reset(nlms);
M = 10; % Decimation factor
mselms = msesim(lms,v2,x,M);
msenlms = msesim(nlms,v2,x,M);
%plot(1:M:n(end),[mselms,msenlms])
plot(1:M:n(end),mselms,'b',1:M:n(end),msenlms,'g')
legend('LMS learning curve','NLMS learning curve')
xlabel('Time index (n)');
ylabel('MSE');

reset(lms);
[mmselms,emselms,meanwlms,pmselms] = msepred(lms,v2,x,M);
x = 1:M:n(end);
y1 = mmselms*ones(500,1);
y2 = emselms*ones(500,1);
y3 = pmselms;
y4 = mselms;
plot(x,y1,'r',x,y2,'b',x,y3,'k',x,y4,'g')
legend('MMSE','EMSE','predicted LMS learning curve',...
    'LMS learning curve')
xlabel('Time index (n)');
ylabel('MSE');
% MMSE: minimum MSE due to limitations in the reproducibility of the desired
% system and to noise in the system
% EMSE: excess MSE, i.e. difference between the average MSE (measured near 
% the end of the adaptation process) and the MMSE


