%metodi iterativi per sistemi lineari con
%interpretazione grafica per il pb di minimizzazione associato (se dim(A)=2)
%Ax=b  <--> min 1/2 <Ax,x> - <b,x>

clear
clc

N=2; %dimensione matrice
toll=10e-10; %precisione approssimazione su || x^(k+1)-x^k ||
it_max=10000;  %numero massimo di iterazioni

%matrice A (def pos)
A=[2 -1; -1 2];
%vettore b
b=ones(N,1);

%vincolo S={x : Cx-q=0}
C=[1 1];
q=0;
eps=0.1; %solo per penalizzazione

%punto iniziale x^0
%x0=[-1.5; 1.5];
%x0=[1; -1.8];
lambda0=2;  %solo Uzawa

%::::::::::::: CORE ::::::::::::::::::::::

%P e' una matrice che contiene il risultato di ogni iterazione: La prima
%riga e' x^0, la k-ma riga e' x^(k-1).
%it e' il numero di righe di P (numero di iterazioni effettuate +1)

%[P,it]=Penalizzazione(A,b,C,q,x0,eps,toll,it_max); scelta=1;
[P,it]=Uzawa(A,b,C,q,lambda0,toll,it_max); scelta=2;
%[P,it]=GradienteProiettato(A,b,x0,toll,it_max); scelta=3;

%:::::::::::::::::::::::::::::::::::::::::

soluz=P(it,:)';  %soluzione ( ultima delle x^k) )

%-------------------------------------
%plot (solo se N=2)
if N==2
 [x,y]=meshgrid(-2:0.1:2,-2:0.1:2);
 %creazione della matrice z contenente i valori della funzione 1/2 <Ax,x> - <b,x>, x in R^2
 for i=1:size(x,1)
    for j=1:size(y,2)
       v=[x(i,j);y(i,j)];
       z(i,j)=( 0.5*dot(A*v,v) ) - dot(b,v);
       z_eps(i,j)=z(i,j) + ( (1/eps)*(norm(C*v-q,2)^2) );
    end
 end
 contour(x,y,z,60)  %disegna le curve di livello di 1/2 <Ax,x> - <b,x>
 title('f')
 axis square
 hold on
 plot(P(:,1),P(:,2),'ks-','markersize',2) %disegna gli x^k
 if (scelta==1 || scelta==2) 
   t=-2:0.2:2; plot(t,-(C(1)*t)/C(2)); %disegna il vincolo Cx-q=0
 end
 if scelta==3 
   plot([-1.5 0],[-1.8 -1.8]);
   plot([-1.5 0],[-1.3 -1.3]);
   plot([-1.5 -1.5],[-1.8 -1.3]);
   plot([0 0],[-1.8 -1.3]);
 end
 if scelta==1
   figure
   contour(x,y,z_eps,60)  %disegna le curve di livello di 1/2 <Ax,x> - <b,x>
   title('f_{eps}')
   axis square
   hold on
   plot(P(:,1),P(:,2),'ks-','markersize',2) %disegna gli x^k
   t=-2:0.2:2; plot(t,-(C(1)*t)/C(2));  %disegna il vincolo Cx-q=0
 end
end