#include <iostream>
#include <iomanip>
#include <cstring>
#include <cmath>
#include "face.h"
#include "tri_elemP2.h"
#include "tri_d.h"
#include "debug.hpp"
#include <fstream>

extern int TriNumBas;
extern bool ModuleFlag; 

static int NUM_OF_DOFS[4]     = {3, 6, 12, 18};
static int GAUSS_POINT_NUM[4] = {9, 9,  9,  9};  // 0,1,2,3 order number of points
static int DOFS_PER_EDGE[4]   = {1, 2,  3,  3};
static int DOFS_PER_FACE[4]   = {0, 0,  3,  6};
static int DOFS_PER_TETRA[4]  = {0, 0,  0,  3};

const int FACE_BASIS_IJK[18][3] ={
{1, 2, -1}, // 0,  edge
{0, 2, -1}, // 1,  edge
{0, 1, -1}, // 2,  edge  (0th order)
{1, 2, -1}, // 3,  edge
{0, 2, -1}, // 4,  edge
{0, 1, -1}, // 5,  edge  (1st order)
{0, 1,  2}, // 6,  face
{0, 1,  2}, // 7,  face
{1, 2, -1}, // 8,  edge
{0, 2, -1}, // 9,  edge
{0, 1, -1}, // 10, edge
{0, 1,  2}, // 11, face  (2nd order)
{0, 1,  2}, // 12, face
{0, 1,  2}, // 13, face
{0, 1,  2}, // 14, face
{0, 1,  2}, // 15, tetra
{0, 1,  2}, // 16, tetra
{0, 1,  2}  // 17, tetra (3rd order)
};

face::face(){
  nd[0] = 0;
  nd[1] = 0;
  nd[2] = 0;
  eg[0] = 0;
  eg[1] = 0;
  eg[2] = 0;
  hydra[0] = 0;
  hydra[1] = 0;
  cnt = -1;
  bType = 0;
  bcPtr = 0;
  face_mat = 0;
  modeIMP = 0.;
  E_coeff = 0;
  H_coeff = 0;
  for(int j = 0; j < 6; j ++){
    evtr[j].setvtr(0.0, 0.0, 0.0);
    hvtr[j].setvtr(0.0, 0.0, 0.0);
  }
  Frequency = 0.;
  to = 0.;
  tau = 0.;
  TimeDistributionFlag = 0;//Port
  ExcitationFlag = 0;// Scattering
  area_ = 0.;
  for(int j = 0; j < 3; j ++){
    gradZeta_[j].setvtr(0.0, 0.0, 0.0);
  }
  center.setvtr(0.0, 0.0, 0.0);
  FaceNormal_.setvtr(0.0, 0.0, 0.0);
  pec_ = 0;
  pmc_ = 0;
  eDOF_ =-1;
  cntpec_ =-1;
  hDOF_ =-1;
  cntpmc_ =-1;
}

face::~face(){}

void face::setFace(node *nd0, node *nd1, node *nd2){
  node *tmp;
  int  i, j;

  nd[0] = nd0;
  nd[1] = nd1;
  nd[2] = nd2;

  for(i = 0; i < 3; i ++){
    for(j = (i + 1); j < 3; j ++){
      if((*nd[i]) > (*nd[j])){
        tmp = nd[i];
        nd[i] = nd[j];
        nd[j] = tmp;
      }
    }
  }
}

void face::setbcPtr(bc *bcPointer){
  if(bcPtr == 0 || bcPointer->bType != 0)
    bcPtr = bcPointer;
}

void face::setbcPtr(int faceBC_type){
  bcPtr = new bc[1];
  bcPtr->set_bType(faceBC_type);  
}

// Operators
int face::operator > (const face &right) const{
  int i;

  for(i = 0; i < 3; i ++){
    if((*nd[i]) > (*(right.nd[i]))) 
      return 1;
    if((*nd[i]) < (*(right.nd[i]))) 
      return 0;
  }

  return 0;
}

int face::operator < (const face &right) const{
  int i;

  for(i = 0; i < 3; i ++){
    if((*nd[i]) < (*(right.nd[i]))) 
      return 1;
    if((*nd[i]) > (*(right.nd[i]))) 
      return 0;
  }
  return 0;
}

int face::operator == (const face &right) const{
  int i;

  for(i = 0; i < 3; i ++){
    if(nd[i] != (right.nd[i])) 
      return 0;
  }

  return 1;
}

face& face::operator = (const face &right){
  int i;

  for(i = 0; i < 3; i ++){
    nd[i] = right.nd[i];
    eg[i] = right.eg[i];
  }
  hydra[0] = right.hydra[0];
  hydra[1] = right.hydra[1];
  bType = right.bType;
  cnt = right.cnt;

  return *this;
}

void face::tetraArrange(){
  tetra *tet;

  if(hydra[1] == 0) 
    return;

  if(hydra[0]->getobjNum() > hydra[1]->getobjNum()){
    tet = hydra[0];
    hydra[0] = hydra[1];
    hydra[1] = tet;
  }
}

/*
// The H^(1) TVFEM variable assignment for the triangle is defined to be
// edge variables:
//      0, edge{1, 2}
//      1, edge{0, 2}
//      2, edge{0, 1}
// grad variables
//      3, 4 grad(zeta1 x zeta2)
//      4, 4 grad(zeta0 x zeta2)
//      5, 4 grad(zeta0 x zeta1)
// face variables
//      6, 4 zeta1 (zeta0 grad zeta2 - zeta2 grad zeta0)
//      7, 4 zeta2 (zeta0 grad zeta1 - zeta1 grad zeta0)
*/

void face::P2toH1(vtr nodalVTR[], fp_t h1COEFF[], Coordinate coord){
  vtr p0, p1, p2, t12, t02, t01;
  vtr fld;

  // first the three edge vectors
  p0 = coord.Transform(nd[0]->coord);
  p1 = coord.Transform(nd[1]->coord);
  p2 = coord.Transform(nd[2]->coord);
  t12 = p2 - p1;
  t02 = p2 - p0;
  t01 = p1 - p0;

  // the edge variables
  h1COEFF[0] = dotP(nodalVTR[3], t12);
  h1COEFF[1] = dotP(nodalVTR[4], t02);
  h1COEFF[2] = dotP(nodalVTR[5], t01);

  // the gradient variables
  h1COEFF[3] = dotP((nodalVTR[1] - nodalVTR[2]), t12) * 0.125;
  h1COEFF[4] = dotP((nodalVTR[0] - nodalVTR[2]), t02) * 0.125;
  h1COEFF[5] = dotP((nodalVTR[0] - nodalVTR[1]), t01) * 0.125;

  // the facial variables
  h1COEFF[6] = dotP((nodalVTR[3] - nodalVTR[4] + (nodalVTR[0] - nodalVTR[1]) * 0.5), t01);
  h1COEFF[7] = dotP((nodalVTR[3] - nodalVTR[5] + (nodalVTR[0] - nodalVTR[2]) * 0.5), t02);
}

void face::H1toP2(vtr nodalVTR[], fp_t h1COEFF[], Coordinate coord){
  vtr p0, p1, p2, n0, n1, n2;
  vtr cn0, cn1, cn2;
  fp_t area;

  // The three vertexes in 2D coordinate system
  p0 = coord.Transform(nd[0]->coord);
  p1 = coord.Transform(nd[1]->coord);
  p2 = coord.Transform(nd[2]->coord);

  // Three normal vectors
  area = (p1.getx() - p0.getx()) * (p2.gety() - p0.gety()) - (p2.getx() - p0.getx()) * (p1.gety() - p0.gety());
  n0.setvtr(p1.gety() - p2.gety(), p2.getx() - p1.getx(), 0.0);
  n1.setvtr(p2.gety() - p0.gety(), p0.getx() - p2.getx(), 0.0);
  n2.setvtr(p0.gety() - p1.gety(), p1.getx() - p0.getx(), 0.0);
  cn0 = n0 / area;
  cn1 = n1 / area;
  cn2 = n2 / area;

  // the three vertex nodal vectors
  nodalVTR[0] = cn2 * (h1COEFF[1] + h1COEFF[4] * 4.0) 
              + cn1 * (h1COEFF[2] + h1COEFF[5] * 4.0);
  nodalVTR[1] = cn2 * (h1COEFF[0] + h1COEFF[3] * 4.0) 
              + cn0 * (h1COEFF[5] * 4.0 - h1COEFF[2]);
  nodalVTR[2] = cn1 * (h1COEFF[3] * 4.0 - h1COEFF[0]) 
              + cn0 * (h1COEFF[4] * 4.0 - h1COEFF[1]);
  nodalVTR[3] = cn0 * (h1COEFF[4] * 2.0 + h1COEFF[5] * 2.0 - h1COEFF[6] - h1COEFF[7] - h1COEFF[1] * 0.5 - h1COEFF[2] * 0.5) 
              + cn1 * (h1COEFF[3] * 2.0 - h1COEFF[0] * 0.5) 
              + cn2 * (h1COEFF[3] * 2.0 + h1COEFF[0] * 0.5);
  nodalVTR[4] = cn1 * (h1COEFF[5] * 2.0 + h1COEFF[3] * 2.0 + h1COEFF[7] + h1COEFF[2] * 0.5 - h1COEFF[0] * 0.5) 
              + cn2 * (h1COEFF[4] * 2.0 + h1COEFF[1] * 0.5) 
              + cn0 * (h1COEFF[4] * 2.0 - h1COEFF[1] * 0.5);
  nodalVTR[5] = cn2 * (h1COEFF[3] * 2.0 + h1COEFF[4] * 2.0 + h1COEFF[6] + h1COEFF[0] * 0.5 + h1COEFF[1] * 0.5) 
              + cn0 * (h1COEFF[5] * 2.0 - h1COEFF[2] * 0.5) 
              + cn1 * (h1COEFF[5] * 2.0 + h1COEFF[2] * 0.5);
}

void face::geometry(Coordinate coord, vtr nvtr[], fp_t *area){
  vtr p0, p1, p2;
  vtr t0, t1;
  vtr normal;

  p0 = coord.Transform(nd[0]->coord);
  p1 = coord.Transform(nd[1]->coord);
  p2 = coord.Transform(nd[2]->coord);

  t0 = p2 - p1;
  t1 = p0 - p2;
  normal = t0 * t1;
  (*area) = 0.5 * normal.magnitude();
  normal.unitvtr();

  nvtr[0] = normal * t0;
  nvtr[1] = normal * t1;
  if(dotP(nvtr[0], t1) < 0.0) 
    nvtr[0] = nvtr[0] * (-1.0);
  if(dotP(nvtr[1], t0) > 0.0) 
    nvtr[1] = nvtr[1] * (-1.0);
}

void face:: geometry(vtr nvtr[], fp_t *area){
  vtr t0, t1;
  vtr normal;

  t0 = nd[2]->getCoord() - nd[1]->getCoord();
  t1 = nd[0]->getCoord() - nd[2]->getCoord();
  normal = t0 * t1;

  (*area) = 0.5 * normal.magnitude();
  normal.unitvtr();

  nvtr[0] = normal * t0;
  nvtr[1] = normal * t1;

  if(dotP(nvtr[0], t1) < 0.0) 
    nvtr[0] = nvtr[0] * (-1.0);

  if(dotP(nvtr[1], t0) > 0.0) 
    nvtr[1] = nvtr[1] * (-1.0);

  nvtr[2].reset();
  nvtr[2] = nvtr[2] - (nvtr[0] + nvtr[1]);
}

vtr face::Center(){
  vtr center;
  int i;
  for(i = 0; i < NumOfNodesPerFace; i ++)
    center = center + nd[i]->getCoord();
  
  center = center * (1. / 3.);
  return center;
}

bool face::essentiallyEqual(double a, double b, double epsilon){
  return fabs(a - b) <= ( (fabs(a) < fabs(b) ? fabs(b) : fabs(a)) * epsilon);
}

// These functions are used to get the coordinate information for a face
// to be used in the calculation of the non-conformal intersection
char* face::ReturnIntferfacePlaneCoord(double coord2D[6],  double& lastcoord){
  char* plane;

	if (   (essentiallyEqual(nd[0]->getCoord().getx(), nd[1]->getCoord().getx(), Tolerance)) 
      && (essentiallyEqual(nd[0]->getCoord().getx(), nd[2]->getCoord().getx(), Tolerance))
      && (essentiallyEqual(nd[1]->getCoord().getx(), nd[2]->getCoord().getx(), Tolerance))){  

    coord2D[0] = nd[0]->getCoord().gety();  
    coord2D[1] = nd[0]->getCoord().getz();
    coord2D[2] = nd[1]->getCoord().gety();  
    coord2D[3] = nd[1]->getCoord().getz();
    coord2D[4] = nd[2]->getCoord().gety();  
    coord2D[5] = nd[2]->getCoord().getz();
    plane = "yz";
    lastcoord  = nd[0]->getCoord().getx();
  }
  else if (   (essentiallyEqual(nd[0]->getCoord().gety(), nd[1]->getCoord().gety(), Tolerance)) 
           && (essentiallyEqual(nd[0]->getCoord().gety(), nd[2]->getCoord().gety(), Tolerance))
           && (essentiallyEqual(nd[1]->getCoord().gety(), nd[2]->getCoord().gety(), Tolerance))){  

    coord2D[0] = nd[0]->getCoord().getx();  
    coord2D[1] = nd[0]->getCoord().getz();
    coord2D[2] = nd[1]->getCoord().getx();  
    coord2D[3] = nd[1]->getCoord().getz();
    coord2D[4] = nd[2]->getCoord().getx();  
    coord2D[5] = nd[2]->getCoord().getz();
    plane = "xz";
    lastcoord  = nd[1]->getCoord().gety();
  }
  else if (   (essentiallyEqual(nd[0]->getCoord().getz(), nd[1]->getCoord().getz(), Tolerance)) 
           && (essentiallyEqual(nd[0]->getCoord().getz(), nd[2]->getCoord().getz(), Tolerance))
           && (essentiallyEqual(nd[1]->getCoord().getz(), nd[2]->getCoord().getz(), Tolerance))){  
            
    coord2D[0] = nd[0]->getCoord().getx();  
    coord2D[1] = nd[0]->getCoord().gety();
    coord2D[2] = nd[1]->getCoord().getx();  
    coord2D[3] = nd[1]->getCoord().gety();
    coord2D[4] = nd[2]->getCoord().getx();  
    coord2D[5] = nd[2]->getCoord().gety();
    plane = "xy";
    lastcoord  = nd[2]->getCoord().getz();
  }
  return plane;
}

void face::ReturnIntferfacePlaneCoord(double coord2D[6],  double& lastcoord, int& PlaneID){ 
	if(PlaneID == 0) {  
     coord2D[0] = nd[0]->getCoord().gety();  
     coord2D[1] = nd[0]->getCoord().getz();
     coord2D[2] = nd[1]->getCoord().gety();  
     coord2D[3] = nd[1]->getCoord().getz();
     coord2D[4] = nd[2]->getCoord().gety();  
     coord2D[5] = nd[2]->getCoord().getz();
     lastcoord = nd[0]->getCoord().getx();
  } else if(PlaneID == 1) {  
     coord2D[0] = nd[0]->getCoord().getx();  
     coord2D[1] = nd[0]->getCoord().getz();
     coord2D[2] = nd[1]->getCoord().getx();  
     coord2D[3] = nd[1]->getCoord().getz();
     coord2D[4] = nd[2]->getCoord().getx();  
     coord2D[5] = nd[2]->getCoord().getz();
     lastcoord = nd[1]->getCoord().gety();
  } else if(PlaneID == 2) {  
     coord2D[0] = nd[0]->getCoord().getx();  
     coord2D[1] = nd[0]->getCoord().gety();
     coord2D[2] = nd[1]->getCoord().getx();  
     coord2D[3] = nd[1]->getCoord().gety();
     coord2D[4] = nd[2]->getCoord().getx();  
     coord2D[5] = nd[2]->getCoord().gety();
     lastcoord = nd[2]->getCoord().getz();
  }
}

/**
  Element Matrices for the matrix-free implementation
*/
// These functions assemble the faces matrices, to be used 
// for the implementaion of the numerical fluxes or trasmition conditions
// in the DGTDFEM formulation
void face::SetUpMatrixFree(){
  vtr normal;
  geometry(gradZeta_, &area_);
  getAreaNormal(&area_, &normal);
  FaceNormal_.setvtr(normal.getx(), normal.gety(), normal.getz());
}

// Fii, Be, Bh are used in the setup phases of both MF and NMF, used on the fly only in MF



void face::Calculate_Fii_Matrix(tetra* TetPtr, denseMat<fp_t>* Fii_){
  int i, j;
  vtr t0, t1;
  vtr nvtr[3];
  vtr normvtr;
  fp_t area;
  fp_t value;
  fp_t coeffN[4];

  Fii_->reset();
  getAreaNormal(&area, &normvtr);
  geometry(nvtr, &area);

  if(TetPtr != hydra[0]) 
    normvtr.negate();

  // dotP(pi(u), gamma(u)) dS
  // dotP(n x u, n x u x n) dS
  // n x u = nvtr, n x u x n = normvtr * nvtr
  t0 = normvtr * nvtr[0];
  t1 = normvtr * nvtr[1];
  coeffN[0] = dotP(nvtr[0], t0);
  coeffN[1] = dotP(nvtr[0], t1);
  coeffN[2] = dotP(nvtr[1], t0);
  coeffN[3] = dotP(nvtr[1], t1);
	
	for(i = 0; i < Fii_->getRowDim(); i ++){
    for(j = 0; j < Fii_->getColDim(); j ++){
      value = coeffN[0] * (T2D00_P2[i][j]) 
            + coeffN[1] * (T2D01_P2[i][j]) 
            + coeffN[2] * (T2D10_P2[i][j]) 
            + coeffN[3] * (T2D11_P2[i][j]);
      value = 0.5 * value * (1.0 / area);
      Fii_->addEntry(i, j, value);
    }
  }
}

void face::Calculate_Be_Matrix(denseMat<fp_t>* Be_){
  vtr t0, t1;
  vtr nvtr[3];
  fp_t area;
  fp_t value, coeffN[4];
  int i, j;

  Be_->reset();
  geometry(nvtr, &area);

  // dotP(pi(u), pi(u)) dS
  // dotP(n x u, n x u) dS
  // n x u = nvtr, n x u = nvtr
  coeffN[0] = dotP(nvtr[0], nvtr[0]);
  coeffN[1] = dotP(nvtr[0], nvtr[1]);
  coeffN[2] = dotP(nvtr[1], nvtr[0]);
  coeffN[3] = dotP(nvtr[1], nvtr[1]);

  for(i = 0; i < Be_->getRowDim(); i ++){
    for(j = 0; j < Be_->getColDim(); j ++){
      value = coeffN[0] * (T2D00_P2[i][j]) 
            + coeffN[1] * (T2D01_P2[i][j]) 
            + coeffN[2] * (T2D10_P2[i][j]) 
            + coeffN[3] * (T2D11_P2[i][j]);
      value = -1.0 * value * (1.0 / area);
      Be_->addEntry(i, j, value);
    }
  }
  ApplyPEC_Be(Be_);
}

void face::Calculate_Bh_Matrix(denseMat<fp_t>* Bh_){
		vtr t0, t1;
		vtr nvtr[3];
		fp_t area;
		fp_t value, coeffN[4];
		int i, j;
		
		Bh_->reset();
		geometry(nvtr, &area);
		
		coeffN[0] = dotP(nvtr[0], nvtr[0]);
		coeffN[1] = dotP(nvtr[0], nvtr[1]);
		coeffN[2] = dotP(nvtr[1], nvtr[0]);
		coeffN[3] = dotP(nvtr[1], nvtr[1]);
		
		for(i = 0; i < Bh_->getRowDim(); i ++){
			for(j = 0; j < Bh_->getColDim(); j ++){
        value = coeffN[0] * (T2D00_P2[i][j]) 
              + coeffN[1] * (T2D01_P2[i][j]) 
              + coeffN[2] * (T2D10_P2[i][j]) 
              + coeffN[3] * (T2D11_P2[i][j]);
				value = -1.0 * value * (1.0 / area);
				Bh_->addEntry(i, j, value);
			}
		}
		ApplyPMC_Bh(Bh_);
}

void face::Calculate_Fii_Matrix_Numeric(tetra* TetPtr, denseMat<fp_t>* Fii_, int PolyOrderFlag){
  int i, j, k;
  vtr w1, w2;
  vtr nxw2;
  vtr normvtr;
  fp_t area;
  fp_t value;
  fp_t wgt = 0.;
  fp_t* z;

  Fii_->reset();
  getAreaNormal(&area, &normvtr);

  if(TetPtr != hydra[0]) 
    normvtr.negate();

  fp_t (*gaussQuadPoints)[4];
	GetFormula2dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs     = new vtr[NUM_OF_DOFS[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussNormBasisVtrs = new vtr[NUM_OF_DOFS[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < NUM_OF_DOFS[PolyOrderFlag]; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			FaceBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussNormBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = normvtr * w1;
		}
	}

  for (i = 0; i < Fii_->getRowDim(); i++){
    for (j = 0; j < Fii_->getColDim(); j++){
      value = 0.0;
      for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][3];
				w1 = gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussNormBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
        value += wgt * dotP(w1, w2);
      }
      Fii_->addEntry(i, j, 0.5 * area * value);
    }
  }
  delete[] gaussBasisVtrs;
  delete[] gaussNormBasisVtrs;
}

void face::Calculate_Be_Matrix_Numeric(denseMat<fp_t>* Be_, int PolyOrderFlag){
  vtr w1, w2;
  fp_t wgt = 0.;
  fp_t value;
  fp_t* z;
  int i, j, k;

  Be_->reset();

  fp_t (*gaussQuadPoints)[4];
	GetFormula2dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs = new vtr[NUM_OF_DOFS[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < NUM_OF_DOFS[PolyOrderFlag]; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			FaceBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
		}
	}

  for (i = 0; i < Be_->getRowDim(); i++){
    for (j = 0; j < Be_->getColDim(); j++){
      value = 0.0;
      for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][3];
				w1 = gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
        value += wgt * dotP(w1, w2);
      }
      Be_->addEntry(i, j, -area_ * value);
    }
  }
  delete[] gaussBasisVtrs;
  ApplyPEC_Be(Be_);
}

void face::Calculate_Bh_Matrix_Numeric(denseMat<fp_t>* Bh_, int PolyOrderFlag){
  vtr w1, w2;
  fp_t wgt = 0.;
  fp_t value;
  fp_t* z;
  int i, j, k;
  
  Bh_->reset();

  fp_t (*gaussQuadPoints)[4];
	GetFormula2dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs = new vtr[NUM_OF_DOFS[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < NUM_OF_DOFS[PolyOrderFlag]; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			FaceBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
		}
	}
  
  for (i = 0; i < Bh_->getRowDim(); i++){
    for (j = 0; j < Bh_->getColDim(); j++){
      value = 0.0;
      for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][3];
				w1 = gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
        value += wgt * dotP(w1, w2);
      }
      Bh_->addEntry(i, j, -area_ * value);
    }
  }
  delete[] gaussBasisVtrs;
  ApplyPMC_Bh(Bh_);
}

// These functions are used when the  DGTD-GUDA options 
// is excecuted. The get the coefficients to assemble the local element matrices
// SYJ: THIS FUNCTION IS NEVER USED; SHOULD DELETE!
void face::setPECMaps(int* pec_map, int PolyOrder){
  int DOFcnt = 0;
  int ArraySize = DOFS_PER_EDGE[PolyOrder] * 3 + DOFS_PER_FACE[PolyOrder]; // 2*3+0=6
 
	for (int i = 0; i < ArraySize; i++) 
    pec_map[i] = -1;
	
	if(PolyOrder == 0){
			for(int i = 0; i < 3; i++){
			   if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
          	pec_map[i] = DOFcnt;
						DOFcnt++;
         }
			}
	}
	else if(PolyOrder == 1){
		for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
						pec_map[i] = DOFcnt;
						DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
						pec_map[i + 3] = DOFcnt;
						DOFcnt++;
			}
		}
	}
	else if(PolyOrder == 2)	{
   	for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
					pec_map[i] = DOFcnt;
					DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
					pec_map[i + 3] = DOFcnt;
					DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 2; i++){ 
						pec_map[i + 6] = DOFcnt;
						DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
						pec_map[i + 8] = DOFcnt;
						DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 1; i++){ 
						pec_map[i + 11] = DOFcnt;
						DOFcnt++;
			}
		}
 	}
}

int* face::setPECMaps(int PolyOrder){
  int DOFcnt = 0;
  int ArraySize = DOFS_PER_EDGE[PolyOrder] * 3 + DOFS_PER_FACE[PolyOrder] + DOFS_PER_TETRA[PolyOrder];
 
  int* pec_map = new int[ArraySize];
	for(int i = 0; i < ArraySize; i++){
    pec_map[i] = -1;
  }
  
	if(PolyOrder == 0){
    for(int i = 0; i < 3; i++){
      if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i] = DOFcnt;
        DOFcnt++;
      }
    }
	}else if(PolyOrder == 1){
		for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i + 3] = DOFcnt;
        DOFcnt++;
			}
		}
	}else if(PolyOrder == 2)	{
   	for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i + 3] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 2; i++){ 
        pec_map[i + 6] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i + 8] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 1; i++){ 
        pec_map[i + 11] = DOFcnt;
        DOFcnt++;
			}
		}
	}else if(PolyOrder == 3)	{
   	for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i + 3] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 2; i++){ 
        pec_map[i + 6] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()){
        pec_map[i + 8] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 1; i++){ 
        pec_map[i + 11] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 3; i++){ 
        pec_map[i + 12] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pecType){
      for(int i = 0; i < 3; i++){ 
        pec_map[i + 15] = DOFcnt;
        DOFcnt++;
			}
		}
 	}
  return pec_map;
}

// SYJ: THIS FUNCTION IS NEVER USED; SHOULD DELETE!
void face::setPMCMaps(int* pmc_map, int PolyOrder){
  int DOFcnt    = 0;
  int ArraySize = DOFS_PER_EDGE[PolyOrder] * 3 + DOFS_PER_FACE[PolyOrder];
 
	for (int i = 0 ; i < ArraySize ; i++) 
    pmc_map[i] = -1;
	
	if(PolyOrder == 0){
			for(int i = 0; i < 3; i++){
			   if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
          	pmc_map[i] = DOFcnt;
						DOFcnt++;
          }
				}
	}
	else if(PolyOrder == 1){
		for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
						pmc_map[i]   = DOFcnt;
						DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
						pmc_map[i + 3] = DOFcnt;
						DOFcnt++;
			}
		}
	}
	else if(PolyOrder == 2)	{
   	for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
					pmc_map[i] = DOFcnt;
					DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
					pmc_map[i + 3] = DOFcnt;
					DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 2; i++){ 
						pmc_map[i + 6] = DOFcnt;
						DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
						pmc_map[i + 8] = DOFcnt;
						DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 1; i++){ 
						pmc_map[i + 11] = DOFcnt;
						DOFcnt++;
			}
		}
 	}
}

int* face::setPMCMaps(int PolyOrder){
  int DOFcnt = 0;
  int ArraySize = DOFS_PER_EDGE[PolyOrder] * 3 + DOFS_PER_FACE[PolyOrder] + DOFS_PER_TETRA[PolyOrder];
 
  int* pmc_map = new int[ArraySize];
	for(int i = 0; i < ArraySize; i++) 
    pmc_map[i] = -1;
	
	if(PolyOrder == 0){
    for(int i = 0; i < 3; i++){
      if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i] = DOFcnt;
        DOFcnt++;
      }
    }
	}else if(PolyOrder == 1){
		for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i + 3] = DOFcnt;
        DOFcnt++;
			}
		}
	}else if(PolyOrder == 2)	{
   	for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i + 3] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 2; i++){ 
        pmc_map[i + 6] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i + 8] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 1; i++){ 
        pmc_map[i + 11] = DOFcnt;
        DOFcnt++;
			}
		}
	}else if(PolyOrder == 3)	{
   	for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i + 3] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 2; i++){ 
        pmc_map[i + 6] = DOFcnt;
        DOFcnt++;
			}
		}
    for(int i = 0; i < 3; i++){
			if(eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()){
        pmc_map[i + 8] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 1; i++){ 
        pmc_map[i + 11] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 3; i++){ 
        pmc_map[i + 12] = DOFcnt;
        DOFcnt++;
			}
		}
   	if(bType != pmcType){
      for(int i = 0; i < 3; i++){ 
        pmc_map[i + 15] = DOFcnt;
        DOFcnt++;
			}
		}
 	}
  return pmc_map;
}

void face::ApplyPEC_Be(denseMat<fp_t>* Be_){
	int max_pol = 3;
  int p1, p2;
	for(int i = 0; i < max_pol; i ++){
		if(NUM_OF_DOFS[i] == Be_->getRowDim()) 
      p1 = i;  // p1 = 1
		if(NUM_OF_DOFS[i] == Be_->getColDim()) 
      p2 = i; // p2 = 1
	}
	int* row_map; 
	int* col_map;
	row_map = setPECMaps(p1);
	col_map = setPECMaps(p2);

  for(int i = 0; i < NUM_OF_DOFS[p1]; i ++){
    if(row_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p2]; j ++){
        Be_->setEntry(i, j, 0.0);
      }
    }	
  }
  for(int i = 0; i < NUM_OF_DOFS[p2]; i ++){
    if(col_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p1]; j ++ ){
        Be_->setEntry(j, i, 0.0);
      }
    }	
  }
  delete [] row_map;
  delete [] col_map;
}

void face::ApplyPMC_Bh(denseMat<fp_t>* Bh_){
	int max_pol = 3;
  int p1, p2;
	for(int i = 0; i < max_pol; i ++){
		if(NUM_OF_DOFS[i] == Bh_->getRowDim()) 
      p1 = i;
		if(NUM_OF_DOFS[i] == Bh_->getColDim()) 
      p2 = i;
	}
	int* row_map; 
	int* col_map;    
	row_map = setPMCMaps(p1);
	col_map = setPMCMaps(p2);

  for(int i = 0; i < NUM_OF_DOFS[p1]; i ++){
    if(row_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p2]; j ++){
        Bh_->setEntry(i, j, 0.0);
      }
    }	
  }
  for(int i = 0; i < NUM_OF_DOFS[p2]; i ++){
    if(col_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p1]; j ++){
        Bh_->setEntry(j, i, 0.0);
      }
    }	
  }
  delete [] row_map;
  delete [] col_map;
}

void face::ApplyPMC_PEC(denseMat<fp_t>* Bh_){
	int max_pol = 3;
  int p1, p2;
	for(int i = 0; i < max_pol; i ++){
		if(NUM_OF_DOFS[i] == Bh_->getRowDim())  
      p1 = i;
		if(NUM_OF_DOFS[i] == Bh_->getColDim())  
      p2 = i;
	}
	int* row_map; 
	int* col_map;    
	row_map = setPMCMaps(p1);
	col_map = setPECMaps(p2);

  for(int i = 0; i < NUM_OF_DOFS[p1]; i ++ ){
    if(row_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p2]; j ++ ){
        Bh_->setEntry(i, j, 0.0);
      }
    }	
  }
  for(int i = 0; i < NUM_OF_DOFS[p2]; i ++ ){
    if(col_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p1]; j ++ ){
        Bh_->setEntry(j, i, 0.0);
      }
    }	
  }
  delete [] row_map;
  delete [] col_map;
}

void face::ApplyPEC_PMC(denseMat<fp_t>* Be_){
	int max_pol = 3;
  int p1, p2;
	for(int i = 0; i < max_pol; i ++){
		if(NUM_OF_DOFS[i] == Be_->getRowDim())
      p1 = i;
		if(NUM_OF_DOFS[i] == Be_->getColDim()) 
      p2 = i;
	}
	int* row_map; 
	int* col_map;    
	row_map = setPECMaps(p1);
	col_map = setPMCMaps(p2);

  for(int i = 0; i < NUM_OF_DOFS[p1]; i ++ ){
    if(row_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p2]; j ++ ){
        Be_->setEntry(i, j, 0.0);
      }
    }	
  }
  for(int i = 0; i < NUM_OF_DOFS[p2]; i ++ ){
    if(col_map[i] < 0){
      for(int j = 0; j < NUM_OF_DOFS[p1]; j ++ ){
        Be_->setEntry(j, i, 0.0);
      }
    }	
  }
  delete [] row_map;
  delete [] col_map;
}

// computes area normal of the triangular face.
// The direction is chosen to pointing away hydra[0].
void face::getAreaNormal(fp_t *area, vtr *nvtr){
  tetra *tet;
  vtr lvtr0, lvtr1, tv;
  vtr normal;

  lvtr0 = nd[1]->getCoord() - nd[0]->getCoord();
  lvtr1 = nd[2]->getCoord() - nd[0]->getCoord();
  normal = lvtr0 * lvtr1;
  (*area) = 0.5 * normal.magnitude();
  normal.unitvtr();

  tet = hydra[0];
  tv = tet->Center();
  tv = tv - nd[0]->getCoord();

  if(dotP(tv, normal) > 0.0) 
    normal = normal * (-1.0);
  (*nvtr) = normal; //The objective is making the normal vector inward hydra[0]
}

fp_t face::getArea(){
  fp_t area = 0.0;
  vtr lvtr0; 
  vtr lvtr1;
  vtr normal;
 
  lvtr0 = nd[1]->getCoord() - nd[0]->getCoord();
  lvtr1 = nd[2]->getCoord() - nd[0]->getCoord();
  normal = lvtr0 * lvtr1;
  area = 0.5 * normal.magnitude();

  return area; 
}

void face::getFaceArea(fp_t *area){
  vtr lvtr0, lvtr1;
  vtr normal;

  lvtr0 = CoordOfFaceNodes[1] - CoordOfFaceNodes[0];
  lvtr1 = CoordOfFaceNodes[2] - CoordOfFaceNodes[0];
  normal = lvtr0 * lvtr1;
  (*area) = 0.5 * normal.magnitude();
}

int TetraTriNum(tetra *tet, node *nd[3]){
  int i;
  face *nt;

  for(i = 0; i < 4; i ++){
    nt = tet->getFacePtr(i);
    if(nt->getNode(0) == nd[0] && nt->getNode(1) == nd[1] && nt->getNode(2) == nd[2]) 
      return i;
  }

  return -1;
}

// given a tetra - tet, neighborTETRA will return the neighboring tetra with
// respect to the current face.
tetra *face::neighborTETRA(tetra *tet){ /* how the hydra is ? */
  if(tet == hydra[0] && hydra[1] != nullptr) 
    return hydra[1];
  else if(tet == hydra[1] && hydra[0] != nullptr) 
    return hydra[0];
  else{
    //DEBUG_INFO("No Tetra is on the other side of face " + to_string(this->cnt) + " of Tetra " + to_string(tet->getcnt()));
    return nullptr;
  }
}

// Basis functions procedures
void face::FaceBasisW(fp_t *zeta, int p, vtr *W){
  int i, j, k;

  vtr zeroVtr;
  zeroVtr.setvtr(0.,0.,0.);

  vtr* gradZeta = gradZeta_;

  i = FACE_BASIS_IJK[p][0];
  j = FACE_BASIS_IJK[p][1];
  k = FACE_BASIS_IJK[p][2];

  switch(p){
    case 0: case 1: case 2:
      *W = gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j];
      break;
    case 3: case 4: case 5:
      *W = (gradZeta[j] * zeta[i] + gradZeta[i] * zeta[j]) * 4.;
      break;
    case 6:
      // face{i,j,k}, group 2
      *W = (gradZeta[k] * zeta[i] - gradZeta[i] * zeta[k]) * zeta[j] * 4.;
      break;
    case 7:
      // face{i,j,k}, group 2
      *W = (gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j]) * zeta[k] * 4.;
      break;			
    case 8: case 9: case 10:
       // edge{i,j}, group 3
      *W = (gradZeta[j] * zeta[i] * zeta[i] + gradZeta[i] * zeta[i] * zeta[j] * 2. 
          - gradZeta[i] * zeta[j] * zeta[j] - gradZeta[j] * zeta[i] * zeta[j] * 2.) * 4.;
      break;
    case 11:
      // face{i,j,k}, group 4
      *W = (gradZeta[k] * zeta[i] * zeta[j] + gradZeta[j] * zeta[i] * zeta[k] 
          + gradZeta[i] * zeta[j] * zeta[k]) * 8.;
      break;
    case 12:
      *W = (gradZeta[k] * zeta[j] - gradZeta[j] * zeta[k]) * zeta[i] * zeta[i] * 4.;
      break;
    case 13:
      *W = (gradZeta[k] * zeta[i] - gradZeta[i] * zeta[k]) * zeta[j] * zeta[j] * 4.;
      break;
    case 14:
      *W = (gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j]) * zeta[k] * zeta[k] * 4.;
      break;
    case 15: case 16: case 17:
      *W = zeroVtr;
      break;
    default:
      *W = zeroVtr;
      break;
  }
  *W = *W / 2. / area_;
}

void face::FaceBasisNxW(fp_t *zeta, int p, vtr &nxW, vtr &FaceNormal){
  int i;
  fp_t area;
  vtr gradZeta[3]; // this are actually the grad-normals
  vtr W;
  vtr dummy;
  getAreaNormal(&area, &dummy);

  for(i = 0; i < 3; i++) 
    gradZeta[i] = gradZeta[i] / 2. / area;

  gradZeta[2].reset();
  gradZeta[2] = gradZeta[2] - gradZeta[0] - gradZeta[1];

  W.reset();

  switch(p){
    case 0:
      W = gradZeta[2] * zeta[1] - gradZeta[1] * zeta[2];
      break;
    case 1:
      W = gradZeta[2] * zeta[0] - gradZeta[0] * zeta[2];
      break;
    case 2:
      W = gradZeta[1] * zeta[0] - gradZeta[0] * zeta[1];
      break;
    case 3:
      W = (gradZeta[2] * zeta[1] + gradZeta[1] * zeta[2]) * 4.;
      break;
    case 4:
      W = (gradZeta[2] * zeta[0] + gradZeta[0] * zeta[2]) * 4.;
      break;
    case 5:
      W = (gradZeta[1] * zeta[0] + gradZeta[0] * zeta[1]) * 4.;
      break;
    default:
      W.reset();
      break;
  }
  nxW = FaceNormal * W;
}

// These functions, implement the excitation part. The excitation is
// plane wave or port and usually assigned on a surface. Some
// analytical models are supported as well as the general waveguide  solver
// option.
void face::E_updateinc(fp_t t , vtr r , vtr &normalvtr, vtr &nxHinc, vtr &nxnxEinc, fp_t* zeta){
  fp_t IncidExcit_E, IncidExcit_H;

  if(bcPtr->getbType() == planeWaveType){
    // Get k Vector
    fp_t theta = bcPtr->getTheta();
    fp_t phi = bcPtr->getPhi();

    fp_t theta_in_rad = theta * Pi / 180.0;
    fp_t phi_in_rad = phi * Pi / 180.0;

    vtr kvtr(sin(theta_in_rad) * cos(phi_in_rad), sin(theta_in_rad) * sin(phi_in_rad), cos(theta_in_rad));

    vtr Epol = bcPtr->getField();     
    vtr Hpol = kvtr * Epol; //crossProduct
    vtr ro = bcPtr->getPW_ro(); //position where the PW starts 

    PlaneWaveFields(ExcitationFlag, t, r, kvtr, IncidExcit_E, IncidExcit_H);
    nxHinc = (normalvtr * Hpol) * IncidExcit_H;
    nxnxEinc = (normalvtr * (normalvtr * Epol)) * IncidExcit_E;      

  }else if(bcPtr->getbType() >= portType && bcPtr->getbType() < pecType){
    int PortFlag = bcPtr->get_PortFlag();
    fp_t Emagnitude;
    fp_t Hmagnitude;
    vtr Epol;
    vtr Hpol;
    vtr kvtr;
    vtr R_o;

    PortFields(PortFlag, Emagnitude, Hmagnitude,Epol, Hpol, kvtr, R_o, zeta); 
    TimeModulationInc(t, r, R_o, kvtr, Emagnitude, Hmagnitude, IncidExcit_E, IncidExcit_H);

    nxHinc = (normalvtr * Hpol) * IncidExcit_H;
    nxnxEinc = (normalvtr * (normalvtr * Epol)) * IncidExcit_E;      
  }
}

void face::Evaluate_E_field(fp_t *zeta, vtr& E_field){
  vtr basis_i;
  //change Dec 1 2011
  for(int i = 0; i < TriNumBas; i++){  
	  FaceBasisW(zeta, i, &basis_i); 
    E_field = E_field + basis_i * E_coeff[i];
  }
}

void face::H_updateinc(fp_t t , vtr r, vtr &normalvtr, vtr &nxEinc, vtr &nxnxHinc, fp_t* zeta){
  fp_t IncidExcit_E, IncidExcit_H;

  if(bcPtr->getbType() == planeWaveType){
    // Get k Vector
    fp_t theta = bcPtr->getTheta();
    fp_t phi = bcPtr->getPhi();

    fp_t theta_in_rad = theta * Pi / 180.0;
    fp_t phi_in_rad = phi * Pi / 180.0;

    vtr kvtr(sin(theta_in_rad) * cos(phi_in_rad), sin(theta_in_rad) * sin(phi_in_rad), cos(theta_in_rad));

    vtr Epol = bcPtr->getField();     
    vtr Hpol = kvtr * Epol;

    PlaneWaveFields(ExcitationFlag, t, r, kvtr, IncidExcit_E, IncidExcit_H);
    nxEinc = (normalvtr * Epol) * IncidExcit_E;
    nxnxHinc = (normalvtr * (normalvtr * Hpol)) * IncidExcit_H;

  }else if(bcPtr->getbType() >= portType && bcPtr->getbType() < pecType){
    int PortFlag = bcPtr->get_PortFlag();
    fp_t Emagnitude;
    fp_t Hmagnitude;
    vtr  Epol;
    vtr  Hpol;
    vtr  kvtr;
    vtr  R_o;

    PortFields(PortFlag,Emagnitude,Hmagnitude,Epol, Hpol,kvtr,R_o,zeta); 
    TimeModulationInc(t, r, R_o, kvtr, Emagnitude, Hmagnitude, IncidExcit_E, IncidExcit_H);

    nxEinc = (normalvtr * Epol) * IncidExcit_E;
    nxnxHinc = (normalvtr * (normalvtr * Hpol)) * IncidExcit_H;
  }
}

void face::Evaluate_H_field (fp_t *zeta, vtr& H_field){
  vtr basis_i;
//   for(int i = 0; i < TriNumH1Basis; i++){  //change Dec 1 2011
  for(int i = 0; i < TriNumBas; i++){ //change Dec 1 2011
    FaceBasisW(zeta, i, &basis_i);
    H_field = H_field + basis_i * H_coeff[i];
  }
}

// Simplex and global coordinate manipulation functions
void face::getCartesianCoord(fp_t *zeta, vtr &pnt){
  pnt.reset();

  for(int i = 0; i < NumOfNodesPerFace; i ++)
    pnt = pnt + nd[i]->getCoord() * zeta[i];
}

void face::getCartesianCoord_NC(fp_t *zeta, vtr &pnt){
  pnt.reset();

  for(int i = 0; i < NumOfNodesPerFace; i ++){
    pnt = pnt + CoordOfFaceNodes[i] * zeta[i];
  }
}

void face::CartesianToSimplex_NC(vtr &pnt, fp_t *zeta){
  fp_t areaTot;
  vtr lvtr0, lvtr1, tv;
  vtr normal;

  lvtr0 = nd[1]->getCoord() - nd[0]->getCoord();
  lvtr1 = nd[2]->getCoord() - nd[0]->getCoord();
  normal = lvtr0 * lvtr1;
  areaTot = 0.5 * normal.magnitude();

  lvtr0 = nd[1]->getCoord() - pnt;
  lvtr1 = nd[2]->getCoord() - pnt;
  normal = lvtr0 * lvtr1;
  zeta[0] = (0.5 * normal.magnitude()) / areaTot;
  
  lvtr0 = nd[0]->getCoord() - pnt;
  lvtr1 = nd[2]->getCoord() - pnt;
  normal = lvtr0 * lvtr1;
  zeta[1] = (0.5 * normal.magnitude()) / areaTot;

  lvtr0 = nd[1]->getCoord() - pnt;
  lvtr1 = nd[0]->getCoord() - pnt;
  normal = lvtr0 * lvtr1;
  zeta[2] = (0.5 * normal.magnitude()) / areaTot;
}

void face::make_upd_Einc(Coordinate &coord, ArrayFP<fp_t>* nxelem, ArrayFP<fp_t>* nxnxelem, fp_t t, tetra* TetPtr, int& PolyOrd){
  int i, k;
  fp_t area = 0.;
  fp_t wgt = 0.;
  fp_t z[3];
  vtr Normal;
  vtr PortNormal;
  vtr PortDirection;
  vtr w;
  vtr nxH_inc;
  vtr nxnxE_inc;
  vtr r;
  fp_t nxentry;
  fp_t nxnxentry;

  getAreaNormal(&area, &Normal);
  // Normal.setvtr(FaceNormal_.getx(), FaceNormal_.gety(), FaceNormal_.getz());
  if(TetPtr != hydra[0]) 
    Normal.negate();//normal points outward hydra[0]

  nxelem->reset();
  nxnxelem->reset();

  for(i = 0; i < NUM_OF_DOFS[PolyOrd]; i++){
    nxentry = 0.0;
    nxnxentry = 0.0;
    for(k = 0; k < GAUSS_POINT_NUM[PolyOrd]; k++){
      GetFormula(GAUSS_POINT_NUM[PolyOrd], k, &(z[0]), &(z[1]), &(z[2]), &wgt);
      FaceBasisW(z, i, &w);
      getCartesianCoord(z, r);
      E_updateinc(t, r, Normal, nxH_inc, nxnxE_inc, z);
      nxentry += area * wgt * dotP(w, nxH_inc);
      nxnxentry += area * wgt * dotP(w, nxnxE_inc);
    }
    nxelem->addentry(i, nxentry);
    nxnxelem->addentry(i, nxnxentry);
  }

  if(bcPtr->getbType() >= portType && bcPtr->getbType() < pecType){
    PortNormal = Normal * (-1.0);
    PortDirection = bcPtr->get_PortDirection();
    fp_t Check = dotP(PortNormal, PortDirection);
    if(Check < 0.0){
      nxelem->reset();
      nxnxelem->reset();
    }
  }
}

void face::make_upd_Hinc(Coordinate &coord, ArrayFP<fp_t>* nxelem, ArrayFP<fp_t>* nxnxelem, fp_t t, tetra* TetPtr, int& PolyOrd){
  int i, k;
  fp_t area = 0.;
  fp_t wgt = 0.;
  fp_t z[3];
  vtr Normal;
  vtr PortNormal;
  vtr PortDirection;
  vtr w;
  vtr nxE_inc;
  vtr nxnxH_inc;
  vtr r;
  fp_t nxentry;
  fp_t nxnxentry;

  getAreaNormal(&area, &Normal);
  // Normal.setvtr(FaceNormal_.getx(), FaceNormal_.gety(), FaceNormal_.getz());
  if(TetPtr != hydra[0]) 
    Normal.negate(); // normal points out of tetra

  nxelem->reset();
  nxnxelem->reset();

  for(i = 0; i < NUM_OF_DOFS[PolyOrd]; i++){
    nxentry = 0.0;
    nxnxentry = 0.0;
    for(k = 0; k < GAUSS_POINT_NUM[PolyOrd]; k++){
      GetFormula(GAUSS_POINT_NUM[PolyOrd], k, &(z[0]), &(z[1]), &(z[2]), &wgt);
      FaceBasisW(z, i, &w);
      getCartesianCoord(z, r);
      H_updateinc(t, r, Normal, nxE_inc, nxnxH_inc, z);
      nxentry += area * wgt * dotP(w, nxE_inc);
      nxnxentry += area * wgt * dotP(w, nxnxH_inc);
    }
    nxelem->addentry(i, nxentry);
    nxnxelem->addentry(i, nxnxentry);
  }

  if(bcPtr->getbType() >= portType && bcPtr->getbType() < pecType){
    PortNormal = Normal * (-1.0);
    PortDirection = bcPtr->get_PortDirection();
    fp_t Check = dotP(PortNormal, PortDirection);
    if(Check < 0.0){
      nxelem->reset();
      nxnxelem->reset();
    }
  }
}

vtr AreaNormal(vtr pt0, vtr pt1, vtr pt2){
  vtr vv1 = pt1 - pt0;
  vtr vv2 = pt2 - pt0;
  vtr area = vv1 * vv2;
  return area * 0.5;
}

int face::PointInFace(vtr pt, fp_t &zeta0, fp_t &zeta1, fp_t &zeta2){
  fp_t tolErr = 1.e-6;
  vtr A  = AreaNormal(nd[0]->coord, nd[1]->coord, nd[2]->coord);
  vtr A0 = AreaNormal(pt, nd[1]->coord, nd[2]->coord);
  vtr A1 = AreaNormal(pt, nd[2]->coord, nd[0]->coord);
  vtr A2 = AreaNormal(pt, nd[0]->coord, nd[1]->coord);

  fp_t Area  = A.magnitude();
  fp_t Area0 = A0.magnitude();
  fp_t Area1 = A1.magnitude();
  fp_t Area2 = A2.magnitude();

  zeta0 = Area0 / Area;
  if(dotP(A, A0) < 0.0) 
    zeta0 = -zeta0;
  zeta1 = Area1 / Area;
  if(dotP(A, A1) < 0.0)
    zeta1 = -zeta1;
  zeta2 = Area2 / Area;
  if(dotP(A, A2) < 0.0) 
    zeta2 = -zeta2;

  if(fabs(Area0 + Area1 + Area2 - Area) > 1.e-08) 
    return 0;

  if(zeta0 < -tolErr)
    return 0;
  if(zeta0 > (1.0 + tolErr)) 
    return 0;
  if(zeta1 < -tolErr) 
    return 0;
  if(zeta1 > (1.0 + tolErr)) 
    return 0;
  if(zeta2 < -tolErr) 
    return 0;
  if(zeta2 > (1.0 + tolErr))
    return 0;

  return 1;
}

void face::Set_WgSolverExcitation_E(vtr E_vtr[6]){
  Coordinate Coord;
  E_coeff = new fp_t[TriNumH1Basis];
  for(int j = 0; j < SecondOrderTri; j ++){
    evtr[j] = E_vtr[j];
  }
  P2toH1(E_vtr, E_coeff, Coord);
}

void face::Set_WgSolverExcitation_H(vtr H_vtr[6]){
  Coordinate Coord;
  H_coeff = new fp_t[TriNumH1Basis];

  for(int j = 0; j < SecondOrderTri; j ++){
    hvtr[j] = H_vtr[j];
  }
  P2toH1(hvtr, H_coeff, Coord);
}

void face::SparEinc(fp_t t , vtr r, vtr &Einc, fp_t* zeta){

  fp_t Emagnitude,Hmagnitude;
  vtr Epol, Hpol, kvtr, R_o;

  // Get Portflag number
  int PortFlag = bcPtr->get_PortFlag();

  // Get Emagnitude,Hmagnitude,epol,Hpol,kvtr,R_o from port fields
  PortFields(PortFlag, Emagnitude, Hmagnitude, Epol, Hpol, kvtr, R_o, zeta);

  // Get Incident Fields for E and H
  fp_t IncidExcit_E, IncidExcit_H;
  TimeModulationInc(t , r, R_o, kvtr, Emagnitude, Hmagnitude, IncidExcit_E, IncidExcit_H);
  
  // Compute EField Incidence Field
  Einc = Epol * IncidExcit_E;
}

/*
nxEinc = Epol * IncidExcit;
nxnxHinc = (normalvtr * (normalvtr * Hpol)) * IncidExcit;
*/
void face::PlaneWaveFields(int ExcitationFlag, fp_t t, vtr r, vtr kvtr, fp_t &IncidExcit_E,fp_t &IncidExcit_H){
  fp_t Emagnitude = bcPtr->magE;  // V/m
  fp_t omega = 2.0  * Pi * Frequency * MEGA;
  vtr Epol = bcPtr->getField();     
  vtr ro = bcPtr->getPW_ro();  //position where the PW starts 
  fp_t eta = No * sqrt(hydra[0]->mat->mur.getEntry(0,0) / hydra[0]->mat->epsr.getEntry(0,0));
  fp_t V_light = Vo / sqrt(hydra[0]->mat->epsr.getEntry(0,0) * hydra[0]->mat->mur.getEntry(0,0));
  fp_t Exponent;
  fp_t SinModul;
  fp_t Neuman;

  switch(ExcitationFlag){
    case 0:
      Exponent = t - to - dotP(kvtr, r - ro) / Vo;    
      if(Exponent >= 0.0){
        // Plane wave E
        SinModul = cos(omega * Exponent);
        IncidExcit_E = Emagnitude * SinModul;
        IncidExcit_H = (Emagnitude / eta) * SinModul;
      }else{
        IncidExcit_E = 0.0;
        IncidExcit_H = 0.0;
      }
      break;
    case 1:
      // Gauss Pulse
      Exponent = t - to - dotP(kvtr, r - ro) / Vo;
      SinModul = ModuleFlag ? cos(omega * Exponent) : 1.0;
      IncidExcit_E = Emagnitude * SinModul * exp(-(Exponent * Exponent) / (tau * tau));
      IncidExcit_H = (Emagnitude / eta) * SinModul * exp(-(Exponent * Exponent) / (tau * tau));
      break;
    case 2:
      // Neuman Pulse E
      Exponent = t - to - dotP(kvtr, r - ro) / Vo;
      Neuman = (2.0 * Exponent) / (tau * tau);
      IncidExcit_E = (Emagnitude * Neuman) * exp(-(Exponent * Exponent) / (tau * tau));
      IncidExcit_H = (Emagnitude / eta) * Neuman * exp(-(Exponent * Exponent) / (tau * tau));
      break;
    default:
      break;
  }
}

void face::PortFields(int PortFlag,fp_t &Emagnitude,fp_t &Hmagnitude,vtr &Epol, vtr &Hpol,vtr &kvtr,vtr &R_o, fp_t* zeta){
  switch(PortFlag){
    case 1:
      kvtr = bcPtr->get_PortDirection(); // direction of propagation
      kvtr.unitvtr(); // direction of propagation
      Evaluate_E_field(zeta, Epol);
      Emagnitude = bcPtr->magE * Epol.magnitude();
      Epol.unitvtr();
      Evaluate_H_field(zeta, Hpol);
      Hmagnitude = Emagnitude * (1.0 / bcPtr->get_PortImpVal() ); // change Mon Nov 14 2011
      Hpol.unitvtr();

      R_o  = bcPtr->get_Ro();    // port-origin

      break;
    default:
      break;
  }
}

void face::TimeModulationInc(fp_t t , vtr r, vtr R_o, vtr kvtr, fp_t Emagnitude, fp_t Hmagnitude, fp_t &IncidExcit_E, fp_t &IncidExcit_H){
  fp_t Exponent;
  fp_t omega = 2.0  * Pi * Frequency * MEGA;
  fp_t V_light = Vo / sqrt(hydra[0]->mat->epsr.getEntry(0, 0) * hydra[0]->mat->mur.getEntry(0, 0));
  fp_t Neuman;
  fp_t SinModul;

  switch(TimeDistributionFlag){
    // Time Harmonic
    case 0: 
      kvtr.Scale((omega / V_light));
      IncidExcit_E = Emagnitude * sin( dotP(kvtr, r - R_o) - omega * (t));
      IncidExcit_H = Hmagnitude * sin( dotP(kvtr, r - R_o) - omega * (t));
      break;

    // Gauss Pulse
    case 1:
      Exponent = t - to - (dotP(kvtr, r - R_o) / V_light);
      SinModul = ModuleFlag ? cos(omega * (t - to)) : 1.0;
      IncidExcit_E = Emagnitude * SinModul * exp(-(Exponent * Exponent) / (tau * tau));
      IncidExcit_H = Hmagnitude * SinModul * exp(-(Exponent * Exponent) / (tau * tau));
      break;

    // Neuman Pulse
    case 2:
      Exponent = t - to - (dotP(kvtr, r - R_o) / V_light);
      Neuman = (2.0 * Exponent) / (tau * tau);
      IncidExcit_E = (Emagnitude * Neuman) * exp(-(Exponent * Exponent) / (tau * tau));
      IncidExcit_H = (Hmagnitude * Neuman) * exp(-(Exponent * Exponent) / (tau * tau));
      break;

    case 3:      
      fp_t UpperBound, LowerBound, Voltage;
      
      if(t > TimeVal[NumofSamples-1]){ 
        Voltage = VoltageVal[NumofSamples-1]; 
      }else{
        for(int idx = 0; idx < NumofSamples; idx++){
          if(TimeVal[idx] > t){
            UpperBound = VoltageVal[idx];
            LowerBound = VoltageVal[idx-1];
            break;
          }
        }
        Voltage = 0.5 * (UpperBound + LowerBound);
      }
      IncidExcit_E = Emagnitude * Voltage;
      IncidExcit_H = Hmagnitude * Voltage;
      break;

    default:
      break;
  }
}

void face::Calculate_w_w_Matrix_NC(face* InterMeshArray, int ArraySize, face* localFace, face* neighFace, denseMat<fp_t>* w_w, int PolyOrderFlag){
  int i, j, k;
  fp_t area = 0.;
  fp_t wgt = 0.;
  fp_t z[3];
  fp_t z1[3];
  fp_t z2[3];
  vtr Normal;
  vtr w1, w2;
  vtr r;
  fp_t entry;

  w_w->reset();

  for (int idx = 0; idx < ArraySize; idx++){
    InterMeshArray[idx].getFaceArea(&area);
    if (area < 0.0) 
      cout << "Found Negative area at intersection" << endl;
    for (i = 0; i < w_w->getRowDim(); i++) {
      for (j = 0; j < w_w->getColDim(); j++){
        entry = 0.0;
        for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
          GetFormula(GAUSS_POINT_NUM[PolyOrderFlag], k, &(z[0]), &(z[1]), &(z[2]), &wgt);
          InterMeshArray[idx].getCartesianCoord_NC(z, r);
          localFace->CartesianToSimplex_NC(r, z1);
          neighFace->CartesianToSimplex_NC(r, z2);
          localFace->FaceBasisW(z1, i, &w1);
          neighFace->FaceBasisW(z2, j, &w2);
          entry += area * wgt * dotP(w1, w2);
        }
        w_w->addEntry(i, j, -1.0 * entry);
      }
    }
  }
}

void face::Calculate_w_nxw_Matrix_NC(face* InterMeshArray, int ArraySize, face* localFace, face* neighFace, denseMat<fp_t>* w_nxw, tetra* TetPtr, int PolyOrderFlag){
  int i, j, k;
  fp_t area = 0.;
  fp_t wgt = 0.;
  fp_t z[3];
  fp_t z1[3];
  fp_t z2[3];
  vtr Normal;
  vtr w1, w2;
  vtr nxw2;
  vtr r;
  fp_t entry;

  getAreaNormal(&area, &Normal);

  if(TetPtr != hydra[0]) 
    Normal.negate(); //normal points outward hydra[0]

  w_nxw->reset();

  for (int idx = 0; idx < ArraySize; idx++){
    InterMeshArray[idx].getFaceArea(&area);
    if (area < 0.0) 
      cout << "Found Negative area at intersection" << endl;  
    for(i = 0; i < w_nxw->getRowDim(); i++){
      for(j = 0; j < w_nxw->getColDim(); j++){
        entry = 0.0;
        for(k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++){
          GetFormula(GAUSS_POINT_NUM[PolyOrderFlag], k, &(z[0]), &(z[1]), &(z[2]), &wgt);
          InterMeshArray[idx].getCartesianCoord_NC(z, r);
          localFace->CartesianToSimplex_NC(r, z1);
          neighFace->CartesianToSimplex_NC(r, z2);
          localFace->FaceBasisW(z1, i, &w1);
          neighFace->FaceBasisW(z2, j, &w2);
          nxw2 = Normal * w2;
          entry += area * wgt * dotP(w1, nxw2);
        }
        w_nxw->addEntry(i, j, 0.5 * entry);	
      }
    }
  }
}