%% JL20200814: CODE TO BE OPTIMIZED 
function [GlbMat,GlbRHS, GlbVecEss, FlagDOFs] ... 
  = DarcyGen_WG_QuadriP0P0AC0_Asm( ... 
    EqnBC, BndryCondType, QuadriMesh, GAUSSQUAD)
% Assuming PermK is an elementwise constant 2x2 SPD matrix 
% James Liu, ColoState; 2014/07--2020/08 

%% Equation info 
% nu = EqnBC.nu;
nu = 1;

%% Mesh info 
NumEms = QuadriMesh.NumEms;
NumEgs = QuadriMesh.NumEgs;
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);

%% Sampling PermK and then re-using it as conductivity 
PermK = Darcy_SmplnPerm_QuadriMesh(EqnBC.fxnK, QuadriMesh, GAUSSQUAD);
PermK = PermK/nu;

%% Setup for degrees of freedom (DOFs) 
DOFs = NumEms + NumEgs;

%% Auxiliary arrays 
GM = Hdiv_QuadriAC0_NmlzPiolaBas_GramMat(QuadriMesh,GAUSSQUAD);
GMK = Hdiv_QuadriAC0_NmlzPiolaBas_GramMatK(QuadriMesh,PermK,GAUSSQUAD);
CDWGB = WG_QuadriP0P0AC0_CofAC0NmlzPiolaBas_DiscWkGradBasFxn(...
  QuadriMesh,GAUSSQUAD);
ArrayEE = zeros(NumEms,1);
ArrayEG = zeros(NumEms,4);
ArrayGG = zeros(NumEms,4,4);
%% JL20171011: TO BE REVISED FOR EFFECIENCY !!
for ie=1:NumEms
  EGM = squeeze(GMK(ie,:,:));
  ArrayEE(ie) = squeeze(CDWGB(ie,1,:))' * EGM * squeeze(CDWGB(ie,1,:));
  for i=1:4
    ArrayEG(ie,i) = squeeze(CDWGB(ie,1,:))' * EGM * squeeze(CDWGB(ie,1+i,:));
    for j=1:4
      ArrayGG(ie,i,j) = squeeze(CDWGB(ie,1+i,:))' * EGM * squeeze(CDWGB(ie,1+j,:));
    end
  end
end

%% Assembling GlbMatStf 
GlbMat = sparse(DOFs,DOFs);
% Part 1: element-element interaction 
II = (1:NumEms);  JJ = II;
GlbMat = GlbMat + sparse(II,JJ,ArrayEE,DOFs,DOFs);
% Part 2: element-edge interaction 
II = (1:NumEms);
for j=1:4
  JJ = NumEms + QuadriMesh.elem2edge(:,j);
  GlbMat = GlbMat + sparse(II,JJ,ArrayEG(:,j),DOFs,DOFs);
  GlbMat = GlbMat + sparse(JJ,II,ArrayEG(:,j),DOFs,DOFs);
end
% Part 3: edge-edge interaction 
for i=1:4
  II = NumEms + QuadriMesh.elem2edge(:,i);
  for j=i:4  % Utilizing symmetry 
    JJ = NumEms + QuadriMesh.elem2edge(:,j);
    if (j==i) 
      GlbMat = GlbMat + sparse(II,JJ,ArrayGG(:,i,j),DOFs,DOFs);
    else
      GlbMat = GlbMat + sparse(II,JJ,ArrayGG(:,i,j),DOFs,DOFs);
      GlbMat = GlbMat + sparse(JJ,II,ArrayGG(:,i,j),DOFs,DOFs);
    end
  end
end

%% Auxiliary but needed stuff 
%% Darcy: Computing local L2-projection of g=-(K/mu)f into elementwise AC0 
CofQhg = zeros(NumEms,4);
EltSrsg = zeros(NumEms,4);
MatJ = zeros(NumEms,2,2);
MatP = zeros(NumEms,2,2);
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);
  J11 = CofA(:,2) + CofA(:,4)*yhat;  
  J12 = CofA(:,3) + CofA(:,4)*xhat;
  J21 = CofB(:,2) + CofB(:,4)*yhat;  
  J22 = CofB(:,3) + CofB(:,4)*xhat;
  jac = J11.*J22 - J12.*J21;
  MatJ(:,1,1) = J11;  MatJ(:,1,2) = J12;
  MatJ(:,2,1) = J21;  MatJ(:,2,2) = J22;
  for i=1:2
    for j=1:2
      MatP(:,i,j) = MatJ(:,i,j)./jac;
    end
  end
  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];
  X = qp(:,1) - QuadriMesh.EmCntr(:,1);
  Y = qp(:,2) - QuadriMesh.EmCntr(:,2);
  U = MatP(:,1,1) .* xhat - MatP(:,1,2) .* yhat;
  V = MatP(:,2,1) .* xhat - MatP(:,2,2) .* yhat;
  gval = EqnBC.fxng(qp);
  EltSrsg(:,1) = EltSrsg(:,1) +  gval(:,1) .* jac *  GAUSSQUAD.RECT(k,3);
  EltSrsg(:,2) = EltSrsg(:,2) +  gval(:,2) .* jac *  GAUSSQUAD.RECT(k,3);
  EltSrsg(:,3) = EltSrsg(:,3) + (gval(:,1).*X + gval(:,2).*Y) .* jac *  GAUSSQUAD.RECT(k,3);
  EltSrsg(:,4) = EltSrsg(:,4) + (gval(:,1).*U + gval(:,2).*V) .* jac *  GAUSSQUAD.RECT(k,3);
end
for ie=1:NumEms 
  CofQhg(ie,:) = squeeze(GM(ie,:,:)) \ squeeze(EltSrsg(ie,:))';
end

%% Assembling GlbVecSrs 
GlbVecSrs = zeros(DOFs,1);
% For the source term 
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);
  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];
  jac = (CofB(:,3)+CofB(:,4)*xhat) .* (CofA(:,2)+CofA(:,4)*yhat)...
      - (CofA(:,3)+CofA(:,4)*xhat) .* (CofB(:,2)+CofB(:,4)*yhat);
  GlbVecSrs(1:NumEms) = GlbVecSrs(1:NumEms) ...
    + EqnBC.fxns(qp) .* jac * GAUSSQUAD.RECT(k,3);
end
% For the additional/gravity term 
EltGrav = zeros(NumEms, 5);
for i=1:5
  for j=1:4
    for k=1:4
      EltGrav(:,i) = EltGrav(:,i) + CDWGB(:,i,j) .* GM(:,j,k) .* CofQhg(:,k);
    end
  end
end
% Now assembly for the additional/gravity term to GlbVecSrs  
pos = zeros(NumEms, 5);
pos(:,1) = (1:NumEms);
for i=2:5
  pos(:,i) = NumEms + QuadriMesh.elem2edge(:,i-1);
end
for i=1:5
  GlbVecSrs(pos(:,i)) = GlbVecSrs(pos(:,i)) - EltGrav(:,i);
end

%% Treatment for Neumann boundary conditions 
GlbVecNeumann = zeros(DOFs,1);
% Darcy: Neumann = natural 
if NumEgsNeumann>0 
  NeumannBC = zeros(NumEgsNeumann,1);
  x1 = QuadriMesh.node(QuadriMesh.edge(NeumannEdge,1),1);
  y1 = QuadriMesh.node(QuadriMesh.edge(NeumannEdge,1),2);
  x2 = QuadriMesh.node(QuadriMesh.edge(NeumannEdge,2),1);
  y2 = QuadriMesh.node(QuadriMesh.edge(NeumannEdge,2),2);
  LenEgNeumann = sqrt((x2-x1).^2+(y2-y1).^2);
  NumQuadPts = size(GAUSSQUAD.LINE,1);
  for k=1:NumQuadPts
    qp = GAUSSQUAD.LINE(k,1)*[x1,y1] + GAUSSQUAD.LINE(k,2)*[x2,y2];
    uNval = EqnBC.fxnuN(qp);
    NeumannBC = NeumannBC + uNval .* LenEgNeumann * GAUSSQUAD.LINE(k,3);
  end
  loc = NumEms + NeumannEdge;  
  GlbVecNeumann(loc) = GlbVecNeumann(loc) + NeumannBC;  % Note "+" sign 
end 

%% Treatment for Dirichlet boundary conditions and flags of DOFs 
GlbVecDirichlet = zeros(DOFs,1);
% For Darcy: Dirichlet edges: Assuming NumDirichletEgs>0 
pDavg = zeros(NumEgsDirichlet,1);
x1 = QuadriMesh.node(QuadriMesh.edge(DirichletEdge,1),1);
y1 = QuadriMesh.node(QuadriMesh.edge(DirichletEdge,1),2);
x2 = QuadriMesh.node(QuadriMesh.edge(DirichletEdge,2),1);
y2 = QuadriMesh.node(QuadriMesh.edge(DirichletEdge,2),2);
qp = zeros(NumEgsDirichlet,2);
NumQuadPts = size(GAUSSQUAD.LINE,1);
for k=1:NumQuadPts
  qp(:,1) = GAUSSQUAD.LINE(k,1)*x1 + GAUSSQUAD.LINE(k,2)*x2;
  qp(:,2) = GAUSSQUAD.LINE(k,1)*y1 + GAUSSQUAD.LINE(k,2)*y2;
  pDavg = pDavg + GAUSSQUAD.LINE(k,3) * EqnBC.fxnpD(qp);
end
loc = NumEms + DirichletEdge;
GlbVecDirichlet(loc) = pDavg; 
% For flags 
FlagDOFs = zeros(DOFs,1);
FlagDOFs(loc) = 1;

%% For this WG approach: Dirichlet = essential, Neumann = natural 
GlbRHS = GlbVecSrs - GlbVecNeumann;  % Note "-" sign 
GlbVecEss = GlbVecDirichlet;

return;