clear
 profile clear
 profile on
seed=1;
rng(seed);         % comment out if same problems desired
%rng('shuffle');         % comment out if same problems desired
%rngsave = rng;          % comment out if same problems desired
%save('rngfile','rngsave')

%r = 3;              %for Waki-Muramatsu problem
%n = 3*r+1;
%sd = 2*r-1;

n = 15;             %set dimension parameters
sdvec = 9;             %singularity degree
%sdvec = 1:9;

tn = n*(n+1)/2;     %triangular number
doprecon = 0;       %1-run precon, 0-don't run
deflatenum = 0;     %number of deflation steps, 0-no deflation steps         
delta = 1;          %vector of trace-bounding values 
tol = 14;           %tolerance is 10^(-tol)
frtol = 1e-13;       %tolerance for deflation
bmod = 1;           %1-modify b, 0-don't       
%tts = logspace(-1,-14,100);   %for alpha loop
tts = 1;


%%%%%%%%%%%%%%%%%%%%%for ssMat/ssvec
n2=n^2;
svind = find(triu(ones(n),1)); %linear indices of the strictly upper triangle
sMind = zeros(n2,1); %matrix indices with respect to linear indices of the diagonal and the strictly upper triangle
sMind(svind) = 1:tn-n; %strictly upper triangle
sMind(1:n+1:n2) = tn-n+1:tn;%the diagonal
%calculate the linear indices of the strictly lower triangle
indM = zeros(n); %matrix indices
indM(:) = 1:n2;
indM = indM';
lsvind = indM(svind); %strictly lower triangular 
sMind(lsvind) = 1:tn-n; %strictly lower triangular

if length(delta)>1
    figure(1)
    clf
    hold on
end
if length(sdvec)>1
    distXF = zeros(length(sdvec),1);
    distXAb = zeros(length(sdvec),1);
    figure(3)
    clf
    title('Feasibility vs. Singularity Degree','Interpreter','latex','FontSize',20)
    xlabel('${\rm sd}(\mathcal{F})$','Interpreter','latex','FontSize',20)
    hold on
end

pfeasvec = zeros(length(delta),1);

Xstore = zeros(n,n,length(tts));
alogdetavec = zeros(length(tts),1);
for tt=1:length(tts)
    for kk=1:length(sdvec)
        for j=1:length(delta)
            
            %% Generalized Levent Problem
 %            Rot = eye(n);
 %            [A,b,Xorig] = MakeProblemLevent(n,delta(j),svind,sdvec(kk),Rot);
 %            
 %            if length(tts)>1
 %                b = b+tts(tt)*A*ssvec(eye(n),svind);
 %            end
             
            
            %% Toeplitz Cycle Completion
            %[A,b,Xorig,Exp,theta,phi] = MakeProblemToeplitz(n,2,svind,pi/9,pi);
            
            %% Waki-Muramatsu Problem
            %[A,b,Xorig] = MakeProblemWaMu(r,svind);
            
                    %% Wei-Wolkowicz Problem Generation
                        dualHasInteriorFlag=1;      % for dual strict feasibility
                        m = 2*n;
                        NoP=round(n/2);             %maximal primal rank
                        %NoD=n-NoP;                  %maximal dual rank
                        %gap=n-NoP-NoD;              %complementarity gap
			%%%%%%%%% to try for S.D. = 2 ... next two lines
                        gap=2;              %complementarity gap
                        NoD=n-NoP-gap;                %maximal dual rank
                        [AA, bb, C, Xorig, yorig, Zorig, status] = ...
                            genBlockHard(m-1, n, gap, NoP, NoD, dualHasInteriorFlag);
                        A = zeros(m,tn);
                        for i=1:m-1
                            A(i,:) = ssvec(AA{i},svind);
                        end
                        A(m,:) = ssvec(C,svind)';
                        AA{m} = C;
                        b = A*ssvec(Xorig,svind);
            
            m = size(A,1);
            Aorig = A;
            borig = b;
            XO = Xorig;
            norig = n;
            morig = m;
            svindorig = svind;
            sMindorig = sMind;
            
            fprintf('new problem in runGN size m,n  %i %i \n',m,n)
            
            %% precondition routine
            rngstor = cell([1 n-1]);
            psd = 0;
            sdcount = 0;
            if doprecon==1
                fprintf('Starting preconditioning routine. \n')
                [V,A,b,psd] = precon(A,b,n,sMind);
            end
            while psd>0 && sdcount<=(norig-2)
                sdcount = sdcount+1;
                rngstor{sdcount} = V;
                XO = V'*XO*V;
                [A,b,svind,sMind,n,m] = freduce(A,b,V,n,sMind);
                [V,A,b,psd] = precon(A,b,n,sMind);
            end
            if doprecon==1
                fprintf('Preconditioning steps: %i \n', sdcount)
                fprintf('Preconditioning dimension reduction: %i \n', morig-m)
            end
            %% facial reduction loop
            countb = 0;
            inds = 0;
            while sum(inds)<n && sdcount<=(norig-2) && countb<=deflatenum    %loop for automatic facial reduction
                
                if countb>0
                    sdcount = sdcount+1;
                    rngstor{sdcount} = V;
                    XO = V'*XO*V;
                    [A,b,svind,sMind,n,m] = freduce(A,b,V,n,sMind);
                end
                
                %%%%%%%%%%%%%%%%%%%%% Set parameters and run problem
                data.A=A;
                data.b=b;
                data.m=m;
                data.n=n;
                data.XO=XO;
                params.quiet=false;
                params.maxiter=63;
                %params.maxlastiter=-1;          %set to negative if no extra iters desired
                params.maxlastiter=10;          %set to negative if no extra iters desired
                params.svind=svind;
                params.sMind=sMind;
                params.tolalpha = 10^(-tol-countb);        %tolerance for alpha
                %params.tolResNorm = 1e-13;      %tolerance for residuals
                params.tolResNorm = 10^(-tol-countb);      %tolerance for residuals
                
                [output,psdflag] = IPmaxdetGN(data,params);
                alpha=output.alpha;
                X = output.X;
                Z = output.Z;
                
                [Q,D] = eig(X);
                inds = diag(D>frtol);
                V = Q(:,inds);
                countb = countb+1;
                
                if bmod==1
                    %keyboard
                    Vnot = Q(:,~inds);
                    Xnot = Vnot*D(~inds,~inds)*Vnot';
                    b = b - A*ssvec(Xnot,svind);
                end
                
                
            end
            
            %% recovery and analysis
            V = eye(norig);
            for i=1:sdcount
                if ~isempty(rngstor{i})
                    V = V*rngstor{i};
                end
            end
            XX = V*X*V';    %recovered X in original problem dimensions
            XX = (XX+XX')/2;
            ZZ = V*Z*V';    %recovered Z in original problem dimensions
            ZZ = (ZZ+ZZ')/2;
            
            %%%%%%%%%%%%%%%%%%%% Dual Approach
            % cvx_begin  quiet
            % cvx_precision best
            % variable y(m,1)
            % %cvx_solver mosek
            % minimize abs(b'*y)
            % subject to
            %      ones(1,tn)*A'*y == 1;
            %      ssMat(A'*y,n,sMind) == semidefinite(n);
            % cvx_end
            % W = ssMat(A'*y,n,sMind); %Exposing vector
            
            %%%%%%%%%%%%%%%%%%%% Check feasibility, psd, etc.
            normAb = norm(Aorig,'fro')+norm(borig);
            [Q,D] = eig(XX);
            mineigX = D(1);
            mineigZ = min(eig(ZZ));
            pfeas = norm(Aorig*ssvec(XX,svindorig) - borig)/normAb;
            pfeasalpha = norm(Aorig*ssvec(XX,svindorig) - borig-alpha*Aorig*ssvec(eye(norig),svindorig))/normAb;
            
            fprintf('\nrelative ||A(X)-b||:             %g \n',pfeas);
            fprintf('relative ||A(X)-b(alpha)||:      %g \n',pfeasalpha);
            fprintf('relative mineig(X):              %g \n',mineigX/norm(XX,'fro'));
            fprintf('relative mineig(Z)):             %g \n',mineigZ/norm(ZZ,'fro'));
            fprintf('relative trace(ZX):              %g \n',trace(ZZ*XX)/normAb);
            fprintf('|trace(ZX)-n*alpha|:             %g \n',abs(trace(ZZ*XX)-norig*alpha));
            
            
            %delta curve plot
            if length(delta)>1
                pfeasvec(j) = pfeas;
                plot(delta(j),pfeas,'.','MarkerSize',25)
            end
        end
        X11 = X(1:(n-sdvec(kk)),1:(n-sdvec(kk)));
        Xnormopt = zeros(n);
        Xnormopt(1:(n-sdvec(kk)),1:(n-sdvec(kk))) = X11+(1/n)*(1-trace(X11))*eye(n-sdvec(kk));
        Xnormopt = X - Xnormopt;
        distXF(kk) = norm(Xnormopt,'fro');
        distXAb(kk) = pfeas;
    end
    alogdetavec(tt) = tts(tt)*log(det(X));
    Xstore(:,:,tt) = X;
end
if length(delta)>1
    title('Trace bound vs. Relative Residual')
    xlabel('\delta')
    ylabel('Relative Residual')
    hold off
    
    figure(2)
    clf
    plot(delta,pfeasvec)
elseif length(sdvec)>1
    %plot(distX,'.','MarkerSize',25,'Color','k');
    plot(log10(distXF),'.','MarkerSize',25,'Color','k');
    plot(log10(distXAb),'x','MarkerSize',10,'Color','k');
    leg1 = legend('$\log_{10}({\rm dist}(\tilde{X},\mathcal{F}))$','$\log_{10}({\rm dist}(\tilde{X},(\mathcal{A},b)))$','location','best');
    set(leg1,'Interpreter','latex');
    set(leg1,'FontSize',20);
    hold off
else
    figure(1)
    clf
    hold on
    plot((1:n),log10(sort(eig(X),'descend')),'.','MarkerSize',25,'Color','k')
    plot((1:n),log10(sort(eig(Z),'ascend')),'x','MarkerSize',10,'Color','k')
    %plot((1:n),log10(sort(abs(eig(W)),'ascend')),'^','MarkerSize',10)
    leg1 = legend('$\log_{10}(\lambda_i(\tilde{X}))$','$\log_{10}(\lambda_i(\tilde{Z}))$','location','best');
    set(leg1,'Interpreter','latex');
    set(leg1,'FontSize',20);
    title('Eigenvalues for the case ${\rm sd}(\mathcal{F})=1$, $r=5$, $n=10$','Interpreter','latex','FontSize',20);
    xlabel('$i$','Interpreter','latex','FontSize',20);
    hold off
    
    figure(2)
    clf
    hold on
    plot(0,pfeas,'.','MarkerSize',25)
    for k=1:1
        D(k,k) = 0;
        Xtemp = Q*D*Q';
        Xtemp = (Xtemp+Xtemp')/2;
        tempfeas = norm(Aorig*ssvec(Xtemp,svindorig)-borig)/normAb;
        plot(k,tempfeas,'.','MarkerSize',25)
    end
    title('Feasibility vs. Eigenvalue Deflation')
    hold off
end
profile report
