function output = semismoothsolver(A,b,v,options) 
%function [xfinal,yk,output] = semismoothsolver(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 the DIFFERENTIABLE!!  f(y) = (1/2)norm(F(y))^2
%   options allows for a WARM START
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INPUT and problem description:
% v - initialization point in R^n
% A - matrix in R^(m*n) for equal. constraints; assumed onto/full row rank
% b vector in R^m 
% options - Struct: maxiter - Maximum number of iterations for while loop
%                   tol - tolerance of stopping criteria                  
%           See in Initialization below for other options
% OUTPUT:  output Struct:
%  xfinal  - final solution
%  yk      - final dual solution in opt conditions
%  see output below for other variables in output
    
% Initialization
warning off   % since Jacobian can be ill-conditioned
% Options
if isfield(options,'maxiter')     
	maxiter = options.maxiter;   
else 
	maxiter = 100;
end
if isfield(options,'modprint')     % #iters between outputs
	modprint = options.modprint;   
else 
	modprint = 10;
end
if isfield(options,'verbose')     
	verbose = options.verbose;   
else 
	verbose = true;
end
if isfield(options,'tol')     
	tol = options.tol;   
else 
	tol = 1e-7;
end

[m,n] = size(A);


%%%%%%%%%% if yk exists then we are doing a warm start
if isfield(options,'yk')     
	yk = options.yk;   
else   %%%%%%%%NO warm start
        yk = zeros(m,1);
end
k = 1;   % iter for while loop
bAv = b-A*v;
Ndir = yk;
normNdir = norm(Ndir);
%yks = yk;   % save all yk
vAty = v+A'*yk;
xk = max(vAty,0);
zk = xk-vAty;   % from the optimality conditions
%xks = xk;      % save all xk  --- remove eventually?
%zks = zk;      % save all zk  --- remove eventually?
normb = (1+norm(b)); % for relative error

lbnd = (-norm(zk+A'*yk)^2/2)+yk'*bAv-zk'*v; % from dual problem
%lbnds = lbnd;
%lambdareg = 0;
%lsetxor = inf;

dM = sign(vAty);   % makes small pos numbers 1
%%% fix this for warm start - move above and skip if warm start????

dM(dM == 0) = 1;   % 'maximal' Jacobian for largest rank
lpfdM = sum(dM > 0);  % number of positive xi
dM(dM<0) = 0;   % makes small numbers 0 for projection
%%?????????fix find as it is done twice when find residual!!!???
fdM = find(dM > 0);   % columns of A for Jacobian
AdMs = A(:,fdM);
Jk = AdMs*AdMs';   % Jacobian
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Fk = A*xk-b;   % replaced by next line - much faster
Fk = AdMs*xk(fdM)-b;
relresid = norm(Fk)/normb;

% Print desirable strings
if verbose
    VarValue = {relresid,lbnd,normNdir};
    VarPrint = {'relres/optcond','lower bnd','||Ndir||',  ...
                '#x_j>=','lambda'};
                %'|#pos x_j|','setxor cols','zerosx+z','lambda'};
    	              %'|#pos x_j|','setxor pos','(act+m)','deg: n-(act+m)'};
    fprintf('\nUsing nonsmooth Newton;  NEW proj. problem on ')
    fprintf(' Ax=b,x>=0 with A %i by %i \n',m,n)
    fprintf('%-9s', 'iter')
    fprintf('%-15s', VarPrint{:})
    fprintf('\n')
    
    % Print desirable information
    fprintf('%-9i', k)
    fprintf('%-15e', VarValue{:})
    %fprintf('%-15i', lpfdM,lsetxor,sum((xk+zk)==0))
    fprintf('%-15i', lpfdM)
    fprintf('%-15e', 0)
    fprintf('\n')
end
    




%fdMold =[];
lnormv = log(norm(v)); % to help with regularization
while relresid > tol && k < maxiter  % k # iters
       
    if k < 20   % ???? change 20??? 5??? into parameters
	    normNdirp = normNdir(end);
    else
	    normNdirp = prod(normNdir(end-5:end));
    end
    lambdareg = ...
           1e-2*mean([...
                        relresid*max(1,log(normNdirp)), ...
                        1e-1*relresid*max(1,lnormv), ...  %log(norm(v))
                        1e-1*relresid, ...
                      ]);

    Jk = (Jk+lambdareg*speye(m));
    %%% which is better/faster Jk\Fk? or chol?
    %%% which can be exploited for stability?
    %Ndir = -Jk\Fk;   % slower???was used before
    % for R'(Rx) = Fk;  x = R\(R'\Fk)
    R = chol(Jk);   Ndir = -R\(R'\Fk);  % appears to be faster here?
    normNdir(k) = norm(Ndir);

    %%UPDATES
    yk = yk + Ndir;   %update
    %yks(:,k+1) = yk;
    vAty = v+A'*yk;
    xk = max(vAty,0);
    %xks(:,k+1) = xk;
    zk = max(0,xk-vAty);  % only xk-vAty is needed; max used to ensure c.s.
    %zks(:,k+1) = zk;      % save all zk


    %fdMold = fdM;
    dM = sign(vAty);   % makes small pos numbers 1


    dM(dM == 0) = 1;   % 'maximal' Jacobian for largest rank
    lpfdM = sum(dM > 0);  % number of positive xi
    dM(dM<0) = 0;   % makes small numbers 0 for projection
    %%?????????fix find as it is done twice when find residual!!!???
    fdM = find(dM > 0);   % columns of A for Jacobian
    AdMs = A(:,fdM);
    Jk = AdMs*AdMs';   % Jacobian
    %Fk = A*xk-b;   % replaced by much faster next line
    Fk = AdMs*xk(fdM)-b;
    relresid = norm(Fk)/normb;

    lbnd = (-norm(zk+A'*yk)^2/2)+yk'*bAv-zk'*v;
    %lbnds = [lbnds lbnd];
    k = k+1;
    if verbose && mod(k,modprint) == 0 % modprint set at while above
             VarValue = {relresid,lbnd};
             fprintf('%-9i', k)
             %fprintf('%-15e', VarValue{:},norm(Ndir))
             fprintf('%-15e', VarValue{:},norm(yk))
             %lsetxor = length(setxor(fdM,fdMold));
             fprintf('%-15i', lpfdM);
             %fprintf('%-15i', sum((xk+zk)==0))
             fprintf('%-15e', lambdareg)
             fprintf('\n')
    end
    
end %%% End of while loop 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if ~verbose || modprint > 0   % print last iteration 
             VarValue = {relresid,lbnd};
             VarPrint = {'relres/optcond','lower bnd','||Ndir||',  ...
	              '#x_j>=','lambda'};
	              %'|#pos x_j|','setxor cols','zerosx+z','lambda'};
	              %'|#pos x_j|','setxor pos','(act+m)','deg: n-(act+m)'};
             fprintf('\nAt end of nonsmooth Newton proj. alg: ')
	     fprintf(' Ax=b,x>=0 with A %i by %i \n',m,n)
             fprintf('%-9s', 'iter')
             fprintf('%-15s',VarPrint{:})
             fprintf('\n')
             fprintf('%-9i', k)
             fprintf('%-15e', VarValue{:},norm(Ndir))
             %lsetxor = length(setxor(fdM,fdMold));
	     %%%%lsetxor very expensive to calculate??????change/?????
             %fprintf('%-15i', lpfdM,lsetxor);
             fprintf('%-15i', lpfdM);
             %fprintf('%-15i', sum((xk+zk)==0))
             fprintf('%-15e', lambdareg)
             fprintf('\n')
             fprintf('\n')
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% check for proper BFS ??????? why do this ????????
    if lpfdM > m
	    [~,indsp] = sort(xk,'descend');
	    if xk(indsp(m+1))/norm(xk) < tol
		    xk(indsp(m+1:lpfdM)) = 0;
		    fprintf('setting %i tiny els of last xk to 0\n',lpfdM-m)
		    %xks(:,end) = xk;
	    end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% We know xk'*zk=0; Check strict complementarity xk+zk>0
if verbose
    strcompl = min(xk+zk);
    sumstrcompl = sum((xk+zk) == 0);
    fprintf('END: ')
    fprintf('SC min(xk+zk) = %g; sum(xk+zk)==0 is %i;   ', ... 
        	strcompl,sumstrcompl)
    %objfn = norm(xks(:,end)-v)^2/2;
    objfn = norm(xk-v)^2/2;
    %startend = tic;
    %fprintf('final condest(Jk) %g, ',condest(Jk))
    %fprintf('and time for condest; %g\n',toc(startend))
    fprintf('rel-gap = %g\n',abs(objfn-lbnd)/(1+objfn))
    fprintf('\n')
end

% Return last iterate
%%% Save output in case of counterexample
output.Jk = Jk;  % for a warm start approach
%%%%%%%%%%%%%%%%%%%%%
%output.xfinal = xks(:,end);
%output.zfinal =  zks(:,end);
output.xfinal = xk;
output.zfinal =  zk;
%output.yfinal = yks(:,end);
output.yfinal = yk;
%output.xks = xks;
%output.yks = yks;
%output.zks = zks;
output.relresid = relresid;
%output.lambdareg = lambdareg;
output.lbnd = lbnd;
output.iter = k;
warning on
end   % end of main semismooth function
