// LinElas3d_CG_TetraP13_Err.cpp
// Computing various errors of CG P1^3 numer.sln. on tetra.mesh for lin.elas.3d
// James Liu, ColoState; 2014/07--2016/10

#include <cmath>

#include "vector.h"

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


// JL20160901: TO BE REVISED FOR EFFECIENCY

int LinElas3d_CG_TetraP13_Err(double &L2ErrDspl, double &L2ErrStrs,
                              double lambda, double mu,
                              PtVec3d (*fxnu)(PtVec3d),
                              Mat3 (*fxnsigma)(PtVec3d), Vector &sln,
                              const TetraMesh &mesh, const GaussQuad &GQTe)
{
  int labelVertex[4];
  double cof[12];
  PtVec3d BasFxnVal[12], ErrDspl, ExactDspl;
  Mat3 BasFxnStrs[12], ErrStrs, ExactStrs, NumerStrs;

  // L2-norm of the error in displacement
  Vector ErrDsplEm(mesh.numberElements());
  for (int labele=1; labele<=mesh.numberElements(); ++labele) {
    int le = labele - 1;
    mesh.getElementNode(labele, labelVertex);
    Tetra tetra = mesh.element(labele);
    double vol = tetra.volume();
    
    // The 12 coefficients for CG P1^3 basis functions
    for (int i=0; i<4; ++i)
      for (int j=0; j<3; ++j)
        cof[3*i+j] = sln[3*(labelVertex[i]-1)+j];

    // Now integration
    double tmp = 0;
    for (int k=0; k<GQTe.numberQuadraturePoints(); ++k) {
      PtVec3d qp(0,0,0);
      for (int j=0; j<GQTe.numberVertices(); ++j) {
        qp = qp + GQTe.baryCoordinate(k,j) * tetra.vertex(j);
      }
      FE_Tetra_CP13_BasFxnVal(BasFxnVal, tetra, qp);
      PtVec3d NumerDspl(0,0,0);
      for (int i=0; i<12; ++i) {
        NumerDspl = NumerDspl + cof[i] * BasFxnVal[i];
      }
      ExactDspl = fxnu(qp);
      ErrDspl = ExactDspl - NumerDspl;
      tmp += GQTe.weight(k) * dotProduct(ErrDspl,ErrDspl);
    }
    ErrDsplEm[le] = sqrt(tmp*vol);
  }
  L2ErrDspl = ErrDsplEm.l2norm();

  // L2-norm of the error in stress
  Vector ErrStrsEm(mesh.numberElements());
  for (int labele=1; labele<=mesh.numberElements(); ++labele) {
    int le = labele - 1;
    mesh.getElementNode(labele, labelVertex);
    Tetra tetra = mesh.element(labele);
    double vol = tetra.volume();

    // The 12 coefficients for CG P1^3 basis functions
    for (int i=0; i<4; ++i)
      for (int j=0; j<3; ++j)
        cof[3*i+j] = sln[3*(labelVertex[i]-1)+j];
    
    // Computing the elementwise constant numerical stress
    FE_Tetra_CP13_BasFxnStress(BasFxnStrs, tetra, lambda, mu);
    NumerStrs.reset();
    for (int i=0; i<12; ++i)
      NumerStrs = NumerStrs + cof[i] * BasFxnStrs[i];

    // Now integration
    double tmp = 0;
    for (int k=0; k<GQTe.numberQuadraturePoints(); ++k) {
      PtVec3d qp(0,0,0);
      for (int j=0; j<GQTe.numberVertices(); ++j) {
        qp = qp + GQTe.baryCoordinate(k,j) * tetra.vertex(j);
      }
      ExactStrs = fxnsigma(qp);
      ErrStrs = ExactStrs - NumerStrs;
      tmp += GQTe.weight(k) * colonProduct(ErrStrs,ErrStrs);
    }
    ErrStrsEm[le] = sqrt(tmp*vol);
  }
  L2ErrStrs = ErrStrsEm.l2norm();

  return(0);  // If successful
}