function [x, it] = wls(C, D, w, b, tol, max)
% [x, it] = wls(C, D, w, b, tol, maxit)
%
% Solve the weighted Least Squares problem
%   min_x || W^{-1/2} ( b - (C kron D) x ) ||_2
% where W = diag(w) is positive definite.
%
% Input:
%   C : mC-by-nC matrix, mC >= nC
%   D : mD-by-nD matrix, mD >= nD
%   w : length mC*mD column vector, all positive elements
%   b : length mC*mD column vector
%   tol: scalar >= 0 and << 1. The Conjugate Gradient step
%     uses this as its convergence tolerance.
%     Optional, default value of 1e-12.
%   max : integer >= 0. The Conjugate Gradient step uses this
%     as its maximum number of iterations.
%     Optional, default value is 3 * mC * mD.
% Output:
%   x : length nC*nD column vector, approximate solution
%     to the Weighted Least Squares problem
%   it : number of Conjugate Gradient iterations used

% Make sure minimum number of arguments is satisfied

if nargin < 4
   error('WLS error: 4 arguments required')
end

% Get sizes.
[mC, nC] = size(C);
[mD, nD] = size(D);
[mw, nw] = size(w);
[mb, nb] = size(b);

% Do size checks.
if (nw ~= 1)
   error('WLS error: w must be a column vector')
end
if (nb ~= 1)
   error('WLS error: b must be a column vector')
end
if (mw ~= mC * mD)
   error('WLS error: size mismatch, W and (C kron D)')
end
if (mb ~= mC * mD)
   error('WLS error: size mismatch, b and (B kron C)')
end

% Compensate for missing arguments.
if nargin < 5
   tol = 1e-12;
end
if nargin < 6
   max = 3 * mC * mD;
end

% We want to solve the system
%   [ W  A ] [ s ] = [ b ]
%   [ A' 0 ] [ x ]   [ 0 ]
% where x is solution and s is scaled residual.

% Step 1. COD factor C and D.
[QC, TC, SC, rC] = cod(C);
[QD, TD, SD, rD] = cod(D);
QC1 = QC(:, 1:rC);
QC2 = QC(:, rC+1 : mC);
QD1 = QD(:, 1:rD);
QD2 = QD(:, rD+1 : mD);

% Step 2. Compute Pi.
pi1 = permute2(rC, rD, mD-rD);
pi2 = permute2(mC-rC, rD, mD-rD);

% Step 3. Compute c = Pi (QC \kron QD)^T b. Form c1 and c2.
c = KPmultT(QC, QD, b);
cA = c(1:rC*mD);
cB = c(rC*mD+1 : mC*mD);
cA = cA(pi1);
cB = cB(pi2);
c = [cA; cB];
clear cA cB
c1 = c(1 : rC*rD);
c2 = c(rC*rD+1 : mC*mD);
clear c

% Step 4. Solve E22 t2 = c2.
[t2, dummy1, dummy2, it] = pcg('mult_E22', c2, tol, max, [], [], [], ...
   QC1, QC2, mC, rC, QD1, QD2, mD, rD, w);

% Step 5. Compute v = E12 t2.
v = mult_E12(t2, QC1, QC2, mC, rC, QD1, QD2, mD, rD, w);

% Step 6. Solve (TC kron TD) y1 = c1 - v.
y1 = kronUTSolve(TC, TD, c1 - v);

% Step 7. Form y.
y = [ y1; zeros(nC*nD - rC*rD, 1) ];

% Step 8. Compute x = (SC \kron SD) y.
x = KPmult(SC(:,1:rC), SD(:,1:rD), y1);
