function [V,Z,js,s]=rksm_tan_lyap_amg_Mdir(A,l1,l2,B,m,tol,iter,n_tang,ckres)
%function [V,Z,js,s]=rksm_tan_lyap_amg_Mdir(A,l1,l2,B,m,tol,iter,n_tang,ckres)
%      
% Basis and representation matrix of rational Krylov space
% A   coeff. matrix
% l1,l2  estimates of spectral interval of (A+A')/2
% B    rhs matrix
% m    max space dim  (e.g. m=500)
% tol  stopping tolerance  (e.g. tol=1e-6)
% iter  0/1  if  iter=1 inner *sym* systems solved iteratively
% n_tang  0/1 number of new tangential directions. n_tang=1 single direction
% ckres   0/1  . ckres=1, Lanczos estimate of 2-norm, ckres=0 true F-norm

ttime=cputime;
[n]=size(A,1);

if (norm(A-A',1)<1e-12),sym=1;else,sym=0;end

%[ss,d]= newpole(A,V,H,B,l1,l2);
ss=l1; %mean([l1,l2]);
k=size(B,2);
%d=randn(k,1); d=d/norm(B*d);
D=[];
s=ss;
E=speye(n);

 [V(:,1:k),beta2]=qr(B,0);
s=zeros(1,k);
s_all=zeros(1,k);
[qqres,rb]=qr([A*B,V(:,1:k)],0);
%V(:,1)=V(:,1)/norm(V(:,1));
k=size(V,2);
Bm(1:k,:)=V(:,1:k)'*B;

fprintf('    space dim    res.norm   new dir vecs\n')
H=sparse(k*m+2,k*m+1);
K=V'*A*V;
WH=rb*[speye(k);-K];
ind=[]; err=[];
nrmrestot=[];
    if (iter & sym)
      tol_inner=1e-10;
%MI20
      addpath('/nethome/valeria/Software/MI20/hsl_mi20d/matlab')
      javaaddpath('/nethome/valeria/Software/MI20/hsl_mi20d/matlab')
    end



%Res=A*V(:,1:k)-V(:,1:k)*K(1:k,1:k);

%if (norm(A-E-(A-E)',1)<1e-14), symm=1; else symm=0;end
i=k-1;
% additional steps
res=zeros(m,1);
while i < m
  i=i+1;

    i1=i+1; t=0.;
    w=V(1:n,i); %w=EL*V(:,i);
    [snew,d]= newpole(WH,K,Bm,l1,l2,s,n_tang);
    s=[s,snew];
    s_all=[s_all,snew*ones(1,size(d,2))];
% d can be a matrix
    D=[D,d];
    if (iter & sym)
     %LA=cholinc(A-snew*E,1e-2);
     %wrk=-pcg_amg(-(A-snew*E),B*d,x0,n,tol_inner);
     hsl_mi20_startup
     control = hsl_mi20_control;
     control.one_pass_coarsen=1;
     inform = hsl_mi20_setup(-(A-snew*E),control);
     wrk=-PBCG_amg(-(A-snew*E),B*d,0*(B*d),n,tol_inner,inform); 
     hsl_mi20_finalize;
    else
      wrk = (A-snew*E)\(B*d);   %V(:,1)=L'*((A-s1*E)\(L*V(:,1)));
    end

    lwrk=size(wrk,2);
% orthogonalize twice
    for it=1:2,
      for j=1:i
        t = V(1:n,j)'*wrk;
        H(j,i:i+lwrk-1)=H(j,i:i+lwrk-1)+t;
        wrk=wrk-V(1:n,j)*t;
      end;
    end;
   [V(1:n,i+1:i+lwrk),H(i+1:i+lwrk,i:i+lwrk-1)]=qr(wrk,0); %QR coefficient, to define rr

gk1=A*V(:,i+1:i+lwrk);
K1=V(:,1:i)'*gk1; K2=V(:,i+1:i+lwrk)'*A*V(:,1:i+lwrk);
%K1=V(:,1:i)'*(A*V(:,i+1:i+lwrk)); K2=V(:,i+1:i+lwrk)'*A*V(:,1:i+lwrk);
K=[K, K1;K2];
Bm(i+1:i+lwrk,:)=V(1:n,i+1:i+lwrk)'*B;
rr=full([ [beta2;sparse(i+lwrk-k,k)] H(1:i+lwrk,k:i+lwrk-1)]); %rr=V(1:n,1:i+lwrk)'*Kspace;

%[qqres,rb]=qr([A*B,V(1:n,1:i+lwrk)],0);
%orthogonalize twice
G1=qqres'*V(1:n,i+1:i+lwrk);
VG=V(1:n,i+1:i+lwrk)-qqres*G1;
gg=qqres'*VG;G1=G1+gg;
VG=VG-qqres*gg;
[qq,G2]=qr(VG,0);
qqres=[qqres,qq]; 
%rb=[rb, G1;0*G1' G2];
rb=[rb, G1;zeros(lwrk,size(G1,1)) G2];
i1=i+lwrk;i=i1-1;

bbm=Bm*Bm';
%Y = lyap1(K(1:i1,1:i1),bbm);      %Y = lyap1(K(1:i1,1:i1),Bm*Bm');
Lt=lyapchol(K(1:i1,1:i1),Bm);Y=Lt'*Lt;
js=i1;

%S=spdiag(s_all);
S=spdiags(s_all', 0, length(s_all), length(s_all));
P1=blkdiag(speye(k),Bm*D) + [sparse(k,size(rr,2));rr*S];
P2=[sparse(k,i1);speye(i1)];
WW = rb*[ P1/rr, P2];
WH=WW*[speye(i1);-K(1:i1,1:i1)];
if ckres
  %FF=WW*[sparse(i1,i1), Y;Y,bbm]*WW';
  res(i)=myres2_lan( WW,[sparse(i1,i1), Y;Y,bbm],0);
else
  res(i)=norm(WW*[sparse(i1,i1), Y;Y,bbm]*WW','fro');
end
res(i)=res(i)/norm(beta2,'fro')^2;

disp([i,res(i),size(d,2)])
if res(i)<tol,break,end
%pause
end;

% factored solution 
[uY,sY]=eig(Y); [sY,id]=sort(diag(sY));
sY=flipud(sY); uY=uY(:,id(end:-1:1));
%is=sum(abs(sY)>sqrt(eps)*max(abs(sY)));   %relative
is=sum(abs(sY)>1e-15);  % absolute
Y0 = uY(:,1:is)*diag(sqrt(sY(1:is)));
Z = V(:,1:size(Y0,1))*Y0;
%Z = EU\(VV(:,1:size(Y0,1))*Y0);
final_rank=is;
RKStotal_time=cputime-ttime;
js=size(Y0,1);


fprintf('Space dim %d  Solution rank %d  time %d \n',size(V,2),is,RKStotal_time);

%X=Z*Z';
%R=A*X+X*A'+B*B';
%nrmx=norm(X,'fro');
%norm(R,'fro')/(nrmb+singE*nrma*nrmx)
fprintf('Poles computed during the iteration\n')

%figure(1)
%semilogy(res,'mo-')
%xlabel('space dimension')
%
%figure(10)
%plot(real(s),imag(s),'x')
%s.'
return
end



function [s,d]=newpole(WH, H,Bm,l1,l2,ss,n_tang)

m=size(H,1);
%maxs=(abs(log10(l1))+abs(log10(l2)))*100;
maxs=200;
s_test=logspace(log10(l1),log10(l2),maxs);

I=speye(m);
[Qh,Rh]=schur(H(1:m,1:m));
Rh=sparse(Rh);
Bmr=Qh'*Bm;
WHQh=WH*Qh;
val=zeros(maxs,1);
 	for k=1:maxs
 	    val(k) = norm(full(WHQh*(  (Rh-s_test(k)*I)\Bmr)),'fro' ); %val(k) = norm(Rrr*(  (Rh-s_test(k)*I)\Bmr) );
 	   %val(k) = norm(WHQh*(  (Rh-s_test(k)*I)\Bmr) ); %val(k) = norm(Rrr*(  (Rh-s_test(k)*I)\Bmr) );
 	end
 	[maxvals,id]=sort(val,'descend'); s=s_test(id(1));


 jk=1;
 while any (abs(s-ss)<1e-15)  && jk <maxs,
   jk=jk+1;
   disp('double pole found'),
%  s=s+(l2-l1)*1e-12;
    s=s_test(id(jk));
 end

%if any (abs(s-ss)<1e-12), disp('double pole found'),s=s_test(id(2)),end
%   y = (H(1:m,1:m)-s*I)\Bm;
 [uu,ssv,vv]=svd(WHQh*(  (Rh-s*I)\Bmr));    
 if (n_tang==1)
     d=vv(:,1);
 else
     sn=sum(diag(ssv)/ssv(1,1)>1e-1);d=vv(:,1:sn);
 end
end



function [nrm,k,T,V] = myres2_lan(F,Y,orth)
%
%  function [nrm,j,H,V] = res2_lan(F,G,Z,maxiter,tol,rv,orth)
%
% Computes the 2 Norm of the Lyapunov residual of Z exploiting the
% symmetry of the residual operator
%
% R := F*Z*Z' + Z*Z'*F' + G*G'
%
% That means it computes the spectral radius of this operator by an
% Lanzcos iteration, exploitig the structure of F and rectangular
% structure of Z and G. Thus it can be computed in O(n) effort and
% is therefore much cheaper than the computation of the e.g. the
% Frobenius norm.
%
%  Input:
%
%    F,G,Z     Matrices F, G, Z defining the Lyapunov residual R
%    maxiter   maximal number of Arnoldi steps (usually k << n)
%              (optional - chosen as 50 if omitted)
%    tol       relative accuracy of the norm computation
%              (optional - chosen as eps if omitted)
%    rv        initial n-vector
%              (optional - chosen by random, if omitted).
%    orth      reorthogonalization flag.
%              (optional - switched off, if omitted).
%
%  Output:
%
%    nrm       the residual of the iterate X=Z*Z'
%    k         number of Lanzcos steps taken
%    T         matrix T ((k+1)-x-k matrix, symmetric tridiagonal);
%    V         matrix V (n-x-(k+1) matrix, orthogonal columns).
%
%  Method:
%
%    The Lanczos method produces matrices V and T such that
%
%      V(:,1) in span{rv},
%      V'*V = eye(k+1),
%      F*V(:,1:k) = V*T.
%
%  Remark:
%    V is only constructed if its required by the output arguments, or if
%    reorthogonalization is used.
% 
%    This implementation does not check for (near-)breakdown!
%


% Input data not completely checked!

na = nargin;


n = size(F,1);                 % Get system order.
if na<4||isempty(maxiter)
    maxiter = 50;
else
    if maxiter >= n-1,
        error('maxiter must be smaller than the order of A!');
    end
end
if na<5||isempty(tol), tol= 1e-6; end
if na<6||isempty(rv), rv = randn(n,1); end
if na<7||isempty(orth), orth=0; end

v1=zeros(n,1);
v2=v1;

T = zeros(maxiter+1,maxiter);

v1 = (1.0/norm(rv))*rv;

% initial step
% Matrix-vector product

w=F*(Y*(F'*v1));
%w=F*(Z*(Z'*v1))+ Z*(Z'*(F'*v1)) + G*(G'*v1);



T(1,1)=v1'*w;
r=w-T(1,1)*v1;
T(1,2)=norm(r);
v2=r./T(1,2);
T(2,1)=T(1,2)';

% store vectors in V if needed
if orth || nargout==4
    V = zeros(n,maxiter+1);
    V(:,1) = v1;
    V(:,2)=v2;
end

nrm = 0;


for k=2:maxiter-1;
    %Lanczos 3-term recursion 
    
    % Matrix-vector product
w=F*(Y*(F'*v2));
%   w=F*(Z*(Z'*v2))+ Z*(Z'*(F'*v2)) + G*(G'*v2);
    T(k,k)=v2'*w;
    r=w-T(k-1,k)*v1-T(k,k)*v2;
    
    %re-orthogonalization by MGS
    if orth
        for j=1:k,
            r = r - (V(:,j)'*r)*V(:,j);
        end
    end
    
    T(k,k+1)=norm(r);
    v1=v2;
    v2=r./T(k,k+1);
    if orth || nargout==4
        V(:,k+1)=v2;
    end
    T(k+1,k)=T(k,k+1)';
    nrmold= nrm;
    nrm = max(abs(eig(T(1:k,1:k))));
%           [S,L]=eig(T(1:k,1:k));
%           [nrm2,idx] = max(abs(diag(L)));
%           riw=L(idx,idx);
%           riv=V(:,1:k)*S(:,idx);
% %           plot(abs(riv))
% %           pause(0.2)
%           er=norm(F*(Z*(Z'*riv))+ Z*(Z'*(F'*riv)) + G*(G'*riv)-riw*riv);
% %           fprintf('%e, %e, %e, %e,%e\n',nrm, norm(F*(Z*Z')+ (Z*Z')*F'+ G*G'), er, abs(nrm-nrmold), tol*nrm)
%           fprintf('%e, %e, %e, %e,%e\n',nrm, nrm2, er, abs(nrm-nrmold), tol*nrm)
    if abs(nrm-nrmold)<tol*nrm
        break;
    end
end

