function main
%clc
clear all

% RT1 - A Raviart-Thomas FEM code
% solution of model problem -div(kgrad p)=0 in <0,1>x<0,1>
% BC: no flow on lower and upper side, p=1 and p=0 on left and right side,
% respectively
% RT(0)-P(0) mixed FEM - assebling block matrices M and B
% Algebraic system
%  | M, B' |  *  |U|  = |G|
%  | B, 0  |     |P|    |F|

%% basic variables
ns=50;                  % number of segments per side
h = 1/ns;   
nN = 3*ns^2 + 2*ns;     % number of nodes
nT = 2*ns^2;            % number of triangles
dim=nN+nT
LK=ones(ns,ns);
sigma=2
rng('default');
RM=randn(ns,ns);
LK = (exp(1).^(sigma*RM));
maxr=max(max(LK));
minr=min(min(LK));
contrast=maxr/minr


%% local matrices and memory allocation for blocks M, B, G and F
M = sparse(nN,nN);
B = sparse(nT,nN);
G = zeros(nN,1);
F = zeros(nT,1);
coeff=zeros(ns*ns,1);
BT = [sqrt(2)*h,-h,-h];
S = h*diag([sqrt(2),-1,-1]);
V = h* [0 -1 0; 0 0 -1; 1 0 1; 0 0 -1; 0 -1 0;1 1 0];
MP=spdiags([ones(6,1),ones(6,1),2*ones(6,1),ones(6,1),ones(6,1)],[-4,-2,0,2,4],6,6);
C = full(MP);
Kcell = 1;
L = 1/Kcell*eye(6);
MT = S*V'*C*L*V*S/(24*h^2);

%% assembling M(nT,nT) and B(nE, nT), vectorization used
t0 = cputime;
lowerT = zeros(3,nT);
upperT = zeros(3,nT);
idxE = 1;
i=1;
% loop over cells - assembling matrix of edges
for r = 1:ns
    for s = 1:ns
        % reference node
        ieprev = (r-1)*(ns+2*ns+1);
        lowerT(:,idxE) = [ieprev+ns+2*s; ieprev+ns+2*s-1; ieprev+s];
        upperT(:,idxE) = [ieprev+ns+2*s; ieprev+ns+2*s+1; ieprev+ns+2*ns+s+1];
        coeff(i) =  1/LK(r,s);
        idxE = idxE+2;
        i=i+1;
    end
end
idxE = 1:2:(2*ns*ns);
% assembling M
for p = 1:3
    for q = 1:3
        M=M+sparse(lowerT(p,idxE),lowerT(q,idxE),coeff*MT(p,q),nN,nN);
        M=M+sparse(upperT(p,idxE),upperT(q,idxE),coeff*MT(p,q),nN,nN);
    end
end
% assembling B
for z = 1:3
    B=B+sparse(idxE,lowerT(z,idxE),repmat(-BT(z),ns*ns,1),nT,nN);
    B=B+sparse(idxE+1,upperT(z,idxE),repmat(BT(z),ns*ns,1),nT,nN);
end
  
%% include BC
% Neumann lower side, upper side
lowerSideNodes = 1:ns;
upperSideNodes = lowerSideNodes+(3*ns+1)*ns;

% zero rows and columns
M(lowerSideNodes,:) = 0;
M(:,lowerSideNodes) = 0;

M(upperSideNodes,:) = 0;
M(:,upperSideNodes) = 0;

B(:,lowerSideNodes) = 0;
B(:,upperSideNodes) = 0;

% units on diagonal
diagVector = zeros(nN,1);
diagVector(upperSideNodes) = 1;
diagVector(lowerSideNodes) = 1;

% update M
M = M + spdiags(diagVector,0,nN,nN);
 
% Dirichlet BC
p = 1;
dirichletNodes = ones (ns,1);
for d = 1:ns
    dirichletNodes(d) = (d-1)*(ns+2*ns+1)+ns+1;  
end
G(dirichletNodes) = (1/ns)*p;

tasemb = cputime - t0;
 
%% Saddle point matrix A and rhs
W=zeros(nT,nT);
A = [M, B'; B,W]; 
b = [G;F];
 
A = sparse(A);
b = sparse(b);

%% solvers
% direct backslash solver
%   x = A\b;
%% GMRES
%[x,flag,relres,iter,resvec] = gmres(A,b,...)
% x solution,
%flag flag = 0 gmres converged to the desired tolerance tol within maxit outer iterations.
%flag = 1         gmres iterated maxit times but did not converge.
%flag = 2         Preconditioner M was ill-conditioned.
%flag = 3         gmres stagnated. (Two consecutive iterates were the same.)
%relres     relative residual norm(b-A*x)/norm(b). If flag is 0, relres <= tol.
% iter      the outer and inner iteration numbers, where 0 <= iter(1) <=
%           maxit and 0 <= iter(2) <= restart. iter=[outer,inner], nit=[iter(1)-1]*restart+iter(2)
% resvec    vector of the residual norms at each inner iteration, including norm(b-A*x0).
%
% gmres(A,b,restart,tol,maxit,P) % P is matrix preconditioner
% gmres(A,b,restart,tol,maxit,M1,M2) % for P=M1*M2
% gmres(A,b,restart,tol,maxit,M1,M2,x0) % with init guess, default x0=zero
% gmres(A,b,restart,tol,maxit,@mfun) % matrix free preconditioner 
% function y = mfun(r)...end

% Example 1: GMRES with exact diagonal preconditioner
S=B*inv(M)*B';
P=[M,zeros(nN,nT);
   zeros(nT,nN),S];
[x,flag,relres,iter,resvec] = gmres(A,b,[],1e-6,dim,P);
it=length(resvec)-1
format long
resvec

% Example 2: GMRES with inexact diagonal preconditioner
MI=diag(diag(M));
SI=B*inv(MI)*B';
P=[MI,zeros(nN,nT);
   zeros(nT,nN),SI];
[x,flag,relres,iter,resvec] = gmres(A,b,[],1e-6,dim,P);
it=length(resvec)-1
format long
resvec