%% JL20191225: BEING REVISED BY James  
function [sln] = LinElas_CG_TriP12_AsmSlv(...
  EqnBC,BndryCondType,TriMesh,GAUSSQUAD)
%% Solving linear elasticity by CG(P1^2) on a triangular mesh
% James Liu, ColoState; 2012/07--2019/12 

%% Equation info 
lambda = EqnBC.lambda;
    mu = EqnBC.mu;

%% Mesh info 
NumNds = TriMesh.NumNds;
NumEms = TriMesh.NumEms;
area = TriMesh.area;

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

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

%% Setup for discrete linear system 
DOFs = 2*NumNds;
% GlbMat = sparse(DOFs,DOFs);
% GlbRHS = zeros(DOFs,1);

%% Computing element matrices 
% Element matrix: strain-strain 
EltMatStrnStrn = zeros(NumEms,6,6);
NumQuadPts = size(GAUSSQUAD.TRIG,1);
for k=1:NumQuadPts
  [BFGrad,BFDiv] = CG_TriBR1_BasFxnGradDiv(TriMesh,...
    GAUSSQUAD.TRIG(k,1), GAUSSQUAD.TRIG(k,2), GAUSSQUAD.TRIG(k,3));
  StrnPhi = zeros(NumEms,9,2,2);
  StrnPhi(:,:,1,1) = BFGrad(:,:,1,1);
  StrnPhi(:,:,2,2) = BFGrad(:,:,2,2);
  StrnPhi(:,:,1,2) = 0.5*(BFGrad(:,:,1,2)+BFGrad(:,:,2,1));
  StrnPhi(:,:,2,1) = StrnPhi(:,:,1,2);
  for i=1:6
    for j=1:6
      EltMatStrnStrn(:,i,j) = EltMatStrnStrn(:,i,j)...
        + ( StrnPhi(:,i,1,1).*StrnPhi(:,j,1,1)...
          + StrnPhi(:,i,1,2).*StrnPhi(:,j,1,2)...
          + StrnPhi(:,i,2,1).*StrnPhi(:,j,2,1)...
          + StrnPhi(:,i,2,2).*StrnPhi(:,j,2,2)) .* area * GAUSSQUAD.TRIG(k,4);
    end
  end
end
% Element matrix: div(avg)-div(avg) 
EltMatDivDiv = zeros(NumEms,6,6);
EltDivAvg = zeros(NumEms,6);
CP1BasGrad = CG_TriP1_BasFxnGrad(TriMesh);
for j=1:3
  EltDivAvg(:,2*j-1) = CP1BasGrad(:,j,1);
  EltDivAvg(:,2*j  ) = CP1BasGrad(:,j,2);
end
for i=1:6
  for j=1:6
    EltMatDivDiv(:,i,j) = EltDivAvg(:,i) .* EltDivAvg(:,j) .* area;
  end
end
% Altogether 
% EltMat = zeros(NumEms,6,6);
EltMat = (2*mu)*EltMatStrnStrn + lambda*EltMatDivDiv;

%% Assembling GlbMat 
GlbMat = sparse(DOFs,DOFs);
pos = zeros(NumEms,6);
for j=1:3 
  k = TriMesh.elem(:,j);
  pos(:,2*j-1) = 2*k - 1;
  pos(:,2*j  ) = 2*k;
end
% Now assembling 
for i=1:6 
  II = pos(:,i);
  for j=1:6 
    JJ = pos(:,j);
    GlbMat = GlbMat + sparse(II,JJ, EltMat(:,i,j), DOFs,DOFs);
  end
end

%% Assembling GlbRHS 
GlbRHS = zeros(DOFs,1);
% Computing elementwise contributions of 9 basis functions 
EltRHS = zeros(NumEms,6);
NumQuadPts = size(GAUSSQUAD.TRIG,1);
for k=1:NumQuadPts
  qp = GAUSSQUAD.TRIG(k,1) * TriMesh.node(TriMesh.elem(:,1),:)...
     + GAUSSQUAD.TRIG(k,2) * TriMesh.node(TriMesh.elem(:,2),:)...
     + GAUSSQUAD.TRIG(k,3) * TriMesh.node(TriMesh.elem(:,3),:);
  BFVal = CG_TriBR1_BasFxnVal(TriMesh,...
    GAUSSQUAD.TRIG(k,1), GAUSSQUAD.TRIG(k,2), GAUSSQUAD.TRIG(k,3));
  fval = EqnBC.fxnf(qp);
  for j=1:6
    EltRHS(:,j) = EltRHS(:,j) + GAUSSQUAD.TRIG(k,4)...
      *( BFVal(:,j,1).*fval(:,1) + BFVal(:,j,2).*fval(:,2) ).*area;
  end  
end
% Assembling contributions of elementwise 6 basis functions 
for ie=1:NumEms
  for j=1:3  % Treating vertices and edges in one loop 
    k = TriMesh.elem(ie,j);
    GlbRHS(2*k-1) = GlbRHS(2*k-1) + EltRHS(ie,2*j-1);
    GlbRHS(2*k  ) = GlbRHS(2*k  ) + EltRHS(ie,2*j  );
  end
end

%% JL20171221: TO BE REVISED 
%% Incorporating boundary conditions: Neumann as natural 
if NumNeumannEgs>0
  NeumannBC = zeros(NumNeumannEgs,2,2);
  j1 = TriMesh.edge(NeumannEdge,1);
  j2 = TriMesh.edge(NeumannEdge,2);
  x1 = TriMesh.node(j1,1);  y1 = TriMesh.node(j1,2);
  x2 = TriMesh.node(j2,1);  y2 = TriMesh.node(j2,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];
    tNval = EqnBC.fxntN(qp);
    NeumannBC(:,1,1) = NeumannBC(:,1,1) + GAUSSQUAD.LINE(k,3)...
      * GAUSSQUAD.LINE(k,1) * tNval(:,1) .* LenEgNeumann;
    NeumannBC(:,1,2) = NeumannBC(:,1,2) + GAUSSQUAD.LINE(k,3)...
      * GAUSSQUAD.LINE(k,1) * tNval(:,2) .* LenEgNeumann;
    NeumannBC(:,2,1) = NeumannBC(:,2,1) + GAUSSQUAD.LINE(k,3)...
      * GAUSSQUAD.LINE(k,2) * tNval(:,1) .* LenEgNeumann;
    NeumannBC(:,2,2) = NeumannBC(:,2,2) + GAUSSQUAD.LINE(k,3)...
      * GAUSSQUAD.LINE(k,2) * tNval(:,2) .* LenEgNeumann;
  end
  for ig=1:NumNeumannEgs 
    j1 = TriMesh.edge(NeumannEdge(ig),1);
    j2 = TriMesh.edge(NeumannEdge(ig),2);
    GlbRHS(2*j1-1) = GlbRHS(2*j1-1) - NeumannBC(ig,1,1);
    GlbRHS(2*j1  ) = GlbRHS(2*j1  ) - NeumannBC(ig,1,2);
    GlbRHS(2*j2-1) = GlbRHS(2*j2-1) - NeumannBC(ig,2,1);
    GlbRHS(2*j2  ) = GlbRHS(2*j2  ) - NeumannBC(ig,2,2);
  end
end

%% Incorporating boundary conditions: Dirichlet as essential 
sln = zeros(DOFs,1);
% Assuming NumDirichletEgs>0 
% For nodes 
uDval = EqnBC.fxnuD(TriMesh.node(DirichletNode,:));
sln(2*DirichletNode-1) = uDval(:,1);
sln(2*DirichletNode  ) = uDval(:,2);

%% Reducing...
GlbRHS = GlbRHS - GlbMat*sln;

%% Flag 
flag = ones(DOFs,1);
flag(2*DirichletNode-1) = 0;
flag(2*DirichletNode  ) = 0;
FreeNdEg = find(flag);

%% Solving the reduced linear system directly 
sln(FreeNdEg) = GlbMat(FreeNdEg,FreeNdEg) \ GlbRHS(FreeNdEg);

return;
% 
% %% Mesh info 
% NumNds = TriMesh.NumNds;
% NumEms = TriMesh.NumEms;
% k1 = TriMesh.elem(:,1);  k2 = TriMesh.elem(:,2);  k3 = TriMesh.elem(:,3); 
% x1 = TriMesh.node(k1,1);  y1 = TriMesh.node(k1,2);
% x2 = TriMesh.node(k2,1);  y2 = TriMesh.node(k2,2);
% x3 = TriMesh.node(k3,1);  y3 = TriMesh.node(k3,2);
% area = 0.5*((x2-x1).*(y3-y1)-(x3-x1).*(y2-y1));
% 
% %% Boundary info 
% NumDirichletEgs = size(DirichletEdge,1);
% NumNeumannEgs = size(NeumannEdge,1);
% % JL20160808: TO BE REVISED FOR EFFICIENCY !
% DirichletNodeFlag = zeros(NumNds,1);
% for ig=1:NumDirichletEgs
%   k1 = TriMesh.edge(DirichletEdge(ig),1);
%   k2 = TriMesh.edge(DirichletEdge(ig),2);
%   DirichletNodeFlag(k1) = 1;
%   DirichletNodeFlag(k2) = 1;
% end
% DirichletNode = find(DirichletNodeFlag);
% NumDirichletNds = sum(DirichletNodeFlag);
% 
% %% Setup 
% DOFs = 2*NumNds;
% GlbMat = sparse(DOFs,DOFs);
% GlbRHS = zeros(DOFs,1);
% 
% %% Auxiliary quantities 
% % For 3 scalar basis functions 
% CP1BasGrad = zeros(NumEms,3,2);
% CP1BasGrad(:,1,:) = [(y2-y3)./(2*area), (x3-x2)./(2*area)];
% CP1BasGrad(:,2,:) = [(y3-y1)./(2*area), (x1-x3)./(2*area)];
% CP1BasGrad(:,3,:) = [(y1-y2)./(2*area), (x2-x1)./(2*area)];
% % For 6 vector basis functions 
% GradPhi = zeros(NumEms,6,4);  % 2x2 matrix treated as 4-vector 
% for i=1:3
%   GradPhi(:,i,  [1,2]) = CP1BasGrad(:,i,:);
%   GradPhi(:,i+3,[3,4]) = CP1BasGrad(:,i,:);
% end
% % For the strain 
% StrainPhi = zeros(NumEms,6,4);  % 2x2 matrix treated as 4-vector
% StrainPhi(:,:,1) = GradPhi(:,:,1);
% StrainPhi(:,:,4) = GradPhi(:,:,4);
% StrainPhi(:,:,2) = 0.5*(GradPhi(:,:,2)+GradPhi(:,:,3));
% StrainPhi(:,:,3) = StrainPhi(:,:,2);
% 
% %% Computing EltMat 
% % For the strain part 
% EltMatStrain = zeros(NumEms,6,6);
% for i=1:6
%   for j=1:6
%     EltMatStrain(:,i,j) = sum(StrainPhi(:,i,:).*StrainPhi(:,j,:),3).*area;
%   end
% end
% % For the divergence part 
% EltMatDiv = zeros(NumEms,6,6);
% for i=1:3
%   for j=1:3
%     EltMatDiv(:,i,  j  ) = CP1BasGrad(:,i,1).*CP1BasGrad(:,j,1).*area;
%     EltMatDiv(:,i,  j+3) = CP1BasGrad(:,i,1).*CP1BasGrad(:,j,2).*area;
%     EltMatDiv(:,i+3,j  ) = CP1BasGrad(:,i,2).*CP1BasGrad(:,j,1).*area;
%     EltMatDiv(:,i+3,j+3) = CP1BasGrad(:,i,2).*CP1BasGrad(:,j,2).*area;
%   end
% end
% % Together 
% % EltMat = zeros(NumEms,6,6);
% EltMat = (2*mu)*EltMatStrain + lambda*EltMatDiv;
% 
% %% Assembling GlbMat 
% for i=1:3
%   II2 = 2*TriMesh.elem(:,i);
%   II1 = II2 - 1;
%   for j=1:3
%     JJ2 = 2*TriMesh.elem(:,j);
%     JJ1 = JJ2 - 1;
%     GlbMat = GlbMat + sparse(II1,JJ1, EltMat(:,i,  j  ), DOFs,DOFs);
%     GlbMat = GlbMat + sparse(II1,JJ2, EltMat(:,i,  j+3), DOFs,DOFs);
%     GlbMat = GlbMat + sparse(II2,JJ1, EltMat(:,i+3,j  ), DOFs,DOFs);
%     GlbMat = GlbMat + sparse(II2,JJ2, EltMat(:,i+3,j+3), DOFs,DOFs);
%   end
% end
% 
% %% Assembling GlbRHS 
% GlbRHS = zeros(DOFs,1);
% %% JL20131004: Old style temporarily, TO BE REVISED FOR EFFECIENCY 
% %% Assembling GlbRHS 
% GlbRHS = zeros(DOFs,1);
% NumQuadPts = size(GAUSSQUAD.TRIG,1);
% phi = zeros(1,3);
% for ie=1:NumEms
%   for k=1:NumQuadPts
%     qp = GAUSSQUAD.TRIG(k,1) * TriMesh.node(TriMesh.elem(ie,1),:)...
%        + GAUSSQUAD.TRIG(k,2) * TriMesh.node(TriMesh.elem(ie,2),:)...
%        + GAUSSQUAD.TRIG(k,3) * TriMesh.node(TriMesh.elem(ie,3),:);
%     fval = EqnBC.fxnf(qp);
%     for j=1:3;  phi(j) = GAUSSQUAD.TRIG(k,j);  end
%     for j=1:3
%       GlbRHS(2*TriMesh.elem(ie,j)-1) = GlbRHS(2*TriMesh.elem(ie,j)-1)...
%         + GAUSSQUAD.TRIG(k,4) * fval(1)*phi(j) * area(ie);
%       GlbRHS(2*TriMesh.elem(ie,j)  ) = GlbRHS(2*TriMesh.elem(ie,j)  )...
%         + GAUSSQUAD.TRIG(k,4) * fval(2)*phi(j) * area(ie);
%     end
%   end
% end
% 
% %% Incorporating boundary conditions: Neumann as natural 
% % Approach I: working on all these edges simultaneously 
% if NumNeumannEgs>0
%   NeumannBC = zeros(NumNeumannEgs,2,2);
%   k1 = TriMesh.edge(NeumannEdge,1);
%   k2 = TriMesh.edge(NeumannEdge,2);
%   x1 = TriMesh.node(k1,1);  y1 = TriMesh.node(k1,2);
%   x2 = TriMesh.node(k2,1);  y2 = TriMesh.node(k2,2);
%   % LenEgNeum = sqrt((x2-x1).^2+(y2-y1).^2);
%   NumQuadPts = size(GAUSSQUAD.LINE,1);
%   qp = zeros(NumNeumannEgs,2);
%   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;
%     phi1 = sqrt((x2-qp(:,1)).^2+(y2-qp(:,2)).^2);  % ./LenEgNeum;
%     phi2 = sqrt((qp(:,1)-x1).^2+(qp(:,2)-y1).^2);  % ./LenEgNeum;
%   end
%   tNval = EqnBC.fxntN(qp);
%   NeumannBC(:,1,1) = NeumannBC(:,1,1) + GAUSSQUAD.LINE(k,3)*(tNval(:,1).*phi1);
%   NeumannBC(:,2,1) = NeumannBC(:,2,1) + GAUSSQUAD.LINE(k,3)*(tNval(:,2).*phi1);
%   NeumannBC(:,1,2) = NeumannBC(:,1,2) + GAUSSQUAD.LINE(k,3)*(tNval(:,1).*phi2);
%   NeumannBC(:,2,2) = NeumannBC(:,2,2) + GAUSSQUAD.LINE(k,3)*(tNval(:,2).*phi2);
%   for ig=1:NumNeumannEgs
%     k1 = TriMesh.edge(NeumannEdge(ig),1);
%     k2 = TriMesh.edge(NeumannEdge(ig),2);
%     GlbRHS(2*k1-1) = GlbRHS(2*k1-1) - NeumannBC(ig,1,1);
%     GlbRHS(2*k1  ) = GlbRHS(2*k1  ) - NeumannBC(ig,2,1);
%     GlbRHS(2*k2-1) = GlbRHS(2*k2-1) - NeumannBC(ig,1,2);
%     GlbRHS(2*k2  ) = GlbRHS(2*k2  ) - NeumannBC(ig,2,2);
%   end
% end
% 
% %% For reducing... 
% sln = zeros(DOFs,1);
% flag = zeros(DOFs,1);
% flag(2*DirichletNode-1) = ones(NumDirichletNds,1);
% flag(2*DirichletNode  ) = ones(NumDirichletNds,1);
% FreeNd = find(~flag);
% 
% %% Incorporating boundary conditions: Dirichlet as essential 
% % Approach I: working on all these edges simultaneously 
% % Assuming NumDirichletEgs>0 
% DiriNd = TriMesh.node(DirichletNode,:);
% uDval = EqnBC.fxnuD(DiriNd);
% sln(2*DirichletNode-1) = uDval(:,1);
% sln(2*DirichletNode  ) = uDval(:,2);
% 
% %% Reducing...
% GlbRHS = GlbRHS - GlbMat*sln;
% 
% %% Solving the reduced global linear system directly 
% sln(FreeNd) = GlbMat(FreeNd,FreeNd) \ GlbRHS(FreeNd);
% 
% return;