function [x, it] = TotalLS(C, D, b, max, steptol)
% [x, it] = TotalLS(C, D, b, max, steptol)
%
% Compute the solution to the Total LS problem
%   (C kron D) x \approx b
%
% Input:
%   C : mC-by-nC matrix, with mC >= nC, full rank
%   D : mD-by-nD matrix, with mD >= nD, full rank
%   b : length mC*mD column vector
%   max : integer >= 0, maximum number of iterations.
%     Optional, default value is (mC + mD) * (nC + nD).
%   steptol: scalar >= 0 and << 1, lower bound on the
%     size of steps taken during the Inverse Iteration.
%     Optional, default value of machine epsilon.
% Output:
%   x : length nC*nD column vector, approximate solution
%     to the Total Least Squares problem
%   it : number of iterations used

% Make sure minimum number of arguments is satisfied

if nargin < 3
   error('TotalLS error: 3 or more arguments required')
end

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

% Do size checks.
if mC <= nC
   error('TotalLS error: C must have more rows than columns')
end
if mD <= nD
   error('TotalLS error: D must have more rows than columns')
end
if mb ~= mC * mD
   error('TotalLS error: size mismatch')
end

% Compensate for missing arguments.

if nargin < 4
   max = (mC + mD) * (nC + nD);
end

if nargin < 5
   steptol = eps;
end

% Square of step tolerance.
steptol2 = steptol * steptol;

n = nC * nD;

% Thin QR factor C and D.
[QC, RC] = qr(C, 0);
[QD, RD] = qr(D, 0);

% Compute w = ((QC kron QD)' b)(1 : n2).
w = KPmultT(QC, QD, b);
clear QC QD
alpha2 = b' * b - w' * w;

% Get starting vector.
v = ones(n+1, 1);
v1 = v(1:n);
v2 = v(n+1);

% Allocation.
y1 = zeros(n, 1);
z1 = zeros(n, 1);
z2 = 0;

% Inverse iteration.
k = 0;
while (k < max)
   k = k + 1;
   
   % Step 1.
   y1 = kronLTSolve(RC', RD', v1);
   
   % Step 2.
   z2 = (v2 - w' * y1) / alpha2;
   z1 = kronUTSolve(RC, RD, y1 - w * z2);
   
   % Normalize z.
   normz = sqrt(z1' * z1 + z2*z2);
   z1 = z1 / normz;
   z2 = z2 / normz;
   z = [z1; z2];
   
   % Check step.
   step = z - v;
   normstep2 = step' * step;
   if normstep2 < steptol2
      break;
   end
   
   % Take step.
   v1 = z1;
   v2 = z2;
   v = [v1; v2];
end

% Assign output.
x = - z1 / z2;
if nargout > 1
   it = k;
end
