function [xs,lmdk,fs,k] = SQP_e(faname,gname,wname,x0,lmd0)
% sqp_e.m: SQP algorithm for nonlinear problems with equality constraints
% 
% faname, gname, wname: funciones para f.o., gradientes y Hessiana Lag.
% x0, initial point; lmd0; initial Lagrange multiplier; 
% xs, solución; lmdk, mult. de Lagrange; fs, función objetivo; k, iter
%
 xk = x0(:);     lmdk = lmd0;
 n = length(xk); p = length(lmdk);
 p1 = p + 1; k = 0; d = 1; ze = zeros(p,p);
 while d >= sqrt(eps)
    fk = feval(faname,xk);
    Gk = feval(gname,xk);
    ak = fk(2:p1); gk = Gk(:,1); Ak = Gk(:,2:p1)';
    xlmdk = [xk; lmdk];
    Lk    = feval(wname,xlmdk);
    xz    = [Lk -Ak'; -Ak ze]\[Ak'*lmdk-gk; ak];
    d_x   = xz(1:n);
    xk    = xk + d_x;
    lmdk  = Ak'\(Lk*d_x+gk);
    d = norm(d_x); k = k + 1;
 end
 disp('Vector solución:'); xs = xk;
 disp('Función objetivo:'); fk = feval(faname,xs); fs = fk(1);
 disp('Número de iteraciones:'); k
end

function z = f_ejsqp1(x)
% Función objetivo y condiciones de Ejemplo SQP1
 x1 = x(1); x2 = x(2); x3 = x(3);
 fk = -x1^4-2*x2^4-x3^4-x1^2*x2^2-x1^2*x3^2;
 ak = [x1^4+x2^4+x3^4-25; 8*x1^2+14*x2^2+7*x3^2-56];
 z = [fk; ak];
end

function z = g_ejsqp1(x)
% Gradientes de f.o. y condiciones de Ejemplo SQP1
 x1 = x(1); x2 = x(2); x3 = x(3);
 gk = [-4*x1^3-2*x1*x2^2-2*x1*x3^2; -8*x2^3-2*x1^2*x2; -4*x3^3-2*x1^2*x3];
 Ak = [4*x1^3 4*x2^3 4*x3^3; 16*x1 28*x2 14*x3];
 z = [gk Ak'];
end

function L = w_ejsqp1(xlmdk)
% Hessiana de f.o. y condiciones de Ejemplo SQP1
 x = xlmdk(1:3); lmd = xlmdk(4:5);
 x1 = x(1); x2 = x(2); x3 = x(3); lm1 = lmd(1); lm2 = lmd(2);
 w11 = -12*x1^2-2*x2^2-2*x3^2-lm1*12*x1^2-16*lm2; w12 = -4*x1*x2; w13 = -4*x1*x3;
 w22 = -24*x2^2-2*x1^2-12*lm1*x2^2-28*lm2; w33 = -12*x3^2-2*x1^2-12*lm1*x3^2-14*lm2;
 L = [w11 w12 w13; w12 w22 0; w13 0 w33];
end 