// JL20170423: TO BE FINISHED BY James
// James Liu, Graham Harper, ColoState; 2014/07--2017/05

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

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


int LinElas3d_WG_HexaQ03Q03RT03Q0_DsplStrsDiv(FullMatrix &NumerDsplCofQ03,
                                              FullMatrix &NumerStrsCofSym, Vector &NumerDivCofQ0,
                                              const double &lambda, const double &mu,
                                              const HexaMesh &mesh, const Vector &sln,
                                              const GaussQuad &GQH, const GaussQuad &GQQ)
{
  int labelFace[6], sign[6];
  PtVec3d EmCntr, FcCntr, FcCntrNml;
  Hexa hexa;
  Quadri3d quadri, SQF[6];
  Vector cof(21), CDWDB(21);
  FullMatrix CDWGB(21,18);
  
  // Setup
  int Num0E = mesh.numberElements();
  int Num1C = mesh.numberFaces();
  int DOFs = 3*Num0E + 3*Num1C;

  // Numerical displacement: Elementwise coefficients in Q0^3
  NumerDsplCofQ03.resize(Num0E,3);
  for (int labele=1; labele<=Num0E; ++labele) {
    int le = labele - 1;
    NumerDsplCofQ03(labele,1) = sln(3*le+1);
    NumerDsplCofQ03(labele,2) = sln(3*le+2);
    NumerDsplCofQ03(labele,3) = sln(3*le+3);
  }
  
  // Numerical divergence/dilation: Elementwise coefficients in Q0
  NumerDivCofQ0.resize(Num0E);
  for (int labele=1; labele<=Num0E; ++labele) {
    int le = labele - 1;
    hexa = mesh.element(labele);
    hexa.enrich();
    mesh.getElementFace(labele, labelFace);
    // Elementwise coefficients for numerical solution in 21 WG basis functions
    cof(1) = sln(3*le+1);
    cof(2) = sln(3*le+2);
    cof(3) = sln(3*le+3);
    for (int j=1; j<=6; ++j) {
      int k = labelFace[j-1];
      cof(3*j+1) = sln(3*Num0E+3*k-2);
      cof(3*j+2) = sln(3*Num0E+3*k-1);
      cof(3*j+3) = sln(3*Num0E+3*k  );
    }
    // std::cout << "cof=" << cof << "\n";
    EmCntr = hexa.trilinearmapping(0.5, 0.5, 0.5);
    for (int j=0; j<6; ++j) {
      SQF[j] = mesh.face(labelFace[j]);
      sign[j] = 1;
      quadri = SQF[j];
      FcCntr = quadri.bilinearmapping(0.5, 0.5);
      FcCntrNml = quadri.normal(0.5, 0.5);
      if (dotProduct(FcCntrNml,FcCntr-EmCntr)<0)  sign[j] = -1;
    }
    CDWDB = WG3d_HexaQ03Q03RT03Q0_CofNmlzBas_DiscWkDivBasFxn(hexa, SQF, sign, GQH, GQQ);
    NumerDivCofQ0(labele) = dotProd(cof,CDWDB);
  }

  // Numerical stress: Elementwise coefficients in avg(RT[0]^3) (18 terms)
  // Depending on NumerDivCofQ0 and CDWGB
  NumerStrsCofSym.resize(Num0E,18);
  for (int labele=1; labele<=Num0E; ++labele) {
    int le = labele - 1;
    hexa = mesh.element(labele);
    hexa.enrich();
    mesh.getElementFace(labele, labelFace);

    // Element center, faces, and related signs
    EmCntr = hexa.trilinearmapping(0.5, 0.5, 0.5);
    for (int j=0; j<6; ++j) {
      SQF[j] = mesh.face(labelFace[j]);
      sign[j] = 1;
      quadri = SQF[j];
      FcCntr = quadri.bilinearmapping(0.5, 0.5);
      FcCntrNml = quadri.normal(0.5, 0.5);
      if (dotProduct(FcCntrNml,FcCntr-EmCntr)<0)  sign[j] = -1;
    }

    // Elementwise coefficients for numerical solution in 21 WG basis functions
    cof(1) = sln(3*le+1);
    cof(2) = sln(3*le+2);
    cof(3) = sln(3*le+3);
    for (int j=1; j<=6; ++j) {
      int k = labelFace[j-1];
      cof(3*j+1) = sln(3*Num0E+3*k-2);
      cof(3*j+2) = sln(3*Num0E+3*k-1);
      cof(3*j+3) = sln(3*Num0E+3*k  );
    }

    // First NumerStrsCofSym used for RT[0]^3
    CDWGB = WG3d_HexaQ03Q03RT03Q0_CofNmlzBas_DiscWkGradBasFxn(hexa, SQF, sign, GQH, GQQ);
    for (int k=1; k<=18; ++k) {
      NumerStrsCofSym(labele,k) = 0;
      for (int j=1; j<=21; ++j) {
        NumerStrsCofSym(labele,k) = NumerStrsCofSym(labele,k) + cof(j)*CDWGB(j,k);
      }
    }

    // Then NumerStrsCofSym used for stress
    for (int k=1; k<=18; ++k) {
      NumerStrsCofSym(labele,k) = (2*mu)*NumerStrsCofSym(labele,k);
    }
    NumerStrsCofSym(labele,1)  = NumerStrsCofSym(labele,1)  + lambda*NumerDivCofQ0(labele);
    NumerStrsCofSym(labele,8)  = NumerStrsCofSym(labele,8)  + lambda*NumerDivCofQ0(labele);
    NumerStrsCofSym(labele,15) = NumerStrsCofSym(labele,15) + lambda*NumerDivCofQ0(labele);
  }
  
  return(0);  // If successful
}

// LinElas3d_WG_HexaQ03Q03RT03Q0_DsplStrsDiv.cpp
