%===============================
% Author: Jake Gunther
% Date:   Fall 2018
% Purpose: Illustrate gradient
% descent for log-barrier
% example function.
%===============================

%===============================
% SEtup problem data
%===============================
A = [-1 1; 2 -1; -1 -1];
b = [1;1;2];

%===============================
% Plotting objective function
%===============================
if 1
    figure(1);
    vecx = [-2:0.01:2];
    vecy = [-2:0.01:3];
    [X,Y]=meshgrid(vecx,vecy);
    J = zeros(size(X));
    for i=1:length(vecy)
        for j=1:length(vecx)
            x = [X(i,j); Y(i,j)];
            w = b - A*x;
            %J(i,j) = all(w >= 0);
            if(all(w > 0))
                J(i,j) = -sum(log(w)) + 1;
            else
                J(i,j) = 0;
            end
        end
    end
    imagesc(vecx,vecy,log(J)); axis image;
    hold on;
    [C,h]=contour(vecx,vecy,J,[0.1:0.1:2],'k');
    clabel(C,h);
    hold off;
    grid on;
    hold on;
    shg;
end

%===============================
% Optimization algorithm
%===============================
% Starting point
x = [1.25;1.6];
%x = [-0.8;-0.8];
alpha = 0.01;
beta = 0.8;
epsilon = 1e-6;

% Convenience function definitions
func = @(xx) -sum(log(b-A*xx));
grad = @(xx) A.'*(1./(b-A*xx));

% Iterations
f = func(x);
g = grad(x);
dx = -g;
disp(f);
cnt = 0;
xold = x;
while(norm(g) > epsilon)
    cnt = cnt + 1;
    cost(cnt) = f;
    gval(cnt) = norm(g);
    
    t = 1;
    while(any(b-A*(x+t*dx)<=0)) % Back up into the problem domain
        t = t * beta;
    end
    fprintf('%3d, %10.7f, ',cnt,t);
    while(func(x+t*dx) > f + (t*alpha)*(g.'*dx)) % Line search
        t = t * beta;
    end
    fprintf('%10.7f, %10.7f, %10.7f\n',t,f,norm(g));
    
    % Step
    x = x + t*dx;
    plot([x(1) xold(1)],[x(2) xold(2)],'wo-'); hold on;
    xold = x;
    
    % Recompute
    f = func(x);
    g = grad(x);
    dx = -g;

    % Convergence
    pause;
end
hold off;

%===============================
% Plot learning curve
%===============================
figure(2);
[ax,h1,h2]=plotyy([1:length(cost)],log10(cost-cost(end)),[1:length(gval)],log10(gval));
set(h1,'LineWidth',2);
set(h2,'LineWidth',2);
grid on;
xlabel('Iteration Index','FontSize',18);
ylabel(ax(1),'$$f(x) - p^\ast$$','Interpreter','latex','FontSize',18);
ylabel(ax(2),'$$\|\nabla f(x) \|$$','Interpreter','latex','FontSize',18);
set(ax(1),'FontSize',18);
set(ax(2),'FontSize',18);
