clear
%%% use semismooth to solve an LP problem
%%% max c'x   s.t. Ax=b x>=0; dual: min b'y s.t. A'y>=c (c=A'y-z,z>=0)
%% replace v = R*c scaling by  b = b/R scaling with Rx converging to opt.  x*

addpath(genpath('../Solvers')); 

%seed = 1;
%rngsave = rng(seed);
rngsave = rng('shuffle');
save('rngfile','rngsave');
%load('rngfile')
%rng(rngsave)
    
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%set options
m=2500;
n=90000;
maxiter = 1200;
density=0.0005;
dexp = 14;
nondegvertex=true;   % true for now as other generation not working yet
tolorig = 10^(-dexp);
setprofile = false;
if setprofile
profile clear
profile on
end 
setverbose = false;
setverboseplot = false;
modprint = 1;
lengthv=0.1;    % difficulty of problem
%%%%%%%%%
dualsimplex = false;   % false for interior-point in linprog
options.m = m;
options.n = n;
options.density = density;
options.nondegvertex = nondegvertex;
options.dexp = dexp;
options.tol = tolorig;
options.maxiter = 1200;  % for solver
options.tolLP = tolorig;
options.profile = setprofile;
options.modprint = modprint;   % number of fprints in while in solver
options.verbose = setverbose;
options.verboseplot = setverboseplot;
options.lengthv = lengthv;   % larger makes problem harder

%%%% generate problem with a nondegerate vertex as opt. (generically)
if nondegvertex  % a nondegenerate vertex generically
    startgen = tic;
    rc = .01;  % for sparsity recip. cond. #
    xB = 10*(rand(m,1)+.1);   % nondeg BFS also ensures strict feas.
    nperm = randperm(n);
    mperm = nperm(1:m);   % indices of basic columns
    nonbas = setdiff(1:n,mperm);  % x nonbasic variables so z>0
    A = spalloc(m,n,m*n*density);
    A(:,nonbas) = sprandn(m,n-m,density,rc);
    %% the following, generically, makes basis matrix nonsingular
    A(:,mperm) = sprandn(m,m,density,rc); % ensure ONE proper basis matrix
    x0 = zeros(n,1);
    x0(mperm) = xB;   % for a ***nondeg.*** BFS
    x0 = x0/norm(x0);  % optimal xo norm = 1
    nA = norm(A,'fro');
    A = sparse(A/nA);
    b = A*x0;
    xo = x0;  % choose the constructed nice vertex
    zo = zeros(n,1);
    zo(nonbas) = lengthv*(rand(n-m,1)+.1);
    yo = randn(m,1);
    c = A'*yo - zo;  % dual str. feas.
    trueoptval = c'*xo;  % true optimal value
    endgentime = toc(startgen);
    fprintf('time to generate random problem %g\n',endgentime)
else  % not polished yet??????????????
    A = randn(m,n);
    A = A/norm(A,'fro');
    x0 = [(rand(m,1)+.1) ;zeros(n-m,1)];
    x0 = x0/norm(x0);
    z0 = [zeros(m,1) ;rand(n-m,1)+.1];
    z0 = 2*z0/norm(z0);
    inds = randperm(n);
    x0 = x0(inds);
    z0 = z0(inds);
    b = A*x0;
    y0 = randn(m,1);
    y0 = 2*y0/norm(y0);
    c = A'*y0-z0;         %      A'y >= c;  c = A'y-z
%%    %%%???????is cvx too expensive?????
%%    cvx_begin quiet
%%    cvx_precision best
%%    variable xx(n,1) 
%%    dual variable yy
%%    dual variable zz
%%    maximize(c'*xx)
%%    subject to 
%%       yy:  A*xx == b;
%%       zz:  xx >= 0;
%%    cvx_end
%%    cvxoptval = cvx_optval;
%%    cvxdualfeas = norm(-A'*yy-c-zz)/(1+norm(c));
%%    cvxprimfeas = norm(A*xx-b)/(1+norm(b));
%%    cvxcomplslack = abs(xx'*zz)/(1+max(norm(xx),norm(zz)));
%%    fprintf('\n\nNew Problem with m,n,density,optval = %i %i %i %g \n', ... 
%%           m,n,density,cvxoptval)
%%    fprintf('\n\ncvx primfeas/dualfeas/complslack %g %g %g \n', ...
%%        cvxdualfeas,cvxprimfeas,cvxcomplslack)
end



% Print desirable strings
%VarPrint = {'c*x','relres'};
%fprintf('%-9s', 'iter')
%fprintf('%-15s', VarPrint{:})
%fprintf('\n')

Roptvals = [];
Roptvalsdiff = [];
options.yk = zeros(m,1);

if options.verbose
      VarPrintint = {'m','#xj>0'};
      VarPrint = {'optvalue','relresid'};
      fprintf('\nUsing nonsmooth Newton  to solve a NEW LP problem \n')
      %fprintf('%-15s', 'R')
      %fprintf('%-9s', VarPrintint{:});
      %fprintf('%-15s', VarPrint{:});
      %fprintf('\n')
end
%relerroropt = inf; %%% This needs to get reassigned in while loop???
                   %%% otherwise iter grows to 500 every time
%Rstep = 1;
Rchange = .1;   % small increase to force new basis for warm start
%R = 10;   % R start??? more adaptive???
R = min(50,sqrt(m*n)*norm(b)/(norm(c)+1)); %??why use this estimate??
Rs = R;
iter = 0;
countbasic = [];
countxfinalpos = [];
%vR = R*c;
%vs = vR;
bR = b/R;
lbnd = -inf;  % lower bound on optimal value
newlbnd = lbnd;
ubnd = inf;
newubnd = inf;
cxcnt = 0;
cycnt = 0;
relresidprojs = inf;

%%% How does R impact performance??
tols = [];
minoriters = [];
normc = norm(c)+1;
relpfdf = inf;
startticLP = tic;   % start time for LP loop
while (R < inf) 
    iter = iter + 1;  % major iterations
    %%% increase accuracy at each iteration for projection
    options.tol = max(tolorig, ...
    min([1e-8 .01*relpfdf .01*relresidprojs(end)])); % increase accuracy 
    tols = [tols options.tol];
	%options.Rs = Rs;   % why??? needed??
    if options.verbose
        fprintf('\n\n LP solve starting major iter. %i \n',iter)
	 %   norm(x)/R %g ,norm(z)/R %g ,' ...
	 %         iter,norm(x)/R,norm(z)/R)
	 %  fprintf('(norm(A''*(-y/R)-c-(z)/R))/norm(c)  %g %g %g\n',...
         %  (norm(A'*(-y/R)-c-(z)/R))/norm(c))
    end
    %output = semismoothsolver(A,b,vR,options);
    output = semismoothsolver(A,bR,c,options);
    relresidprojs = [relresidprojs output.relresid(end)];
    minoriters = [minoriters output.iter];
    xapprox = R*output.xfinal;
    %yapprox = output.yfinal;
    %zapprox = output.zfinal;
    %pfapprox = norm(A*xapprox-b);
    %dfapprox = norm(c+A'*(-yapprox)+zapprox);
    %csapprox = sum(prod([xapprox zapprox],2));
    %KKTapprox = pfapprox+dfapprox+csapprox;
    newlbnd = c'*xapprox;  %  lower bound
    %% change to no improv in lower or upper bounds?????
    if newlbnd <= lbnd   % no improvement in lower feas bnd
	cxcnt = cxcnt+1;
    else
        lbnd = newlbnd;
	lbnds(iter) = lbnd;
	cxcnt = 0;
    end

    Roptvals = [Roptvals newlbnd];
    %relerroropt = abs(trueoptval - Roptvals(end))/(1+abs(trueoptval));

%    if options.verboseplot
%        %%% Plot optimal values
%	    figure(1)
%	    clf
%	    plot([Roptvals trueoptval],'-.')
%	    title('improving lower bounds to cvxoptval')
%    
%        %%% Plot difference to cvxoptval
%        %Roptvalsdiff = [Roptvalsdiff relerroropt];
%	%    figure(2)
%	%    clf
%	%    semilogy(Roptvalsdiff,'-.')
%	%    title('relative difference between cvxoptval and Roptval')
%    end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%do Tapia indices work without quadr cnvgnce???????????
%%%    %%% Tapia Indices : xtapia = x^{k+1} / x^k
%%%    if iter >= 2
%%%
%%%        xtapia = output.xfinal./xpre;
%%%
%%%        %%% To handle dividing by zero
%%%            xprezero = find(xpre == 0);
%%%            xcurrentzero = find(output.xfinal == 0);
%%%            
%%%            %%% To handle NaN values:
%%%            % We use the intersection of indices as 
%%%            % find(xpre == 0) is not always a subset of find(output.xfinal == 0)
%%%            xtapia(intersect(xprezero, xcurrentzero)) = 0;
%%%        %%% To handle dividing by zero
%%%
%%%        %%% Based on change in x, we have the following pos. variables
%%%        % measured by the rate of change between iterations
%%%        xbasic = find(xtapia > 0.1);
%%%        countbasic = [countbasic length(xbasic)];
%%%    end
%%%    xpre = output.xfinal;
%%%    %%% end of Tapia Indices
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    xfinalpos = sum(output.xfinal>0);
    countxfinalpos = [countxfinalpos xfinalpos];

    if options.verbose
        fprintf('%-15s', 'R')
        fprintf('%-9s', VarPrintint{:});
        fprintf('%-15s', VarPrint{:});
        fprintf('\n')
        fprintf('%-15e',R) 
        fprintf('%-9i', m,xfinalpos)
        fprintf('%-15e',Roptvals(end),output.relresid(end))
        fprintf('\n')
    end
    %x = output.xfinal;
    %y = output.yfinal;
    %z = output.zfinal;

    %if options.verboseplot
%	    xplot % plot sorted positive x values
%    end
    

    %%%% calculate new y for warm start
    wopt = output.xfinal;
    zopt = output.zfinal;
    yopt = output.yfinal;
    indsN = find(zopt > 0);
    indsB = setdiff(1:n,indsN);
    wBopt = wopt(indsB);
    zNopt = zopt(indsN);
    AB = A(:,indsB);  % basic type cols
    AN = A(:,indsN);  % nonbasic type cols
    deltaypre = lsqminnorm((AB*AB'),b);
    %bB = AB'*lsqminnorm((AB*AB'),b);
    bB = AB'*deltaypre;
    etemp = bB-R*wBopt;
    ftemp = R*bB;
    indsratio = find(etemp > 1e-6);
    
    if isempty(indsratio)
	    wBstep = inf;
    else
        wBstep = min(ftemp(indsratio)./etemp(indsratio));
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %bN = AN'*lsqminnorm((AB*AB'),b);
    bN = AN'*deltaypre;
    etemp = -(bN+R*zNopt);
    ftemp = -R*bN;
    indsratio = find(etemp > 1e-6);

    if isempty(indsratio)
	    zNstep = inf;
    else
        zNstep = min(ftemp(indsratio)./etemp(indsratio));
    end

    Rnew = min(wBstep,zNstep);
    if Rnew < R
	    fprintf('Rnew < R???error????\n')
	    keyboard
    elseif Rnew == inf
	    fprintf('\nopt. LP basis found in while loop, Rnew=inf; ')
        yoptLP = lsqminnorm((A(:,indsB)'),c(indsB));
	    xoptLP = zeros(n,1);
        xoptLP(indsB) = lsqminnorm(A(:,indsB),b);
	    %yoptLP = (A(:,indsB)')\c(indsB);
	    zoptLP = zeros(n,1);
	    zoptLP(indsN) = A(:,indsN)'*yoptLP - c(indsN);
	    fprintf(' and min zoptLP dual opt check is %g \n',min(zoptLP))
	    R = Rnew;
	   % keyboard  % ???? works?? returns where??? fix logic???
    else
	    R = Rnew;
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %Rstep = min(ratiov)+(Rchange/iter);  
    % without Rchange, the current basis should NOT change

    if R < inf   % opt not found yet
        %options.yk = (R/(R + Rstep))*output.yfinal;   % for input for next run
        %options.yk = 0*options.yk;
        %R = R + Rstep;
        deltay = ( (1/R) - (1/Rs(end)))*deltaypre;
        deltaw = AB'*deltay;  % confirms dual feas. by construction
        deltaz = -AN'*deltay;
        pfcheck = norm(AB*deltaw - ( (1/R) - (1/Rs(end)))*b);
        pfdfcheck = -min([0;wBopt+deltaw;zNopt+deltaz]);
        if max(pfdfcheck,pfcheck) > 1e-8
	        fprintf('Rn step too large???\n')
	        keyboard
        end
        options.yk = yopt + deltay;
        R = R + Rchange/(iter^2);  % increase R slightly beyond last opt.
        Rs = [Rs R];
        bR = b/R;
    end  % of if R == inf
end   % end of while for Rs
stopticLP = toc(startticLP);  % end time for LP loop 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%check for final LP optimality using the xapprox - and assume it is opt.
%%% do not need this again here at end???
indsB = find(xapprox>0);  % opt basis
if length(indsB) ~= m
	fprintf('\nend LP: basis has wrong length, not m, at end of code\n')
end

%%default algorithm is dual simplex
if dualsimplex
	fprintf('using dual simplex in linprog\n')
        linprogoptions = optimoptions('linprog','Display','none');
else
	fprintf('using interior-point in linprog\n')
        linprogoptions = optimoptions('linprog','Display','none', ...
          	'Algorithm','interior-point');
end
startticlinprog = tic;
[xlinprog,flinprog,flaglinprog,outputlinprog,lambdalinprog] = ...
	       linprog(-c,[],[],A,b,zeros(n,1),[],linprogoptions);
stopticlinprog = toc(startticlinprog);  % end time for linprog

pflinprog = norm(A*xlinprog-b)/(1+norm(b));
dflinprog = norm(lambdalinprog.lower - A'*lambdalinprog.eqlin+c)/(1+norm(c));
pfproj = norm(A*xoptLP-b)/(1+norm(b));
dfproj = norm(zoptLP - A'*yoptLP+c)/(1+norm(c));
fprintf('gen. prob time; [proj-time linprog-time] = %g; [%g, %g] \n', ...
	      endgentime,stopticLP,stopticlinprog)
fprintf('diff. opt cxlinprog-cxproj=%g; ', ...
	 (c'*xlinprog-c'*xapprox))
fprintf(' [opt conds: proj linprog] = [%g %g]\n', ...
         pfproj+dfproj,pflinprog+dflinprog)

fprintf('%i major iterations ',iter)
fprintf('and each is: ')
fprintf('%-7i', minoriters)
fprintf('\n')
fprintf('R values are:')
fprintf('%-15e',Rs)
fprintf('\n')
if setprofile
profile report
end
