// JL20170612: TO BE FINISHED/REVISED
// WG3d_Tetra.cpp
// This file will be expanded as we develop more weak Galerkin finite elements 
// James Liu, Graham Harper, ColoState; 2014/07--2017/07

// Weak Galerkin 3-dim finite elements on tetrahedra:
//   (P0,P0;RT0):                      Good for sure
//   (P1,P0;P0^3) with stabilization:  Theoretically good

#include "matrix.h"
#include "vector.h"

#include "cell3d.h"
#include "GaussQuad.h"
#include "Hdiv3d.h"
#include "PtVec3d.h"
#include "TetraMesh.h"


////////////////////////////////////////////////////////////////////////////////
// For WG3d: Tetra(P0,P0;RT0)
////////////////////////////////////////////////////////////////////////////////

// Computing the coefficients of disc.wk.grad.
// WG3d: Tetra(P0,P0;RT0): Coeff. of disc.wk.grad. in RT0 nmlz.bas. (5-by-4)

int WG3d_TetraP0P0RT0_CofDiscWkGrad_NmlzBas(FullMatrix &CDWG,
                                            const Tetra &tetra,
                                            Tri3d STF[4], int sign[4],
                                            const GaussQuad &GQTe)
{
  int i, j;
  double sa, vol;
  PtVec3d EmCntr, FcCntr, nml;
  Tri3d tri;
  Mat3 MatK;
  Vector RHS(4);
  DiagMatrix GM(4);
  FullMatrix GMK(4,4);
  CDWG.resize(5,4);  // WG bas.fxns. as rows, RT0 bas.fxns. as columns

  //
  vol = tetra.volume();
  EmCntr = tetra.center();

  Hdiv_TetraRT0_GramMat_NmlzBas(GM, tetra, GQTe);

  // For WG interior constant function
  // RHS(1) = 0;  RHS(2) = 0;  RHS(3) = 0;  RHS(4) = -3*vol;
  // for (j=1; j<=4; ++j)  CDWG(1,j) = RHS(j) / GM.entry(j);
  RHS(4) = -3*vol;
  CDWG(1,4) = RHS(4) / GM.entry(4);

  // For WG face constant functions
  for (i=1; i<=4; ++i) {
    tri = STF[i-1];
    FcCntr = tri.center();
    sa = tri.area();
    nml = tri.normal();
    RHS(1) = nml.xCrd() * sa * sign[i-1];
    RHS(2) = nml.yCrd() * sa * sign[i-1];
    RHS(3) = nml.zCrd() * sa * sign[i-1];
    RHS(4) = dotProduct(FcCntr-EmCntr,nml) * sa * sign[i-1];
    for (j=1; j<=4; ++j)  CDWG(i+1,j) = RHS(j) / GM.entry(j);
  }
  
  return(0);  // if successful
}


// WG3d: Tetra(P0,P0;RT0): Element grad-grad matrix (5-by-5)
// with diffusion/permeability (3x3) matrix MatK

int WG3d_TetraP0P0RT0_EltGradGradMatK(FullMatrix &EGGMK, const Tetra &tetra,
                                      Tri3d STF[4], int sign[4],
                                      Mat3 &MatK, const GaussQuad &GQTe)
{
  int i, j, k, kr, kc;
  Vector cofi(4), cofj(4);
  FullMatrix GMK(4,4), CDWG(5,4);
  EGGMK.resize(5,5);

  Hdiv_TetraRT0_GramMatK_NmlzBas(GMK, tetra, MatK, GQTe);
  
  WG3d_TetraP0P0RT0_CofDiscWkGrad_NmlzBas(CDWG, tetra, STF, sign, GQTe);
  // std::cout << CDWG << "\n";

  for (i=1; i<=5; ++i) {
    for (j=1; j<=5; ++j) {
      EGGMK(i,j) = 0;
      for (kr=1; kr<=4; ++kr)
        for (kc=1; kc<=4; ++kc)
          EGGMK(i,j) += CDWG(i,kr) * GMK(kr,kc) * CDWG(j,kc);
    }
  }

/*
  for (i=1; i<=5; ++i) {
    for (k=1; k<=4; ++k)  cofi(k) = CDWG(i,k);
    for (j=1; j<=5; ++j) {
      for (k=1; k<=4; ++k)  cofj(k) = CDWG(j,k);
      EGGMK(i,j) = dotProd(cofi, GMK*cofj);
    }
  }
*/
  
  return(0);  // if successful
}


// WG3d: Tetra(P0,P0;RT0): Mixed product of disc.wk.grad. & bas.fxn.val.
// with another vector-valued function, e.g., velocity

int WG3d_TetraP0P0RT0_EltMatGradVal(FullMatrix &EGV,
                                    PtVec3d (*fxnVec)(const PtVec3d &),
                                    const Tetra &tetra, Tri3d STF[4], int sign[4],
                                    const GaussQuad &GQTe)
{
  FullMatrix CDWG(5,4);
  Vector w(4);

  PtVec3d cntr = tetra.center();
  double xc = cntr.xCrd();
  double yc = cntr.yCrd();
  double zc = cntr.zCrd();
  double vol = tetra.volume();

  WG3d_TetraP0P0RT0_CofDiscWkGrad_NmlzBas(CDWG, tetra, STF, sign, GQTe);

  EGV.resize(5,1);
  for (int i=0; i<GQTe.numberQuadraturePoints(); ++i) {
    PtVec3d qp(0,0,0);
    for (int j=0; j<GQTe.numberVertices(); ++j) {
      qp = qp + GQTe.baryCoordinate(i,j) * tetra.vertex(j);
    }
    double x = qp.xCrd();
    double y = qp.yCrd();
    double z = qp.zCrd();
    double X = x - xc;
    double Y = y - yc;
    double Z = z - zc;
    PtVec3d vec = fxnVec(qp);
    w(1) = vec.xCrd();
    w(2) = vec.yCrd();
    w(3) = vec.zCrd();
    w(4) = vec.xCrd()*X + vec.yCrd()*Y + vec.zCrd()*Z;
    for (int k=1; k<=5; ++k) {
      double tmp = CDWG(k,1)*w(1) + CDWG(k,2)*w(2)
                 + CDWG(k,3)*w(3) + CDWG(k,4)*w(4);
      EGV(k,1) = EGV(k,1) + tmp * GQTe.weight(i) * vol;
    }
  }

  return(0);  // if successful
}


// WG3d: Tetra(P0,P0;RT0):
// Projecting a spatial scalar function to WG(P0,P0) on a tetrahedral mesh

int WG3d_TetraP0P0RT0_ProjScaFxn0(Vector &ProjScaEm, Vector &ProjScaFc,
                                  double (*fxnSca)(const PtVec3d&),
                                  const TetraMesh &mesh,
                                  const GaussQuad &GQTe, const GaussQuad &GQT)
{
  // Setup
  ProjScaEm.resize(mesh.numberElements());
  ProjScaFc.resize(mesh.numberFaces());

  // Projecting the scalar function onto p.w. const. space over tetrahedra
  // i.e., Computing averages using a Gaussian quadrature for tetrahedron
  for (int le=0; le<mesh.numberElements(); ++le) {
    int labele = le + mesh.beginLabelElement();
    Tetra tetra = mesh.element(labele);
    // double vol = tetra.volume();  // Not needed
    double ftmp = 0;
    for (int i=0; i<GQTe.numberQuadraturePoints(); ++i) {
      PtVec3d qp(0,0,0);
      for (int j=0; j<GQTe.numberVertices(); ++j) {
        qp = qp + GQTe.baryCoordinate(i,j) * tetra.vertex(j);
      }
      double fval = fxnSca(qp);
      ftmp += GQTe.weight(i) * fval;  // */ vol
    }
    ProjScaEm(labele) = ftmp;
  }

  // Projecting the scalar function onto p.w. const. space over triangular faces
  // i.e., Computing averages using a Gaussian quadrature for triangle
  for (int lc=0; lc<mesh.numberFaces(); ++lc) {
    int labelc = lc + mesh.beginLabelFace();
    Tri3d tri = mesh.face(labelc);
    double ftmp = 0;
    for (int i=0; i<GQT.numberQuadraturePoints(); ++i) {
      PtVec3d qp(0,0,0);
      for (int j=0; j<GQT.numberVertices(); ++j) {
        qp = qp + GQT.baryCoordinate(i,j) * tri.vertex(j);
      }
      double fval = fxnSca(qp);
      ftmp += GQT.weight(i) * fval;  // */ area
    }
    ProjScaFc(labelc) = ftmp;
  }

  return(0);  // if successful
}


////////////////////////////////////////////////////////////////////////////////
// JL20161016: TO BE FINISHED/REVISED
// For WG3d: Tetra(P1,P0;P0^3) with stabilizer
////////////////////////////////////////////////////////////////////////////////

// WG3d: Tetra(P1,P0;P0^3): Coeffs. of disc.wk.grad. in P_0^3 nat.bas. (8-by-3)

FullMatrix WG3d_TetraP1P0P03_CofDiscWkGrad_NatBas(const Tetra &tetra,
                                                  Tri3d STF[4], int sign[4])
{
  FullMatrix CDWGB(8,3);
  
  //
  double vol = tetra.volume();
  double vol1 = 1/vol;
  
  // NOTE: For 1<=i<=4: coeff.disc.wk.grad.=0
  for (int i=5; i<=8; ++i) {
    double ar = STF[i-5].area();
    PtVec3d vtmp = (vol1 * ar * sign[i-5]) * STF[i-5].normal();
    CDWGB(i,1) = vtmp.xCrd();
    CDWGB(i,2) = vtmp.yCrd();
    CDWGB(i,3) = vtmp.zCrd();
  }
  
  return CDWGB;
}


// WG3d: Tetra(P1,P0;P0^3): Element grad-grad matrix (8-by-8)

FullMatrix WG3d_TetraP1P0P03_EltGradGradMatK(const Tetra &tetra, Mat3 &MatK,
                                             Tri3d STF[4], int sign[4],
                                             const GaussQuad &GQTe,
                                             const GaussQuad &GQT)
{
  FullMatrix EGGMK(8,8);
  FullMatrix CDWGB(8,3);
  FullMatrix GMK(3,3);

  //
  double vol = tetra.volume();
  for (int i=1; i<=3; ++i)
    for (int j=1; j<=3; ++j)
      GMK(i,j) = vol * MatK(i,j);

  CDWGB = WG3d_TetraP1P0P03_CofDiscWkGrad_NatBas(tetra, STF, sign);

  // JL20160828: TO BE REVISED FOR EFFECIENCY
  // NOTE: i,j start at 5, because coeff.disc.wk.grad.=0 for 1<=i<=4
  for (int i=5; i<=8; ++i) {
    for (int j=5; j<=8; ++j) {
      EGGMK(i,j) = 0;
      for (int k=1; k<=3; ++k)
        for (int l=1; l<=3; ++l)
          EGGMK(i,j) += GMK(k,l) * CDWGB(i,k) * CDWGB(j,l);
    }
  }
  
  return EGGMK;
}


// WG3d: Tetra(P1,P0;P0^3): Elementwise stabilizer matrix (8-by-8)

FullMatrix WG3d_TetraP1P0P03_EltStabMat(const Tetra &tetra,
                                        Tri3d STF[4], int sign[4],
                                        const GaussQuad &GQTe,
                                        const GaussQuad &GQT)
{
  FullMatrix ESM(8,8);
  double ar[4];  // ar for tri.surf. area
  double Xm[4], Ym[4], Zm[4];  // midpoint coordinates for X,Y,Z on 4 faces

  PtVec3d cntr = tetra.center();
  double xc = cntr.xCrd();
  double yc = cntr.yCrd();
  double zc = cntr.zCrd();
  
  for (int i=1; i<=4; ++i) {
    PtVec3d midpt = STF[i-1].center();
    Xm[i-1] = midpt.xCrd() - xc;
    Ym[i-1] = midpt.yCrd() - yc;
    Zm[i-1] = midpt.zCrd() - zc;
    ar[i-1] = STF[i-1].area();
  }

  // For element interior basis functions themselves
  for (int j=1; j<=4; ++j) {
    ESM(1,1) += ar[j-1];
    ESM(1,2) += ar[j-1]*Xm[j-1];
    ESM(1,3) += ar[j-1]*Ym[j-1];
    ESM(1,4) += ar[j-1]*Zm[j-1];
    ESM(2,2) += ar[j-1]*Xm[j-1]*Xm[j-1];
    ESM(2,3) += ar[j-1]*Ym[j-1]*Xm[j-1];
    ESM(2,4) += ar[j-1]*Zm[j-1]*Xm[j-1];
    ESM(3,3) += ar[j-1]*Ym[j-1]*Ym[j-1];
    ESM(3,4) += ar[j-1]*Zm[j-1]*Ym[j-1];
    ESM(4,4) += ar[j-1]*Zm[j-1]*Zm[j-1];
  }
  for (int i=2; i<=4; ++i)
    for (int j=1; j<=i-1; ++j)
      ESM(i,j) = ESM(j,i);
  
  // For interaction of interior bas.fxns. and face bas.fxns.
  for (int j=1; j<=4; ++j) {
    ESM(1,4+j) = -ar[j-1];          ESM(4+j,1) = ESM(1,4+j);
    ESM(2,4+j) = -ar[j-1]*Xm[j-1];  ESM(4+j,2) = ESM(2,4+j);
    ESM(3,4+j) = -ar[j-1]*Ym[j-1];  ESM(4+j,3) = ESM(3,4+j);
    ESM(4,4+j) = -ar[j-1]*Zm[j-1];  ESM(4+j,4) = ESM(4,4+j);
  }

  // For the face basis functions themselves
  for (int i=1; i<=4; ++i)  ESM(4+i,4+i) = ar[i-1];

  return ESM;
}


////////////////////////////////////////////////////////////////////////////////
// Old-style code
////////////////////////////////////////////////////////////////////////////////

// Tetra: Tetrahedron coordinates (x[i],y[i],z[i]) (i=0,1,2,3) for 4 vertices
// RT0 Normalized Basis:
//   [1;0;0], [0;1;0], [0;0;1], [X;Y;Z]
// where X=x-xc, Y=y-yc, Z=z-zc, and (xc,yc,zc) is the element center

/*
 int WG3d_TetraP0P0RT0_CofDiscWkGrad_NmlzBas(
 double x[], double y[], double z[], double CDWG[][4])
 {
 double vol, vol1, w, w1;
 double GM[4], GMI[4];
 
 Hdiv_TetraRT0_GramMat_NmlzBas(x, y, z, GM, GMI);
 
 vol = GM[0];  w = GM[3];
 vol1 = GMI[0];  w1 = GMI[3];
 
 // double CDWG[5][4];
 // 1st index: 0 for element interior bas.fxn. 1-4 for face bas.fxns.,
 // 2nd index: 0-3 for the four normalized basis function of RT0
 // for (int i=0; i<5; ++i)
 //   for (int j=0; j<4; ++j)
 //     CDWG[i][j] = 0.0;
 
 // For the WG basis function in the element interior
 CDWG[0][0] = 0;
 CDWG[0][1] = 0;
 CDWG[0][2] = 0;
 CDWG[0][3] = -3 * vol * w1;
 
 // JL20150121: NEED DOUBLE-CHECK!!
 // For the WG basis function on the 4 faces
 CDWG[1][0] = 0.5*vol1*((y[2]-y[1])*(z[3]-z[1]) - (y[3]-y[1])*(z[2]-z[1]));
 CDWG[1][1] = 0.5*vol1*((z[2]-z[1])*(x[3]-x[1]) - (z[3]-z[1])*(x[2]-x[1]));
 CDWG[1][2] = 0.5*vol1*((x[2]-x[1])*(y[3]-y[1]) - (x[3]-x[1])*(y[2]-y[1]));
 CDWG[1][3] = 0.75 * vol * w1;
 //
 CDWG[2][0] = 0.5*vol1*((y[0]-y[2])*(z[3]-z[2]) - (y[3]-y[0])*(z[2]-z[0]));
 CDWG[2][1] = 0.5*vol1*((x[0]-x[2])*(y[3]-y[2]) - (x[3]-x[0])*(y[2]-y[0]));
 CDWG[2][2] = 0.5*vol1*((x[0]-x[2])*(y[3]-y[2]) - (x[3]-x[0])*(y[2]-y[0]));
 CDWG[2][3] = 0.75 * vol * w1;
 //
 CDWG[3][0] = 0.5*vol1*((y[0]-y[3])*(z[1]-z[3]) - (y[1]-y[3])*(z[0]-z[3]));
 CDWG[3][1] = 0.5*vol1*((z[0]-z[3])*(x[1]-x[3]) - (z[1]-z[3])*(x[0]-x[3]));
 CDWG[3][2] = 0.5*vol1*((x[0]-x[3])*(y[1]-y[3]) - (x[1]-x[3])*(y[0]-y[3]));
 CDWG[3][3] = 0.75 * vol * w1;
 //
 CDWG[4][0] = 0.5*vol1*((y[2]-y[0])*(z[1]-z[0]) - (y[1]-y[0])*(z[2]-z[0]));
 CDWG[4][1] = 0.5*vol1*((z[2]-z[0])*(x[1]-x[0]) - (z[1]-z[0])*(x[2]-x[0]));
 CDWG[4][2] = 0.5*vol1*((x[2]-x[0])*(y[1]-y[0]) - (x[1]-x[0])*(y[2]-y[0]));
 CDWG[4][3] = 0.75 * vol * w1;
 
 return(0);
 }
 */


// WG3d: Tetra(P0,P0,RT0): element grad-grad matrix
/*
 void WG3d_TetraP0P0RT0_EltGradGrad_NmlzBas(
 double x[], double y[], double z[], double EGGM[][5])
 {
 // JL20150121: SEEMS NOT NEEDED
 // double a1, a2, a3, b1, b2, b3, c1, c2, c3, det, vol;
 // a1 = x[1] - x[0];  b1 = y[1] - y[0];  c1 = z[1] - z[0];
 // a2 = x[2] - x[0];  b2 = y[2] - y[0];  c2 = z[2] - z[0];
 // a3 = x[3] - x[0];  b3 = y[3] - y[0];  c3 = z[3] - z[0];
 // det = a1*b2*c3 + a2*b3*c1 + a3*b1*c2 - a1*b3*c2 - a2*b1*c3 - a3*b2*c1;
 /// vol = det/6.0;  // 6=3!
 
 int i, j, k;
 double GM[4], GMI[4], CDWG[5][4];
 
 // JL20150121: NEED RESOLVE REDUNDANT CALLS!
 Hdiv_TetraRT0_GramMat_NmlzBas(x, y, z, GM, GMI);
 // WG3d_TetraP0P0RT0_CofDiscWkGrad_NmlzBas(x, y, z, CDWG);
 
 // for (k=0; k<4; ++k)
 //   std::cout << GM[k] << "  ";
 // std::cout << "\n";
 
 // double EGGM[5][5];
 // 1st/2nd index: 0 for element interior bas.fxn., 1-4 for face bas.fxns.
 // Using an implicit formula
 for (i=0; i<5; ++i) {
 for (j=0; j<5; ++j) {
 EGGM[i][j] = 0.0;
 for (k=0; k<4; ++k)
 EGGM[i][j] = EGGM[i][j] + CDWG[i][k] * GM[k] * CDWG[j][k];
 }
 }
 
 return;
 }
 */


////////////////////////////////////////////////////////////////////////////////
// For WG(P0^3,P0^3;RT0^3,P0) for elasticity
////////////////////////////////////////////////////////////////////////////////

// JL20170612: TO BE FINISHED/REVISED By Graham
// WG3d: Tetra(P0^3,P0^3;RT0^3,P0):
// Coeff. of disc.wk.grad. in RT0^3 nmlz.bas. for 15 WG bas.fxns
// 15-by-12 matrix

FullMatrix WG3d_TetraP03P03RT03P0_CofNmlzBas_DiscWkGradBasFxn(
  const Tetra &tetra, Tri3d STF[4], int sign[4],
  const GaussQuad &GQTe, const GaussQuad &GQT)
{
  double vol = tetra.volume();
  PtVec3d EmCntr = tetra.center();
  double xc = EmCntr.xCrd();
  double yc = EmCntr.yCrd();
  double zc = EmCntr.zCrd();
  
  FullMatrix AllRHS(15,12);
  AllRHS(1,4)  = -3*vol;
  AllRHS(2,8)  = -3*vol;
  AllRHS(3,12) = -3*vol;
  for (int j=0; j<4; ++j) {
    double ar = STF[j].area();
    PtVec3d nml = STF[j].normal();
    nml = sign[j] * nml;  // JL20170614: NOTE the sign change in nml
    PtVec3d FcCntr = STF[j].center();
    double Xm = FcCntr.xCrd() - xc;
    double Ym = FcCntr.yCrd() - yc;
    double Zm = FcCntr.zCrd() - zc;
    AllRHS(3+3*j+1, 1) = ar * nml.xCrd();
    AllRHS(3+3*j+1, 2) = ar * nml.yCrd();
    AllRHS(3+3*j+1, 3) = ar * nml.zCrd();
    AllRHS(3+3*j+1, 4) = ar * dotProduct(PtVec3d(Xm,Ym,Zm),nml);
    AllRHS(3+3*j+2, 5) = ar * nml.xCrd();
    AllRHS(3+3*j+2, 6) = ar * nml.yCrd();
    AllRHS(3+3*j+2, 7) = ar * nml.zCrd();
    AllRHS(3+3*j+2, 8) = ar * dotProduct(PtVec3d(Xm,Ym,Zm),nml);
    AllRHS(3+3*j+3, 9) = ar * nml.xCrd();
    AllRHS(3+3*j+3,10) = ar * nml.yCrd();
    AllRHS(3+3*j+3,11) = ar * nml.zCrd();
    AllRHS(3+3*j+3,12) = ar * dotProduct(PtVec3d(Xm,Ym,Zm),nml);
  }

  DiagMatrix GM(12);
  Hdiv_TetraRT03_GramMat_NmlzBas(GM, tetra, GQTe);
  
  FullMatrix CDWGB(15,12);
  for (int i=1; i<=15; ++i) {
    for (int j=1; j<=12; ++j) {
      CDWGB(i,j) = AllRHS(i,j)/GM.entry(j);
    }
  }
  
  return CDWGB;
}


// WG3d: Tetra(P0^3,P0^3;RT0^3,P0):
// Coeff. of disc.wk.div. in P0 nmlz.bas. for 15 WG bas.fxns
// 15-dim vector

Vector WG3d_TetraP03P03RT03P0_CofNmlzBas_DiscWkDivBasFxn(
  const Tetra &tetra, Tri3d STF[4], int sign[4],
  const GaussQuad &GQTe, const GaussQuad &GQT)
{
  double vol = tetra.volume();
  Vector CDWDB(15);
  // For the 3 WG basis functions for element interior, it is 0
  for (int j=0; j<4; ++j) {
    PtVec3d nml = STF[j].normal();
    double ar = STF[j].area();
    CDWDB(3+3*j+1) = (sign[j]*nml.xCrd()) * ar/vol;
    CDWDB(3+3*j+2) = (sign[j]*nml.yCrd()) * ar/vol;
    CDWDB(3+3*j+3) = (sign[j]*nml.zCrd()) * ar/vol;
  }
  return CDWDB;
}


// WG3d: Tetra(P0^3,P0^3;RT0^3,P0):
// Elementwise discrete weak div-div matrix

FullMatrix WG3d_TetraP03P03RT03P0_NmlBas_EltDivDivMat(
  const Tetra &tetra, Tri3d STF[4], int sign[4],
  const GaussQuad &GQTe, const GaussQuad &GQT)
{
  Vector CDWDB(15);
  CDWDB = WG3d_TetraP03P03RT03P0_CofNmlzBas_DiscWkDivBasFxn(tetra,STF,sign,GQTe,GQT);
  
  double vol = tetra.volume();
  
  FullMatrix EDDM(15,15);
  for (int i=1; i<=15; ++i) {
    for (int j=1; j<=15; ++j) {
      EDDM(i,j) = CDWDB(i) * CDWDB(j) * vol;
    }
  }
  
  return EDDM;
}


// WG3d: Tetra(P0^3,P0^3;RT0^3,P0):
// Elementwise discrete weak grad-grad matrix

FullMatrix WG3d_TetraP03P03RT03P0_NmlBas_EltGradGradMat(
  const Tetra &tetra, Tri3d STF[4], int sign[4],
  const GaussQuad &GQTe, const GaussQuad &GQT)
{
  FullMatrix CDWGB(15,12);
  CDWGB = WG3d_TetraP03P03RT03P0_CofNmlzBas_DiscWkGradBasFxn(tetra,STF,sign,GQTe,GQT);
  
  DiagMatrix GM(12);
  Hdiv_TetraRT03_GramMat_NmlzBas(GM, tetra, GQTe);

  FullMatrix EGGM(15,15);
  for (int i=1; i<=15; ++i) {
    for (int j=1; j<=15; ++j) {
      EGGM(i,j) = 0;
      for (int k=1; k<=12; ++k) {
        EGGM(i,j) = EGGM(i,j) + CDWGB(i,k) * GM.entry(k) * CDWGB(j,k);
      }
    }
  }
  
  return EGGM;
}


// WG3d: Tetra(P0^3,P0^3;RT0^3,P0):
// Elementwise discrete weak strain-strain matrix

FullMatrix WG3d_TetraP03P03RT03P0_NmlBas_EltStrnStrnMat(
  const Tetra &tetra, Tri3d STF[4], int sign[4],
  const GaussQuad &GQTe, const GaussQuad &GQT)
{
  FullMatrix CDWGB(15,12);
  CDWGB = WG3d_TetraP03P03RT03P0_CofNmlzBas_DiscWkGradBasFxn(tetra,STF,sign,GQTe,GQT);
  
  FullMatrix GMA(12,12);
  Hdiv_TetraRT03_GramMatAvg_NmlzBas(GMA, tetra, GQTe);

  FullMatrix ESSM(15,15);
  for (int i=1; i<=15; ++i) {
    for (int j=1; j<=15; ++j) {
      ESSM(i,j) = 0;
      for (int k=1; k<=12; ++k) {
        for (int l=1; l<=12; ++l) {
          ESSM(i,j) = ESSM(i,j) + CDWGB(i,k) * GMA(k,l) * CDWGB(j,l);
        }
      }
    }
  }
  
  return ESSM;
}

// WG3d_Tetra.cpp
