function [xs,fs,k] = SQP_ie_1(fun,x0,mu0,epsi)
% SQP algorithm for nonlinear problems with inequality constraints.
% x0, punto de partida; mu0, multiplicadores de Lagrange; epsi, tolerancia óptimo
% xs, óptimo; fs, función objetivo solución; k, iteraciones
%
 xk = x0(:); muk = mu0(:); n = length(x0); q = length(muk);
 q1 = q + 1; k = 0; In = eye(n); d = 1;
 while d >= epsi
    [fck,gAk,Lk] = fun([xk;muk]);
    ck = fck(2:q1);
    gk = gAk(:,1);
    Ak = gAk(:,2:q1)';
    %options = optimset('Display','off');
    %[d_x,fval,exitflag,output,lambda] = quadprog(Lk,gk,-Ak,ck,[],[],[],[],[],options);
    [d_x,muk] = qp_path_ie(Lk,gk,Ak,-ck,zeros(n,1),epsi);
    xk  = xk + d_x; 
    d   = norm(d_x); k = k + 1;
 end
 disp('Óptimo:'); xs = xk, muk      % lambda.ineqlin si quadprod
 disp('Función objetivo:'); fck = f_ejsqp2([xs;muk]); fs = fck(1)
 disp('Iteraciones:'); k
end

function [xs muk] = qp_path_ie(H,p,A,b,x0,epsi)
% Punto interior primal-dual path-following con partida no factible para prog. cua.
% x0, punto de partida A*x0 - b > 0; 
% xs, óptimo; fs, función objetivo en la solución; muk, multiplicadores en óptimo
 n = length(x0); q  = length(b); a_max = 1-1e-5; x = x0(:);
 s = ones(q,1);  mu = ones(q,1); k = 0;
 rsz = s.*mu;
 gap = mean(rsz);
   while gap > epsi                                   % Proceso iterativo
      rs  = -A*x + b + s;
      rl   = H*x + p - A'*mu;
      d_x  = -(H + A'*diag(mu./s)*A)\(A'*((rsz - rs.*mu)./s) + rl); % Newton KKT.
      d_s  = A*d_x-rs;
      d_mu = -(rsz + mu.*d_s)./s;
    % Amplitud de paso
      ind = find(d_s < 0); a_p=1;
      if isempty(ind)==0, a_p = min(a_p,min(-s(ind)./d_s(ind)));   end
      ind = find(d_mu < 0); a_d=1;
      if isempty(ind)==0, a_d = min(a_d,min(-mu(ind)./d_mu(ind))); end
      a_k = a_max*min([1 a_p a_d]);
    % Nuevo punto
      x   = x + a_k*d_x; mu  = mu + a_k*d_mu; s = s+a_k*d_s;
      rsz = s.*mu;       gap = mean(rsz); k = k + 1;
   end
   xs = x; muk = mu;
end

function [fck gAk Lk] = f_ejsqp2(x)
% Función objetivo, condiciones gradientes y der_2 Lag del ejemplo sqpej_2
 x1  = x(1); x2 = x(2); x3 = x(3); x4 = x(4);
 fk  = 0.5*((x1-x3)^2+(x2-x4)^2);
 ck  = [-(x1^2/4+x2^2)+x1/2+0.75; -(5*x3^2+6*x3*x4+5*x4^2)/8+(11*x3+13*x4)/2-35/2];
 fck = [fk; ck];
 gk  = [x1-x3, x2-x4, x3-x1, x4-x2]';
 Ak  = [-x1/2+.5, -2*x2, 0, 0; 0, 0, -(5*x3+3*x4)/4+11/2, -(3*x3+5*x4)/4+13/2];
 gAk = [gk Ak'];
 mu  = x(5:6); mu1 = mu(1); mu2 = mu(2);
 y11 = 1+mu1/2; y22 = 1+2*mu1; y33 = 1+5*mu2/4; y34 = 3*mu2/4;
 Lk  = [y11, 0, -1, 0; 0, y22, 0, -1; -1, 0, y33, y34; 0, -1, y34, y33]; 
end