%%*********************************************************************
%% linsysolve: adapted from LIPSOL (sherman.m) by Yin Zhang, April, 1995.
%%          Using the Sherman-Morrison formula to solve
%%          (H + U*V')x = b
%%          where U, V are low-rank, possibly []. 
%%
%%  [x,solve_ok] = linsysolve(P,U,V,b,flag)
%%
%%  if chol_options == 'matlab'
%%     P is the Cholesky factor of H computed by Matlab's rountine
%%  elseif chol_options == 'lipsol'
%%     P = H,  the Cholesky factorization of H is computed 
%%     via LIPSOL's routines. 
%%  end
%%
%% SDPT3: version 3.0 
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*********************************************************************

  function [x,solve_ok] = linsysolve(P,U,V,b,flag)

  global Mdense chol_options

  ismtU = isempty(U);  
  if ~ismtU 
     solve_ok = -1; 
     if (flag == 1) 
        n = size(U,2); 
        PinvU = zeros(size(U));
        if chol_options == 'matlab'
           for i = 1:n
               PinvU(:,i) = bsolve(P,U(:,i));   
           end;
        elseif chol_options == 'lipsol'
           for i = 1:n
              if (i==1)
                 PinvU(:,i) = linsys(P,U(:,i));   
              else
                 PinvU(:,i) = linsys([],U(:,i));   
              end
           end;           
        end
        Mdense = eye(n) + V'*PinvU; 
     end
     if chol_options == 'matlab'
        x = bsolve(P,b); 
        tmp = U* (Mdense\ (V'*x));
        x = x - bsolve(P,tmp);
     elseif chol_options == 'lipsol'
        x = linsys([],b); 
        tmp = U* (Mdense\ (V'*x));
        x = x - linsys([],tmp);
     end
  else
     solve_ok = 1; 
     if chol_options == 'matlab'
        x = bsolve(P,b);  
     elseif chol_options == 'lipsol'
        if (flag == 1) 
           x = linsys(P,b);  
        else
           x = linsys([],b);  
        end
     end
  end;
%%
%% iterative refinement.
%% this step is crucial to ensure that computed 
%% solution is sufficiently accuraete. 
%%
  if ~ismtU 
     if length(b) > 5000; 
        maxit = 50; 
     else
        maxit = 25; 
     end
     rtol = 1e-6; 
     bnorm = max(1,norm(b)); 
     for k = 1:maxit
        if chol_options == 'matlab'
           tmp = P*x; tmp = (tmp'*P)';
           r = b - (tmp + U*(V'*x)); 
        elseif chol_options == 'lipsol'
           r = b -(P*x + U*(V'*x)); 
        end 
        %%
        rrnrm(k) = norm(r)/bnorm;      
        if (rrnrm(k) < rtol); solve_ok = 1; break; end; 
        %%
        if chol_options == 'matlab'           
           d = bsolve(P,r); 
           tmp = U*(Mdense\(V'*d)); 
           d = d - bsolve(P,tmp);           
        elseif chol_options == 'lipsol'
           d = linsys([],r); 
           tmp = U*(Mdense\(V'*d));
           d = d - linsys([],tmp);
        end
        x = x + d;
     end
  end
%%
%%*************************************************************************
%% bsolve: solve H*x = b; 
%%
%% where P is the Cholesky factor of H. 
%%
%%*************************************************************************

  function x = bsolve(P,b); 

  if issparse(P)  
     tmp = b' / P; 
     x = P \ tmp'; 
  else     
     tmp = mexbsolve(P,b,2); 
     x = mexbsolve(P,tmp,1); 
  end
%%
%%*************************************************************************
%% LINSYS: Solve a positive definite system of linear equations.
%%         LINSYS(P,rhs) returns a "solution" to linear system Px = rhs,
%%         where the matrix P is sparse symmetric and positive definite.
%%         The function SYMBFCT must be called at least once before 
%%         LINSYS is called.  LINSYS uses three Fortran MEX programs.
%%
%%         If P = [], LINSYS will try to use, if any, an existing Cholesky 
%%         factor.
%%
%% Yin Zhang, July, 1995
%% Department of Mathematics and Statistics
%% University of Maryland  Baltimore County
%%*************************************************************************

  function [sol] = linsys(P,rhs)

  global XLNZ NNZL PERM INVP LNZ LOOP_LEVEL
  global XSUPER XLINDX LINDX SNODE SPLIT TMPSIZ
  global LNZ0

  if issparse(rhs)
     rhs = full(rhs); 
  end;
  m = length(PERM);
  if (max(size(rhs)) ~= m) | (min(size(rhs)) ~= 1)
     error('No symbolic factor or input sizes mismatch.');
  end;

  if ~isempty(P)
     %% check input matrix P 
     if (size(P,1) ~= m | size(P,2) ~= m | ~issparse(P))
        error('Input matrix must be square and sparse.');
     end
     %% Remove diagonal from P 
     Pdiag = full(diag(P));
     P = P - sparse(1:m,1:m,Pdiag);
     %%
     %% Cholesky factorization 
     %%
     [LNZ] = inpnv(Pdiag,P,INVP,PERM,XLNZ,XSUPER,XLINDX,LINDX,NNZL);
     [LNZ] = blkfct(XLNZ,XSUPER,SNODE,SPLIT,XLINDX,LINDX,LNZ,TMPSIZ,LOOP_LEVEL);
  end;
%%
%% back solve 
%%
  sol = zeros(m,1);
  sol(PERM) = blkslv(XLNZ,XSUPER,XLINDX,LINDX,LNZ,rhs(PERM));
%%*************************************************************************



