%% JL20200814: CODE TO BE OPTIMIZED 
function [GlbMat,GlbRHS, GlbVecEss, FlagDOFs] = ...
  Stokes_CG_QuadriBR1Q0_Asm(EqnBC,BndryCondType,QuadriMesh,GAUSSQUAD)
%% Stokes_CG_QuadriBR1Q0_Asm: Assembing only 
% James Liu, ColoState; 2012/07--2020/08 

%% Equation info 
% nu = EqnBC.nu 
nu = 1;  % Remember to change it when needed 

%% Mesh info 
NumNds = QuadriMesh.NumNds;
NumEgs = QuadriMesh.NumEgs;
NumEms = QuadriMesh.NumEms;
CofA = QuadriMesh.CofA;
CofB = QuadriMesh.CofB;

%% Sorting out boundary edges: Dirichlet, Neumann 
DirichletEdge = find(BndryCondType(QuadriMesh.BndryEdge+1)==1);
NeumannEdge   = find(BndryCondType(QuadriMesh.BndryEdge+1)==2);

%% Boundary info 
NumEgsDirichlet = size(DirichletEdge,1);
NumEgsNeumann = size(NeumannEdge,1);
% JL20160808: TO BE REVISED FOR EFFICIENCY !
DirichletNodeFlag = zeros(NumNds,1);
for ig=1:NumEgsDirichlet
  j1 = QuadriMesh.edge(DirichletEdge(ig),1);
  j2 = QuadriMesh.edge(DirichletEdge(ig),2);
  DirichletNodeFlag(j1) = 1;
  DirichletNodeFlag(j2) = 1;
end
DirichletNode = find(DirichletNodeFlag);

%% Setup for degrees of freedom (DOFs) 
DOFsVel = 2*NumNds + NumEgs;
DOFsPres = NumEms;
DOFs = DOFsVel + DOFsPres;

%% Computing and modifying elemeentwise matrices 
EltMatGradGrad = zeros(NumEms,12,12);
NumQuadPts = size(GAUSSQUAD.RECT,1);
GAUSSQUADRECT1 = ones(NumQuadPts,2) - GAUSSQUAD.RECT(:,1:2);
for k=1:NumQuadPts
  xhat = GAUSSQUADRECT1(k,1);
  yhat = GAUSSQUADRECT1(k,2);
  jac = (CofB(:,3)+CofB(:,4)*xhat) .* (CofA(:,2)+CofA(:,4)*yhat)...
      - (CofA(:,3)+CofA(:,4)*xhat) .* (CofB(:,2)+CofB(:,4)*yhat);
  [BFGrad,BFDiv] = CG_QuadriBR1_BasFxnGradDiv(QuadriMesh,xhat,yhat);
  for i=1:12
    for j=1:12
      EltMatGradGrad(:,i,j) = EltMatGradGrad(:,i,j) ...
        + nu*( BFGrad(:,i,1,1).*BFGrad(:,j,1,1) ...
             + BFGrad(:,i,1,2).*BFGrad(:,j,1,2) ...
             + BFGrad(:,i,2,1).*BFGrad(:,j,2,1) ...
             + BFGrad(:,i,2,2).*BFGrad(:,j,2,2) ... 
             ) .* jac * GAUSSQUAD.RECT(k,3);
    end
  end
end
for i=1:12
  for j=9:12
    EltMatGradGrad(:,i,j) = EltMatGradGrad(:,i,j) .* QuadriMesh.EmEgSign(:,j-8);
  end
end
for i=9:12
  for j=1:12
    EltMatGradGrad(:,i,j) = EltMatGradGrad(:,i,j) .* QuadriMesh.EmEgSign(:,i-8);
  end
end
EltMatPresDiv = zeros(NumEms,12);
NumQuadPts = size(GAUSSQUAD.RECT,1);
GAUSSQUADRECT1 = ones(NumQuadPts,2) - GAUSSQUAD.RECT(:,1:2);
for k=1:NumQuadPts
  xhat = GAUSSQUADRECT1(k,1);
  yhat = GAUSSQUADRECT1(k,2);
  jac = (CofB(:,3)+CofB(:,4)*xhat) .* (CofA(:,2)+CofA(:,4)*yhat)...
      - (CofA(:,3)+CofA(:,4)*xhat) .* (CofB(:,2)+CofB(:,4)*yhat);
  [BFGrad,BFDiv] = CG_QuadriBR1_BasFxnGradDiv(QuadriMesh,xhat,yhat);
  for i=1:12
    EltMatPresDiv(:,i) = EltMatPresDiv(:,i) ...
      + BFDiv(:,i) .* jac * GAUSSQUAD.RECT(k,3);
  end
end
for i=9:12
  EltMatPresDiv(:,i) = EltMatPresDiv(:,i) .* QuadriMesh.EmEgSign(:,i-8);
end

%% JL20180224: NEED TO FIND A MORE EFFECIENT WAY FOR NODE-BASED ASSEMBLY
%% Assembling GlbMat 
GlbMat = sparse(DOFs,DOFs);
pos = zeros(NumEms,12);
for j=1:4  % Treating vertices and edges in one loop 
  k = QuadriMesh.elem(:,j);
  pos(:,2*j-1) = 2*k-1; 
  pos(:,2*j  ) = 2*k; 
  k = QuadriMesh.elem2edge(:,j);
  pos(:,8+j) = 2*NumNds + k;  
end
% For velocity grad-grad 
% JL20180224: THIS IS RISKY 
for i=1:12 
  II = pos(:,i);
  for j=1:12 
    JJ = pos(:,j);
    GlbMat = GlbMat + sparse(II,JJ, EltMatGradGrad(:,i,j), DOFs,DOFs);
  end
end
% For pressure and velocity div 
% JL20180224: THIS IS ALSO RISKY 
JJ = 2*NumNds + NumEgs + (1:NumEms);
for i=1:12 
  II = pos(:,i);
  GlbMat = GlbMat + sparse(II,JJ, -EltMatPresDiv(:,i), DOFs,DOFs);
  GlbMat = GlbMat + sparse(JJ,II, -EltMatPresDiv(:,i), DOFs,DOFs);
end

%% Computing elementwise source terms for 12 basis functions 
EltSrs = zeros(NumEms,12);
NumQuadPts = size(GAUSSQUAD.RECT,1);
GAUSSQUADRECT1 = ones(NumQuadPts,2) - GAUSSQUAD.RECT(:,1:2);
for k=1:NumQuadPts
  xhat = GAUSSQUADRECT1(k,1);
  yhat = GAUSSQUADRECT1(k,2);
  jac = (CofB(:,3)+CofB(:,4)*xhat) .* (CofA(:,2)+CofA(:,4)*yhat)...
      - (CofA(:,3)+CofA(:,4)*xhat) .* (CofB(:,2)+CofB(:,4)*yhat);
  qp = [CofA(:,1) + CofA(:,2)*xhat + CofA(:,3)*yhat + CofA(:,4)*xhat*yhat,...
        CofB(:,1) + CofB(:,2)*xhat + CofB(:,3)*yhat + CofB(:,4)*xhat*yhat];
  fval = EqnBC.fxnf(qp);
  BFVal = CG_QuadriBR1_BasFxnVal(QuadriMesh,xhat,yhat);
  for j=1:12
    EltSrs(:,j) = EltSrs(:,j) + GAUSSQUAD.RECT(k,3)...
      *( BFVal(:,j,1).*fval(:,1) + BFVal(:,j,2).*fval(:,2) ).*jac;
  end 
end
% Modification for 4 edge-based basis functions 
for j=1:4
  EltSrs(:,8+j) = EltSrs(:,8+j) .* QuadriMesh.EmEgSign(:,j);
end

%% Assembling contributions of elementwise 12 basis functions
GlbVecSrs = zeros(DOFs,1);
for ie=1:NumEms
  for j=1:4  % Treating vertices and edges in one loop 
    k = QuadriMesh.elem(ie,j);
    GlbVecSrs(2*k-1) = GlbVecSrs(2*k-1) + EltSrs(ie,2*j-1);
    GlbVecSrs(2*k  ) = GlbVecSrs(2*k  ) + EltSrs(ie,2*j  );
    k = QuadriMesh.elem2edge(ie,j);
    GlbVecSrs(2*NumNds+k) = GlbVecSrs(2*NumNds+k) + EltSrs(ie,8+j);
  end
end

%% JL20200813: TO BE ADDED LATER 
%% Treatment for Neumann boundary conditions 
GlbVecNeumann = zeros(DOFs,1);

%% Treatment for Dirichlet boundary conditions and flags of DOFs 
GlbVecDirichlet = zeros(DOFs,1);
% For Dirichlet nodes 
uDval = EqnBC.fxnuD(QuadriMesh.node(DirichletNode,:));
GlbVecDirichlet(2*DirichletNode-1) = uDval(:,1);
GlbVecDirichlet(2*DirichletNode  ) = uDval(:,2);
% For Dirichlet edges: Based on projection of the interpolation residual 
intgrl = zeros(NumEgsDirichlet,1);
nml = QuadriMesh.EgNml(DirichletEdge,:);
j1 = QuadriMesh.edge(DirichletEdge,1);
j2 = QuadriMesh.edge(DirichletEdge,2);
tmp1 = GlbVecDirichlet([2*j1-1, 2*j1]);
tmp2 = GlbVecDirichlet([2*j2-1, 2*j2]);
NumQuadPts = size(GAUSSQUAD.LINE,1);
for k=1:NumQuadPts
  r = GAUSSQUAD.LINE(k,2);
  qp = GAUSSQUAD.LINE(k,1) * QuadriMesh.node(j1,:) ... 
     + GAUSSQUAD.LINE(k,2) * QuadriMesh.node(j2,:);
  val = EqnBC.fxnuD(qp);
  intplt = (1-r) * tmp1 + r * tmp2;
  res = val - intplt;
  intgrl = intgrl + dot(res,nml,2) * GAUSSQUAD.LINE(k,3);
end
% JL20200726: Double-check "6" 
GlbVecDirichlet(2*NumNds+DirichletEdge) = intgrl * 6;
% For flags 
FlagDOFs = zeros(DOFs,1);
FlagDOFs(2*DirichletNode-1) = 1;
FlagDOFs(2*DirichletNode  ) = 1;
FlagDOFs(2*NumNds+DirichletEdge) = 1;

%% In this CG approach: Dirichlet = essential, Neumann = natural 
GlbRHS = GlbVecSrs + GlbVecNeumann;
GlbVecEss = GlbVecDirichlet;

return;