function filtinfo = adapt_noise_supp_fix(srce,dest,blksize,num_blk_cte,forgetfact,adaptcte)
% filtinfo = adapt_noise_supp(srce, dest, blksize, num_blk_cte, forgetfact, adaptcte)
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file reads in from a sqd file: 157 neuronal channels, 3 reference channels,
% 32 trigger channels (and all other information). It filters the neuronal channels
% based on the 3 reference channels, then creates a new sqd file with the filtered
% neuronal data and everything else unmodified.
% The method is the Fast LMS adaptive filter.
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% srce : raw sqd file (if not specified, a sqd file is requested)
% dest : filtered sqd file (srce + '-filtered.sqd' is the default)
% blksize : size of LMS block in samples; typical: 64, 128(default), 256, 512
% num_blk_cte : number of blocks (weighted by sampling frequency in kHz) to filter at
%  once (reduce if 'out of memory' errors occur); typical: 0.25, 0.5, 1(default), 2, 4
% forgetfact : forgetting factor,  0 < gamma < 1, (0.94 default)
% adaptcte : adaptation constant, 0 < alpha < 0.5 (0.01 default)
% filtinfo = filtering parameters and times
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Author: Nayef Ahmar aenayef@glue.umd.edu
%
% for the Computational Sensorimotor Systems Lab (CSSL) UMCP
% http://www.isr.umd.edu/Labs/CSSL/
% Version 0.91 Dec. 1 2005
%
% Latest version at
% http://www.isr.umd.edu/Labs/CSSL/simonlab/resources/resources.html
%
if nargin == 0,
    [fn, pn] = uigetfile('*.sqd','Select your SQD file source');
    if sum(fn==0)&&sum(pn==0),return,end
    srce = fullfile(pn,fn);
end
info = sqdread(srce,'info');%extract info
sf = info.SampleRate;%sampling frequency in HZ

if nargin < 6, adaptcte = 0.01; end;
if nargin < 5, forgetfact = 0.94; end;
if nargin < 4, num_blk_cte = 500/sf; end;
if nargin < 3, blksize = 2^ceil(log2(2*sf/8));end %128;
if nargin < 2, [pathstr,name,ext] = fileparts(srce); dest = fullfile(pathstr,[name '-filtered' ext ]); end;

if exist(dest,'file'),delete(dest),end %delete any existing file because we use append command in sqdwrite
t0 = clock;
num_samples = info.SamplesAvailable;%total number of samples
tot_num_chan = info.ChannelCount;
%create a destination sqd file and write first sample
data = sqdread(srce,'Channels',[0:tot_num_chan-1],'Samples',[1 1]);
sqdwrite(srce,dest,data);

%Input parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
alpha = adaptcte;%0.01;%0.005;%adaptation constant
gamma = forgetfact;%0.94;%forgetting factor
M = blksize;%64;%Block size
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cte = floor(1000*64/blksize);
num_blks = num_blk_cte*cte;%1000;%reduce if have out of memory problems
N = floor((num_samples-1)/M/num_blks);%number of blocks to pass to fast_lms, should be integer
blk_len = M*num_blks;

infoc.sf = sf;
infoc.blocksze = M;
infoc.forgetfact = gamma;
infoc.adaptcte = alpha;
infoc.multiblk = blk_len;
infoc.source = srce;
infoc.destin = dest;

%initialize P and W
P_chan = gamma*ones(2*M,3,157);
W_chan = zeros(2*M,3,157);
U = ones(M,3);%@for in between block transition update

h = waitbar(0,'Please wait...');

for blk = 1:N,
    blk_lb = (blk-1)*blk_len + 2;%start at 2 because we already copied first sample
    blk_ub = blk*blk_len + 1;
    data = sqdread(srce,'Channels',[0:tot_num_chan-1],'Samples',[blk_lb blk_ub]);
    data_chan = data;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %Warnings%@
%     if min(abs(mean(diff(data_chan(1:100:end,1:160)))-mean(data_chan(1:10:end,1:160))))== 0 ,
%         display('Warning: channel is clipped');
%     end
    if max(abs(mean(data_chan(:,1:160)))) > 1000,%need to verify this threshold
        display('Warning: For better filtering, remove Dc before applying noise suppression');
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    u = data_chan(:,158:160);
    for chan = 1:157,
        P = P_chan(:,:,chan);
        W = W_chan(:,:,chan);
        d = data_chan(:,chan);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [E,p,w]=fastlms3ref(alpha,M,u,d,gamma,P,W,U);%@
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Input arguments:
        % alpha =step size, dim 1x1
        % M =filter length, dim 1x1
        % u =input signal, dim Nx3
        % d =desired signal, dim Nx1
        % gamma =forgetting factor, dim 1x1
        % P =initial value, energy, dim 2Mx3
        % W =final filter vector from previous iteration, dim 2*Mx3 only last half is used
        % U = previous input reference channel from previous multi-block, dim Mx3
        %
        % Output arguments:
        % e =estimation error, or filtered signal dim Nx1
        % P =output value, energy, dim 2Mx3
        % w =final filter vector, dim 2*Mx3 only last half is used
        data_chan(:,chan) = E;
        P_chan(:,:,chan) = p;
        W_chan(:,:,chan) = w;
    end %chan
    U = u(end-M+1:end,:);%@for in between block transition
    %sqdwrite goes here
    putdata(info,dest,'Action','Append','Data',data_chan);
    waitbar(blk/N,h);
end %blk

%append last fraction of a block if there is any
if blk_ub < num_samples,
    data = sqdread(srce,'Channels',[0:tot_num_chan-1],'Samples',[blk_ub+1 num_samples]);
    num_samp_fract = num_samples - blk_ub;
    num_app_zeros = blk_len - num_samp_fract;
    data_chan = [data; zeros(num_app_zeros,tot_num_chan)];
    u = data_chan(:,158:160);
    for chan = 1:157,
        P = P_chan(:,:,chan);
        W = W_chan(:,:,chan);
        d = data_chan(:,chan);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        [E,p,w]=fastlms3ref(alpha,M,u,d,gamma,P,W,U);%@
        data_chan_f(:,chan) = E(1:num_samp_fract,:);
    end %chan
    %sqdwrite goes here
    data_chan_f(:,158:160) = u(1:num_samp_fract,:);
    data_chan_f(:,161:tot_num_chan) = data(1:num_samp_fract,161:tot_num_chan);

    putdata(info,dest,'Action','Append','Data',data_chan_f);
end
close(h);
deltat = etime(clock,t0)
infoc.processtime = deltat;
filtinfo = infoc;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [E,p,w]=fastlms3ref(alpha,M,u,d,gamma,P,W,U);%@
% [e,p,w]=fastlms_3ref(alpha,M,u,d,gamma,P,W,U);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input arguments:
% alpha =step size, dim 1x1
% M =filter length, dim 1x1
% u =input signal, dim Nx3
% d =desired signal, dim Nx1
% gamma =forgetting factor, dim 1x1
% P =initial value, energy, dim 2Mx3
% W =final filter vector from previous iteration, dim 2*Mx3
% U = previous input reference channel from previous multi-block, dim Mx3
%
% Output arguments:
% e =estimation error, or filtered signal dim Nx1
% P =output value, energy, dim 2Mx3
% w =final filter vector, dim 2*Mx3 only last half is used
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% initialization
N=length(u);
E=d;%Initially, the recovered signal is set to observed signal
zero1 = zeros(M,1);
zero3 = zeros(M,3);
twoM = 2*M;
% no.of blocks
Blocks=N/M;
ref = [1:3];
for k=0:Blocks-1%@
    if k>0,
        Uvec=fft([u((k-1)*M+1:(k+1)*M,ref)],twoM);
    else%in between multiblock
        Uvec = fft([U;u(1:M,:)]);%concatenate last block from previous multiblock, and first block from current multiblock
    end
    yvec=real(ifft(Uvec(:,ref).*W(:,ref)));
    yvec2=yvec(M+1:twoM,ref);
    % block k; desired and error signal
    dvec=d(k*M+1:(k+1)*M);
    E(k*M+1:(k+1)*M,1)=dvec-sum(yvec2(:,ref),2);
    % FFT of estimation error
    Evec=fft([zero1;E(k*M+1:(k+1)*M)],twoM);
    % estimated power
    P=gamma*P(:,ref)+(1-gamma)*abs(Uvec(:,ref)).^2;
    % block k, inverse of power
    Dvec=1./P(:,ref);
    % estimated gradient
    phivec=ifft(Dvec(:,ref).*conj(Uvec(:,ref)).*[Evec Evec Evec],twoM);
    phivec=phivec(1:M,ref);
    % update of weights of filter coefficients
    W=W+alpha*fft([phivec;zero3],twoM);
end % no.of blocks
w=W;%save filter coefficient(fourier domain) to use in the next multiblock
p = P;
