// FE_Tetra_CP13.cpp
// James Liu, ColoState; 2014/07--2018/02

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

#include "cell3d.h"
#include "GaussQuad.h"
#include "mat3.h"
#include "PtVec3d.h"


// JL20150513: TO BE REVISED FOR EFFECIENCY
// The vector values of the 12 basis functions at a point

int FE_Tetra_CP13_BasFxnVal(PtVec3d val[12], const Tetra &tetra, PtVec3d pt)
{
  double cof[4][4], dd, x[4], y[4], z[4];
  double xx, yy, zz;
  PtVec3d P[4], U[4], V[4], W[4];

  // The determinant for the tetra
  dd = 6 * tetra.volume();
  
  // Primary points/vectors
  for (int i=0; i<4; ++i) {
    P[i] = tetra.vertex(i);
    x[i] = P[i].xCrd();
    y[i] = P[i].yCrd();
    z[i] = P[i].zCrd();
  }
  
  // Auxiliary points/vectors
  for (int i=0; i<4; ++i) {
    U[i] = PtVec3d(1,y[i],z[i]);
    V[i] = PtVec3d(1,x[i],z[i]);
    W[i] = PtVec3d(1,x[i],y[i]);
  }

  // For scalar basis function phi_0
  cof[0][0] =  det3P(P[1],P[2],P[3])/dd;
  cof[0][1] = -det3P(U[1],U[2],U[3])/dd;
  cof[0][2] =  det3P(V[1],V[2],V[3])/dd;
  cof[0][3] = -det3P(W[1],W[2],W[3])/dd;

  // For scalar basis function phi_1
  cof[1][0] = -det3P(P[0],P[2],P[3])/dd;
  cof[1][1] =  det3P(U[0],U[2],U[3])/dd;
  cof[1][2] = -det3P(V[0],V[2],V[3])/dd;
  cof[1][3] =  det3P(W[0],W[2],W[3])/dd;

  // For scalar basis function phi_2
  cof[2][0] =  det3P(P[0],P[1],P[3])/dd;
  cof[2][1] = -det3P(U[0],U[1],U[3])/dd;
  cof[2][2] =  det3P(V[0],V[1],V[3])/dd;
  cof[2][3] = -det3P(W[0],W[1],W[3])/dd;

  // For scalar basis function phi_3
  cof[3][0] = -det3P(P[0],P[1],P[2])/dd;
  cof[3][1] =  det3P(U[0],U[1],U[2])/dd;
  cof[3][2] = -det3P(V[0],V[1],V[2])/dd;
  cof[3][3] =  det3P(W[0],W[1],W[2])/dd;

  //
  xx = pt.xCrd();  yy = pt.yCrd();  zz = pt.zCrd();
  for (int i=0; i<4; ++i) {
    double sval = cof[i][0] + cof[i][1]*xx + cof[i][2]*yy + cof[i][3]*zz;
    val[3*i+0] = PtVec3d(sval,0,0);
    val[3*i+1] = PtVec3d(0,sval,0);
    val[3*i+2] = PtVec3d(0,0,sval);
  }

  return(0);  // If successful
}


// The constant gradient (3x3) matrices of the 12 basis functions
// regardless of the point(s) in the tetrahedral element

int FE_Tetra_CP13_BasFxnGrad(Mat3 BasFxnGrad[12], const Tetra &tetra)
{
  double cof[4][4], dd, x[4], y[4], z[4];
  PtVec3d P[4], U[4], V[4], W[4];

  // The determinant for the tetra
  dd = 6 * tetra.volume();
  
  // Primary points/vectors
  for (int i=0; i<4; ++i) {
    P[i] = tetra.vertex(i);
    x[i] = P[i].xCrd();
    y[i] = P[i].yCrd();
    z[i] = P[i].zCrd();
  }
  
  // Auxiliary points/vectors
  for (int i=0; i<4; ++i) {
    U[i] = PtVec3d(1,y[i],z[i]);
    V[i] = PtVec3d(1,x[i],z[i]);
    W[i] = PtVec3d(1,x[i],y[i]);
  }
  
  // For scalar basis function phi_0
  cof[0][1] = -det3P(U[1],U[2],U[3])/dd;
  cof[0][2] =  det3P(V[1],V[2],V[3])/dd;
  cof[0][3] = -det3P(W[1],W[2],W[3])/dd;
  
  // For scalar basis function phi_1
  cof[1][1] =  det3P(U[0],U[2],U[3])/dd;
  cof[1][2] = -det3P(V[0],V[2],V[3])/dd;
  cof[1][3] =  det3P(W[0],W[2],W[3])/dd;
  
  // For scalar basis function phi_2
  cof[2][1] = -det3P(U[0],U[1],U[3])/dd;
  cof[2][2] =  det3P(V[0],V[1],V[3])/dd;
  cof[2][3] = -det3P(W[0],W[1],W[3])/dd;
  
  // For scalar basis function phi_3
  cof[3][1] =  det3P(U[0],U[1],U[2])/dd;
  cof[3][2] = -det3P(V[0],V[1],V[2])/dd;
  cof[3][3] =  det3P(W[0],W[1],W[2])/dd;

  // For any point (the gradient is a constant 3x3 matrix across the element)
  for (int i=0; i<4; ++i) {
    for (int j=0; j<3; ++j) {
      BasFxnGrad[3*i+j](1+j,1) = cof[i][1];
      BasFxnGrad[3*i+j](1+j,2) = cof[i][2];
      BasFxnGrad[3*i+j](1+j,3) = cof[i][3];
    }
  }

  return(0);  // If successful
}


// The constant divergences of the 12 basis functions
// regardless of the point(s) in the tetrahedral element

int FE_Tetra_CP13_BasFxnDiv(double BasFxnDiv[12], const Tetra &tetra)
{
  double cof[4][4], dd, x[4], y[4], z[4];
  PtVec3d P[4], U[4], V[4], W[4];
  
  // The determinant for the tetra
  dd = 6 * tetra.volume();
  
  // Primary points/vectors
  for (int i=0; i<4; ++i) {
    P[i] = tetra.vertex(i);
    x[i] = P[i].xCrd();
    y[i] = P[i].yCrd();
    z[i] = P[i].zCrd();
  }
  
  // Auxiliary points/vectors
  for (int i=0; i<4; ++i) {
    U[i] = PtVec3d(1,y[i],z[i]);
    V[i] = PtVec3d(1,x[i],z[i]);
    W[i] = PtVec3d(1,x[i],y[i]);
  }

  // For scalar basis function phi_0
  cof[0][1] = -det3P(U[1],U[2],U[3])/dd;
  cof[0][2] =  det3P(V[1],V[2],V[3])/dd;
  cof[0][3] = -det3P(W[1],W[2],W[3])/dd;
  
  // For scalar basis function phi_1
  cof[1][1] =  det3P(U[0],U[2],U[3])/dd;
  cof[1][2] = -det3P(V[0],V[2],V[3])/dd;
  cof[1][3] =  det3P(W[0],W[2],W[3])/dd;
  
  // For scalar basis function phi_2
  cof[2][1] = -det3P(U[0],U[1],U[3])/dd;
  cof[2][2] =  det3P(V[0],V[1],V[3])/dd;
  cof[2][3] = -det3P(W[0],W[1],W[3])/dd;
  
  // For scalar basis function phi_3
  cof[3][1] =  det3P(U[0],U[1],U[2])/dd;
  cof[3][2] = -det3P(V[0],V[1],V[2])/dd;
  cof[3][3] =  det3P(W[0],W[1],W[2])/dd;
  
  // For any point (the divergence is a constant scalar across the element)
  for (int i=0; i<4; ++i) {
    for (int j=0; j<3; ++j) {
      BasFxnDiv[3*i+j] = cof[i][1+j];
    }
  }

  return(0);  // If successful
}


// The constant stress tensors (3x3 symmetric matrices) of the 12 basis functions
// regardless of the point(s) in the tetrahedral element

int FE_Tetra_CP13_BasFxnStress(Mat3 BasFxnStrs[12], const Tetra &tetra,
                               double lambda, double mu)
{
  double cof[4][4], dd, x[4], y[4], z[4];
  double BasFxnDiv[12];
  PtVec3d P[4], U[4], V[4], W[4];
  Mat3 BasFxnGrad[12];
  
  // The determinant for the tetra
  dd = 6 * tetra.volume();
  
  // Primary points/vectors
  for (int i=0; i<4; ++i) {
    P[i] = tetra.vertex(i);
    x[i] = P[i].xCrd();
    y[i] = P[i].yCrd();
    z[i] = P[i].zCrd();
  }
  
  // Auxiliary points/vectors
  for (int i=0; i<4; ++i) {
    U[i] = PtVec3d(1,y[i],z[i]);
    V[i] = PtVec3d(1,x[i],z[i]);
    W[i] = PtVec3d(1,x[i],y[i]);
  }
  
  // For scalar basis function phi_0
  cof[0][1] = -det3P(U[1],U[2],U[3])/dd;
  cof[0][2] =  det3P(V[1],V[2],V[3])/dd;
  cof[0][3] = -det3P(W[1],W[2],W[3])/dd;
  
  // For scalar basis function phi_1
  cof[1][1] =  det3P(U[0],U[2],U[3])/dd;
  cof[1][2] = -det3P(V[0],V[2],V[3])/dd;
  cof[1][3] =  det3P(W[0],W[2],W[3])/dd;
  
  // For scalar basis function phi_2
  cof[2][1] = -det3P(U[0],U[1],U[3])/dd;
  cof[2][2] =  det3P(V[0],V[1],V[3])/dd;
  cof[2][3] = -det3P(W[0],W[1],W[3])/dd;
  
  // For scalar basis function phi_3
  cof[3][1] =  det3P(U[0],U[1],U[2])/dd;
  cof[3][2] = -det3P(V[0],V[1],V[2])/dd;
  cof[3][3] =  det3P(W[0],W[1],W[2])/dd;
  
  // The gradient and divergence
  for (int i=0; i<4; ++i) {
    for (int j=0; j<3; ++j) {
      BasFxnGrad[3*i+j](1+j,1) = cof[i][1];
      BasFxnGrad[3*i+j](1+j,2) = cof[i][2];
      BasFxnGrad[3*i+j](1+j,3) = cof[i][3];
      BasFxnDiv[3*i+j] = cof[i][1+j];
    }
  }

  // Now the stress tensors (3x3 symmetric matrices)
  for (int i=0; i<4; ++i) {
    for (int j=0; j<3; ++j) {
      int k = 3*i + j;
      BasFxnStrs[k] = mu * (BasFxnGrad[k]+BasFxnGrad[k].transpose());
      BasFxnStrs[k](1,1) += lambda * BasFxnDiv[k];
      BasFxnStrs[k](2,2) += lambda * BasFxnDiv[k];
      BasFxnStrs[k](3,3) += lambda * BasFxnDiv[k];
    }
  }

  return(0);  // If successful
}


// JL20160901: TO BE REVISED FOR EFFECIENCY
// The 12x12 grad-grad matrix

FullMatrix FE_Tetra_CP13_EltMatGradGrad(const Tetra &tetra)
{
  Mat3 BFG[12];
  FullMatrix EGGM(12,12);

  double vol = tetra.volume();
  
  FE_Tetra_CP13_BasFxnGrad(BFG, tetra);

  for (int i=1; i<=12; ++i)
    for (int j=1; j<=12; ++j)
      EGGM(i,j) = colonProduct(BFG[i-1], BFG[j-1]) * vol;

  return EGGM;
}


// JL20160901: TO BE REVISED FOR EFFECIENCY
// The 12x12 div-div matrix

FullMatrix FE_Tetra_CP13_EltMatDivDiv(const Tetra &tetra)
{
  double BFD[12];
  FullMatrix EDDM(12,12);

  double vol = tetra.volume();

  FE_Tetra_CP13_BasFxnDiv(BFD, tetra);
  
  for (int i=1; i<=12; ++i)
    for (int j=1; j<=12; ++j)
      EDDM(i,j) = BFD[i-1] * BFD[j-1] * vol;
      
  return EDDM;
}


// A size-12 vector for integrals of 12 bas.fxns. against data at quad.points
// Assume data have type PtVec3d, size NumQuadPts

Vector FE_Tetra_CP13_IntgrlBasFxnData(const Tetra &tetra, PtVec3d *data,
                                      const GaussQuad &GQTe)
{
  Vector intgrl(12);
  PtVec3d qp, BFV[12];

  double vol = tetra.volume();

  for (int k=0; k<GQTe.numberQuadraturePoints(); ++k) {
    qp = PtVec3d(0,0,0);
    for (int j=0; j<GQTe.numberVertices(); ++j) {
      qp = qp + GQTe.baryCoordinate(k,j) * tetra.vertex(j);
    }
    FE_Tetra_CP13_BasFxnVal(BFV, tetra, qp);
    for (int i=1; i<=12; ++i) {
      intgrl(i) += GQTe.weight(k) * dotProduct(BFV[i-1],data[k]);
    }
  }
  intgrl = vol * intgrl;
  
  return intgrl;
}


// Interaction: CP1^3 (as test fxns.) and RT0 (as trial fxns.) normalized basis
// 12*4 matrix

FullMatrix FE_Tetra_CP13_Intrac_RT0NmlzBas(const Tetra &tetra)
{
  double x[4], y[4], z[4];
  
  double vol = tetra.volume();
  double vol4 = (1.0/4) * vol;
  double vol20 = (1.0/20) * vol;
  
  PtVec3d cntr = tetra.center();
  double xc = cntr.xCrd();
  double yc = cntr.yCrd();
  double zc = cntr.zCrd();
  
  PtVec3d P[4];
  for (int i=0; i<4; ++i) {
    P[i] = tetra.vertex(i);
    x[i] = P[i].xCrd();
    y[i] = P[i].yCrd();
    z[i] = P[i].zCrd();
  }

  FullMatrix EltMatIntrac(12,4);

  for (int i=0; i<4; ++i) {
    for (int j=1; j<=3; ++j) {
      EltMatIntrac(3*i+j,j) = vol4;
    }
    EltMatIntrac(3*i+1,4) = vol20 * (x[i]-xc);
    EltMatIntrac(3*i+2,4) = vol20 * (y[i]-yc);
    EltMatIntrac(3*i+3,4) = vol20 * (z[i]-zc);
  }

  return EltMatIntrac;
}

// FE_Tetra_CP13.cpp
