%% sqlp: solve an semidefinite-quadratic-linear program 
%%       by infeasible path-following method. 
%%
%%  [obj,X,y,Z,gaphist,infeashist,info,Xiter,yiter,Ziter] = ...
%%                     sqlp(blk,Avec,C,b,X0,y0,Z0,OPTIONS);
%%
%%  Input: blk: a cell array describing the block diagonal structure
%%              of SQL data.
%%        Avec: a cell array with Avec{p} = [svec(Ap1) ... svec(Apm)] 
%%         b,C: data for the SQL instance.
%%  (X0,y0,Z0): an initial iterate,
%%              (if it is not given, the default is used).
%%     OPTIONS: a structure that specifies parameters required in sqlp.m,
%%              (if it is not given, the default in parameters.m is used). 
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate. 
%%          gaphist = history of reduction in <X,Z>. 
%%          infeashist = history of reduction in the infeasibility measure. 
%%          info = [termination-code iters gap infeasibity CPUtime].
%%          (Xiter,yiter,Ziter): last iterates.
%%
%%----------------------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters: 
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol  
%%      maxit  printyes  scale_data
%%      (all have default values set in parameters.m).
%%
%%  vers = version of the direction to use. 
%%  gam  = steplength parameter
%%         [default: gam = 0, it is adaptively selected within the algorithm]. 
%%  gaptol = tolerance for duality gap as a fraction of the value of the 
%%         objective functions   
%%         [default 1e-8]
%%  inftol = tolerance for stopping due to suspicion of infeasibility
%%         [default 1e-13]
%%  steptol = tolerance for stopping due to small steps 
%%         [default 1e-6].
%%  maxit = maximum number of iteration allowed
%%         [default 50].
%%  printyes = 1, if want to display result in each iteration, else = 0
%%         [default 1].
%%  scale_data = 1, if want to scale the data before solving the problem, 
%%         else 0
%%         [default 0].
%%  randnstate = state to be used for matlab random normal generator
%%         [default 0]. 
%%
%%*****************************************************************************
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*****************************************************************************

  function [obj,X,y,Z,gaphist,infeashist,info,Xiter,yiter,Ziter] = ...
                              sqlp(blk,Avec,C,b,X0,y0,Z0,OPTIONS);
%%                                      
%%-----------------------------------------
%% get parameters from the OPTIONS structure. 
%%-----------------------------------------
%%
   global spdensity  iter  chol_options

   if (nargin <= 7); parameters; end; 

   warning off; 
   vers         = OPTIONS.vers; 
   gam          = OPTIONS.gam; 
   predcorr     = OPTIONS.predcorr; 
   expon        = OPTIONS.expon;
   gaptol       = OPTIONS.gaptol;
   inftol       = OPTIONS.inftol;
   steptol      = OPTIONS.steptol;
   maxit        = OPTIONS.maxit;
   printyes     = OPTIONS.printyes;
   scale_data   = OPTIONS.scale_data;
   randnstate   = OPTIONS.randnstate;
   spdensity    = OPTIONS.spdensity;
   rmdepconstr  = OPTIONS.rmdepconstr;
%%
   if isempty(gam) | isempty(predcorr) | ~all([expon gaptol steptol]);
      fprintf('\n The variable OPTIONS in ** parameters.m **');
      fprintf(' require default settings. \n'); 
      termcode = -10;
      break; 
   end;
   if all(vers-[1 2]); error('*** vers must be 1 or 2 ***'); end; 
%%
%%-----------------------------------------
%% convert matrices to cell arrays. 
%%-----------------------------------------
%%
   if ~iscell(Avec); Avec = {Avec}; end;
   if ~iscell(C);  C = {C}; end;
   if (size(Avec,2) == length(b)); 
      for p = 1:size(blk,1)
         tmp(p) = (size(Avec{p},1) ~= sum(blk{p,2}.*(blk{p,2}+1)/2));   
      end    
      if all(tmp)
         Avec = svec(blk,Avec,ones(size(blk,1),1)); 
      end
   end; 
   if (nargin <= 4) | (isempty(X0) | isempty(y0) | isempty(Z0)); 
      [X0,y0,Z0] = infeaspt(blk,Avec,C,b); 
   end
   X = X0; y = y0; Z = Z0;  
   clear X0 y0 Z0; 
   if ~iscell(X);  X = {X}; end;
   if ~iscell(Z);  Z = {Z}; end;
%%
%%-----------------------------------------
%% validate SDP data. 
%%-----------------------------------------
%%
   tstart = cputime; 
   [Avec,C,X,Z] = validate(blk,Avec,C,b,X,y,Z);
%%
%%-----------------------------------------
%% check if the matrices Ak are 
%% linearly independent. 
%%-----------------------------------------
%%
   [Avec,b,y,indeprows,feasible] = checkdeprows(blk,Avec,b,y,rmdepconstr);
%%
%%-----------------------------------------
%% scale SDP data. Note: must be done only 
%% after the computation of AAt
%%-----------------------------------------
%%
   normb2 = max(1,norm(b)); normC2 = max(1,ops(C,'norm')); 
   if (scale_data);
      [Avec,C,b,X,y,Z,normA,normC,normb] = scaling(blk,Avec,C,b,X,y,Z);
   else
      normA = 1; normC = 1; normb = 1;
   end;   
   normX0 = ops(X,'norm'); normZ0 = ops(Z,'norm');
%%
%%-----------------------------------------
%% find the combined list of non-zero 
%% elements of Aj, j = 1:k, for each k. 
%%-----------------------------------------
%% 
   [Avec,C,Z,permA,permZ] = sortA(blk,Avec,C,Z);
   [isspA,nzlistA,nzlistAsum,isspAy,nzlistAy] = nzlist(blk,Avec); 
   preproctime = cputime-tstart; 
%%
%%-----------------------------------------
%% initialization
%%-----------------------------------------
%%
   m = length(b); n = ops(C,'getM');
   rp = b-AXfun(blk,Avec,permA,X); 
   Rd = ops(C,'-',ops(Z,'+',Atyfun(blk,Avec,permA,isspAy,y)));
   obj = (normb*normC)*[blktrace(blk,C,X)  b'*y];
   trXZ = blktrace(blk,X,Z); 
   gap = (normb*normC)*trXZ; 
   mu  = trXZ/n;  
   rel_gap = gap/max(1,mean(abs(obj)));
   pri_infeas  = norm(rp)/normb2;
   dual_infeas = ops(Rd,'norm')/normC2;
   infeas_meas = max(pri_infeas,dual_infeas); 
   if scale_data
      pri_infeas_org  = norm(normA.*rp)*normb/normb2;
      dual_infeas_org = ops(Rd,'norm')*normC/normC2;
   end
   pstep = 0; dstep = 0; pred_convg_rate = 1; corr_convg_rate = 1;
   termcode = -6; 
   gaphist(1) = gap; 
   infeashist(1) = infeas_meas;
   maxblk = ops(blk(:,2),'max'); 
   pri_infeas_bad = 0;
phi = 1;
psi = 1;
%%
%%-----------------------------------------
%% display parameters, and initial info
%%-----------------------------------------
%%
   fprintf('\n********************************************');
   fprintf('***********************\n');
   fprintf('         Infeasible path-following algorithms'); 
   fprintf('\n********************************************');
   fprintf('***********************\n');
   fprintf(' version  predcorr  gam  expon  scale_data\n');
   if (vers == 1); fprintf('   HKM '); elseif (vers == 2); fprintf('    NT '); end
   fprintf('     %1.0f      %4.3f',predcorr,gam);
   fprintf('   %1.0f        %1.0f    %1.0f\n',expon(vers),scale_data); 
   if printyes;
      fprintf('\nit  pstep dstep p_infeas d_infeas  gap')
      fprintf('        obj       cputime\n');
      fprintf('------------------------------------------------');
      fprintf('-------------------\n');
      fprintf('%2.0f  %4.3f %4.3f %2.1e',0,0,0,pri_infeas);
      fprintf(' %2.1e  %2.1e %- 7.6e\n',dual_infeas,gap,mean(obj));
   end;
%%
%%---------------------------------------------------------------
%% start main loop
%%---------------------------------------------------------------
%%
   pcholtime = 0; dcholtime = 0; predtime = 0; pstep_predtime = 0;
   dstep_predtime = 0; corrtime = 0; pstep_corrtime = 0; dstep_corrtime = 0; 
   misctime = 0; 
   [Xchol,indef(1)] = blkchol(blk,X); 
   [Zchol,indef(2)] = blkchol(blk,Z); 
   if any(indef);  
      fprintf('\n Stop: X, Z are not both positive definite');
      termcode = -3;
      break;
   end; 
%%
   for iter = 1:maxit;  

       update_iter = 0;
       tstart = cputime;  
       time = zeros(1,11); 
       time(1) = cputime;
%%
%%---------------------------------------------------------------
%% predictor step.
%%---------------------------------------------------------------
%%
       if predcorr; 
          sigma = 0; 
       else; 
          sigma = 1-0.9*min(pstep,dstep); 
          if (iter == 1); sigma = 0.5; end; 
       end;
       sigmu = sigma*mu; 

       invZchol = ops(Zchol,'inv'); 
       if (vers == 1);
          [dX,dy,dZ,hRd,schur,solve_ok,UU,VV,Zinv,dd,ee,gamz2] = ...
           HKMpred(blk,Avec,rp,Rd,sigmu,permA,isspA,isspAy,nzlistA,...
           nzlistAsum,nzlistAy,X,Z,invZchol);
       elseif (vers == 2);
          [dX,dy,dZ,hRd,schur,solve_ok,UU,VV,W,G,sv,dd,ee,rr,vv,gamx,gamz] = ...
           NTpred(blk,Avec,rp,Rd,sigmu,permA,isspA,isspAy,nzlistA,nzlistAsum,...
           nzlistAy,X,Z,Zchol,invZchol);
       end;
       if (solve_ok <= 0)
          if (solve_ok == -2); 
             fprintf('\n   Schur complement matrix not positive definite');  
          elseif (solve_ok == -1)
             fprintf('\n   fail to solve the Schur complement equation by\n'); 
             fprintf('   using the Sherman-Morrison update'); 
          end
          CPUtime(iter) = cputime-tstart; 
          termcode = -4;
          break;
       end;
       clear Zchol; 
       time(2) = cputime;
       predtime = predtime + time(2)-time(1);
%%
%%-----------------------------------------
%% step-lengths for predictor step
%%-----------------------------------------
%%
      if (gam == 0); 
         gamused = 0.9 + 0.09*min(pstep,dstep); 
      else;
         gamused = gam;
      end; 
      invXchol = cell(size(blk,1),1); Zchol = cell(size(blk,1),1);
      [Xstep,Xchol,invXchol] = steplength(blk,X,dX,Xchol,invXchol,randnstate); 
      time(3) = cputime; 
      Zstep = steplength(blk,Z,dZ,Zchol,invZchol,randnstate);
      time(4) = cputime; 
      pstep = min(1,gamused*Xstep); 
      dstep = min(1,gamused*Zstep); 
%%***
bty = b'*y; bdy = b'*dy;
if Zstep > .99e12 & bdy > 0,
   dZpATdynorm = ops(ops(dZ,'+',Atyfun(blk,Avec,permA,isspAy,dy)),'norm')/bdy;
fprintf('\n dual step shows primal infeas, dZpATdynorm = %3.2e \n',dZpATdynorm),
   fprintf(' bTy and bTdy (pred) are %3.2e , %3.2e \n \n',bty,bdy);
end;
if bty > 0 & bdy > 0 & psi < 1e-6 & phi > 10*psi,
   fpstep = (Xstep*bdy)/((1-Xstep)*bty);
if (Xstep < 0.2 & fpstep > 0.8) | Zstep > 10,
      if Zstep < .99e12,
         fprintf('\n bTy and bTdy (pred) are %3.2e , %3.2e \n',bty,bdy);
      end; 
      oldpstep = pstep;
      pstep = bty/(bty + bdy);
      fprintf(' fhompstep, old prim step, new prim step are %3.2e , %3.2e , %3.2e \n',fpstep,oldpstep,pstep);
      fdstep = Zstep*bdy/(bty + Zstep*bdy); 
      olddstep = dstep;
      dstep = gamused*Zstep;
      fprintf(' fhomdstep, old dual step, new dual step are %3.2e , %3.2e , %3.2e \n',fdstep,olddstep,dstep);
   end;
end;
%%
ctx = blktrace(blk,C,X); cdx = blktrace(blk,C,dX);
if Xstep > .99e12 & cdx < 0,
   AdXnorm = norm(AXfun(blk,Avec,permA,X))/(-cdx);
fprintf('\n primal step shows dual infeas, AdXnorm = %3.2e \n',AdXnorm),
   fprintf(' cTX and cTdX (pred) are %3.2e , %3.2e \n \n',ctx,cdx);
end;
if ctx < 0 & cdx < 0 & phi < 1e-6 & psi > 10*phi,
   fdstep = (Zstep*cdx)/((1-Zstep)*ctx);
if (Zstep < 0.2 & fdstep > 0.8) | Xstep > 10,
      if Xstep < .99e12,
         fprintf('\n cTx and cTdx (pred) are %3.2e , %3.2e \n',ctx,cdx);
      end;
      olddstep = dstep;
      dstep = ctx/(ctx + cdx);
     fprintf(' fhomdstep, old dual step, new dual step are %3.2e , %3.2e , %3.2e \n',fdstep,olddstep,dstep);
      fpstep = Xstep*cdx/(ctx + Xstep*cdx);
      oldpstep = pstep;
      pstep = gamused*Xstep;
     fprintf(' fhompstep, old prim step, new prim step are %3.2e , %3.2e , %3.2e \n',fpstep,oldpstep,pstep);
   end;
end;
%%***
      trXZpred = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ) ...
                 + pstep*dstep*blktrace(blk,dX,dZ); 
      gappred = (normb*normC)*trXZpred; 
      mupred  = trXZpred/n; 
      mupredhist(iter) = mupred; 
      step_pred = min(pstep,dstep);
      pstep_predtime = pstep_predtime + time(3)-time(2);
      dstep_predtime = dstep_predtime + time(4)-time(3);  
%%
%%-----------------------------------------
%%  stopping criteria for predictor step.
%%-----------------------------------------
%%
      if (pstep < steptol) | (dstep < steptol);
         fprintf('\n Stop: steps in predictor too short:');
         fprintf(' pstep = %3.2e,  dstep = %3.2e\n',pstep,dstep);
         CPUtime(iter) = cputime-tstart; 
         termcode = -2; 
         break; 
      end;
      %% test for slow progress when mu is small
      %%
      if (iter >= 2); 
         idx = [max(2,iter-5) : iter];
         pred_convg_rate = mean(mupredhist(idx)./mupredhist(idx-1));
         idx = [max(2,iter-2) : iter];
         pred_slow = all(mupredhist(idx)./mupredhist(idx-1) > 0.4);
      end; 
      if ~predcorr; 
         if (mu < 1e-8 & infeas_meas < 1e-8) ...
            & ((mupred/mu > 5*pred_convg_rate) | (pred_slow));
            fprintf('\n Stop: lack of progress in predictor:');
            fprintf(' mupred/mu = %3.2f, pred_convg_rate = %3.2f\n',...
                      mupred/mu,pred_convg_rate);
            CPUtime(iter) = cputime-tstart; 
            termcode = -1; 
            break;
         else; 
            update_iter = 1; 
         end;
      end;
%%
%%---------------------------------------------------------------
%% corrector step.
%%---------------------------------------------------------------
%%
      if (predcorr)
         if (mu > 1e-6)
            if (step_pred < 1/sqrt(3)); 
               expon_used = 1; 
            else
               expon_used = max(expon(vers),3*step_pred^2); 
            end
         else 
            expon_used = max(1,min(expon(vers),3*step_pred^2)); 
         end 
         sigma = min( 1, (mupred/mu)^expon_used ); 
         sigmu = sigma*mu; 
%%
         if (vers == 1)
            [dX,dy,dZ,solve_ok] = HKMcorr(blk,dX,dZ,Avec,rp,Rd,sigmu,hRd,...
             permA,isspAy,nzlistAy,schur,UU,VV,X,Z,Zinv,dd,ee,gamz2);
             clear Zinv; 
         elseif (vers == 2)
            [dX,dy,dZ,solve_ok] = NTcorr(blk,dX,dZ,Avec,rp,Rd,sigmu,hRd,...
             permA,isspAy,nzlistAy,schur,UU,VV,X,Z,W,G,sv,dd,ee,rr,vv,gamx,gamz);
             clear W G;
         end
         if (solve_ok == -1)
            fprintf('\n   fail to solve the Schur complement equation by\n'); 
            fprintf('   using the Sherman-Morrison update'); 
            CPUtime(iter) = cputime-tstart; 
            termcode = -4;
            break;
         end;
         clear schur Rd;
         time(5) = cputime;
         corrtime = corrtime + time(5)-time(4);
%%
%%-----------------------------------
%% step-lengths for corrector step
%%-----------------------------------
%%
         if (gam == 0); 
            gamused = 0.9 + 0.09*min(pstep,dstep); 
         else;
            gamused = gam;
         end            
         Xstep = steplength(blk,X,dX,Xchol,invXchol,randnstate); 
         time(6) = cputime; 
         Zstep = steplength(blk,Z,dZ,Zchol,invZchol,randnstate); 
         time(7) = cputime; 
         pstep = min(1,gamused*Xstep);
         dstep = min(1,gamused*Zstep);
%%***
bty = b'*y; bdy = b'*dy;
if Zstep > .99e12 & bdy > 0,
   dZpATdynorm = ops(ops(dZ,'+',Atyfun(blk,Avec,permA,isspAy,dy)),'norm')/bdy;
fprintf('\n dual step shows primal infeas, dZpATdynorm = %3.2e \n',dZpATdynorm),
   fprintf(' bTy and bTdy (corr) are %3.2e , %3.2e \n \n',bty,bdy);
end;
if bty > 0 & bdy > 0 & psi < 1e-6 & phi > 10*psi,
   fpstep = (Xstep*bdy)/((1-Xstep)*bty);
if (Xstep < 0.2 & fpstep > 0.8) | Zstep > 10,
      if Zstep < .99e12,
         fprintf('\n bTy and bTdy (corr) are %3.2e , %3.2e \n',bty,bdy);
      end;
      oldpstep = pstep;
      pstep = bty/(bty + bdy);
      fprintf(' fhompstep, old prim step, new prim step are %3.2e , %3.2e , %3.2e \n',fpstep,oldpstep,pstep);
      fdstep = Zstep*bdy/(bty + Zstep*bdy);
      olddstep = dstep;
      dstep = gamused*Zstep;
      fprintf(' fhomdstep, old dual step, new dual step are %3.2e , %3.2e , %3.2e \n',fdstep,olddstep,dstep);
      sighom = sigma * bty / bdy;
      fprintf(' sigma and sigma-homog are %3.2e , %3.2e \n \n',sigma,sighom);
   end;
end;
%%
ctx = blktrace(blk,C,X); cdx = blktrace(blk,C,dX);
if Xstep > .99e12 & cdx < 0,
   AdXnorm = norm(AXfun(blk,Avec,permA,X))/(-cdx);
fprintf('\n primal step shows dual infeas, AdXnorm = %3.2e \n',AdXnorm),
   fprintf(' cTX and cTdX (corr) are %3.2e , %3.2e \n \n',ctx,cdx);
end;
if ctx < 0 & cdx < 0 & phi < 1e-6 & psi > 10*phi,
   fdstep = (Zstep*cdx)/((1-Zstep)*ctx);
if (Zstep < 0.2 & fdstep > 0.8) | Xstep > 10,
      if Xstep < .99e12,
         fprintf('\n cTx and cTdx (corr) are %3.2e , %3.2e \n',ctx,cdx);
      end;
      olddstep = dstep;
      dstep = ctx/(ctx + cdx);
     fprintf(' fhomdstep, old dual step, new dual step are %3.2e , %3.2e , %3.2e \n',fdstep,olddstep,dstep);
      fpstep = Xstep*cdx/(ctx + Xstep*cdx);
      oldpstep = pstep;
      pstep = gamused*Xstep;
     fprintf(' fhompstep, old prim step, new prim step are %3.2e , %3.2e , %3.2e \n',fpstep,oldpstep,pstep);
      sighom = sigma * ctx / cdx;
      fprintf(' sigma and sigma-homog are %3.2e , %3.2e \n \n',sigma,sighom);
   end;
end;
%%***
         trXZcorr = trXZ + pstep*blktrace(blk,dX,Z) + dstep*blktrace(blk,X,dZ)...
                    + pstep*dstep*blktrace(blk,dX,dZ); 
         gapcorr = (normb*normC)*trXZcorr;
         mucorr  = trXZcorr/n;
         pstep_corrtime = pstep_corrtime + time(6)-time(5);
         dstep_corrtime = dstep_corrtime + time(7)-time(6);
%%
%%-----------------------------------------
%%  stopping criteria for corrector step
%%-----------------------------------------
%%
         if (pstep < steptol) | (dstep < steptol); 
            fprintf('\n Stop: steps in corrector too short,');
            fprintf('  pstep = %3.2e,  dstep = %3.2e\n',pstep,dstep);
            CPUtime(iter) = cputime-tstart; 
            termcode = -2; 
            break;
         end;
         %% test for slow progress when mu is small
         %%
         if (iter >= 2); 
            idx = [max(2,iter-5) : iter];
            corr_convg_rate = mean(gaphist(idx)./gaphist(idx-1));
            idx = [max(2,iter-2) : iter];
            corr_slow = all(gaphist(idx)./gaphist(idx-1) > 0.5); 
         end; 
         if (mu < 1e-6 & infeas_meas < 1e-6); 
            if (mucorr/mu < min(1,5*corr_convg_rate)) & (~corr_slow); 
               update_iter = 1; 
            else;
               fprintf('\n  lack of progress in corrector:');
               fprintf(' mucorr/mu = %3.2f, corr_convg_rate = %3.2f\n',...
                         mucorr/mu,corr_convg_rate); 
               fprintf('  Stop: lack of progress in corrector.\n');
               CPUtime(iter) = cputime-tstart; 
               termcode = -1; 
               break;
            end; 
         else;
            update_iter = 1;
         end;
      end; 
%%
%%---------------------------------------------------------------
%% udpate iterate
%%---------------------------------------------------------------
%%
      clear Xchol Zchol invXchol invZchol;
      indef = [1 1]; 
      if update_iter
         for t = 1:5
            [Xchol,indef(1)] = blkchol(blk,ops(X,'+',dX,pstep)); time(8) = cputime;  
            if predcorr; pcholtime = pcholtime + time(8)-time(7); 
            else;        pcholtime = pcholtime + time(8)-time(4); 
            end 
            if indef(1) == 0; break; 
            else;             pstep = 0.8*pstep; end
         end
	 for t = 1:5
            [Zchol,indef(2)] = blkchol(blk,ops(Z,'+',dZ,dstep)); time(9) = cputime; 
            dcholtime = dcholtime + time(9)-time(8); 
            if indef(2) == 0; break; 
            else;             dstep = 0.8*dstep; end
         end
         if any(indef);  
            fprintf('\n Stop: X, Z are not both positive definite');
            termcode = -3;
            break;
         end; 
         X = ops(X,'+',dX,pstep);  clear dX;
         y = y+dstep*dy;           clear dy;
         Z = ops(Z,'+',dZ,dstep);  clear dZ;
phi = phi*(1-pstep);
psi = psi*(1-dstep);
      end 
%%
%%---------------------------------------------------------------
%% compute rp, Rd, infeasibities, etc.
%%---------------------------------------------------------------
%%
      trXZ = blktrace(blk,X,Z); 
      gap = (normb*normC)*trXZ;
      mu  = trXZ/n;
      AX  = AXfun(blk,Avec,permA,X); 
      rp  = b-AX;
      ZpATy = ops(Z,'+',Atyfun(blk,Avec,permA,isspAy,y));
      ZpATynorm = ops(ZpATy,'norm');
      Rd  = ops(C,'-',ZpATy); 
      clear ZpATy;
      obj = (normb*normC)*[blktrace(blk,C,X)  b'*y]; 
      rel_gap = gap/max(1,mean(abs(obj))); 
      pri_infeas  = norm(rp)/normb2;
      dual_infeas = ops(Rd,'norm')/normC2;
      if scale_data
         pri_infeas_org  = norm(normA.*rp)*normb/normb2;
         dual_infeas_org = ops(Rd,'norm')*normC/normC2;
      end
      infeas_meas = max(pri_infeas,dual_infeas); 
      gaphist(iter+1)     = gap;  
      infeashist(iter+1)  = infeas_meas;
      stephist(iter+1) = min(pstep,dstep); 
      CPUtime(iter) = cputime-tstart;
      time(10) = cputime;
      misctime = misctime + time(10)-time(9); 
      if printyes;
         fprintf('%2.0f  %4.3f %4.3f',iter,pstep,dstep);
         fprintf(' %2.1e %2.1e  %2.1e',pri_infeas,dual_infeas,gap);
         fprintf(' %- 7.6e  %3.1f\n',mean(obj),sum(CPUtime));
      end;
%%
%%--------------------------------------------------
%% check convergence.
%%--------------------------------------------------
%%
      if (obj(2)/(normb*normC) > ZpATynorm / max(inftol,1e-13));
         fprintf('\n Stop: primal problem is suspected of being infeasible');
         termcode = 1;
         break;
      end;
      if (-obj(1)/(normb*normC) > norm(AX) / max(inftol,1e-13));
         fprintf('\n Stop: dual problem is suspected of being infeasible');
         termcode = 2;
         break;
      end;
      if ( ops(X,'norm') > 1e+15*normX0 | ops(Z,'norm') > 1e+15*normZ0 );
	 fprintf('\n Stop: primal or dual is diverging');
	 termcode = 3;
         maxblowup = max(ops(X,'norm')/normX0,ops(Z,'norm')/normZ0);
	 break;
      end;
      if (max(rel_gap,infeas_meas) < gaptol);
         fprintf('\n Stop: max(relative gap, infeasibilities) < %3.2e',gaptol);
         termcode = 0;
         break;
      end;
      min_pri_infeas = min(infeashist(1:iter)); 
      pri_infeas_bad = pri_infeas_bad + ...
         (pri_infeas > max(1e-10,min_pri_infeas) & (min_pri_infeas < 1e-2));
      if (infeas_meas < 1e-6 | pri_infeas_bad) & (mu < 1e-3); 
         if (mu < 1e-8);
            idx = [max(1,iter-1): iter];
         elseif (mu < 1e-4);
            idx = [max(1,iter-2): iter]; 
         else
            idx = [max(1,iter-3): iter];
         end;
         gap_ratio = gaphist(idx+1)./gaphist(idx); 
         idx2 = [max(1,iter-4): iter]; 
         gap_slowrate = max(0.5, 1.2*mean(gaphist(idx2+1)./gaphist(idx2)));
         gap_slow = ( all(gap_ratio > gap_slowrate) | ...
                     (pri_infeas_bad & any(gap_ratio > 0.99)) ) ...
                   & (gap/maxblk < 1e-3 | rel_gap < 1e-3);
         if (max(infeashist) > 1e-6) & (mu < 1e-4); 
            step_short = all(stephist([iter:iter+1]) < 0.1) | ...
                        (stephist(iter+1) < 0.05); 
         else; 
            %% if starting point is nearly feasible, do not test for 
            %% short step here. 
            step_short = 0; 
         end;
         if (rel_gap < 5*infeas_meas) & (mu < 1e-4); 
            fprintf('\n Stop: relative gap < 5*infeasibility');  
            termcode = 0;
            break;  
         end;
         if (gap_slow) | (step_short);
            fprintf('\n Stop: progress is too slow.\n');
            termcode = -5; 
            break;
         end;  
      end;
   end;
%%---------------------------------------------------------------
%% end of main loop
%%---------------------------------------------------------------
%%
   if (termcode == -6);
      fprintf('\n Stop: maximum number of iterations reached \n');
   end;
%%
%%---------------------------------------------------------------
%% unscale and produce infeasibility certificates if appropriate
%%---------------------------------------------------------------
%%
   if (termcode <= 0),
      if (scale_data == 1),
         X = ops(normb,'*',X); y = normC*y./normA; Z = ops(normC,'*',Z);
      end;
      if (nargout >= 8); Xiter = X; yiter = y; Ziter = Z; end; 
   end;
   if (termcode == 1);
      if (scale_data == 1),
         b = normb*b; b = b.*normA; y = y./normA;
         X = ops(normb,'*',X);
         if (nargout >= 8); 
            yiter = normC*y; Ziter = ops(normC,'*',Z); Xiter = X; 
         end; 
      else;
         if (nargout >= 8); Xiter = X; yiter = y; Ziter = Z; end;
      end;
      rby = 1/(b'*y); y = rby*y; Z = ops(Z,'*',rby);
      norm_resid = ZpATynorm * rby;
   end;  
   if (termcode == 2);
      if (scale_data == 1),
         C = ops(C,'*',normC);
         y = normC*y./normA; Z = ops(normC,'*',Z);
         if (nargout >= 8); 
            Xiter = ops(normb,'*',X); yiter = y; Ziter = Z;
         end; 
      else;
         if (nargout >= 8);  Xiter = X; yiter = y; Ziter = Z; end; 
      end;
      tCX = blktrace(blk,C,X);
      X = ops(X,'*',1/(-tCX));
      norm_resid = norm(normA.*AX)/(-tCX);
   end;
   if (nargout >= 9)
       [X,Z,Xiter,Ziter] = unperm(blk,permZ,X,Z,Xiter,Ziter);
   else
       [X,Z] = unperm(blk,permZ,X,Z);
   end;
%%
%%-----------------------------------------
%%  print summary
%%-----------------------------------------
%%
   fprintf('\n----------------------------------------------------\n');
   fprintf(' number of iterations   = %2.0f\n',iter);
   totaltime = preproctime + sum(CPUtime);
   if (termcode <= 0),
      info = [termcode iter gap infeas_meas totaltime];
      fprintf(' gap                    = %3.2e\n',gap);
      fprintf(' relative gap           = %3.2e\n',rel_gap);
      if scale_data
         fprintf(' primal infeasibilities (scaled problem)   = %3.2e\n',pri_infeas);
         fprintf(' dual        "        "       "            = %3.2e\n',dual_infeas);
         fprintf(' primal infeasibilities (unscaled problem) = %3.2e\n',pri_infeas_org);
         fprintf(' dual        "        "       "            = %3.2e\n',dual_infeas_org);
      else
         fprintf(' primal infeasibilities = %3.2e\n',pri_infeas);
         fprintf(' dual   infeasibilities = %3.2e\n',dual_infeas);
      end
   elseif (termcode == 1),
      info = [termcode iter gap norm_resid totaltime];
      fprintf(' residual of primal infeasibility      \n')
      fprintf(' certificate (y,Z)      = %3.2e\n',norm_resid);
   elseif (termcode == 2),
      info = [termcode iter gap norm_resid totaltime];
      fprintf(' residual of dual infeasibility        \n')
      fprintf(' certificate X          = %3.2e\n',norm_resid);
   elseif (termcode == 3),
	   info = [termcode iter gap maxblowup totaltime];
	%fprintf(' residual of dual infeasibility		\n')
	%fprintf(' certificate X		= %3.2e\n',norm_resid);
   end;
   fprintf(' Total CPU time (secs)  = %3.1f  \n',totaltime);
   fprintf(' CPU time per iteration = %3.1f  \n',totaltime/iter);
   fprintf(' termination code       = %2.0f\n',termcode);
   fprintf('------------------------------------------------');
   fprintf('-------------------\n');
   fprintf(' Percentage of CPU time spent in various parts \n'); 
   fprintf('------------------------------------------------');
   fprintf('-------------------\n');
   fprintf(' preproc  Xchol\tZchol \tpred\tpred_steplen\tcorr\tcorr_steplen\tmisc\n')
   ttime = [preproctime pcholtime dcholtime predtime pstep_predtime dstep_predtime  ...
            corrtime pstep_corrtime dstep_corrtime misctime];
   tt = ttime/sum(ttime)*100; 
   fprintf('   %2.1f    %2.1f\t%2.1f\t%2.1f\t%2.1f\t%2.1f ',...
             tt(1),tt(2),tt(3),tt(4),tt(5),tt(6));
   fprintf(' \t%2.1f\t%2.1f\t%2.1f\t%2.1f\n',tt(7),tt(8),tt(9),tt(10)); 
   fprintf('------------------------------------------------');
   fprintf('-------------------\n');
%%=============================================================================

