%
% CG Method 共轭梯度法，以 2 阶对称正定矩阵为例
%

clear all;
close all;

tol = 1e-6; 
IterMax = 10;

A = [2 1; 1 3]; 
xt = [1; 1];
b = A*xt; % [3; 4]

% initial guess
x0 = [-3; 0.5];
% x0 = [-2.552; 1.577] + xt;

% CG iteration
n = length(b);
x = x0; 
xx = x; % store the iterate solution

r = b - A*x;
r0norm = norm(r);
if abs(r0norm)<1e-16, return, end

for k = 1 : IterMax
    rho = dot(r,r);    
    if ( k == 1 ) 
        p = r;
    else
        mu = rho / rho_0;
        p = r + mu*p;
    end
    
    q = A*p;
    xi = rho / (p'*q );    
    x = x + xi * p; 
    xx = [xx, x];
    r = r - xi*q;    
    
    relres = norm(r)/r0norm;
    fprintf('k=%d, x=[%.4f,%.4f], relres=%.2e\n', k,x(1),x(2),relres);
    if relres < tol
        break
    end
 
    rho_0 = rho;    
end

figure
% f = @(x,y) 1/2*[x y] * A * [x; y] - [x y]*b;
f = @(x,y) x.*x + x.*y + 3/2*y.*y - 3*x - 4*y;

hold on 
set(gcf,'position',[50,50,900,900]);
fc = fcontour(f,[-5,5,-5,5],'b-','linewidth',1);
fc.LevelList = [-3.4 -2.3 0 3 8 15 25 50];
plot([-5,5],[0,0],'r-', [0,0],[-5,5],'r-')
% plot(1,1,'sb','markersize',10,'linewidth',2)
% text(-0.4,-0.2,'\it O','fontsize',20)
text([-4:-1]-0.2,[zeros(4,1)]-0.2, num2cell(-4:-1),'fontsize',20)
text([1:4]-0.1,[zeros(4,1)]-0.2, num2cell(1:4),'fontsize',20)
text([zeros(4,1)]-0.3,[-4:-1], num2cell([-4:-1]),'fontsize',20)
text([zeros(4,1)]-0.2,[1:4], num2cell([1:4]),'fontsize',20)
text(-0.2,-0.2, {0}, 'fontsize', 20)
set(gca,'fontsize',20)
grid on
axis equal
xticks([-5:5]); yticks([-5:5]);
set(gca,'YTickLabel',[],'XTickLabel',[]);

plot(xx(1,:),xx(2,:),'*--r','linewidth',1,'markersize',10)
hold off

