clear
%%% Best Approximation Problem
%%%% generates a random feas. set  Ax=b, x1>=0 data; x2 free and a random v
%%% solves min  .5norm(v-x)^2  s.t.  Ax=b, x1>=0, x2 free, |x2| = free
%%% i.e., finds the projection of v onto the feas set 
%%% Various options for choosing the opt as a (nondeg) vertex
%  calls     output = semismoothsolver(A,b,v,options); for the projection
addpath(genpath('../Solvers')); 

%seed = 1;
%rngsave1 = rng(seed);
%rngsave = rng('shuffle');
%save('rngfile','rngsave');
%load('rngfile')
%rng(rngsave)
    
%%% Options
options.maxiter = 9000;
options.tol = 1e-14;
options.profile = false;
options.modprint = 1;  % number of iterations between printing in solver
simplex = false;
xvertex = true;   % choose v so opt. x is a vertex
polarconev = true;   % choose obj so x-v is in polar cone 
                            %(v-x in normal cone)
randxpos = 0; 
        % 0 choose random number of xpos >m; pos number is fixed number
lengthv = .1;   % multiply v to make problem harder
options.verbose = true;
loadcvgfailexample = false;

expon = 4;
%n = 3*10^expon;
n = 180;
m = 19;
%density = .0001;
density = .008;
free = min(m-1,round(rand*m));    
n2 = free;
n1 = n - n2;

if simplex
	m = 1;
	density = 1;
        A = ones(1,n);     % to compare onto simplex with Ting Kei code???
        b = 10^sqrt(expon);
else
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	%% use seeds to find counterexample????
      %rngsave = rng(seed,'twister');
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	tic
	%% n active set == m equal constr + n-m in the n1 constr vrbls
	m1 = m-n2;   % number of basic variables
    xB = 10*(rand(m1,1)+.1);   % nondeg BFS, also ensures strict feas.
 	nperm = randperm(n);
	m1perm = nperm(1:m1);  % basic vrbles constrained
	nonbas = setdiff(nperm(1:n1),m1perm);  % x nonbasic variables so z1>0
	free = nperm(n1+1:end);
	notfree = nperm(1:n1);
	options.free = free;
    
    rc = .01;
    
    A(:,nonbas) = sprandn(m,n-m,density,rc);
    A(:,[m1perm free]) = sprandn(m,m,density,rc); % ensure ONE proper basis matrix
	x0 = zeros(n,1);
	x0(m1perm) = xB;   % for a ***nondeg.*** BFS
	x0(free) = randn(n2,1);
	x0 = x0/norm(x0);
	nA = norm(A,'fro');
	A = sparse(A/nA);
	b = A*x0;
	fprintf('generation time for new random problem = %g\n',toc)
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Generating v
if polarconev   % choose obj so x*-v is in polar cone
        if xvertex % ensure a vertex is opt.
	       fprintf('Choosing v to ensure a NICE vertex is optimal\n')
	       options.nperm = nperm;
	       xo = x0;  % choose the constructed nice vertex
	       fprintf('#pos else in xo = %i; m= %i \n',sum(xo>0),m)
	       fprintf('rank of corresp B submatrix = %i\n', ...
		       rank(full(A(:,find(xo>0)))));
	       zo = zeros(n,1);
	       zo(nonbas) = lengthv*(rand(length(nonbas),1)+.1);
	       yo = randn(m,1);
	       v = xo -A'*yo - zo;   % xo = v+A'*yo+zo is optimal
	          % norm(xo - v+A'*yk+zk) is distance to true opt xo at xk
	%%% verify optimality conditions at xo
	Atyo = A'*yo;
	dfxo = xo - v - Atyo - zo;
	pfxo = A*xo - b;
	csxo = xo'*zo;
	fprintf('xo optimal vertex generated; norm KKT = %g\n', ...
		norm(dfxo)+norm(pfxo)+norm(csxo))
	%???? done above??? notfree = nperm(1:n1);
	elseif randxpos == 0 % choose a random number of pos
	       xpnum = randi(n-m,1,1); % m+xpnum els of x > 0
	       fprintf('Choosing v to ensure opt fixed x with ')
               fprintf('m+%i = %i positive xi''s \n',xpnum,m+xpnum)
	       xo = x0; 
	       randxN = randperm(length(nonbas));
	       %% add pos els to BFS xo
	       xo(nonbas(randxN(1:xpnum))) = 10*(rand(xpnum,1)+.1);
	       xo = xo/norm(xo);
	       b = A*xo;
	       xzdeg = randi(min(m,n-m)-1,1,1); % # overlapping zeros
	       zo = zeros(n,1);
	       zo(nonbas(randxN(xpnum+1:end))) = ...
	                  lengthv*(rand(length(nonbas)-xpnum,1)+.1);
	       indszpos = find(zo>0);
	       randzpos = randperm(length(indszpos));
	       xzdeg = min(20,length(randzpos));  %????
	       zo(indszpos(randzpos(1:xzdeg))) = 0; % set degeneracy
	       fprintf('xzdeg: setting %i els of zo to zero \n',xzdeg)
	       yo = randn(m,1);
	       v = xo -A'*yo - zo;
	end
else
        v = randn(n,1);
        v = lengthv*v/norm(v);   % how large should v be?????
end
if loadcvgfailexample
    %load('counterexample.mat')
    load('cvgncefail.mat')
    %v = .94*v;
    nA = norm(A,'fro');
    A = A/nA;b = b/nA;
end
[m,n] = size(A);
fprintf('Problem has m,n,n2(free),density =: %i, %i, %i, %i \n', ...
	          m,n,n2,density)


if simplex   %???????make special nonsmooth since Jk is 1by1
	tic
	d = TKPsimplex_proj(v,b);
	timesimplex = toc;
   if options.profile
	   profile clear
	   profile on
   end
        tic
        output = simplexsemismoothfree(b,v,options);
   if options.profile
	   profile report
   end
        timesemism = toc;
	fprintf('times semism/simplex  %g / %g \n',timesemism,timesimplex)
	fprintf('relres semism/simplex  %g / %g \n', ...
	        norm(sum(output.xfinal)-b)/b,norm(sum(d)-b)/b)
else
   if options.profile
	   profile clear
	   profile on
   end
    tic
    %[x,prs,dists,dist2s,optcond] = HLWB_1(v,A,b,options.maxiter,xo);
    output = semismoothsolverfree(A,b,v,options);
    %output = HLWB2solver(A,b,v,options);
    %output = semismooth_Sup(A,b,v,options);
    timesemism = toc;
if options.profile
	profile report
end
	fprintf('time semism algor  %g \n',timesemism)
	x = output.xfinal;
	z = output.zfinal;
	y = output.yfinal;
end

