#include"Mesh_Base.h"
#include"../common/util.h"
#include<algorithm>
#include<array>
#include<complex>
#include<iostream>
#include<cmath>
#include<vector>

//get edges with edge's flag of domain
void Mesh_2D::GetIndexOfDomain(Eigen::VectorXi domain, Eigen::VectorXi& nodeIndex, Eigen::VectorXi& edgeIndex)
{
	int NbrEdges = 0;

	int NbrDomain = domain.rows();
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrEdges; j++)
		{
			if (GetDomainOfEdges(j) == domain(i))
				NbrEdges++;
		}
	}

	//store edgeIndex and nodeIndex
	Eigen::Vector2i coonOfEdges;
	int* tempEdgeIndex, * tempNodeIndex;
	tempEdgeIndex = new int[NbrEdges];
	tempNodeIndex = new int[NbrEdges * 2];
	int numNode = 0;
	int numEdge = 0;
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrEdges; j++)
		{
			if (GetDomainOfEdges(j) == domain(i))
			{
				this->GetCoonOfEdges(j, coonOfEdges); //coonOfEdges 0:tri 1:numOfEdge
				tempEdgeIndex[numEdge] = GetEdgeOfTri(coonOfEdges(0), coonOfEdges(1));
				numEdge++;
				tempNodeIndex[numNode] = GetEdge(GetEdgeOfTri(coonOfEdges(0), coonOfEdges(1)), 0);
				numNode++;
				tempNodeIndex[numNode] = GetEdge(GetEdgeOfTri(coonOfEdges(0), coonOfEdges(1)), 1);
				numNode++;
			}
		}
	}



	//sort and unique nodeIndex
	std::sort(tempNodeIndex, tempNodeIndex + NbrEdges * 2);
	int NbrNodes = (std::unique(tempNodeIndex, tempNodeIndex + NbrEdges * 2) - tempNodeIndex);
	nodeIndex = Eigen::VectorXi::Zero(NbrNodes);
	for (int i = 0; i < NbrNodes; i++)
		nodeIndex(i) = tempNodeIndex[i];

	//sort edgeIndex
	edgeIndex = Eigen::VectorXi::Zero(NbrEdges);
	std::sort(tempEdgeIndex, tempEdgeIndex + NbrEdges);
	for (int i = 0; i < NbrEdges; i++)
		edgeIndex(i) = tempEdgeIndex[i];

	delete[] tempEdgeIndex, tempNodeIndex;


}

void Mesh_2D::GetIndexOfDomain2(Eigen::VectorXi domain, Eigen::VectorXi& edgeIndex, Eigen::VectorXi& edgeNum)
{
	int NbrEdges = 0;

	int NbrDomain = domain.rows();
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrEdges; j++)
		{
			if (GetDomainOfEdges(j) == domain(i))
				NbrEdges++;
		}
	}

	//store edgeIndex
	edgeIndex = Eigen::VectorXi::Zero(NbrEdges);
	edgeNum = Eigen::VectorXi::Zero(NbrEdges);
	int numEdge = 0;
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrEdges; j++)
		{
			if (GetDomainOfEdges(j) == domain(i))
			{
				edgeIndex(numEdge) = j;
				edgeNum(numEdge) = i;
				numEdge++;
			}
		}
	}

}

namespace {

void appendFaceEdgesOfTri(Mesh_3D* mesh, int triIdx, std::vector<int>& edges)
{
	Eigen::Vector3i conn;
	mesh->GetCoonOfTri(triIdx, conn);
	const int numTet = conn(0);
	const int numFace = conn(1) + 1;
	int e0 = 0, e1 = 0, e2 = 0;
	if (numFace == 1)
	{
		e0 = mesh->GetEdgeOfTet(numTet, 0);
		e1 = mesh->GetEdgeOfTet(numTet, 1);
		e2 = mesh->GetEdgeOfTet(numTet, 3);
	}
	else if (numFace == 2)
	{
		e0 = mesh->GetEdgeOfTet(numTet, 0);
		e1 = mesh->GetEdgeOfTet(numTet, 2);
		e2 = mesh->GetEdgeOfTet(numTet, 4);
	}
	else if (numFace == 3)
	{
		e0 = mesh->GetEdgeOfTet(numTet, 1);
		e1 = mesh->GetEdgeOfTet(numTet, 2);
		e2 = mesh->GetEdgeOfTet(numTet, 5);
	}
	else if (numFace == 4)
	{
		e0 = mesh->GetEdgeOfTet(numTet, 3);
		e1 = mesh->GetEdgeOfTet(numTet, 4);
		e2 = mesh->GetEdgeOfTet(numTet, 5);
	}
	else
	{
		return;
	}
	edges.push_back(e0);
	edges.push_back(e1);
	edges.push_back(e2);
}

void collectEdgesOnDomains(Mesh_3D* mesh, const Eigen::VectorXi& domains, std::vector<int>& edges)
{
	// Mirror MATLAB findTri + edge collect: gather/sort tris, then unique edges.
	std::vector<int> triIndex;
	for (int d = 0; d < domains.size(); d++)
	{
		const int domainId = domains(d);
		for (int j = 0; j < mesh->GetNbrTri(); j++)
		{
			if (mesh->GetDomainOfTri(j) == domainId)
				triIndex.push_back(j);
		}
	}
	std::sort(triIndex.begin(), triIndex.end());
	for (int j : triIndex)
		appendFaceEdgesOfTri(mesh, j, edges);
	std::sort(edges.begin(), edges.end());
	edges.erase(std::unique(edges.begin(), edges.end()), edges.end());
}

void getEdgeVertices(Mesh_3D* mesh, int edgeId, Eigen::Vector3d& p0, Eigen::Vector3d& p1)
{
	mesh->GetVertex(mesh->GetEdge(edgeId, 0), p0);
	mesh->GetVertex(mesh->GetEdge(edgeId, 1), p1);
}

bool matchEdgePair(Mesh_3D* mesh, int srcEdge, int dstEdge, double theta, double tol, int& signOut)
{
	Eigen::Vector3d v1, v2, v3, v4;
	getEdgeVertices(mesh, srcEdge, v1, v2);
	getEdgeVertices(mesh, dstEdge, v3, v4);

	Eigen::Matrix3d traMat;
	traMat << std::cos(theta), -std::sin(theta), 0.0,
		std::sin(theta), std::cos(theta), 0.0,
		0.0, 0.0, 1.0;
	v3 = traMat * v3;
	v4 = traMat * v4;

	const double l1 = (v1 - v3).norm();
	const double l2 = (v2 - v4).norm();
	const double l3 = (v1 - v4).norm();
	const double l4 = (v2 - v3).norm();
	if (l1 + l2 < tol)
	{
		signOut = 1;
		return true;
	}
	if (l3 + l4 < tol)
	{
		signOut = -1;
		return true;
	}
	return false;
}

// ---------------------------------------------------------------------------
// PBC pairing — mirrors MATLAB:
//   order1 translation:  matlab 3D一阶基+散射边界条件+周期边界/findPBCIndex.m
//   order2 faces:        matlab 3D二阶基+...+单周期边界/findPBCIndex.m (face
//                        branch), with translation instead of rotation.
// ---------------------------------------------------------------------------

static double matlabPbcTol(const Eigen::Vector3d& dis)
{
	// order1: err = norm(dis)*5e-5 (keep bit-identical for validated order1)
	const double dl = dis.norm();
	return (dl > 0.0) ? dl * 0.00005 : 0.01 * 0.00005;
}

static double matlabFacePbcTol(const Eigen::Vector3d& dis)
{
	// order2 face branch hardcodes dl=0.01 → err=5e-7. For µm translation
	// periods, norm(dis)*5e-5 is ~1e-10 and rejects true pairs; keep the
	// order2 floor while still scaling with larger periods.
	const double dl = dis.norm();
	const double rel = (dl > 0.0) ? dl * 0.00005 : 0.0;
	return std::max(rel, 0.01 * 0.00005);
}

bool matchEdgePairTranslation(Mesh_3D* mesh, int srcEdge, int dstEdge,
	const Eigen::Vector3d& dis, double tol, int& signOut)
{
	// MATLAB: l1=|‖v1−v3‖−dl|, l2=|‖v2−v4‖−dl|; flip uses (v1,v4)/(v2,v3).
	Eigen::Vector3d v1, v2, v3, v4;
	getEdgeVertices(mesh, srcEdge, v1, v2);
	getEdgeVertices(mesh, dstEdge, v3, v4);
	const double dl = dis.norm();
	const double l1 = std::abs((v1 - v3).norm() - dl);
	const double l2 = std::abs((v2 - v4).norm() - dl);
	const double l3 = std::abs((v1 - v4).norm() - dl);
	const double l4 = std::abs((v2 - v3).norm() - dl);
	if ((l1 + l2) < tol)
	{
		signOut = 1;
		return true;
	}
	if ((l3 + l4) < tol)
	{
		signOut = -1;
		return true;
	}
	return false;
}

void pairEdgesByTranslation(Mesh_3D* mesh,
	const std::vector<int>& srcEdges,
	const std::vector<int>& dstEdges,
	const Eigen::Vector3d& dis,
	std::vector<int>& pairedSrc,
	std::vector<int>& pairedDst,
	std::vector<int>& pairedSign,
	bool useFaceStyleTolFloor = false)
{
	const double tol = useFaceStyleTolFloor ? matlabFacePbcTol(dis) : matlabPbcTol(dis);

	if (!useFaceStyleTolFloor)
	{
		// Order-1: MATLAB first-match, no destination locking (bit-identical).
		for (int srcEdge : srcEdges)
		{
			bool matched = false;
			for (int dstEdge : dstEdges)
			{
				int sign = 1;
				if (matchEdgePairTranslation(mesh, srcEdge, dstEdge, dis, tol, sign))
				{
					pairedSrc.push_back(srcEdge);
					pairedDst.push_back(dstEdge);
					pairedSign.push_back(sign);
					matched = true;
					break;
				}
			}
			if (!matched)
			{
				pairedSrc.push_back(srcEdge);
				pairedDst.push_back(srcEdge);
				pairedSign.push_back(1);
			}
		}
		return;
	}

	// Order-2: first-match with loose tol, then drop duplicate dst (keep first).
	// Pure first-match without dedup breaks projection (duplicate dst).
	std::vector<int> srcU = srcEdges;
	std::vector<int> dstU = dstEdges;
	std::sort(srcU.begin(), srcU.end());
	srcU.erase(std::unique(srcU.begin(), srcU.end()), srcU.end());
	std::sort(dstU.begin(), dstU.end());
	dstU.erase(std::unique(dstU.begin(), dstU.end()), dstU.end());

	std::vector<char> dstUsed(static_cast<size_t>(dstU.size()), 0);
	for (int srcEdge : srcU)
	{
		bool matched = false;
		for (size_t dj = 0; dj < dstU.size(); dj++)
		{
			if (dstUsed[dj])
				continue;
			int sign = 1;
			if (!matchEdgePairTranslation(mesh, srcEdge, dstU[dj], dis, tol, sign))
				continue;
			pairedSrc.push_back(srcEdge);
			pairedDst.push_back(dstU[dj]);
			pairedSign.push_back(sign);
			dstUsed[dj] = 1;
			matched = true;
			break;
		}
		if (!matched)
		{
			pairedSrc.push_back(srcEdge);
			pairedDst.push_back(srcEdge);
			pairedSign.push_back(1);
		}
	}
	return;
}

void pairEdgesByGeometry(Mesh_3D* mesh,
	const std::vector<int>& srcEdges,
	const std::vector<int>& dstEdges,
	double theta,
	std::vector<int>& pairedSrc,
	std::vector<int>& pairedDst,
	std::vector<int>& pairedSign)
{
	// MATLAB rotation PBC: err = 0.01*5e-5, first match, no dst lock.
	const double tol = 0.01 * 0.00005;
	for (int srcEdge : srcEdges)
	{
		bool matched = false;
		for (int dstEdge : dstEdges)
		{
			int sign = 1;
			if (matchEdgePair(mesh, srcEdge, dstEdge, theta, tol, sign))
			{
				pairedSrc.push_back(srcEdge);
				pairedDst.push_back(dstEdge);
				pairedSign.push_back(sign);
				matched = true;
				break;
			}
		}
		if (!matched)
		{
			pairedSrc.push_back(srcEdge);
			pairedDst.push_back(srcEdge);
			pairedSign.push_back(1);
		}
	}
}

void getFaceCentroid(Mesh_3D* mesh, int faceId, Eigen::Vector3d& centroid)
{
	Eigen::Vector3d v0, v1, v2;
	mesh->GetVertex(mesh->GetFace(faceId, 0), v0);
	mesh->GetVertex(mesh->GetFace(faceId, 1), v1);
	mesh->GetVertex(mesh->GetFace(faceId, 2), v2);
	centroid = (v0 + v1 + v2) / 3.0;
}

void collectFacesOnDomains(Mesh_3D* mesh, const Eigen::VectorXi& domains, std::vector<int>& faces)
{
	// Mirror MATLAB findTri: gather tris on all domains, sort, then FaceOfTet.
	std::vector<int> triIndex;
	for (int d = 0; d < domains.size(); d++)
	{
		const int domainId = domains(d);
		for (int j = 0; j < mesh->GetNbrTri(); j++)
		{
			if (mesh->GetDomainOfTri(j) == domainId)
				triIndex.push_back(j);
		}
	}
	std::sort(triIndex.begin(), triIndex.end());
	faces.clear();
	faces.reserve(triIndex.size());
	for (int j : triIndex)
	{
		Eigen::Vector3i conn;
		mesh->GetCoonOfTri(j, conn);
		faces.push_back(mesh->GetFaceOfTet(conn(0), conn(1)));
	}
}

bool matchFacePair(Mesh_3D* mesh, int srcFace, int dstFace, double theta, double tol)
{
	// MATLAB order2 face branch (rotation): ‖c_src − R·c_dst‖ < err, sign = +1.
	Eigen::Vector3d c1, c2;
	getFaceCentroid(mesh, srcFace, c1);
	getFaceCentroid(mesh, dstFace, c2);
	Eigen::Matrix3d traMat;
	traMat << std::cos(theta), -std::sin(theta), 0.0,
		std::sin(theta), std::cos(theta), 0.0,
		0.0, 0.0, 1.0;
	c2 = traMat * c2;
	return (c1 - c2).norm() < tol;
}

bool matchFacePairTranslation(Mesh_3D* mesh, int srcFace, int dstFace,
	const Eigen::Vector3d& dis, double tol)
{
	// Translation port of MATLAB face branch: transform dst by −dis (src+dis=dst).
	Eigen::Vector3d c1, c2;
	getFaceCentroid(mesh, srcFace, c1);
	getFaceCentroid(mesh, dstFace, c2);
	return (c1 - (c2 - dis)).norm() < tol;
}

void pairFacesByTranslation(Mesh_3D* mesh,
	const std::vector<int>& srcFaces,
	const std::vector<int>& dstFaces,
	const Eigen::Vector3d& dis,
	std::vector<int>& pairedSrc,
	std::vector<int>& pairedDst,
	std::vector<int>& pairedSign)
{
	// Face branch of MATLAB findPBCIndex (translation form of ‖c_src−T(c_dst)‖).
	// MATLAB uses first-match without dst-lock; that yields duplicate dst rows and
	// breaks buildPeriodicProjection* (assumes unique sorted dst). We unique face
	// ids, then assign globally best residual first with exclusive dst — same
	// geometric criterion, projection-safe.
	std::vector<int> srcU = srcFaces;
	std::vector<int> dstU = dstFaces;
	std::sort(srcU.begin(), srcU.end());
	srcU.erase(std::unique(srcU.begin(), srcU.end()), srcU.end());
	std::sort(dstU.begin(), dstU.end());
	dstU.erase(std::unique(dstU.begin(), dstU.end()), dstU.end());

	const double tol = matlabFacePbcTol(dis);
	const int nSrc = static_cast<int>(srcU.size());
	const int nDst = static_cast<int>(dstU.size());

	struct Cand { double err; int si; int di; };
	std::vector<Cand> cands;
	cands.reserve(static_cast<size_t>(nSrc));
	for (int si = 0; si < nSrc; si++)
	{
		Eigen::Vector3d c1;
		getFaceCentroid(mesh, srcU[static_cast<size_t>(si)], c1);
		for (int di = 0; di < nDst; di++)
		{
			Eigen::Vector3d c2;
			getFaceCentroid(mesh, dstU[static_cast<size_t>(di)], c2);
			const double err = (c1 - (c2 - dis)).norm();
			if (err < tol)
				cands.push_back({ err, si, di });
		}
	}
	std::sort(cands.begin(), cands.end(),
		[](const Cand& a, const Cand& b) { return a.err < b.err; });

	std::vector<int> matchDst(static_cast<size_t>(nSrc), -1);
	std::vector<char> srcUsed(static_cast<size_t>(nSrc), 0);
	std::vector<char> dstUsed(static_cast<size_t>(nDst), 0);
	for (const Cand& c : cands)
	{
		if (srcUsed[static_cast<size_t>(c.si)] || dstUsed[static_cast<size_t>(c.di)])
			continue;
		srcUsed[static_cast<size_t>(c.si)] = 1;
		dstUsed[static_cast<size_t>(c.di)] = 1;
		matchDst[static_cast<size_t>(c.si)] = c.di;
	}

	for (int si = 0; si < nSrc; si++)
	{
		pairedSrc.push_back(srcU[static_cast<size_t>(si)]);
		if (matchDst[static_cast<size_t>(si)] >= 0)
			pairedDst.push_back(dstU[static_cast<size_t>(matchDst[static_cast<size_t>(si)])]);
		else
			pairedDst.push_back(srcU[static_cast<size_t>(si)]);
		pairedSign.push_back(1);
	}
}

void pairFacesByGeometry(Mesh_3D* mesh,
	const std::vector<int>& srcFaces,
	const std::vector<int>& dstFaces,
	double theta,
	std::vector<int>& pairedSrc,
	std::vector<int>& pairedDst,
	std::vector<int>& pairedSign)
{
	const double tol = 0.01 * 0.00005;
	for (int srcFace : srcFaces)
	{
		bool matched = false;
		for (int dstFace : dstFaces)
		{
			if (matchFacePair(mesh, srcFace, dstFace, theta, tol))
			{
				pairedSrc.push_back(srcFace);
				pairedDst.push_back(dstFace);
				pairedSign.push_back(1);
				matched = true;
				break;
			}
		}
		if (!matched)
		{
			pairedSrc.push_back(srcFace);
			pairedDst.push_back(srcFace);
			pairedSign.push_back(1);
		}
	}
}

void removeSelfPairs(std::vector<int>& srcEdges,
	std::vector<int>& dstEdges,
	std::vector<std::complex<double>>& phis)
{
	std::vector<int> newSrc, newDst;
	std::vector<std::complex<double>> newPhi;
	for (size_t i = 0; i < srcEdges.size(); i++)
	{
		if (srcEdges[i] == dstEdges[i])
			continue;
		newSrc.push_back(srcEdges[i]);
		newDst.push_back(dstEdges[i]);
		newPhi.push_back(phis[i]);
	}
	srcEdges.swap(newSrc);
	dstEdges.swap(newDst);
	phis.swap(newPhi);
}

} // namespace

void Mesh_2D::GetIndexOfPBC(Eigen::MatrixXi PBC, Eigen::VectorXcd PBCData,
	Eigen::VectorXi& srcNodeIndex, Eigen::VectorXi& dstNodeIndex, Eigen::VectorXcd& nodePhi,
	Eigen::VectorXi& srcEdgeIndex, Eigen::VectorXi& dstEdgeIndex, Eigen::VectorXcd& edgePhi)
{
	int NbrEdges = 0;

	int NbrDomain = PBC.rows();
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrEdges; j++)
		{
			if (GetDomainOfEdges(j)-1 == PBC(i, 0))
				NbrEdges++;
		}
	}
	//store srcEdgeIndex and srcNodeIndex
	Eigen::Vector2i coonOfEdges;
	int* tempSrcEdgeIndex, * tempSrcNodeIndex;
	tempSrcEdgeIndex = new int[NbrEdges];
	tempSrcNodeIndex = new int[NbrEdges * 2];
	std::complex<double>* tempEdgePhi, * tempNodePhi;
	tempEdgePhi = new std::complex<double>[NbrEdges];
	tempNodePhi = new std::complex<double>[NbrEdges * 2];
	int numNode = 0;
	int numEdge = 0;
	int Tage_numEdge = 0;
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrEdges; j++)
		{
			if (GetDomainOfEdges(j) - 1 == PBC(i, 0))
			{
				tempSrcEdgeIndex[numEdge] = j;
				tempEdgePhi[numEdge] = PBCData(i);
				numEdge++;
				this->GetCoonOfEdges(j, coonOfEdges);
				Tage_numEdge = GetEdgeOfTri(coonOfEdges(0), coonOfEdges(1));
				tempSrcNodeIndex[numNode] = GetEdge(Tage_numEdge, 0);
				tempNodePhi[numNode] = PBCData(i);
				numNode++;
				tempSrcNodeIndex[numNode] = GetEdge(Tage_numEdge, 1);
				tempNodePhi[numNode] = PBCData(i);
				numNode++;
			}
		}
	}
	//store dstEdgeIndex and dstNodeIndex
	int* tempDstEdgeIndex, * tempDstNodeIndex;
	tempDstEdgeIndex = new int[NbrEdges];
	tempDstNodeIndex = new int[NbrEdges * 2];
	for (int i = 0; i < NbrEdges; i++)
	{
		for (int j = 0; j < _mCopyOfEdges.rows(); j++)
		{
			if (tempSrcEdgeIndex[i] == _mCopyOfEdges(j, 0))
			{
				tempDstEdgeIndex[i] = _mCopyOfEdges(j, 1);
				break;
			}

		}
	}
	for (int i = 0; i < NbrEdges; i++)
	{
		this->GetCoonOfEdges(tempDstEdgeIndex[i], coonOfEdges);
		Tage_numEdge = GetEdgeOfTri(coonOfEdges(0), coonOfEdges(1));
		tempDstNodeIndex[i * 2] = GetEdge(Tage_numEdge, 0);
		tempDstNodeIndex[i * 2 + 1] = GetEdge(Tage_numEdge, 1);
	}
	//????????? DstNodeIndex ?? edgePhi
	//??????rotational bloch bounadry
	for (int i = 0; i < NbrEdges; i++)
	{
		for (int j = 0; j < _mCopyOfVertex.rows(); j++)
		{
			if (tempSrcNodeIndex[2 * i] == _mCopyOfVertex(j, 0) && tempDstNodeIndex[2 * i + 1] == _mCopyOfVertex(j, 1))
			{
				int temp = tempDstNodeIndex[i * 2];
				tempDstNodeIndex[i * 2] = tempDstNodeIndex[i * 2 + 1];
				tempDstNodeIndex[i * 2 + 1] = temp;
                         tempEdgePhi[i] = - tempEdgePhi[i];
			}
		}
	}
	srcEdgeIndex = Eigen::VectorXi::Zero(NbrEdges);
	dstEdgeIndex = Eigen::VectorXi::Zero(NbrEdges);
	edgePhi = Eigen::VectorXcd::Zero(NbrEdges);
	for (int i = 0; i < NbrEdges; i++)
	{
		this->GetCoonOfEdges(tempSrcEdgeIndex[i], coonOfEdges);
		Tage_numEdge = GetEdgeOfTri(coonOfEdges(0), coonOfEdges(1));
		srcEdgeIndex(i) = Tage_numEdge;
		this->GetCoonOfEdges(tempDstEdgeIndex[i], coonOfEdges);
		Tage_numEdge = GetEdgeOfTri(coonOfEdges(0), coonOfEdges(1));
		dstEdgeIndex(i) = Tage_numEdge;
		edgePhi(i) = tempEdgePhi[i];
	}
	//???????
	Eigen::VectorXi overlapIndex1, overlapIndex2;
	overlapIndex1 = Eigen::VectorXi::Zero(NbrEdges * 2);
	overlapIndex2 = Eigen::VectorXi::Zero(NbrEdges * 2);
	int Nbr_overlap;
	Nbr_overlap = 0;
	for (int i = 0; i < NbrEdges * 2; i++)
	{
		for (int j = i + 1; j < NbrEdges * 2; j++)
		{

			if (tempSrcNodeIndex[i] == tempSrcNodeIndex[j])
			{
				if (tempDstNodeIndex[i] == tempDstNodeIndex[j])
				{
					overlapIndex1[Nbr_overlap] = i;
					overlapIndex2[Nbr_overlap] = j;
					Nbr_overlap = Nbr_overlap + 1;
					break;
				}
			}

		}
	}
	overlapIndex2.conservativeResize(Nbr_overlap);
	int tempNum = 0;
	srcNodeIndex = Eigen::VectorXi::Zero(NbrEdges * 2 - Nbr_overlap);
	dstNodeIndex = Eigen::VectorXi::Zero(NbrEdges * 2 - Nbr_overlap);
	nodePhi = Eigen::VectorXcd::Zero(NbrEdges * 2 - Nbr_overlap);
	for (int i = 0; i < NbrEdges * 2; i++)
	{
		if ((overlapIndex2.array() == i).any())
		{
			tempNum++;
			continue;
		}
		srcNodeIndex(i - tempNum) = tempSrcNodeIndex[i];
		dstNodeIndex(i - tempNum) = tempDstNodeIndex[i];
		nodePhi(i - tempNum) = tempNodePhi[i];
	}
	srcNodeIndex.conservativeResize(NbrEdges * 2 - Nbr_overlap);
	dstNodeIndex.conservativeResize(NbrEdges * 2 - Nbr_overlap);
	int blochIndex, blochtemp = -1;
	for (int i = 0; i < NbrEdges * 2 - Nbr_overlap; i++)
	{
		for (int j = i + 1; j < NbrEdges * 2 - Nbr_overlap; j++)
		{
			if (srcNodeIndex(i) == srcNodeIndex(j))
			{
				blochtemp = srcNodeIndex[i];
			}
		}
	}
	for (int i = 0; i < NbrEdges * 2 - Nbr_overlap; i++)
	{
		for (int j = i + 1; j < NbrEdges * 2 - Nbr_overlap; j++)
		{
			if (dstNodeIndex(i) == dstNodeIndex(j))
			{
				srcNodeIndex(i) = blochtemp;
				nodePhi(i) = nodePhi(i) * nodePhi(j);
				if (j != srcNodeIndex.size() - 1)
				{
					srcNodeIndex.segment(j, NbrEdges * 2 - Nbr_overlap - j - 1) = srcNodeIndex.segment(j + 1, NbrEdges * 2 - Nbr_overlap - j - 1);
					dstNodeIndex.segment(j, NbrEdges * 2 - Nbr_overlap - j - 1) = dstNodeIndex.segment(j + 1, NbrEdges * 2 - Nbr_overlap - j - 1);
				}
			}
		}
	}
	if (blochtemp != -1)
	{
		srcNodeIndex.conservativeResize(NbrEdges * 2 - Nbr_overlap - 1);
		dstNodeIndex.conservativeResize(NbrEdges * 2 - Nbr_overlap - 1);
	}
	delete[] tempSrcEdgeIndex, tempSrcNodeIndex, tempEdgePhi, tempNodePhi, tempDstEdgeIndex, tempDstNodeIndex;
}

void Mesh_2D::GetEdgesIndexOfDomain(int domain, Eigen::VectorXi& edgesIndex)
{
	int NbrEdgesIndex = 0;
	for (int j = 0; j < _mNbrEdges; j++)
	{
		if (GetDomainOfEdges(j) == domain)
			NbrEdgesIndex++;
	}

	edgesIndex = Eigen::VectorXi::Zero(NbrEdgesIndex);
	int NumEdgesIndex = 0;
	for (int j = 0; j < _mNbrEdges; j++)
	{
		if (GetDomainOfEdges(j) == domain)
		{
			edgesIndex(NumEdgesIndex) = j;
			NumEdgesIndex++;
		}
	}
}

void Mesh_3D::GetTriIndexOfDomain(Eigen::VectorXi domain, Eigen::VectorXi& edgeIndex)
{
	int NbrTri = 0;

	int NbrDomain = domain.rows();
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrTri; j++)
		{
			if (GetDomainOfTri(j) == domain(i))
				NbrTri++;
		}
	}

	//store edgeIndex
	Eigen::Vector3i coonOfTri;
	int* tempEdgeIndex;
	tempEdgeIndex = new int[NbrTri * 3];
	int numTri = 0;
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrTri; j++)
		{
			if (GetDomainOfTri(j) == domain(i))
			{
				Eigen::Vector3i connOfTri;
				this->GetCoonOfTri(j, connOfTri);
				if (connOfTri(0) == 0)
				{
					if (connOfTri(2) == 0)
					{
						tempEdgeIndex[numTri * 3] = this->GetEdgeOfTet(connOfTri(1), 0);
						tempEdgeIndex[numTri * 3 + 1] = this->GetEdgeOfTet(connOfTri(1), 1);
						tempEdgeIndex[numTri * 3 + 2] = this->GetEdgeOfTet(connOfTri(1), 2);
					}
					else if (connOfTri(2) == 1)
					{
						tempEdgeIndex[numTri * 3] = this->GetEdgeOfTet(connOfTri(1), 0);
						tempEdgeIndex[numTri * 3 + 1] = this->GetEdgeOfTet(connOfTri(1), 2);
						tempEdgeIndex[numTri * 3 + 2] = this->GetEdgeOfTet(connOfTri(1), 4);
					}
					else if (connOfTri(2) == 2)
					{
						tempEdgeIndex[numTri * 3] = this->GetEdgeOfTet(connOfTri(1), 1);
						tempEdgeIndex[numTri * 3 + 1] = this->GetEdgeOfTet(connOfTri(1), 2);
						tempEdgeIndex[numTri * 3 + 2] = this->GetEdgeOfTet(connOfTri(1), 5);
					}
					else if (connOfTri(2) == 3)
					{
						tempEdgeIndex[numTri * 3] = this->GetEdgeOfTet(connOfTri(1), 3);
						tempEdgeIndex[numTri * 3 + 1] = this->GetEdgeOfTet(connOfTri(1), 4);
						tempEdgeIndex[numTri * 3 + 2] = this->GetEdgeOfTet(connOfTri(1), 5);
					}
				}
				else
				{

				}
			}
		}
	}

	//sort and unique nodeIndex
	std::sort(tempEdgeIndex, tempEdgeIndex + NbrTri * 3);
	int NbrEdge = (std::unique(tempEdgeIndex, tempEdgeIndex + NbrTri * 3) - tempEdgeIndex);
	edgeIndex = Eigen::VectorXi::Zero(NbrEdge);
	for (int i = 0; i < NbrEdge; i++)
		edgeIndex(i) = tempEdgeIndex[i];

	delete[] tempEdgeIndex;
}

void Mesh_3D::GetTriIndexOfDomain2(Eigen::VectorXi domain, Eigen::VectorXi indexNum,
	Eigen::VectorXi& triIndex, Eigen::VectorXi& triNum)
{
	int NbrTri = 0;

	int NbrDomain = domain.rows();
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < _mNbrTri; j++)
		{
			if (GetDomainOfTri(j) == domain(i))
				NbrTri++;
		}
	}

	//store triIndex
	triIndex = Eigen::VectorXi::Zero(NbrTri);
	triNum = Eigen::VectorXi::Zero(NbrTri);
	int numTri = 0;
	for (int i = 0; i < NbrDomain; i++)
	{
		for (int j = 0; j < NbrTri; j++)
		{
			if (GetDomainOfTri(j) == domain(i))
			{
				triIndex(numTri) = j;
				triNum(numTri) = i;
				numTri++;
			}
		}
	}
}

void Mesh_3D::GetIndexOfPBC(Eigen::MatrixXi PBC, Eigen::VectorXcd PBCData,
	Eigen::VectorXi& srcEdgeIndex, Eigen::VectorXi& dstEdgeIndex, Eigen::VectorXcd& edgePhi)
{
	std::vector<Eigen::VectorXi> srcDomains(PBC.rows());
	std::vector<Eigen::VectorXi> dstDomains(PBC.rows());
	Eigen::VectorXd angles = Eigen::VectorXd::Zero(PBC.rows());
	Eigen::MatrixXd waveVecs = Eigen::MatrixXd::Zero(3, PBC.rows());
	for (int i = 0; i < PBC.rows(); i++)
	{
		srcDomains[i] = Eigen::VectorXi(1);
		dstDomains[i] = Eigen::VectorXi(1);
		srcDomains[i](0) = PBC(i, 0);
		dstDomains[i](0) = PBC(i, 1);
	}
	GetIndexOfPBC(srcDomains, dstDomains, PBCData, angles, waveVecs, srcEdgeIndex, dstEdgeIndex, edgePhi);
}

void Mesh_3D::GetIndexOfPBC(const std::vector<Eigen::VectorXi>& srcDomains,
	const std::vector<Eigen::VectorXi>& dstDomains,
	const Eigen::VectorXcd& PBCData, const Eigen::VectorXd& pbcAngles,
	const Eigen::MatrixXd& pbcWaveVecs,
	Eigen::VectorXi& srcEdgeIndex, Eigen::VectorXi& dstEdgeIndex, Eigen::VectorXcd& edgePhi,
	bool useFaceStyleEdgeTol)
{
	std::vector<int> allSrc, allDst;
	std::vector<std::complex<double>> allPhi;

	for (int pair = 0; pair < static_cast<int>(srcDomains.size()); pair++)
	{
		std::vector<int> srcEdges, dstEdges;
		collectEdgesOnDomains(this, srcDomains[pair], srcEdges);
		collectEdgesOnDomains(this, dstDomains[pair], dstEdges);

		const double theta = (pair < pbcAngles.size()) ? pbcAngles(pair) : 0.0;
		const std::complex<double> blochPhi = (pair < PBCData.size()) ? PBCData(pair) : 1.0;
		Eigen::Vector3d waveVec = Eigen::Vector3d::Zero();
		if (pair < pbcWaveVecs.cols())
			waveVec = pbcWaveVecs.col(pair);

		if (_mNbrCopyOfTri > 0)
		{
			for (int i = 0; i < _mNbrCopyOfTri; i++)
			{
				allSrc.push_back(_mCopyOfTri(i, 0));
				allDst.push_back(_mCopyOfTri(i, 1));
				allPhi.push_back(blochPhi);
			}
			continue;
		}

		std::vector<int> pairedSrc, pairedDst, pairedSign;
		if (waveVec.norm() >= 1e-30)
			pairEdgesByTranslation(this, srcEdges, dstEdges, waveVec, pairedSrc, pairedDst, pairedSign,
				useFaceStyleEdgeTol);
		else
			pairEdgesByGeometry(this, srcEdges, dstEdges, theta, pairedSrc, pairedDst, pairedSign);
		for (size_t i = 0; i < pairedSrc.size(); i++)
		{
			allSrc.push_back(pairedSrc[i]);
			allDst.push_back(pairedDst[i]);
			allPhi.push_back(blochPhi * static_cast<double>(pairedSign[i]));
		}
	}

	removeSelfPairs(allSrc, allDst, allPhi);

	const int n = static_cast<int>(allSrc.size());
	srcEdgeIndex = Eigen::VectorXi(n);
	dstEdgeIndex = Eigen::VectorXi(n);
	edgePhi = Eigen::VectorXcd(n);
	for (int i = 0; i < n; i++)
	{
		srcEdgeIndex(i) = allSrc[i];
		dstEdgeIndex(i) = allDst[i];
		edgePhi(i) = allPhi[i];
	}

	if (n == 0)
		return;

	Eigen::VectorXi tempIndex = Eigen::VectorXi::LinSpaced(n, 0, n - 1);
	QuickSort(dstEdgeIndex, tempIndex, 0, n - 1);
	Unique(dstEdgeIndex, tempIndex);

	Eigen::VectorXcd tempPhi = Eigen::VectorXcd::Zero(dstEdgeIndex.size());
	Eigen::VectorXi tempSrc = Eigen::VectorXi::Zero(dstEdgeIndex.size());
	for (int i = 0; i < dstEdgeIndex.size(); i++)
	{
		tempPhi(i) = edgePhi(tempIndex(i));
		tempSrc(i) = srcEdgeIndex(tempIndex(i));
	}
	edgePhi = tempPhi;
	srcEdgeIndex = tempSrc;
}

void Mesh_3D::GetIndexOfPBCFaces(const std::vector<Eigen::VectorXi>& srcDomains,
	const std::vector<Eigen::VectorXi>& dstDomains,
	const Eigen::VectorXd& pbcAngles,
	const Eigen::MatrixXd& pbcWaveVecs,
	Eigen::VectorXi& srcFaceIndex, Eigen::VectorXi& dstFaceIndex, Eigen::VectorXcd& facePhi)
{
	std::vector<int> allSrc, allDst;
	std::vector<std::complex<double>> allPhi;

	for (int pair = 0; pair < static_cast<int>(srcDomains.size()); pair++)
	{
		// MATLAB findPBCIndex(src,dst,...): all src domains together, all dst together.
		std::vector<int> srcFaces, dstFaces;
		collectFacesOnDomains(this, srcDomains[pair], srcFaces);
		collectFacesOnDomains(this, dstDomains[pair], dstFaces);

		const double theta = (pair < pbcAngles.size()) ? pbcAngles(pair) : 0.0;
		Eigen::Vector3d waveVec = Eigen::Vector3d::Zero();
		if (pair < pbcWaveVecs.cols())
			waveVec = pbcWaveVecs.col(pair);

		std::vector<int> pairedSrc, pairedDst, pairedSign;
		if (waveVec.norm() >= 1e-30)
			pairFacesByTranslation(this, srcFaces, dstFaces, waveVec, pairedSrc, pairedDst, pairedSign);
		else
			pairFacesByGeometry(this, srcFaces, dstFaces, theta, pairedSrc, pairedDst, pairedSign);

		for (size_t i = 0; i < pairedSrc.size(); i++)
		{
			allSrc.push_back(pairedSrc[i]);
			allDst.push_back(pairedDst[i]);
			allPhi.push_back(static_cast<double>(pairedSign[i]));
		}
	}

	removeSelfPairs(allSrc, allDst, allPhi);

	const int n = static_cast<int>(allSrc.size());
	srcFaceIndex = Eigen::VectorXi(n);
	dstFaceIndex = Eigen::VectorXi(n);
	facePhi = Eigen::VectorXcd(n);
	for (int i = 0; i < n; i++)
	{
		srcFaceIndex(i) = allSrc[i];
		dstFaceIndex(i) = allDst[i];
		facePhi(i) = allPhi[i];
	}

	if (n == 0)
		return;

	Eigen::VectorXi tempIndex = Eigen::VectorXi::LinSpaced(n, 0, n - 1);
	QuickSort(dstFaceIndex, tempIndex, 0, n - 1);
	Unique(dstFaceIndex, tempIndex);

	Eigen::VectorXcd tempPhi = Eigen::VectorXcd::Zero(dstFaceIndex.size());
	Eigen::VectorXi tempSrc = Eigen::VectorXi::Zero(dstFaceIndex.size());
	for (int i = 0; i < dstFaceIndex.size(); i++)
	{
		tempPhi(i) = facePhi(tempIndex(i));
		tempSrc(i) = srcFaceIndex(tempIndex(i));
	}
	facePhi = tempPhi;
	srcFaceIndex = tempSrc;
}

// Match MATLAB getMesh.m: unique face rows in column-major order (face slot, then tet).
void Mesh_3D::BuildFaceTopology()
{
	if (_mNbrTet <= 0)
	{
		_mNbrFace = 0;
		return;
	}

	static const int kFaceVert[4][3] = { {0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3} };
	std::vector<std::array<int, 3>> faces;
	faces.reserve(static_cast<size_t>(_mNbrTet) * 4);
	_mFaceOfTet = Eigen::MatrixXi::Zero(_mNbrTet, 4);

	auto findFace = [&](const std::array<int, 3>& key) -> int {
		for (int i = 0; i < static_cast<int>(faces.size()); i++)
		{
			if (faces[static_cast<size_t>(i)] == key)
				return i;
		}
		return -1;
	};

	for (int f = 0; f < 4; f++)
	{
		for (int t = 0; t < _mNbrTet; t++)
		{
			std::array<int, 3> key = {
				_mTet(t, kFaceVert[f][0]),
				_mTet(t, kFaceVert[f][1]),
				_mTet(t, kFaceVert[f][2])
			};
			int fid = findFace(key);
			if (fid < 0)
			{
				fid = static_cast<int>(faces.size());
				faces.push_back(key);
			}
			_mFaceOfTet(t, f) = fid;
		}
	}

	_mNbrFace = static_cast<int>(faces.size());
	_mFace = Eigen::MatrixXi(_mNbrFace, 3);
	for (int i = 0; i < _mNbrFace; i++)
	{
		_mFace(i, 0) = faces[static_cast<size_t>(i)][0];
		_mFace(i, 1) = faces[static_cast<size_t>(i)][1];
		_mFace(i, 2) = faces[static_cast<size_t>(i)][2];
	}
}