function output = HLWBsolver(A,b,v,options) 
%function [xfinal,yk,output] = HLWBsolver(A,b,v,options) 
%
%  Solve the nearest/proj. problem min ||v-x|| s.t. Ax=b, x>=0
%  Use the opt condiitons   F(y) = A(v+A'*y)_+ - b = 0
%   So we minimize  f(y) = (1/2)norm(F(y))^2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INPUT and problem description:
% v - initialization point in R^n
% A - matrix in R^(m*n) for equal. constraints
% b vector in R^m 
% options - Struct: maxiter - Maximum number of iterations for while loop
%                   tol - tolerance of stopping criteria                  
% OUTPUT: 
%  xfinal  - final solution
%  yk      - final dual solution in opt conditions
    
% Initialization
warning off   % since Jacobian can be ill-conditioned
% Options
if isfield(options,'tol')     
	tol = options.tol;   
else 
	tol = 1e-8;
end
if isfield(options,'verbose')     
	verbose = options.verbose;   
else 
	verbose = true;
end
if isfield(options,'fc')     
	fc = options.fc;   
else 
	fc = @(x)(x);
end

if isfield(options,'maxiter')     
	maxiter = options.maxiter;   
else 
	maxiter = 2000;
end

[m,n] = size(A);
if isfield(options,'xo')     % temporary! remove!!
	xo = options.xo;   
	fprintf('WARNING: the optimal xo is passed to solver; BEWARE\n')
else 
	xo = zeros(n,1);
end
k = 0;   % iter for while loop

%%% Initialization: x_k = v (y_k is assigned in the while loop)

%%% Make initial starting point the projection of v onto the nonnegative
%%% orthant ????????

xk = max(v,0);
x0 = xk; 
%xks = xk;
yk = xk;
%yks = [];

normb = 1+norm(b);

% Print desirable strings
if options.verbose
    VarPrint = {'PrimalFeas'};
    fprintf('%-9s', 'iter')
    fprintf('%-20s', VarPrint{:})
    fprintf('\n')
end

% Proximity Function for eq (7) of a-150-linsup-IP-reprint-060317
%relresid = (1/(2*m))*sum(A*xk-b)^2 + (1/(2*n))*sum(max(0,-xk))^2;

%% At optimality xk = (v+A'*dualk)_+ so norm-residual is norm(A(xk)_+ - b)
relresid = norm(A*yk - b)/(1+normb);  %yk>=0; relresid matching semismooth
%relresid = norm(A*xk - b)/(1+normb) + norm(max(0,-xk))/(1+norm(xk));
relresidxo = norm(xk - xo)/(norm(xo)+1);
relresidxko = norm(xk - xo)/(norm(x0-xo)+1);

% Print desirable information
if options.verbose
    VarValue = {relresid};
    fprintf('%-9i', k)
    fprintf('%-20e', VarValue{:})
    fprintf('\n')
end
ik = mod(k,m+1)+1;
msweeps = 0;
VarValues(1) = Inf;

%%% Start of while loop
while (relresid(msweeps+1) > tol && msweeps < options.maxiter) 

    %%% Projection of xk onto A_ik 
    if 1 <= ik && ik <= m    % if ik is NOT m+1 i.e.  1 <= ik <=m
        Pk = b(ik) - A(ik,:)*xk;  % proj scalar/rows are normalized
        yk = xk + Pk*A(ik,:)';   % take step/proj to boundary of equality
    
    %%% Projection of xk onto nonnegative orthant each m+1 step
    else   % ik = m+1
        yk = max(0, xk);
    end
    
    %%% We've seen this 'anchoring' from the H-W Algorithm
    sigma = 1/(fc(k)+1);  % sigma sequence for each iteration
    %sigma = 1/(fc(msweeps+1)+1);  % sigma sequence for each iteration
    xk = sigma*v + (1-sigma)*yk;

    %%% Update k, xks, yks, ik
    k = k+1;
    %xks(:,k+1) = xk;
    %yks(:,k+1) = yk;
    ik = mod(k,m+1)+1;

    % Print desirable information
    if mod(k,m+1) == 0
        msweeps = msweeps+1;
        %%%% what do we use for relresid?????
        relresid(msweeps+1) = norm(A*yk - b)/(1+normb); %yk>=0!! already
        %relresid(msweeps+1) = norm(A*xk - b)/(1+normb) + norm(max(0,-xk))/(1+norm(xk));
        relresidxo(msweeps+1) = norm(xk - xo)/(norm(xo)+1);
        relresidxko(msweeps+1) = norm(xk - xo)/(norm(x0-xo)+1);
        VarValue = {relresid(msweeps+1)};
        VarValues = relresid(msweeps+1);
        %if mod(k,m+1) == 0
        if options.verbose
            fprintf('%-9i', msweeps)
            fprintf('%-20e', VarValue{:})
            fprintf('\n')
        end
    end
end %%% end of while loop


if options.verbose
    [minrelresid,indmin] = min(VarValues);
    
    fprintf('last msweeps is: ')
    fprintf('%-9i', msweeps)
    fprintf('%-20e', VarValue{:})
    fprintf('\n')
    fprintf('best msweeps is: ')
    fprintf('%-9i', indmin(1))
    fprintf('%-20e', minrelresid(1))
    fprintf('\n')

    semilogy(VarValues)
    title('all values for HLWB for each msweeps')
    xlabel('msweeps')
    ylabel('residual value')
end

if ~options.verbose    % print the last iteration
    % Print desirable strings
    fprintf('\nUsing HLWB;  NEW proj. problem on ')
    fprintf(' Ax=b,x>=0 with A %i by %i \n',m,n)
    VarPrint = {'PrimalFeas'};
    fprintf('%-9s', 'iter')
    fprintf('%-20s', VarPrint{:})
    fprintf('\n')
    
    yk = max(0,xk);
    relresid(msweeps) = norm(A*yk - b)/(1+normb);
    relresidxo(msweeps) = norm(xk - xo)/(norm(xo)+1);
    relresidxko(msweeps) = norm(xk - xo)/(norm(x0-xo)+1);
    VarValue = {relresid(msweeps)};
    fprintf('%-9i', k)
    fprintf('%-20e', VarValue{:})
    fprintf('\n')
end

output.xfinal = xk;
output.yfinal =  yk;
%output.xks = xks;
%output.yks = yks;
output.relresid = relresid;
output.relresidxo = relresidxo;
output.relresidxko = relresidxko;
output.iter = k;
end




