% [y,k, DAy] = loopPcg(L,Lden,At,dense,d, DAt,K, b,p,ssqrNew,cgpars, restol)
%
% LOOPPCG Solve y from AP(d)A' * y = b
% using PCG-method and Cholesky L as conditioner.
% If L is sufficiently accurate, then only 1 CG-step is needed.
% It assumes that the previous step was p, with
% ssqrNew = bOld'*inv(L*THETA*L')*bOld, and bOld the residual before
% the step p was taken. If p = [], then the PCG is started from scratch.
%
%  k = #(CG-iterations).
%  DAy = D*A'*y with D'*D = P(d).
%
% Warning: if the scaling operation P(d) gets ill-conditioned, the
% precision in y may be insufficient to compute DAy satisfactory.
% In this case, one should allow refinement, see wrapPcg.
%
% SEE ALSO sedumi, wrapPcg
% **********  INTERNAL FUNCTION OF SEDUMI **********
function [y,k, DAy] = loopPcg(L,Lden,At,dense,d, DAt,K, b,p,ssqrNew,...
    cgpars, restol)
 %  
 %   This file is part of SeDuMi 1.05
 %   Copyright (C) 2001 Jos F. Sturm
 %     Dept. Econometrics & O.R., Tilburg University, the Netherlands.
 %     Supported by the Netherlands Organization for Scientific Research (NWO).
 %   Affiliation SeDuMi 1.03 and 1.04Beta (2000):
 %     Dept. Quantitative Economics, Maastricht University, the Netherlands.
 %   Affiliations up to SeDuMi 1.02 (AUG1998):
 %     CRL, McMaster University, Canada.
 %     Supported by the Netherlands Organization for Scientific Research (NWO).
 % 
 %   This program is free software; you can redistribute it and/or modify
 %   it under the terms of the GNU General Public License as published by
 %   the Free Software Foundation; either version 2 of the License, or
 %   (at your option) any later version.
 % 
 %   This program is distributed in the hope that it will be useful,
 %   but WITHOUT ANY WARRANTY; without even the implied warranty of
 %   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 %   GNU General Public License for more details.
 % 
 %   You should have received a copy of the GNU General Public License
 %   along with this program; if not, write to the Free Software
 %   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 %

% --------------------------------------------------
% INITIALIZE:
%  y=0 with r = b.
% --------------------------------------------------
 k = 0;
 STOP = 0;
 r = b;
 finew = 0;
 y = [];        % means all-0
 normrmin = norm(r,inf);
 ymin = [];
% --------------------------------------------------
% Conjugate Gradient until convergence
% --------------------------------------------------
 while STOP == 0
% --------------------------------------------------
% P r e - C o n d i t i o n i n g:
% Solve L*Lr = r, L*THETA*tmp = r.
% p=[] ==> initialize p to solve L*THETA*L'*p = r.
% --------------------------------------------------
   Lr = fwdpr1(Lden,sparfwslv(L, r));
   tmp = Lr ./ L.d;
   if isempty(p)
     ssqrNew = Lr'*tmp;
     p = sparbwslv(L, bwdpr1(Lden,tmp));
   else
% --------------------------------------------------
% General iterate: make p conjugate to previous iterate(s):
% --------------------------------------------------
     ssqrOld = ssqrNew;
     ssqrNew = Lr'*tmp;
     p = (ssqrNew/ssqrOld) * p;
     p = p + sparbwslv(L, bwdpr1(Lden,tmp));
   end
% --------------------------------------------------
% SCALING OPERATION AND MATRIX*VECTOR.
% Let DDAp = P(d)*A'*p and ssqrDAp = ||P(d)^{1/2}*A'*p||^2.
% Set alpha = ssqrNew / ssqrDAp
% --------------------------------------------------
   Ap = vecsym(Amul(At,dense,p,1), K);
   [DDAp, DApq, DAps, ssqrDAp] = PopK(d,Ap,K);
   if ssqrDAp > 0.0
     k = k + 1;
% --------------------------------------------------
% Take step:  y := y + alpha*p
%--------------------------------------------------
     alpha = ssqrNew / ssqrDAp;
     if ~isempty(y)
       if isstruct(y)
	 [y.hi,y.lo] = quadadd(y.hi,y.lo,alpha*p);
       else
	 y = y + alpha * p;
       end
     elseif cgpars.qprec > 0
       y.hi = alpha * p; y.lo = zeros(length(p),1);
     else
       y = alpha * p;     
     end
% --------------------------------------------------
% Update residual r := r - alpha * A*[P(d)*Ap]. MATRIX*VECTOR.
% --------------------------------------------------
     tmp = Amul(At,dense,DDAp)+ DAt.q'*DApq(:,1) +...
         DAt.denq*DApq(dense.q,1);
     r = r - alpha * tmp;
% --------------------------------------------------
% Convergence check (HEURISTIC)
% --------------------------------------------------
     fiprev = finew;
     if isstruct(y)
       finew = (b+r)'*y.hi + (b+r)'*y.lo;
     else
       finew = (b+r)'*y;
     end
     normr = norm(r,inf);
     if normr < normrmin
       ymin = y;
       normrmin = normr;
     end
     if normr < restol
       STOP = 1;
     elseif (finew-fiprev < cgpars.stagtol * fiprev)
       STOP = 2;
     elseif k >= cgpars.maxiter
       STOP = 2;
     end
   else
%     fprintf('Warning: DAp = 0 in PCG\n');
     STOP = 1;         % If DAp == 0 then can't go on.
   end
 end
% --------------------------------------------------
% OUTPUT: return projection D * At*y on request.
% --------------------------------------------------
 if STOP == 2
   y = ymin;      % Take best so far
 end
 if isempty(y)
   DAy = [];
   return
 end
 if nargout >= 3
   if k == 1
     DAy = alpha*[sqrt(d.l).*Ap(1:K.l); asmDxq(d,Ap,K,DApq); DAps];
   else
     if isstruct(y)
       Ap = vecsym(Amul(At,dense,y.hi,1), K);
     else
       Ap = vecsym(Amul(At,dense,y,1), K);
     end
     DAy = [sqrt(d.l).*Ap(1:K.l); asmDxq(d,Ap,K); psdscale(d,Ap,K)];
     if isstruct(y)
       Ap = vecsym(Amul(At,dense,y.lo,1), K);
       DAy = DAy + [sqrt(d.l).*Ap(1:K.l); asmDxq(d,Ap,K); psdscale(d,Ap,K)];
     end
   end
 end
 if isstruct(y)
   y = y.hi;
 end