// LinElas3d_WG_HexaQ03Q03RT03Q0_Err.cpp
// James Liu, Graham Harper, ColoState; 2014/07--2017/07

#include <cmath>

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

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


int LinElas3d_WG_HexaQ03Q03RT03Q0_Err(double &L2ErrDspl, double &L2ErrStrs, double &L2ErrDiv,
                                      PtVec3d (*fxnu)(PtVec3d), Mat3 (*fxnsigma)(PtVec3d),
                                      double (*fxndivu)(PtVec3d), const HexaMesh &mesh,
                                      const FullMatrix &NumerDsplCofQ03,
                                      const FullMatrix &NumerStrsCofSym,
                                      const Vector &NumerDivCofQ0,
                                      const GaussQuad &GQH, const GaussQuad &GQQ)
{
  int labelFace[6], sign[6];
  PtVec3d EmCntr, FcCntr, FcCntrNml;
  PtVec3d NumverDspl, uerr, uval;
  Hexa hexa;
  Mat3 NumerStrs;
  
  // Setup
  int Num0E = mesh.numberElements();
  int Num1C = mesh.numberFaces();

  // Computing L2-norm of error in displacement
  L2ErrDspl = 0;
  for (int labele=1; labele<=Num0E; ++labele) {
    int le = labele - 1;
    hexa = mesh.element(labele);
    hexa.enrich();
    // mesh.getElementFace(labele, labelFace);
    double u1= NumerDsplCofQ03(labele,1);
    double u2= NumerDsplCofQ03(labele,2);
    double u3= NumerDsplCofQ03(labele,3);
    PtVec3d NumerDspl = PtVec3d(u1,u2,u3);
    double ErrDsplEm = 0;
    for (int k=0; k<GQH.numberQuadraturePoints(); ++k) {
      double xhat = GQH.CartesianCoordinate(k,0);
      double yhat = GQH.CartesianCoordinate(k,1);
      double zhat = GQH.CartesianCoordinate(k,2);
      double jac = hexa.JacobianDeterminant(xhat, yhat, zhat);
      jac = fabs(jac);  // JL20150422: DON'T USE fabs?
      /*
       PtVec3d qp(0,0,0);
       for (int j=0; j<GQH.numberVertices(); ++j) {
       qp = qp + GQH.baryCoordinate(k,j)*hexa.vertex(j);
       }
       */
      PtVec3d qp = hexa.trilinearmapping(xhat,yhat,zhat);
      PtVec3d uval = fxnu(qp);
      PtVec3d uerr = uval - NumerDspl;
      double tmp = uerr.l2norm();
      ErrDsplEm += (GQH.weight(k)*jac) * (tmp*tmp);
    }
    L2ErrDspl += ErrDsplEm;
  }
  L2ErrDspl = sqrt(L2ErrDspl);

  // Computing L2-norm of error in stress
  L2ErrStrs = 0;
  for (int labele=1; labele<=Num0E; ++labele) {
    int le = labele - 1;
    hexa = mesh.element(labele);
    hexa.enrich();
    EmCntr = hexa.center();
    double ErrStrsEm = 0;
    for (int k=0; k<GQH.numberQuadraturePoints(); ++k) {
      double xhat = GQH.CartesianCoordinate(k,0);
      double yhat = GQH.CartesianCoordinate(k,1);
      double zhat = GQH.CartesianCoordinate(k,2);
      double jac = hexa.JacobianDeterminant(xhat, yhat, zhat);
      jac = fabs(jac);  // JL20150422: DON'T USE fabs?
      PtVec3d qp = hexa.trilinearmapping(xhat,yhat,zhat);
      double X = qp.xCrd() - EmCntr.xCrd();
      double Y = qp.yCrd() - EmCntr.yCrd();
      double Z = qp.zCrd() - EmCntr.zCrd();
      NumerStrs.reset();
      NumerStrs(1,1) = NumerStrsCofSym(labele, 1) + NumerStrsCofSym(labele, 4)*X;
      NumerStrs(2,2) = NumerStrsCofSym(labele, 8) + NumerStrsCofSym(labele,11)*Y;
      NumerStrs(3,3) = NumerStrsCofSym(labele,15) + NumerStrsCofSym(labele,18)*Z;
      NumerStrs(1,2) = NumerStrsCofSym(labele, 2) * 0.5
                     + NumerStrsCofSym(labele, 7) * 0.5
                     + NumerStrsCofSym(labele,10) * 0.5 * X
                     + NumerStrsCofSym(labele, 5) * 0.5 * Y;
      NumerStrs(1,3) = NumerStrsCofSym(labele, 3) * 0.5
                     + NumerStrsCofSym(labele,13) * 0.5
                     + NumerStrsCofSym(labele,16) * 0.5 * X
                     + NumerStrsCofSym(labele, 6) * 0.5 * Z;
      NumerStrs(2,3) = NumerStrsCofSym(labele, 9) * 0.5
                     + NumerStrsCofSym(labele,14) * 0.5
                     + NumerStrsCofSym(labele,17) * 0.5 * Y
                     + NumerStrsCofSym(labele,12) * 0.5 * Z;
      NumerStrs(2,1) = NumerStrs(1,2);
      NumerStrs(3,1) = NumerStrs(1,3);
      NumerStrs(3,2) = NumerStrs(2,3);
      Mat3 strs = fxnsigma(qp);
      Mat3 serr = strs - NumerStrs;
      double tmp2 = colonProduct(serr,serr);
      ErrStrsEm += (GQH.weight(k)*jac) * tmp2;
    }
    L2ErrStrs += ErrStrsEm;
  }
  L2ErrStrs = sqrt(L2ErrStrs);

  // Computing L2-norm of error in divergence
  L2ErrDiv = 0;
  for (int labele=1; labele<=Num0E; ++labele) {
    int le = labele - 1;
    hexa = mesh.element(labele);
    hexa.enrich();
    // EmCntr = hexa.center();
    double NumerDiv = NumerDivCofQ0(labele);
    double ErrDivEm = 0;
    for (int k=0; k<GQH.numberQuadraturePoints(); ++k) {
      double xhat = GQH.CartesianCoordinate(k,0);
      double yhat = GQH.CartesianCoordinate(k,1);
      double zhat = GQH.CartesianCoordinate(k,2);
      double jac = hexa.JacobianDeterminant(xhat, yhat, zhat);
      jac = fabs(jac);  // JL20150422: DON'T USE fabs?
      PtVec3d qp = hexa.trilinearmapping(xhat,yhat,zhat);
      double divu = fxndivu(qp);
      double derr = divu - NumerDiv;
      ErrDivEm += (GQH.weight(k)*jac) * (derr*derr);
    }
    L2ErrDiv += ErrDivEm;
  }
  L2ErrDiv = sqrt(L2ErrDiv);

  return(0);  // If successful
}

// LinElas3d_WG_HexaQ03Q03RT03Q0_Err.cpp
