% test 60 Hz canceller
clear all
figure(1); clf;

%=== settable parameters =======================
lowpass_cutoff = 25; %Hz
highpass_cutoff = 1; %Hz
noise_amp = 0.025 ; % thermal noise relative to QRS
interference_amp = 10 ; % 60 Hz nose amp relative to QRS
mu2e =.001 ; % LMS convergence parameter 
baseline_amp = 1 ; %baseline drift amplitude
Fs = 60*16; % sample rate
%=== end settable parameters ===================

dt = 1/Fs;
t = 0:dt:10;

%reference noise
f = 60.5 ;
ref = sin(2*pi*f*t);
%refshift = sin(2*pi*60*t + pi/2);

%define QRS
AP1amp = 1;
AP1dur1 = .050;

%make single QRS
oneT = 0:dt:(AP1dur1);
oneAP = AP1amp*sin(2*pi/(2*AP1dur1)*oneT).*(oneT<=AP1dur1) ;%+ ...
  %  AP1amp*0.5*sin(2*pi/(2*AP1dur2)*(oneT+AP1dur1)).*(oneT>AP1dur1) ;

%make a bunch of QRS
%locationAP1 = double(rand(size(t))<AP1prob); %poisson
locationAP1 =  double(0==mod(t,0.95));
%locationAP1 = mod([1:length(t)],457)==0; %uniform
vAP = conv(locationAP1,oneAP);
vAP = vAP(1:length(t)); %truncate
noisefree = vAP+ noise_amp*randn(size(t));
sig = noisefree + interference_amp* sin(2*pi*f*t)  ;
sig = sig + baseline_amp * sin(2*pi*0.2*t);

[b,a] = butter(2, lowpass_cutoff/(Fs/2));
sig1 = filter(b, a, sig );

[b,a] = butter(2, highpass_cutoff/(Fs/2),'high');
sig2 = filter(b, a, sig1 );
%%%%%%%%%%%%%%%%%%%%%%%%%
w1 = 0;
w2 = 0;

for i=5:length(t)
    out(i) = sig2(i) -  (w1*ref(i) + w2*ref(i-4)) ; %90 degreee phase shift
    w1 = w1 + mu2e*out(i)*sign(ref(i)) ;
    w2 = w2 + mu2e*out(i)*sign(ref(i-4)) ;
end

np=2;

subplot(np,1,1)
plot(t,sig);

subplot(np,1,2)
plot(t,out)
hold on
plot(t,vAP-2,'r')
set(gca,'ylim', [-2 2])
title(['lowpass cutoff=', num2str(lowpass_cutoff), ...
'  highpass cutoff=' ,  num2str(highpass_cutoff) , ...
'  noise amp=',  num2str(noise_amp), ...
'  interference amp=' ,  num2str(interference_amp), ...
'  mu=' ,  num2str(mu2e) , ...
'  baseline amp=' ,  num2str(baseline_amp) ...
'  Fs=',  num2str(Fs) ], 'fontsize', 12)


