#include <cmath>
// #include <direct.h> // for windows   getcwd()
#include "array.h"
#include "densemat.h"
#include "material.h"
#include "tensor.h"
#include "tetra.elemDG"
#include "tetra.h"
#include "tetra_MatNew.h"
#include "vtr.h"
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <unistd.h> // for linux   getcwd()

extern "C" {
#include "mkl.h"
}



#include "Octree.h"  // Added by qi jian

using namespace std;
using namespace ClipperLib; //library used to get the geometeries in nonConformal cases

static int TetPolyOrderDim[4]  = {6, 12, 30, 45};
static int FacePolyOrderDim[4] = {3,  6, 12, 18};

static int fac2tet[4][18] = {
    {5, 4, 3, 11, 10, 9, 12, 13, 25, 24, 23, 26, 30, 31, 32, 42, 43, 44},
    {5, 2, 1, 11,  8, 7, 14, 15, 25, 22, 21, 27, 33, 34, 35, 42, 43, 44},
    {4, 2, 0, 10,  8, 6, 16, 17, 24, 22, 20, 28, 36, 37, 38, 42, 43, 44},
    {3, 1, 0,  9,  7, 6, 18, 19, 23, 21, 20, 29, 39, 40, 41, 42, 43, 44}
	// edge group 0, "", "",edge group 1, "", "",face group 2,"", edge group 3, "","",face group 4,face group 5,"","",tetra group 6
};

static int GAUSS_POINT_NUM[4] = {4,20,20,20};   // Number of points for volume order 0,1,2,3
const int TETRA_BASIS_IJK[45][4] ={
	{0, 1, -1, -1}, // edge basis funcs
	{0, 2, -1, -1}, // group 0
	{0, 3, -1, -1},
	{1, 2, -1, -1},
	{1, 3, -1, -1},
	{2, 3, -1, -1}, // W6 (end of 0th order basis functions)

	{0, 1, -1, -1}, // edge basis funcs
	{0, 2, -1, -1}, // group 1
	{0, 3, -1, -1},
	{1, 2, -1, -1},
	{1, 3, -1, -1},
	{2, 3, -1, -1}, // W12 (end of 1st order basis functions)

	{1, 2,  3, -1}, // face basis funcs
	{1, 2,  3, -1}, // group 2
	{0, 2,  3, -1}, 
	{0, 2,  3, -1}, 
	{0, 1,  3, -1}, 
	{0, 1,  3, -1}, 
	{0, 1,  2, -1},
	{0, 1,  2, -1}, // W20

	{0, 1, -1, -1}, // edge basis funcs
	{0, 2, -1, -1}, // group 3
	{0, 3, -1, -1}, 
	{1, 2, -1, -1}, 
	{1, 3, -1, -1}, 
	{2, 3, -1, -1}, // W26

	{1, 2,  3, -1}, // face basis funcs
	{0, 2,  3, -1}, // group 4
	{0, 1,  3, -1}, 
	{0, 1,  2, -1}, // W30 (end of 2nd order basis functions)

	{1, 2,  3, -1}, // face basis funcs
	{1, 2,  3, -1}, // group 5
	{1, 2,  3, -1}, 
	{0, 2,  3, -1}, 
	{0, 2,  3, -1}, 
	{0, 2,  3, -1}, 
	{0, 1,  3, -1}, 
	{0, 1,  3, -1}, 
	{0, 1,  3, -1}, 
	{0, 1,  2, -1}, 
	{0, 1,  2, -1}, 
	{0, 1,  2, -1}, // W42

	{0, 3,  2,  1}, // tetra basis funcs
	{0, 2,  3,  1}, // group 6
	{0, 1,  3,  2}  // W45 (end of 3rd order basis functions)
};

static int DOFS_PER_EDGE[4]   = {1, 2, 3, 3};
static int DOFS_PER_FACE_1[4] = {0, 0, 2, 2};
static int DOFS_PER_FACE_2[4] = {0, 0, 1, 1};
static int DOFS_PER_FACE_3[4] = {0, 0, 0, 3};
static int DOFS_PER_TET[4]    = {0, 0, 0, 3};

tetra::tetra(int ob) {
    int i;
    objNum = ob;
    for (i = 0; i < 4; i++)
			nd[i] = nullptr;
    for (i = 0; i < 6; i++)
			eg[i] = nullptr;
    for (i = 0; i < 4; i++)
			fc[i] = nullptr;

    cnt = -1;
    mat = nullptr;
    nbc[0] = 0;
    nbc[1] = 0;
    nbc[2] = 0;
    nbc[3] = 0;
    LocalEDOF = 0;
    LocalHDOF = 0;
    edgeEDOF = 0;
    edgeHDOF = 0;
    faceEDOF = 0;
    faceHDOF = 0;
    TetrahedronFlag = -1;
    PolyOrderFlag = -1;
    LocalOffsetE = -1;
    LocalOffsetH = -1;
    ExcitationFlag = false;
    LTS_Flag = -1;
    isNonConformal = false;
		regularGroup = 0;
		nDipoles = 0;

    Conduct_Flag = -1;
    MassE = nullptr;
    MassH = nullptr;
    StiffE = nullptr;
    StiffH = nullptr;
    BiiE = nullptr;
    BiiH = nullptr;
    FiiE = nullptr;
    FiiH = nullptr;
    FijE = nullptr;
    FijH = nullptr;
    BijE = nullptr;
    BijH = nullptr;
    LHSInvE = nullptr;
    LHSInvH = nullptr;

    RHSLoc1E = nullptr;
    RHSLoc1H = nullptr;
    RHSLoc2E = nullptr;
    RHSLoc2H = nullptr;
    RHSCoup1E = nullptr;
    RHSCoup1H = nullptr;
    RHSCoup2E = nullptr;
    RHSCoup2H = nullptr;

    mapPEC = nullptr;
    mapPMC = nullptr;
    Class_dt = 0.0;
    Stability_dt = 0.0;
    NeighNum = 0; //-1 because it counts itself
    NeighNCNum = 0; //-1 because it counts itself

    vol_ = 0.0;
	for(int j = 0; j < 4; j ++){
		gradZeta_[j].setvtr(0.0, 0.0, 0.0);
	}

    LocMapE = nullptr;
    LocMapH = nullptr;
    cntpec_ = 0;
    cntpmc_ = 0;

    EdofMap = nullptr;
    HdofMap = nullptr;
}

tetra::~tetra() {
    freeDofMap();
    FreeNonExcitatonTetra();
}

void tetra::FreeNonExcitatonTetra() {
    if (MassE != nullptr)
        delete MassE;
    MassE = nullptr;
    if (LHSInvE != nullptr)
        delete LHSInvE;
    LHSInvE = nullptr;
    if (MassH != nullptr)
        delete MassH;
    MassH = nullptr;
    if (LHSInvH != nullptr)
        delete LHSInvH;
    LHSInvH = nullptr;

    if (StiffE != nullptr)
        delete StiffE;
    StiffE = nullptr;
    if (StiffH != nullptr)
        delete StiffH;
    StiffH = nullptr;
    if (BiiE != nullptr)
        delete BiiE;
    BiiE = nullptr;
    if (BiiH != nullptr)
        delete BiiH;
    BiiH = nullptr;
    if (FiiE != nullptr)
        delete FiiE;
    FiiE = nullptr;
    if (FiiH != nullptr)
        delete FiiH;
    FiiH = nullptr;
    if (FijE != nullptr)
        delete FijE;
    FijE = nullptr;
    if (FijH != nullptr)
        delete FijH;
    FijH = nullptr;
    if (BijE != nullptr)
        delete BijE;
    BijE = nullptr;
    if (BijH != nullptr)
        delete BijH;
    BijH = nullptr;

    if (RHSLoc1E != nullptr)
        delete RHSLoc1E;
    RHSLoc1E = nullptr;
    if (RHSLoc1H != nullptr)
        delete RHSLoc1H;
    RHSLoc1H = nullptr;
    if (RHSLoc2E != nullptr)
        delete RHSLoc2E;
    RHSLoc2E = nullptr;
    if (RHSLoc2H != nullptr)
        delete RHSLoc2H;
    RHSLoc2H = nullptr;
    if (RHSCoup1E != nullptr)
        delete RHSCoup1E;
    RHSCoup1E = nullptr;
    if (RHSCoup1H != nullptr)
        delete RHSCoup1H;
    RHSCoup1H = nullptr;
    if (RHSCoup2E != nullptr)
        delete RHSCoup2E;
    RHSCoup2E = nullptr;
    if (RHSCoup2H != nullptr)
        delete RHSCoup2H;
    RHSCoup2H = nullptr;
}

void tetra::set_node(node* nd0, node* nd1, node* nd2, node* nd3) {
    nd[0] = nd0;
    nd[1] = nd1;
    nd[2] = nd2;
    nd[3] = nd3;
}

void tetra::reArrange() {
    int i, j;
    int tmpCNT;
    node* tmp;
    // make sure local and global numberings are consistent
    for (i = 0; i < 4; i++) {
        for (j = (i + 1); j < 4; j++) {
            if ((*nd[i]) > (*nd[j])) {
                tmp = nd[i];
                nd[i] = nd[j];
                nd[j] = tmp;
                tmpCNT = nbc[i];
                nbc[i] = nbc[j];
                nbc[j] = tmpCNT;
            }
        }
    }
}

void tetra::set_objNum(int ob) { objNum = ob; }

void tetra::set_nbc(int bcd0, int bcd1, int bcd2, int bcd3) {
    nbc[0] = bcd0;
    nbc[1] = bcd1;
    nbc[2] = bcd2;
    nbc[3] = bcd3;
}

void tetra::setEdge(edge* egPtr, int n) { eg[n] = egPtr; }

void tetra::setFace(face* fcPtr, int n) { fc[n] = fcPtr; }

edge* tetra::getEdgePtr(int n) { return eg[n]; }

face* tetra::getFacePtr(int n) { return fc[n]; }

void tetra::TetraBasisW(fp_t *zeta, int p, vtr *W){
	int i, j, k, l;

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

	vtr* gradZeta = gradZeta_;

	i = TETRA_BASIS_IJK[p][0];
	j = TETRA_BASIS_IJK[p][1];
	k = TETRA_BASIS_IJK[p][2];
	l = TETRA_BASIS_IJK[p][3];

	switch(p){
		case 0: case 1: case 2: case 3: case 4: case 5: // 6 edges
			*W = gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j];
			break;
		case 6: case 7: case 8: case 9: case 10: case 11: // 6 edges
			*W = (gradZeta[j] * zeta[i] + gradZeta[i] * zeta[j]) * 4.;
			break;		
		case 12: case 14: case 16: case 18: // 4 faces
			// face{i,j,k}, group 2
			*W = (gradZeta[k] * zeta[i] - gradZeta[i] * zeta[k]) * zeta[j] * 4.;
			break;
		case 13: case 15: case 17: case 19:
			// face{i,j,k}, group 2
			*W = (gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j]) * zeta[k] * 4.;
			break;			
		case 20: case 21: case 22: case 23: case 24: case 25:
			// 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 26: case 27: case 28: case 29:
			// 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 30: case 33: case 36: case 39:
			*W = (gradZeta[k] * zeta[j] - gradZeta[j] * zeta[k]) * zeta[i] * zeta[i] * 4.;
			break;
		case 31: case 34: case 37: case 40:
			*W = (gradZeta[k] * zeta[i] - gradZeta[i] * zeta[k]) * zeta[j] * zeta[j] * 4.;
			break;
		case 32: case 35: case 38: case 41:
			*W = (gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j]) * zeta[k] * zeta[k] * 4.;
			break;
		case 42: case 43: case 44:
			*W = (gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j]) * zeta[k] * zeta[l] * 8.;
			break;
		default:
			*W = zeroVtr;
			break;
  	}
	*W = *W / 3. / vol_;
}

void tetra::TetraBasisCurlW(fp_t *zeta, int p, vtr *W){
	int i, j, k, l;

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

	vtr* gradZeta = gradZeta_;

	i = TETRA_BASIS_IJK[p][0];
	j = TETRA_BASIS_IJK[p][1];
	k = TETRA_BASIS_IJK[p][2];
	l = TETRA_BASIS_IJK[p][3];

	switch(p){
		case 0: case 1: case 2: case 3: case 4: case 5: // 6 edges
			*W = gradZeta[i] * gradZeta[j] - gradZeta[j] * gradZeta[i];
			break;
		case 6: case 7: case 8: case 9: case 10: case 11: // 6 edges
			*W = zeroVtr;
			break;		
		case 12: case 14: case 16: case 18: // 4 faces
			// face{i,j,k}, group 2
			*W = ( gradZeta[j] * (gradZeta[k] * zeta[i] - gradZeta[i] * zeta[k])
				+ (gradZeta[i] * gradZeta[k] - gradZeta[k] * gradZeta[i]) * zeta[j] )* 4.;
			break;
		case 13: case 15: case 17: case 19:
			// face{i,j,k}, group 2
			*W = (gradZeta[k] * (gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j]) + 
				  (gradZeta[i] * gradZeta[j] - gradZeta[j] * gradZeta[i]) * zeta[k] )* 4.;
			break;
		case 20: case 21: case 22: case 23: case 24: case 25:
			// edge{i,j}, group 3
			*W = zeroVtr;
			break;	
		case 26: case 27: case 28: case 29:
			// face{i,j,k}, group 4
			*W = zeroVtr;
			break;
		case 30: case 33: case 36: case 39:
			*W = ( (gradZeta[j] * gradZeta[k] - gradZeta[k] * gradZeta[j])*zeta[i]*zeta[i] 
				 + (gradZeta[i] * zeta[i] * 2.)*(gradZeta[k] * zeta[j] - gradZeta[j] * zeta[k]) ) * 4.;
			break;
		case 31: case 34: case 37: case 40:
			*W = ( (gradZeta[i] * gradZeta[k] - gradZeta[k] * gradZeta[i])*zeta[j]*zeta[j] 
				 + (gradZeta[j] * zeta[j] * 2.)*(gradZeta[k] * zeta[i] - gradZeta[i] * zeta[k]) ) * 4.;
			break;
		case 32: case 35: case 38: case 41:
			*W = ( (gradZeta[i] * gradZeta[j] - gradZeta[j] * gradZeta[i])*zeta[k]*zeta[k] 
				 + (gradZeta[k] * zeta[k] * 2.)*(gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j]) ) * 4.;
			break;
		case 42: case 43: case 44:
			*W = ( (gradZeta[i] * gradZeta[j] - gradZeta[j] * gradZeta[i]) * zeta[k] * zeta[l] 
				 + (gradZeta[l]*zeta[k] + gradZeta[k]*zeta[l])*(gradZeta[j] * zeta[i] - gradZeta[i] * zeta[j])) * 8.;
			break;
		default:
			*W = zeroVtr;
			break;
  	}
	*W = *W / 3. / 3. / vol_ / vol_;
}

void tetra::CountDOF_E() {
	int i, j;
	int cntpec = 0;
	int tetEDOF = 0;
	edgeEDOF = 0;
	faceEDOF = 0;

	for (i = 0; i < NumOfFaces; i++) {
		if (fc[i]->getbType() == pecType)
			for (j = 0; j < NumOfEdgesPerFace; j++)
				eg[fac2tet[i][j]]->setbType(pecType);
		else
			faceEDOF++;
	}

	// as long as one face is non-pec, then add tetrahedral basis functions for third order case
	if (faceEDOF > 0)
		tetEDOF = 1;

	for (i = 0; i < NumOfEdges; i++) {
		if (eg[i]->getbType() == pecType || eg[i]->IsPecPmc())
			cntpec++;
		else
			edgeEDOF++;
		;
	}

	LocalEDOF = DOFS_PER_EDGE[PolyOrderFlag] * edgeEDOF +
							DOFS_PER_FACE_1[PolyOrderFlag] * faceEDOF +
							DOFS_PER_FACE_2[PolyOrderFlag] * faceEDOF + 
							DOFS_PER_FACE_3[PolyOrderFlag] * faceEDOF + 
							DOFS_PER_TET[PolyOrderFlag] * tetEDOF;

}

void tetra::CountDOF_H() {
	int i, j;
	int cntpmc = 0;
	int tetHDOF = 0;
	edgeHDOF = 0;
	faceHDOF = 0;

	for (i = 0; i < NumOfFaces; i++) {
		if (fc[i]->getbType() == pmcType) {
			for (j = 0; j < NumOfEdgesPerFace; j++)
				eg[fac2tet[i][j]]->setbType(pmcType);
		} else
			faceHDOF++;
	}

	// as long as one face is non-pmc, then add tetrahedral basis functions for third order case
	if (faceHDOF > 0)
		tetHDOF = 1;

	for (i = 0; i < NumOfEdges; i++) {
		if (eg[i]->getbType() == pmcType || eg[i]->IsPecPmc())
			cntpmc++;
		else
			edgeHDOF++;
	}

	LocalHDOF = DOFS_PER_EDGE[PolyOrderFlag] * edgeHDOF +
							DOFS_PER_FACE_1[PolyOrderFlag] * faceHDOF +
							DOFS_PER_FACE_2[PolyOrderFlag] * faceHDOF +
							DOFS_PER_FACE_3[PolyOrderFlag] * faceHDOF +
							DOFS_PER_TET[PolyOrderFlag] * tetHDOF;
}

void tetra::Local_DG_mapE(int* mapID, int offset) {
    // dofcount=12+8+6+4=30 -> P2 complete
    int i, j;
    // edges and faces !PEC or !PMC
    if (PolyOrderFlag > 3) {
			cout << "Be careful with DOFS_PER_EDGE length" << endl;
			exit(-1);
    }
    int counter = offset;
    int index = 0;
    for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
			mapID[i] = NOT_NUMBERED;
    }
    // 0 and 1st order edge functions (6 or 12)
    for (int k = 0; k < DOFS_PER_EDGE[PolyOrderFlag] && k < 2; k++) {
			for (int i = 0; i < NumOfEdges; i++) {
				if (eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()) {
					mapID[index] = counter;
					counter++;
				}
				index++;
			}
    }
    // Functions for the first 8 face functions (2 per face) for the 2 order
    // basis
    if (DOFS_PER_FACE_1[PolyOrderFlag] > 0) {
			for (i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() != pecType) {
					for (j = 0; j < DOFS_PER_FACE_1[PolyOrderFlag]; j++) {
						mapID[index] = counter;
						counter++;
						index++;
					}
				} else {
					index += DOFS_PER_FACE_1[PolyOrderFlag];
				}
			}
    }
    // Second order edge functions (8)
    if (DOFS_PER_EDGE[PolyOrderFlag] > 2) {
			for (int i = 0; i < NumOfEdges; i++) {
				if (eg[i]->getbType() != pecType && !eg[i]->IsPecPmc()) {
					mapID[index] = counter;
					counter++;
				}
				index++;
			}
    }
    // Functions for the next 4 face functions (1 per face) for the 2 order
    // basis
    if (DOFS_PER_FACE_2[PolyOrderFlag] > 0) {
			for (i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() != pecType) {
					for (j = 0; j < DOFS_PER_FACE_2[PolyOrderFlag]; j++) {
						mapID[index] = counter;
						counter++;
						index++;
					}
				} else {
					index += DOFS_PER_FACE_2[PolyOrderFlag];
				}
			}
    }
    // dofcount=12+8+6+4=30 -> P2 complete

	// Functions for the next 12 face functions (3 per face) for the 3rd order basis
	if (DOFS_PER_FACE_3[PolyOrderFlag] > 0){
		for (i = 0; i < NumOfFaces; i++){
			if (fc[i]->getbType() != pecType){
				for (j = 0; j < DOFS_PER_FACE_3[PolyOrderFlag]; j++){
					mapID[index] = counter;
					counter++;
					index++;
				}
			} else {
				index += DOFS_PER_FACE_3[PolyOrderFlag];
			}
		}
	}

	// Functions for the next 3 tetra functions (3 per tetra) for the 3rd order basis
	int pecFaceCounter = 0;
	if (DOFS_PER_TET[PolyOrderFlag] > 0){
		for (i = 0; i < NumOfFaces; i++){
			if (fc[i]->getbType() == pecType)
				pecFaceCounter += 1;
		}
		if (pecFaceCounter != 4) {
			for (j = 0; j < DOFS_PER_TET[PolyOrderFlag]; j++){
				mapID[index] = counter;
				counter++;
				index++;
			}
		} else {
			index += DOFS_PER_TET[PolyOrderFlag];
		}
	}
	// dofcount = 12+8+6+4+12+3 = 45 -> P3 complete
}

void tetra::Local_DG_mapH(int* mapID, int offset) {
	// dofcount = 0
	int i, j;

	// edges and faces !PEC or !PMC
	if (PolyOrderFlag > 3) {
		cout << "Be careful with DOFS_PER_EDGE length" << endl;
		exit(-1);
	}
	int counter = offset;
	int index = 0;
	for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
		mapID[i] = NOT_NUMBERED;
	}
	// 0 and 1st order edge functions (6 or 12)
	for (int k = 0; k < DOFS_PER_EDGE[PolyOrderFlag] && k < 2; k++) {
		for (int i = 0; i < NumOfEdges; i++) {
			if (eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()) {
				mapID[index] = counter;
				counter++;
			}
			index++;
		}
	}
	// Functions for the first 8 face functions (2 per face) for the 2 order
	// basis
	if (DOFS_PER_FACE_1[PolyOrderFlag] > 0) {
		for (i = 0; i < NumOfFaces; i++) {
			if (fc[i]->getbType() != pmcType) {
				for (j = 0; j < DOFS_PER_FACE_1[PolyOrderFlag]; j++) {
					mapID[index] = counter;
					counter++;
					index++;
				}
			} else {
				index += DOFS_PER_FACE_1[PolyOrderFlag];
			}
		}
	}
	// Second order edge functions (8)
	if (DOFS_PER_EDGE[PolyOrderFlag] > 2) {
		for (int i = 0; i < NumOfEdges; i++) {
			if (eg[i]->getbType() != pmcType && !eg[i]->IsPecPmc()) {
				mapID[index] = counter;
				counter++;
			}
			index++;
		}
	}
	// Functions for the next 4 face functions (1 per face) for the 2 order
	// basis
	if (DOFS_PER_FACE_2[PolyOrderFlag] > 0) {
		for (i = 0; i < NumOfFaces; i++) {
			if (fc[i]->getbType() != pmcType) {
				for (j = 0; j < DOFS_PER_FACE_2[PolyOrderFlag]; j++) {
					mapID[index] = counter;
					counter++;
					index++;
				}
			} else {
				index += DOFS_PER_FACE_2[PolyOrderFlag];
			}
		}
	}
	// dofcount=12+8+6+4=30 -> P2 complete

	// Functions for the next 12 face functions (3 per face) for the 3rd order basis
	if (DOFS_PER_FACE_3[PolyOrderFlag] > 0){
		for (i = 0; i < NumOfFaces; i++){
			if (fc[i]->getbType() != pmcType){
				for (j = 0; j < DOFS_PER_FACE_3[PolyOrderFlag]; j++){
					mapID[index] = counter;
					counter++;
					index++;
				}
			} else {
				index += DOFS_PER_FACE_3[PolyOrderFlag];
			}
		}
	}

	// Functions for the next 3 tetra functions (3 per tetra) for the 3rd order basis
	int pmcFaceCounter = 0;
	if (DOFS_PER_TET[PolyOrderFlag] > 0){
		for (i = 0; i < NumOfFaces; i++){
			if (fc[i]->getbType() == pmcType)
				pmcFaceCounter += 1;
		}
		if (pmcFaceCounter != 4) {
			for (j = 0; j < DOFS_PER_TET[PolyOrderFlag]; j++){
				mapID[index] = counter;
				counter++;
				index++;
			}
		} else {
			index += DOFS_PER_TET[PolyOrderFlag];
		}
	}
	// dofcount = 12+8+6+4+12+3 = 45 -> P3 complete
}

void tetra::set_TetrahedronFlag() {
    int i;
    int DG_FaceFlag;
    int InterFaceCounter = 0;
    int ABCFaceCounter = 0;

    // loop through 4 faces to see if this tetra is on any BC
    for (i = 0; i < NumOfFaces; i++) {
        DG_FaceFlag = fc[i]->bcPtr->getbType();

        if (DG_FaceFlag == 0 || DG_FaceFlag == outputSurfType) {
            // || DG_FaceFlag == fieldPlaneType
            InterFaceCounter++;
        } else if (DG_FaceFlag == abcType) {
            ABCFaceCounter++;
        } else if (DG_FaceFlag == pmlType) {   // Add PML type 
            ExcitationFlag = true;
		} else if (DG_FaceFlag == planeWaveType) {
            ABCFaceCounter++;
            ExcitationFlag = true;
        } else if (DG_FaceFlag >= portType && DG_FaceFlag < pecType) {
            ABCFaceCounter++;
            ExcitationFlag = true;
        }
    }

    // TODO: review InterFaceCounter <= 4
    if (InterFaceCounter <= 4 && ABCFaceCounter == 0)
			TetrahedronFlag = 0; // not ABC tetra etc
    else if (ABCFaceCounter >= 1)
			TetrahedronFlag = 1; // is ABC tetra
    else
			TetrahedronFlag = 0; // not ABC tetra etc


    if (TetrahedronFlag == -1) {
			cout << "ERROR: TetrahedronFlag not set (" << TetrahedronFlag << ")" << endl;
			return;
    }
}

void tetra::set_PolyOrderFlagDebug(int n) {
  PolyOrderFlag = n;
  if (PolyOrderFlag == NOT_NUMBERED) {
    cout << "ERROR: PolyOrderFlag not set (" << PolyOrderFlag << ")" << endl;
    return;
  }
}





// Modified by qi jian
void tetra::set_NeighborTetra(tetra* tetARRAY, int* ncARRAY, int nonConformalCNT, Octree* octree_ptr, double min_AABB_size)
{
    int i, j;
    tetra* neigh;
    std::vector<int> myNeighset;         // Stores cnt() indices of neighboring tetrahedra
    std::vector<int> myNeighsetFaceMap;  // Maps each face[i] to neighbor[i]

	// ------------------------------------------------------------------------------------------
	// Procedure for tetrahedral with all conformal faces
    if (!isNonConformal)
    {
        // Standard conformal neighbor assignment
        for (i = 0; i < NumOfFaces; i++)
        {
            neigh = fc[i]->neighborTETRA(this);  // Lookup neighbor through shared face
            if (neigh != NULL)
            {
                myNeighset.push_back(neigh->getcnt());
                myNeighsetFaceMap.push_back(i);
                NeighNum++;
            }
        }
    }
	// ------------------------------------------------------------------------------------------
	// Procedure for tetrehedral with nonconformal faces
    else
    {
        // Nonconformal case: use octree to find spatially overlapping tets
        std::vector<int> NC_NeighSet;
        face* neighface;

        for (i = 0; i < NumOfFaces; i++)
        {
            if (fc[i]->getbType() == nonConformal)
            {

								
				// Step 1: Extract node coordinates for the face
				std::array<std::array<double, 3>, 3> probe_xyz = fc[i]->getNodeCoordinates();

				// Step 2: Define triangle's AABB
				AABB face_box;
				face_box.xmin = std::min({probe_xyz[0][0], probe_xyz[1][0], probe_xyz[2][0]});
				face_box.xmax = std::max({probe_xyz[0][0], probe_xyz[1][0], probe_xyz[2][0]});
				face_box.ymin = std::min({probe_xyz[0][1], probe_xyz[1][1], probe_xyz[2][1]});
				face_box.ymax = std::max({probe_xyz[0][1], probe_xyz[1][1], probe_xyz[2][1]});
				face_box.zmin = std::min({probe_xyz[0][2], probe_xyz[1][2], probe_xyz[2][2]});
				face_box.zmax = std::max({probe_xyz[0][2], probe_xyz[1][2], probe_xyz[2][2]});


				auto enforce_min_extent = [&](double& min_val, double& max_val) {
					double extent = max_val - min_val;
					if (extent < min_AABB_size) {
						double center = 0.5 * (min_val + max_val);
						min_val = center - 0.5 * min_AABB_size;
						max_val = center + 0.5 * min_AABB_size;
					}
				};

				enforce_min_extent(face_box.xmin, face_box.xmax);
				enforce_min_extent(face_box.ymin, face_box.ymax);
				enforce_min_extent(face_box.zmin, face_box.zmax);


				// Step 3: Use Octree to find candidate tets
				std::vector<int> found_tets;
				bool success = octree_ptr->findNCTetraInOctree(face_box, found_tets);				

                if (!success)
                {
                    std::cout << "No candidate tetrahedron found for nonconformal face " << i << " in tetra " << cnt << ".\n";
                }
				
                // Step 4: Refine candidates by checking face-level geometric overlap
                for (int tet_id : found_tets)
                {
                    neigh = &(tetARRAY[tet_id]);  // Candidate neighbor


                    if (neigh->getcnt() != cnt)  // Skip self
                    {
                        for (j = 0; j < NumOfFaces; j++)
                        {
                            neighface = neigh->fc[j];

                            if (neighface->getbType() == nonConformal)
                            {
                                if (triangle_overlaps_triangle(fc[i], neighface) &&
                                    CheckIntersection(fc[i], neighface))
                                {
                                    // Record neighbor indices and mapping
                                    NC_NeighSet.push_back(neigh->getcnt());
                                    myNeighset.push_back(neigh->getcnt());
                                    myNeighsetFaceMap.push_back(i);
                                    NeighNum++;
                                    NeighNCNum++;
                                }
                            }
                        }
                    }
                }
				

				/*
				for(int idx = 0; idx < nonConformalCNT; idx++)
				{
					neigh = &(tetARRAY[ncARRAY[idx]]);
							  if(neigh->getcnt() != cnt)
							  {
								  for(j = 0; j < NumOfFaces; j++)
								  {
									  neighface = neigh->fc[j];
									  if(neighface->getbType() == nonConformal)
									  {
										  if (triangle_overlaps_triangle(fc[i], neighface))
										  {
											  if(CheckIntersection(fc[i], neighface))
											  {
												  NC_NeighSet.push_back(neigh->getcnt());
												  myNeighset.push_back(neigh->getcnt());
												  myNeighsetFaceMap.push_back(i); 
												  NeighNum++; 
												  NeighNCNum++;
											  }
										  }
									  }
								  }
							  }
				  }		
				*/


            }
            else
            {
                // Fallback for conformal faces in mixed mesh
                neigh = fc[i]->neighborTETRA(this);
                if (neigh != NULL)
                {
                    myNeighset.push_back(neigh->getcnt());
                    myNeighsetFaceMap.push_back(i);
                    NeighNum++;
                }
            }
        }

        // Step 4: Store nonconformal neighbor tetrahedra
        NeighNCTetra = new tetra*[NeighNCNum];
        for (i = 0; i < NeighNCNum; i++)
        {
            NeighNCTetra[i] = &(tetARRAY[NC_NeighSet[i]]);
        }
    }

    // Step 5: Store all neighbors (conformal or mixed)
    NeighTetra = new tetra*[NeighNum];
    NeighTetraFaceMap = new int[NeighNum];
    for (i = 0; i < NeighNum; i++)
    {
        NeighTetra[i] = &(tetARRAY[myNeighset[i]]);
        NeighTetraFaceMap[i] = myNeighsetFaceMap[i];
    }
}




bool tetra::triangle_overlaps_triangle(face* LocalFace, face* NeighFace){
  bool SamePlaneX = false;
  bool SamePlaneY = false;
  bool SamePlaneZ = false;
  bool SamePlane = false;
  bool Overlap = false;
  double tri1Coords[6];
  double tri2Coords[6];
  int PlaneID  = 0;
	double thridCoord = 0.0;
  vtr triangle1[3];
  vtr triangle2[3]; 
	
  for (int i = 0; i < 3; i++){
		triangle1[i].setvtr(LocalFace->nd[i]->getCoord().getx(),
												LocalFace->nd[i]->getCoord().gety(),
												LocalFace->nd[i]->getCoord().getz());  
	
    triangle2[i].setvtr(NeighFace->nd[i]->getCoord().getx(),
												NeighFace->nd[i]->getCoord().gety(),
												NeighFace->nd[i]->getCoord().getz()); 
	}

	// Check which Plane is the source
  if(   (essentiallyEqual(triangle1[0].getx(), triangle1[1].getx(), Tolerance)) 
     && (essentiallyEqual(triangle1[0].getx(), triangle1[2].getx(), Tolerance))
     && (essentiallyEqual(triangle1[1].getx(), triangle1[2].getx(), Tolerance))){
		SamePlaneX = true;
	}

  if( (essentiallyEqual(triangle1[0].gety(), triangle1[1].gety(), Tolerance)) 
   && (essentiallyEqual(triangle1[0].gety(), triangle1[2].gety(), Tolerance)) 
   && (essentiallyEqual(triangle1[1].gety(), triangle1[2].gety(), Tolerance))){
		SamePlaneY = true;
	}

  if(   (essentiallyEqual(triangle1[0].getz(), triangle1[1].getz(), Tolerance)) 
     && (essentiallyEqual(triangle1[0].getz(), triangle1[2].getz(), Tolerance)) 
     && (essentiallyEqual(triangle1[1].getz(), triangle1[2].getz(), Tolerance))){
		SamePlaneZ = true;
	}

	// Check if source and target are Coplanar
  if(SamePlaneX){ 
		if( (essentiallyEqual(triangle1[0].getx(), triangle2[0].getx(), Tolerance))  
     && (essentiallyEqual(triangle1[1].getx(), triangle2[1].getx(), Tolerance)) 
     && (essentiallyEqual(triangle1[2].getx(), triangle2[2].getx(), Tolerance))){
			SamePlane = true; 
			PlaneID = 0;
		}
  }else if(SamePlaneY){
		if( (essentiallyEqual(triangle1[0].gety(), triangle2[0].gety(), Tolerance)) 
		 && (essentiallyEqual(triangle1[1].gety(), triangle2[1].gety(), Tolerance)) 
     && (essentiallyEqual(triangle1[2].gety(), triangle2[2].gety(), Tolerance))){
			SamePlane = true; 
			PlaneID = 1;
		}
  }else if(SamePlaneZ){
		if( (essentiallyEqual(triangle1[0].getz(), triangle2[0].getz(), Tolerance)) 
  	 && (essentiallyEqual(triangle1[1].getz(), triangle2[1].getz(), Tolerance)) 
     && (essentiallyEqual(triangle1[2].getz(), triangle2[2].getz(), Tolerance))){
			SamePlane = true; 
			PlaneID = 2;
		}
  } 
  
  if(SamePlane){
    LocalFace->ReturnIntferfacePlaneCoord(tri1Coords, thridCoord, PlaneID);
    NeighFace->ReturnIntferfacePlaneCoord(tri2Coords, thridCoord, PlaneID);
      
    ///// new version
    Paths subj(1), clip(1), solution;

    subj[0]<< IntPoint(cInt(tri1Coords[0] * scaleFact),cInt(tri1Coords[1] * scaleFact))
           << IntPoint(cInt(tri1Coords[2] * scaleFact),cInt(tri1Coords[3] * scaleFact))
	   			 << IntPoint(cInt(tri1Coords[4] * scaleFact),cInt(tri1Coords[5] * scaleFact));
	  
    clip[0]<< IntPoint(cInt(tri2Coords[0] * scaleFact),cInt(tri2Coords[1] * scaleFact))
           << IntPoint(cInt(tri2Coords[2] * scaleFact),cInt(tri2Coords[3] * scaleFact))
	   			 << IntPoint(cInt(tri2Coords[4] * scaleFact),cInt(tri2Coords[5] * scaleFact));
	   
    Clipper c;
    c.AddPaths(subj, ptSubject, true);
    c.AddPaths(clip, ptClip, true);  
    if(c.Execute(ctIntersection, solution, pftNonZero, pftNonZero)){
      if(solution.size() > 0){
        if(solution[0].size() >= 3) 
					Overlap = true;
        else
					Overlap = false;
      }else 
				Overlap = false;
    }else
			Overlap = false;
	}
   
  return (Overlap || triangle_overlaps_triangleCO(LocalFace, NeighFace));
}

bool tetra::triangle_overlaps_triangleCO(face* LocalFace, face* NeighFace){
  vtr triangle1[3];
  vtr triangle2[3]; 
	
  for(int i = 0; i < 3; i++){
    triangle1[i].setvtr(LocalFace->nd[i]->getCoord().getx(),
			 									LocalFace->nd[i]->getCoord().gety(),
			 									LocalFace->nd[i]->getCoord().getz());  
		
    triangle2[i].setvtr(NeighFace->nd[i]->getCoord().getx(),
			 									NeighFace->nd[i]->getCoord().gety(),
			 									NeighFace->nd[i]->getCoord().getz()); 
  }

  vector <vtr> PointsDiff;
  vtr tmpvtr;

  // conformal but not planar triangles 
  bool Check1 = false;
  bool Check2 = false;
  bool Check3 = false;
  for (int i = 0 ; i < 3 ; i++){     
     for (int j = 0; j < 3 ; j++){
       tmpvtr = triangle1[i] - triangle2[j];
       PointsDiff.push_back(tmpvtr);
     }
  }
  Check1 = ((PointsDiff[0].magnitude() < Tolerance) ||
            (PointsDiff[1].magnitude() < Tolerance) ||
            (PointsDiff[2].magnitude() < Tolerance));

  Check2 = ((PointsDiff[3].magnitude() < Tolerance) ||
            (PointsDiff[4].magnitude() < Tolerance) ||
            (PointsDiff[5].magnitude() < Tolerance));

  Check3 = ((PointsDiff[6].magnitude() < Tolerance) ||
            (PointsDiff[7].magnitude() < Tolerance) ||
            (PointsDiff[8].magnitude() < Tolerance));

  bool OverlapCon = (Check1 && Check2 && Check3);
  return OverlapCon;
}

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

void tetra::set_ConductivityFlag() {
  if (mat->sigma.getEntry(0, 0) == 0.0)
    Conduct_Flag = 0;
  else
    Conduct_Flag = 1;

  if (Conduct_Flag == NOT_NUMBERED) {
    cout << "ERROR: Conduct_Flag not set (" << Conduct_Flag << ")" << endl;
    return;
  }
}

void tetra::SetFacePEC() {
  int i, j;
  for (i = 0; i < NumOfFaces; i++) {
    if (fc[i]->getbType() == pecType) {
      for (j = 0; j < NumOfEdgesPerFace; j++)
        eg[fac2tet[i][j]]->setbType(pecType);
    }
  }
}

void tetra::SetFacePMC() {
  int i, j;
  for (i = 0; i < NumOfFaces; i++) {
    if (fc[i]->getbType() == pmcType) {
      for (j = 0; j < NumOfEdgesPerFace; j++)
        eg[fac2tet[i][j]]->setbType(pmcType);
    }
  }
}

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

    for (i = 0; i < NumOfNodes; i++) {
        nd[i] = right.nd[i];
        fc[i] = right.fc[i];
        nbc[i] = right.nbc[i];
    }
    for (i = 0; i < NumOfEdges; i++)
        eg[i] = right.eg[i];

    objNum = right.objNum;

    return *this;
}

int tetra::operator==(const tetra& right) const {
    for (int i = 0; i < NumOfNodes; i++) {
        if (nd[i] != (right.nd[i]))
            return NO;
    }
    return YES;
}

void tetra::geometry(vtr* lvtr, vtr* avtr, fp_t* vol) {
	int i, i1, i2, i3;
	vtr vv, tt;
	// Just needed 3 vectors because the fourth one is the addition of the others
	for (i = 0; i < 3; i++) {
			lvtr[i] = nd[i + 1]->coord - nd[0]->coord;
	}
	for (i = 0; i < 3; i++) {
		i1 = (i + 1) % NumOfNodes;
		i2 = (i + 2) % NumOfNodes;
		i3 = (i + 3) % NumOfNodes;

		tt = nd[i2]->coord - nd[i1]->coord;
		vv = nd[i3]->coord - nd[i1]->coord;
		avtr[i] = tt * vv;
		avtr[i] = avtr[i] * 0.5;

		tt = nd[i]->coord - nd[i1]->coord;
		if (dotP(avtr[i], tt) < 0.0)
			avtr[i] = avtr[i] * (-1.0);
	}
	avtr[3].reset();

	avtr[3] = avtr[3] - (avtr[0] + avtr[1] + avtr[2]);

	(*vol) = dotP(avtr[2], lvtr[1]) / 3.0;
}

void tetra::makeCoeff(vtr avtr[], tensor epsr, fp_t coeffA[][3]) {
	int i, j;

	for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++)
			coeffA[i][j] = dotP((epsr * avtr[j]), avtr[i]);
	}
}

void tetra::makeCoeff_curl(vtr avtr[], fp_t coeffCurl[][3], fp_t& scale) {
	coeffCurl[0][0] = scale * dotP(avtr[0], (avtr[1] * avtr[2]));
	coeffCurl[0][1] = scale * dotP(avtr[0], (avtr[2] * avtr[0]));
	coeffCurl[0][2] = scale * dotP(avtr[0], (avtr[0] * avtr[1]));
	coeffCurl[1][0] = scale * dotP(avtr[1], (avtr[1] * avtr[2]));
	coeffCurl[1][1] = scale * dotP(avtr[1], (avtr[2] * avtr[0]));
	coeffCurl[1][2] = scale * dotP(avtr[1], (avtr[0] * avtr[1]));
	coeffCurl[2][0] = scale * dotP(avtr[2], (avtr[1] * avtr[2]));
	coeffCurl[2][1] = scale * dotP(avtr[2], (avtr[2] * avtr[0]));
	coeffCurl[2][2] = scale * dotP(avtr[2], (avtr[0] * avtr[1]));
}

void tetra::set_einc(fp_t t, ArrayFP<fp_t>* nxh_inc, ArrayFP<fp_t>* nxnxe_inc) {
	int i, j;
	Coordinate coord;
	int LocalFaceDim;
	ArrayFP<fp_t>* nxh;
	ArrayFP<fp_t>* nxnxe;

	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	nxh = new ArrayFP<fp_t>(LocalFaceDim);
	nxnxe = new ArrayFP<fp_t>(LocalFaceDim);

	for (i = 0; i < 4; i++) {
		if (fc[i]->getbType() == planeWaveType || (fc[i]->getbType() >= portType && fc[i]->getbType() < pecType)) {
			fc[i]->make_upd_Einc(coord, nxh, nxnxe, t, this, PolyOrderFlag);

			for (j = 0; j < LocalFaceDim; j++) {
				nxh_inc->addentry(fac2tet[i][j], nxh->getentry(j));
				nxnxe_inc->addentry(fac2tet[i][j], nxnxe->getentry(j));
			}
		}
	}
	delete nxh;
	delete nxnxe;
}

void tetra::set_hinc(fp_t t, ArrayFP<fp_t>* nxe_inc, ArrayFP<fp_t>* nxnxh_inc) {
	int i, j;
	Coordinate coord;
	int LocalFaceDim;
	ArrayFP<fp_t>* nxe;
	ArrayFP<fp_t>* nxnxh;

	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	nxe = new ArrayFP<fp_t>(LocalFaceDim);
	nxnxh = new ArrayFP<fp_t>(LocalFaceDim);

	for (i = 0; i < 4; i++) {
		if (fc[i]->getbType() == planeWaveType || (fc[i]->getbType() >= portType && fc[i]->getbType() < pecType)) {
			fc[i]->make_upd_Hinc(coord, nxe, nxnxh, t, this, PolyOrderFlag);
			for (j = 0; j < LocalFaceDim; j++) {
				nxe_inc->addentry(fac2tet[i][j], nxe->getentry(j));
				nxnxh_inc->addentry(fac2tet[i][j], nxnxh->getentry(j));
			}
		}
	}
	delete nxe;
	delete nxnxh;
}

/**
   Apply PEC PMC boundary conditions
*/

void tetra::MapPEC() { // TODO: make it dynamic
	if (PolyOrderFlag > 3) {
		cout << "Be careful with MapPEC" << endl;
		exit(-1);
	}
	int index = 0;
	int face_pec_cnt = 0;
	int tet_pec_cnt = 0;
	int cntpec_aux = 0;
	int counter = 0;

	for (int i = 0; i < NumOfEdges; i++)
		if (eg[i]->getbType() == pecType || eg[i]->IsPecPmc())
			cntpec_aux++;

	if (DOFS_PER_FACE_1[PolyOrderFlag] > 0 || DOFS_PER_FACE_2[PolyOrderFlag] > 0)
		for (int j = 0; j < 4; j++)
			if (fc[j]->getbType() == pecType)
				face_pec_cnt++;

	if (face_pec_cnt == 4)
		tet_pec_cnt = 1;

	cntpec_ += DOFS_PER_EDGE[PolyOrderFlag] * cntpec_aux +
						(DOFS_PER_FACE_1[PolyOrderFlag] + DOFS_PER_FACE_2[PolyOrderFlag] + DOFS_PER_FACE_3[PolyOrderFlag]) * face_pec_cnt + 
						 DOFS_PER_TET[PolyOrderFlag] * tet_pec_cnt;

	if (cntpec_ != 0) {
		mapPEC = new int[cntpec_];
		for (int i = 0; i < NumOfEdges; i++) {
			if (eg[i]->getbType() == pecType && eg[i]->IsPecPmc()) {
				mapPEC[index] = i;
				if (DOFS_PER_EDGE[PolyOrderFlag] > 1)
					mapPEC[index + cntpec_aux] = i + NumOfEdges;
				index++;
			}
		}

		if (DOFS_PER_EDGE[PolyOrderFlag] > 1) {
				index += cntpec_aux;
				counter = 2 * NumOfEdges; // If this statement is achieved it mean that we
												// have checked all the values of the 12 previous
												// basis functions (edges order 0 & edges order 1)
		}

		if (DOFS_PER_FACE_1[PolyOrderFlag] > 0) {
			for (int i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() == pecType) {
					mapPEC[index++] = counter;
					mapPEC[index++] = counter + 1;
				}
				counter += 2;
			}
		}

		if (DOFS_PER_EDGE[PolyOrderFlag] > 2)
			for (int i = 0; i < NumOfEdges; i++) {
				if (eg[i]->getbType() == pecType && eg[i]->IsPecPmc())
					mapPEC[index++] = counter;
				counter++;
			}

		if (DOFS_PER_FACE_2[PolyOrderFlag] > 0)
			for (int i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() == pecType)
					mapPEC[index++] = counter;
				counter++;
			}

		if (DOFS_PER_FACE_3[PolyOrderFlag] > 0)
			for (int i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() == pecType){
					mapPEC[index++] = counter;
					mapPEC[index++] = counter+1;
					mapPEC[index++] = counter+2;
				}
				counter += 3;
			}

		if (DOFS_PER_TET[PolyOrderFlag] > 0){
			if (tet_pec_cnt == 1){
				mapPEC[index++] = counter;
				mapPEC[index++] = counter+1;
				mapPEC[index++] = counter+2;
			}
			counter += 3;
		}
	}
}

void tetra::MapPMC() {
	if (PolyOrderFlag > 3) {
		cout << "Be careful with MapPMC" << endl;
		exit(-1);
	}
	int index = 0;
	int face_pmc_cnt = 0;
	int tet_pmc_cnt = 0;
	int cntpmc_aux = 0;
	int counter = 0;

	for (int i = 0; i < NumOfEdges; i++)
		if (eg[i]->getbType() == pmcType || eg[i]->IsPecPmc())
			cntpmc_aux++;

	if (DOFS_PER_FACE_1[PolyOrderFlag] > 0 || DOFS_PER_FACE_2[PolyOrderFlag] > 0)
		for (int j = 0; j < 4; j++)
			if (fc[j]->getbType() == pmcType)
				face_pmc_cnt++;

	cntpmc_ += DOFS_PER_EDGE[PolyOrderFlag] * cntpmc_aux +
						 (DOFS_PER_FACE_1[PolyOrderFlag] + DOFS_PER_FACE_2[PolyOrderFlag] + DOFS_PER_FACE_3[PolyOrderFlag]) * face_pmc_cnt +
						  DOFS_PER_TET[PolyOrderFlag] * tet_pmc_cnt;

	if (cntpmc_ != 0) {
		mapPMC = new int[cntpmc_];
		for (int i = 0; i < NumOfEdges; i++) {
			if (eg[i]->getbType() == pmcType && eg[i]->IsPecPmc()) {
				mapPMC[index] = i;
				if (DOFS_PER_EDGE[PolyOrderFlag] > 1)
					mapPMC[index + cntpmc_aux] = i + NumOfEdges;
				index++;
			}
		}

		if (DOFS_PER_EDGE[PolyOrderFlag] > 1) {
			index += cntpmc_aux;
			counter = 2 * NumOfEdges; // If this statement is achieved it mean that we
																// have checked all the values of the 12 previous
																// basis functions (edges order 0 & edges order 1)
		}

		if (DOFS_PER_FACE_1[PolyOrderFlag] > 0) {
			for (int i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() == pmcType) {
					mapPMC[index++] = counter;
					mapPMC[index++] = counter + 1;
				}
				counter += 2;
			}
		}

		if (DOFS_PER_EDGE[PolyOrderFlag] > 2)
			for (int i = 0; i < NumOfEdges; i++) {
				if (eg[i]->getbType() == pmcType && eg[i]->IsPecPmc())
					mapPMC[index++] = counter;
				counter++;
			}

		if (DOFS_PER_FACE_2[PolyOrderFlag] > 0)
			for (int i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() == pmcType)
					mapPMC[index++] = counter;
				counter++;
			}

		if (DOFS_PER_FACE_3[PolyOrderFlag] > 0)
			for (int i = 0; i < NumOfFaces; i++) {
				if (fc[i]->getbType() == pmcType){
					mapPMC[index++] = counter;
					mapPMC[index++] = counter + 1;
					mapPMC[index++] = counter + 2;
				}
				counter += 3;
			}

		if (DOFS_PER_TET[PolyOrderFlag] > 0){
			if (tet_pmc_cnt == 1){
				mapPMC[index++] = counter;
				mapPMC[index++] = counter + 1;
				mapPMC[index++] = counter + 2;
			}
			counter += 3;
		}
	}
}

void tetra::ApplyPEC_Vector(ArrayFP<fp_t>* b, int LocalDim) {
	if (cntpec_ != 0) {
		for (int i = 0; i < LocalDim; i++)
			if (LocMapE[i] == -1)
					b->setentry(i, 0.0);
	}
}

void tetra::ApplyPEC(denseMat<fp_t>* A, int LocalDim) {
	if (cntpec_ != 0) {
		for (int i = 0; i < LocalDim; i++) {
			if (LocMapE[i] == -1) {
				for (int j = 0; j < LocalDim; j++) {
					A->setEntry(i, j, 0.0);
					A->setEntry(j, i, 0.0);
				}
			}
		}
	}
}

void tetra::ApplyPEC_E(denseMat<fp_t>* A, int LocalDim) {
	if (cntpec_ != 0) {
		for (int i = 0; i < LocalDim; i++) {
			if (LocMapE[i] == -1) {
				for (int j = 0; j < LocalDim; j++)
					A->setEntry(i, j, 0.0);
			}
		}
	}
}

void tetra::ApplyPEC_H(denseMat<fp_t>* A, int LocalDim) {
	if (cntpec_ != 0) {
		for (int i = 0; i < LocalDim; i++) {
			if (LocMapE[i] == -1) {
				for (int j = 0; j < LocalDim; j++)
					A->setEntry(j, i, 0.0);
			}
		}
	}
}

void tetra::ApplyPEC_Neigh_H(denseMat<fp_t>* A, tetra* neigh, int NeighDim) {
	if (neigh->cntpec_ != 0) {
		for (int i = 0; i < neigh->cntpec_; i++) {
			for (int j = 0; j < NeighDim; j++) {
				A->setEntry(j, neigh->mapPEC[i], 0.0);
			}
		}
	}
}

void tetra::ApplyPMC_Vector(ArrayFP<fp_t>* b, int LocalDim) {
    if (cntpmc_ != 0) {
        for (int i = 0; i < LocalDim; i++)
            if (LocMapH[i] == -1)
                b->setentry(i, 0.0);
    }
}

void tetra::ApplyPMC_E(denseMat<fp_t>* A, int LocalDim) {
    if (cntpmc_ != 0) {
        for (int i = 0; i < LocalDim; i++) {
            if (LocMapH[i] == -1) {
                for (int j = 0; j < LocalDim; j++)
                    A->setEntry(j, i, 0.0);
            }
        }
    }
}

void tetra::ApplyPMC_Neigh_E(denseMat<fp_t>* A, tetra* neigh, int NeighDim) {
    if (neigh->cntpmc_ != 0) {
        for (int i = 0; i < neigh->cntpmc_; i++) {
            for (int j = 0; j < NeighDim; j++) {
                A->setEntry(j, neigh->mapPMC[i], 0.0);
            }
        }
    }
}

void tetra::ApplyPMC(denseMat<fp_t>* A, int LocalDim) {
    if (cntpmc_ != 0) {
        for (int i = 0; i < LocalDim; i++) {
            if (LocMapH[i] == -1) {
                for (int j = 0; j < LocalDim; j++) {
                    A->setEntry(i, j, 0.0);
                    A->setEntry(j, i, 0.0);
                }
            }
        }
    }
}

void tetra::ApplyPMC_H(denseMat<fp_t>* A, int LocalDim) {
    if (cntpmc_ != 0) {
        for (int i = 0; i < LocalDim; i++) {
            if (LocMapH[i] == -1) {
                for (int j = 0; j < LocalDim; j++)
                    A->setEntry(i, j, 0.0);
            }
        }
    }
}

void tetra::Calculate_S_Matrix(
    denseMat<fp_t>* Curl_Sc) { // Se or Sh in the formulation
    int i, j, dim;
    fp_t vol;
    fp_t coeffC[3][3];
    fp_t cval;
    vtr lvtr[4];
    vtr avtr[4];
    fp_t scale = 1.0;
    Curl_Sc->reset();
    dim = Curl_Sc->getRowDim();
    geometry(lvtr, avtr, &vol);
    makeCoeff_curl(avtr, coeffC, scale);

    // dotP(u, \nabla x u) dV
    // dotP(u, u x u) dV
    // u = avtr
    for (i = 0; i < dim; i++) {
        for (j = 0; j < dim; j++) {
            cval =
                (coeffC[0][0] * C00_P2[i][j]) + (coeffC[0][1] * C01_P2[i][j]) +
                (coeffC[1][0] * C10_P2[i][j]) + (coeffC[0][2] * C02_P2[i][j]) +
                (coeffC[2][0] * C20_P2[i][j]) + (coeffC[1][1] * C11_P2[i][j]) +
                (coeffC[1][2] * C12_P2[i][j]) + (coeffC[2][1] * C21_P2[i][j]) +
                (coeffC[2][2] * C22_P2[i][j]);
            cval = (2.0 * cval) / (9.0 * vol * vol * (fp_t)(C_DNUM));
            Curl_Sc->addEntry(i, j, cval);
        }
    }
}

void tetra::Calculate_S_Matrix_Numeric(denseMat<fp_t>* Curl_Sc) { // Se or Sh in the formulation
    int i, j, k, dim;
    fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
    fp_t cval;
    vtr lvtr[4];
    vtr avtr[4];
    Curl_Sc->reset();
    dim = Curl_Sc->getRowDim();
	vtr w1, w2;
    geometry(lvtr, avtr, &vol);

	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs		= new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussBasisCurlVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			TetraBasisCurlW(z, i, &w2);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;  // Removed epsr
			gaussBasisCurlVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w2;
		}
	}

	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisCurlVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			Curl_Sc->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	delete[] gaussBasisVtrs;
	delete[] gaussBasisCurlVtrs;
}

/** E matrices */
void tetra::Calculate_M_Matrix_E() {
    int i, j, dim;
    fp_t cval = 0.0;
    fp_t vol;
    fp_t coeffA[3][3];
    vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
                 // others [{0,1}, {0,2}, {0,3}]
    vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
                 // equal to the area of them [f0, f1, f2, f3]

    int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
    Local_DG_mapE(map, 0);

    if (LocalEDOF == 0) {
			MassE = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
			MassE->reset();
			return;
    }

    MassE = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    dim = MassE->getRowDim();
    if (dim != MassE->getColDim()) {
        cerr << "MassE: Non-square matrix passed." << endl;
    }
    MassE->reset();
    geometry(lvtr, avtr, &vol);
    makeCoeff(avtr, mat->epsr, coeffA);

    // dotP(u, [t]*u) dV
    // u = avtr, [t]*u = [t] * avtr
    for (i = 0; i < dim; i++) {
        for (j = 0; j < dim; j++) {
            cval =
                (coeffA[0][0] * T00_P2[i][j]) + (coeffA[0][1] * T01_P2[i][j]) +
                (coeffA[1][0] * T10_P2[i][j]) + (coeffA[0][2] * T02_P2[i][j]) +
                (coeffA[2][0] * T20_P2[i][j]) + (coeffA[1][1] * T11_P2[i][j]) +
                (coeffA[1][2] * T12_P2[i][j]) + (coeffA[2][1] * T21_P2[i][j]) +
                (coeffA[2][2] * T22_P2[i][j]);
            cval = cval / (vol * (fp_t)T_DNUM);

            MassE->addEntry(i, j, cval);
        }
    }

    // If it's PEC, set a 1 if it's in the diagonal, 0 if not
    for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
        for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
            if (map[i] < 0 || map[j] < 0) {
                if (i == j) {
                    MassE->setEntry(i, j, 1.0); // need to be inversed
                } else {
                    MassE->setEntry(i, j, 0); // need to be inversed
                }
            }
        }
    }
    if (map != nullptr)
        delete[] map;
}

void tetra::Calculate_R_Matrix_E(denseMat<fp_t>* R_Matrix_E, fp_t dt) {
    if (LocalEDOF == 0)
        return;
    int LocalDim, LocalFaceDim;
    int DGface_bc;
    int AbcFlag = 0;

    fp_t eps_o = Eo;
    fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
    fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

    int i, j, dim;
    int k = 0;
    fp_t cval = 0.0;
    fp_t vol;
    fp_t coeffA[3][3];
    vtr lvtr[4];
    vtr avtr[4];

    int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
    Local_DG_mapE(map, 0);

    dim = R_Matrix_E->getRowDim();
    if (dim != R_Matrix_E->getColDim()) {
			cerr << "MassE: Non-square matrix passed." << endl;
    }
    R_Matrix_E->reset();

    geometry(lvtr, avtr, &vol);
    makeCoeff(avtr, mat->sigma, coeffA);
    for (i = 0; i < dim; i++) {
        for (j = 0; j < dim; j++) {
            cval =
                (coeffA[0][0] * T00_P2[i][j]) + (coeffA[0][1] * T01_P2[i][j]) +
                (coeffA[1][0] * T10_P2[i][j]) + (coeffA[0][2] * T02_P2[i][j]) +
                (coeffA[2][0] * T20_P2[i][j]) + (coeffA[1][1] * T11_P2[i][j]) +
                (coeffA[1][2] * T12_P2[i][j]) + (coeffA[2][1] * T21_P2[i][j]) +
                (coeffA[2][2] * T22_P2[i][j]);
            cval = cval / (vol * (fp_t)T_DNUM);

            R_Matrix_E->addEntry(i, j, cval);
        }
    }

    if (map != nullptr)
        delete[] map;

    (*R_Matrix_E).scale(0.5 * (dt / eps_o));

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
    denseMat<fp_t> tetBe(LocalDim, LocalDim);
    // Matrices On the Faces
    denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

    for (i = 0; i < 4; i++) {
        DGface_bc = fc[i]->bcPtr->getbType();
        if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
            AbcFlag = 1;
            fc[i]->Calculate_Be_Matrix(&Be);
            for (j = 0; j < LocalFaceDim; j++) {
                for (k = 0; k < LocalFaceDim; k++) {
                    tetBe.addEntry(fac2tet[i][j], fac2tet[i][k], Be.getEntry(j, k));
                }
            }
        }
    }

    // ABC Boundary
    if (AbcFlag == 1) {
        for (i = 0; i < 4; i++) {
            // the same
            if (fc[i]->bcPtr->getbType() == abcType) {
                Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
            } else if (fc[i]->bcPtr->getbType() == planeWaveType) {
                Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
            } else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
                Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();
            }
        }
        tetBe.scale(-0.25 * Y * (dt / eps_o));
        *R_Matrix_E = *R_Matrix_E + tetBe;
    }
}

void tetra::Calculate_Bii_Matrix_E() {
	if (LocalEDOF == 0)
		return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
	fp_t Zneigh = 0.0;
	fp_t Yt = 0.0;

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	BiiE = new denseMat<fp_t>(LocalDim, LocalDim);

	denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr){
				printf("ERROR: Null pointer at bType = 0 in tetra %d in face %d = [(%.8f, %.8f, %.8f), (%.8f, %.8f, %.8f), (%.8f, %.8f, %.8f)]", cnt, i, fc[i]->nd[0]->getCoord().getx(), fc[i]->nd[0]->getCoord().gety(), fc[i]->nd[0]->getCoord().getz(), fc[i]->nd[1]->getCoord().getx(), fc[i]->nd[1]->getCoord().gety(), fc[i]->nd[1]->getCoord().getz(), fc[i]->nd[2]->getCoord().getx(), fc[i]->nd[2]->getCoord().gety(), fc[i]->nd[2]->getCoord().getz());
				cout << endl;
			}
			Zneigh = No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0));
			Yt = 1.0 / (0.5 * (Zneigh + Z));
			fc[i]->Calculate_Be_Matrix(&Be);

			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					BiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Yt * Be.getEntry(j, k) * GAMMA);
				}
			}
		} else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			fc[i]->Calculate_Be_Matrix(&Be);
			if (fc[i]->bcPtr->getbType() == abcType)
				Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
			else if (fc[i]->bcPtr->getbType() == planeWaveType)
				Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
			else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
				Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();

			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++)
					BiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Y * Be.getEntry(j, k));
		} else if (DGface_bc == pecType) {
		} else if (DGface_bc == pmcType) {
		} else if (DGface_bc == nonConformal){
			for(int idx = 0; idx < NeighNCNum; idx++){
				if (OverlapMapVec[i].find(idx)->second){
          if(IntersectionMapVec[i].find(idx)->second){
						Zneigh = No * sqrt(NeighNCTetra[idx]->mat->mur.getEntry(0, 0) / NeighNCTetra[idx]->mat->epsr.getEntry(0, 0));
						Yt = 1.0 / (0.5 * (Zneigh + Z));
						for (j = 0; j < LocalFaceDim; j++) {
							for (k = 0; k < LocalFaceDim; k++) {
								BiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Yt * NC_CouplingMatArray[idx * NumCouplingMatrices].getEntry(j, k) * GAMMA);
							}
						}
					}
				}
			}
		}		
	}
}

void tetra::Calculate_Bij_Matrix_E() {
    if (LocalEDOF == 0)
			return;
    int i, j, k;
    int LocalDim, LocalFaceDim;
    int DGface_bc;

    fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
    fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
    fp_t Zneigh = 0.0;
    fp_t Yt = 0.0;

    tetra* neigh;

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

    BijE = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

    denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

    // Loop through Faces
    for (i = 0; i < NumOfFaces; i++) {
			DGface_bc = fc[i]->bcPtr->getbType();

			if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
				neigh = fc[i]->neighborTETRA(this);

				if (neigh == nullptr)
					cout << "ERROR: Null pointer at bType = 0" << endl;

				fc[i]->Calculate_Be_Matrix(&Be);
				Zneigh = No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0));
				Yt = 1.0 / (0.5 * (Zneigh + Z));
				for (j = 0; j < LocalFaceDim; j++) {
					for (k = 0; k < LocalFaceDim; k++) {
						if (LocMapE[fac2tet[i][j]] >= 0) {
							BijE->addEntry(fac2tet[i][j], i * LocalFaceDim + k, -0.5 * Yt * Be.getEntry(j, k) * GAMMA);
						}
					}
				}
			} 
			else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} 
			else if (DGface_bc == pecType) {} 
			else if (DGface_bc == pmcType) {} 
    }
}

void tetra::Calculate_S_Matrix_E() {
	if (LocalEDOF == 0)
		return;

	int LocalDim;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];

	StiffE = new denseMat<fp_t>(LocalDim, LocalDim);
	Calculate_S_Matrix(StiffE); // [S]
}

void tetra::Calculate_Fii_Matrix_E() {
	if (LocalEDOF == 0)
		return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	FiiE = new denseMat<fp_t>(LocalDim, LocalDim);

	denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == nonConformal || DGface_bc == pmlType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr && DGface_bc != nonConformal)
				cout << "ERROR: Null pointer at bType = 0" << endl;
			else if (DGface_bc == nonConformal && NeighNCNum == 0)
				cout << "ERROR: Null pointer at bType = nonConformal no neighNC" << endl;
			fc[i]->Calculate_Fii_Matrix(this, &Fii);
			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					FiiE->addEntry(fac2tet[i][j], fac2tet[i][k], Fii.getEntry(j, k)); // 1
				}
			}
		} else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			fc[i]->Calculate_Fii_Matrix(this, &Fii);
			if (fc[i]->bcPtr->getbType() == abcType)
				Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
			else if (fc[i]->bcPtr->getbType() == planeWaveType)
				Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
			else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
				Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();

			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					FiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 2.0 * Fii.getEntry(j, k)); // 2
				}
			}
		} else if (DGface_bc == pecType) {
		} else if (DGface_bc == pmcType) {
		} 
	}
}
void tetra::Calculate_Fij_Matrix_E() {
	if (LocalEDOF == 0)
		return;

	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	FijE = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

	denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) { // not ABC and not PEC
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;
			fc[i]->Calculate_Fii_Matrix(this, &Fii);
			fc[i]->ApplyPEC_PMC(&Fii);
			for (j = 0; j < LocalFaceDim; j++){
				for (k = 0; k < LocalFaceDim; k++) {
					if (LocMapE[fac2tet[i][j]] >= 0)
						FijE->addEntry(fac2tet[i][j], i * LocalFaceDim + k, Fii.getEntry(j, k)); //  1
				}
			}
		} 
		else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} 
		else if (DGface_bc == pecType) {}
		else if (DGface_bc == pmcType) {} 
	}
}

/** E matrices numerically by quadrature */
void tetra::Calculate_M_Matrix_E_Numeric() {
    int i, j, k, dim;
    fp_t cval = 0.0;
    fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
    vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
                 // others [{0,1}, {0,2}, {0,3}]
    vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
                 // equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;

    int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
    Local_DG_mapE(map, 0);

    if (LocalEDOF == 0) 
	{
			cout << "RESET\n";
			MassE = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
			MassE->reset();
			return;
    }


    MassE = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    dim = MassE->getRowDim();
    if (dim != MassE->getColDim()) {
        cerr << "MassE: Non-square matrix passed." << endl;
    }
    MassE->reset();
    geometry(lvtr, avtr, &vol);

	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = mat->epsr * w1;
		}
	}

	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			MassE->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;

    // If it's PEC, set a 1 if it's in the diagonal, 0 if not
    for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
        for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
            if (map[i] < 0 || map[j] < 0) {
                if (i == j) {
                    MassE->setEntry(i, j, 1.0); // need to be inversed
                } else {
                    MassE->setEntry(i, j, 0); // need to be inversed
                }
            }
        }
    }


    if (map != nullptr)
        delete[] map;


}

void tetra::Calculate_R_Matrix_E_Numeric(denseMat<fp_t>* R_Matrix_E, fp_t dt) {
    if (LocalEDOF == 0)
        return;
    int LocalDim, LocalFaceDim;
    int DGface_bc;
    int AbcFlag = 0;

    fp_t eps_o = Eo;
    fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
    fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

    int i, j, k, dim;
    fp_t cval = 0.0;
    fp_t vol;
    fp_t wgt;
    fp_t* z;
    vtr lvtr[4];
    vtr avtr[4];
	vtr w1, w2;

    int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
    Local_DG_mapE(map, 0);

    dim = R_Matrix_E->getRowDim();
    if (dim != R_Matrix_E->getColDim()) {
			cerr << "MassE: Non-square matrix passed." << endl;
    }
    R_Matrix_E->reset();
    geometry(lvtr, avtr, &vol);

	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = mat->sigma * w1;
		}
	}

	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			R_Matrix_E->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;

    if (map != nullptr)
        delete[] map;

    (*R_Matrix_E).scale(0.5 * (dt / eps_o));

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
    denseMat<fp_t> tetBe(LocalDim, LocalDim);
    // Matrices On the Faces
    denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

    for (i = 0; i < 4; i++) {
        DGface_bc = fc[i]->bcPtr->getbType();
        if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
            AbcFlag = 1;
            fc[i]->Calculate_Be_Matrix_Numeric(&Be,PolyOrderFlag);
            for (j = 0; j < LocalFaceDim; j++) {
                for (k = 0; k < LocalFaceDim; k++) {
                    tetBe.addEntry(fac2tet[i][j], fac2tet[i][k], Be.getEntry(j, k));
                }
            }
        }
    }

    // ABC Boundary
    if (AbcFlag == 1) {
        for (i = 0; i < 4; i++) {
            // the same
            if (fc[i]->bcPtr->getbType() == abcType) {
                Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
            } else if (fc[i]->bcPtr->getbType() == planeWaveType) {
                Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
            } else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
                Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();
            }
        }
        tetBe.scale(-0.25 * Y * (dt / eps_o));
        *R_Matrix_E = *R_Matrix_E + tetBe;
    }
}

void tetra::Calculate_Bii_Matrix_E_Numeric() {
	if (LocalEDOF == 0)
		return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
	fp_t Zneigh = 0.0;
	fp_t Yt = 0.0;

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	BiiE = new denseMat<fp_t>(LocalDim, LocalDim);

	denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) 
		{
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr){
				printf("ERROR: Null pointer at bType = 0 in tetra %d in face %d = [(%.8f, %.8f, %.8f), (%.8f, %.8f, %.8f), (%.8f, %.8f, %.8f)]", cnt, i, fc[i]->nd[0]->getCoord().getx(), fc[i]->nd[0]->getCoord().gety(), fc[i]->nd[0]->getCoord().getz(), fc[i]->nd[1]->getCoord().getx(), fc[i]->nd[1]->getCoord().gety(), fc[i]->nd[1]->getCoord().getz(), fc[i]->nd[2]->getCoord().getx(), fc[i]->nd[2]->getCoord().gety(), fc[i]->nd[2]->getCoord().getz());
				cout << endl;
			}
			Zneigh = No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0));
			Yt = 1.0 / (0.5 * (Zneigh + Z));
			fc[i]->Calculate_Be_Matrix_Numeric(&Be,PolyOrderFlag);

			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					BiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Yt * Be.getEntry(j, k) * GAMMA);
				}
			}
		} else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			fc[i]->Calculate_Be_Matrix_Numeric(&Be,PolyOrderFlag);
			if (fc[i]->bcPtr->getbType() == abcType)
				Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
			else if (fc[i]->bcPtr->getbType() == planeWaveType)
				Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
			else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
				Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();

			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++)
					BiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Y * Be.getEntry(j, k));
		} else if (DGface_bc == pecType) {
		} else if (DGface_bc == pmcType) {
		} else if (DGface_bc == nonConformal){
			for(int idx = 0; idx < NeighNCNum; idx++){
				if (OverlapMapVec[i].find(idx)->second){
          if(IntersectionMapVec[i].find(idx)->second){
						Zneigh = No * sqrt(NeighNCTetra[idx]->mat->mur.getEntry(0, 0) / NeighNCTetra[idx]->mat->epsr.getEntry(0, 0));
						Yt = 1.0 / (0.5 * (Zneigh + Z));
						for (j = 0; j < LocalFaceDim; j++) {
							for (k = 0; k < LocalFaceDim; k++) {
								BiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Yt * NC_CouplingMatArray[idx * NumCouplingMatrices].getEntry(j, k) * GAMMA);
							}
						}
					}
				}
			}
		}		
	}
}

void tetra::Calculate_Bij_Matrix_E_Numeric() {
    if (LocalEDOF == 0)
			return;
    int i, j, k;
    int LocalDim, LocalFaceDim;
    int DGface_bc;

    fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
    fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
    fp_t Zneigh = 0.0;
    fp_t Yt = 0.0;

    tetra* neigh;

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

    BijE = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

    denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

    // Loop through Faces
    for (i = 0; i < NumOfFaces; i++) {
			DGface_bc = fc[i]->bcPtr->getbType();

			if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
				neigh = fc[i]->neighborTETRA(this);

				if (neigh == nullptr)
					cout << "ERROR: Null pointer at bType = 0" << endl;

				fc[i]->Calculate_Be_Matrix_Numeric(&Be,PolyOrderFlag);
				Zneigh = No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0));
				Yt = 1.0 / (0.5 * (Zneigh + Z));
				for (j = 0; j < LocalFaceDim; j++) {
					for (k = 0; k < LocalFaceDim; k++) {
						if (LocMapE[fac2tet[i][j]] >= 0) {
							BijE->addEntry(fac2tet[i][j], i * LocalFaceDim + k, -0.5 * Yt * Be.getEntry(j, k) * GAMMA);
						}
					}
				}
			} 
			else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} 
			else if (DGface_bc == pecType) {} 
			else if (DGface_bc == pmcType) {} 
    }
}

void tetra::Calculate_S_Matrix_E_Numeric() {
	if (LocalEDOF == 0)
		return;

	int LocalDim;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];

	StiffE = new denseMat<fp_t>(LocalDim, LocalDim);
	Calculate_S_Matrix_Numeric(StiffE); // [S]
}

void tetra::Calculate_Fii_Matrix_E_Numeric() {
	if (LocalEDOF == 0)
		return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	FiiE = new denseMat<fp_t>(LocalDim, LocalDim);

	denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == nonConformal || DGface_bc == pmlType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr && DGface_bc != nonConformal)
				cout << "ERROR: Null pointer at bType = 0" << endl;
			else if (DGface_bc == nonConformal && NeighNCNum == 0)
				cout << "ERROR: Null pointer at bType = nonConformal no neighNC" << endl;
			fc[i]->Calculate_Fii_Matrix_Numeric(this, &Fii,PolyOrderFlag);
			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					FiiE->addEntry(fac2tet[i][j], fac2tet[i][k], Fii.getEntry(j, k)); // 1
				}
			}
		} else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			fc[i]->Calculate_Fii_Matrix_Numeric(this, &Fii,PolyOrderFlag);
			if (fc[i]->bcPtr->getbType() == abcType)
				Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
			else if (fc[i]->bcPtr->getbType() == planeWaveType)
				Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
			else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
				Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();

			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					FiiE->addEntry(fac2tet[i][j], fac2tet[i][k], 2.0 * Fii.getEntry(j, k)); // 2
				}
			}
		} else if (DGface_bc == pecType) {
		} else if (DGface_bc == pmcType) {
		} 
	}
}
void tetra::Calculate_Fij_Matrix_E_Numeric() {
	if (LocalEDOF == 0)
		return;

	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	FijE = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

	denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) { // not ABC and not PEC
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;
			fc[i]->Calculate_Fii_Matrix_Numeric(this, &Fii, PolyOrderFlag);
			fc[i]->ApplyPEC_PMC(&Fii);
			for (j = 0; j < LocalFaceDim; j++){
				for (k = 0; k < LocalFaceDim; k++) {
					if (LocMapE[fac2tet[i][j]] >= 0)
						FijE->addEntry(fac2tet[i][j], i * LocalFaceDim + k, Fii.getEntry(j, k)); //  1
				}
			}
		} 
		else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} 
		else if (DGface_bc == pecType) {}
		else if (DGface_bc == pmcType) {} 
	}
}

/** H matrices */
void tetra::Calculate_M_Matrix_H() {
	int i, j, dim;
	fp_t vol;
	fp_t coeffA[3][3];
	fp_t cval = 0.0;
	vtr lvtr[4];
	vtr avtr[4];

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	Local_DG_mapH(map, 0);

	if (LocalHDOF == 0) {
		MassH = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
		MassH->reset();
		return;
	}

	MassH = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
	dim = MassH->getRowDim();

	if (dim != MassH->getColDim()) {
			cerr << "MassH: Non-square matrix passed." << endl;
	}

	MassH->reset();
	geometry(lvtr, avtr, &vol);
	makeCoeff(avtr, mat->mur, coeffA);
	// dotP(u, [t]*u) dV
	// u = avtr, [t]*u = [t] * avtr
	for (i = 0; i < dim; i++) {
			for (j = 0; j < dim; j++) {
					cval =
							(coeffA[0][0] * T00_P2[i][j]) + (coeffA[0][1] * T01_P2[i][j]) +
							(coeffA[1][0] * T10_P2[i][j]) + (coeffA[0][2] * T02_P2[i][j]) +
							(coeffA[2][0] * T20_P2[i][j]) + (coeffA[1][1] * T11_P2[i][j]) +
							(coeffA[1][2] * T12_P2[i][j]) + (coeffA[2][1] * T21_P2[i][j]) +
							(coeffA[2][2] * T22_P2[i][j]);

					cval = cval / (vol * (fp_t)T_DNUM);
					MassH->addEntry(i, j, cval);
			}
	}

	// If it's PMC, set a 1 if it's in the diagonal, 0 if not
	for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
		for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
			if (map[i] < 0 || map[j] < 0) {
				if (i == j) {
					MassH->setEntry(i, j, 1.0); // need to be inversed
				} else {
					MassH->setEntry(i, j, 0); // need to be inversed
				}
			}
		}
	}
	if (map != nullptr)
		delete[] map;
}

void tetra::Calculate_R_Matrix_H(denseMat<fp_t>* CondMatrixH, fp_t dt) {
	if (LocalHDOF == 0)
		return;

	int LocalDim, LocalFaceDim;
	int DGface_bc;
	int AbcFlag = 0;
	fp_t mu_o = Uo;
	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	int i, j, dim;
	int k = 0;
	fp_t vol;
	fp_t coeffA[3][3];
	fp_t cval = 0.0;
	vtr lvtr[4];
	vtr avtr[4];

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	Local_DG_mapH(map, 0);

	dim = CondMatrixH->getRowDim();
	if (dim != CondMatrixH->getColDim()) {
		cerr << "MassH: Non-square matrix passed." << endl;
	}
	
	CondMatrixH->reset();
	geometry(lvtr, avtr, &vol);
	makeCoeff(avtr, mat->sigma, coeffA);
	for (i = 0; i < dim; i++) {
		for (j = 0; j < dim; j++) {
			cval =
					(coeffA[0][0] * T00_P2[i][j]) + (coeffA[0][1] * T01_P2[i][j]) +
					(coeffA[1][0] * T10_P2[i][j]) + (coeffA[0][2] * T02_P2[i][j]) +
					(coeffA[2][0] * T20_P2[i][j]) + (coeffA[1][1] * T11_P2[i][j]) +
					(coeffA[1][2] * T12_P2[i][j]) + (coeffA[2][1] * T21_P2[i][j]) +
					(coeffA[2][2] * T22_P2[i][j]);
			cval = cval / (vol * (fp_t)T_DNUM);

			CondMatrixH->addEntry(i, j, cval);
		}
	}

	if (map != nullptr)
		delete[] map;

	fp_t sigma_m = 0.0;
	(*CondMatrixH).scale(0.5 * (dt / mu_o) * sigma_m);

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	denseMat<fp_t> tetBh(LocalDim, LocalDim);

	// Matrices On the Faces
	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	for (i = 0; i < 4; i++) {
		DGface_bc = getFacePtr(i)->bcPtr->getbType();

		if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			AbcFlag = 1;
			fc[i]->Calculate_Bh_Matrix(&Bh);
			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					tetBh.addEntry(fac2tet[i][j], fac2tet[i][k], Bh.getEntry(j, k));
				}
			}
		}
	}

	// ABC Boundary
	if (AbcFlag == 1) {
		for (i = 0; i < 4; i++) {
			if (fc[i]->bcPtr->getbType() == abcType) {
				Z = fc[i]->bcPtr->get_ABCImpVal();
			} else if (fc[i]->bcPtr->getbType() == planeWaveType) {
				Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			} else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
				Z = fc[i]->bcPtr->get_PortImpVal();
			}
		}
		tetBh.scale(-0.25 * Z * (dt / mu_o));
		*CondMatrixH = *CondMatrixH + tetBh;
	}
}

void tetra::Calculate_Bii_Matrix_H() {
	if (LocalHDOF == 0)
			return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
	fp_t Yneigh = 0.0;
	fp_t Zt = 0.0;

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	BiiH = new denseMat<fp_t>(LocalDim, LocalDim);

	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;

			Yneigh = 1.0 / (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
			Zt = 1.0 / (0.5 * (Y + Yneigh));
			fc[i]->Calculate_Bh_Matrix(&Bh);
			
			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++)
					BiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Zt * Bh.getEntry(j, k) * GAMMA); // 0.5

		} else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			fc[i]->Calculate_Bh_Matrix(&Bh);

			if (fc[i]->bcPtr->getbType() == abcType)
				Z = fc[i]->bcPtr->get_ABCImpVal();
			else if (fc[i]->bcPtr->getbType() == planeWaveType)
				Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
				Z = fc[i]->bcPtr->get_PortImpVal();

			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++)
					BiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Z * Bh.getEntry(j, k));
		} else if (DGface_bc == pecType) {
		} else if (DGface_bc == pmcType) {
		} else if (DGface_bc == nonConformal){
			for(int idx = 0; idx < NeighNCNum; idx++){
				if (OverlapMapVec[i].find(idx)->second){
          if(IntersectionMapVec[i].find(idx)->second){
						Yneigh = 1.0 / (No * sqrt(NeighNCTetra[idx]->mat->mur.getEntry(0, 0) / NeighNCTetra[idx]->mat->epsr.getEntry(0, 0)));
						Zt = 1.0 / (0.5 * (Y + Yneigh));

						for (j = 0; j < LocalFaceDim; j++) {
							for (k = 0; k < LocalFaceDim; k++) {
								BiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Zt * NC_CouplingMatArray[idx * NumCouplingMatrices].getEntry(j, k) * GAMMA);
							}
						}
					}
				}
			}
		}
	}
}

void tetra::Calculate_Bij_Matrix_H() {
	if (LocalHDOF == 0)
		return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
	fp_t Yneigh = 0.0;
	fp_t Zt = 0.0;

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	BijH = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;
			fc[i]->Calculate_Bh_Matrix(&Bh);
			Yneigh = 1.0 / (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
			Zt = 1.0 / (0.5 * (Y + Yneigh));
			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++) {
					if (LocMapH[fac2tet[i][j]] >= 0)
						BijH->addEntry(fac2tet[i][j], i * LocalFaceDim + k, -0.5 * Zt * Bh.getEntry(j, k) * GAMMA); 
				}
		} 
		else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} else if (DGface_bc == pecType) {} 
		else if (DGface_bc == pmcType) {}
	}
}

void tetra::Calculate_S_Matrix_H() {
	if (LocalHDOF == 0)
		return;

	int LocalDim;
	LocalDim = TetPolyOrderDim[PolyOrderFlag];

	StiffH = new denseMat<fp_t>(LocalDim, LocalDim);
	Calculate_S_Matrix(StiffH); // [S]
}

void tetra::Calculate_Fii_Matrix_H() {
  if (LocalHDOF == 0)
    return;

  int i, j, k;
  int LocalDim, LocalFaceDim;
  int DGface_bc;

  fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
  fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

  tetra* neigh;

  LocalDim = TetPolyOrderDim[PolyOrderFlag];
  LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

  FiiH = new denseMat<fp_t>(LocalDim, LocalDim);

  denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

  // Loop through Faces
  for (i = 0; i < NumOfFaces; i++) {
    DGface_bc = fc[i]->bcPtr->getbType();

    if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == nonConformal || DGface_bc == pmlType) {
      neigh = fc[i]->neighborTETRA(this);
      if (neigh == nullptr && DGface_bc != nonConformal)
				cout << "ERROR: Null pointer at bType = 0" << endl;
			else if (DGface_bc == nonConformal && NeighNCNum == 0)
				cout << "ERROR: Null pointer at bType = nonConformal no neighNC" << endl;
	    fc[i]->Calculate_Fii_Matrix(this, &Fii);
      for (j = 0; j < LocalFaceDim; j++) {
        for (k = 0; k < LocalFaceDim; k++) {
          FiiH->addEntry(fac2tet[i][j], fac2tet[i][k], Fii.getEntry(j, k)); 
        }
      }
    } else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
      fc[i]->Calculate_Fii_Matrix(this, &Fii);
      if (fc[i]->bcPtr->getbType() == abcType)
        Z = fc[i]->bcPtr->get_ABCImpVal();
      else if (fc[i]->bcPtr->getbType() == planeWaveType)
        Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
      else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
        Z = fc[i]->bcPtr->get_PortImpVal();

      for (j = 0; j < LocalFaceDim; j++)
        for (k = 0; k < LocalFaceDim; k++)
          FiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 2.0 * Fii.getEntry(j, k)); 
    } 
		else if (DGface_bc == pecType) {} 
		else if (DGface_bc == pmcType) {}
  }
}

void tetra::Calculate_Fij_Matrix_H() {
  if (LocalHDOF == 0)
    return;
  int i, j, k;
  int LocalDim, LocalFaceDim;
  int DGface_bc;

  fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
  fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

  tetra* neigh;

  LocalDim = TetPolyOrderDim[PolyOrderFlag];
  LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

  FijH = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

  denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

  // Loop through Faces
  for (i = 0; i < NumOfFaces; i++) {

    DGface_bc = fc[i]->bcPtr->getbType();

    if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
      neigh = fc[i]->neighborTETRA(this);
      if (neigh == nullptr)
        cout << "ERROR: Null pointer at bType = 0" << endl;
      fc[i]->Calculate_Fii_Matrix(this, &Fii);
      fc[i]->ApplyPMC_PEC(&Fii);
      for (j = 0; j < LocalFaceDim; j++)
        for (k = 0; k < LocalFaceDim; k++)
          if (LocMapH[fac2tet[i][j]] >= 0)
            FijH->addEntry(fac2tet[i][j], i * LocalFaceDim + k, -Fii.getEntry(j, k));
    } 
		else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} 
		else if (DGface_bc == pecType) {} 
		else if (DGface_bc == pmcType) {}
  }
}

void tetra::Calculate_M_Matrix_H_Numeric() {
	int i, j, k, dim;
    fp_t cval = 0.0;
    fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
    vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
                 // others [{0,1}, {0,2}, {0,3}]
    vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
                 // equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	Local_DG_mapH(map, 0);

	if (LocalHDOF == 0) {
		MassH = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
		MassH->reset();
		return;
	}
	MassH = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
	dim = MassH->getRowDim();

	if (dim != MassH->getColDim()) {
			cerr << "MassH: Non-square matrix passed." << endl;
	}

	MassH->reset();
	geometry(lvtr, avtr, &vol);

	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = mat->mur * w1;
		}
	}

	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			MassH->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;

	// If it's PMC, set a 1 if it's in the diagonal, 0 if not
	for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
		for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
			if (map[i] < 0 || map[j] < 0) {
				if (i == j) {
					MassH->setEntry(i, j, 1.0); // need to be inversed
				} else {
					MassH->setEntry(i, j, 0); // need to be inversed
				}
			}
		}
	}
	if (map != nullptr)
		delete[] map;
}

void tetra::Calculate_R_Matrix_H_Numeric(denseMat<fp_t>* CondMatrixH, fp_t dt) {
	if (LocalHDOF == 0)
		return;

	int LocalDim, LocalFaceDim;
	int DGface_bc;
	int AbcFlag = 0;
	fp_t mu_o = Uo;
	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	int i, j, k, dim;
	fp_t vol;
	fp_t cval = 0.0;
	fp_t wgt;
	fp_t* z;
	vtr lvtr[4];
	vtr avtr[4];
	vtr w1, w2;

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	Local_DG_mapH(map, 0);

	dim = CondMatrixH->getRowDim();
	if (dim != CondMatrixH->getColDim()) {
		cerr << "MassH: Non-square matrix passed." << endl;
	}
	
	CondMatrixH->reset();
	geometry(lvtr, avtr, &vol);

	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = mat->sigma * w1;
		}
	}

	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			CondMatrixH->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;

	if (map != nullptr)
		delete[] map;

	fp_t sigma_m = 0.0;
	(*CondMatrixH).scale(0.5 * (dt / mu_o) * sigma_m);

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	denseMat<fp_t> tetBh(LocalDim, LocalDim);

	// Matrices On the Faces
	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	for (i = 0; i < 4; i++) {
		DGface_bc = getFacePtr(i)->bcPtr->getbType();

		if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			AbcFlag = 1;
			fc[i]->Calculate_Bh_Matrix_Numeric(&Bh, PolyOrderFlag);
			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					tetBh.addEntry(fac2tet[i][j], fac2tet[i][k], Bh.getEntry(j, k));
				}
			}
		}
	}

	// ABC Boundary
	if (AbcFlag == 1) {
		for (i = 0; i < 4; i++) {
			if (fc[i]->bcPtr->getbType() == abcType) {
				Z = fc[i]->bcPtr->get_ABCImpVal();
			} else if (fc[i]->bcPtr->getbType() == planeWaveType) {
				Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			} else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
				Z = fc[i]->bcPtr->get_PortImpVal();
			}
		}
		tetBh.scale(-0.25 * Z * (dt / mu_o));
		*CondMatrixH = *CondMatrixH + tetBh;
	}
}

void tetra::Calculate_Bii_Matrix_H_Numeric() {
	if (LocalHDOF == 0)
			return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
	fp_t Yneigh = 0.0;
	fp_t Zt = 0.0;

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	BiiH = new denseMat<fp_t>(LocalDim, LocalDim);

	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;

			Yneigh = 1.0 / (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
			Zt = 1.0 / (0.5 * (Y + Yneigh));
			fc[i]->Calculate_Bh_Matrix_Numeric(&Bh,PolyOrderFlag);
			
			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++)
					BiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Zt * Bh.getEntry(j, k) * GAMMA); // 0.5

		} else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			fc[i]->Calculate_Bh_Matrix_Numeric(&Bh,PolyOrderFlag);

			if (fc[i]->bcPtr->getbType() == abcType)
				Z = fc[i]->bcPtr->get_ABCImpVal();
			else if (fc[i]->bcPtr->getbType() == planeWaveType)
				Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
				Z = fc[i]->bcPtr->get_PortImpVal();

			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++)
					BiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Z * Bh.getEntry(j, k));
		} else if (DGface_bc == pecType) {
		} else if (DGface_bc == pmcType) {
		} else if (DGface_bc == nonConformal){
			for(int idx = 0; idx < NeighNCNum; idx++){
				if (OverlapMapVec[i].find(idx)->second){
          if(IntersectionMapVec[i].find(idx)->second){
						Yneigh = 1.0 / (No * sqrt(NeighNCTetra[idx]->mat->mur.getEntry(0, 0) / NeighNCTetra[idx]->mat->epsr.getEntry(0, 0)));
						Zt = 1.0 / (0.5 * (Y + Yneigh));

						for (j = 0; j < LocalFaceDim; j++) {
							for (k = 0; k < LocalFaceDim; k++) {
								BiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 0.5 * Zt * NC_CouplingMatArray[idx * NumCouplingMatrices].getEntry(j, k) * GAMMA);
							}
						}
					}
				}
			}
		}
	}
}

void tetra::Calculate_Bij_Matrix_H_Numeric() {
	if (LocalHDOF == 0)
		return;
	int i, j, k;
	int LocalDim, LocalFaceDim;
	int DGface_bc;

	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
	fp_t Yneigh = 0.0;
	fp_t Zt = 0.0;

	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	BijH = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	// Loop through Faces
	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;
			fc[i]->Calculate_Bh_Matrix_Numeric(&Bh,PolyOrderFlag);
			Yneigh = 1.0 / (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
			Zt = 1.0 / (0.5 * (Y + Yneigh));
			for (j = 0; j < LocalFaceDim; j++)
				for (k = 0; k < LocalFaceDim; k++) {
					if (LocMapH[fac2tet[i][j]] >= 0)
						BijH->addEntry(fac2tet[i][j], i * LocalFaceDim + k, -0.5 * Zt * Bh.getEntry(j, k) * GAMMA); 
				}
		} 
		else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} else if (DGface_bc == pecType) {} 
		else if (DGface_bc == pmcType) {}
	}
}

void tetra::Calculate_S_Matrix_H_Numeric() {
	if (LocalHDOF == 0)
		return;

	int LocalDim;
	LocalDim = TetPolyOrderDim[PolyOrderFlag];

	StiffH = new denseMat<fp_t>(LocalDim, LocalDim);
	Calculate_S_Matrix_Numeric(StiffH); // [S]
}

void tetra::Calculate_Fii_Matrix_H_Numeric() {
  if (LocalHDOF == 0)
    return;

  int i, j, k;
  int LocalDim, LocalFaceDim;
  int DGface_bc;

  fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
  fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

  tetra* neigh;

  LocalDim = TetPolyOrderDim[PolyOrderFlag];
  LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

  FiiH = new denseMat<fp_t>(LocalDim, LocalDim);

  denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

  // Loop through Faces
  for (i = 0; i < NumOfFaces; i++) {
    DGface_bc = fc[i]->bcPtr->getbType();

    if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == nonConformal || DGface_bc == pmlType) {
      neigh = fc[i]->neighborTETRA(this);
      if (neigh == nullptr && DGface_bc != nonConformal)
				cout << "ERROR: Null pointer at bType = 0" << endl;
	  else if (DGface_bc == nonConformal && NeighNCNum == 0)
				cout << "ERROR: Null pointer at bType = nonConformal no neighNC" << endl;
	    fc[i]->Calculate_Fii_Matrix_Numeric(this, &Fii, PolyOrderFlag);
      for (j = 0; j < LocalFaceDim; j++) {
        for (k = 0; k < LocalFaceDim; k++) {
          FiiH->addEntry(fac2tet[i][j], fac2tet[i][k], Fii.getEntry(j, k)); 
        }
      }
    } else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
      fc[i]->Calculate_Fii_Matrix_Numeric(this, &Fii, PolyOrderFlag);
      if (fc[i]->bcPtr->getbType() == abcType)
        Z = fc[i]->bcPtr->get_ABCImpVal();
      else if (fc[i]->bcPtr->getbType() == planeWaveType)
        Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
      else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
        Z = fc[i]->bcPtr->get_PortImpVal();

      for (j = 0; j < LocalFaceDim; j++)
        for (k = 0; k < LocalFaceDim; k++)
          FiiH->addEntry(fac2tet[i][j], fac2tet[i][k], 2.0 * Fii.getEntry(j, k)); 
    } 
		else if (DGface_bc == pecType) {} 
		else if (DGface_bc == pmcType) {}
  }
}

void tetra::Calculate_Fij_Matrix_H_Numeric() {
  if (LocalHDOF == 0)
    return;
  int i, j, k;
  int LocalDim, LocalFaceDim;
  int DGface_bc;

  fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
  fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

  tetra* neigh;

  LocalDim = TetPolyOrderDim[PolyOrderFlag];
  LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

  FijH = new denseMat<fp_t>(LocalDim, 4 * LocalFaceDim);

  denseMat<fp_t> Fii(LocalFaceDim, LocalFaceDim);

  // Loop through Faces
  for (i = 0; i < NumOfFaces; i++) {

    DGface_bc = fc[i]->bcPtr->getbType();

    if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType || DGface_bc == pmlType) {
      neigh = fc[i]->neighborTETRA(this);
      if (neigh == nullptr)
        cout << "ERROR: Null pointer at bType = 0" << endl;
      fc[i]->Calculate_Fii_Matrix_Numeric(this, &Fii, PolyOrderFlag);
      fc[i]->ApplyPMC_PEC(&Fii);
      for (j = 0; j < LocalFaceDim; j++)
        for (k = 0; k < LocalFaceDim; k++)
          if (LocMapH[fac2tet[i][j]] >= 0)
            FijH->addEntry(fac2tet[i][j], i * LocalFaceDim + k, -Fii.getEntry(j, k));
    } 
		else if (DGface_bc == abcType || DGface_bc == planeWaveType) {} 
		else if (DGface_bc == pecType) {} 
		else if (DGface_bc == pmcType) {}
  }
}



// Here, the time step is calculated based on the new formula
// This, can handle both ocnformal and non-conformal for
// non uniform polynomial ordes as well

//***************************
// From experience we found that the stabilty conditions are sufficient
// but not necessary. In example, the upwind flux results in a slightly more
// restrictive time step compared to central flux.
// However, in practise we found one can use the central flux timestep for both
// upwing and central flux
// without any instability occuring. Here we use the central flux timestep to be
// more effecient.
//***************************

//********************************
// Simplified stability for conformal meshes only and with uniform p=0,1 only.
// This way of getting the dt is much faster since you dont compute the local
// eigenvalues so for p=0,1 and conformal this maybe prefered since it is much
// faster.
void tetra::TimeStepEstimate(fp_t& dtEstim, fp_t& V_P) {
    vtr lvtr[4];
    vtr avtr[4];
    fp_t Vol_i;
    fp_t Per_i = 0.0;
    fp_t area = 0.0;
    fp_t eps_i;
    fp_t tmp_eps_k;
    fp_t eps_k_min = 10000.0;
    fp_t mu_i;
    fp_t tmp_mu_k;
    fp_t mu_k_min = 10000.0;
    vtr Normal;
    geometry(lvtr, avtr, &Vol_i);

    for (int i = 0; i < 4; i++) {
        fc[i]->getAreaNormal(&area, &Normal);
        Per_i = Per_i + area;
    }

    eps_i = mat->epsr.getEntry(0, 0);
    mu_i = mat->mur.getEntry(0, 0);
    eps_k_min = eps_i;
    mu_k_min = mu_i;

    for (int k = 0; k < NeighNum; k++) {
        if (NeighTetra[k] != nullptr) {
            tmp_eps_k = NeighTetra[k]->mat->epsr.getEntry(0, 0);
            tmp_mu_k = NeighTetra[k]->mat->mur.getEntry(0, 0);

            if (tmp_eps_k < eps_k_min)
                eps_k_min = tmp_eps_k;

            if (tmp_mu_k < mu_k_min)
                mu_k_min = tmp_mu_k;
        }
    }

    V_P = Vol_i / Per_i;

    if (sqrt(mu_i / mu_k_min) != 1.0) {
        cout << "tetra::TimeStepEstimate::ERROR" << endl;
    }

    fp_t Maxe_ie_k;
    if (sqrt(eps_i / eps_k_min) < sqrt(mu_i / mu_k_min)) {
        Maxe_ie_k = sqrt(mu_i / mu_k_min);
    } else {
        Maxe_ie_k = sqrt(eps_i / eps_k_min);
    }

    fp_t Denom;
    fp_t Numer = 4.0 * (Vol_i / Per_i);
    fp_t MatProp = sqrt(mat->mur.getEntry(0, 0) * mat->epsr.getEntry(0, 0));
    

	fp_t StabilityScale;
    StabilityScale = 2.0;

    if (PolyOrderFlag == 0) {
        Denom = StabilityScale * (Vo / MatProp) * (Maxe_ie_k);
    } else if (PolyOrderFlag == 1) {
        // Luis Diaz Angulo, Discontinuous Galerkin Time Domain
        // Methods in Computational Electrodynamics:
        // State of the Art, EQ(47)
        Denom = (1.0) * (Vo / MatProp) * (8.0 * Maxe_ie_k + sqrt(20.0)) * (1. / 3.); // 40 -> 20
    }
    else if (PolyOrderFlag == 2) {
        Denom = (1.0) * (Vo / MatProp) * (8.0 * Maxe_ie_k + sqrt(20.0)) * (1. / 3.); // 40 -> 20
        Denom *= 2.0; //1.5 provokes an error in lambda/8. The value is empirically obtained from the analytical formula
    }
    else if (PolyOrderFlag == 3) {
        Denom = (1.0) * (Vo / MatProp) * (8.0 * Maxe_ie_k + sqrt(20.0)) * (1. / 3.); // 40 -> 20
        Denom *= 3.0; //1.5 provokes an error in lambda/8. The value is empirically obtained from the analytical formula
    }
    dtEstim = 0.8 * (Numer / Denom); 

	if (PML_Flag)
	{
		fp_t h = V_P;  // Characteristic length
		fp_t sigma = mat->sigma.getEntry(0, 0);  // Assume sigma_z dominates (adjust for anisotropy)
		fp_t c = Vo;  // Speed of light in the medium

		fp_t factor = 1.0 + (sigma * sigma * h * h) / (8.0 * c * c);
		fp_t dt_pml = h / (c * sqrt(2.0 * factor));

		// Final conservative dt for this tetra
		if (PolyOrderFlag == 2) {
			dtEstim = std::min(dtEstim, dt_pml * 0.8 / 2.0);  // 0.8 safety factor
		}
		else if (PolyOrderFlag == 3) {
			dtEstim = std::min(dtEstim, dt_pml * 0.8 / 3.0);  // 0.8 safety factor
		}
	

	}
	
	// Reduce the time step if the PML is used with high conducitivty
	/*
	if (PML_Flag)
	{
		vtr rc = Center();

		// This is the polynomial order of the conductivity profile of the PML
		int m = mat->get_PML_m_ord();  
		
		// (Cartesian-Aligned) PML
		// Obtain the the thickness of the PML	
		fp_t pml_thick = mat->get_PML_thick();

		fp_t reflection_coeff = mat->sigma.getEntry(0, 0); // This is the reflection coefficient
		fp_t eps_o = 8.854187817e-12;  
		fp_t sigma_base = - (m + 1.0) / (240.0 * Pi * pml_thick) * log(reflection_coeff);
		sigma_base = sigma_base;


		fp_t Rx = 0.2;
		fp_t Ry = 0.2;
		fp_t Rz = 0.2;


		// Compute radial distance from object surface into PML
		fp_t distance = rc.magnitude() - std::max({Rx, Ry, Rz});
		fp_t D = pml_thick;

		// Gaussian profile settings
		fp_t dc = 0.6 * D;  // Peak at 60% of PML thickness
		fp_t w  = 0.3 * D;  // Width of Gaussian

		// Clamp distance into [0, D]
		distance = std::max(0.0, std::min(D, distance));

		// Compute Gaussian conductivity profile
		fp_t s3 = sigma_base * std::exp(-std::pow((distance - dc) / w, 2));

		if (s3 > 1.0)
		{
			cout << "s3: " << s3 << endl;
			dtEstim = dtEstim / (s3) / 2.0;
		}
	}
	*/
	
	

    /*
        In order to fulfill the LTS requirement for stability the dt must be multiplied by 0.8 
        "Dissipative terms and local time-stepping improvements
            in a spatial high order Discontinuous Galerkin scheme
            for the time-domain Maxwell’s equations"
            , E. Montseny
    */
}

void tetra::SetUpMatrixFree() {
	vtr lvtr[NumOfFaces]; // vectors of the edges from node 0 to the others (the fourth value is not used)
	vtr avtr[NumOfFaces]; // inward normal vectors to the faces with the magnitude equal to the area of the faces
	geometry(lvtr, gradZeta_, &vol_);

	LocMapE = new int[TetPolyOrderDim[PolyOrderFlag]];
	LocMapH = new int[TetPolyOrderDim[PolyOrderFlag]];

	Local_DG_mapE(LocMapE, LocalOffsetE);
	Local_DG_mapH(LocMapH, LocalOffsetH);

	MapPMC();
	MapPEC();

	if(isNonConformal){
    SetUpInterfaceMatrices_NC();
		SetUpInterfaceMaps_NC();
	}
}

// MF Set-up, pre-compute and store the inverse  of the LHS matrices to be use
// at every time iteration. These inverses along with the massmatrix are the
// only ones stored in the matrix-free implementation

// NMF Set-up,  Here we precompute the update matrices before the time step is
// started. This part is only used when the non-matrix free option is choosen,
// i.e. when memory resources are suffecient. This implementation is 2-3 time
// faster compared to matrix-free but also consumes about 3x more memory as
// well.

/* E setup */
void tetra::SetUp_LocalFaceToTetraMapE_NMF1(fp_t dt) {
	if (LocalEDOF == 0)
		return;
	int LocalDim, LocalFaceDim;
	fp_t eps_o = Eo;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

		// sigma and abc
	denseMat<fp_t> RMatrixE(LocalDim, LocalDim);
	Calculate_R_Matrix_E(&RMatrixE, dt);

	RHSLoc1E = new denseMat<fp_t>(LocalDim, LocalDim);
	RHSLoc2E = new denseMat<fp_t>(LocalDim, LocalDim);
	RHSCoup1E = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
	RHSCoup2E = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);

	BiiE->scale(dt / eps_o);

	*RHSLoc1E = *MassE + *BiiE - RMatrixE;
	ApplyPEC(RHSLoc1E, LocalDim);

	*RHSLoc2E = *StiffE - *FiiE;
	RHSLoc2E->scale(dt / eps_o);
	ApplyPEC_E(RHSLoc2E, LocalDim);
	ApplyPMC_E(RHSLoc2E, LocalDim);

	*RHSCoup1E = *FijE;
	RHSCoup1E->scale(dt / eps_o);

	*RHSCoup2E = *BijE;
	RHSCoup2E->scale(dt / eps_o);

	// Inv
	LHSInvE = new denseMat<fp_t>(LocalDim, LocalDim);
	*LHSInvE = *MassE + RMatrixE;
	*LHSInvE = LHSInvE->inverse();
	
	MassE->Clear();
	StiffE->Clear();
	BiiE->Clear();
	FiiE->Clear();
	BijE->Clear();
	FijE->Clear();
}

/* E setup */
void tetra::SetUp_LocalFaceToTetraMapE_NMF1_Numeric(fp_t dt) {
	if (LocalEDOF == 0)
		return;
	int LocalDim, LocalFaceDim;
	fp_t eps_o = Eo;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
	
	// sigma and abc
	denseMat<fp_t> RMatrixE(LocalDim, LocalDim);
	Calculate_R_Matrix_E_Numeric(&RMatrixE, dt);

	RHSLoc1E = new denseMat<fp_t>(LocalDim, LocalDim);
	RHSLoc2E = new denseMat<fp_t>(LocalDim, LocalDim);
	RHSCoup1E = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
	RHSCoup2E = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);

	BiiE->scale(dt / eps_o);

	*RHSLoc1E = *MassE + *BiiE - (RMatrixE) ;
	ApplyPEC(RHSLoc1E, LocalDim);

	*RHSLoc2E = *StiffE - *FiiE;
	RHSLoc2E->scale(dt / eps_o);
	ApplyPEC_E(RHSLoc2E, LocalDim);
	ApplyPMC_E(RHSLoc2E, LocalDim);

	*RHSCoup1E = *FijE;
	RHSCoup1E->scale(dt / eps_o);

	*RHSCoup2E = *BijE;
	RHSCoup2E->scale(dt / eps_o);



	// Inv
	LHSInvE = new denseMat<fp_t>(LocalDim, LocalDim);
	*LHSInvE = *MassE + RMatrixE;
	*LHSInvE = LHSInvE->inverse();
	
	MassE->Clear();
	StiffE->Clear();
	BiiE->Clear();
	FiiE->Clear();
	BijE->Clear();
	FijE->Clear();
}

/* H setup */
void tetra::SetUp_LocalFaceToTetraMapH_NMF1(fp_t dt) {
    if (LocalHDOF == 0)
        return;
    int LocalDim, LocalFaceDim;
    fp_t mu_o = Uo;

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

		// sigma and abc
    denseMat<fp_t> RMatrixH(LocalDim, LocalDim);
    Calculate_R_Matrix_H(&RMatrixH, dt);

    RHSLoc1H = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSLoc2H = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSCoup1H = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
    RHSCoup2H = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);

    BiiH->scale(dt / mu_o);

    *RHSLoc1H = *MassH + *BiiH - RMatrixH;
    ApplyPMC(RHSLoc1H, LocalDim);

    *RHSLoc2H = *FiiH - *StiffH;
    RHSLoc2H->scale(dt / mu_o);
    ApplyPEC_H(RHSLoc2H, LocalDim);
    ApplyPMC_H(RHSLoc2H, LocalDim);

    *RHSCoup1H = *FijH;
    RHSCoup1H->scale(dt / mu_o);

    *RHSCoup2H = *BijH;
    RHSCoup2H->scale(dt / mu_o);

    

    // Inv
    LHSInvH = new denseMat<fp_t>(LocalDim, LocalDim);
    *LHSInvH = *MassH + RMatrixH;
    *LHSInvH = LHSInvH->inverse();

		// cout.precision(7);
		// if(cnt == 37 || cnt == 114){
		// 	cout << "Node 0 = (" << nd[0]->getCoord().getx() << ", " << nd[0]->getCoord().gety() << ", " << nd[0]->getCoord().getz() << ") " << endl;
		// 	cout << "Node 1 = (" << nd[1]->getCoord().getx() << ", " << nd[1]->getCoord().gety() << ", " << nd[1]->getCoord().getz() << ") " << endl;
		// 	cout << "Node 2 = (" << nd[2]->getCoord().getx() << ", " << nd[2]->getCoord().gety() << ", " << nd[2]->getCoord().getz() << ") " << endl;
		// 	cout << "Node 3 = (" << nd[3]->getCoord().getx() << ", " << nd[3]->getCoord().gety() << ", " << nd[3]->getCoord().getz() << ") " << endl;
		// 	cout << "cnt = " << cnt << endl;
		// 	for(int i1 = 0; i1 < TetPolyOrderDim[PolyOrderFlag]; i1++){
		// 		for(int i2 = 0; i2 < TetPolyOrderDim[PolyOrderFlag]; i2++){
					
		// 			cout << LHSInvH->getEntry(i1, i2) << " ";
		// 		}
		// 		cout << endl;
		// 	}cout << endl;
		// }

    MassH->Clear();
    StiffH->Clear();
    BiiH->Clear();
    FiiH->Clear();
    BijH->Clear();
    FijH->Clear();
}

/* H setup Numeric */
void tetra::SetUp_LocalFaceToTetraMapH_NMF1_Numeric(fp_t dt) {
    if (LocalHDOF == 0)
        return;
    int LocalDim, LocalFaceDim;
    fp_t mu_o = Uo;

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
		
    // sigma and abc
    denseMat<fp_t> RMatrixH(LocalDim, LocalDim);
		Calculate_R_Matrix_H_Numeric(&RMatrixH, dt);

    RHSLoc1H = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSLoc2H = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSCoup1H = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
    RHSCoup2H = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);

    BiiH->scale(dt / mu_o);
	
    *RHSLoc1H = *MassH + *BiiH - RMatrixH;
    ApplyPMC(RHSLoc1H, LocalDim);

    *RHSLoc2H = *FiiH - *StiffH;
    RHSLoc2H->scale(dt / mu_o);
    ApplyPEC_H(RHSLoc2H, LocalDim);
    ApplyPMC_H(RHSLoc2H, LocalDim);

    *RHSCoup1H = *FijH;
    RHSCoup1H->scale(dt / mu_o);

    *RHSCoup2H = *BijH;
    RHSCoup2H->scale(dt / mu_o);

    // Inv
    LHSInvH = new denseMat<fp_t>(LocalDim, LocalDim);
    *LHSInvH = *MassH + RMatrixH;
    *LHSInvH = LHSInvH->inverse();

		// cout.precision(7);
		// if(cnt == 37 || cnt == 114){
		// 	cout << "Node 0 = (" << nd[0]->getCoord().getx() << ", " << nd[0]->getCoord().gety() << ", " << nd[0]->getCoord().getz() << ") " << endl;
		// 	cout << "Node 1 = (" << nd[1]->getCoord().getx() << ", " << nd[1]->getCoord().gety() << ", " << nd[1]->getCoord().getz() << ") " << endl;
		// 	cout << "Node 2 = (" << nd[2]->getCoord().getx() << ", " << nd[2]->getCoord().gety() << ", " << nd[2]->getCoord().getz() << ") " << endl;
		// 	cout << "Node 3 = (" << nd[3]->getCoord().getx() << ", " << nd[3]->getCoord().gety() << ", " << nd[3]->getCoord().getz() << ") " << endl;
		// 	cout << "cnt = " << cnt << endl;
		// 	for(int i1 = 0; i1 < TetPolyOrderDim[PolyOrderFlag]; i1++){
		// 		for(int i2 = 0; i2 < TetPolyOrderDim[PolyOrderFlag]; i2++){
					
		// 			cout << LHSInvH->getEntry(i1, i2) << " ";
		// 		}
		// 		cout << endl;
		// 	}cout << endl;
		// }

    MassH->Clear();
    StiffH->Clear();
    BiiH->Clear();
    FiiH->Clear();
    BijH->Clear();
    FijH->Clear();
}

// The functions following are mainly all the update functions.
// The ones named _NMF correspond to the non matrix free implementation option.
// The ones named _BLAS are for the matrix-free option.

void tetra::LocalFaceToTetraMapE_NMF1(ArrayFP<fp_t>& En_1, ArrayFP<fp_t>& En, ArrayFP<fp_t>& Hn_12, fp_t dt, fp_t t) {
	if (LocalEDOF == 0)
		return;

	int i;
	int LocalDim, LocalFaceDim, NeighFaceDim;
	int DGface_bc;
	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	ArrayFP<fp_t> work(LocalDim);

	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();

		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType) {
			neigh = fc[i]->neighborTETRA(this);
			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;

			int neFac;

			if (neigh != nullptr && (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType)) {
				NeighFaceDim = FacePolyOrderDim[neigh->PolyOrderFlag];
				ArrayFP<fp_t> nei_e(NeighFaceDim);
				ArrayFP<fp_t> nei_h(NeighFaceDim);

				assert(NeighFaceDim == LocalFaceDim);
				for (int m = 0; m < NumOfFaces; m++) {
					if (neigh->getFacePtr(m) == fc[i])
						neFac = m;
				}

				for (int l = 0; l < NeighFaceDim; l++) {
					if (neigh->LocMapE[fac2tet[neFac][l]] >= 0)
						nei_e[l] = En[neigh->LocMapE[fac2tet[neFac][l]]];
					else
						nei_e[l] = 0.0;
						
					if (neigh->LocMapH[fac2tet[neFac][l]] >= 0)
						nei_h[l] = Hn_12[neigh->LocMapH[fac2tet[neFac][l]]];
					else
						nei_h[l] = 0.0;
				}


				denseMat<fp_t> RHSCoup1E_aux(LocalDim, NeighFaceDim);
				denseMat<fp_t> RHSCoup2E_aux(LocalDim, NeighFaceDim);
				
				for(int l = 0; l < LocalDim; l++){
					for(int k = 0; k < NeighFaceDim; k++){
						RHSCoup1E_aux.setEntry(l, k, RHSCoup1E->getEntry(l, i * NeighFaceDim + k));
						RHSCoup2E_aux.setEntry(l, k, RHSCoup2E->getEntry(l, i * NeighFaceDim + k));
					}
				}
				ArrayFP<fp_t> work_aux(LocalDim);
				Ax(RHSCoup2E_aux, nei_e, work_aux);
				Axpy(RHSCoup1E_aux, nei_h, work_aux);

				for (int l = 0; l < LocalDim; l++)
					work[l] += work_aux[l];
			}
		} 
    else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {} 
		else if (DGface_bc == pecType) {} 
		else if (DGface_bc == pmcType) {}
		else if (DGface_bc == nonConformal){
			int neighFaceIdx = 0;
			fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			fp_t Y = 1.0 / Z;
			fp_t Zneigh = 0.0;
			fp_t Yt = 0.0;
			for (int idx = 0 ; idx < NeighNCNum ; idx++){
				if (OverlapMapVec[i].find(idx)->second){
          if(IntersectionMapVec[i].find(idx)->second){
						neigh = NeighNCTetra[idx];
						NeighFaceDim = FacePolyOrderDim[neigh->PolyOrderFlag];
						neighFaceIdx = IntersectionFaceMapVec[i].find(idx)->second;
						
						Zneigh = (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
						Yt = 1.0 / (0.5 * (Z + Zneigh));

						ArrayFP<fp_t> nei_e(NeighFaceDim);
						ArrayFP<fp_t> nei_h(NeighFaceDim);

						for (int l = 0; l < NeighFaceDim; l++) {
							if (neigh->LocMapE[fac2tet[neighFaceIdx][l]] >= 0)
								nei_e[l] = En[neigh->LocMapE[fac2tet[neighFaceIdx][l]]];
							else
								nei_e[l] = 0.0;
								
							if (neigh->LocMapH[fac2tet[neighFaceIdx][l]] >= 0)
								nei_h[l] = Hn_12[neigh->LocMapH[fac2tet[neighFaceIdx][l]]];
							else
								nei_h[l] = 0.0;
						}
						denseMat<fp_t> RHSCoup1E_aux(LocalDim, NeighFaceDim);
						denseMat<fp_t> RHSCoup2E_aux(LocalDim, NeighFaceDim);
						
						for (int l = 0; l < LocalFaceDim; l++){
							for (int k = 0; k < NeighFaceDim; k++){
                RHSCoup1E_aux.setEntry(l, k, (dt / Eo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(l, k));
								RHSCoup2E_aux.setEntry (l, k, -(dt / Eo) * 0.5 * Yt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(l, k) * GAMMA);
							}
						} 

						ArrayFP<fp_t> work_aux(LocalFaceDim);
						Ax(RHSCoup2E_aux, nei_e, work_aux);
						Axpy(RHSCoup1E_aux, nei_h, work_aux);

						for (int l = 0; l < LocalFaceDim; l++)
							if (LocMapE[fac2tet[i][l]] >=0)
								work[fac2tet[i][l]] += work_aux[l];
					}
				}
			}
		}
	}
	ArrayFP<fp_t> loc_e(LocalDim);
	ArrayFP<fp_t> loc_h(LocalDim);
	// get vector entries
	for (i = 0; i < LocalDim; i++) {
		if (LocMapE[i] >= 0)
			loc_e[i] = En[LocMapE[i]];
		else
			loc_e[i] = 0.0;

    if (LocMapH[i] >= 0)
			loc_h[i] = Hn_12[LocMapH[i]];
		else
			loc_h[i] = 0.0;
	}


	Axpy(*RHSLoc1E, loc_e, work); // 1st local term
	Axpy(*RHSLoc2E, loc_h, work); // 2nd local term

	if (ExcitationFlag)
		Excitation_E_BLAS(En_1, t, dt, work); // excitation (PW or port)

	Ax(*LHSInvE, work, loc_e);

	// add back into y with mapping
	for (i = 0; i < LocalDim; i++) {
		if (LocMapE[i] >= 0)
			En_1[LocMapE[i]] = loc_e[i];
	}
	
}

void tetra::LocalFaceToTetraMapH_NMF1(ArrayFP<fp_t>& Hn_32, ArrayFP<fp_t>& En_1, ArrayFP<fp_t>& Hn_12, fp_t dt, fp_t t) {
	if (LocalHDOF == 0)
		return;
	int i;
	int LocalDim, LocalFaceDim, NeighFaceDim;
	int DGface_bc;
	tetra* neigh;

	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	ArrayFP<fp_t> work(LocalDim);

	for (i = 0; i < NumOfFaces; i++) {
		DGface_bc = fc[i]->bcPtr->getbType();
		if (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType) {
			neigh = fc[i]->neighborTETRA(this);

			if (neigh == nullptr)
				cout << "ERROR: Null pointer at bType = 0" << endl;

			int neFac;

			if (neigh != nullptr && (DGface_bc == 0 || DGface_bc == outputSurfType || DGface_bc == fieldPlaneType)) {
				NeighFaceDim = FacePolyOrderDim[neigh->PolyOrderFlag];
				ArrayFP<fp_t> nei_e(NeighFaceDim);
				ArrayFP<fp_t> nei_h(NeighFaceDim);

				assert(NeighFaceDim == LocalFaceDim);
				for (int m = 0; m < NumOfFaces; m++) {
					if (neigh->getFacePtr(m) == fc[i])
						neFac = m;
				}

				for (int l = 0; l < NeighFaceDim; l++) {
					if (neigh->LocMapE[fac2tet[neFac][l]] >= 0)
						nei_e[l] = En_1[neigh->LocMapE[fac2tet[neFac][l]]];
					else
						nei_e[l] = 0.0;

					if (neigh->LocMapH[fac2tet[neFac][l]] >= 0)
						nei_h[l] = Hn_12[neigh->LocMapH[fac2tet[neFac][l]]];
					else
						nei_h[l] = 0.0;
				}
				
				denseMat<fp_t> RHSCoup1H_aux(LocalDim, NeighFaceDim);
				denseMat<fp_t> RHSCoup2H_aux(LocalDim, NeighFaceDim);
				
				for(int l = 0; l < LocalDim; l++){
					for(int k = 0; k < NeighFaceDim; k++){
						RHSCoup1H_aux.setEntry(l, k, RHSCoup1H->getEntry(l, i * NeighFaceDim + k));
						RHSCoup2H_aux.setEntry(l, k, RHSCoup2H->getEntry(l, i * NeighFaceDim + k));
					}
				}

				ArrayFP<fp_t> work_aux(LocalDim);
				Ax(RHSCoup2H_aux, nei_h, work_aux);
				Axpy(RHSCoup1H_aux, nei_e, work_aux);

				for (int l = 0; l < LocalDim; l++)
					work[l] += work_aux[l];
			}
		} else if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
		} else if (DGface_bc == pecType) {
		} else if (DGface_bc == pmcType) {
		} else if (DGface_bc == nonConformal){
			int neighFaceIdx = 0;
			fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			fp_t Y = 1.0 / Z;
			fp_t Yneigh = 0.0;
			fp_t Zt = 0.0;
			for (int idx = 0 ; idx < NeighNCNum ; idx++){
				if (OverlapMapVec[i].find(idx)->second){
					if(IntersectionMapVec[i].find(idx)->second){
						neigh = NeighNCTetra[idx];
						NeighFaceDim = FacePolyOrderDim[neigh->PolyOrderFlag];
						neighFaceIdx = IntersectionFaceMapVec[i].find(idx)->second;

						Yneigh = 1.0 / (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
						Zt = 1.0 / (0.5 * (Y + Yneigh));
						
						ArrayFP<fp_t> nei_e(NeighFaceDim);
						ArrayFP<fp_t> nei_h(NeighFaceDim);

						for (int l = 0; l < NeighFaceDim; l++) {
							if (neigh->LocMapE[fac2tet[neighFaceIdx][l]] >= 0)
								nei_e[l] = En_1[neigh->LocMapE[fac2tet[neighFaceIdx][l]]];
							else
								nei_e[l] = 0.0;
								
							if (neigh->LocMapH[fac2tet[neighFaceIdx][l]] >= 0)
								nei_h[l] = Hn_12[neigh->LocMapH[fac2tet[neighFaceIdx][l]]];
							else
								nei_h[l] = 0.0;
						}

						denseMat<fp_t> RHSCoup1H_aux(LocalDim, NeighFaceDim);
						denseMat<fp_t> RHSCoup2H_aux(LocalDim, NeighFaceDim);

						for (int l = 0; l < LocalFaceDim; l++){
							for (int k = 0; k < NeighFaceDim; k++){
								RHSCoup1H_aux.setEntry(l, k, -(dt / Uo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(l, k));
								RHSCoup2H_aux.setEntry (l, k, -(dt / Uo) * 0.5 * Zt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(l, k) * GAMMA);
							}
						} 

						ArrayFP<fp_t> work_aux(LocalFaceDim);
						Ax(RHSCoup2H_aux, nei_h, work_aux);
						Axpy(RHSCoup1H_aux, nei_e, work_aux);

						for (int l = 0; l < LocalFaceDim; l++)
							if (LocMapH[fac2tet[i][l]] >=0)
								work[fac2tet[i][l]] += work_aux[l];
					}
				}
			}
		}
	}

	ArrayFP<fp_t> loc_e(LocalDim);
	ArrayFP<fp_t> loc_h(LocalDim);

	// get vector entries
	for (i = 0; i < LocalDim; i++) {
		if (LocMapE[i] >= 0)
			loc_e[i] = En_1[LocMapE[i]];
		else
			loc_e[i] = 0.0;

		if (LocMapH[i] >= 0)
			loc_h[i] = Hn_12[LocMapH[i]];
		else
			loc_h[i] = 0.0;    
	}

	Axpy(*RHSLoc1H, loc_h, work);
	Axpy(*RHSLoc2H, loc_e, work);
	if (ExcitationFlag)
		Excitation_H_BLAS(Hn_32, t, dt, work);
	Ax(*LHSInvH, work, loc_h);

	// add back to Hn_32
	for (i = 0; i < LocalDim; i++) {
		if (LocMapH[i] >= 0)
			Hn_32[LocMapH[i]] = loc_h[i];
	}
}

/* Excitations */
void tetra::Excitation_E_BLAS(ArrayFP<fp_t>& En_1, fp_t t, fp_t dt, ArrayFP<fp_t>& work2) {
	if (LocalEDOF == 0)
		return;
	if (t < 0.)
		return;
	int i;
	int LocalDim;
	fp_t Y;
	fp_t eps_o = Eo;
	ArrayFP<fp_t>* nxh;
	ArrayFP<fp_t>* nxnxe;

	// excitation
	LocalDim = TetPolyOrderDim[PolyOrderFlag];

	nxh = new ArrayFP<fp_t>(LocalDim);
	nxnxe = new ArrayFP<fp_t>(LocalDim);

	// Check for excitation face
	for (i = 0; i < NumOfFaces; i++) {
		if (fc[i]->bcPtr->getbType() == planeWaveType) {
			Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
		} else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
			Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();
		}
	}

	nxh->reset();
	nxnxe->reset();

	/*  since the calling already has if(excitationflag==1), what is the purpose
		* of this if ?    for PML ?        */
	if (ExcitationFlag) {
		set_einc(t, nxh, nxnxe);

		nxh->scale(dt / eps_o); 
		nxh->axpy(-Y * (dt / eps_o), *nxnxe);
		ApplyPEC_Vector(nxh, LocalDim);
	}

	for (int l = 0; l < LocalDim; l++)
		if (LocMapE[l] >= 0)
			work2[l] += nxh->getentry(l);

	// add PML part
	delete nxh;
	delete nxnxe;
}

void tetra::Excitation_H_BLAS(ArrayFP<fp_t>& Hn_32, fp_t t, fp_t dt, ArrayFP<fp_t>& work2) {
    if (LocalHDOF == 0)
			return;
    if (t < 0.)
			return;
    int i;
    int LocalDim;
    fp_t Z;
    fp_t mu_o = Uo;
    ArrayFP<fp_t>* nxe;
    ArrayFP<fp_t>* nxnxh;

    // excitation
    LocalDim = TetPolyOrderDim[PolyOrderFlag];

    nxe = new ArrayFP<fp_t>(LocalDim);
    nxnxh = new ArrayFP<fp_t>(LocalDim);

    // Check for excitation face
    for (i = 0; i < NumOfFaces; i++) {
			if (fc[i]->bcPtr->getbType() == planeWaveType)
				Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType)
				Z = fc[i]->bcPtr->get_PortImpVal();
    }

    nxe->reset();
    nxnxh->reset();

    if (ExcitationFlag) {
			set_hinc(t, nxe, nxnxh);

			nxe->scale(-(dt / mu_o));
			nxe->axpy(-Z * (dt / mu_o), *nxnxh);
			ApplyPMC_Vector(nxe, LocalDim);
    }
    for (int l = 0; l < LocalDim; l++)
			if (LocMapH[l] >= 0)
				work2[l] += nxe->getentry(l);

    // add PML part
    delete nxe;
    delete nxnxh;
}

void tetra::setEHGlobalMap(int i, int ide, int idh) {
    EdofMap[i] = ide;
    HdofMap[i] = idh;
}

void tetra::allocDofMap() {
    EdofMap = new int[TetPolyOrderDim[PolyOrderFlag]];
    HdofMap = new int[TetPolyOrderDim[PolyOrderFlag]];
    for (int i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
        EdofMap[i] = NOT_NUMBERED;
        HdofMap[i] = NOT_NUMBERED;
    }
}

void tetra::freeDofMap() {
    //   cout<<"Calling freeDofMap"<<endl;
    if (EdofMap != 0)
        delete[] EdofMap;

    EdofMap = 0;

    if (HdofMap != 0)
        delete[] HdofMap;

    HdofMap = 0;
}

int* tetra::get_NeighMapsE() {
	int FaceNum = 4;
	int NeighFace = 0;
	int MapCol = FacePolyOrderDim[PolyOrderFlag] * 2;
	int MapDim = MapCol * FaceNum;
	int Offset = FacePolyOrderDim[PolyOrderFlag] * 2;
	int* NeighMaps = (int*)malloc(sizeof(int) * MapDim);
	tetra* neigh;

	for (int i = 0; i < FaceNum; i++) {
		for (int j = 0; j < MapCol; j++) {
			NeighMaps[i * MapCol + j] = -1;
		}
	}

	for (int i = 0; i < FaceNum; i++) {
		neigh = fc[i]->neighborTETRA(this);
		if (fc[i]->getbType() == 0 || fc[i]->getbType()) {
			if (neigh != nullptr) {
				for (int m = 0; m < FaceNum; m++) {
					if (neigh->getFacePtr(m) == fc[i]) {
						NeighFace = m;
					}
				}
				for (int j = 0; j < FacePolyOrderDim[PolyOrderFlag]; j++) {
					NeighMaps[i * Offset + j] = neigh->LocMapH[fac2tet[NeighFace][j]];
					NeighMaps[i * Offset + FacePolyOrderDim[PolyOrderFlag] + j] = neigh->LocMapE[fac2tet[NeighFace][j]];
				}
			}
		}
	}
	return NeighMaps;
}

int* tetra::get_NeighMapsH() {
	int FaceNum = 4;
	int NeighFace = 0;
	int MapCol = FacePolyOrderDim[PolyOrderFlag] * 2;
	int MapDim = MapCol * 4;
	int Offset = FacePolyOrderDim[PolyOrderFlag] * 2;
	int* NeighMaps = (int*)malloc(sizeof(int) * (MapDim));
	tetra* neigh;

	for (int i = 0; i < FaceNum; i++)
		for (int j = 0; j < MapCol; j++)
			NeighMaps[i * MapCol + j] = -1;

	for (int i = 0; i < FaceNum; i++) {
		neigh = fc[i]->neighborTETRA(this);
		if (fc[i]->getbType() == 0 || fc[i]->getbType()) {
			if (neigh != nullptr) {
				for (int m = 0; m < FaceNum; m++) {
					if (neigh->getFacePtr(m) == fc[i]) {
						NeighFace = m;
					}
				}
				for (int j = 0; j < FacePolyOrderDim[PolyOrderFlag]; j++) {
					NeighMaps[i * Offset + j] = neigh->LocMapE[fac2tet[NeighFace][j]];
					NeighMaps[i * Offset + FacePolyOrderDim[PolyOrderFlag] + j] = neigh->LocMapH[fac2tet[NeighFace][j]];
				}
			}
		}
	}
	return NeighMaps;
}

void tetra::SetUpInterfaceMatrices_NC(){
	if(NeighNCNum <= 0) 
		return;

	int neighFaceIdx = -1;
	int tcount = 0;
	bool Overlap = false;
	int LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
	face* IntersectionFaceArray;
	face* neighborFace;

	// Allocate memory to pre-compute and store 
	// the non-conformal coupling matrices
	NC_CouplingMatArray = new denseMat<fp_t>[NumCouplingMatrices * NeighNCNum];

	for (int i = 0 ; i < NeighNCNum ; i++){
		NC_CouplingMatArray[i * NumCouplingMatrices + 0].setDim(LocalFaceDim, FacePolyOrderDim[PolyOrderFlag]);
		NC_CouplingMatArray[i * NumCouplingMatrices + 1].setDim(LocalFaceDim, FacePolyOrderDim[NeighNCTetra[i]->PolyOrderFlag]);
		NC_CouplingMatArray[i * NumCouplingMatrices + 2].setDim(LocalFaceDim, FacePolyOrderDim[NeighNCTetra[i]->PolyOrderFlag]);
	}
	denseMat<fp_t> Beii_aux(LocalFaceDim, LocalFaceDim);
	
		
	for(int i = 0 ; i < NumOfFaces; i++){
		// non-conformal face
		if (fc[i]->getbType() == nonConformal){ 
			// get local non-conformal face
			for (int idx = 0 ; idx < NeighNCNum ; idx++){
				Overlap = false;
				// Find the face of the neigh tet that couples to the localFace
				for(int faceIdx = 0; faceIdx < NumOfFaces; faceIdx++){
					neighborFace = NeighNCTetra[idx]->fc[faceIdx];
					if (neighborFace->getbType() == nonConformal){
						if(triangle_overlaps_triangle(fc[i], neighborFace)){
							if(CheckIntersection(fc[i], NeighNCTetra[idx]->fc[faceIdx])){								
								neighFaceIdx = faceIdx;
								Overlap = true;
							}
						}
					}           
				}
				if(Overlap){
					IntersectionFaceArray = CreateInterSectionMesh(fc[i], NeighNCTetra[idx]->fc[neighFaceIdx], tcount);
					if(IntersectionFaceArray != NULL){
						// local Beii and Feii
						fc[i]->Calculate_w_w_Matrix_NC(IntersectionFaceArray, tcount, fc[i], fc[i], &Beii_aux, PolyOrderFlag);
						for (int row = 0; row < Beii_aux.getRowDim(); row++){
							for (int col = 0; col < Beii_aux.getColDim(); col++){
								NC_CouplingMatArray[idx * NumCouplingMatrices + 0].setEntry(row, col, Beii_aux.getEntry(row, col)); 
							}
						}
						
						// Coupling Feij, Beij
						denseMat<fp_t> Feij_aux, Beij_aux;
						Feij_aux.setDim(LocalFaceDim, FacePolyOrderDim[NeighNCTetra[idx]->PolyOrderFlag]);
						Beij_aux.setDim(LocalFaceDim, FacePolyOrderDim[NeighNCTetra[idx]->PolyOrderFlag]);
						int auxPoly = (PolyOrderFlag > NeighNCTetra[idx]->PolyOrderFlag) ? PolyOrderFlag : NeighNCTetra[idx]->PolyOrderFlag;
						fc[i]->Calculate_w_nxw_Matrix_NC(IntersectionFaceArray, tcount, fc[i], NeighNCTetra[idx]->fc[neighFaceIdx], &Feij_aux, this, auxPoly);
						fc[i]->Calculate_w_w_Matrix_NC(IntersectionFaceArray, tcount, fc[i], NeighNCTetra[idx]->fc[neighFaceIdx] , &Beij_aux, auxPoly);
						for (int row = 0; row < Beij_aux.getRowDim(); row++){
							for (int col = 0; col < Beij_aux.getColDim(); col++){
								NC_CouplingMatArray[idx * NumCouplingMatrices + 1].setEntry(row, col, Feij_aux.getEntry(row, col)); 
								NC_CouplingMatArray[idx * NumCouplingMatrices + 2].setEntry(row, col, Beij_aux.getEntry(row, col));
							}
						} 
					}
					// free the memory
					if(IntersectionFaceArray != NULL) 
						delete [] IntersectionFaceArray;
				}
			}
		}
	}
}

void tetra::SetUpInterfaceMaps_NC(){
	if (NeighNCNum <= 0) 
		return;

	int neighFaceIdx = -1;
	int tcount = 0;
	bool Overlap = false;
	face* IntersectionFaceArray;
	face* neighborFace;
	
	map<int, bool> OverlapMap;
	map<int, bool> IntersectionMap;
	map<int, int> IntersectionFaceMap;
	vector<map<int, bool>>::iterator it2;
	vector<map<int, int>>::iterator it3;

 	for(int i = 0; i < 4; i++){
    OverlapMap.clear();
    IntersectionMap.clear();
    IntersectionFaceMap.clear();
    // non-conformal face
    if(fc[i]->getbType() == nonConformal){ 
      // get local non-conformal face
      for(int idx = 0 ; idx < NeighNCNum; idx++){
        Overlap = false;
        // Find the face of the neigh tet that couples to the localFace
        for(int faceIdx = 0; faceIdx < 4; faceIdx++){
					neighborFace = NeighNCTetra[idx]->fc[faceIdx];
					if (neighborFace->getbType() == nonConformal){
						if(triangle_overlaps_triangle(fc[i], neighborFace)){
							if (CheckIntersection(fc[i], NeighNCTetra[idx]->fc[faceIdx])){
								neighFaceIdx = faceIdx;
								Overlap = true;
							}
						}
          }           
        }
        OverlapMap[idx] = Overlap;
        IntersectionMap[idx] = false;
        if(Overlap){
          IntersectionFaceArray = CreateInterSectionMesh(fc[i], NeighNCTetra[idx]->fc[neighFaceIdx], tcount);
					if(IntersectionFaceArray != NULL){
						IntersectionMap[idx] = true;
						IntersectionFaceMap[idx] = neighFaceIdx;
          }
          if(IntersectionFaceArray != NULL) 
						delete [] IntersectionFaceArray;
        }
      }
    }
    // Add the maps to a vector
    it2 = OverlapMapVec.begin();
    OverlapMapVec.insert(it2 + i, OverlapMap);
    it2 = IntersectionMapVec.begin();
    IntersectionMapVec.insert(it2 + i, IntersectionMap);
    it3 = IntersectionFaceMapVec.begin();
    IntersectionFaceMapVec.insert(it3 + i, IntersectionFaceMap);
  }
}

bool tetra::CheckIntersection(face* LocalFace, face* NeighFace){
  double tri2Coords[6],tri1Coords[6];
  char* plane;
  int tcount = 0; 
  double thridCoord = 0.;

  // Non-Planar conformal case
  for (int i = 0 ; i < 4; i++){
    if (triangle_overlaps_triangleCO(fc[i], NeighFace)){
      return true; 
    }
  }

  plane = NeighFace->ReturnIntferfacePlaneCoord(tri2Coords, thridCoord);

  int PlaneID;
   if(0 == strcmp(plane, "yz")) 
    PlaneID = 0;
   if(0 == strcmp(plane, "xz")) 
    PlaneID = 1;
   if(0 == strcmp(plane, "xy")) 
    PlaneID = 2;
   
   LocalFace->ReturnIntferfacePlaneCoord(tri1Coords, thridCoord, PlaneID);
   NeighFace->ReturnIntferfacePlaneCoord(tri2Coords, thridCoord, PlaneID);
    Paths subj(1),clip(1), solution;

    subj[0]<< IntPoint(cInt(tri1Coords[0] * scaleFact),cInt(tri1Coords[1] * scaleFact))
           << IntPoint(cInt(tri1Coords[2] * scaleFact),cInt(tri1Coords[3] * scaleFact))
           << IntPoint(cInt(tri1Coords[4] * scaleFact),cInt(tri1Coords[5] * scaleFact));
	  
    clip[0]<< IntPoint(cInt(tri2Coords[0] * scaleFact),cInt(tri2Coords[1] * scaleFact))
           << IntPoint(cInt(tri2Coords[2] * scaleFact),cInt(tri2Coords[3] * scaleFact))
           << IntPoint(cInt(tri2Coords[4] * scaleFact),cInt(tri2Coords[5] * scaleFact));
	   
    Clipper c;
    c.AddPaths(subj, ptSubject, true);
    c.AddPaths(clip, ptClip, true);  	
    Vector2dVector TriangulatedPolygon;
    if(c.Execute(ctIntersection, solution, pftNonZero, pftNonZero)){
      if(solution.size() > 0){
        if(solution[0].size() >= 3){
          Path& path = solution[0];  
          for (int t = 0 ; t < path.size() - 2;t++){
            TriangulatedPolygon.push_back(Vector2d(double(path[0].X) * scaleFactinv, double(path[0].Y) * scaleFactinv));
            TriangulatedPolygon.push_back(Vector2d(double(path[t + 1].X) * scaleFactinv, double(path[t + 1].Y) * scaleFactinv));
            TriangulatedPolygon.push_back(Vector2d(double(path[t + 2].X) * scaleFactinv, double(path[t + 2].Y) * scaleFactinv));
          }
	      }
      }
    }else{
      return false;
    }

    tcount = TriangulatedPolygon.size() / 3;

    if(tcount == 0) 
      return false; 
  
    vtr nd0, nd1, nd2;
    face IntersecFace;
    fp_t sik = 0.0;
    fp_t area = 0.0;
  
    for (int tri = 0; tri < tcount; tri++){
      if (strcmp(plane, "yz") == 0){
        nd0.setvtr(thridCoord, TriangulatedPolygon[tri * 3 + 0].GetX(), TriangulatedPolygon[tri * 3 + 0].GetY());
        nd1.setvtr(thridCoord, TriangulatedPolygon[tri * 3 + 1].GetX(), TriangulatedPolygon[tri * 3 + 1].GetY());
        nd2.setvtr(thridCoord, TriangulatedPolygon[tri * 3 + 2].GetX(), TriangulatedPolygon[tri * 3 + 2].GetY());
      }
      else if (strcmp(plane, "xz") == 0){
        nd0.setvtr(TriangulatedPolygon[tri * 3 + 0].GetX(), thridCoord, TriangulatedPolygon[tri * 3 + 0].GetY());
        nd1.setvtr(TriangulatedPolygon[tri * 3 + 1].GetX(), thridCoord, TriangulatedPolygon[tri * 3 + 1].GetY());
        nd2.setvtr(TriangulatedPolygon[tri * 3 + 2].GetX(), thridCoord, TriangulatedPolygon[tri * 3 + 2].GetY());
      }
      else if (strcmp(plane, "xy") == 0){
        nd0.setvtr(TriangulatedPolygon[tri * 3 + 0].GetX(), TriangulatedPolygon[tri * 3 + 0].GetY(), thridCoord);
        nd1.setvtr(TriangulatedPolygon[tri * 3 + 1].GetX(), TriangulatedPolygon[tri * 3 + 1].GetY(), thridCoord);
        nd2.setvtr(TriangulatedPolygon[tri * 3 + 2].GetX(), TriangulatedPolygon[tri * 3 + 2].GetY(), thridCoord);
      }
          
      IntersecFace.setCoordOfFaceNodes(nd0, nd1, nd2);
      IntersecFace.getFaceArea(&area);
      sik += area;
    }
    
    TriangulatedPolygon.clear();
    
    if(sik == 0.0)
      return false;  

  return true;
}

face* tetra::CreateInterSectionMesh(face* LocalFace, face* NeighFace, int& tcount){
  double tri1Coords[6];
  double tri2Coords[6];
  char* plane;
  double thridCoord = 0.0;

  // Non-Planar conformal case
  for (int i = 0 ; i < 4; i++){
    if (triangle_overlaps_triangleCO(fc[i], NeighFace)){
      tcount = 1;
      face* IntersectionFaceArray = new face[tcount];
      IntersectionFaceArray[0].setCoordOfFaceNodes(fc[i]->getNodePtr(0)->getCoord(),
                                                   fc[i]->getNodePtr(1)->getCoord(), 
                                                   fc[i]->getNodePtr(2)->getCoord());
      return IntersectionFaceArray; 
    }
  }

  plane = NeighFace->ReturnIntferfacePlaneCoord(tri2Coords, thridCoord);
    ///// new version
  
  int PlaneID;
  if(0 == strcmp(plane, "yz")) 
  	PlaneID = 0;
  if(0 == strcmp(plane, "xz")) 
  	PlaneID = 1;
  if(0 == strcmp(plane, "xy")) 
  	PlaneID = 2;
  
  LocalFace->ReturnIntferfacePlaneCoord(tri1Coords, thridCoord, PlaneID);
  NeighFace->ReturnIntferfacePlaneCoord(tri2Coords, thridCoord, PlaneID);
  Paths subj(1),clip(1), solution;

  subj[0]<< IntPoint(cInt(tri1Coords[0] * scaleFact),cInt(tri1Coords[1] * scaleFact))
         << IntPoint(cInt(tri1Coords[2] * scaleFact),cInt(tri1Coords[3] * scaleFact))
         << IntPoint(cInt(tri1Coords[4] * scaleFact),cInt(tri1Coords[5] * scaleFact));
  
  clip[0]<< IntPoint(cInt(tri2Coords[0] * scaleFact),cInt(tri2Coords[1] * scaleFact))
         << IntPoint(cInt(tri2Coords[2] * scaleFact),cInt(tri2Coords[3] * scaleFact))
         << IntPoint(cInt(tri2Coords[4] * scaleFact),cInt(tri2Coords[5] * scaleFact));
    
  Clipper c;
  c.AddPaths(subj, ptSubject, true);
  c.AddPaths(clip, ptClip, true);  	
  Vector2dVector TriangulatedPolygon;
  if(c.Execute(ctIntersection, solution, pftNonZero, pftNonZero)){
    if(solution.size() > 0){
      if(solution[0].size() >= 3){
        Path& path = solution[0];  
        for (int t = 0 ; t < path.size() - 2; t++){
          TriangulatedPolygon.push_back(Vector2d(double(path[0].X) * scaleFactinv, double(path[0].Y) * scaleFactinv));
          TriangulatedPolygon.push_back(Vector2d(double(path[t + 1].X) * scaleFactinv, double(path[t + 1].Y) * scaleFactinv));
          TriangulatedPolygon.push_back(Vector2d(double(path[t + 2].X) * scaleFactinv, double(path[t + 2].Y) * scaleFactinv));
        }
      }
    }
  }else{
    return NULL;
  }

  tcount = TriangulatedPolygon.size() / 3;
  
  if(tcount == 0)
    return NULL;
  
  face* IntersectionFaceArray = new face[tcount];
  vtr nd0, nd1, nd2;

  for(int tri = 0; tri < tcount; tri++){
    if (strcmp(plane, "yz") == 0){
      nd0.setvtr(thridCoord, TriangulatedPolygon[tri * 3 + 0].GetX(), TriangulatedPolygon[tri * 3 + 0].GetY());
      nd1.setvtr(thridCoord, TriangulatedPolygon[tri * 3 + 1].GetX(), TriangulatedPolygon[tri * 3 + 1].GetY());
      nd2.setvtr(thridCoord, TriangulatedPolygon[tri * 3 + 2].GetX(), TriangulatedPolygon[tri * 3 + 2].GetY());
    }
    else if (strcmp(plane, "xz") == 0){
      nd0.setvtr(TriangulatedPolygon[tri * 3 + 0].GetX(), thridCoord, TriangulatedPolygon[tri * 3 + 0].GetY());
      nd1.setvtr(TriangulatedPolygon[tri * 3 + 1].GetX(), thridCoord, TriangulatedPolygon[tri * 3 + 1].GetY());
      nd2.setvtr(TriangulatedPolygon[tri * 3 + 2].GetX(), thridCoord, TriangulatedPolygon[tri * 3 + 2].GetY());
    }
    else if (strcmp(plane, "xy") == 0){
      nd0.setvtr(TriangulatedPolygon[tri * 3 + 0].GetX(), TriangulatedPolygon[tri * 3 + 0].GetY(), thridCoord);
      nd1.setvtr(TriangulatedPolygon[tri * 3 + 1].GetX(), TriangulatedPolygon[tri * 3 + 1].GetY(), thridCoord);
      nd2.setvtr(TriangulatedPolygon[tri * 3 + 2].GetX(), TriangulatedPolygon[tri * 3 + 2].GetY(), thridCoord);
    }
      
    IntersectionFaceArray[tri].setCoordOfFaceNodes(nd0, nd1, nd2);
  }
  TriangulatedPolygon.clear();

  return IntersectionFaceArray;
}

int tetra::getNeighFace(tetra* neigh){
	tetra* neigh_aux;
	int DGface_bc;
	if(!isNonConformal){
		for (int i = 0; i < NumOfFaces; i++){
			neigh_aux = fc[i]->neighborTETRA(this);
			if (neigh_aux != nullptr && neigh_aux == neigh){
				for (int m = 0; m < NumOfFaces; m++) {
					if (neigh->getFacePtr(m) == fc[i]) {
						return m;
					}
				}
			}
		}
	}else{
		for (int i = 0; i < NumOfFaces; i++){
			DGface_bc = fc[i]->bcPtr->getbType();
			if(DGface_bc == nonConformal){
				for (int idx = 0 ; idx < NeighNCNum ; idx++){
					if (neigh != nullptr && NeighNCTetra[idx] == neigh){
						if (OverlapMapVec[i].find(idx)->second){
          		if(IntersectionMapVec[i].find(idx)->second){
								return IntersectionFaceMapVec[i].find(idx)->second;
							}
						}
						// cout << "NEIGH Node 0 = (" << neigh->getNode(0)->getCoord().getx() << ", " << neigh->getNode(0)->getCoord().gety() << ", " << neigh->getNode(0)->getCoord().getz() << ") " << endl;
						// cout << "NEIGH Node 1 = (" << neigh->getNode(1)->getCoord().getx() << ", " << neigh->getNode(1)->getCoord().gety() << ", " << neigh->getNode(1)->getCoord().getz() << ") " << endl;
						// cout << "NEIGH Node 2 = (" << neigh->getNode(2)->getCoord().getx() << ", " << neigh->getNode(2)->getCoord().gety() << ", " << neigh->getNode(2)->getCoord().getz() << ") " << endl;
						// cout << "NEIGH Node 3 = (" << neigh->getNode(3)->getCoord().getx() << ", " << neigh->getNode(3)->getCoord().gety() << ", " << neigh->getNode(3)->getCoord().getz() << ") " << endl;
						// cout << "NEIGH cnt = " << neigh->getcnt() << endl;
					}
					// return 0;
					// cout << "Node 0 = (" << nd[0]->getCoord().getx() << ", " << nd[0]->getCoord().gety() << ", " << nd[0]->getCoord().getz() << ") " << endl;
					// cout << "Node 1 = (" << nd[1]->getCoord().getx() << ", " << nd[1]->getCoord().gety() << ", " << nd[1]->getCoord().getz() << ") " << endl;
					// cout << "Node 2 = (" << nd[2]->getCoord().getx() << ", " << nd[2]->getCoord().gety() << ", " << nd[2]->getCoord().getz() << ") " << endl;
					// cout << "Node 3 = (" << nd[3]->getCoord().getx() << ", " << nd[3]->getCoord().gety() << ", " << nd[3]->getCoord().getz() << ") " << endl;
					// cout << "cnt = " << cnt << endl;
					// cout << i << endl;
				}

			}else{
				neigh_aux = fc[i]->neighborTETRA(this);
				if (neigh_aux != nullptr && neigh_aux == neigh){
					for (int m = 0; m < NumOfFaces; m++) {
						if (neigh->getFacePtr(m) == fc[i]) {
							return m;
						}
					}
				}
			}
		}
	}
	cout << "Return -1 " << NeighNCNum << " " << cnt << endl;
	return -1;
}

void tetra::prepare_GPU(fp_t_ts* LHS_InvE_init, fp_t_ts* LHS_InvH_init, fp_t_ts* LE_Matrices_init, fp_t_ts* LH_Matrices_init, bool needInverse){
	int LocalDim = TetPolyOrderDim[PolyOrderFlag];
	int LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	denseMat<fp_t>* RHSCoup1E_aux;
	denseMat<fp_t>* RHSCoup2E_aux;
	denseMat<fp_t>* RHSLoc1E_aux;
	denseMat<fp_t>* RHSLoc2E_aux;

	if(LocalEDOF != 0){
		RHSCoup1E_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvE, *RHSCoup1E, *RHSCoup1E_aux);
		RHSCoup1E->Clear();
		
		RHSCoup2E_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvE, *RHSCoup2E, *RHSCoup2E_aux);
		RHSCoup2E->Clear();

		RHSLoc1E_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvE, *RHSLoc1E, *RHSLoc1E_aux);
		RHSLoc1E->Clear();

		RHSLoc2E_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvE, *RHSLoc2E, *RHSLoc2E_aux);
		RHSLoc2E->Clear();
	}

	denseMat<fp_t>* RHSCoup1H_aux;
	denseMat<fp_t>* RHSCoup2H_aux;
	denseMat<fp_t>* RHSLoc1H_aux;
	denseMat<fp_t>* RHSLoc2H_aux;
	if(LocalHDOF != 0){
		RHSCoup1H_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvH, *RHSCoup1H, *RHSCoup1H_aux);
		RHSCoup1H->Clear();

		RHSCoup2H_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvH, *RHSCoup2H, *RHSCoup2H_aux);
		RHSCoup2H->Clear();

		RHSLoc1H_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvH, *RHSLoc1H, *RHSLoc1H_aux);
		RHSLoc1H->Clear();

		RHSLoc2H_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvH, *RHSLoc2H, *RHSLoc2H_aux);
		RHSLoc2H->Clear();
	}

	int DGface_bc;
	tetra* neigh;
	int rowDim = TypeOfFields * LocalDim + TypeOfFields * NeighNum * LocalFaceDim;
	for(int i = 0; i < LocalDim; i++){
		for(int j = 0; j < LocalDim; j++){
			if(LocalEDOF != 0){
				if(needInverse)
					LHS_InvE_init[i * LocalDim + j] = LHSInvE->getEntry(i, j);
				LE_Matrices_init[i * rowDim + j] = RHSLoc1E_aux->getEntry(i, j);
				LE_Matrices_init[i * rowDim + LocalDim + j] = RHSLoc2E_aux->getEntry(i, j);
			}else{
				if(needInverse)
					LHS_InvE_init[i * LocalDim + j] = 0;
				LE_Matrices_init[i * rowDim + j] = 0;
				LE_Matrices_init[i * rowDim + LocalDim + j] = 0;
			}
			if(LocalHDOF != 0){
				if(needInverse)
					LHS_InvH_init[i * LocalDim + j] = LHSInvH->getEntry(i, j);
				LH_Matrices_init[i * rowDim + j] = RHSLoc1H_aux->getEntry(i, j);
				LH_Matrices_init[i * rowDim + LocalDim + j] = RHSLoc2H_aux->getEntry(i, j);			
			}else{
				if(needInverse)
					LHS_InvH_init[i * LocalDim + j] = 0;
				LH_Matrices_init[i * rowDim + j] = 0;
				LH_Matrices_init[i * rowDim + LocalDim + j] = 0;
			}
		}
	}
  if(LocalEDOF != 0){
		RHSLoc1E_aux->Clear();
		RHSLoc2E_aux->Clear();
	}

	if(LocalHDOF != 0){
		RHSLoc1H_aux->Clear();
		RHSLoc2H_aux->Clear();
	}

	int offset = 2 * LocalDim;
	fp_t dt = Class_dt;
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
	fp_t Y = 1.0 / Z;
	fp_t Zneigh = 0.0;
	fp_t Yt = 0.0;
	fp_t Yneigh = 0.0;
	fp_t Zt = 0.0;
	for(int i = 0; i < NeighNum; i++){
		DGface_bc = fc[NeighTetraFaceMap[i]]->bcPtr->getbType();
		if(DGface_bc != nonConformal){
			for(int j = 0; j < LocalDim; j++){
				for(int k = 0; k < LocalFaceDim; k++){
					if(LocalEDOF != 0){
						LE_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + k] = RHSCoup1E_aux->getEntry(j, NeighTetraFaceMap[i] * LocalFaceDim + k);
						LE_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + LocalFaceDim + k] = RHSCoup2E_aux->getEntry(j, NeighTetraFaceMap[i] * LocalFaceDim + k);
					}else{
						LE_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + k] = 0.0;
						LE_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + LocalFaceDim + k] = 0.0;
					}
					if(LocalHDOF != 0){
						LH_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + k] = RHSCoup1H_aux->getEntry(j, NeighTetraFaceMap[i] * LocalFaceDim + k);
						LH_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + LocalFaceDim + k] = RHSCoup2H_aux->getEntry(j, NeighTetraFaceMap[i] * LocalFaceDim + k);
					}else{
						LH_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + k] = 0;
						LH_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + LocalFaceDim + k] = 0;
					}

				}
			}
		}else{
			for (int idx = 0 ; idx < NeighNCNum; idx++){
				if (OverlapMapVec[NeighTetraFaceMap[i]].find(idx)->second){
          if(IntersectionMapVec[NeighTetraFaceMap[i]].find(idx)->second){
						neigh = NeighNCTetra[idx];
						if(neigh->getcnt() == NeighTetra[i]->getcnt()){
							denseMat<fp_t>* RHSCoup1E_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup2E_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup1H_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup2H_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							Zneigh = (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
							Yt = 1.0 / (0.5 * (Z + Zneigh));
							Yneigh = 1.0 / (No * sqrt(neigh->mat->mur.getEntry(0, 0) / neigh->mat->epsr.getEntry(0, 0)));
							Zt = 1.0 / (0.5 * (Y + Yneigh));
							for (int l = 0; l < LocalFaceDim; l++){
								for (int k = 0; k < LocalFaceDim; k++){
									RHSCoup1E_NC->setEntry(fac2tet[NeighTetraFaceMap[i]][l], k, (dt / Eo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(l, k));
									RHSCoup2E_NC->setEntry (fac2tet[NeighTetraFaceMap[i]][l], k, -(dt / Eo) * 0.5 * Yt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(l, k) * GAMMA);
									RHSCoup1H_NC->setEntry(fac2tet[NeighTetraFaceMap[i]][l], k, -(dt / Uo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(l, k));
									RHSCoup2H_NC->setEntry (fac2tet[NeighTetraFaceMap[i]][l], k, -(dt / Uo) * 0.5 * Zt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(l, k) * GAMMA);
								}	
							} 
							
							NC_CouplingMatArray[idx * NumCouplingMatrices + 1].Clear();
							NC_CouplingMatArray[idx * NumCouplingMatrices + 2].Clear();
							
							denseMat<fp_t>* RHSCoup1E_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvE, *RHSCoup1E_NC, *RHSCoup1E_NC_aux);
							RHSCoup1E_NC->Clear();

							denseMat<fp_t>* RHSCoup2E_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvE, *RHSCoup2E_NC, *RHSCoup2E_NC_aux);
							RHSCoup2E_NC->Clear();

							denseMat<fp_t>* RHSCoup1H_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvH, *RHSCoup1H_NC, *RHSCoup1H_NC_aux);
							RHSCoup1H_NC->Clear();

							denseMat<fp_t>* RHSCoup2H_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvH, *RHSCoup2H_NC, *RHSCoup2H_NC_aux);
							RHSCoup2H_NC->Clear();

							for(int j = 0; j < LocalDim; j++){
								for(int k = 0; k < LocalFaceDim; k++){
									LE_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + k] = RHSCoup1E_NC_aux->getEntry(j, k);
									LE_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + LocalFaceDim + k] = RHSCoup2E_NC_aux->getEntry(j, k);
									LH_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + k] = RHSCoup1H_NC_aux->getEntry(j, k);
									LH_Matrices_init[j * rowDim + offset + i * LocalFaceDim * 2 + LocalFaceDim + k] = RHSCoup2H_NC_aux->getEntry(j, k);
								}
							}

							RHSCoup1E_NC_aux->Clear();
							RHSCoup2E_NC_aux->Clear();
							RHSCoup1H_NC_aux->Clear();
							RHSCoup2H_NC_aux->Clear();
							break;
						}
					}
				}
			}
		}
	}
	if(LocalEDOF != 0){ 
		LHSInvE->Clear();
		RHSCoup1E_aux->Clear();
		RHSCoup2E_aux->Clear();
	}
	if(LocalHDOF != 0){ 
		LHSInvH->Clear();
		RHSCoup1H_aux->Clear();
		RHSCoup2H_aux->Clear();
	}
}

void tetra::invxCol(fp_t* col, bool Efield){
	if(Efield){
		if(LHSInvE == nullptr){
			cerr << "ERROR LHSInvE null" << endl;
			return;
		}
		ArrayFP<fp_t> x(TetPolyOrderDim[PolyOrderFlag]);
		ArrayFP<fp_t> y(TetPolyOrderDim[PolyOrderFlag]);
		for(int i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++){
			x[i] = col[i];
		}

		Ax(*LHSInvE, x, y);

		for(int i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++){
			col[i] = y[i];
		}
	}else{
		if(LHSInvH == nullptr){
			cerr << "ERROR LHSInvE null" << endl;
			return;
		}
		ArrayFP<fp_t> x(TetPolyOrderDim[PolyOrderFlag]);
		ArrayFP<fp_t> y(TetPolyOrderDim[PolyOrderFlag]);
		for(int i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++){
			x[i] = col[i];
		}
		
		Ax(*LHSInvH, x, y);

		for(int i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++){
			col[i] = y[i];
		}
	}
}

void tetra::prepareCuBLAS(fp_t_ts* Loc1E_h, fp_t_ts* Loc2E_h, fp_t_ts* Neigh1E_h, fp_t_ts* Neigh2E_h, fp_t_ts* InvE_h,  
													fp_t_ts* Loc1H_h, fp_t_ts* Loc2H_h, fp_t_ts* Neigh1H_h, fp_t_ts* Neigh2H_h, fp_t_ts* InvH_h){
	
	int LocalDim = TetPolyOrderDim[PolyOrderFlag];
	int LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	denseMat<fp_t>* RHSCoup1E_aux;
	denseMat<fp_t>* RHSCoup2E_aux;
	denseMat<fp_t>* RHSLoc1E_aux;
	denseMat<fp_t>* RHSLoc2E_aux;

	if(LocalEDOF != 0){
		RHSCoup1E_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvE, *RHSCoup1E, *RHSCoup1E_aux);
		RHSCoup1E->Clear();
		
		RHSCoup2E_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvE, *RHSCoup2E, *RHSCoup2E_aux);
		RHSCoup2E->Clear();

		RHSLoc1E_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvE, *RHSLoc1E, *RHSLoc1E_aux);
		RHSLoc1E->Clear();

		RHSLoc2E_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvE, *RHSLoc2E, *RHSLoc2E_aux);
		RHSLoc2E->Clear();
	}

	denseMat<fp_t>* RHSCoup1H_aux;
	denseMat<fp_t>* RHSCoup2H_aux;
	denseMat<fp_t>* RHSLoc1H_aux;
	denseMat<fp_t>* RHSLoc2H_aux;

	if(LocalHDOF != 0){
		RHSCoup1H_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvH, *RHSCoup1H, *RHSCoup1H_aux);
		RHSCoup1H->Clear();

		RHSCoup2H_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvH, *RHSCoup2H, *RHSCoup2H_aux);
		RHSCoup2H->Clear();

		RHSLoc1H_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvH, *RHSLoc1H, *RHSLoc1H_aux);
		RHSLoc1H->Clear();

		RHSLoc2H_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvH, *RHSLoc2H, *RHSLoc2H_aux);
		RHSLoc2H->Clear();
	}

	//Complete local matrices (they must be saved column-wise)
	for(int i = 0; i < LocalDim; i++){
		for(int j = 0; j < LocalDim; j++){
			//E-field matrices
			if(LocalEDOF != 0){
				Loc1E_h[i * LocalDim + j] = RHSLoc1E_aux->getEntry(j, i);
				Loc2E_h[i * LocalDim + j] = RHSLoc2E_aux->getEntry(j, i);
				if(InvE_h != nullptr){
					// cout << "here" << endl;
					InvE_h[i * LocalDim + j] = LHSInvE->getEntry(j, i);
				}
			}else{
				Loc1E_h[i * LocalDim + j] = 0.0;
				Loc2E_h[i * LocalDim + j] = 0.0;
				if(InvE_h != nullptr){
					InvE_h[i * LocalDim + j] = 0.0;
				}
			}			

			//H-field matrices
			if(LocalHDOF != 0){
				Loc1H_h[i * LocalDim + j] = RHSLoc1H_aux->getEntry(j, i);
				Loc2H_h[i * LocalDim + j] = RHSLoc2H_aux->getEntry(j, i);
				if(InvH_h != nullptr){
					InvH_h[i * LocalDim + j] = LHSInvH->getEntry(j, i);
				}
			}else{
				Loc1H_h[i * LocalDim + j] = 0.0;
				Loc2H_h[i * LocalDim + j] = 0.0;
				if(InvH_h != nullptr){
					InvH_h[i * LocalDim + j] = 0.0;
				}
			}		

		}
	}

	if(LocalEDOF != 0){
		RHSLoc1E_aux->Clear();
		RHSLoc2E_aux->Clear();
	}
	if(LocalHDOF != 0){
		RHSLoc1H_aux->Clear();
		RHSLoc2H_aux->Clear();
	}

	//Complete coupling matrices (they must be saved column-wise)
	for(int neigh = 0; neigh < NeighNum; neigh++){
		int DGface_bc = fc[NeighTetraFaceMap[neigh]]->bcPtr->getbType();
		int block = LocalDim * LocalFaceDim * neigh;
		if(DGface_bc != nonConformal){
			//Conformal faces (already stored in the RHS coupling terms)
			for(int i = 0; i < LocalFaceDim; i++){
				for(int j = 0; j < LocalDim; j++){
					//E-field matrices
					if(LocalEDOF != 0){
						Neigh1E_h[block + i * LocalDim + j] = RHSCoup1E_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
						Neigh2E_h[block + i * LocalDim + j] = RHSCoup2E_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
					}else{
						Neigh1E_h[block + i * LocalDim + j] = 0.0;
						Neigh2E_h[block + i * LocalDim + j] = 0.0;
					}
					//H-field matrices
					if(LocalHDOF != 0){
						Neigh1H_h[block + i * LocalDim + j] = RHSCoup1H_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
						Neigh2H_h[block + i * LocalDim + j] = RHSCoup2H_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
					}else{
						Neigh1H_h[block + i * LocalDim + j] = 0.0;
						Neigh2H_h[block + i * LocalDim + j] = 0.0;
					}
				}
			}
		}else{
			//Non-conformal faces (Not stored in the aux matrices so they need to be calculated)
			fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			fp_t Y = 1.0 / Z;

			for (int idx = 0 ; idx < NeighNCNum; idx++){
				if (OverlapMapVec[NeighTetraFaceMap[neigh]].find(idx)->second){
					if(IntersectionMapVec[NeighTetraFaceMap[neigh]].find(idx)->second){
						tetra* neighbor = NeighNCTetra[idx];
						if(neighbor->getcnt() == NeighTetra[neigh]->getcnt()){
							denseMat<fp_t>* RHSCoup1E_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup2E_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup1H_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup2H_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);

							fp_t Zneigh = (No * sqrt(neighbor->mat->mur.getEntry(0, 0) / neighbor->mat->epsr.getEntry(0, 0)));
							fp_t Yt = 1.0 / (0.5 * (Z + Zneigh));
							fp_t Yneigh = 1.0 / (No * sqrt(neighbor->mat->mur.getEntry(0, 0) / neighbor->mat->epsr.getEntry(0, 0)));
							fp_t Zt = 1.0 / (0.5 * (Y + Yneigh));

							for(int i = 0; i < LocalFaceDim; i++){
								for(int j = 0; j < LocalFaceDim; j++){
									RHSCoup1E_NC->setEntry(fac2tet[NeighTetraFaceMap[neigh]][i], j, (Class_dt / Eo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(i, j));
									RHSCoup2E_NC->setEntry (fac2tet[NeighTetraFaceMap[neigh]][i], j, -(Class_dt / Eo) * 0.5 * Yt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(i, j) * GAMMA);
									RHSCoup1H_NC->setEntry(fac2tet[NeighTetraFaceMap[neigh]][i], j, -(Class_dt / Uo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(i, j));
									RHSCoup2H_NC->setEntry (fac2tet[NeighTetraFaceMap[neigh]][i], j, -(Class_dt / Uo) * 0.5 * Zt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(i, j) * GAMMA);
								}
							}

							NC_CouplingMatArray[idx * NumCouplingMatrices + 1].Clear();
							NC_CouplingMatArray[idx * NumCouplingMatrices + 2].Clear();
							
							denseMat<fp_t>* RHSCoup1E_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvE, *RHSCoup1E_NC, *RHSCoup1E_NC_aux);
							RHSCoup1E_NC->Clear();

							denseMat<fp_t>* RHSCoup2E_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvE, *RHSCoup2E_NC, *RHSCoup2E_NC_aux);
							RHSCoup2E_NC->Clear();

							denseMat<fp_t>* RHSCoup1H_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvH, *RHSCoup1H_NC, *RHSCoup1H_NC_aux);
							RHSCoup1H_NC->Clear();

							denseMat<fp_t>* RHSCoup2H_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvH, *RHSCoup2H_NC, *RHSCoup2H_NC_aux);
							RHSCoup2H_NC->Clear();

							for(int i = 0; i < LocalFaceDim; i++){
								for(int j = 0; j < LocalDim; j++){
									Neigh1E_h[block + i * LocalDim + j] = RHSCoup1E_NC_aux->getEntry(j, i);
									Neigh2E_h[block + i * LocalDim + j] = RHSCoup2E_NC_aux->getEntry(j, i);
									Neigh1H_h[block + i * LocalDim + j] = RHSCoup1H_NC_aux->getEntry(j, i);
									Neigh2H_h[block + i * LocalDim + j] = RHSCoup2H_NC_aux->getEntry(j, i);
								}
							}
							RHSCoup1E_NC_aux->Clear();
							RHSCoup2E_NC_aux->Clear();
							RHSCoup1H_NC_aux->Clear();
							RHSCoup2H_NC_aux->Clear();
							break;
						}
					}
				}
			}
		}
	}
	if(LocalEDOF != 0){ 
		LHSInvE->Clear();
		RHSCoup1E_aux->Clear();
		RHSCoup2E_aux->Clear();
	}
	if(LocalHDOF != 0){ 
		LHSInvH->Clear();
		RHSCoup1H_aux->Clear();
		RHSCoup2H_aux->Clear();
	}
}




void tetra::prepareCuBLAS_PML(fp_t_ts* Loc1E_h, fp_t_ts* Loc2E_h, fp_t_ts* Neigh1E_h, fp_t_ts* Neigh2E_h,  
                                fp_t_ts* Loc1H_h, fp_t_ts* Loc2H_h, fp_t_ts* Neigh1H_h, fp_t_ts* Neigh2H_h,
                                fp_t_ts* AuxE_h, fp_t_ts*  AuxH_h, fp_t_ts* AuxM1_h, fp_t_ts* AuxM2_h, fp_t_ts* AuxJ1_h, fp_t_ts* AuxJ2_h)
{
	
	int LocalDim = TetPolyOrderDim[PolyOrderFlag];
	int LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	denseMat<fp_t>* RHSCoup1E_aux;
	denseMat<fp_t>* RHSCoup2E_aux;
	denseMat<fp_t>* RHSLoc1E_aux;
	denseMat<fp_t>* RHSLoc2E_aux;

    denseMat<fp_t>* AuxE_aux;
    denseMat<fp_t>* AuxJ1_aux;
    denseMat<fp_t>* AuxJ2_aux;
    

	if(LocalEDOF != 0){
		RHSCoup1E_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvE, *RHSCoup1E, *RHSCoup1E_aux);
		RHSCoup1E->Clear();
		
		RHSCoup2E_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvE, *RHSCoup2E, *RHSCoup2E_aux);
		RHSCoup2E->Clear();

		RHSLoc1E_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvE, *RHSLoc1E, *RHSLoc1E_aux);
		RHSLoc1E->Clear();

		RHSLoc2E_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvE, *RHSLoc2E, *RHSLoc2E_aux);
		RHSLoc2E->Clear();

		AuxE_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvE, *RHSAuxE, *AuxE_aux);
		RHSAuxE->Clear();

        AuxJ1_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvAuxJ, *RHSLocAuxJ1, *AuxJ1_aux);
		RHSLocAuxJ1->Clear();

        AuxJ2_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvAuxJ, *RHSLocAuxJ2, *AuxJ2_aux);
		RHSLocAuxJ2->Clear();
	}

	denseMat<fp_t>* RHSCoup1H_aux;
	denseMat<fp_t>* RHSCoup2H_aux;
	denseMat<fp_t>* RHSLoc1H_aux;
	denseMat<fp_t>* RHSLoc2H_aux;

    denseMat<fp_t>* AuxH_aux;
    denseMat<fp_t>* AuxM1_aux;
    denseMat<fp_t>* AuxM2_aux;



	if(LocalHDOF != 0){
		RHSCoup1H_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvH, *RHSCoup1H, *RHSCoup1H_aux);
		RHSCoup1H->Clear();

		RHSCoup2H_aux = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
		AB(*LHSInvH, *RHSCoup2H, *RHSCoup2H_aux);
		RHSCoup2H->Clear();

		RHSLoc1H_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvH, *RHSLoc1H, *RHSLoc1H_aux);
		RHSLoc1H->Clear();

		RHSLoc2H_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvH, *RHSLoc2H, *RHSLoc2H_aux);
		RHSLoc2H->Clear();

		AuxH_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvH, *RHSAuxH, *AuxH_aux);
		RHSAuxH->Clear();

        AuxM1_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvAuxM, *RHSLocAuxM1, *AuxM1_aux);
		RHSLocAuxM1->Clear();

        AuxM2_aux = new denseMat<fp_t>(LocalDim, LocalDim);
		AB(*LHSInvAuxM, *RHSLocAuxM2, *AuxM2_aux);
		RHSLocAuxM2->Clear();
	}


	//Complete local matrices (they must be saved column-wise)
	for(int i = 0; i < LocalDim; i++){
		for(int j = 0; j < LocalDim; j++){
			//E-field matrices
			if(LocalEDOF != 0){
				Loc1E_h[i * LocalDim + j] = RHSLoc1E_aux->getEntry(j, i);
				Loc2E_h[i * LocalDim + j] = RHSLoc2E_aux->getEntry(j, i);

                AuxE_h[i * LocalDim + j] = AuxE_aux->getEntry(j, i);
                AuxJ1_h[i * LocalDim + j] = AuxJ1_aux->getEntry(j, i);
                AuxJ2_h[i * LocalDim + j] = AuxJ2_aux->getEntry(j, i);

			}else{
				Loc1E_h[i * LocalDim + j] = 0.0;
				Loc2E_h[i * LocalDim + j] = 0.0;

                AuxE_h[i * LocalDim + j]  = 0.0;
                AuxJ1_h[i * LocalDim + j] = 0.0;
                AuxJ2_h[i * LocalDim + j] = 0.0;
			}			

			//H-field matrices
			if(LocalHDOF != 0){
				Loc1H_h[i * LocalDim + j] = RHSLoc1H_aux->getEntry(j, i);
				Loc2H_h[i * LocalDim + j] = RHSLoc2H_aux->getEntry(j, i);

                AuxH_h[i * LocalDim + j] = AuxH_aux->getEntry(j, i);
                AuxM1_h[i * LocalDim + j] = AuxM1_aux->getEntry(j, i);
                AuxM2_h[i * LocalDim + j] = AuxM2_aux->getEntry(j, i);


			}else{
				Loc1H_h[i * LocalDim + j] = 0.0;
				Loc2H_h[i * LocalDim + j] = 0.0;

                AuxH_h[i * LocalDim + j]  = 0.0;
                AuxM1_h[i * LocalDim + j] = 0.0;
                AuxM2_h[i * LocalDim + j] = 0.0;

			}		

		}
	}

	if(LocalEDOF != 0){
		RHSLoc1E_aux->Clear();
		RHSLoc2E_aux->Clear();
	}
	if(LocalHDOF != 0){
		RHSLoc1H_aux->Clear();
		RHSLoc2H_aux->Clear();
	}

	//Complete coupling matrices (they must be saved column-wise)
	for(int neigh = 0; neigh < NeighNum; neigh++){
		int DGface_bc = fc[NeighTetraFaceMap[neigh]]->bcPtr->getbType();
		int block = LocalDim * LocalFaceDim * neigh;
		if(DGface_bc != nonConformal){
			//Conformal faces (already stored in the RHS coupling terms)
			for(int i = 0; i < LocalFaceDim; i++){
				for(int j = 0; j < LocalDim; j++){
					//E-field matrices
					if(LocalEDOF != 0){
						Neigh1E_h[block + i * LocalDim + j] = RHSCoup1E_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
						Neigh2E_h[block + i * LocalDim + j] = RHSCoup2E_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
					}else{
						Neigh1E_h[block + i * LocalDim + j] = 0.0;
						Neigh2E_h[block + i * LocalDim + j] = 0.0;
					}
					//H-field matrices
					if(LocalHDOF != 0){
						Neigh1H_h[block + i * LocalDim + j] = RHSCoup1H_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
						Neigh2H_h[block + i * LocalDim + j] = RHSCoup2H_aux->getEntry(j, NeighTetraFaceMap[neigh] * LocalFaceDim + i);
					}else{
						Neigh1H_h[block + i * LocalDim + j] = 0.0;
						Neigh2H_h[block + i * LocalDim + j] = 0.0;
					}
				}
			}
		}else{
			//Non-conformal faces (Not stored in the aux matrices so they need to be calculated)
			fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			fp_t Y = 1.0 / Z;

			for (int idx = 0 ; idx < NeighNCNum; idx++){
				if (OverlapMapVec[NeighTetraFaceMap[neigh]].find(idx)->second){
					if(IntersectionMapVec[NeighTetraFaceMap[neigh]].find(idx)->second){
						tetra* neighbor = NeighNCTetra[idx];
						if(neighbor->getcnt() == NeighTetra[neigh]->getcnt()){
							denseMat<fp_t>* RHSCoup1E_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup2E_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup1H_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							denseMat<fp_t>* RHSCoup2H_NC = new denseMat<fp_t>(LocalDim, LocalFaceDim);

							fp_t Zneigh = (No * sqrt(neighbor->mat->mur.getEntry(0, 0) / neighbor->mat->epsr.getEntry(0, 0)));
							fp_t Yt = 1.0 / (0.5 * (Z + Zneigh));
							fp_t Yneigh = 1.0 / (No * sqrt(neighbor->mat->mur.getEntry(0, 0) / neighbor->mat->epsr.getEntry(0, 0)));
							fp_t Zt = 1.0 / (0.5 * (Y + Yneigh));

							for(int i = 0; i < LocalFaceDim; i++){
								for(int j = 0; j < LocalFaceDim; j++){
									RHSCoup1E_NC->setEntry(fac2tet[NeighTetraFaceMap[neigh]][i], j, (Class_dt / Eo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(i, j));
									RHSCoup2E_NC->setEntry (fac2tet[NeighTetraFaceMap[neigh]][i], j, -(Class_dt / Eo) * 0.5 * Yt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(i, j) * GAMMA);
									RHSCoup1H_NC->setEntry(fac2tet[NeighTetraFaceMap[neigh]][i], j, -(Class_dt / Uo) * NC_CouplingMatArray[idx * NumCouplingMatrices + 1].getEntry(i, j));
									RHSCoup2H_NC->setEntry (fac2tet[NeighTetraFaceMap[neigh]][i], j, -(Class_dt / Uo) * 0.5 * Zt * NC_CouplingMatArray[idx * NumCouplingMatrices + 2].getEntry(i, j) * GAMMA);
								}
							}

							NC_CouplingMatArray[idx * NumCouplingMatrices + 1].Clear();
							NC_CouplingMatArray[idx * NumCouplingMatrices + 2].Clear();
							
							denseMat<fp_t>* RHSCoup1E_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvE, *RHSCoup1E_NC, *RHSCoup1E_NC_aux);
							RHSCoup1E_NC->Clear();

							denseMat<fp_t>* RHSCoup2E_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvE, *RHSCoup2E_NC, *RHSCoup2E_NC_aux);
							RHSCoup2E_NC->Clear();

							denseMat<fp_t>* RHSCoup1H_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvH, *RHSCoup1H_NC, *RHSCoup1H_NC_aux);
							RHSCoup1H_NC->Clear();

							denseMat<fp_t>* RHSCoup2H_NC_aux = new denseMat<fp_t>(LocalDim, LocalFaceDim);
							AB(*LHSInvH, *RHSCoup2H_NC, *RHSCoup2H_NC_aux);
							RHSCoup2H_NC->Clear();

							for(int i = 0; i < LocalFaceDim; i++){
								for(int j = 0; j < LocalDim; j++){
									Neigh1E_h[block + i * LocalDim + j] = RHSCoup1E_NC_aux->getEntry(j, i);
									Neigh2E_h[block + i * LocalDim + j] = RHSCoup2E_NC_aux->getEntry(j, i);
									Neigh1H_h[block + i * LocalDim + j] = RHSCoup1H_NC_aux->getEntry(j, i);
									Neigh2H_h[block + i * LocalDim + j] = RHSCoup2H_NC_aux->getEntry(j, i);
								}
							}
							RHSCoup1E_NC_aux->Clear();
							RHSCoup2E_NC_aux->Clear();
							RHSCoup1H_NC_aux->Clear();
							RHSCoup2H_NC_aux->Clear();
							break;
						}
					}
				}
			}
		}
	}
	if(LocalEDOF != 0){ 
		LHSInvE->Clear();
		RHSCoup1E_aux->Clear();
		RHSCoup2E_aux->Clear();
	}
	if(LocalHDOF != 0){ 
		LHSInvH->Clear();
		RHSCoup1H_aux->Clear();
		RHSCoup2H_aux->Clear();
	}
}















void tetra::Calculate_M_Matrix_I_H_Numeric() {
	int i, j, k, dim;
    fp_t cval = 0.0;
    fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
    vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
                 // others [{0,1}, {0,2}, {0,3}]
    vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
                 // equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	Local_DG_mapH(map, 0);

	if (LocalHDOF == 0) {
		MassI_H = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
		MassI_H->reset();
		return;
	}
	MassI_H = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
	dim = MassI_H->getRowDim();

	if (dim != MassI_H->getColDim()) {
			cerr << "MassI_H: Non-square matrix passed." << endl;
	}

	MassI_H->reset();
	geometry(lvtr, avtr, &vol);
	dim = MassI_H->getRowDim();



	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = mat->mur * w1;
		}
	}

	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			MassI_H->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;

	// If it's PMC, set a 1 if it's in the diagonal, 0 if not
	for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
		for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
			if (map[i] < 0 || map[j] < 0) {
				if (i == j) {
					MassI_H->setEntry(i, j, 1.0); // need to be inversed
				} else {
					MassI_H->setEntry(i, j, 0); // need to be inversed
				}
			}
		}
	}
	if (map != nullptr)
		delete[] map;
}




void tetra::Calculate_ABC_H_Numeric() 
{    

	if (LocalHDOF == 0)
		return;

	int LocalDim, LocalFaceDim;
	int DGface_bc;
	int AbcFlag = 0;
	fp_t mu_o = Uo;
	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	int i, j, k, dim;    
	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	denseMat<fp_t> tetBh(LocalDim, LocalDim);

	// Matrices On the Faces
	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	for (i = 0; i < 4; i++) {
		DGface_bc = getFacePtr(i)->bcPtr->getbType();

		if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			AbcFlag = 1;
			fc[i]->Calculate_Bh_Matrix_Numeric(&Bh, PolyOrderFlag);
			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					tetBh.addEntry(fac2tet[i][j], fac2tet[i][k], Bh.getEntry(j, k));
				}
			}
		}
	}

	// ABC Boundary
	if (AbcFlag == 1) {
		for (i = 0; i < 4; i++) {
			if (fc[i]->bcPtr->getbType() == abcType) {
				Z = fc[i]->bcPtr->get_ABCImpVal();
			} else if (fc[i]->bcPtr->getbType() == planeWaveType) {
				Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			} else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
				Z = fc[i]->bcPtr->get_PortImpVal();
			}
		}
		tetBh.scale(-0.5 * Z);
	}

    ABC_tetBh = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    *ABC_tetBh = tetBh;
}

















void tetra::Calculate_M_Matrix_I_E_Numeric() {
    int i, j, k, dim;
    fp_t cval = 0.0;
    fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
    vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
                 // others [{0,1}, {0,2}, {0,3}]
    vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
                 // equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;

    int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
    Local_DG_mapE(map, 0);

    if (LocalEDOF == 0) 
	{
			cout << "RESET\n";
			MassI_E = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
			MassI_E->reset();
			return;
    }


    MassI_E = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    dim = MassI_E->getRowDim();
    if (dim != MassE->getColDim()) {
        cerr << "MassI_E: Non-square matrix passed." << endl;
    }
    MassI_E->reset();
    geometry(lvtr, avtr, &vol);

	dim = MassI_E->getRowDim();

	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = mat->epsr * w1;
		}
	}

	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			MassI_E->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;

    // If it's PEC, set a 1 if it's in the diagonal, 0 if not
    for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
        for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
            if (map[i] < 0 || map[j] < 0) {
                if (i == j) {
                    MassI_E->setEntry(i, j, 1.0); // need to be inversed
                } else {
                    MassI_E->setEntry(i, j, 0); // need to be inversed
                }
            }
        }
    }


    if (map != nullptr)
        delete[] map;


}




void tetra::Calculate_ABC_E_Numeric()
{
    if (LocalEDOF == 0)
        return;
    int LocalDim, LocalFaceDim;
    int DGface_bc;
    int AbcFlag = 0;

    fp_t eps_o = Eo;
    fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
    fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
    int i, j, k, dim;

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
    denseMat<fp_t> tetBe(LocalDim, LocalDim);
    // Matrices On the Faces
    denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

    for (i = 0; i < 4; i++) {
        DGface_bc = fc[i]->bcPtr->getbType();
        if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
            AbcFlag = 1;
            fc[i]->Calculate_Be_Matrix_Numeric(&Be,PolyOrderFlag);
            for (j = 0; j < LocalFaceDim; j++) {
                for (k = 0; k < LocalFaceDim; k++) {
                    tetBe.addEntry(fac2tet[i][j], fac2tet[i][k], Be.getEntry(j, k));
                }
            }
        }
    }

    // ABC Boundary
    if (AbcFlag == 1) {
        for (i = 0; i < 4; i++) {
            // the same
            if (fc[i]->bcPtr->getbType() == abcType) {
                Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
            } else if (fc[i]->bcPtr->getbType() == planeWaveType) {
                Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
            } else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
                Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();
            }
        }
        tetBe.scale(-0.5 * Y);
    }

    ABC_tetBe = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    *ABC_tetBe = tetBe;
}












void tetra::SetUp_LocalFaceToTetraMapE_NMF1_PML(fp_t dt) {

	if (LocalEDOF == 0)
		return;
	int LocalDim, LocalFaceDim;
	fp_t eps_o = Eo;

	bool has_null = false;
	if (!BiiE)         { std::cerr << "❌ BiiE is null.\n"; has_null = true; }
	if (!Mass_epA_E)   { std::cerr << "❌ Mass_epA_E is null.\n"; has_null = true; }
	if (!Mass_epB_E)   { std::cerr << "❌ Mass_epB_E is null.\n"; has_null = true; }
	if (!ABC_tetBe)    { std::cerr << "❌ ABC_tetBe is null.\n"; has_null = true; }
	if (!StiffE)       { std::cerr << "❌ StiffE is null.\n"; has_null = true; }
	if (!FiiE)         { std::cerr << "❌ FiiE is null.\n"; has_null = true; }
	if (!FijE)         { std::cerr << "❌ FijE is null.\n"; has_null = true; }
	if (!BijE)         { std::cerr << "❌ BijE is null.\n"; has_null = true; }
	if (!Mass_epC_E)   { std::cerr << "❌ Mass_epC_E is null.\n"; has_null = true; }
	if (!MassI_E)      { std::cerr << "❌ MassI_E is null.\n"; has_null = true; }
	if (!Mass_D_E)     { std::cerr << "❌ Mass_D_E is null.\n"; has_null = true; }
	if (!Mass_F_E)     { std::cerr << "❌ Mass_F_E is null.\n"; has_null = true; }
	if (!MassE)        { std::cerr << "❌ MassE is null.\n"; has_null = true; }
	
	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
	

	RHSLoc1E = new denseMat<fp_t>(LocalDim, LocalDim);
	RHSLoc2E = new denseMat<fp_t>(LocalDim, LocalDim);
	RHSCoup1E = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
	RHSCoup2E = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);

	BiiE->scale(dt / eps_o);
    Mass_epB_E->scale(0.5 * dt);
    ABC_tetBe->scale(0.5 * dt / eps_o);

	*RHSLoc1E = *Mass_epA_E + *BiiE - (*Mass_epB_E + *ABC_tetBe) ;
	ApplyPEC(RHSLoc1E, LocalDim);

	*RHSLoc2E = *StiffE - *FiiE;
	RHSLoc2E->scale(dt / eps_o);
	ApplyPEC_E(RHSLoc2E, LocalDim);
	ApplyPMC_E(RHSLoc2E, LocalDim);

	*RHSCoup1E = *FijE;
	RHSCoup1E->scale(dt / eps_o);

	*RHSCoup2E = *BijE;
	RHSCoup2E->scale(dt / eps_o);



	// Inv
	LHSInvE = new denseMat<fp_t>(LocalDim, LocalDim);
	*LHSInvE = *Mass_epA_E + (*Mass_epB_E + *ABC_tetBe);
	*LHSInvE = LHSInvE->inverse();


	RHSAuxE     = new denseMat<fp_t>(LocalDim, LocalDim);
    *RHSAuxE = *Mass_epC_E;
    RHSAuxE->scale(-1.0 * dt);
	ApplyPEC(RHSAuxE, LocalDim);

    // ------------------------------
    // PML

    LHSInvAuxJ  = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSLocAuxJ1 = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSLocAuxJ2 = new denseMat<fp_t>(LocalDim, LocalDim);

	MassI_E->scale(1.0 / dt);
	Mass_D_E->scale(0.5);
    *LHSInvAuxJ = (*MassI_E + *Mass_D_E);
	*LHSInvAuxJ = LHSInvAuxJ->inverse();


    *RHSLocAuxJ1 = (*MassI_E - *Mass_D_E);
	*RHSLocAuxJ2 = *Mass_F_E;
	

	MassE->Clear();
	StiffE->Clear();
	BiiE->Clear();
	FiiE->Clear();
	BijE->Clear();
	FijE->Clear();
    ABC_tetBe->Clear();

	Mass_epA_E->Clear();
	Mass_epB_E->Clear();
	Mass_epC_E->Clear();
	Mass_D_E->Clear();
	Mass_F_E->Clear();
	MassI_E->Clear();
}



void tetra::SetUp_LocalFaceToTetraMapH_NMF1_PML(fp_t dt) {
    if (LocalHDOF == 0)
        return;
    int LocalDim, LocalFaceDim;
    fp_t mu_o = Uo;

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];


	bool has_null = false;
	if (!BiiH)         { std::cerr << "❌ BiiH is null.\n"; has_null = true; }
	if (!Mass_muA_H)   { std::cerr << "❌ Mass_muA_H is null.\n"; has_null = true; }
	if (!Mass_muB_H)   { std::cerr << "❌ Mass_muB_H is null.\n"; has_null = true; }
	if (!ABC_tetBh)    { std::cerr << "❌ ABC_tetBh is null.\n"; has_null = true; }
	if (!StiffH)       { std::cerr << "❌ StiffH is null.\n"; has_null = true; }
	if (!FiiH)         { std::cerr << "❌ FiiH is null.\n"; has_null = true; }
	if (!FijH)         { std::cerr << "❌ FijH is null.\n"; has_null = true; }
	if (!BijH)         { std::cerr << "❌ BijH is null.\n"; has_null = true; }
	if (!Mass_muC_H)   { std::cerr << "❌ Mass_muC_H is null.\n"; has_null = true; }
	if (!MassI_H)      { std::cerr << "❌ MassI_H is null.\n"; has_null = true; }
	if (!Mass_D_H)     { std::cerr << "❌ Mass_D_H is null.\n"; has_null = true; }
	if (!Mass_F_H)     { std::cerr << "❌ Mass_F_H is null.\n"; has_null = true; }
	if (!MassH)        { std::cerr << "❌ MassH is null.\n"; has_null = true; }


    RHSLoc1H = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSLoc2H = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSCoup1H = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);
    RHSCoup2H = new denseMat<fp_t>(LocalDim, NumOfFaces * LocalFaceDim);

    BiiH->scale(dt / mu_o);
	Mass_muB_H->scale(0.5 * dt);
    ABC_tetBh->scale(0.5 * dt / mu_o);

    *RHSLoc1H = *Mass_muA_H + *BiiH - (*Mass_muB_H + *ABC_tetBh);
    ApplyPMC(RHSLoc1H, LocalDim);

    *RHSLoc2H = *FiiH - *StiffH;
    RHSLoc2H->scale(dt / mu_o);
    ApplyPEC_H(RHSLoc2H, LocalDim);
    ApplyPMC_H(RHSLoc2H, LocalDim);

    *RHSCoup1H = *FijH;
    RHSCoup1H->scale(dt / mu_o);

    *RHSCoup2H = *BijH;
    RHSCoup2H->scale(dt / mu_o);

    // Inv
    LHSInvH = new denseMat<fp_t>(LocalDim, LocalDim);
    *LHSInvH = *Mass_muA_H + (*Mass_muB_H + *ABC_tetBh);
    *LHSInvH = LHSInvH->inverse();


    RHSAuxH     = new denseMat<fp_t>(LocalDim, LocalDim);
    *RHSAuxH = *Mass_muC_H;
    RHSAuxH->scale(-1.0 * dt);
    ApplyPMC(RHSAuxH, LocalDim);

    // ------------------------------
    // PML
    LHSInvAuxM  = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSLocAuxM1 = new denseMat<fp_t>(LocalDim, LocalDim);
    RHSLocAuxM2 = new denseMat<fp_t>(LocalDim, LocalDim);
	MassI_H->scale(1.0 / dt);
	Mass_D_H->scale(0.5);
    *LHSInvAuxM = (*MassI_H + *Mass_D_H);
	*LHSInvAuxM = LHSInvAuxM->inverse();
    *RHSLocAuxM1 = (*MassI_H - *Mass_D_H);

    *RHSLocAuxM2 = *Mass_F_H;

	MassH->Clear();
    StiffH->Clear();
    BiiH->Clear();
    FiiH->Clear();
    BijH->Clear();
    FijH->Clear();
    ABC_tetBh->Clear();

	Mass_muA_H->Clear();
	Mass_muB_H->Clear();
	Mass_muC_H->Clear();
	Mass_D_H->Clear();
	Mass_F_H->Clear();
	MassI_H->Clear();

}




// ------------------------------------------------------------------------


// Function for Ellipsoid system of equations
void computeF(fp_t x[3], fp_t F[3],	fp_t a, fp_t b, fp_t c, fp_t x1, fp_t y1, fp_t z1) 
{
	fp_t x0 = x[0], y0 = x[1], z0 = x[2];
	F[0] = (x0 * x0) / (a * a) + (y0 * y0) / (b * b) + (z0 * z0) / (c * c) - 1.0;
	F[1] = z1 * (2 * y0 / (b * b)) - y1 * (2 * z0 / (c * c)) + (2.0 / (c * c) - 2.0 / (b * b)) * y0 * z0;
	F[2] = y1 * (2 * x0 / (a * a)) - x1 * (2 * y0 / (b * b)) + (2.0 / (b * b) - 2.0 / (a * a)) * x0 * y0;
}

// Jacobian to solve the Ellipsoidal System of Equations
void computeJacobian(fp_t x[3], fp_t J[3][3], fp_t a, fp_t b, fp_t c, fp_t x1, fp_t y1, fp_t z1) 
{
	fp_t x0 = x[0], y0 = x[1], z0 = x[2];

	// Row 1: ∂F0/∂x0, ∂F0/∂y0, ∂F0/∂z0
	J[0][0] = 2 * x0 / (a * a);
	J[0][1] = 2 * y0 / (b * b);
	J[0][2] = 2 * z0 / (c * c);

	// Row 2: ∂F1/∂x0, ∂F1/∂y0, ∂F1/∂z0
	J[1][0] = 0;
	J[1][1] = z1 * (2.0 / (b * b)) + (2.0 / (c * c) - 2.0 / (b * b)) * z0;
	J[1][2] = -y1 * (2.0 / (c * c)) + (2.0 / (c * c) - 2.0 / (b * b)) * y0;

	// Row 3: ∂F2/∂x0, ∂F2/∂y0, ∂F2/∂z0
	J[2][0] = y1 * (2.0 / (a * a)) + (2.0 / (b * b) - 2.0 / (a * a)) * y0;
	J[2][1] = -x1 * (2.0 / (b * b)) + (2.0 / (b * b) - 2.0 / (a * a)) * x0;
	J[2][2] = 0;
}


// Solve 3x3 linear system A x = b using Cramer's Rule
bool solveLinearSystem(double A[3][3], double b[3], double x[3]) 
{
    double detA =
        A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) -
        A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) +
        A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]);

    if (fabs(detA) < 1e-12) return false;

    for (int i = 0; i < 3; ++i) 
	{
        double Ai[3][3];
        for (int row = 0; row < 3; ++row)
            for (int col = 0; col < 3; ++col)
                Ai[row][col] = (col == i) ? b[row] : A[row][col];

        double detAi =
            Ai[0][0] * (Ai[1][1] * Ai[2][2] - Ai[1][2] * Ai[2][1]) -
            Ai[0][1] * (Ai[1][0] * Ai[2][2] - Ai[1][2] * Ai[2][0]) +
            Ai[0][2] * (Ai[1][0] * Ai[2][1] - Ai[1][1] * Ai[2][0]);

        x[i] = detAi / detA;
    }
    return true;
}

auto printMatrix = [](const std::string& name, denseMat<fp_t>* M) {
    if (!M) {
        std::cout << name << " is null.\n";
        return;
    }

    int rows = M->getRowDim();
    int cols = M->getColDim();

    std::cout << "---- " << name << " (" << rows << "x" << cols << ") ----\n";
    for (int i = 0; i < rows; ++i) {
        for (int j = 0; j < cols; ++j) {
            std::cout << M->getEntry(i, j) << " ";
        }
        std::cout << "\n";
    }
    std::cout << "\n";
};


void tetra::set_Conductivity_Profile_Spherical(fp_t Rx, fp_t Ry, fp_t Rz) 
{
	// This is the polynomial order of the conductivity profile of the PML
	fp_t m = mat->get_PML_m_ord();  
		
	// Obtain the the thickness of the PML	
	fp_t pml_thick = mat->get_PML_thick();

	fp_t eps_o = 8.854187817e-12;   
	fp_t sigma_max = mat->sigma.getEntry(0, 0);
	
	// Center of the tetrahedron
	// Point P1
	vtr rc = Center(); 
	fp_t x1 = rc.getx();
	fp_t y1 = rc.gety();
	fp_t z1 = rc.getz();
	

	// -------------------------------------------------------------
	// Step 1: Solve for Point P0 on PML surface
    fp_t x[3] = {x1, y1, z1};  // Initial guess

	int MAX_ITER = 100;
	fp_t TOL = 1e-8;
    for (int iter = 0; iter < MAX_ITER; ++iter) 
	{
        fp_t F[3], J[3][3];
        computeF(x, F, Rx, Ry, Rz, x1, y1, z1);
        computeJacobian(x, J, Rx, Ry, Rz, x1, y1, z1);

        // Solve J * delta = -F
        fp_t minusF[3] = {-F[0], -F[1], -F[2]};
        fp_t delta[3];
        if (!solveLinearSystem(J, minusF, delta)) 
		{
            cout << "Jacobian is singular!" << endl;
            break;
        }

        // Update x
        fp_t norm_delta = 0.0;
        for (int i = 0; i < 3; ++i) 
		{
            x[i] += delta[i];
            norm_delta += delta[i] * delta[i];
        }

        if (sqrt(norm_delta) < TOL) 
		{
            //cout << "Converged in " << iter + 1 << " iterations." << endl;
            break;
        }
    }

	// Points of P0
	fp_t x0 = x[0];  
	fp_t y0 = x[1];  
	fp_t z0 = x[2];  

	vtr r0 (x0,y0,z0);
	// -------------------------------------------------------------


	fp_t r0_mag = r0.magnitude();
	fp_t r1_mag = rc.magnitude();
	fp_t distance = r1_mag - r0_mag;
	fp_t growth_factor = std::abs(std::pow(distance / (pml_thick), m));
	fp_t growth_factor2 = std::abs(std::pow(distance / (pml_thick), m + 1.0));

	fp_t K_max = 2.0;
	fp_t K = 1.0 + (K_max - 1.0) * growth_factor;

	fp_t s_r = sigma_max * growth_factor;
	fp_t s_theta = s_r / (m + 1.0);
	fp_t s_phi = s_theta;

	fp_t s3 = s_theta * s_phi / s_r;
	fp_t s2 = s_r;
	fp_t s1 = s_r;

	matA = new denseMat<fp_t>(3, 3); 
	matB = new denseMat<fp_t>(3, 3); 
	matC = new denseMat<fp_t>(3, 3); 
	matD = new denseMat<fp_t>(3, 3); 
	matF = new denseMat<fp_t>(3, 3); 
	
	matA->reset(); matB->reset(); matC->reset(); matD->reset(); matF->reset();

	matA->setEntry(0, 0, K);
	matA->setEntry(1, 1, K);
	matA->setEntry(2, 2, K);

	fp_t A11 = matA->getEntry(0, 0);
	fp_t A22 = matA->getEntry(1, 1);
	fp_t A33 = matA->getEntry(2, 2);
	matB->setEntry(0, 0, (s2*K + s3*K - A11*s1) / eps_o);
	matB->setEntry(1, 1, (s1*K + s3*K - A22*s2) / eps_o);
	matB->setEntry(2, 2, (s1*K + s2*K - A33*s3) / eps_o);

	
	fp_t B11 = matB->getEntry(0, 0);
	fp_t B22 = matB->getEntry(1, 1);
	fp_t B33 = matB->getEntry(2, 2);
	matC->setEntry(0, 0, s2*s3/(eps_o * eps_o) - B11 * s1 / eps_o);
	matC->setEntry(1, 1, s1*s3/(eps_o * eps_o) - B22 * s2 / eps_o);
	matC->setEntry(2, 2, s1*s2/(eps_o * eps_o) - B33 * s3 / eps_o);
	
	matD->setEntry(0, 0, s1/(K * eps_o));
	matD->setEntry(1, 1, s2/(K * eps_o));
	matD->setEntry(2, 2, s3/(K * eps_o));

	matF->setEntry(0, 0, 1.0/K);
	matF->setEntry(1, 1, 1.0/K);
	matF->setEntry(2, 2, 1.0/K);

	// ----- Rotation ---------- //
	fp_t xx = rc.getx();
	fp_t yy = rc.gety();
	fp_t zz = rc.getz();

	fp_t rr = std::sqrt(xx*xx + yy*yy + zz*zz);
	fp_t theta = std::acos(zz / rr);       // θ ∈ [0, π]
	fp_t phi   = std::atan2(yy, zz);       // φ ∈ [−π, π]

	vtr ur( std::sin(theta)*std::cos(phi),
        std::sin(theta)*std::sin(phi),
        std::cos(theta) );

	vtr utheta( std::cos(theta)*std::cos(phi),
				std::cos(theta)*std::sin(phi),
				-std::sin(theta) );

	vtr uphi( -std::sin(phi),
			std::cos(phi),
			0.0 );

	denseMat<fp_t> R(3, 3);
	R.setEntry(0, 0, ur.getx());     R.setEntry(0, 1, ur.gety());     R.setEntry(0, 2, ur.getz());
	R.setEntry(1, 0, utheta.getx()); R.setEntry(1, 1, utheta.gety()); R.setEntry(1, 2, utheta.getz());
	R.setEntry(2, 0, uphi.getx());   R.setEntry(2, 1, uphi.gety());   R.setEntry(2, 2, uphi.getz());

	denseMat<fp_t> RT = R.transpose();  // Needed for tensor rotation
	denseMat<fp_t> A_cart(3,3);
	denseMat<fp_t> tmp(3,3);

	AB(*matA, R, tmp);      
	AB(RT, tmp, *matA);   

	AB(*matB, R, tmp);      
	AB(RT, tmp, *matA);   

	AB(*matC, R, tmp);       
	AB(RT, tmp, *matC);    

	AB(*matD, R, tmp);       
	AB(RT, tmp, *matD);    

	AB(*matF, R, tmp);       
	AB(RT, tmp, *matF);    


}

void tetra::set_Conductivity_Profile_Conformal(fp_t Rx, fp_t Ry, fp_t Rz) 
{

	if (!mat) 
	{
        std::cerr << "Error: mat pointer is NULL!" << std::endl;
        return;
    }

	// This is the polynomial order of the conductivity profile of the PML
	fp_t m = mat->get_PML_m_ord();  

		
	// (Cartesian-Aligned) PML
	// Obtain the the thickness of the PML	
	fp_t pml_thick = mat->get_PML_thick();

    if (std::abs(pml_thick) < 1e-10) 
	{
        std::cerr << "Error: PML thickness is zero or too small." << std::endl;
        return;
        }
		
	// Max loss
	fp_t eps_o = 8.854187817e-12;   
	fp_t sigma_max = mat->sigma.getEntry(0, 0);
        
	// Center of the tetrahedron
	// Point P1
	vtr rc = Center(); 
	fp_t x1 = rc.getx();
	fp_t y1 = rc.gety();
	fp_t z1 = rc.getz();
	

	// -------------------------------------------------------------
	// Step 1: Solve for Point P0 on PML surface
    fp_t x[3] = {x1, y1, z1};  // Initial guess

	int MAX_ITER = 100;
	fp_t TOL = 1e-8;
    for (int iter = 0; iter < MAX_ITER; ++iter) 
	{
        fp_t F[3], J[3][3];
        computeF(x, F, Rx, Ry, Rz, x1, y1, z1);
        computeJacobian(x, J, Rx, Ry, Rz, x1, y1, z1);

        // Solve J * delta = -F
        fp_t minusF[3] = {-F[0], -F[1], -F[2]};
        fp_t delta[3];
        if (!solveLinearSystem(J, minusF, delta)) 
		{
            cout << "Jacobian is singular!" << endl;
            break;
        }

        // Update x
        fp_t norm_delta = 0.0;
        for (int i = 0; i < 3; ++i) 
		{
            x[i] += delta[i];
            norm_delta += delta[i] * delta[i];
        }

        if (sqrt(norm_delta) < TOL) 
		{
            //cout << "Converged in " << iter + 1 << " iterations." << endl;
            break;
        }
    }

	// Points of P0
	fp_t x0 = x[0];  
	fp_t y0 = x[1];  
	fp_t z0 = x[2];  

	vtr r0 (x0,y0,z0);


	// -------------------------------------------------------------
	// Step 2: Calculate the principal curvature radii
	// We use the Analytical solutions from Analytical Derivation of Ellipsoid Conformal Perfectly Matched Layers for
	// Electromagnetic Simulations, Qian Yang , Bing Wei , Linqian Li , and Debiao Ge

    // Precompute powers
	fp_t  a = Rx;  
	fp_t  b = Ry;  
	fp_t  c = Rz;
    fp_t  a2 = a * a, b2 = b * b, c2 = c * c;
    fp_t  a4 = a2 * a2, b4 = b2 * b2, c4 = c2 * c2;

    fp_t  abc2 = (a * b * c) * (a * b * c); // (abc)^2

    // Z denominator
    fp_t Z = x0 * x0 / a4 + y0 * y0 / b4 + z0 * z0 / c4;

    // Gauss curvature
    fp_t kG = 1.0 / (abc2 * Z * Z);

    // Mean curvature
    fp_t  numerator = x0 * x0 + y0 * y0 + z0 * z0 - (a2 + b2 + c2);
    fp_t  denominator = 2.0 * abc2 * pow(Z, 1.5);
    fp_t kM = numerator / denominator;


    // Principal curvatures

	fp_t delta = kM * kM - kG;
	if (delta < 0) 
	{
		//std::cerr << "Warning: negative discriminant in curvature, clamping.\n";
		delta = 0.0;
	}
	fp_t sqrt_term = sqrt(delta);

    fp_t k1 = kM + sqrt_term;
    fp_t k2 = kM - sqrt_term;

	// Principal curvature radii
	fp_t R01 = abs(1.0 / k1);
	fp_t R02 = abs(1.0 / k2);

	// -------------------------------------------------------------
	// Step 3: Calculate Orthonormal basis u1, u2, u3

	// U3
	vtr u3 = rc - r0;  // Normal vector at P0
	fp_t u3_mag = u3.magnitude();
	u3.unitvtr();


	// If Sphere, All directions in the tangent plane are principal.
	// u1 and u2 are not uniquely defined — they are ambiguous but still orthogonal.
	vtr u1 , u2;
	u1.reset();
	u2.reset();

	fp_t EPS = 1e-6;

	// Sphere case — choose arbitrary orthonormal tangent vectors
	if (fabs(a - b) < EPS && fabs(b - c) < EPS) 
	{
		// Pick arbitrary direction not colinear with normal
		if (fabs(u3.getx()) < fabs(u3.gety())) 
		{
			u1 = vtr(0, -u3.getz(), u3.gety());
		} 
		else 
		{
			u1 = vtr(-u3.getz(), 0, u3.getx());
		}

		u1.unitvtr();

		// Perpendicular
		u2 = crossP(u3, u1);  
		u2.unitvtr();
	}
	else
	{
		fp_t theta = acos(z0 / c); // theta ∈ [0, π]
		fp_t phi   = atan2(y0 / b, x0 / a); // phi ∈ [-π, π]

		vtr r_theta(
			a * cos(theta) * cos(phi),
			b * cos(theta) * sin(phi),
		   -c * sin(theta)
		);
		
		vtr r_phi(
		   -a * sin(theta) * sin(phi),
			b * sin(theta) * cos(phi),
			0.0
		);
	
		// Compute coefficients
		fp_t E = pow(cos(theta),2) * (a*a * pow(cos(phi),2) + b*b * pow(sin(phi),2)) + c*c * pow(sin(theta),2);
		fp_t F = (b*b - a*a) * sin(theta) * cos(theta) * sin(phi) * cos(phi);
		fp_t G = pow(sin(theta),2) * (a*a * pow(sin(phi),2) + b*b * pow(cos(phi),2));
	
		fp_t denomL = sqrt(c*c * pow(sin(theta),2) * (a*a * pow(sin(phi),2) + b*b * pow(cos(phi),2)) + a*a * b*b * pow(cos(theta),2));
		fp_t L = -a * b * c / denomL;
		fp_t M = 0;
		fp_t N = L * pow(sin(theta), 2);
	
		// Quadratic coefficients
		fp_t A = E * M - F * L;
		fp_t B = -(L * G - E * N);
		fp_t C = F * N - G * M;
	
		// Solve quadratic
		fp_t discrim = B*B - 4*A*C;
		fp_t dtheta_dphi1 = (-B + sqrt(discrim)) / (2*A);
		fp_t dtheta_dphi2 = (-B - sqrt(discrim)) / (2*A);
	
		// Compute principal direction vectors
		u1 = r_theta * dtheta_dphi1 + r_phi;
		u2 = r_theta * dtheta_dphi2 + r_phi;
			
		u1.unitvtr();  // Normalize
		u2.unitvtr();
	
	}

    if (std::abs(dotP(u1, u2)) > 1e-5 || std::abs(dotP(u1, u3)) > 1e-5)
        std::cerr << "Warning: u1/u2/u3 may not be orthonormal!\n";

	// -------------------------------------------------------------
	// Step 4: Calculate the Rotation Matrix
	denseMat<fp_t> R(3, 3);
	R.reset();
	R.setEntry(0, 0, u1.getx()); R.setEntry(0, 1, u1.gety()); R.setEntry(0, 2, u1.getz());
	R.setEntry(1, 0, u2.getx()); R.setEntry(1, 1, u2.gety()); R.setEntry(1, 2, u2.getz());
	R.setEntry(2, 0, u3.getx()); R.setEntry(2, 1, u3.gety()); R.setEntry(2, 2, u3.getz());

	denseMat<fp_t> RT = R.transpose();


	// -------------------------------------------------------------
	// Step 5: Update R1 and R2

	fp_t R1 = R01 + u3_mag;
	fp_t R2 = R02 + u3_mag;

	// -------------------------------------------------------------
	// Step 6: Calculate the Loss Parameters in local coordinates u1, u2, u3

	sigma_1 = 0.0;
	sigma_2 = 0.0; 
	sigma_3 = 0.0;

	fp_t rc_ro = dotP(rc - r0, u3);
	fp_t growth_factor = std::abs(std::pow(rc_ro / (pml_thick), m));

	fp_t K_max = 2.0;
	fp_t K3 = 1.0 + (K_max - 1.0) * growth_factor;
	fp_t s3 = sigma_max * growth_factor;

	fp_t K2 = K3;
	fp_t K1 = K3;

	fp_t s1 = s3 / (m + 1.0); 
	fp_t s2 = s3 / (m + 1.0);

	matA = new denseMat<fp_t>(3, 3); 
	matB = new denseMat<fp_t>(3, 3); 
	matC = new denseMat<fp_t>(3, 3); 
	matD = new denseMat<fp_t>(3, 3); 
	matF = new denseMat<fp_t>(3, 3); 
	
	matA->reset(); matB->reset(); matC->reset(); matD->reset(); matF->reset();


	matA->setEntry(0, 0, K2*K3/K1);
	matA->setEntry(1, 1, K1*K3/K2);
	matA->setEntry(2, 2, K1*K2/K3);

	fp_t A11 = matA->getEntry(0, 0);
	fp_t A22 = matA->getEntry(1, 1);
	fp_t A33 = matA->getEntry(2, 2);
	matB->setEntry(0, 0, (s2*K3 + s3*K2 - A11*s1) / eps_o);
	matB->setEntry(1, 1, (s1*K3 + s3*K1 - A22*s2) / eps_o);
	matB->setEntry(2, 2, (s1*K2 + s2*K1 - A33*s3) / eps_o);

	
	fp_t B11 = matB->getEntry(0, 0);
	fp_t B22 = matB->getEntry(1, 1);
	fp_t B33 = matB->getEntry(2, 2);
	matC->setEntry(0, 0, s2*s3/(eps_o * eps_o) - B11 * s1 / eps_o);
	matC->setEntry(1, 1, s1*s3/(eps_o * eps_o) - B22 * s2 / eps_o);
	matC->setEntry(2, 2, s1*s2/(eps_o * eps_o) - B33 * s3 / eps_o);
	
	matD->setEntry(0, 0, s1/(K1 * eps_o));
	matD->setEntry(1, 1, s2/(K2 * eps_o));
	matD->setEntry(2, 2, s3/(K3 * eps_o));

	matF->setEntry(0, 0, 1.0/K1);
	matF->setEntry(1, 1, 1.0/K2);
	matF->setEntry(2, 2, 1.0/K3);


	// -------------------------------------------------------------
	// Step 7: Rotate the Loss Parameters to global Euclidean space x,y,z
	denseMat<fp_t> tmp(3, 3);
	AB(*matA, R, tmp);   AB(RT, tmp, *matA);
	AB(*matB, R, tmp);   AB(RT, tmp, *matB);
	AB(*matC, R, tmp);   AB(RT, tmp, *matC);
	AB(*matD, R, tmp);   AB(RT, tmp, *matD);
	AB(*matF, R, tmp);   AB(RT, tmp, *matF);

}




void tetra::set_Conductivity_Profile_Planar(fp_t planewave_xmin, fp_t planewave_ymin, fp_t planewave_zmin,
											fp_t planewave_xmax, fp_t planewave_ymax, fp_t planewave_zmax) 
{

    // Polynomial order of the PML conductivity profile
    fp_t m = static_cast<fp_t>(mat->get_PML_m_ord());

    // PML thickness
    fp_t pml_thick = mat->get_PML_thick();
    if (std::abs(pml_thick) < 1e-10)
    {
        std::cerr << "Error: PML thickness is zero or too small." << std::endl;
        return;
    }

    // Permittivity of free space
    fp_t eps_o = 8.854187817e-12;
    fp_t sigma_base = mat->sigma.getEntry(0, 0);  // Assumed max σ

    // Get center of tetrahedron
    vtr rc = Center();
    fp_t x1 = rc.getx();
    fp_t y1 = rc.gety();
    fp_t z1 = rc.getz();

    // Distance to PML boundaries and conductivities
    fp_t dx = 0.0, dy = 0.0, dz = 0.0;
    fp_t s1 = 0.0, s2 = 0.0, s3 = 0.0;

    if (x1 < planewave_xmin)
        dx = planewave_xmin - x1;
    else if (x1 > planewave_xmax)
        dx = x1 - planewave_xmax;

    if (y1 < planewave_ymin)
        dy = planewave_ymin - y1;
    else if (y1 > planewave_ymax)
        dy = y1 - planewave_ymax;

    if (z1 < planewave_zmin)
        dz = planewave_zmin - z1;
    else if (z1 > planewave_zmax)
        dz = z1 - planewave_zmax;

    // Compute directional conductivity σi(x)
    if (dx > 0.0) s1 = sigma_base * std::pow(dx / pml_thick, m);
    if (dy > 0.0) s2 = sigma_base * std::pow(dy / pml_thick, m);
    if (dz > 0.0) s3 = sigma_base * std::pow(dz / pml_thick, m);

    // Use Euclidean distance for growth profile
    fp_t distance = std::sqrt(dx * dx + dy * dy + dz * dz);
    fp_t growth_factor = std::pow(distance / pml_thick, m);

    // K scaling (Jacobians)
    fp_t K_max = 2.0;
    fp_t K1 = 1.0 + (K_max - 1.0) * growth_factor;
    fp_t K2 = K1;
    fp_t K3 = K1;


	matA = new denseMat<fp_t>(3, 3); 
	matB = new denseMat<fp_t>(3, 3); 
	matC = new denseMat<fp_t>(3, 3); 
	matD = new denseMat<fp_t>(3, 3); 
	matF = new denseMat<fp_t>(3, 3); 
	
	matA->reset(); matB->reset(); matC->reset(); matD->reset(); matF->reset();


	matA->setEntry(0, 0, K2*K3/K1);
	matA->setEntry(1, 1, K1*K3/K2);
	matA->setEntry(2, 2, K1*K2/K3);

	fp_t A11 = matA->getEntry(0, 0);
	fp_t A22 = matA->getEntry(1, 1);
	fp_t A33 = matA->getEntry(2, 2);
	matB->setEntry(0, 0, (s2*K3 + s3*K2 - A11*s1) / eps_o);
	matB->setEntry(1, 1, (s1*K3 + s3*K1 - A22*s2) / eps_o);
	matB->setEntry(2, 2, (s1*K2 + s2*K1 - A33*s3) / eps_o);

	
	fp_t B11 = matB->getEntry(0, 0);
	fp_t B22 = matB->getEntry(1, 1);
	fp_t B33 = matB->getEntry(2, 2);
	matC->setEntry(0, 0, s2*s3/(eps_o * eps_o) - B11 * s1 / eps_o);
	matC->setEntry(1, 1, s1*s3/(eps_o * eps_o) - B22 * s2 / eps_o);
	matC->setEntry(2, 2, s1*s2/(eps_o * eps_o) - B33 * s3 / eps_o);
	
	matD->setEntry(0, 0, s1/(K1 * eps_o));
	matD->setEntry(1, 1, s2/(K2 * eps_o));
	matD->setEntry(2, 2, s3/(K3 * eps_o));

	matF->setEntry(0, 0, 1.0/K1);
	matF->setEntry(1, 1, 1.0/K2);
	matF->setEntry(2, 2, 1.0/K3);


}




















void tetra::Calculate_M_Matrix_I_H() {
	int i, j, k, dim;
    fp_t cval = 0.0;
    fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
    vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
                 // others [{0,1}, {0,2}, {0,3}]
    vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
                 // equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;
	fp_t coeffA[3][3];


	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	Local_DG_mapH(map, 0);

	if (LocalHDOF == 0) {
		MassI_H = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
		MassI_H->reset();
		return;
	}
	MassI_H = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
	dim = MassI_H->getRowDim();

	if (dim != MassI_H->getColDim()) {
			cerr << "MassI_H: Non-square matrix passed." << endl;
	}

	MassI_H->reset();
	geometry(lvtr, avtr, &vol);
	dim = MassI_H->getRowDim();

	tensor matrix_I(1.0,0.0,0.0,
					0.0,1.0,0.0,
				    0.0,0.0,1.0);

	makeCoeff(avtr, matrix_I, coeffA);
	// dotP(u, [t]*u) dV
	// u = avtr, [t]*u = [t] * avtr
	for (i = 0; i < dim; i++) {
			for (j = 0; j < dim; j++) {
					cval =
							(coeffA[0][0] * T00_P2[i][j]) + (coeffA[0][1] * T01_P2[i][j]) +
							(coeffA[1][0] * T10_P2[i][j]) + (coeffA[0][2] * T02_P2[i][j]) +
							(coeffA[2][0] * T20_P2[i][j]) + (coeffA[1][1] * T11_P2[i][j]) +
							(coeffA[1][2] * T12_P2[i][j]) + (coeffA[2][1] * T21_P2[i][j]) +
							(coeffA[2][2] * T22_P2[i][j]);

					cval = cval / (vol * (fp_t)T_DNUM);
					MassI_H->addEntry(i, j, cval);
			}
	}

	// If it's PMC, set a 1 if it's in the diagonal, 0 if not
	for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
		for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
			if (map[i] < 0 || map[j] < 0) {
				if (i == j) {
					MassI_H->setEntry(i, j, 1.0); // need to be inversed
				} else {
					MassI_H->setEntry(i, j, 0); // need to be inversed
				}
			}
		}
	}

	if (map != nullptr)
		delete[] map;
}




void tetra::Calculate_ABC_H() 
{    

	if (LocalHDOF == 0)
		return;

	int LocalDim, LocalFaceDim;
	int DGface_bc;
	int AbcFlag = 0;
	fp_t mu_o = Uo;
	fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
	fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));

	int i, j, k, dim;    
	LocalDim = TetPolyOrderDim[PolyOrderFlag];
	LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];

	denseMat<fp_t> tetBh(LocalDim, LocalDim);

	// Matrices On the Faces
	denseMat<fp_t> Bh(LocalFaceDim, LocalFaceDim);

	for (i = 0; i < 4; i++) {
		DGface_bc = getFacePtr(i)->bcPtr->getbType();

		if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
			AbcFlag = 1;
			fc[i]->Calculate_Bh_Matrix(&Bh);
			for (j = 0; j < LocalFaceDim; j++) {
				for (k = 0; k < LocalFaceDim; k++) {
					tetBh.addEntry(fac2tet[i][j], fac2tet[i][k], Bh.getEntry(j, k));
				}
			}
		}
	}

	// ABC Boundary
	if (AbcFlag == 1) {
		for (i = 0; i < 4; i++) {
			if (fc[i]->bcPtr->getbType() == abcType) {
				Z = fc[i]->bcPtr->get_ABCImpVal();
			} else if (fc[i]->bcPtr->getbType() == planeWaveType) {
				Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
			} else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
				Z = fc[i]->bcPtr->get_PortImpVal();
			}
		}
		tetBh.scale(-0.5 * Z);
	}

    ABC_tetBh = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    *ABC_tetBh = tetBh;
}

















void tetra::Calculate_M_Matrix_I_E() {
    int i, j, k, dim;
    fp_t cval = 0.0;
    fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
    vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
                 // others [{0,1}, {0,2}, {0,3}]
    vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
                 // equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;
	fp_t coeffA[3][3];


    int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
    Local_DG_mapE(map, 0);

    if (LocalEDOF == 0) 
	{
			cout << "RESET\n";
			MassI_E = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
			MassI_E->reset();
			return;
    }


    MassI_E = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    dim = MassI_E->getRowDim();
    if (dim != MassE->getColDim()) {
        cerr << "MassI_E: Non-square matrix passed." << endl;
    }
    MassI_E->reset();
    geometry(lvtr, avtr, &vol);
	dim = MassI_E->getRowDim();

	tensor matrix_I(1.0,0.0,0.0,
					0.0,1.0,0.0,
				    0.0,0.0,1.0);

	makeCoeff(avtr, matrix_I, coeffA);

    // dotP(u, [t]*u) dV
    // u = avtr, [t]*u = [t] * avtr
    for (i = 0; i < dim; i++) {
        for (j = 0; j < dim; j++) {
            cval =
                (coeffA[0][0] * T00_P2[i][j]) + (coeffA[0][1] * T01_P2[i][j]) +
                (coeffA[1][0] * T10_P2[i][j]) + (coeffA[0][2] * T02_P2[i][j]) +
                (coeffA[2][0] * T20_P2[i][j]) + (coeffA[1][1] * T11_P2[i][j]) +
                (coeffA[1][2] * T12_P2[i][j]) + (coeffA[2][1] * T21_P2[i][j]) +
                (coeffA[2][2] * T22_P2[i][j]);
            cval = cval / (vol * (fp_t)T_DNUM);

            MassI_E->addEntry(i, j, cval);
        }
    }

    // If it's PEC, set a 1 if it's in the diagonal, 0 if not
    for (i = 0; i < TetPolyOrderDim[PolyOrderFlag]; i++) {
        for (j = 0; j < TetPolyOrderDim[PolyOrderFlag]; j++) {
            if (map[i] < 0 || map[j] < 0) {
                if (i == j) {
                    MassI_E->setEntry(i, j, 1.0); // need to be inversed
                } else {
                    MassI_E->setEntry(i, j, 0); // need to be inversed
                }
            }
        }
    }


    if (map != nullptr)
        delete[] map;


}




void tetra::Calculate_ABC_E()
{
    if (LocalEDOF == 0)
        return;
    int LocalDim, LocalFaceDim;
    int DGface_bc;
    int AbcFlag = 0;

    fp_t eps_o = Eo;
    fp_t Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
    fp_t Z = No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0));
    int i, j, k, dim;

    LocalDim = TetPolyOrderDim[PolyOrderFlag];
    LocalFaceDim = FacePolyOrderDim[PolyOrderFlag];
    denseMat<fp_t> tetBe(LocalDim, LocalDim);
    // Matrices On the Faces
    denseMat<fp_t> Be(LocalFaceDim, LocalFaceDim);

    for (i = 0; i < 4; i++) {
        DGface_bc = fc[i]->bcPtr->getbType();
        if (DGface_bc == abcType || DGface_bc == planeWaveType || (DGface_bc >= portType && DGface_bc < pecType)) {
            AbcFlag = 1;
            fc[i]->Calculate_Be_Matrix(&Be);
            for (j = 0; j < LocalFaceDim; j++) {
                for (k = 0; k < LocalFaceDim; k++) {
                    tetBe.addEntry(fac2tet[i][j], fac2tet[i][k], Be.getEntry(j, k));
                }
            }
        }
    }

    // ABC Boundary
    if (AbcFlag == 1) {
        for (i = 0; i < 4; i++) {
            // the same
            if (fc[i]->bcPtr->getbType() == abcType) {
                Y = 1.0 / fc[i]->bcPtr->get_ABCImpVal();
            } else if (fc[i]->bcPtr->getbType() == planeWaveType) {
                Y = 1.0 / (No * sqrt(mat->mur.getEntry(0, 0) / mat->epsr.getEntry(0, 0)));
            } else if (fc[i]->bcPtr->getbType() >= portType && fc[i]->bcPtr->getbType() < pecType) {
                Y = 1.0 / fc[i]->bcPtr->get_PortImpVal();
            }
        }
        tetBe.scale(-0.5 * Y);
    }

    ABC_tetBe = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
    *ABC_tetBe = tetBe;
}















void tetra::Calculate_Mass_Material_Matrix(
	denseMat<fp_t>*& outputMassMat,         // Output mass matrix to fill
	const denseMat<fp_t>* inputTensorMat,  // Input tensor like A1, A3
	const tensor& materialTensor,          // Material tensor (e.g., epsr, mur)
	bool isElectricField                   // true → use E-field space, false → H-field space
) {
	if ((isElectricField && LocalEDOF == 0) || (!isElectricField && LocalHDOF == 0))
	{
		outputMassMat = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
		outputMassMat->reset();
		return;
	}

	outputMassMat = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);


	int i, j, dim;
	fp_t vol, cval = 0.0;
	fp_t coeffA[3][3];
	vtr lvtr[4], avtr[4];

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	if (isElectricField)
		Local_DG_mapE(map, 0);
	else
		Local_DG_mapH(map, 0);

	outputMassMat->reset();
	geometry(lvtr, avtr, &vol);
	dim = outputMassMat->getRowDim();

	// Form tensor from input matrix
	tensor Atensor(
		inputTensorMat->getEntry(0, 0), inputTensorMat->getEntry(0, 1), inputTensorMat->getEntry(0, 2),
		inputTensorMat->getEntry(1, 0), inputTensorMat->getEntry(1, 1), inputTensorMat->getEntry(1, 2),
		inputTensorMat->getEntry(2, 0), inputTensorMat->getEntry(2, 1), inputTensorMat->getEntry(2, 2)
	);

	tensor scaledTensor = materialTensor * Atensor;

	makeCoeff(avtr, scaledTensor, coeffA);

	for (i = 0; i < dim; ++i) {
		for (j = 0; j < dim; ++j) {
			cval =
				coeffA[0][0] * T00_P2[i][j] + coeffA[0][1] * T01_P2[i][j] +
				coeffA[1][0] * T10_P2[i][j] + coeffA[0][2] * T02_P2[i][j] +
				coeffA[2][0] * T20_P2[i][j] + coeffA[1][1] * T11_P2[i][j] +
				coeffA[1][2] * T12_P2[i][j] + coeffA[2][1] * T21_P2[i][j] +
				coeffA[2][2] * T22_P2[i][j];
			cval /= (vol * (fp_t)T_DNUM);

			outputMassMat->addEntry(i, j, cval);
		}
	}

	delete[] map;
}



void tetra::Calculate_Mass_Material_Matrix_Numeric(
	denseMat<fp_t>*& outputMassMat,         // Output mass matrix to fill
	const denseMat<fp_t>* inputTensorMat,  // Input tensor like A1, A3
	const tensor& materialTensor,          // Material tensor (e.g., epsr, mur)
	bool isElectricField                   // true → use E-field space, false → H-field space
) {
	if ((isElectricField && LocalEDOF == 0) || (!isElectricField && LocalHDOF == 0))
	{
		outputMassMat = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
		outputMassMat->reset();
		return;
	}

	outputMassMat = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);

	int i, j, k, dim;
	fp_t cval = 0.0;
	fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
	vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
					// others [{0,1}, {0,2}, {0,3}]
	vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
					// equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	if (isElectricField)
		Local_DG_mapE(map, 0);
	else
		Local_DG_mapH(map, 0);

	outputMassMat->reset();
	geometry(lvtr, avtr, &vol);
	dim = outputMassMat->getRowDim();

	// Form tensor from input matrix
	tensor Atensor(
		inputTensorMat->getEntry(0, 0), inputTensorMat->getEntry(0, 1), inputTensorMat->getEntry(0, 2),
		inputTensorMat->getEntry(1, 0), inputTensorMat->getEntry(1, 1), inputTensorMat->getEntry(1, 2),
		inputTensorMat->getEntry(2, 0), inputTensorMat->getEntry(2, 1), inputTensorMat->getEntry(2, 2)
	);

	tensor scaledTensor = materialTensor * Atensor;



	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;
			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = scaledTensor * w1;
		}
	}



	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			outputMassMat->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;



	if (map != nullptr)
		delete[] map;

}




void tetra::getCartesianCoord(fp_t *zeta, vtr &pnt)
{
	pnt.reset();
  
	for(int i = 0; i < NumOfNodes; i ++)
	{
	  pnt = pnt + nd[i]->getCoord() * zeta[i];
	}
}





void tetra::set_Conductivity_smoothProfile_Planar(fp_t planewave_xmin, fp_t planewave_ymin, fp_t planewave_zmin,
											fp_t planewave_xmax, fp_t planewave_ymax, fp_t planewave_zmax,
											vtr rc, vtr gaussPointCoord, string mode) 
{

    // Polynomial order of the PML conductivity profile
    fp_t m = static_cast<fp_t>(mat->get_PML_m_ord());

    // PML thickness
    fp_t pml_thick = mat->get_PML_thick();
    if (std::abs(pml_thick) < 1e-10)
    {
        std::cerr << "Error: PML thickness is zero or too small." << std::endl;
        return;
    }



	fp_t x1 = gaussPointCoord.getx();
	fp_t y1 = gaussPointCoord.gety();
	fp_t z1 = gaussPointCoord.getz();

	fp_t x2 = rc.getx();
	fp_t y2 = rc.gety();
	fp_t z2 = rc.getz();

    // Permittivity of free space
    fp_t eps_o = 8.854187817e-12;
    fp_t sigma_base = mat->sigma.getEntry(0, 0);  // Assumed max σ

    // Distance to PML boundaries and conductivities
    fp_t dx = 0.0, dy = 0.0, dz = 0.0;
    fp_t dx2 = 0.0, dy2 = 0.0, dz2 = 0.0;

	fp_t s1 = 0.0, s2 = 0.0, s3 = 0.0;

    if (x1 < planewave_xmin)
	{
        dx = planewave_xmin - x1;
        dx2 = planewave_xmin - x2;
	}
	else if (x1 > planewave_xmax)
	{
		dx = x1 - planewave_xmax;
		dx2 = x2 - planewave_xmax;
	}


    if (y1 < planewave_ymin)
	{
        dy = planewave_ymin - y1;
        dy2 = planewave_ymin - y2;
	}
	else if (y1 > planewave_ymax)
	{
		dy = y1 - planewave_ymax;
		dy2 = y2 - planewave_ymax;
	}


    if (z1 < planewave_zmin)
	{
        dz = planewave_zmin - z1;
        dz2 = planewave_zmin - z2;
	}
	else if (z1 > planewave_zmax)
	{
		dz = z1 - planewave_zmax;
		dz2 = z2 - planewave_zmax;
	}


    // Compute directional conductivity σi(x)
    if (dx > 0.0) 
    {
      s1 = sigma_base * std::pow(dx / pml_thick, m);
    }
    if (dy > 0.0) 
    {
      s2 = sigma_base * std::pow(dy / pml_thick, m);
    }
    if (dz > 0.0) 
    {
      s3 = sigma_base * std::pow(dz / pml_thick, m);
    }
	
	// diagonal of a cuboid
	fp_t diagonal = pml_thick * std::sqrt(3.0);  
	// Use Euclidean distance for growth profile
	fp_t distance = std::sqrt(dx2 * dx2 + dy2 * dy2 + dz2 * dz2);
	fp_t growth_factor = std::pow(distance / diagonal, m);

	// K scaling (Jacobians)
	fp_t K_max = 2.0;
	fp_t K1 = 1.0 + (K_max - 1.0) * growth_factor;
	fp_t K2 = K1;
	fp_t K3 = K1;
	

	// === ALLOCATE ONCE ===
	if (!matA) matA = new denseMat<fp_t>(3, 3);
	if (!matB) matB = new denseMat<fp_t>(3, 3);
	if (!matC) matC = new denseMat<fp_t>(3, 3);
	if (!matD) matD = new denseMat<fp_t>(3, 3);
	if (!matF) matF = new denseMat<fp_t>(3, 3);

	matA->reset(); matB->reset(); matC->reset(); matD->reset(); matF->reset();

	if (mode == "A" || mode == "ALL") {
        matA->reset();
        matA->setEntry(0, 0, K2 * K3 / K1);
        matA->setEntry(1, 1, K1 * K3 / K2);
        matA->setEntry(2, 2, K1 * K2 / K3);
    }

    if (mode == "B" || mode == "ALL") {

		matA->reset();
        matA->setEntry(0, 0, K2 * K3 / K1);
        matA->setEntry(1, 1, K1 * K3 / K2);
        matA->setEntry(2, 2, K1 * K2 / K3);

        matB->reset();
        fp_t A11 = matA->getEntry(0, 0);
        fp_t A22 = matA->getEntry(1, 1);
        fp_t A33 = matA->getEntry(2, 2);
        matB->setEntry(0, 0, (s2 * K3 + s3 * K2 - A11 * s1) / eps_o);
        matB->setEntry(1, 1, (s1 * K3 + s3 * K1 - A22 * s2) / eps_o);
        matB->setEntry(2, 2, (s1 * K2 + s2 * K1 - A33 * s3) / eps_o);
    }

    if (mode == "C" || mode == "ALL") {

		matA->reset();
        matA->setEntry(0, 0, K2 * K3 / K1);
        matA->setEntry(1, 1, K1 * K3 / K2);
        matA->setEntry(2, 2, K1 * K2 / K3);

		matB->reset();
        fp_t A11 = matA->getEntry(0, 0);
        fp_t A22 = matA->getEntry(1, 1);
        fp_t A33 = matA->getEntry(2, 2);
        matB->setEntry(0, 0, (s2 * K3 + s3 * K2 - A11 * s1) / eps_o);
        matB->setEntry(1, 1, (s1 * K3 + s3 * K1 - A22 * s2) / eps_o);
        matB->setEntry(2, 2, (s1 * K2 + s2 * K1 - A33 * s3) / eps_o);

        matC->reset();
        fp_t B11 = matB->getEntry(0, 0);
        fp_t B22 = matB->getEntry(1, 1);
        fp_t B33 = matB->getEntry(2, 2);
        matC->setEntry(0, 0, s2 * s3 / (eps_o * eps_o) - B11 * s1 / eps_o);
        matC->setEntry(1, 1, s1 * s3 / (eps_o * eps_o) - B22 * s2 / eps_o);
        matC->setEntry(2, 2, s1 * s2 / (eps_o * eps_o) - B33 * s3 / eps_o);
    }

    if (mode == "D" || mode == "ALL") {
        matD->reset();
        matD->setEntry(0, 0, s1 / (K1 * eps_o));
        matD->setEntry(1, 1, s2 / (K2 * eps_o));
        matD->setEntry(2, 2, s3 / (K3 * eps_o));
    }

    if (mode == "F" || mode == "ALL") {
        matF->reset();
        matF->setEntry(0, 0, 1.0 / K1);
        matF->setEntry(1, 1, 1.0 / K2);
        matF->setEntry(2, 2, 1.0 / K3);
    }	

}









void tetra::LocalRadiiCurvature(vtr& r, fp_t radius_x, fp_t radius_y, fp_t radius_z, fp_t& r01, fp_t& r02, vtr& u1, vtr& u2, vtr& u3) 
{

  fp_t a = radius_x;
  fp_t b = radius_y;
  fp_t c = radius_z;

  // Calculate the local radii of curvature for a tetrahedron
  fp_t u;
  if (fabs(r.gety()) < 1.e-15 && fabs(r.getx()) < 1.e-15)
    u = Pi / 2.;
  else
    u = atan2(r.gety(), r.getx());

  if (u < 0.0) u = u + 2.0 * Pi;
  fp_t v = acos(r.getz() / r.magnitude());

  // sphere case Test
  if (a == b && b == c) 
  {
    // vtr uu1, uu2, uu3;
    u1.setvtr(cos(u) * cos(v), sin(u) * cos(v), -sin(v));  // thet
    u2.setvtr(-sin(u), cos(u), 0.0);                       // phi
    u3.setvtr(cos(u) * sin(v), sin(u) * sin(v), cos(v));   // r
    r01 = r.magnitude();
    r02 = r.magnitude();
  }

  // Ellipsoid
  else 
  {
    if (u == 0. && v == 0.) 
	{
      r01 = b * b / c;
      r02 = a * a / c;
      u1.setvtr(1., 0., 0.);
      u2.setvtr(0., 1., 0.);
      u3 = u2 * u1;
    } 
	else 
	{
      // v = th;	u = ph;
      // Tangent vectors
      vtr Xph(-a * sin(v) * sin(u), b * sin(v) * cos(u), 0.0);
      vtr Xth(a * cos(v) * cos(u), b * sin(u) * cos(v), -c * sin(v));

      // First Fundamental form
      double E = ((b * b) * cos(u) * cos(u) + (a * a) * sin(u) * sin(u)) *
                 sin(v) * sin(v);
      double F = (b * b - a * a) * cos(u) * sin(u) * cos(v) * sin(v);
      double G = (a * a * cos(u) * cos(u) + b * b * sin(u) * sin(u)) * cos(v) *
                     cos(v) +
                 c * c * sin(v) * sin(v);
      // Second Fundamental form
      double L =
          (a * b * c * sin(v) * sin(v)) /
          (sqrt(a * a * b * b * cos(v) * cos(v) +
                c * c * (b * b * cos(u) * cos(u) + a * a * sin(u) * sin(u)) *
                    sin(v) * sin(v)));
      double M = 0.0;
      double N =
          (a * b * c) /
          (sqrt(a * a * b * b * cos(v) * cos(v) +
                c * c * (b * b * cos(u) * cos(u) + a * a * sin(u) * sin(u)) *
                    sin(v) * sin(v)));

      vtr e1 = Xph / sqrt(E);
      vtr e2 = (Xth * E - Xph * F) / sqrt(E * (E * G - F * F));
      e1.unitvtr();
      e2.unitvtr();
      vtr np = e2 * e1;
      double H = (G * L - 2.0 * F * M + E * N) / (2.0 * (E * G - F * F));
      double A = (L * (E * G - 2.0 * F * F) + 2.0 * E * F * M - E * E * N) /
                 (2.0 * E * (E * G - F * F));
      double B = (E * M - F * L) / (E * sqrt(E * G - F * F));
      double C = sqrt(A * A + B * B);
      double th0 = 0.5 * acos(A / C);
      double kmin = H - C;
      double kmax = H + C;
      double a0 = cos(0.5 * th0);
      vtr aa = np * sin(0.5 * th0);
      denseMat<fp_t> R(3, 3);
      R.setEntry(0, 0,
                 a0 * a0 + aa.getx() * aa.getx() - aa.gety() * aa.gety() -
                     aa.getz() * aa.getz());
      R.setEntry(0, 1, 2.0 * (aa.getx() * aa.gety() + a0 * aa.getz()));
      R.setEntry(0, 2, 2.0 * (aa.getx() * aa.getz() - a0 * aa.gety()));
      R.setEntry(1, 0, 2.0 * (aa.getx() * aa.gety() - a0 * aa.getz()));
      R.setEntry(1, 1,
                 a0 * a0 - aa.getx() * aa.getx() + aa.gety() * aa.gety() -
                     aa.getz() * aa.getz());
      R.setEntry(1, 2, 2.0 * (aa.gety() * aa.getz() + a0 * aa.getx()));
      R.setEntry(2, 0, 2.0 * (aa.getx() * aa.getz() + a0 * aa.gety()));
      R.setEntry(2, 1, 2.0 * (aa.gety() * aa.getz() - a0 * aa.getx()));
      R.setEntry(2, 2,
                 a0 * a0 - aa.getx() * aa.getx() - aa.gety() * aa.gety() +
                     aa.getz() * aa.getz());

		// Rotation to match the principal directions
		std::array<fp_t, 3> t1, t2, f1, f2;

		t1[0] = e1.getx();  t2[0] = e2.getx();
		t1[1] = e1.gety();  t2[1] = e2.gety();
		t1[2] = e1.getz();  t2[2] = e2.getz();

		// Matrix-vector multiplication: f1 = R * t1, f2 = R * t2
		MXV(&R, t1.data(), f1.data());
		MXV(&R, t2.data(), f2.data());

      // Principal raddii of Curvature
      r01 = 1. / kmax;
      r02 = 1. / kmin;
      // Normal at P(u,v)
      if (a == b && b == c && a == c) 
	  {
        //  sphere case
        u1.setvtr(cos(u) * cos(v), sin(u) * cos(v), -sin(v));  // thet
        u2.setvtr(-sin(u), cos(u), 0.0);                       // phi
        u3.setvtr(cos(u) * sin(v), sin(u) * sin(v), cos(v));   // r
      } 
	  else 
	  {
        u1.setvtr(f1[0], f1[1], f1[2]);
        u2.setvtr(f2[0], f2[1], f2[2]);
        u3 = u2 * u1;  // outward normal correct
      }

      u1.unitvtr();
      u2.unitvtr();
      u3.unitvtr();
    }
  }

  //   // Rotation Matrix
  if (!Jrot) Jrot = new denseMat<fp_t>(3, 3);
  Jrot->setEntry(0, 0, u1.getx());
  Jrot->setEntry(0, 1, u1.gety());
  Jrot->setEntry(0, 2, u1.getz());
  Jrot->setEntry(1, 0, u2.getx());
  Jrot->setEntry(1, 1, u2.gety());
  Jrot->setEntry(1, 2, u2.getz());
  Jrot->setEntry(2, 0, u3.getx());
  Jrot->setEntry(2, 1, u3.gety());
  Jrot->setEntry(2, 2, u3.getz());

}

void tetra::Et_S_E(denseMat<fp_t>* E, denseMat<fp_t>* S, denseMat<fp_t>* EtSE) 
{
	int i, j, k, l, dim;
	double sum1 = 0.0, value = 0.0;
	dim = S->getRowDim();
  
	for (i = 0; i < dim; i++) {
	  for (j = 0; j < dim; j++) {
		for (l = 0; l < dim; l++) {
		  for (k = 0; k < dim; k++) {
			sum1 = sum1 + S->getEntry(l, k) * E->getEntry(k, j);
		  }
		  value = value + E->getEntry(l, i) * sum1;
		  sum1 = 0.0;
		}
		EtSE->addEntry(i, j, value);
		value = 0.0;
	  }
	}
}
  

void tetra::ApplyConformalPMLRotation(denseMat<fp_t>* J, denseMat<fp_t>* tensorU1U2U3) 
{
    denseMat<fp_t> XYZ(3, 3);
    denseMat<fp_t> U1U2U3(3, 3);

    XYZ.reset();
    U1U2U3.reset();

    // Copy tensorU1U2U3 into U1U2U3
    for (int j = 0; j < 3; ++j)
        for (int k = 0; k < 3; ++k)
            U1U2U3.setEntry(j, k, tensorU1U2U3->getEntry(j, k));

    // Perform similarity transform: Et * S * E
    Et_S_E(J, &U1U2U3, &XYZ);

    // Copy result back into original tensor
    for (int j = 0; j < 3; ++j)
        for (int k = 0; k < 3; ++k)
            tensorU1U2U3->setEntry(j, k, XYZ.getEntry(j, k));
}



void tetra::set_Conductivity_smoothProfile_conformal(fp_t radius_x, fp_t radius_y, fp_t radius_z,
											vtr rc, vtr gaussPointCoord, string mode) 
{
    // Polynomial order of the PML conductivity profile
    fp_t m = static_cast<fp_t>(mat->get_PML_m_ord());

    // PML thickness
    fp_t pml_thick = mat->get_PML_thick();
    if (std::abs(pml_thick) < 1e-10)
    {
        std::cerr << "Error: PML thickness is zero or too small." << std::endl;
        return;
    }

    // Permittivity of free space
    fp_t eps_o = 8.854187817e-12;
    fp_t sigma_max = mat->sigma.getEntry(0, 0);  // Assumed max σ

    fp_t r01, r02;
    fp_t s1, s2, s3;

    fp_t ph = atan2(rc.gety(), rc.getx());
    if (ph < 0.0) ph = ph + 2.0 * Pi;
    fp_t th = acos(rc.getz() / rc.magnitude());
    
	vtr ro(radius_x * sin(th) * cos(ph), radius_y * sin(th) * sin(ph), radius_z * cos(th));
    vtr u1, u2, u3;
    
	
	LocalRadiiCurvature(rc, radius_x, radius_y, radius_z, r01, r02, u1, u2, u3);



    fp_t z3 = abs(dotP(rc - ro, u3));


    s1 = (sigma_max / ((m + 1) * pow(pml_thick, m) * (r01 + z3))) * abs(pow(z3, m + 1));
    s2 = (sigma_max / ((m + 1) * pow(pml_thick, m) * (r02 + z3))) * abs(pow(z3, m + 1));
    s3 = sigma_max * abs(pow(z3 / pml_thick, m));
    

	// K scaling (Jacobians)
	fp_t K_max = 2.0;
	fp_t growth_factor = std::pow(z3 / pml_thick, m);
	fp_t K1 = 1.0 + (K_max - 1.0) * growth_factor;
	fp_t K2 = K1;
	fp_t K3 = K1;
	

	// === ALLOCATE ONCE ===
	if (!matA) matA = new denseMat<fp_t>(3, 3);
	if (!matB) matB = new denseMat<fp_t>(3, 3);
	if (!matC) matC = new denseMat<fp_t>(3, 3);
	if (!matD) matD = new denseMat<fp_t>(3, 3);
	if (!matF) matF = new denseMat<fp_t>(3, 3);

	matA->reset(); matB->reset(); matC->reset(); matD->reset(); matF->reset();

	if (mode == "A" || mode == "ALL") {
        matA->reset();
        matA->setEntry(0, 0, K2 * K3 / K1);
        matA->setEntry(1, 1, K1 * K3 / K2);
        matA->setEntry(2, 2, K1 * K2 / K3);
		
		ApplyConformalPMLRotation(Jrot, matA);
    }

    if (mode == "B" || mode == "ALL") {

		matA->reset();
        matA->setEntry(0, 0, K2 * K3 / K1);
        matA->setEntry(1, 1, K1 * K3 / K2);
        matA->setEntry(2, 2, K1 * K2 / K3);

        matB->reset();
        fp_t A11 = matA->getEntry(0, 0);
        fp_t A22 = matA->getEntry(1, 1);
        fp_t A33 = matA->getEntry(2, 2);
        matB->setEntry(0, 0, (s2 * K3 + s3 * K2 - A11 * s1) / eps_o);
        matB->setEntry(1, 1, (s1 * K3 + s3 * K1 - A22 * s2) / eps_o);
        matB->setEntry(2, 2, (s1 * K2 + s2 * K1 - A33 * s3) / eps_o);
		
		ApplyConformalPMLRotation(Jrot, matB);

    }

    if (mode == "C" || mode == "ALL") {

		matA->reset();
        matA->setEntry(0, 0, K2 * K3 / K1);
        matA->setEntry(1, 1, K1 * K3 / K2);
        matA->setEntry(2, 2, K1 * K2 / K3);

		matB->reset();
        fp_t A11 = matA->getEntry(0, 0);
        fp_t A22 = matA->getEntry(1, 1);
        fp_t A33 = matA->getEntry(2, 2);
        matB->setEntry(0, 0, (s2 * K3 + s3 * K2 - A11 * s1) / eps_o);
        matB->setEntry(1, 1, (s1 * K3 + s3 * K1 - A22 * s2) / eps_o);
        matB->setEntry(2, 2, (s1 * K2 + s2 * K1 - A33 * s3) / eps_o);

        matC->reset();
        fp_t B11 = matB->getEntry(0, 0);
        fp_t B22 = matB->getEntry(1, 1);
        fp_t B33 = matB->getEntry(2, 2);
        matC->setEntry(0, 0, s2 * s3 / (eps_o * eps_o) - B11 * s1 / eps_o);
        matC->setEntry(1, 1, s1 * s3 / (eps_o * eps_o) - B22 * s2 / eps_o);
        matC->setEntry(2, 2, s1 * s2 / (eps_o * eps_o) - B33 * s3 / eps_o);

		ApplyConformalPMLRotation(Jrot, matC);

    }

    if (mode == "D" || mode == "ALL") {
        matD->reset();
        matD->setEntry(0, 0, s1 / (K1 * eps_o));
        matD->setEntry(1, 1, s2 / (K2 * eps_o));
        matD->setEntry(2, 2, s3 / (K3 * eps_o));

		ApplyConformalPMLRotation(Jrot, matD);
    }

    if (mode == "F" || mode == "ALL") {
        matF->reset();
        matF->setEntry(0, 0, 1.0 / K1);
        matF->setEntry(1, 1, 1.0 / K2);
        matF->setEntry(2, 2, 1.0 / K3);

		ApplyConformalPMLRotation(Jrot, matF);
    }		



}


void tetra::Calculate_Mass_Material_Matrix_Vary_Numeric(
	denseMat<fp_t>*& outputMassMat,         // Output mass matrix to fill
	const denseMat<fp_t>* inputTensorMat,  // Input tensor like A1, A3
	const tensor& materialTensor,          // Material tensor (e.g., epsr, mur)
	bool isElectricField,                   // true → use E-field space, false → H-field space
	string mode,
	fp_t planewave_xmin, fp_t planewave_ymin, fp_t planewave_zmin,
	fp_t planewave_xmax, fp_t planewave_ymax, fp_t planewave_zmax,
	fp_t radius_x, fp_t radius_y, fp_t radius_z
) {
	if ((isElectricField && LocalEDOF == 0) || (!isElectricField && LocalHDOF == 0))
	{
		outputMassMat = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);
		outputMassMat->reset();
		return;
	}

	
	outputMassMat = new denseMat<fp_t>(TetPolyOrderDim[PolyOrderFlag], TetPolyOrderDim[PolyOrderFlag]);

	int i, j, k, dim;
	fp_t cval = 0.0;
	fp_t vol;
	fp_t* z;
	fp_t wgt = 0.;
	vtr lvtr[4]; // the 3 first positions are the vectors from the node 0 to the
					// others [{0,1}, {0,2}, {0,3}]
	vtr avtr[4]; // array of vectors normal to the faces and with a magnitude
					// equal to the area of them [f0, f1, f2, f3]
	vtr w1, w2;

	int* map = new int[TetPolyOrderDim[PolyOrderFlag]];
	if (isElectricField)
		Local_DG_mapE(map, 0);
	else
		Local_DG_mapH(map, 0);

	outputMassMat->reset();
	geometry(lvtr, avtr, &vol);
	dim = outputMassMat->getRowDim();


	fp_t (*gaussQuadPoints)[5];
	GetFormula3dPtr(GAUSS_POINT_NUM[PolyOrderFlag], gaussQuadPoints);

	vtr* gaussBasisVtrs    = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	vtr* gaussMatBasisVtrs = new vtr[TetPolyOrderDim[PolyOrderFlag] * GAUSS_POINT_NUM[PolyOrderFlag]];
	for (i = 0; i < dim; i++){
		for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
			z = &gaussQuadPoints[k][0];
			TetraBasisW(z, i, &w1);
			gaussBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = w1;

			// Get the Cartesian coordinate of the Gauss point
			vtr gaussPointCoord;
			getCartesianCoord(z, gaussPointCoord);


			vtr rc = Center();

			if (radius_x > 0.0 && radius_y > 0.0 && radius_z > 0.0)
			{
				set_Conductivity_smoothProfile_conformal(radius_x, radius_y, radius_z,rc, rc, mode); 
			}
			else
			{
				set_Conductivity_smoothProfile_Planar(planewave_xmin, planewave_ymin, planewave_zmin, 
													planewave_xmax, planewave_ymax, planewave_zmax,
													rc, rc, mode); 
			}
												
			// Form tensor from input matrix
			tensor Atensor(
				inputTensorMat->getEntry(0, 0), inputTensorMat->getEntry(0, 1), inputTensorMat->getEntry(0, 2),
				inputTensorMat->getEntry(1, 0), inputTensorMat->getEntry(1, 1), inputTensorMat->getEntry(1, 2),
				inputTensorMat->getEntry(2, 0), inputTensorMat->getEntry(2, 1), inputTensorMat->getEntry(2, 2)
			);

			/*
			if (mode == "B" && isElectricField)
			{
				fp_t mv = mat->sigma.getEntry(0, 0) * 2.0;
				tensor mod (mv,0.0,0.0,0.0,mv,0.0,0.0,0.0,mv); // Modify the tensor for B-field
				Atensor = mod + Atensor;
			}
			*/

			tensor scaledTensor = materialTensor * Atensor;



			gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k] = scaledTensor * w1;
		}
	}



	for (i = 0; i < dim; i++){
		for (j = 0; j < dim; j++){
			for (k = 0; k < GAUSS_POINT_NUM[PolyOrderFlag]; k++) {
				wgt = gaussQuadPoints[k][4];
				w1 = gaussMatBasisVtrs[i * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				w2 = gaussBasisVtrs[j * GAUSS_POINT_NUM[PolyOrderFlag] + k];
				cval += wgt * dotP(w1, w2);
			}
			outputMassMat->addEntry(i, j, cval * vol);
			cval = 0.0;
		}
	}
	delete[] gaussBasisVtrs;
	delete[] gaussMatBasisVtrs;



	if (map != nullptr)
		delete[] map;

}


vtr tetra::get_centroid()
{
	vtr center(0.0,0.0,0.0);
	for (int n = 0; n < 4; ++n) 
	{
		center = center + nd[n]->getCoord();
	}
	center = center / 4.0;
	return center;
}