function [x,iter] = sbls2(A,b,l,u)
%SNNLS	Solution of sparse box constrained least squares problems.
%       %Z%%M% Version %I% %G%
%       Mikael Lundquist, Linkoping University.
%       e-mail: milun@mai.liu.se 
%       http://www.mai.liu.se/~milun/sls/
%
%       x=sbls2(A,b,l,u) solves the constrained 
%                        linear least squares problem
%
%       min_x ||Ax-b||_2  s.t. l <= x <= u
%       upper and lower may be inf.
%       l and u may be scalars and is then treated as bound for all x
%
%       x=sbls2(A,b,l) solves the  the problem with bounds l <= x
%       x=sbls2(A,b) solves the non-negativity problem     0 <= c
%
%       Numerical method: Block pivoting
%
%       MATLAB v.4 and sqr (Matstoms (1994)) or MATLAB v.5 assumed.
%

% Check the input data.

if nargin > 4,
  disp('??? Error using ==> sbls2')
  disp('Too many input arguments.')
  return
end

if nargin<=3,u=inf;end;
if nargin==2,l=0;end;

[m,n]=size(A);
if size(l,1)==1,l=ones(n,1)*l;end
if size(u,1)==1,u=ones(n,1)*u;end

% Minimum degree analysis of A.

Pc=colmmd(A);
A=A(:,Pc);
u=u(Pc);
l=l(Pc);
if nargin==6,
  xx=xx(Pc);
end

% Initiate some variables.
tol=n*eps*1000;

NB=isinf(l) & isinf(u);              % NotBounded variables

swap=find(isinf(l) & ~isinf(u));     % Make only upperbound inf.
if length(swap)>0,
  l(swap)=-u(swap);
  A(:,swap)=-A(:,swap);
  u(swap)=inf*ones(size(swap));
end

bb=b-A(:,~NB)*l(~NB);                % l<x<u => 0<x<uu
uu=u-l;
x=zeros(n,1);

F=find(~NB); L=[];   U=[];          % x(F), y(L) and z(U) basic variables.
NB=find(NB);

iter=1;
R=chol(A'*A);
xnp1=R\(R'\(A'*bb));

xtemp=xnp1;
xtemp(xtemp<0)=zeros(size(find(xtemp<0)));
xtemp(xtemp>uu)=uu(xtemp>uu);
term2=A(:,F)*xtemp(F);
term=zeros(m,1);

xn=zeros(n,1);
p=xnp1;
qxnp1=bb'*bb;
qxn=0;

% - or you could use the QR factorization
% [c,R]=qr(A,bb,0);
% x=R\c;
% Iterative refinemant one step.
% dx=R\(R'\(A'*(bb-A*x)));
% x=x+dx;

y=[]; z=[];

% Start of main loop

while any([xnp1(F)<-tol; xnp1(F)>uu(F)+tol ; y(L)<-tol ; z(U)<-tol]),

  if iter==100,disp('VARNING! Sbls2 do not converge'),break,end
  
  H1=F(find(xnp1(F)<0));
  H2=F(find(xnp1(F)>uu(F)));
  H3=L(find(y(L)<0));
  H4=U(find(z(U)<0));
  theta=1;

  % Bind variables until stationary point is found.
  if length(H1)+length(H2)>0,
        
    % Idea: Bind all variables such that q(xn)-q(xn+\theta p) > 0.
    %       p = xnp1-xn.

    qxn=qxnp1;

    xtemp=xnp1;
    xtemp(xtemp<0)=zeros(size(find(xtemp<0)));
    xtemp(xtemp>uu)=uu(xtemp>uu);

    r=A*xtemp-bb;

    qxnp1=r'*r;

    thetaL=xn(H1)./-p(H1);
    thetaU=(uu(H2)-xn(H2))./p(H2);
    [thetaLmax,rL]=max(thetaL);
    [thetaUmax,rU]=max(thetaU);
    
    if isempty(thetaL),thetaLmax=-1; end
    if isempty(thetaU),thetaUmax=-4; end
    
% Remove step size 0.
    tL0=find(thetaL~=0);
    tU0=find(thetaU~=0);
    if ~isempty(H1), H1=H1(tL0);thetaL=thetaL(tL0);end;
    if ~isempty(H2), H2=H2(tU0);thetaU=thetaU(tU0);end;
%   Calculate singel pivot step.
    thetam=min([min(thetaL) min(thetaU)]);
    
    while qxn-qxnp1<-tol,
      
      if thetaLmax>thetaUmax,
	theta=max([thetaLmax 0]);
	thetaL(rL)=-2;
	[thetaLmax,rL]=max(thetaL);
      else
	theta=max([thetaUmax 0]);
	thetaU(rU)=-5;
	[thetaUmax,rU]=max(thetaU);
      end

      xtemp=xn+theta*p;
      xtemp(xtemp<0)=zeros(size(find(xtemp<0)));
      xtemp(xtemp>uu)=uu(xtemp>uu);
      
      r=A*xtemp-bb;
      qxnp1=r'*r;
    end

    % Determine what variables was bounded.

    H1=F(find(xtemp(F)<=0));
    H2=F(find(xtemp(F)>=uu(F)));
      
    ADD=[NB]; SUB=[H1; H2];
    FLAG=zeros(n,1);
    FLAG([F; ADD])=ones(size([F; ADD])) ; FLAG(SUB)=zeros(size(SUB));
    F=find(FLAG);
    
    ADD=[H1]; SUB=[];
    FLAG=zeros(n,1); 
    FLAG([L; ADD])=ones(size([L; ADD])) ; FLAG(SUB)=zeros(size(SUB));
    L=find(FLAG);
    
    ADD=[H2]; SUB=[];
    FLAG=zeros(n,1); 
    FLAG([U; ADD])=ones(size([U; ADD])) ; FLAG(SUB)=zeros(size(SUB));
    U=find(FLAG);
 end   

   % Stationary point is found, release variables.

   ADD=[H3;H4;NB]; SUB=[];
   FLAG=zeros(n,1);
   FLAG([F; ADD])=ones(size([F; ADD])) ; FLAG(SUB)=zeros(size(SUB));
   F=find(FLAG);
   
   ADD=[]; SUB=[H3];
   FLAG=zeros(n,1); 
   FLAG([L; ADD])=ones(size([L; ADD])) ; FLAG(SUB)=zeros(size(SUB));
   L=find(FLAG);
   
   ADD=[]; SUB=[H4];
   FLAG=zeros(n,1); 
   FLAG([U; ADD])=ones(size([U; ADD])) ; FLAG(SUB)=zeros(size(SUB));
   U=find(FLAG);
  
% Compute x(F), y(L) and z(U).
  if length(U)>0,
    term=A(:,U)*uu(U);
  else
    term=zeros(m,1);
  end


  if theta==1,
    xn(F)=xnp1(F);
    xn(L)=zeros(size(L));
    xn(U)=uu(U);
  else
    xn=xtemp;
  end
  
  xnp1=zeros(n,1); 
  if (length(F)>0),
    iter=iter+1;
    R=chol(A(:,F)'*A(:,F));
    xnp1(F)=R\(R'\(A(:,F)'*(bb-term)));
    xnp1(U)=uu(U);
    p=xnp1-xn;

    % - or you could use the QR factorization
    %    [c,R]=qr(A(:,F),bb-term,0);
    %    x(F)=R\c;
    % - iterative refinement one step
    %    dx=R\(R'\(A(:,F)'*(bb-A(:,F)*x(F))));
    %    x(F)=x(F)+dx;

    xtemp=xnp1;
    xtemp(xtemp<0)=zeros(size(find(xtemp<0)));
    xtemp(xtemp>uu)=uu(xtemp>uu);
    term2=A(:,F)*xtemp(F);
  
    ADD=[]; SUB=[NB];
    FLAG=zeros(n,1);
    FLAG(F)=ones(size(F)) ; FLAG(SUB)=zeros(size(SUB));
    F=find(FLAG);
  else
    xnp1(U)=uu(U);
    p=zeros(n,1);
    term2=zeros(m,1);
  end
  y = zeros(n,1); y(L) = A(:,L)'*(term2+term-bb);
  z = zeros(n,1); z(U) =-A(:,U)'*(term2+term-bb);
end   % end while

% Compute the final solution using QR factorization.

ADD=[NB]; SUB=[];
FLAG=zeros(n,1);
FLAG([F;NB])=ones(size([F;NB]));
F=find(FLAG);
l(NB)=zeros(size(NB));
if (iter==0),
  x(F)=l(F)+xnp1(F);
else
  if length(F)>0,
%    x(F)=l(F)+qls(A(:,F),bb-term);
    [c,R]=qr(A(:,F),bb-term,0);
    x(F)=l(F)+R\c;
  end
end

x(L)=l(L);
x(U)=u(U);
if length(swap)>0,
  x(swap)=-x(swap);
end
x(Pc)=x;

% ----------------------------------------------------------------------
% Remove this part and save it as a seperate file if you run MATLAB V4.
% ----------------------------------------------------------------------

function x = qls(A,b)
%QLS	Solution of sparse linear least squares problems.
%       @(#)qls.m Version 1.6 6/23/93
%       Pontus Matstoms, Linkoping University.
%       e-mail: pomat@math.liu.se
%
%       x=qls(A,b) solves the sparse linear least squares problem
%                    min ||Ax-b||
%                     x          2
%       using QR factorization and application of Q to the right-hand 
%       side b.

[m,n]=size(A);

% Minimum-degree ordering of A.

q=colmmd(A);
A=A(:,q);

[v,d]=version;
% Check for MATLAB V5.
if str2num(v(1))<5,
  [R,p,c]=sqr(A,b);

% Resulting column permutation.
  Pc=q(p);
else
  [c,R]=qr(A,b,0);
  Pc=q;
end
% Compute the least squares solution.

x=R\c;

% Permute the computed solution

x(Pc)=x;

% ----------------------------------------------------------------------
% Remove this part and save it as a seperate file if you run MATLAB V4.
% ----------------------------------------------------------------------