clear
rng(1,'twister');
%close all

%% Generate a random sym pos def matrix
n = 20;
B = rand(n,n);
%get a random symmetric matrix
B = 0.5*(B + B');
[V,D] = eig(B);
%make its n eigs > 0 by picking from uniform pd [1,n^2]
%Dp = randi(n^2,n,1);
Dp = [1:n];
%Dp = [1:3:3*n]; 
Dp(end) = 10000;
%Dp = [1,1,1,5,5,5,5,10,10,10];
A = V * diag(Dp) * V';
%prepare a col vector b for use in the quadratic functional
b = rand(n,1);

%% Prepare the quadratic functional and sim params
f = @(x) 0.5 * x' * A * x - b' * x;
gradf = @(x) A * x - b;
%get the true soln
x_true = A\b;
%pick a random starting point 
x0 = rand(n,1);
%sim params
max_iter = 50;
tol = 1e-6; %how low we want the gradient to be

%% Optimize using conjugate gradient method
%x = x + alpha p, where alpha = r'*r/(p'*A*p)
x = x0;
r = gradf(x); 
p = -r;
x_CG = [x];

gf = gradf(x);
gf_log = [norm(gf)];
k = 0;

while norm(gf)>tol && k < max_iter
    alpha = r'*r/(p'*A*p);
    x_new = x + alpha * p;
    %new x found, update params for next iter
    r_new = r + alpha * A * p;
    beta = r_new'*r_new/(r'*r);
    p = -r_new + beta *p;
    r = r_new;
    x = x_new;
    %book keeping
    gf = gradf(x_new);
    gf_log = [gf_log, norm(gf)];
    k = k+1;
    disp(['CG: k=' num2str(k) ', gf=' num2str(norm(gf))]);
    x_CG = [x_CG, x];
end

plot(log(vecnorm(x_CG-x_true)),'k','LineWidth',2);
ylabel('log($\|x_k-x^*\|$)','Interpreter','latex'); xlabel('iters'); 
title('CG rel error v/s cond no');

