function [x,y,result,itn,finaleta] = PathFollow(problem,p1,p2,x0,y0,pickmul0,rate0)


% Generates a point for approximating the solution to the parametric NLP
%    min             f(x,p)
% subject to     cL <= c(x,p) <= cU
%
% prob(p) defines the problem. the dependence of f and c on p is assumed to
% be linear
% x0 is the solution to prob(p1), or an estimate thereof.
% If none is given, prob(p1) is solved as a
% standalone optimization problem.

if nargin==7
    rate = rate0;
else
    rate = 1.4;
end
if nargin==6
    pickmul = pickmul0;
else
    pickmul = 1;
end


options = optimset('Display','off','Algorithm','active-set');
optionslin = optimset('Display','off','Algorithm', 'active-set');

result=0;
p=p1;
param = parampath;
clear prob;
sym pp;
theprob = @(pp)problem(pp);
prob = theprob(p);

m   = prob.m;
me  = prob.me;
eqs = 1:me;
ineqs = me+1:m;
n   = prob.n ;
cL  = prob.cl;      cU  = prob.cu;

[f,g] = prob.obj(x0);
[c,J] = prob.cons( x0 );

[etaold,~] = calceta(x0,y0,prob,f,g,c,J);

if etaold>.01
    %get a more accurate multiplier
    Act = setdiff(find(c<.01),eqs);
    
    [yact,~,exitflag,~,~] = quadprog([J(Act,:);J(eqs,:)]*[J(Act,:);J(eqs,:)]',(-2*g'*[J(Act,:);J(eqs,:)]')',[],[],[],[],[zeros(length(Act),1);-Inf*ones(length(eqs),1)],[Inf*ones(length(Act)+length(eqs),1)], y0(union(Act,eqs)),options);
    y0(Act) = yact(1:length(Act));
    y0(eqs) = yact(length(Act)+1:end);
    [etaold,~] = calceta(x0,y0,prob,f,g,c,J);
end


%obtain active set estimates
epsA = find(abs(y0) > etaold^param.gamma);
epsA = setdiff(epsA,eqs);
epsF = setdiff(ineqs,epsA);
Act = union(epsA,union(eqs,find(c< etaold^(param.gamma))));



converged = 0;
itn = 0;

fprintf('Solving problem %s with p=%3.1e using estimate at p=%3.1e\n',prob.name,p2(1),p1(1));
eqs = eqs';
t = 0;
deltat = param.deltat0;



fprintf('iteration  p        t        eta(x,y,p) deltat      sizeepsA    Success\n');
fprintf('%6g     %3.1e  %3.1e  %5.2e   %3.1e  %4g \n',itn,p(1),t,etaold,deltat,length(epsA));

x=x0;
y=y0;


etanew = etaold;
success = 0;
fail = 0;
etamax = etaold;
alphahat = 1;
ts = [0];
ys =[x(1)];
testrepeat = 0;
xs = [x'];
ys = [y'];
ts = [0];
Deltats = [deltat];

dcdp = prob.dcdp;

while ~converged
    itn = itn+1;
    success = 0;
    pred = 0;
    p0 = p2*(t)+p1*(1-t);
    clear p;
    p = p2*(t+deltat)+p1*(1-t-deltat);
    clear prob;

    %define the problem data at the current parameter
    prob = theprob(p0);
    m   = prob.m;
    n   = prob.n ;
    cL  = prob.cl;      cU  = prob.cu;
    [f,g] = prob.obj(x);
    [c,J] = prob.cons(x);
    keepgoingrank = 1;
    yold=y;

    %only necessary when a multiplier on a face is chosen. has never been reached in practice, can be ignored
    while keepgoingrank
        nAct = length(eqs)+length(epsA);
        rk = rank(J([eqs;epsA],:));
        if rk == nAct
            keepgoingrank = 0;
            break;
        end
        NullJ = null(J([eqs;epsA],:)');
        wnJ = NullJ(:,1);
        wnJ(find(wnJ>0)) = -eps;
        alphahat = min(min(-(y([epsA])./wnJ((length(eqs)+1):end))),1);
        y([eqs;epsA]) = y([eqs;epsA])+alphahat*wnJ;
        epsA = setdiff(find(y>max(param.etamin,etaold^param.gamma)),eqs);
    end
    
    
    
    HC = prob.chess(x);
    H = prob.hess(x,y);
    [etaold,~] = calceta(x,y,prob,f,g,c,J);
    xhat = x;
    yhat = y;
    

    
    
    
    
    
    if (etaold >=1) %previous result gets you too far from the solution
        deltat = deltat*param.gamma1;
        success = 0;
    else
        %Corrector step
        delc = -[H -J([eqs;epsA],:)'; J([eqs;epsA],:) zeros(length(epsA)+length(eqs))]\[g-J'*y;c([eqs;epsA])];
        delxc = delc(1:n);
        delyc = delc(n+1:end);
        delycf = zeros(m,1);
        delycf([eqs;epsA]) = delyc;
        Jnew = J;
        for i=1:m
            Jnew(i,:) = Jnew(i,:)+(squeeze(HC(:,:,i))*delxc)';
        end
        %Predictor Step
        [deltax,~,exitflag,~,lambda] = quadprog(H,zeros(n,1),-Jnew(epsF,:),c(epsF)+Jnew(epsF,:)*delxc+dcdp(epsF,:)*(deltat*(p2-p1)),[Jnew(eqs,:);Jnew(epsA,:)],[-dcdp(eqs,:)*(deltat*(p2-p1));-dcdp(epsA,:)*(deltat*(p2-p1))],[],[], x,options);
        dynew = zeros(m,1);
        dynew(epsF) = (lambda.ineqlin(1:(length(epsF))));
        dynew([eqs;epsA]) = -lambda.eqlin;
        prob = theprob(p);
        [f,g] = prob.obj(xhat+delxc+deltax);
        [c,J] = prob.cons(xhat +delxc+deltax);
        ynew = y+dynew+delycf;
        [etanew,z] = calceta(xhat+delxc+deltax,ynew,prob,f,g,c,J);
        
        %Somewhat heuristic acceptance criteria 
        if (etanew<=param.isopt) ||  (etanew <= etaold^rate && etanew< max(0.01,etamax)) || (etanew < etamax) 
            success = 1;
            xold = x;
            yold = y;
           
            y = ynew;
            t = t+deltat;
            x = x+deltax+delxc;
            
            if (etanew <= etaold^1.6 || etanew<=param.isopt) && ~testrepeat
                deltat = min(1-t,deltat/param.gamma1);
            else
                deltat = min(1-t,deltat);
            end
            testrepeat = 0;
            etaold = etanew;
            %get new active set data
            epsA = find(abs(y) > max(param.etamin,etaold^param.gamma));
            epsA = setdiff(epsA,eqs);
            epsF = setdiff(ineqs,epsA);
            Act = [eqs;epsA;setdiff(setdiff(find(c< max(param.etamin,etaold^(param.gamma))),eqs),epsA)];

            fail = 0;
            H = prob.hess(x,y);
            
            %Solve LPJump for the new multiplier
            lb = zeros(length(Act),1);
            lb(1:me) = -Inf;
            ub = Inf*ones(length(Act),1);

            if pickmul
            [ydel,~,exitflag] = linprog(prob.dcdp(Act,:)*deltat*(p2-p1), [J(Act,:)';-J(Act,:)'],[g+abs(z);abs(z)-g],[],[],lb,ub,y(Act),optionslin);

            linobj = prob.dcdp(Act,:)*deltat*(p2-p1);
            if exitflag==1 && (linobj'*ydel < linobj'*y(Act)-sqrt(eps))      
              y(Act) = ydel;
              epsA = find(abs(y) > max(param.etamin,etaold^param.gamma));
              epsA = setdiff(epsA,eqs);
              epsF = setdiff(ineqs,epsA);
              Act = union(epsA,union(eqs,find(c< max(param.etamin^0.5,etaold^(param.gamma)))));             
            end
            end
            xs = [xs;x'];
            ts = [ts;t];
            ys = [ys;y'];
            Deltats = [Deltats;deltat];
            if etanew > etamax
                etamax = etanew;
            end
        else
            deltat = deltat*param.gamma1;
            if fail == 0
                testrepeat = 1;
            end
            fail = fail+1;
        end
    end
    
    
    
    %looks like it will be impossible to perform the homotopy, delta t has to be too small
    if deltat <1/(100000*param.itnmax) && t<1
        result = 0;
        return
    end
    
    
    
    sizeact = length(epsA);
    % Print iteration, exit if necessary
    if mod(itn,10) == 0
        fprintf('iteration  p        t        eta(x,y,p) deltat      sizeepsA    Success\n');
    end
    fprintf('%6g     %3.1e  %3.1e  %5.2e   %3.1e  %4g        %4g\n',itn,p(1),t,etaold,deltat,sizeact,success);

    if t >= 1 && success
        fprintf('Successful approximate solution to %s with p=%3.1e\n',prob.name,p2(1));
        result = 1;
        finaleta = etanew;
        break;
    end
    if itn>=param.itnmax
        fprintf('Maximum iterations reached\n');
        result = 0;
        break;
    end
    
end
%Print plot of results
for i=1:n
    h(i) = subplot(n+m+1,1,i);
    plot(ts,xs(:,i));
    ylabel(strcat('x_',num2str(i)));
    set(h(i),'ylim',[min(xs(:,i))-sqrt(eps),max(xs(:,i))+sqrt(eps)]);
end
for j=1:m
    h(n+j) = subplot(n+m+1,1,j+n);
    plot(ts,ys(:,j));
    ylabel(strcat('y_',num2str(j)));
    set(h(n+j),'ylim',[min(ys(:,j))-sqrt(eps),max(ys(:,j))+sqrt(eps)]);
end
     h(n+m+1) = subplot(n+m+1,1,m+n+1);
     plot(ts,log(Deltats));
     ylabel('log \Delta t');
     %set(h(n+j),'ylim',[min(ys(:,j))-sqrt(eps),max(ys(:,j))+sqrt(eps)]);

xlabel('t')

end

function [eta,z] = calceta(x,y,prob,f,g,c,J)
vl = c(prob.me+1:end);

z = g-J'*y;

cmp      = max(0,y(prob.me+1:end).*min(vl,1));
vioL     = min(0,vl);
vioE     = c(1:prob.me);


maxComp  = norm( cmp ,Inf);
maxViol  = norm(    [ vioL; vioE ] ,Inf);
eta = .1*max(maxComp,max(maxViol,norm(z,Inf)));
end

function param = parampath
param = struct('gamma', 0.7, ...
    'gamma1', 0.6, ...
    'itnmax',10000, ...
    'deltat0',.1, ...%.05
    'maxfail',3, ...
    'isopt',10^(-6),...
    'etamin',10^(-6));
end
