%% Gradient method for minimization of a quadratic function

%% Defining a quadratic function

Q = [1,0;0,3]; c = [1;2]; x0 = [2;3]; epsilon = 1e-5;
%Q = [1000,20;20,1]; c = [0;0]; x0 = [1;1000]; epsilon = 1e-5;

%% Optimization

[xopt,X,fun_val,E] = gradient_method_quadratic_plot(Q,c,x0,epsilon);

%% Plotting

figure(1)
x1 = linspace(-10,10,100);
x2 = linspace(-10,10,100);
[X1,X2] = meshgrid(x1,x2);

X(1:2,1) = x0;
E(1) = 1/2 * x0'*Q*x0 + c'*x0; 

Y = 1/2*(X1.^2*Q(1,1) + X1.*X2*(Q(1,2)+Q(2,1)) + X2.^2*Q(2,2)) + c(1)*X1 + c(2)*X2;
contour(X1,X2,Y,150)
hold on
plot(X(1,:),X(2,:),'.-r')
hold off
axis([-5 5 -5 5])
xlabel('x_1')
ylabel('x_2')

figure(2)
plot(E(1:end),'o-')
xlabel('Iteration number k')
ylabel('|f(x_k)|')

%% Gradient solver

function [x,X,fun_val,E] = gradient_method_quadratic_plot(Q,c,x0,epsilon)
% f(x) = 1/2 x'*Q*x + p'*x 
    x = x0;
    iter = 0;
    grad = Q*x+c;
    while (norm(grad)>epsilon)
        iter = iter+1;
        t = norm(grad)^2/(grad'*Q*grad);
        x = x-t*grad;
        X(1:2,iter+1) = x;
        grad = Q*x+c;
        fun_val = 1/2*x'*Q*x+ c'*x;
        F(iter+1) = fun_val;
        fprintf('iter_number = %3d norm_grad = %2.6f fun_val = %2.6f   t = %2.6f \n',...
            iter, norm(grad), fun_val, t);
    end
    E = F-fun_val;
end