function notch( channel )

fig = channel;
linewidth = 2; markersize = 6;
if ( ~exist( 'dataStruct' ) )
    load( 'bic2_data1.mat' );
end
data = dataStruct.data;
s = data(:,channel);
s = s - mean(s);
t = dataStruct.timeVec;
t = t/1000;

S = fft(s);

Smag = abs(S);
Slen = length(S);
Smag2 = Smag(end:-1:ceil(Slen/2)+1);
Smag = Smag(2:floor(Slen/2)+1);
Sdiff = Smag - Smag2;

Fs = 1/t(2);            % sampling frequency
f = (1:length(Smag))*Fs/length(S);

figure( fig-1 ); clf; subplot(2,1,1);

plot( f, Smag, 'b',  'LineWidth', linewidth, 'MarkerSize', markersize ); hold on;
plot( f, Smag2, 'r.', 'LineWidth', linewidth, 'MarkerSize', markersize );
plot( f, Sdiff, 'g',  'LineWidth', linewidth, 'MarkerSize', markersize );

ylabel( 'S(f)', 'fontsize', 12, 'fontweight', 'bold' );
xlabel( 'Frequency (Hz)', 'fontsize', 12, 'fontweight', 'bold'  );
titletext = sprintf( 'FFT of function s(t)' ); 
title( titletext, 'fontsize', 12, 'fontweight', 'bold' );

grid on; AX = axis;
%axis( [AX(1) AX(2) 0 AX(4)] );
axis( [-20 850 -20 620] );
tx = text( 0.4*AX(1)+0.6*AX(2), 0.5*AX(3)+0.5*AX(4), ...
           sprintf( 'S(0) = %f', Smag(1) ) );
set( tx, 'fontsize', 12, 'fontweight', 'bold' );

%--- NOTCH FILTER ---

Snew = S;
filterBW = 20;
fbinwidth = Fs/length(S);
peaks = find( Smag > 11 );
disp( f(peaks) );
for fNull = f(peaks)
    [closest_f,closest_i] = min( abs(f-fNull) );
    delta_i = ceil( filterBW / fbinwidth );
    
    for middle_i = [ closest_i+1 length(Snew)-closest_i+1 ]
        atten = abs(Snew(middle_i));
        for i = middle_i - delta_i : middle_i + delta_i
            if ( i < 1 || i > length(Snew) )
                continue;
            end
%            Snew(i) = Snew(i) / (1 + atten*exp(-(i-middle_i)^2/delta_i));
%            Snew(i) = Snew(i) / (1 + atten/(1+abs(i-middle_i)));
            Snew(i) = Snew(i) / (1 + atten/(0.1+abs(i-middle_i)^3));
        end
    end
end

Smag = abs(Snew);
Slen = length(Snew);
Smag2 = Smag(end:-1:ceil(Slen/2)+1);
Smag = Smag(2:floor(Slen/2)+1);

figure(fig); clf;
subplot(2,1,1);
plot( f, Smag, 'b',  'LineWidth', linewidth, 'MarkerSize', markersize ); hold on;
plot( f, Smag2, 'r.', 'LineWidth', linewidth, 'MarkerSize', markersize );
plot( f, Sdiff, 'g',  'LineWidth', linewidth, 'MarkerSize', markersize );

ylabel( 'S(f)', 'fontsize', 12, 'fontweight', 'bold' );
titletext = sprintf( 'FFT of function s(t)' ); 
title( titletext, 'fontsize', 12, 'fontweight', 'bold' );
legends = { '|S(f)| after notching' };
ll = legend( legends );  set( ll, 'fontsize', 12, 'fontweight', 'bold' );

grid on; AX = axis;
axis( [AX(1) AX(2) 0 AX(4)] );
tx = text( 0.4*AX(1)+0.6*AX(2), 0.5*AX(3)+0.5*AX(4), ...
           sprintf( 'S(0) = %f', Smag(1) ) );
set( tx, 'fontsize', 12, 'fontweight', 'bold' );

is = real(ifft(Snew));

fn = 5000;   % cutoff frequency
Q = 1/sqrt(2);
wn = 2*pi*fn;
tau  = 1/wn;
tau1 = tau/Q;
tau2 = tau*Q;

x = tf('s');
LPF = 1 / (tau^2*x^2 + tau1*x + 1);
%LPF = 1-LPF
%figure(fig+2); clf; bode(LPF);
z = lsim( LPF, is, t );

figure(fig+1); clf;
subplot(2,1,1);
plot( t, s, 'b:',  'LineWidth', linewidth, 'MarkerSize', markersize ); hold on;
plot( t, is, 'r',  'LineWidth', linewidth, 'MarkerSize', markersize );
ylabel( 's(t)', 'fontsize', 12, 'fontweight', 'bold' );
titletext = sprintf( 'Time signal s(t)' ); 
title( titletext, 'fontsize', 12, 'fontweight', 'bold' );
legends = { 's(t)', 's(t) after notching', '5\sigma threshold' };
grid on; AX = axis;
threshold = 5*std(is);
plot( [0 AX(2)], [-threshold -threshold], 'g' );
plot( [0 AX(2)], [threshold threshold], 'g' );
ll = legend( legends );  set( ll, 'fontsize', 12, 'fontweight', 'bold' );
%axis( [AX(1) AX(2) -.04 .04] );
%axis( [1.90 1.95 -.03 .03] );

subplot(2,1,2);
plot( t, s, 'b:',  'LineWidth', linewidth, 'MarkerSize', markersize ); hold on;
plot( t, z, 'r',  'LineWidth', linewidth, 'MarkerSize', markersize ); hold on;
ylabel( 'z(t)', 'fontsize', 12, 'fontweight', 'bold' );
xlabel( 'Time (s)', 'fontsize', 12, 'fontweight', 'bold'  );
legends = { 's(t)', 'z(t) after LPF', '5\sigma threshold' };
grid on; AX = axis;
threshold = 5*std(z);
plot( [0 AX(2)], [-threshold -threshold], 'g' );
plot( [0 AX(2)], [threshold threshold], 'g' );
ll = legend( legends, 4 );  set( ll, 'fontsize', 12, 'fontweight', 'bold' );
%axis( [AX(1) AX(2) -.04 .04] );
%axis( [1.90 1.95 -.03 .03] );

Z = fft( z );
Zmag = abs(Z);
Zlen = length(Z);
Zmag2 = Zmag(end:-1:ceil(Zlen/2)+1);
Zmag = Zmag(2:floor(Zlen/2)+1);
Zdiff = Zmag - Zmag2;

figure(fig);
subplot(2,1,2);
plot( f, Zmag, 'b',  'LineWidth', linewidth, 'MarkerSize', markersize ); hold on;
plot( f, Zmag2, 'r.', 'LineWidth', linewidth, 'MarkerSize', markersize );
plot( f, Zdiff, 'g',  'LineWidth', linewidth, 'MarkerSize', markersize );
plot( f(1), Z(1), 'mo', 'LineWidth', linewidth, 'MarkerSize', markersize );

xlabel( 'Frequency (Hz)', 'fontsize', 12, 'fontweight', 'bold'  );
ylabel( 'Z(f)', 'fontsize', 12, 'fontweight', 'bold' );
legends = { '|Z(f)| after LPF' };
ll = legend( legends );  set( ll, 'fontsize', 12, 'fontweight', 'bold' );

grid on; AX = axis;
axis( [AX(1) AX(2) 0 AX(4)] );
tx = text( 0.4*AX(1)+0.6*AX(2), 0.5*AX(3)+0.5*AX(4), ...
           sprintf( 'Z(0) = %f', Zmag(1) ) );
set( tx, 'fontsize', 12, 'fontweight', 'bold' );

return;