function [f,e,r,itn] = stlnone(K,g,lambda,tol)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  function [f,e,r,itn] = stlnone(K,g,lambda,tol)
%
%  This function computes a solution to a structured
%  total least squares problem.
%  Given a matrix K, a positive scalar lambda, and 
%  a vector g, we minimize the function
%    || E ||^2 + || r ||^2 + lambda^2 || f ||^2
%  over all choices of E and f
%  subject to the constraints that
%      (K+E) f = g + r and
%       E is a Toeplitz matrix defined by parameters e.
%  We measure matrix norms using the Frobenius norm
%  (the square root of the sum of the elements squared)
%  and we measure vector norms by the Euclidean norm.
%  The algorithm uses Newton's method with steplength = 1
%  and stops when the norm of the Newton direction is
%  less than tol.  The scalar itn records the number
%  of iterations.
%
%  stlnone.m   Dianne P. O'Leary 11/2004
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Modified 12-2008 to suppress print from linprog.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization: 
%          f = solution to least squares problem
%              min || K f - g ||
%          D = diagonal matrix of weights
%              so that || E || can be computed
%              from the elements e that
%              define it.
%          e = 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


[nrows,ncols]=size(K);
length_e = nrows + ncols - 1;
f = ones(ncols,1);
r = g - K*f;
d = sqrt([1:ncols,ncols*ones(1,nrows-ncols-1),ncols:-1:1]');
D = diag(d);

e = zeros(length_e,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We solve the problem using Newton's method.
% The Newton direction p is the solution to the least
% squares problem || M p - s ||
% where
%   M = [ F     K+E   ]        s = [ -r       ]
%       [ D      0    ]   ,        [ D e   ] .
%       [ 0  lambda I ]            [ lambda f ]
%
% and F is a Toeplitz matrix formed from the entries of f. 
% The matrix part2 containes the constant part of M.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

part2 = [D,zeros(length_e,ncols);
         zeros(ncols,length_e), lambda*eye(ncols)];
xx = ones(length_e+ncols,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This loop performs the Newton iteration, computing
% p and then updating e and f until convergence.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

itn = 0;

while norm(xx(1:length_e+ncols)) > tol,
   f_row = [f(ncols:-1:1);zeros(nrows-1,1)];
   f_col = [f(ncols);zeros(nrows-1,1)];
   F = toeplitz(f_col,f_row);
   E = toeplitz(e(ncols:length_e),e(ncols:-1:1));
   lpmat = [F,K+E;part2];
   lpmat = [lpmat;-lpmat];
   ee = [eye(nrows+ncols+length_e);eye(nrows+ncols+length_e)];
   lpmat = [lpmat,-ee;
     zeros(ncols,length_e),-eye(ncols),zeros(ncols,nrows+ncols+length_e)];
   lpb = [r;-D*e;-lambda*f];
   lpb = [lpb;-lpb;f];
   lpc = [zeros(length_e+ncols,1);ones(nrows+ncols+length_e,1)];
   [xx,myf,myflag] = linprog(lpc,lpmat,lpb,[],[],[],[],[], ...
                     optimset('Display','off')); 
   if (myflag ~= 1)
      disp(sprintf('Linprog reported an error: exitflag = %d',myflag'))
   end
   
   dele = xx(1:length_e);
   delf = xx(length_e+1:length_e+ncols);
   step = 1;
   e = e + step* dele;
   f = f + step* delf;
   r = g - K*f - E*f;
   itn = itn + 1;
%  [itn, norm(f), norm(r), norm(dele), norm(delf)]
end