function output = semismoothsolverfree(A,b,v,optionsfree) 
%function output = semismoothsolverfree(A,b,v,optionsfree) 
%
%  Solve the nearest/proj. problem min ||v-x|| s.t. Ax=b, x1>=0,x2  free
%  where |x2|=n2 < m rows of A!!
%  Use the opt condiitons   F(y) = A(v+A'*y)_+ - b = 0, _+ proper proj.
%   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 
% optionsfree - 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(optionsfree,'free')     
	free = optionsfree.free;   
else 
	free = [];
end
n2 = length(free);
[m,n] = size(A);
if n2 >=m
	%% make this more general; remove this restriction!!!!
	%% since A(:,free) can be invertible - do NOT solve
	fprintf('ERROR: # free vrbles %i >= m= %i RETURN\n',n2,m)
	return
end
if isfield(optionsfree,'maxiter')     
	maxiter = optionsfree.maxiter;   
else 
	maxiter = 100;
end
if isfield(optionsfree,'modprint')     
	modprint = optionsfree.modprint;   
else 
	modprint = 10;
end
if isfield(optionsfree,'tol')     
	tol = optionsfree.tol;   
else 
	tol = 1e-7;
end

notfree = setdiff(1:n,free);
n1 = n - n2;
m1 = m - n2;
k = 1;   % iter for while loop
bAv = b-A*v;
if isfield(optionsfree,'yk')     
	yk = optionsfree.yk;   
else   
        yk = zeros(size(A,1),1);
end
Ndir = yk;
normNdir = norm(Ndir);
yks = yk;   % save all yk
xk = zeros(n,1);
zk = zeros(n,1);
vAty = v+A'*yk;
xk(notfree) = max(vAty(notfree),0);
Atyk = A'*yk;
zk(notfree) = xk(notfree)-v(notfree)-Atyk(notfree);
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;
lambdareg = 0;
lsetxor = inf;

VarValue = {relresid,lbnd,normNdir};
dM = ones(n,1);
dM(notfree) = sign(vAty(notfree));   % makes small pos numbers 1
lpfdM = sum(dM(notfree) > 0);  % number of positive xi

% Print desirable strings
if optionsfree.verbose
    VarPrint = {'relres/optcond','lower bnd','||Ndir||',  ...
                '|#pos x_j|','setxor cols','zerosx+z','lambda'};
    	              %'|#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')
    
    % Print desirable information
    fprintf('%-9i', k)
    fprintf('%-15e', VarValue{:})
    fprintf('%-15i', lpfdM,lsetxor,sum((xk+zk)==0))
    fprintf('%-15e', 0)
    fprintf('\n')
end
    




%badcond = 0;  % counting number of regulariz steps
fdMold =[];
fdM =[];
lnormv = log(norm(v)); % to help with regularization
condsJK=0;
while relresid(k) > tol && k < maxiter  % k # iters
       
    dMnf = dM(notfree);
    dMnf(dMnf == 0) = 1;   % 'maximal' Jacobian for largest rank
    dMnf(dMnf<0) = 0;   % makes small numbers 0 for projection
    dM(notfree) = dMnf;
    %%?????????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
    
    if true   % always use regularization for now
	    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%% study regulariz???? how to do it properly???
        %lambdareg = min(1e-3,max(tol,relresid))/m;
        %lambdareg = min(1e-3,relresid)/m;  % /m removed as regulariz param too small causes failure???
	%%% should this be adaptive? i.e. m/n small then divide by m? or too many???
	%%% not dividing by m is working well as of jul/11/22
	    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %lambdareg = min(1e-3,relresid);
        %lambdareg = min(1e-3,max(tol,relresid))/m;
	%%%%%%% temp should be what??????????
	%%%% added jul31/2022; emphasize normNdir large increase lambdereg
	if k < 20   % ???? change 20??? 5??? into parameters
		normNdirp = normNdir(end);
	else
		normNdirp = prod(normNdir(end-5:end));
	end
        lambdareg(k) = ...
               1e-2*mean([...
		     relresid(k)*max(1,log(normNdirp)), ...
                     1e-1*relresid(k)*max(1,lnormv), ...  %log(norm(v))
	              1e-1*relresid(k), ...
	              %min(1e-3,relresid(k)), ...
	                      ]);


        %%% Following is Tyler's idea but does it help?????Aug1/22
        %%% adds a random steplength if stalling due to short steps
	%%% ???????????check this out/????? really helps????
        %if mod(k,25) == 0 && lambdareg(k) > 1e-5
        %    lambdareg(k) = 1e-5;
        %end

        %1e-3*relresid*max(1,log(k)) ...
%         if mod(k,25) == 0 && lambdareg(k) > 1e-5
%             lambdareg(k) = 1e-5;
%         end

        Jk = (Jk+lambdareg(k)*speye(m));
        condsJK(k) = cond(Jk);
	%badcond = badcond + 1;
    end  % end of if for using regularization
    %%% 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(notfree) = max(vAty(notfree),0);
    xk(free) = vAty(free);
    Atyk = A'*yk;
    zk(notfree) = xk(notfree)-v(notfree)-Atyk(notfree);
    xks(:,k+1) = xk;
    zk = max(0,xk-vAty);  % only xk-vAty is needed; max used to ensure c.s.
    if norm(zk(free))>0
	    keyboard
    end
    zks(:,k+1) = zk;      % save all xk
    Fk = A*xk-b;
    relresid(k+1) = norm(Fk)/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(k),lbnd};
    if optionsfree.verbose && mod(k,modprint) == 0 % modprint set at while above
             fprintf('%-9i', k)
             fprintf('%-15e', VarValue{:},norm(Ndir))
             lsetxor = length(setxor(fdM,fdMold));
             fprintf('%-15i', lpfdM,lsetxor);
             fprintf('%-15i', sum((xk+zk)==0))
             fprintf('%-15e', lambdareg(k-1))
             fprintf('\n')
    end
    fdMold = fdM;
    dM = ones(n,1);
    dM(notfree) = sign(vAty(notfree));   % makes small pos numbers 1
    lpfdM = sum(dM(notfree) > 0);  % number of positive xi
    
end %%% End of while loop 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if ~optionsfree.verbose || modprint > 0   % print last iteration 
             VarPrint = {'relres/optcond','lower bnd','||Ndir||',  ...
	              '|#pos x_j|','setxor cols','zerosx+z','lambda'};
	              %'|#pos x_j|','setxor pos','(act+m)','deg: n-(act+m)'};
             fprintf('\nAt end of semismooth proj FREE. 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));
             fprintf('%-15i', lpfdM,lsetxor);
             fprintf('%-15i', sum((xk+zk)==0))
             fprintf('%-15e', lambdareg(end))
             fprintf('\n')
             fprintf('\n')
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% check for proper BFS  ????????????fix????????
    if false % lpfdM > m1 ???????????????????????
	    [~,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
strcompl = min(abs(xk)+zk);
sumstrcompl = sum((abs(xk)+zk) == 0);
fprintf('strict compl min(xk+zk) = %g; sum(abs(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.lambdareg = lambdareg;
output.lbnd = lbnd;
output.iter = k;
%output.badcond = badcond;
output.condsJK= condsJK;
output.deginds = intersect(find(xk==0),find(zk==0));
warning on
if optionsfree.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
end   % end of main semismooth function
