function output = Inexactsemismoothsolver(A,b,v,delta,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  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,'maxiter')     
	maxiter = options.maxiter;   
else 
	maxiter = 100;
end
if isfield(options,'tol')     
	tol = options.tol;   
else 
	tol = 1e-7;
end

[m,n] = size(A);
k = 1;   % iter for while loop
if isfield(options,'yk')     
	yk = options.yk;   
else 
	yk = zeros(m,1);    
end
Ndir = yk;
normNdir = norm(Ndir);
bAv = b-A*v;
yks = yk;   % save all yk
vAty = v+A'*yk;
xk = max(vAty,0);
zk = xk-v-A'*yk;
xks = xk;      % save all xk
zks = zk;      % save all xk
normb = (1+norm(b));
Fk = A*xk-b;
relresid = norm(Fk)/normb;

%%% For now:              ?????
%relresid = (1/(2*m))*norm(A*xk-b)^2;
%%% For now:

lbnd = (-norm(zk+A'*yk)^2/2)+yk'*bAv-zk'*v;
lbnds = lbnd;
lfdM = 0;
fdM = 0;
fdMold = 0;
Ndir = 0;
lambdareg = 0;
lsetxor = inf;

VarValue = {relresid,lbnd,Ndir};


% Print desirable strings
if options.verbose
VarPrint = {'relres/optcond','lower bnd','||Ndir||',  ...
	              '||Ndir Resid.||','|#pos x_j|','setxor pos','zerosxz','deg: n-(act+m)'};
	              %'|#pos x_j|','setxor pos','(act+m)','deg: n-(act+m)'};
fprintf('\nUsing the TESTsemismooth testing solver;  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
lfdM = 0;
lsetxor = inf;
VarValue = {relresid,lbnd,0,0};
fprintf('%-9i', k)
fprintf('%-15e', VarValue{:})
fprintf('%-15i', lfdM,lsetxor,sum((xk+zk)==0),n-lfdM)
fprintf('\n')
%fprintf('%-15i', lfdM,lsetxor,(n-lfdM)+m,n-lfdM)
end
    




badcond = 0;  % counting number of regulariz steps
fdMold =[];
asymptotic = [];
lnormv = log(norm(v));
while relresid > tol && k < maxiter  % k # iters
       
    dM = sign(vAty);   % makes small numbers 1
    dM(dM == 0) = 1;   % 'maximal' Jacobian for largest rank
    dM(dM<0) = 0;   % makes small numbers 0 for projection
    fdM = find(dM > 0);   % columns of A for Jacobian
    lfdM = length(fdM);   % # of 'basic' vrbles; positives
    AdMs = A(:,fdM);
    Jk = AdMs*AdMs';   % Jacobian
    
    %if lfdM < m  || issparse(Jk) % generically not full rank Jacobian????
    if true % generically not full rank Jacobian????
        %%% study regulariz???? how to do it properly???
        %lambdareg = min(1e-3,max(tol,relresid))/m;
        %lambdareg = min(1e-3,relresid)/m;

        %%% Added June1/2022 by Tyler
        %if k > 100 && relresid > 1e-4
        %    lambdareg = 0.01;
        asymptotic(end+1) = relresid;  
        %else

        %%% Changed by Tyler June17/2022 to consider 2019.LM.coap
        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, ...
	              %min(1e-3,relresid(k)), ...
	                      ]);
        %end
        Jk = (Jk+lambdareg*speye(m));
        normNresid = (0.8)*lambdareg^(1 + delta/2);
    
	    badcond = badcond + 1;
    end
    R = chol(Jk);   
    % Tyler - July3/2022: maxiter for lsqr is too many iterations?
    [Ndir,~] = lsqr(-R, R'\Fk, normNresid,maxiter); 
    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 xk
    Fk = A*xk-b;
    relresid = norm(Fk)/(1+normb);
    
    %%% For now:  ???????????????
    %relresid = (1/(2*m))*sum(A*xk-b)^2 + (1/(2*n))*sum(max(0,-xk))^2;
    %%% For now:

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

    if ~options.verbose    % print the last iteration
             VarPrint = {'relres/optcond','lower bnd','||Ndir||',  ...
	              '|#pos x_j|','setxor pos','zerosxz','deg: n-(act+m)'};
	              %'|#pos x_j|','setxor pos','(act+m)','deg: n-(act+m)'};
             fprintf('\nUsing semismooth;  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')
             fprintf('%-9i', k)
             fprintf('%-15e', VarValue{:},norm(Ndir))
             lsetxor = length(setxor(fdM,fdMold));
             fprintf('%-15i', lfdM,lsetxor);
             fprintf('%-15i', sum((xk+zk)==0),n-lfdM)
             fprintf('\n')
             fprintf('\n')
    end
%%% We know xk'*zk=0; Check strict complementarity xk+zk>0
strcompl = min(xk+zk);
sumstrcompl = sum((xk+zk) == 0);
fprintf('strict compl min(xk+zk) = %g; sum(xk+zk)==0 is %i \n', ... 
	strcompl,sumstrcompl)

% Return last iterate
%%% Save output in case of counterexample
output.xfinal = xks(:,end);
output.zfinal =  zks(:,end);
output.yfinal = yks(:,end);
output.xks = xks;
output.yks = yks;
output.zks = zks;
output.relresid = relresid;
output.lbnd = lbnd;
output.iter = k;
output.badcond = badcond;
warning on
if options.verbose
    objfn = norm(xks(:,end)-v)^2/2;
    fprintf('END semismooth: ')
    %startend = tic;
    %fprintf('final condest(Jk) %g, ',condest(Jk))
    %fprintf('and time for condest; %g\n',toc(startend))
    fprintf(' #badcond-regulariz = %i, ',badcond) 
    fprintf('and rel-gap = %g\n',abs(objfn-lbnd)/(1+objfn))
    fprintf('\n')
end

% figure(1)
% semilogy(asymptotic)
% is_asymptotic_sorted = issorted(asymptotic,'descend');
% str = num2str(is_asymptotic_sorted);
% title(['Are the Relative Resdiuals using lambda = 0.01 are asymptotic, 1 for Yes, 0 for No:' str])
end   % end of main semismooth function
