function [K,f] = FEM_system(X,T,referenceElement,example)
% [K,f] = FEM_system(X,T,referenceElement,example)
% Matrix K and r.h.s vector f obtained after discretizong a 2D convection-diffusion-reaction equation
%
% X:            nodal coordinates
% T:            connectivities (elements)
% referenceElement: reference element properties (quadrature, shape functions...)
% example: example properties


velo = example.velo;
a = example.a;
nu = example.nu;
sigma=example.sigma;
method = example.method;
s=example.s;
%ax = velo(1);
%ay = velo(2);
Te = T(1,:); Xe = X(Te,:); 
h=example.h;


nen = referenceElement.nen;
ngaus = referenceElement.ngaus;
wgp = referenceElement.GaussWeights;
N = referenceElement.N;
Nxi = referenceElement.Nxi;
Neta = referenceElement.Neta;
N2xi = referenceElement.N2xi; 
N2eta = referenceElement.N2eta;
N2xieta = referenceElement.N2xieta;
N2etaxi = referenceElement.N2etaxi;

% Number of elements and number of nodes in the mesh
nElem = size(T,1);
nPt = size(X,1);

K = zeros(nPt,nPt);
f = zeros(nPt,1);

if method == 0
    % Galerkin
    tau = 0;
elseif method == 1
  % SUPG
    Pe = a*h/(2*nu);
    tau_p = h*(1 + 9/Pe^2 +(sigma*h)/(2*a))^(-1/2)/(2*a); % modified
    disp(strcat('Recommended stabilization parameter = ',num2str(tau_p)));
    tau = cinput('Stabilization parameter',tau_p);
    if isempty(tau)
        tau = tau_p;
    end
elseif method == 2
    % GLS
    Pe = a*h/(2*nu);
    tau_p = h*(1 + 9/Pe^2 +(sigma*h)/(2*a))^(-1/2)/(2*a); % modified
    disp(strcat('Recommended stabilization parameter = ',num2str(tau_p)));
    tau = cinput('Stabilization parameter',tau_p);
    if isempty(tau)
        tau = tau_p;
    end
else
    error ('Unavailable method')
end

% Loop on elements
for ielem = 1:nElem
    % Te: global number of the nodes in the current element
    Te = T(ielem,:);
    % Xe: coordenates of the nodes in the current element
    Xe = X(Te,:);
    % Element matrices
    [Ke,fe] = EleMat(Xe,nen,ngaus,wgp,N,Nxi,Neta,N2xi,N2eta,N2xieta,N2etaxi,method,tau,velo,nu,sigma,s);
    % Assemble the element matrices
    K(Te,Te) = K(Te,Te) + Ke;
    f(Te) = f(Te) + fe;
end



function [Ke,fe] = EleMat(Xe,nen,ngaus,wgp,N,Nxi,Neta,N2xi,N2eta,N2xieta,N2etaxi,method,tau,velo,nu,sigma,s)
%

ax = velo(1);
ay = velo(2);

Ke = zeros(nen,nen);
fe = zeros(nen,1);


% Loop on Gauss points (computation of integrals on the current element)
for ig = 1:ngaus
    N_ig = N(ig,:);
    Nxi_ig = Nxi(ig,:);
    Neta_ig = Neta(ig,:);
    N2xi_ig = N2xi(ig,:);
    N2eta_ig = N2eta(ig,:);
    N2xieta_ig = N2xieta(ig,:);
    N2etaxi_ig = N2etaxi(ig,:);
    
    Jacob = [Nxi_ig*(Xe(:,1))	Nxi_ig*(Xe(:,2))
        Neta_ig*(Xe(:,1))	Neta_ig*(Xe(:,2))];
    
    % Laplacian term added to account for second order term in stabilasation part
    Hassian = [(Nxi_ig*Xe(:,1))^2 (Nxi_ig*Xe(:,2))*(Nxi_ig*Xe(:,1)) N2xi_ig*Xe(:,1) (Nxi_ig*Xe(:,1))*(Nxi_ig*Xe(:,2)) (Nxi_ig*Xe(:,2))^2 N2xi_ig*Xe(:,2);
             (Neta_ig*Xe(:,1))*(Nxi_ig*Xe(:,1)) (Neta_ig*Xe(:,2))*(Nxi_ig*Xe(:,1)) N2xieta_ig*Xe(:,1) (Neta_ig*Xe(:,1))*(Nxi_ig*Xe(:,2)) (Neta_ig*Xe(:,2))*(Nxi_ig*Xe(:,2)) N2xieta_ig*Xe(:,2)
             0 0 Nxi_ig*Xe(:,1) 0 0 Nxi_ig*(Xe(:,2))
             (Nxi_ig*Xe(:,1))*(Neta_ig*Xe(:,1)) (Nxi_ig*Xe(:,2))*(Neta_ig*Xe(:,1)) N2etaxi_ig*Xe(:,1) (Nxi_ig*Xe(:,1))*(Neta_ig*Xe(:,2)) (Nxi_ig*Xe(:,2))*(Neta_ig*Xe(:,2)) N2etaxi_ig*Xe(:,2)
             (Neta_ig*Xe(:,1))^2 (Neta_ig*Xe(:,2))*(Neta_ig*Xe(:,1)) N2eta_ig*Xe(:,1) (Neta_ig*Xe(:,1))*(Neta_ig*Xe(:,2)) (Neta_ig*Xe(:,2))^2 N2eta_ig*Xe(:,2);
             0 0 Neta_ig*Xe(:,1) 0 0 Neta_ig*(Xe(:,2))];
         
    dvolu = wgp(ig)*det(Jacob);
    res   = Hassian\[N2xi_ig;N2xieta_ig;Nxi_ig;N2etaxi_ig;N2eta_ig;Neta_ig];
    Nx = res(3,:);
    Ny = res(6,:);
    Nxx = res(1,:);
    Nyy = res(5,:);
    Hes = Nxx + Nyy;
    
    if method == 0
        % Galerkin (modified)
        Ke = Ke + (nu*(Nx'*Nx+Ny'*Ny) + N_ig'*(ax*Nx+ay*Ny)+ N_ig'*sigma*N_ig )*dvolu;
        aux = N_ig*Xe; 
        f_ig = SourceTerm(aux,s);
        fe = fe + N_ig'*(f_ig*dvolu);
    elseif method == 1
       % SUPG (modified)
        Ke = Ke + (nu*(Nx'*Nx+Ny'*Ny) + N_ig'*(ax*Nx+ay*Ny)+ N_ig'*sigma*N_ig + ...
            tau*(ax*Nx+ay*Ny)'*(ax*Nx+ay*Ny - nu*Hes+ N_ig*sigma))*dvolu;
        aux = N_ig*Xe; 
        f_ig = SourceTerm(aux,s);
        fe = fe + (N_ig+tau*(ax*Nx+ay*Ny))'*(f_ig*dvolu);
    else
        % GLS (modified)
        Ke = Ke + (nu*(Nx'*Nx+Ny'*Ny) +N_ig'*(ax*Nx+ay*Ny)+ N_ig'*sigma*N_ig + ...
            tau*(ax*Nx+ay*Ny-nu*Hes+ N_ig*sigma)'*(ax*Nx+ay*Ny-nu*Hes+ N_ig*sigma))*dvolu;
        aux = N_ig*Xe; 
        f_ig = SourceTerm(aux,s);
        fe = fe + (N_ig+tau*(ax*Nx+ay*Ny-nu*Hes+ N_ig*sigma))'*(f_ig*dvolu);
    end
end

