function [ x_stop,z_stop,s_stop,k ] = QP_IPM( x,z,s,G,g,C,d )
% Solve QP problem of the type:
% min 0.5 x'Gx + g'x
% s.t.
% A'x = b
% C'x >= d
%   
% G: Hessian
% C: lhs of inequality constraints
% d: rhs of inequality constraints
% A: lhs of equality constraints
% b: rhs of equality constraints
% y: lagrange multipliers for the equality constraints
% z: lagrange multipliers for the inequality constraints
% dmp: Dampening factor

dmp = 0.95;

% Residuals
% [mA,nA] = size(A);
[~,nC] = size(C);
e = ones(nC,1);
% rL = G*x + g -A*y -C*z;
rL = G*x + g -C*z;
% rA = -A'*x + b;
rC = -C'*x + s;
rsz = s .* z;
mu = sum(z .* s) /nC;

k = 0 ;
maxk = 200;
eps_L = 1e-10; eps_C = 1e-10; eps_mu = 1e-10;

while (k <= maxk && norm(rL) >= eps_L && norm(rC)>=eps_C ...
        && abs(mu) >= eps_mu)
    
% lhs = [G -A -C;-A' spalloc(nA,nA,0) spalloc(nA,nC,0);...
%        -C' spalloc(nC,nA,0) sparse(-diag(s./z))]
% rhs = [-rL;-rA;-rC + rsz./z];
lhs = [G -C;-C' sparse(-diag(s./z))];
rhs = [-rL;-rC + rsz./z];
[L,D,P] = ldl(lhs);
dxyz_a = P*(L'\(D\(L\(P'*rhs))));
% dx_a = dxyz_a(1:length(x));
dz_a = dxyz_a(length(x)+1:length(x)+length(z));
ds_a = -((rsz + s.*dz_a) ./ z);

% Compute alpha_aff
alpha_a = 1;
idx_z = find(dz_a < 0);
if (isempty(idx_z) == 0)
    alpha_a = min(alpha_a, min(-z(idx_z)./dz_a(idx_z)));
end
idx_s = find(ds_a < 0);
if (isempty(idx_s) == 0)
    alpha_a = min(alpha_a, min(-s(idx_s)./ds_a(idx_s)));
end

% Compute the affine duality gap
mu_a = ((z + alpha_a*dz_a)'*(s + alpha_a*ds_a))/nC;

% Compute the centering parameter
sigma = (mu_a/mu)^3;

% Solve the system
rsz = rsz + ds_a.*dz_a - sigma*mu*e;
rhs = [-rL;-rC + rsz./z];

[L,D,P] = ldl(lhs);
dxyz = P*(L'\(D\(L\(P'*rhs))));
dx = dxyz(1:length(x));
dz = dxyz(length(x)+1:length(x)+length(z));
ds = -((rsz + s.*dz) ./ z);

% Compute alpha
alpha = 1;
idx_z = find(dz < 0);
if (isempty(idx_z) == 0)
    alpha = min(alpha, min(-z(idx_z)./dz(idx_z)));
end
idx_s = find(ds < 0);
if (isempty(idx_s) == 0)
    alpha = min(alpha, min(-s(idx_s)./ds(idx_s)));
end

% Update x,z,s
x = x + dmp*alpha*dx;
z = z + dmp*alpha*dz;
s = s + dmp*alpha*ds;

k = k + 1;

% Update the rhs
rL = G*x + g - C*z;
rC = -C'*x + s + d;
rsz = s.*z;
mu = sum(z.*s)/nC;

end

% Output
x_stop = x;
z_stop = z;
s_stop = s;

end

