function  [x,cons,fc] = qrfacsol(cs,AF,b)
%QRFACSOL  Compute and/or use QR factorization.  Householder
% transformation with pivoting,  P_r A P_c = Q R .
% Used to solve linear least squares and related problems. 
% Call
%     [x {,cons {,fc}}] = qrfacsol(cs,AF {,b {, A}})
% Input parameters
%   cs = 'NA'  :  x = nulspace of  A
%        'NAT' :  x = nulspace of  A'
%        'BA'  :  Basic solution to  Ax "=" b .
%        'MA'  :  Min. norm solution to  Ax "=" b .
%        'BAT' :  Basic solution to  A'x "=" b .
%        'MAT' :  Min. norm solution to  A'x "=" b .
%        'BAA' :  Basic solution to  (A'A)x "=" b .
%        'MAA' :  Min. norm solution to  (A'A)x "=" b .
%   AF :  The matrix  A or its factorization (preserved from
%         a previous call).  See  fc  below.
%   b  :  Right hand side(s). 
% 
% Output parameters
%   x    :  Solution (empty if  b  was not present). 
%   cons :  Consistency flag.  cons = 1  if the system is
%           consistent, otherwise  cons = 0 .
%   fc   : Factorization, a struct with fields
%          F  :  R  in upper triangle
%                Householder vectors in lower triangle and in
%          D  :  'Diagonal' elements of Householder vectors.
%          pr :  Row pivoting order (representing  P_r ).
%          pc :  Column pivoting order (representing  P_c ).
%          rk :  Estimated rank.

% Hans Bruun Nielsen, IMM, DTU.  00.02.11

  %  Initialize
  if  ~isstruct(AF)    % Matrix is given.  Compute factorization
    AF = myqrfac(AF); 
    if  nargout > 2,  fc = AF; end
  end
  mn = [length(AF.pr) length(AF.pc)];  n = mn(2);
  p = AF.rk;  cons = 1;
  
  switch  upper(cs)  
    case 'NA'
      if  p < n
        x = orth([triu(AF.F(1:p,1:p))\AF.F(1:p,p+1:n); -eye(n-p)]);
        x(AF.pc,:) = x;
      else,  x = []; end
    case  'NAT',  x = myqrsol(AF, [], 0);
    case  {'BA', 'MA'},    [x  cons] = myqrsol(AF, b, 4); q = 2;
    case  {'BAT', 'MAT'},  [x  cons] = myqrsol(AF, b, 3); q = 1;
    case  {'BAA', 'MAA'},  [x  cons] = myqrsol(AF, b, -1); q = 2;
    otherwise, error('Unknown problem')
  end
  if  upper(cs(1)) == 'M'    % Min. norm solution
    if  p < mn(q)    % Rank deficient.  Get null space
      if  q == 2,  W = qrfacsol('na',AF);
      else,        W = myqrsol(AF, [], 0); end
      x = x - W*(W'*x);
    end
  end    
      
% -------------------------  Auxiliary functions

function  fac = myqrfac(A)
%MYQRFAC  QR factorization with row and column pivoting
%       fac = myqrfac(A)
%  fac  is a struct with fields
%  F  :  R  in upper triangle
%        Householder vectors in lower triangle and in
%  D  :  'Diagonal' elements of Householder vectors
%  pr :  Row pivoting order
%  pc :  Column  pivoting order
%  rk :  Estimated rank

%  Hans Bruun Nielsen, IMM, DTU.  99.11.01 / 11.09 / 12.14

   [m n] = size(A);   mn = min(m,n); 
   F = A;   D = zeros(1,mn);   nc = zeros(1,n);
   for  j = 1 : n,  nc(j) = sum(F(:,j).^2); end
   nc0 = nc;   pr = 1:m;   pc = 1:n;   rk = 0;
   for  k = 1 : mn
     %  Find and check pivotal column
     [mc qk] = max(nc(k:n));   qk = qk + rk;
     if  mc <= (min(2*n,100)*eps)^2*nc0(qk),
       break, end  
     [mr pk] = max(abs(F(k:m,qk)));   pk = pk + rk;
     if  qk > k    % Swap columns
       f = [qk k];   t = [k qk];
       nc(t) = nc(f);   nc0(t) = nc0(f);
       pc(t) = pc(f);   F(:,t) = F(:,f);
     end % Swap columns
     if  pk > k    % Swap rows
       f = [pk k];   t = [k pk];
       pr(t) = pr(f);   F(t,:) = F(f,:);
     end % Swap rows
     %  Householder transformation
     Ok = F(k,k);   Nk = -sign(Ok)*norm(F(k:m,k));
     F(k,k) = Ok - Nk;
     if  k < n    % Transform and update  nc
       k1 = k + 1;
       g = (F(k:m,k)'*F(k:m,k1:n))/(F(k,k)*Nk);
       F(k:m,k1:n) = F(k:m,k1:n) + F(k:m,k)*g;
       nc(k1:n) = nc(k1:n) - F(k,k1:n).^2;
       rj = find(nc(k1:n) < min(n,100)*eps*nc0(k1:n));
       for  j = 1 : length(rj)  % Compute actual value
         j = k + rj(j);  nc(j) = sum(F(k1:m,j).^2);
       end       
     end
     rk = rk + 1;   D(k) = F(k,k);   F(k,k) = Nk;
   end % Loop
   
   fac = struct('F',F, 'D',D, 'pr',pr, 'pc',pc, 'rk',rk);
% --------------------------------  end of MYQRFAC

function  [x, cons] = myqrsol(FC, b, cs)
%MYQRSOL  Use QR factorization of A to compute  x
%    [x, cons] = myqrsol(F, b {,cs})
%  FC :  Factorization, see  MYQRFAC
%  b  :  Right hand side
%  cs = 0 :  x = N ,  nulspace of  A'
%       1 :  Qx = P_r b  with consistency check
%       2 :  R'x = P_c' b  with consistency check
%       3 :  A'x = b
%			 -1 :  (A'A)x = b
%  otherwise (default):  Ax = b
%  cons = 1  if consistent
%         0  otherwise

%  Hans Bruun Nielsen, IMM, DTU.  99.11.01 / 11.08 / 00.01.12

   [m n] = size(FC.F);   mn = min(m, n);  rk = FC.rk;
   if  nargin < 3,  cs = 4; end
   if  min(size(b)) == 1,  b = b(:); end
   sb = size(b);   cons = ones(1,sb(2));
   switch  cs

   case -1    % Solve  (A'A) x = b
     %  First :  solve  R' x = P_c' b
     [x cons] = myqrsol(FC,b, 2);   q = size(x,2);
     %  Next :  Solve  R x = x
     x = [triu(FC.F(1:rk,1:rk))\x(1:rk,:); zeros(n-rk,q)];
     %  Reorder solution
     x(FC.pc,:) = x;
     
   case 0    %  Orthonormal null space vectors for A'
     if      rk == 0,  x = eye(m);
     elseif  rk >= m,  x = [];
     else
       x = [zeros(rk,m-rk); eye(m-rk)];
       for  k = rk : -1 : 1
         w = [FC.D(k); FC.F(k+1:m,k)];
         g = (w'*x(k:m,:)) / (FC.F(k,k)*FC.D(k));
         x(k:m,:) = x(k:m,:) + w*g;
       end
     end
     x(FC.pr,:) = x;

   case 1    %  x = Q' * P_r * b
     if  sb(1) ~= m,  error('b  has wrong length'), end
     x = b(FC.pr,:);
     for  k = 1 : rk
       w = [FC.D(k); FC.F(k+1:m,k)];
       g = (w'*x(k:m,:)) / (FC.F(k,k)*FC.D(k));
       x(k:m,:) = x(k:m,:) + w*g;
     end
     if  (n < m) & (nargout > 1)    % Check consistency
       for  k = 1 : sb(2)
         cons(k) = norm(x(n+1:m,k)) <= min(n,100)*eps*norm(b(:,k));
       end
     end

   case 2    % Solve  R' x = P_c' b
     if  sb(1) ~= n,  error('b  has wrong length'), end
     x = b(FC.pc,:);   x(1,:) = x(1,:)/FC.F(1,1);
     for  k = 2 : rk
       x(k:n,:) = x(k:n,:) - FC.F(k-1,k:n)'*x(k-1,:);
       x(k,:) = x(k,:) / FC.F(k,k);
     end
     if  (m < n) & (nargout > 1)    % Check consistency
       for  k = 1 : sb(2)
         cons(k) = norm(x(m+1:n,k)) <= min(n,100)*eps*norm(b(:,k));
       end
     end

   case 3    % Solve  A' x = b
     %  First :  solve  R' x = P_c' b
     [x cons] = myqrsol(FC,b, 2);  
     %  Next :  x  := Q * x
     if      m > n,  x = [x; zeros(m-n,sb(2))];
     elseif  m < n,  x = x(1:m,:);           end
     for  k = rk : -1 : 1
       k1 = k + 1;
       g = ([FC.D(k); FC.F(k1:m,k)]' * x(k:m,:)) / (FC.F(k,k)*FC.D(k));
       x(k:m,:) = x(k:m,:) + [FC.D(k); FC.F(k1:m,k)] * g;
     end
     %  Reorder solution
     x(FC.pr,:) = x;

   otherwise    % Solve  A x = b
     %  First :  solve  Q x = P_r b
     [x cons] = myqrsol(FC,b, 1);
     %  Next :  solve  R x = b 
     if      n > m,  x = [x; zeros(n-m,sb(2))];
     elseif  n < m,  x = x(1:n,:);           end  
     x(rk,:) = x(rk,:)/FC.F(rk,rk);
     for  k = rk-1 : -1 : 1
       x(k,:) = (x(k,:) - FC.F(k,k+1:rk)*x(k+1:rk,:)) / FC.F(k,k);
     end
     if  rk < mn,  x(rk+1:end,:) = zeros(mn-rk,sb(2)); end
     %  Reorder solution
     x(FC.pc,:) = x;
     
   end % Cases