close all
clear all
clc

alphav=linspace(0,3);
[A,a] = Aa(alphav);
b=[3;14;9;5;3];
x=A\b
Nmax=100;
toll=sqrt(eps);
[xg,kg,eg] = gradiente(A,b,Nmax,toll);
[xc,kc,ec] = gradientecon(A,b,Nmax,toll);

K=linspace(1,kg,kg);
K1=linspace(1,kc,kc);

plot(K,eg(K),'g')
hold on
plot(K1,ec(K1),'b')

function [A,a] = Aa(alphav)

for av=alphav
    Ai=[av 0 1 0 0; 0 10 2 1 1; 1 2 6 0 0;0 1 0 4 0;0 1 0 0 2];
    [H]=hess(Ai);
    [esito]=sturm(H);
    if esito==1
        a=av;
        A=Ai;
        return
    end
end
end

function [H] = hess(A)
n=length(A);

Q=eye(n);

for i=1:n-1
    v=A(i+1:n,i);
    u=zeros(n,1);

    if v(1)==0
        u(i+1:n)=v + norm(v)*eye(n-i,1);
    else
        u(i+1:n)=v + sign(v(1))*norm(v)*eye(n-i,1);
    end

    Qi=eye(n) -2*(u*u')/(u'*u);
    Q=Qi*Q;
    A=Qi*A*Qi;
end
H=A;
end

function [esito] = sturm(H)
D=diag(H);
C=diag(H,1);

n=length(D);
p(1)=1;
p(2)=-D(1);
esito=0;
q=0;
for i=2:n
    if p(i)==0
        return
    else
        p(i+1)=-D(i)*p(i) -C(i-1)^2*p(i-1);
        if p(i)*p(i-1)<0
            q=q+1;
        end
    end
end

if p(n+1)*p(n)<0 && q==n-1
    esito=1;
end
end

function [xg,k,e] = gradiente(A,b,Nmax,toll)
x0=zeros(size(b));
r=A*x0 -b;
al=r'*r/(r'*A*r);
x= x0-al*r;
k=1;
e(k)=norm(r);

while k<Nmax && e(k)>toll
r=A*x -b;
al=r'*r/(r'*A*r);
x= x-al*r;
k=1;
e(k)=norm(r);
end
xg=x;

end

function [xc,k,e] = gradientecon(A,b,Nmax,toll)
n=length(b);
x0=zeros(size(b));
r=A*x0 -b;
d0=-r;
e(1)=norm(r);
k=1;

while k<Nmax && e(k)>toll*norm(b)
    a=(r'*r)/(r'*A*r);
    x=x0 +a*d0;
    r=A*x-b;
   
    beta=(d0'*A*r)/(d0'*A*d0);
    d=-r +beta*d0;
    k=k+1;
    e(k)=norm(r);
    x0=x;
    d0=d;
    
end

xc=x;
end