function [w,s] = sep(x)


%gives back [w,s] and uses the ecg tests

w=relnewton(x,1e-2);
s=w*x;







function [W,res]=relnewton(X,lam,niter,normg_tol,method)
% Relative Newton method for ICA 
%
% Use:
%
% W=relnewton(X)
% W=relnewton(X,lam)
% W=relnewton(X,lam,niter)
% W=relnewton(X,lam,niter,normg_tol)
% W=relnewton(X,lam,niter,normg_tol,method)
%[W,res]=relnewton(X,lam,niter,normg_tol,method)
%
%
%Input:
%
%  X         - matrix with the mixed signals (row-wise)
%
%  lam       - parameter of nonlinearity, default: lam=1e-2
%
%                 lam > 0 - use  smoothing of the absolute value function with parameter lambda,
%                                becomes more sharp and accurate  when lam -> 0;
%                                recommended for sparse/sepergaussian sources 
%                                (see the paper newt_ica_ica2003.pdf)
%
%                 lam < 0 - use  the power function |s|^|lam| (lam= -4 ... -20),  
%                                recommended for subgaussian sources
%                
%  niter     - Maximum number of Newton steps. Default: 200
%
%  normg_tol - norm of the gradient for the stopping criterion. Default: 1e-5
%
%  method    - 'fastnwt' - fast relative Newton (default)
%              'relnwt'  - standard relative Newton
%              'natgrad' - natural gradient (batch mode)
%              'diagprecond' -  diaginally preconditioned natural gradient (batch mode)
%
%Output:
%
%  W   - separating matrix:   recovered sources S = WX
%  res - report of the iterative process

%%%%%%%%%%%%%%%%%%%%%%%%  by Michael Zibulevsky, version: 30.10.2002 %%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%  by Michael Zibulevsky, version: 03.12.2003 %%%%%%%%%%%%%%%%%%



pcaflag= 0;   % 1 - use PCA preprocessing (accelerates convergence, but you can loose 
              %                            superefficiency for sparse sources !!!)

armijoflag=1; % 1 - armijo step (recommended), 0 - cubic linesearch


fghpar.penpar=lam;  % parameter of smoothing of absolute value function
fghpar.pentype='smooth_abs'; % Type of non-linearity
fghpar.fghname='fghd_lagr'; % Don't ask about this parameter: just write

fghname=fghpar.fghname;

  
  if nargin <2
    lam=1e-2;      % smoothing of the absolute value function with parametre lambda
  end


  if nargin <3
    niter=200;      % Maximum number of newton steps
  end
  
   if nargin <4
     normg_tol=1e-4; % norm of the gradient for the stopping criterion.
   end
   
   if nargin <5
     method='fastnwt';
   end
   
 
  [Nsn,TotT]=size(X);
  
  I=eye(Nsn);
  W=I;
 
  fold=1e10;
  normg=1e10;
  %res.df=[];
  res.normg=[];
  res.dw=[];
  res.f=[];
  res.time=[];
  time0=cputime;
  
  if pcaflag
    [X,B]=mypca(X);  % normalized principal components X=B*X
  else
    B=eye(Nsn);
  end
  

  for i=1:niter   %%%%%%%%%%%% Optimization loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     U=W*X;
     w=I(:);
     
     if strcmp(method,'relnwt')  %%%%%%%%%%%%%%%% STANDARD RELATIVE NEWTON %%%%%%%%%%
         [f,g,hess_diag,hess]=feval(fghname,w,U,fghpar); 
         %[f,g,hess]=fgh_stand(w,U,lam);   %hess
         %[f,g,hess_diag,hess]=fghdiag(w,U,lam);   %Approximate hess: diag + logdet''
         
         if 1   
             [CholFact,D]=mcholmz3(full(hess)); CholFact=CholFact'; 
             d= cholsub(CholFact,-g,D); % Cholessky substitution
         else
             d=zeros(Nsn^2,1);
             for j=1:Nsn      % Solve block-diagonal Newton system
                 k0=(j-1)*Nsn; kk= [(k0+1):(k0+Nsn)];
                 d(kk)= -hess(:,:,j)\g(kk);
             end
         end

     elseif strcmp(method,'fastnwt')  %%%%%%%%%%%%%%% FAST RELATIVE NEWTON %%%%%%%%%%%%%%%%%%
        
         [f,g,hess_diag]=feval(fghname,w,U,fghpar); 
         
         %%%%%%%%%% Fast solution of Newton matrix equation  D*X + X' = G %%%%%%%%%%
         
         D=reshape(hess_diag,Nsn,Nsn)';
         G=reshape(g,Nsn,Nsn)';
         dd=fast_newt_dir(D,-G); d=dd'; d=d(:);
         
         %dd= -(G.*D'-G') ./ (D.*D'-ones(Nsn)); d=dd'; d=d(:);
         % D.*dd+dd'+G; d1= -hess\g; dmd1=d-d1, keyboard
     
     elseif strcmp(method,'natgrad') 
        [f,g]=feval(fghname, w,U,fghpar);   %hess
        d=-g;
     elseif strcmp(method,'diagprecond') 
        [f,g,hess_diag]=feval(fghname, w,U,fghpar); 
        d= -g./(hess_diag+1); % Preconditioned gradient descent direction
     else   
        error('Unknown optimization method') 
     end

      
     tbest=1;
     
     if normg > 1e-5 | strcmp(method,'natgrad') %%%%%%%% Line search %%%%%%%%%%%%%%%%%

       if armijoflag, %% Armijo step (recommended)
	 
	      [wnew,fnew,gradnew,tbest]= armijostep(fghname,w,f,g,d,0.3,0.3, U,fghpar);
       
       else  %%%  cubic line search

	     b1=0;b2=1;EpsGold=0.4; dftol=1e-3; dxtol=1e-3; nlinsrchmax=10;
         [wnew,fnew,gradnew,tbest,resEpsGold,b1,b2] = cublinsrch1(fghname,...
             w,f,g,d,b1,b2,EpsGold,dftol,dxtol,nlinsrchmax,  U,fghpar);
       end

     else  % make full step when newton method is close to solution
       
       wnew=w+d;  
       [fnew,gradnew] = feval(fghname,wnew,U,fghpar);   % new func. and grad
     
     end  %%%%%%%%%%%%%%%%%%%%%%%% end of Line search %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
     %fprintf('tbest=%g\n',tbest);
      
     Wtmp=reshape(wnew,Nsn,Nsn)';

     GRAD=reshape(gradnew,Nsn,Nsn)';
     %NATGRAD=GRAD*Wtmp'*Wtmp;

     W=Wtmp*W;  % Update separation matrix
     
     %WW=W'; f0=feval(fghname,WW(:),X,fghpar);  df=fold-f0; fold=f0;   res.df=[res.df;df];

     
     dW=Wtmp-I;
     maxdW=max(abs(dW(:)));
     normg=norm(gradnew);
     res.normg=[res.normg;normg];
     res.dw=[res.dw;maxdW];
     res.f=[res.f;fnew];
     res.time=[res.time;cputime-time0];
    % fprintf('iter %d: f=%g  normg=%.2e,  dW=%.2e    ',i, fnew,normg,maxdW);

    % figure(200); semilogy([1:i],res.normg,'*'); %,[1:i],res.dw,'o'); 
     %title('Relative Newton iterations: gradient norm'); xlabel('Iteration'); drawnow
      
     if normg < normg_tol, break; end
     
     if i==niter, fprintf('WARNING: MAXIMAL NUMBER OF ITERATIONX EXEEDED IN RELNEWTON.M !!!');end
     
  end    %%%%%%%%%%%% end of optimization loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
  W=W*B; % Account for the PCA, which was done  at the beginning
    
return


%%%%%%%%%%%%%%%%%%%%%%% TEST %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%% Generate the data %%%%%%%%
N=3;
T=1000;
A=rand(N);
S=sprandn(N,T,0.5);
X=A*S;

%%%%%% Setup the parameters

lam=1e-1; % parameter of smoothing of the absolute value function
niter=100
normg_tol=1e-12;
%method='relnwt';  
method= 'fastnwt'; 


%%%%%%% Separate with the relative NEWTON %%%%%%%%
W=relnewton(X,lam,niter,normg_tol,method)

%%%%%%%% COMPUTE QUALITY OF SEPARATION
WA=W*A
ISR=max(sep_quality(WA))


%165, 184, 186---changed







function [x,f,grad,t] = armijostep(fg,x0,f0,grad0,dx,alpha,beta,varargin)

%armijostep - Descent of the function f(x) in direction dx:
%
%                     f(x0 + t*dx) <= f(x0) - alpha*|f'_dx (x0)|

%CALLED AS:
%==========
%
%        [x,f,grad,t,res_alpha] = armijostep(fg,x0,f0,grad0,dx,alpha,beta,varargin)
%
%INPUT:
%======
%
% fg    - name of matlab function, which computes function f and 
%         gradient grad at the point x:
%
%                   [f,grad]=fg(x,varargin)
%
%                   varargin - an arbitrary number of additional arguments   
%          
%
% x0    - initial value of the optimized variable x
% f0    - value  of the optimized function at the point x0
% grad0 - value  of the gradient at the point x0
% dx    - direction of search
% alpha - required descent rate:  f(x0 + t*dx) <= f(x0) - alpha*|f'_dx (x0)|
% beta  - factor of step decreasing: t_new = beta*t_old
%
% varargin - an arbitrary number of additional arguments,which are 
%            passed to fg(...)
%
%OUTPUT:
%=======
% x          - resulting x
% f          - resulting f(x)
% grad       - resulting gradient of f(x)
% t          - resulting search parameter: x = x0 + t*dx


  %Some parameters:
 
  nitermax=30;   % - max number of iterations
  t=1;           %  - initial stepsize


  df0=grad0'*dx; 
  if df0>0, 
    disp('Armijo search: Direction dx is of increase, changing it to the opposite !!!');
    dx=-dx; df0=-df0; 
  end

  x=x0+t*dx;
  [f,grad] = feval(fg,x,varargin{:});
  df=grad'*dx; 
  %if df<=0, return; end
    
  for i=1:nitermax,
    %fprintf('armijostep ');keyboard
    
    if (f <= f0 + alpha*df0*t) | ((df < 0) & (f < f0 + 1e-9 * max(1,abs(f0)))), return; end;
    %    if (f <= f0 + alpha*df0*t) | (df<0 & f<f0), return; end;
  
    %fprintf('-');
    t= beta*t;
    x=x0+t*dx;
    [f,grad]=feval(fg,x,varargin{:}); 
    df=grad'*dx; 
    %if t<1e-6, fprintf('\n Armijostep.m); keyboard,end
  end    

  if f>f0, disp('Armijostep.m : f>f0 !!!');keyboard;end

return

%%%%%%%%%%%%%%% Garbage
ff=[];
ttt=[-1:0.01:1];
for tt= ttt
    x=x0+t*dx;
    ff=[ff  feval(fg,x,varargin{:})];
end
%figure;plot(ttt,ff);
%85,66---changed




function X=fast_newt_dir(D,B),

%%%%%%%%%% Fast solution of Newton matrix equation  D .* X + X' = B %%%%%%%%%%
%          using eigenvalue decomposition of 2x2 matrices                         
delta=1e-8;

normB=norm(B(:)); %% Scale to avoid too small/large numbers
B=B/normB;

b1=B;
b2=B';

a11=D;
a22=D';

Tr=a11+a22;    % Trace
dt=a11.*a22-1; % Determinant

T2= Tr/2;
sq=sqrt(T2.^2-dt);
lam1=(T2-sq);
lam2=(T2+sq);

ss=lam1-a11;

norms=sqrt(1+ss.^2);

s11=1./norms;
s21=ss./norms;
s12=s21;
s22=-s11;

c1=(s11.*b1+s21.*b2)./max(abs(lam1),delta);
c2=(s12.*b1+s22.*b2)./max(abs(lam2),delta);
%c1=(s11.*b1+s21.*b2)./abs(lam1);
%c2=(s12.*b1+s22.*b2)./abs(lam2);
%c1=(s11.*b1+s21.*b2)./lam1;
%c2=(s12.*b1+s22.*b2)./lam2;

x1=s11.*c1+s12.*c2;
%x2=s21*c1+s22*c2

X=x1;

% Diagonal should be treated separately
[n,n]=size(D); idiag=[1:(n+1):n^2]';
X(idiag)=B(idiag)./(D(idiag)+1);

lam1(idiag)= D(idiag)+1;
lam2(idiag)=lam1(idiag);


lammin=min([lam1(:);lam2(:)]);
lammax=max([lam1(:);lam2(:)]);
%fprintf(' eigval=%.2g _ %.2g     ',lammin,lammax)
%if lammin < delta, fprintf('\nNegative or small eig. value of the Hessian: lammin=%.2e\n',lammin);end
%keyboard

X=X*normB;  %% Scale back to avoid too small/large numbers
%56 ----changed




function [f,df,d2f]=psi(t,c),

%                 c > 0 - use  smoothing of the absolute value function with parameter 'c'
%                              become more sharp and accurate  when c->0
%                                recommended for sparse/sepergaussian sources 
%                                (see the paper newt_ica_ica2003.pdf)
%
%                  c < 0 - use  the power function |t|^|c| (lam= -4 ... -20),  
%                                recommended for subgaussian sources


if c<0,          % the power function 
  c=-c;  
  f=t.^c;
  df=c.*t.^(c-1);
  d2f=c.*(c-1).*t.^(c-2);

else              %smoothing of the absolute value function 

  tt=(1./c).*t;
  u = abs(tt);
  f= c.*(u-log(1+u));
  df=tt./(1+u);
  d2f= (1/c)./ (1+u).^2;

end

return


%%%%%%%% Test %%%%%%%%%%%%%
figure; c=-10; x=[-1.1:0.1:1.1]';[f,df, d2f]=psi(x,c);plot(x,[f,df, d2f]);grid
eps=1e-7; [f1,df1, d2f1]=psi(x+eps,c);
dfnum=(f1-f)/eps;
d2fnum=(df1-df)/eps;
err_df=(df-dfnum)', err_d2f=(d2f-d2fnum)'
figure;plot(x,[d2f, d2fnum]);grid



function [f,grd,hess_diag,h]=fghd_lagr(w,X,fghpar)

  mu= 1e0;
  
  penpar=fghpar.penpar;
  pentype= fghpar.pentype;
  
  [Nsn,T]=size(X);
  W=reshape(w,Nsn,Nsn)';
  S=W*X;
  A=inv(W);

  if strcmp(pentype,'boxconstr'), % Quadr-log penalty with multipliers
      lagrmult=fghpar.lagrmult;
      [h1,dh1,d2h1]=penfun(-1-S,penpar,lagrmult(1:Nsn,:)); 
      [h2,dh2,d2h2]=penfun(S-1,penpar,lagrmult((Nsn+1):end,:)); 
      h=h1+h2; dh=dh2-dh1; d2h=d2h1+d2h2;
  elseif strcmp(pentype,'smooth_abs')
      [h,dh,d2h]=psi(S,penpar); % Smoothing abs value function
  elseif strcmp(pentype,'smooth_lq')
      lagrmult=fghpar.lagrmult;
      [h,dh,d2h]=phimax_lq(S,penpar,lagrmult,-1,1); % Smoothing abs value function
  else
      error('unknown penalty function');
  end
      
      
  f= -log(abs(det(W))) + (mu/T)*sum(h(:));
 
  if nargout > 1   %%%%%%%%%%%%%%% compute gradient 
    grd= -A' + (mu/T)*dh*X';

    %natgrd=grd*W'*W;
    %natgrd=natgrd(:);

    %gnat=grd*W'*W; gnat=gnat'; gnat= gnat(:);  %Natural gradient
    
    grd=grd'; grd= grd(:);
    
  end

  %hess_diag=[];
  if nargout > 2  %%%%%%% compute hessian diagonal for W=I
      XX=X.^2;
      %for i=1:Nsn
      %    hess_diag=[hess_diag;(mu/T)*XX*d2h(i,:)'+1];
      %end
      hess_diagM=(mu/T)*XX*d2h';  hess_diag=hess_diagM(:);
  end
  if nargout > 3  %%%%%%% compute hessian 
      A=eye(Nsn);
      %h = diag(hess_diag);
      h = zeros(Nsn^2);
      k=1;
      for j=1:Nsn
         for i=1:Nsn
             tmp=A(:,j)*A(i,:);
             h(k,:)=h(k,:)+tmp(:)';
             k=k+1;
         end
      end
      
     k=1;
     tt=1:k:T; T1=length(tt);
     ind=[1:Nsn];
     for i=1:Nsn
         Xd2h_i=mulmd(X(:,tt),(mu/T1)*d2h(i,tt));
         %hess(:,:,i)=Xd2h_i*X(:,tt)' + I;
         hh =Xd2h_i*X(:,tt)';
         h(ind,ind)=h(ind,ind)+hh; 
         %hess(ind,ind)=hess(ind,ind)+diag(diag(h)); %%%%%% for testing only!!! 
         ind=ind+Nsn;
     end

      
      
  end
      
      
  return

  
  
  
  
  function [w,s] = sepplot(test)


%gives back [x,w,s] and the plots using the ecg tests
[m,n]=size(test);
x=[test(:,1:n)];
w=relnewton(test,1e-2);
s=w*x;

 plot4sig(s(:,1:2000),'s');

% plot2(s(:,1:2000),'s');
  
  





function [checks,vari,kur]=checks(test)
%varis a vector and so is kur
%returnes the vector checkof 0, 1 for the rows of noise (0)
[m,n]= size(test);

for in= 1:1:m
      for i = 1:1:n/500

     a(in,i)= var(test(in,(500*(i-1)+1):(500*i)),1);
    
 end

b(in)=var(a(in,:),1);

 %insted we shoud put the correct var
fourth_moment=(1/n)*sum(test(in,:).^4);
second_moment=(1/n)*sum((test(in,:).^2));
c(in)=abs(fourth_moment-3*((second_moment)^2));





if (b(in)<0.5)&(c(in)>5)
    checks(in)=1;
else
    checks(in)=0;
end
end
vari=b;
kur=c;



  
  
  function [wc,xc]= correctx(w,s,check)
%corrects w &x using check ( [wc,xc]= correctx(w,s,check))
n =length(check);
winv=inv(w);
for i = 1:1:n
    if check(i)==0
        winv(:,i)=0   ;     
    end
end

wc=winv;
xc=wc*s;

end





function [checks,vari,kur]=checks(test)
%varis a vector and so is kur
%returnes the vector checkof 0, 1 for the rows of noise (0)
[m,n]= size(test);

for in= 1:1:m
      for i = 1:1:n/500

     a(in,i)= var(test(in,(500*(i-1)+1):(500*i)),1);
    
 end

b(in)=var(a(in,:),1);

 %insted we shoud put the correct var
fourth_moment=(1/n)*sum(test(in,:).^4);
second_moment=(1/n)*sum((test(in,:).^2));
c(in)=abs(fourth_moment-3*((second_moment)^2));





if (b(in)<0.5)&(c(in)>5)
    checks(in)=1;
else
    checks(in)=0;
end
end
vari=b;
kur=c;




function  plot2(test,tit)
% makes seperate plots of the signals un the rows of test
colordef white;
figure;
[m,n]=size(test);
     subplot(2,1,1);
    plot([0.002:0.002:n/500],test(1,:));
    title([tit,'1'] )   ;
 xlabel('t(sec)');
    ylabel('V(mV)');
       
        
      subplot(2,1,2);
    plot([0.002:0.002:n/500],test(2,:));
    title([tit,'2']);
    xlabel('t(sec)');
    ylabel('V(mV)');
end
end





function  plot4(test,tit)
% makes seperate plots of the signals un the rows of test
[m,n]=size(test);
 figure;
for in=1:1:m 
  if mod(in,4)==0
     subplot(4,1,4);
    plot(1:n,test(in,:));
    title([tit,num2str(in)] )   ;
        figure;
    else
        
    subplot(4,1,mod(in,4));
    plot(1:n,test(in,:));
    title([tit,num2str(in)]);
    
end
end





function Xn=addwhitenoise(X);
Xn=X;
for i=1:2:100
    Xn=adnoise(Xn,i);
    
end




function testn=adnoise(test,f)
[M,N]=size(test);

% figure
% plot(test(1,:))

 I=1:1:N;
    n(I)= 200*sin(2*pi*f/500*I);


testn = test+repmat(n,M,1);
% figure
% plot(testn(1,:))
% end



function freqres=freqres(dft)
%calculates the frequency response freqres=freqres(dft,wmid)
[M,N]=size(dft);
H=freqz(fir1(60,[0.2,0.8]),1,N);%the filter freq. response

for i=1:1:M             %multipling
    freqres(i,:)=dft(i,:).*H';
end




% example for filltering with random noise
figure;                                                                                                                  % for all kinds of noises the frequncy that  
                                                                                                                                   %   the filter will give good results depands 
                                                                                                                                   %on the frequncy of the signal
subplot(2,2,1)
x=0:0.002:0.002*(length(s1)-1);
t=0:0.002:0.002*(length(s1)-1);
s2= s1'  +randn(length(s1),1);                                             %here we put the kind of noise we want
s15=s2;                                                                                         % in this case - white noise
plot(x,s15);
title('normal ecg with white noise  ');
xlabel('time[sec]');
ylabel('mv');


z=ones(2000,1);
  
s2=s15+z;
s5=x;
x=fft(s2);
s4=fftshift(x);
s6=s4';

s6=abs(s6);
x=abs(x);

subplot(2,2,2);
f=500*(1:2000)/2000;

plot(f,x);
xlabel('frequency [hz]');
ylabel('fft');
title('fft normal ecg with noise' );

axis([0 500 0 2500])



s4=fil(s4,2000,[0.45 0.55]);     %[0.45 0.55]
s5=s4;

s4=abs(x);

f=500*(1:2000)/2000;
s4=s4';


x=ifftshift(s5);

x=ifft(x);
x=abs(x);

z=z'
s3=x-z;
subplot(2,2,3);
x=0:0.002:0.002*(length(x)-1);
plot(x,s3);
title(' normal ecg after fiter');
xlabel('time [sec]');
ylabel('mv');