function [x] = barrier_method_all_inc(A, b, v, c, x0, t0)

%
% [x] = barrier_method_all_inc(A, b, v, c, x0, t0)
%
% solves the optimization problem
% 
%             minimize    ||x||_1 + v'*x + c||x||^2
%             subject to  Ax = b
%
% where ||x||_1=|x_1|+|x_2|+...+|x_n|, ||x||=sqrt(x_1^2+x_2^2+...+x_n^2),
% A is a p x n matrix, v is a n x 1 vector, b is a p x 1 vector, and c > 0 
% is a scalar. The n x 1 vectors x0 and t0 may be not specified by making 
% them equal to [] (in this case A(1,1) and b(1) differ from 0). The
% optimal solution x is returned.



K = 8;

factor = 50;

mu = 0.05;

if isempty(x0)
    [p,n] = size(A);
    x = zeros(n,1);
    x(1) = b(1) / A(1,1);
    t = abs(x) + 1e-4*ones(n,1);
else
    x = x0;
    t = t0;
end

for k = 1 : K

    [x,t] = LineSearchAlgorithm(mu, A, b, v, c, x, t);
    mu = factor*mu;
end

end


function [x,t] = LineSearchAlgorithm(mu, A, b, v, c, x0, t0)

epsilon = 1e-3;                  epsilon = epsilon^2;

N = 20;

L = 50;

[p,n] = size(A);

onesv = ones(n,1);

x = x0;
t = t0;

                                                       

for j = 1 : N
    
    % Evaluate g1 and g2
    B1 = 1./(t - x);
    B2 = 1./(t + x);
    g1 = mu*v + (2*mu*c)*x + B1 - B2;
    g2 = mu*onesv - (B1 + B2);
                                                                                                                   
    % Solve the linear system
    if p == 1
        [d1, d2, lambda] = LinearSystem(A', x, b, c, mu, B1, B2, g1, g2);
    else
        [d1, d2, lambda] = LinearSystem2(A, x, b, c, mu, B1, B2, g1, g2);
    end
    
                                                                                
    if -(g1'*d1 + g2'*d2) < epsilon 
        break;
    end
                                                          
    % Calculate the phi's
    var1 = mu*(v'*x + onesv'*t + c*x'*x);
    var2 = t-x;
    var3 = t+x;
    if j == 1
        %phi_0 = var1 - onesv'*(log(var2.*var3));
                        log_factor = 1;
                        aux = var2.*var3;
                        for multiply = 1 : n
                            log_factor = log_factor*aux(multiply);
                        end
                        phi_0 = var1 - log(log_factor);
    else
        phi_0 = phi_alpha;
    end
    phi_deriv_0 = g1'*d1 + g2'*d2;                                     
    var4 = mu*(v'*d1 + onesv'*d2 + (2*c)*x'*d1);
    var5 = (mu*c)*(d1'*d1);
    diff_d = d2 - d1;
    sum_d = d2 + d1;
    
    % Armijo's rule
    alpha = 1; beta = 0.5; c1 = 10^(-2);
    for l = 1 : L
        arglog1 = var2 + alpha*diff_d;
        arglog2 = var3 + alpha*sum_d;
                                        
        if min(arglog1) > 0 && min(arglog2) > 0
            %phi_alpha = var1 + alpha*var4 + alpha^2*var5 - onesv'*(log(arglog1.*arglog2));
                                        log_factor = 1;
                                        aux = arglog1.*arglog2;
                                        for multiply = 1 : n
                                            log_factor = log_factor*aux(multiply);
                                        end
                                        phi_alpha = var1 + alpha*var4 + alpha^2*var5 - log(log_factor);
            if phi_alpha <= phi_0 + c1*alpha*phi_deriv_0
                break;
            end
        end
        alpha = beta*alpha;
    end
    
    x = x + alpha*d1; 
    t = t + alpha*d2;
     
end

end




function [d1, d2, lambda] = LinearSystem(a, x, b, c, mu, B1, B2, g1, g2)

% [d1, d2, lambda] = LinearSystem(a, x, b, c, mu, B1, B2, g1, g2)
%
% solves the linear system
%                          [ hessian(f)    p ] [  d1  ] = [ -g1 ] 
%                          [    p'         0 ] [  d2  ]   [ -g2 ]
%                                              [lambda]   [  0  ]
% 
% where hessian(f) = 2*miu*c*R + M'*diag(d)*M  ( being 
% R = [eye(n) , zeros(n,n); zeros(n,2*n)], 
% M = [eye(n), -eye(n); -eye(n), -eye(n)] 
% and 
% d = diag([B1.^2 ; B2.^2])).
%
% We also have p = [a ; zeros(n,1)]. 
% The variables a,g1,g2,B1,B2 are vectors of size n. The others are 
% scalars.
%
% This system inversion can be carried out within 48*n + 8 flops 
% and n square roots.

n = length(x);

onesv = ones(n,1);

D1 = B1.^2;
D2 = B2.^2;

d_plus = D1 + D2;
d_minus = D2 - D1;

A_h_inv = 1./(d_plus + (2*mu*c)*onesv);

S_h_inv = 1./(d_plus - (d_minus.*A_h_inv).*d_minus);

aux1 = (S_h_inv.*d_minus).*A_h_inv;

Q12 = -aux1;

Q11 = A_h_inv + (A_h_inv.*d_minus).*aux1;

S_inv = -1/(Q11'*(a.^2));

Hg1 = (Q11.*g1) + (Q12.*g2);

Hg2 = (Q12.*g1) + (S_h_inv.*g2);

Qa1 = Q11.*a;

Qa2 = Q12.*a;

lambda = (a'*(Hg1 - x) + b)*S_inv;

d1 = -(Hg1 + lambda*Qa1);

d2 = -(Hg2 + lambda*Qa2);

end


function [d1, d2, lambda] = LinearSystem2(A, x, b, c, mu, B1, B2, g1, g2)

% [d1, d2, lambda] = LinearSystem(A, x, b, c, mu, B1, B2, g1, g2)
%
% solves the linear system
%                          [ hessian(f)    P' ] [  d1  ] = [ -g1 ] 
%                          [     P         0  ] [  d2  ]   [ -g2 ]
%                                               [lambda]   [  0  ]
% 
% where hessian(f) = 2*miu*c*R + M'*diag(d)*M  ( being 
% R = [eye(n) , zeros(n,n); zeros(n,2*n)], 
% M = [eye(n), -eye(n); -eye(n), -eye(n)] 
% and 
% d = diag([B1.^2 ; B2.^2])).
%
% We also have P = [a ; zeros(p,n)]. 
% The variables g1,g2,B1,B2 are vectors of size n. The others are 
% scalars, with the exception of P which is a p x n matrix.
%
% This system inversion can be carried out within O(p^2 n) + O(p^3) flops.

[p, n] = size(A);

onesv = ones(n,1);

D1 = B1.^2;
D2 = B2.^2;

d_plus = D1 + D2;
d_minus = D2 - D1;

A_h_inv = 1./(d_plus + (2*mu*c)*onesv);

S_h_inv = 1./(d_plus - (d_minus.*A_h_inv).*d_minus);

aux1 = (S_h_inv.*d_minus).*A_h_inv;

Q12 = -aux1;

Q11 = A_h_inv + (A_h_inv.*d_minus).*aux1;

auxiliarymatrix = zeros(n,p);
auxiliarymatrix2 = zeros(n,p);

for l = 1 : p
    auxiliarymatrix(:,l) = Q11.*A(l,:)';
    auxiliarymatrix2(:,l) = Q12.*A(l,:)';
end

S = -A*auxiliarymatrix;

Hg1 = (Q11.*g1) + (Q12.*g2);

Hg2 = (Q12.*g1) + (S_h_inv.*g2);

Qa1 = auxiliarymatrix;

Qa2 = auxiliarymatrix2;

lambda = S \ (A*(Hg1 - x) + b);

d1 = -(Hg1 + Qa1*lambda);

d2 = -(Hg2 + Qa2*lambda);

end