%% Gradient method for minimization of an ill-conditioned quadratic function

%% Defining a quadratic function

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

cond(Q)

%% Preconditioning

D = diag(1./diag(Q))
cond(sqrtm(D)*Q*sqrtm(D))

%% Optimization

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

%% Plotting

figure(1)
x1 = linspace(-100,1200,100);
x2 = linspace(-15000,500,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([-15 15 -15 15])
xlabel('x_1')
ylabel('x_2')

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

%% Solver

function [x,X,fun_val,E] = gradient_method_scaled_quadratic_plot(Q,c,D,x0,epsilon)
% f(x) = 1/2 x'*Q*x + c'*x 
    x = x0;
    iter = 0;
    grad = Q*x+c;
    while (norm(D*grad)>epsilon)
        iter = iter+1;
        t = grad'*D*grad/(grad'*D*Q*D*grad);
        x = x-t*D*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\n',...
            iter, norm(grad), fun_val);
    end
    E = F-fun_val;
end