clear all
close all
clc

A=[4 -1 1 1 0 0; -1 4 -1 0 1 0; 0 -1 4 -1 0 0 ; 0 0 0 4 -1 0; 1 0 0 -1 4 -1; 0 0 0 0 -1 4];
b=[1 0 0 0 0 0]';

%La convergenza dipende dal raggio spettrale della matrice di iterazione

invD= diag(1./diag(A));
rhoj=max(abs(eig(-invD*(A-diag(diag(A))))));

w=linspace(0,1);
f=@(w) RHOW(A,b,w)-rhoj;
fplot(f,[0,1]) %intersezione in un solo punto (0.8,0.9)
I=linspace(0.8,0.9);
Nmax=100;
tol=sqrt(eps);
[w0]=searchz(f,I,Nmax,tol);
xline(w0)
yline(0)

w1=w0+0.1;
w2=w0-0.1;
toll=10^-8;

[x1,err1] = SOTTOR(A,b,w1,Nmax,toll);
[x2,err2] = SOTTOR(A,b,w2,Nmax,toll);
[xj,errj] = JACOBIMIA(A,b,Nmax,toll);

figure
plot(log10(err1))
hold on
plot(log10(err2))
plot(log10(errj))

function [rhow] = RHOW(A,b,w)
%funzione che mi calcola il raggio spettrale della matrice del
%sottorilassamento

D=diag(diag(A));
L=tril(A,-1);
U=triu(A,1);

BW=(D+w*L)\((1-w)*D-w*U);

rhow=max(abs(eig(BW)));

end

function [x0] = searchz(f,I,Nmax,tol)
%funzione che mi cerca lo zero, utilizzo metodo delle secanti

x1=I(end);
x0=I(1);
l= (f(x0)-f(x1))/(x0-x1);
x1=x0;
x0=x0-f(x0)/l;
k=1;
e(k)=abs(f(x0));

while k<Nmax && e(k)>tol
    l= (f(x0)-f(x1))/(x0-x1);
    x1=x0;
    x0=x0-f(x0)/l;
    k=k+1;
    e(k)=abs(f(x0));

end
end

function [x,err] = SOTTOR(A,b,w,Nmax,toll)
%funzione sottorilassamento

n=length(A);

D=diag(diag(A));
L=tril(A,-1);
U=triu(A,1);

BS=(D+w*L)\((1-w)*D-w*U);
h=(D+w*L)\b*w;
x0=zeros(n,1);
x=h;
k=1;
err(k)=norm(x-x0);

while (k<Nmax)&&(err(k)/norm(x)>toll)
   x0=x;
   x=BS*x0 + h;
   k=k+1;
   err(k)=norm(x-x0);
    
end
end

function [x,errj] = JACOBIMIA(A,b,Nmax,toll)
%Jacobi

n=length(A);

D=diag(diag(A));
E=A-D;

B=-(diag(1./diag(A)))*E;
h=(diag(1./diag(A)))*b;
x0=zeros(n,1);
x=h;
k=1;
errj(k)=norm(x-x0);
while (k<Nmax)&&(errj(k)/norm(x)>toll)
     x0=x;
     x=B*x0 + h;
    
    k=k+1;
    errj(k)=norm(x-x0);
   
end
end