%%%%%%%%%%%%%%%%% IPGN %%%%%%%%%%%%%%%%%%%%
%
%Implementation of the Gauss-Newton Interior Point algorithm for finding
%a relative interior point of a spectrahedron.  Our approach is based on
%references [9] and [15] from our manuscript.
%
%Inputs:
%  data - A,b,m,n: define spectrahedron
%         XO: known relative interior point
%  params - quiet: controls printed output
%           maxiter: iteration bound
%           maxlastiter: number of additional high-accuracy steps at the
%                           end
%           svind, sMind: speed up ssvec and ssMat
%           tolalpha, tolResNorm: stopping tolerances
%
%Outputs:
%  output - X,Z: relative interior primal-dual solutions 
%           eigXstore,eigZstore,diagZstore: stored eigenvalues and 
%                       diagonal values of X and Z sequences
%           alpha: the value of alpha at termination
%           ResNorm: norm of residual at termination
%           psdflag: 1 if early termination, 0 otherwise
%
%Functions Called: ssvec, ssMat, vec
%
%Sremac, Woerdeman, and Wolkowicz
%2020


function    [output,psdflag] = IPGN(data,params)

%%%%% Given data and parameters for  Ax=b, A m by n, X psd
A=data.A;
b=data.b;
m=data.m;
n=data.n;
X0=data.X0;       %known feasible relint point, obtained in construction
quiet=params.quiet;
maxiter=params.maxiter;
svind=params.svind;     %for ssvec
sMind=params.sMind;     %for ssMat
tolalpha = params.tolalpha;
tolResNorm = params.tolResNorm;
maxlastiter = params.maxlastiter;

%%%%% Additional Tracking Storage
eigXstore = zeros(maxlastiter,n);
eigZstore = eigXstore;
diagZstore = eigXstore;

%%%%% Some constants
In = eye(n);
n2 = n^2;
tn = n*(n+1)/2;         %number of variables in G-N formulation
tnm = tn-m;

%%%%% Ensure problem is bounded
%Check boundedness by projecting In onto the linear manifold range(A')
[Y,R]=qr(A',0);
bbar=Y'*ssvec(In,svind);
y0=R\bbar;
%If unbounded, introduce a trace constraint
projeig = min(eig(ssMat(A'*y0,n,sMind)));
if (projeig <= 10^(-12))
    if quiet == 0
        fprintf('problem is unbounded, introducing trace constraint using XO supplied by the user. \n')
    end
    temp = ssvec(eye(n),svind);
    A = [temp'; A];
    b = [trace(X0); b];
    m = m+1;
    tnm = tn-m;
    y0 = zeros(m,1);        %a new strictly feasible dual starting point
    y0(1) = 1;
end


%%%%% Nullspace representation of manifold Ax=b
V = null(A);            %nullspace of A - assuming A is full row rank
VVt=V*V';
Adagb = A\b;
bI = sum(A(:,end-n+1:end),2);  %=  A*ssvec(In,svind);  - i.e. sum of traces
AdagbI = A\bI;

%%%%% Initial variables
y = y0;
v = zeros(tnm,1);
X = In;
x = ssvec(X,svind);
Z = ssMat(A'*y,n,sMind);
z = ssvec(Z,svind);
alpha = 1;              %initial value of alpha - for parametrized problem
xhat = Adagb + alpha*AdagbI;   % A xhat = b(alpha)
if quiet == 0
    fprintf('rel. accuracy for original xhat is %g with alpha = %g \n',...
        (norm(A*xhat-b-alpha*bI)/norm(b+alpha*bI)),alpha);
end

%%%%% Initial residuals
Rc=Z*X-alpha*In;    %complementary slackness
rd= A'*y-z;         %dual feasibility
rp=x-xhat-V*v;      %primal feasibility
ResTot=vec(Z*ssMat(rp,n,sMind)-ssMat(rd,n,sMind)*X-Rc); %total residual

%keyboard

%%%%% Stopping conditions
iter = 0;                   %iteration count
normAb = norm(A)+norm(b);   %for computing relative error
ResNorm = norm(ResTot)/normAb;
psdflag = 0;                %for early termination
contalg = ResNorm > tolResNorm && alpha > tolalpha && iter<maxiter;
lastiter = 0;               %detection of termination

%%%%% Housekeeping
sigma = 0.6;            %initialize sigma for decreasing alpha
%maxiter calculated when step lengths are 1
maxiter = max(maxiter,ceil(log10(tolalpha)/log10(sigma))+3);
Fp = zeros(n2,tn);      %Jacobian for G-N step
sMV = cell(tnm,1);      %for assigning values to Jacobian
sMA = cell(tn-tnm,1);
for ii=1:tnm
    sMV{ii} = ssMat(V(:,ii),n,sMind);     %columns corresponding to v
end
for ii=1:tn-tnm
    sMA{ii} =  ssMat(A(ii,:)',n,sMind);  %columns corresponding to y
end

%%%%% Display progress
if quiet == 0
    fprintf('\n\n iter  alphaX     alphaZ     ResNorm       alpha        norm(Rc)      norm(rp)    norm(rd)    norm(Ax-b)  norm(v)      descent      descent2 \n');
end

%%%%% Main While Loop
while contalg || (lastiter <= maxlastiter  &&...
        (norm(rd)/normAb>0 || norm(rp)/normAb>0 ||...
        norm(Rc,'fro')> tolResNorm*normAb) )
    
    iter = iter + 1;
    
    %%%%% Matrix representation of Jacobian
    for ii=1:tnm
        Fp(:,ii) = vec(Z*sMV{ii});   %columns corresponding to v
    end
    for ii=1:tn-tnm
        Fp(:,ii+tnm) =  vec(sMA{ii}*X);  %columns corresponding to y
    end
    
    %%%%% Obtain G-N search direction
    if lastiter == 0
        [Y,R]=qr(Fp,0);
        bbar=Y'*ResTot;    % ResTot is right-hand side
        dvy=R\bbar;
    end
    if lastiter>0 
        warning off
        if lastiter ==0
            keyboard
            lastiter=lastiter+1;
            output.Xlastalpha=X;  % save before alpha constant
            if quiet ==0
                fprintf('start: extra iterations with refinement; ill-conditioning; alpha constant; improving linear feasibility and complementarity \n')
                fprintf('iter  alphaX     alphaZ       alpha       Resnorm        lammin       norm(Rc)    norm(rp)    norm(rd)    norm(Ax-b)  norm(v)      descent      descent2 \n');
            end
        end
        %%%%%%%%%%%%%%%%test default vpa
        R = triu(qr(Fp));
        xx = R\(R'\(Fp'*ResTot));
        r = ResTot - Fp*xx;
        ex = R\(R'\(Fp'*r));
        dvy = xx + ex;    % use refinement if ill-conditioned
        warning on
    end

    dv=dvy(1:tnm);
    dX=ssMat(V*dv-rp,n,sMind);
    dy=dvy(tnm+1:end);
    dZ=ssMat(A'*dy+rd,n,sMind);
    
    %%%%% Step size for X and v
    alphaX = 1.1;               % initial steplength
    [~,posdefX] = chol( X + alphaX * dX );  % test if pos.def
    pditer=1;
    while posdefX > 0 && pditer <= 50
        alphaX = alphaX * .9;
        [~,posdefX] = chol( X + alphaX * dX );
        pditer = pditer+1;
    end
    if alphaX > 1
        alphaX=1;   %step size 1 is preferable
    else
        alphaX=.99*alphaX;  %addiotional step to ensure PD
    end
    if pditer == 51 %dX too large
        if quiet == 0
            fprintf('primal step size is too small \n');
        end
        psdflag = 1;
        break %terminate while loop
    end
    
    
    %%%%% Step size for Z and y
    alphaZ = 1.1;               % initial steplength
    [~,posdefZ] = chol( Z + alphaZ * dZ );  % test if pos.def
    pditer=1;
    while posdefZ > 0 && pditer <= 50
        alphaZ = alphaZ * .9;
        [~,posdefZ] = chol( Z + alphaZ * dZ );
        pditer = pditer+1;
    end
    if alphaZ > 1
        alphaZ=1;
    else
        alphaZ=.99*alphaZ;
    end
    if pditer == 51 %dZ too large
        if quiet == 0
            fprintf('dual step size is too small \n');
        end
        psdflag = 1;
        break %terminate while loop
    end
    
    %%%%% Variable updates
    if lastiter > 0
        %keyboard
        v = v + alphaX*dv;
        if alphaX==1
            x=xhat+V*v;
            X=ssMat(x,n,sMind);
        else
            X = X + alphaX*dX;
            x = ssvec(X,svind);
        end
        y = y + alphaZ*dy;
        if alphaZ==1
            z=A'*y;
            Z=ssMat(z,n,sMind);
        else
            Z = Z + alphaZ*dZ;
            z = ssvec(Z,svind);
        end
    else
        %%%%% Variable updates
        v = v + alphaX*dv;
        if alphaX==1
            x=xhat+V*v;
            X=ssMat(x,n,sMind);
        else
            X = X + alphaX*dX;
            x = ssvec(X,svind);
        end
        y = y + alphaZ*dy;
        if alphaZ==1
            z=A'*y;
            Z=ssMat(z,n,sMind);
        else
            Z = Z + alphaZ*dZ;
            z = ssvec(Z,svind);
        end
    end
    
    %%%%% Adaptive updating of alpha wrt step sizes
    if lastiter==0   %Do not change alpha at end, for improved primal feas.
        projxhat=Adagb +(VVt)*(x-Adagb );
        resxhat=A*projxhat-b;
        diter=A\resxhat;
        projxhat=projxhat-diter;
        Adagb=projxhat;
        alpha = trace(X*Z)/n;
        if min(alphaX,alphaZ) > .79
            alpha = sigma*alpha;
        elseif min(alphaX,alphaZ) < .1
            alpha = 2*alpha;
        elseif min(alphaX,alphaZ) < .5
            alpha = 1.2*alpha;
        end
        xhat = Adagb + alpha*AdagbI;
    end
    
    %%%%% Update residuals
    Rc=Z*X-alpha*In;
    rd= A'*y-z;
    rp=x-(xhat+V*v);
    ResTot=vec(Z*ssMat(rp,n,sMind)-ssMat(rd,n,sMind)*X-Rc);
    ResNorm = norm(ResTot)/normAb;
    
    %%%%% Detecting termination
    contalg = ResNorm > tolResNorm && alpha > tolalpha && iter<=maxiter;
    if lastiter > 0
        lastiter=lastiter+1;
    end
    if ~contalg
        lastiter=lastiter+1;
    end
    
    %%%%% Display Progress
    if lastiter == 1 && maxlastiter > 0
        if quiet == 0
            fprintf('starting extra iterations with alpha constant to improve linear feasibility and complementarity \n')
        end
        output.Xlastalpha=X;  % save before alpha constant
    end
    if quiet == 0
        descent=0;descent2=0;
        fprintf(' %-5g %-10g %-10g %-13g %-12g %-13g %-11g %-11g %-11g %-11g %-11g %-11g \n',...
            iter,alphaX,alphaZ,ResNorm,alpha,norm(Rc)/normAb,...
            norm(rp)/normAb,norm(rd)/normAb,norm(A*x-b)/normAb,norm(v),descent,descent2);
    end
    
    %%%% Additional Tracking of Eigenvalues
    eigXstore(iter,:) = eig(X)';
    eigZstore(iter,:) = eig(Z)';
    diagZstore(iter,:) = sort(diag(Z))';
    
end          % end of main while loop

output.X=X;
output.Z=Z;
output.eigXstore=eigXstore;
output.eigZstore=eigZstore;
output.diagZstore=diagZstore;
output.alpha=alpha;
output.res=ResNorm;

end   % end of function
