function [fST,ST] = glsMethod(X,T,av,nu,sigma,s,tau,referenceElement)
% [fST,ST] = glsMethod(X,T,av,nu,sigma,s,tau,referenceElement)
% Creates stabilization terms for GLS method.
% Takes as inputs system problems and tau term.
% Outputs fST vector of stabilization to be added to f and ST matrix of
% stabilization terms to be added at K.

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

ST  = zeros(nPt);
fST = zeros(nPt,1);

% Loop on elements
for ielem=1:nElem
    Te = T(ielem,:); 
    Xe = X(Te,:); 
    ae = av(Te,:);
    se = s(Te,:);

    [fSTe,STe] = EleMat(Xe,ae,nu,sigma,se,tau,referenceElement);
    
    % Assembly
    ST(Te,Te)  = ST(Te,Te) + STe;
    fST(Te)    = fST(Te)   + fSTe;
end


function [fSTe,STe] = EleMat(Xe,ae,nu,sigma,se,tau,referenceElement)

% reference element information
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;

STe  = zeros(nen);
fSTe = zeros(nen,1);

% Loop on Gauss points 
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,:);
    
    J     = [Nxi_ig*Xe(:,1)  Nxi_ig*(Xe(:,2))
             Neta_ig*Xe(:,1) Neta_ig*(Xe(:,2))];
         
    Jacob = [(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(J);
    res   = Jacob\[N2xi_ig;N2xieta_ig;Nxi_ig;N2etaxi_ig;N2eta_ig;Neta_ig];
    d2N   = res(1,:)+res(5,:);
    a     = N_ig*ae; 
    a_dN = a(1)*res(3,:) + a(2)*res(6,:);

    STe  = STe  + tau*(a_dN-nu*d2N+sigma*N_ig)'*(a_dN-nu*d2N+sigma*N_ig)*dvolu;
    fSTe = fSTe + tau*(a_dN-nu*d2N+sigma*N_ig)'*mean(se)*dvolu;
end
