function x2=SG_demo(A,b,x0)
% function SG_demo(A,b,x0)
% demonstrace metody sdruzenych gradinetu pro system rovnic Ax=b
% x0 - pocatecni iterace
%

[m,n]=size(A);
[V,D]=eig(A);
D=diag(D);
if m~=2 || n~=2 || any(any(A'~=A)) || any(D<=0)
    error('Matice musi byt pozitivne definitni 2x2!');
end
b=b(:);x0=x0(:); % prevod na sloupce
[m,n]=size(b);
if m~=2 || n~=1
    error('Spatny rozmer vektoru b!');
end
[m,n]=size(x0);
if m~=2 || n~=1
    error('Spatny rozmer vektoru pocatecniiterace x0!');
end

Q=@(x,y)[x y]*A*[x; y]/2-[x y]*b;

s=inv(A)*b;
Qs=Q(s(1),s(2));
Q0=Q(x0(1),x0(2));
Qs0=Qs+0.0004*(Q0-Qs);
d=sqrt(2*(Q0-Qs)/min(D));
LList=[Qs0,Q0];
warning ('off','all');
Qcont=fcontour(Q,[s(1)-1*d,s(1)+1*d,s(2)-1*d,s(2)+1*d]);
%Qcont=fcontour(Q);
set(Qcont,'LevelList',LList,'LineColor','green','LineStyle','--');
hold on
plot(x0(1),x0(2),'r*');
r=b-A*x0;
% prvni iterace
%title('Neco stiskni.')
pause(3)
v=r;
alfa=r'*v/(v'*A*v);
x1=x0+alfa*v;
Q1=Q(x1(1),x1(2));
LList=[LList,Q1];
set(Qcont,'LevelList',LList);
plot(x1(1),x1(2),'r*')
plot([x0(1),x1(1)],[x0(2),x1(2)],'b--');
r=r-alfa*A*v;
x0=x1;
% druha iterace
%title('Neco stiskni.')
pause(3)
beta=-r'*A*v/(v'*A*v);
v=r+beta*v;
alfa=r'*v/(v'*A*v);
t=r'*r/(r'*A*r);
x1=x0+t*r;
x2=x0+alfa*v;
Q1=Q(x1(1),x1(2));
LList=[LList,Q1];
set(Qcont,'LevelList',LList);
plot(x1(1),x1(2),'r*')
plot([x0(1),x1(1)],[x0(2),x1(2)],'b--');
plot(x2(1),x2(2),'r*')
plot([x0(1),x2(1)],[x0(2),x2(2)],'r--');
title('HOTOVO!')
hold off
