function x = conls(A,b,B,d)
% CONLS Equality Constrainted Least Squares 
%
% x = conls(A,b,B,d) solves
%
%   minimize     ||b - Ax||_2
%   subject to   Bx = d
%
% when B is p-by-n, p < n, and rank(B) = p, by using the QR factorization
% of B' to find a basis for the nullspace of B.
% 
% See also \ (backslash)

% David F. Gleich
% Purdue University
% CS 59000-OPT, Spring 2012
% Computational Methods in Optimization

[m,n] = size(A);
p = size(B,1);

assert(size(B,2) == n, 'incompatible dimensions for A and B');

% Step 1, compute the QR factorization of B'
[Q,R] = qr(B')

if p==1, assert(B(1,1) ~= 0, 'B is not full-rank'); 
else assert(all(diag(B) ~= 0), 'B is not full-rank'); % not a full check
end

% Note, see Golub and van Loan for a better implementation, this is for
% pedagogical purposes only
Q1 = Q(:,1:p);
Q2 = Q(:,p+1:end);

% Step 2, find y

y = R(1:p,1:p)'\d; %

% Step 3, find z
z = (A*Q2)\(b - A*(Q1*y));

% Step 4, recompute x

x = Q1*y+Q2*z;

