#include"Assemble_Base.h"
#include"Nedelec3D_Util.h"
#include"../function/BF.h"
#include"../function/Gauss.h"
#include"../common/define.h"
#include"../material/Material_Base.h"
#include"../phy/Phy_Base.h"
#include"../parser/mpParser.h"
#include"SBC_NormalUtil.h"

#include<algorithm>
#include<cmath>
#include<complex>
#include<string>
#include<vector>

using namespace Eigen;

namespace {

Vector3cd crossNormal(const Vector3d& normal, const Vector3cd& a)
{
	return normal.cross(a.cross(normal));
}

Vector3cd evalVec3At(
	mup::ParserX& parser,
	mup::Value& xx, mup::Value& yy, mup::Value& zz,
	const std::string& sx, const std::string& sy, const std::string& sz,
	double x, double y, double z)
{
	xx = x;
	yy = y;
	zz = z;
	// Use string_type explicitly; _T(c_str) is not reliable for dynamic strings.
	parser.SetExpr(mup::string_type(sx.begin(), sx.end()));
	mup::Value vx = parser.Eval();
	parser.SetExpr(mup::string_type(sy.begin(), sy.end()));
	mup::Value vy = parser.Eval();
	parser.SetExpr(mup::string_type(sz.begin(), sz.end()));
	mup::Value vz = parser.Eval();
	return Vector3cd(
		std::complex<double>(vx.GetFloat(), vx.GetImag()),
		std::complex<double>(vy.GetFloat(), vy.GetImag()),
		std::complex<double>(vz.GetFloat(), vz.GetImag()));
}

bool setupFaceIntegration(
	Mesh_3D* mesh,
	int triIdx,
	Matrix3d& invJac,
	double& integCoe,
	Vector3d& normal,
	int mappingIndex[3],
	int bfIndex[3],
	Vector3d& x2, Vector3d& y2, Vector3d& z2,
	Vector3d& x3, Vector3d& y3, Vector3d& z3,
	double xv[4], double yv[4], double zv[4])
{
	Vector3i conn;
	mesh->GetCoonOfTri(triIdx, conn);
	const int numTet = conn(0);
	const int numFace = conn(1) + 1;

	for (int i = 0; i < 4; i++)
	{
		Vector3d vtx;
		mesh->GetVertex(mesh->GetTet(numTet, i), vtx);
		xv[i] = vtx(0); yv[i] = vtx(1); zv[i] = vtx(2);
	}

	if (numFace == 1)
	{
		x2 << 1, 0, 0; y2 << 0, 1, 0; z2 << 0, 0, 1;
		x3 << xv[0], xv[1], xv[2]; y3 << yv[0], yv[1], yv[2]; z3 << zv[0], zv[1], zv[2];
		bfIndex[0] = 1; bfIndex[1] = 2; bfIndex[2] = 4;
		mappingIndex[0] = mesh->GetEdgeOfTet(numTet, 0);
		mappingIndex[1] = mesh->GetEdgeOfTet(numTet, 1);
		mappingIndex[2] = mesh->GetEdgeOfTet(numTet, 3);
	}
	else if (numFace == 2)
	{
		x2 << 1, 0, 0; y2 << 0, 1, 0; z2 << 0, 0, 0;
		x3 << xv[0], xv[1], xv[3]; y3 << yv[0], yv[1], yv[3]; z3 << zv[0], zv[1], zv[3];
		bfIndex[0] = 1; bfIndex[1] = 3; bfIndex[2] = 5;
		mappingIndex[0] = mesh->GetEdgeOfTet(numTet, 0);
		mappingIndex[1] = mesh->GetEdgeOfTet(numTet, 2);
		mappingIndex[2] = mesh->GetEdgeOfTet(numTet, 4);
	}
	else if (numFace == 3)
	{
		x2 << 1, 0, 0; y2 << 0, 0, 0; z2 << 0, 1, 0;
		x3 << xv[0], xv[2], xv[3]; y3 << yv[0], yv[2], yv[3]; z3 << zv[0], zv[2], zv[3];
		bfIndex[0] = 2; bfIndex[1] = 3; bfIndex[2] = 6;
		mappingIndex[0] = mesh->GetEdgeOfTet(numTet, 1);
		mappingIndex[1] = mesh->GetEdgeOfTet(numTet, 2);
		mappingIndex[2] = mesh->GetEdgeOfTet(numTet, 5);
	}
	else if (numFace == 4)
	{
		x2 << 0, 0, 0; y2 << 1, 0, 0; z2 << 0, 1, 0;
		x3 << xv[1], xv[2], xv[3]; y3 << yv[1], yv[2], yv[3]; z3 << zv[1], zv[2], zv[3];
		bfIndex[0] = 4; bfIndex[1] = 5; bfIndex[2] = 6;
		mappingIndex[0] = mesh->GetEdgeOfTet(numTet, 3);
		mappingIndex[1] = mesh->GetEdgeOfTet(numTet, 4);
		mappingIndex[2] = mesh->GetEdgeOfTet(numTet, 5);
	}
	else
	{
		return false;
	}

	Matrix3d Jac;
	Jac(0, 0) = xv[0] - xv[3]; Jac(0, 1) = yv[0] - yv[3]; Jac(0, 2) = zv[0] - zv[3];
	Jac(1, 0) = xv[1] - xv[3]; Jac(1, 1) = yv[1] - yv[3]; Jac(1, 2) = zv[1] - zv[3];
	Jac(2, 0) = xv[2] - xv[3]; Jac(2, 1) = yv[2] - yv[3]; Jac(2, 2) = zv[2] - zv[3];
	const double detJ = Jac.determinant();
	if (std::abs(detJ) < 1e-30)
		return false;
	invJac = Jac.inverse();

	const double a = std::sqrt((x3(0) - x3(1)) * (x3(0) - x3(1)) + (y3(0) - y3(1)) * (y3(0) - y3(1)) + (z3(0) - z3(1)) * (z3(0) - z3(1)));
	const double b = std::sqrt((x3(0) - x3(2)) * (x3(0) - x3(2)) + (y3(0) - y3(2)) * (y3(0) - y3(2)) + (z3(0) - z3(2)) * (z3(0) - z3(2)));
	const double c = std::sqrt((x3(1) - x3(2)) * (x3(1) - x3(2)) + (y3(1) - y3(2)) * (y3(1) - y3(2)) + (z3(1) - z3(2)) * (z3(1) - z3(2)));
	double heron = (a + b + c) * (a + b - c) * (a - b + c) * (b + c - a);
	if (heron < 0.0) heron = 0.0;
	integCoe = 0.25 * std::sqrt(heron);

	mesh->GetNormOfFace(mesh->GetDomainOfTri(triIdx), normal);
	if (normal.norm() < 1e-30)
	{
		Vector3d e1 = Vector3d(x3(1) - x3(0), y3(1) - y3(0), z3(1) - z3(0));
		Vector3d e2 = Vector3d(x3(2) - x3(0), y3(2) - y3(0), z3(2) - z3(0));
		normal = e1.cross(e2);
		if (normal.norm() > 1e-30)
			normal.normalize();
	}
	return true;
}

void physicalPointOnFace(
	double u, double v,
	const Vector3d& x2, const Vector3d& y2, const Vector3d& z2,
	const Vector3d& x3, const Vector3d& y3, const Vector3d& z3,
	double& px, double& py, double& pz)
{
	// Triangle barycentric (u, v, 1-u-v) on the three face vertices stored in x3/y3/z3.
	// Do NOT route through tet ref coords (x2,y2,z2): that only matches face 1 and
	// yields wrong z (and breaks z-dependent sources like SCD J0) on faces 2/3/4.
	(void)x2; (void)y2; (void)z2;
	const double w = 1.0 - u - v;
	px = x3(0) * u + x3(1) * v + x3(2) * w;
	py = y3(0) * u + y3(1) * v + y3(2) * w;
	pz = z3(0) * u + z3(1) * v + z3(2) * w;
}

bool tetContainsVertex(Mesh_3D* mesh, int tetIdx, int vertexId)
{
	for (int j = 0; j < 4; j++)
	{
		if (mesh->GetTet(tetIdx, j) == vertexId)
			return true;
	}
	return false;
}

// IBP 边界闭合：b -= ∮ N·(n×curl E_b) dS（PML 面或 SBC 外表面，不是 Robin 的 q）
void accumulateBeleFaceIbp(
	Mesh_3D* mesh,
	VectorXcd& B,
	mup::ParserX& parser,
	mup::Value& xx, mup::Value& yy, mup::Value& zz,
	const std::string& curlEbx, const std::string& curlEby, const std::string& curlEbz,
	int triIdx,
	const double* uFace, const double* vFace, const double* wghtFace, int nbrGPFace,
	BF& bfN,
	bool useScatterSbcNormal,
	int elementOrder)
{
	Matrix3d invJac;
	double integCoe = 0.0;
	Vector3d normal = Vector3d::Zero();
	int mappingIndex3[3], bfIndex3[3];
	Vector3d x2, y2, z2, x3, y3, z3;
	double xv[4], yv[4], zv[4];
	if (!setupFaceIntegration(mesh, triIdx, invJac, integCoe, normal,
		mappingIndex3, bfIndex3, x2, y2, z2, x3, y3, z3, xv, yv, zv))
		return;

	int mappingIndex[8], bfIndex[8];
	int nLocal = 3;
	if (elementOrder == 2)
	{
		Eigen::Vector3i conn;
		mesh->GetCoonOfTri(triIdx, conn);
		Nedelec3D::buildSbcSecondOrderDofMap(mesh, conn(0), conn(1) + 1, mappingIndex);
		Nedelec3D::sbcFaceSecondOrderBfIndex(conn(1) + 1, bfIndex);
		nLocal = 8;
	}
	else
	{
		for (int i = 0; i < 3; i++)
		{
			mappingIndex[i] = mappingIndex3[i];
			bfIndex[i] = bfIndex3[i];
		}
	}

	if (useScatterSbcNormal)
	{
		const int domain = mesh->GetDomainOfTri(triIdx);
		Vector3d meshNorm;
		mesh->GetNormOfFace(domain, meshNorm);
		const Vector3d faceP0(x3(0), y3(0), z3(0));
		const Vector3d faceP1(x3(1), y3(1), z3(1));
		const Vector3d faceP2(x3(2), y3(2), z3(2));
		normal = OpticsFEM::computeScatterSBCNormal(
			false, domain, xv, yv, faceP0, faceP1, faceP2, &meshNorm);
	}

	std::vector<std::vector<Vector3d>> Egp(static_cast<size_t>(nbrGPFace),
		std::vector<Vector3d>(static_cast<size_t>(nLocal)));
	for (int gp = 0; gp < nbrGPFace; gp++)
	{
		const double wgp = 1.0 - uFace[gp] - vFace[gp];
		const double u2 = x2(0) * uFace[gp] + x2(1) * vFace[gp] + x2(2) * wgp;
		const double v2 = y2(0) * uFace[gp] + y2(1) * vFace[gp] + y2(2) * wgp;
		const double w2 = z2(0) * uFace[gp] + z2(1) * vFace[gp] + z2(2) * wgp;
		for (int j = 0; j < nLocal; j++)
		{
			bfN.GetValueBF(bfIndex[j], u2, v2, w2, Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)]);
			Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)] =
				invJac * Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)];
		}
	}

	std::vector<Vector3cd> bcurlE(static_cast<size_t>(nbrGPFace));
	for (int gp = 0; gp < nbrGPFace; gp++)
	{
		double px, py, pz;
		physicalPointOnFace(uFace[gp], vFace[gp], x2, y2, z2, x3, y3, z3, px, py, pz);
		bcurlE[static_cast<size_t>(gp)] = evalVec3At(parser, xx, yy, zz, curlEbx, curlEby, curlEbz, px, py, pz);
	}

	VectorXcd bt = VectorXcd::Zero(nLocal);
	for (int i = 0; i < nLocal; i++)
	{
		for (int gp = 0; gp < nbrGPFace; gp++)
		{
			const Vector3d& Ei = Egp[static_cast<size_t>(gp)][static_cast<size_t>(i)];
			const Vector3cd& curlEb = bcurlE[static_cast<size_t>(gp)];
			// 2D BELE PML legacy: b -= ∮ N·(n×curl E_b)
			const std::complex<double> curlContrib = wghtFace[gp] * integCoe
				* (std::complex<double>(Ei(0), 0.0) * (normal(1) * curlEb(2) - normal(2) * curlEb(1))
				+ std::complex<double>(Ei(1), 0.0) * (normal(2) * curlEb(0) - normal(0) * curlEb(2))
				+ std::complex<double>(Ei(2), 0.0) * (normal(0) * curlEb(1) - normal(1) * curlEb(0)));
			bt(i) -= curlContrib;
		}
	}

	for (int i = 0; i < nLocal; i++)
		B(mappingIndex[i]) += bt(i);
}

} // namespace

void OpticsFEM_3D_Scatter::Assemble_BELE()
{
	const double k0 = 2.0 * Pi / _mSolver->GetLda0();
	const int elementOrder = _mPhy->GetElementOrder();
	const int bfOrder = Nedelec3D::bfOrderParam(elementOrder);
	const int gaussVol = Nedelec3D::gaussOrderVol(elementOrder);
	const int gaussTri = Nedelec3D::gaussOrderTri(elementOrder);

	Gauss gauss;
	const int nbrGPVol = gauss.GetNbrGaussPoints(THREEDIM, TETRAHEDRON, gaussVol);
	double* uVol = new double[nbrGPVol];
	double* vVol = new double[nbrGPVol];
	double* wVol = new double[nbrGPVol];
	double* wghtVol = new double[nbrGPVol];
	gauss.GetGaussPoints(THREEDIM, TETRAHEDRON, uVol, vVol, wVol, wghtVol);

	BF bfN;
	const int vdof = bfN.GetNbrBF(THREEDIM, TETRAHEDRON, BF_NEDELEC, bfOrder);

	Vector3d** E = new Vector3d*[nbrGPVol];
	for (int i = 0; i < nbrGPVol; i++)
		E[i] = new Vector3d[vdof];

	mup::ParserX parser(mup::pckALL_COMPLEX);
	parser.EnableAutoCreateVar(true);
	mup::Value xx, yy, zz;
	parser.DefineVar(_T("x"), mup::Variable(&xx));
	parser.DefineVar(_T("y"), mup::Variable(&yy));
	parser.DefineVar(_T("z"), mup::Variable(&zz));

	std::string Ebx, Eby, Ebz, curlcurlEbx, curlcurlEby, curlcurlEbz;
	_mPhy->GetEb(Ebx, Eby, Ebz);
	_mPhy->GetCurlCurlEb(curlcurlEbx, curlcurlEby, curlcurlEbz);

	Eigen::VectorXi beleDomains;
	_mPhy->GetBELE(beleDomains);

	Vector3d vertex[4];
	for (int n = 0; n < _mMesh->GetNbrTet(); n++)
	{
		const int domain = _mMesh->GetDomainOfTet(n);
		if (std::find(beleDomains.begin(), beleDomains.end(), domain) == beleDomains.end())
			continue;

		for (int i = 0; i < 4; i++)
			_mMesh->GetVertex(_mMesh->GetTet(n, i), vertex[i]);

		Matrix3d Jac, InvJac;
		Jac(0, 0) = vertex[0](0) - vertex[3](0); Jac(0, 1) = vertex[0](1) - vertex[3](1); Jac(0, 2) = vertex[0](2) - vertex[3](2);
		Jac(1, 0) = vertex[1](0) - vertex[3](0); Jac(1, 1) = vertex[1](1) - vertex[3](1); Jac(1, 2) = vertex[1](2) - vertex[3](2);
		Jac(2, 0) = vertex[2](0) - vertex[3](0); Jac(2, 1) = vertex[2](1) - vertex[3](1); Jac(2, 2) = vertex[2](2) - vertex[3](2);
		const double detJac = std::abs(Jac.determinant());
		if (detJac < 1e-30)
			continue;
		InvJac = Jac.inverse();

		for (int gp = 0; gp < nbrGPVol; gp++)
		{
			for (int j = 0; j < vdof; j++)
			{
				bfN.GetValueBF(j + 1, uVol[gp], vVol[gp], wVol[gp], E[gp][j]);
				E[gp][j] = InvJac * E[gp][j];
			}
		}

		Vector3cd* bE = new Vector3cd[nbrGPVol];
		Vector3cd* bCurlCurlE = new Vector3cd[nbrGPVol];
		for (int gp = 0; gp < nbrGPVol; gp++)
		{
			const double px = vertex[3](0) + Jac(0, 0) * uVol[gp] + Jac(1, 0) * vVol[gp] + Jac(2, 0) * wVol[gp];
			const double py = vertex[3](1) + Jac(0, 1) * uVol[gp] + Jac(1, 1) * vVol[gp] + Jac(2, 1) * wVol[gp];
			const double pz = vertex[3](2) + Jac(0, 2) * uVol[gp] + Jac(1, 2) * vVol[gp] + Jac(2, 2) * wVol[gp];
			bE[gp] = evalVec3At(parser, xx, yy, zz, Ebx, Eby, Ebz, px, py, pz);
			bCurlCurlE[gp] = evalVec3At(parser, xx, yy, zz, curlcurlEbx, curlcurlEby, curlcurlEbz, px, py, pz);
		}

		Matrix3cd epsr = _mMatLib->GetEpsr(domain);
		const double k0sq = k0 * k0;

		std::vector<int> mapBuf(static_cast<size_t>(vdof), 0);
		Nedelec3D::buildTetDofMap(_mMesh, n, elementOrder, mapBuf.data(), vdof);
		for (int i = 0; i < vdof; i++)
		{
			std::complex<double> contrib = 0.0;
			for (int gp = 0; gp < nbrGPVol; gp++)
			{
				const Vector3cd Jb = bCurlCurlE[gp] - k0sq * (epsr * bE[gp]);
				contrib += wghtVol[gp] * detJac * E[gp][i].dot(Jb);
			}
			_mB_complex(mapBuf[static_cast<size_t>(i)]) -= contrib;
		}
		delete[] bE;
		delete[] bCurlCurlE;
	}

	const int nbrGPFace = gauss.GetNbrGaussPoints(TWODIM, TRIANGLE, gaussTri);
	double* uFace = new double[nbrGPFace];
	double* vFace = new double[nbrGPFace];
	double* wFace = new double[nbrGPFace];
	double* wghtFace = new double[nbrGPFace];
	gauss.GetGaussPoints(TWODIM, TRIANGLE, uFace, vFace, wFace, wghtFace);

	BF bfNFace;
	bfNFace.GetNbrBF(THREEDIM, TETRAHEDRON, BF_NEDELEC, bfOrder);

	std::string curlEbx, curlEby, curlEbz;
	_mPhy->GetCurlEb(curlEbx, curlEby, curlEbz);

	const int nbrPML = _mPhy->GetNbrPML();
	for (int p = 0; p < nbrPML; p++)
	{
		VectorXi triIndices;
		_mMesh->GetTriIndicesOfDomain(_mPhy->GetPMLEdge(p) + 1, triIndices);
		for (int t = 0; t < triIndices.size(); t++)
		{
			accumulateBeleFaceIbp(_mMesh, _mB_complex, parser, xx, yy, zz,
				curlEbx, curlEby, curlEbz, triIndices(t),
				uFace, vFace, wghtFace, nbrGPFace, bfNFace, false, elementOrder);
		}
	}

	delete[] uVol; delete[] vVol; delete[] wVol; delete[] wghtVol;
	delete[] uFace; delete[] vFace; delete[] wFace; delete[] wghtFace;
	for (int i = 0; i < nbrGPVol; i++)
		delete[] E[i];
	delete[] E;
}

void OpticsFEM_3D_Scatter::Assemble_MAG()
{
	const double k0 = 2.0 * Pi / _mSolver->GetLda0();
	const std::complex<double> iUnit(0.0, 1.0);
	const double zFactor = std::sqrt(mu0 / epsilon0);
	const int elementOrder = _mPhy->GetElementOrder();
	const int bfOrder = Nedelec3D::bfOrderParam(elementOrder);
	const int gaussTri = Nedelec3D::gaussOrderTri(elementOrder);
	const int nBfFace = (elementOrder == 2) ? 8 : 3;

	Gauss gauss;
	const int nbrGP = gauss.GetNbrGaussPoints(TWODIM, TRIANGLE, gaussTri);
	double* u = new double[nbrGP];
	double* v = new double[nbrGP];
	double* w = new double[nbrGP];
	double* wght = new double[nbrGP];
	gauss.GetGaussPoints(TWODIM, TRIANGLE, u, v, w, wght);

	BF bfN;
	bfN.GetNbrBF(THREEDIM, TETRAHEDRON, BF_NEDELEC, bfOrder);

	mup::ParserX parser(mup::pckALL_COMPLEX);
	parser.EnableAutoCreateVar(true);
	mup::Value xx, yy, zz;
	parser.DefineVar(_T("x"), mup::Variable(&xx));
	parser.DefineVar(_T("y"), mup::Variable(&yy));
	parser.DefineVar(_T("z"), mup::Variable(&zz));

	for (int n = 0; n < _mPhy->GetNbrMAG(); n++)
	{
		std::string H0x, H0y, H0z;
		_mPhy->GetH0(H0x, H0y, H0z, n);

		VectorXi triIndices;
		_mMesh->GetTriIndicesOfDomain(_mPhy->GetMAGDomain(n), triIndices);
		for (int t = 0; t < triIndices.size(); t++)
		{
			Matrix3d invJac;
			double integCoe = 0.0;
			Vector3d normal = Vector3d::Zero();
			int mappingIndex3[3], bfIndex3[3];
			Vector3d x2, y2, z2, x3, y3, z3;
			double xv[4], yv[4], zv[4];
			if (!setupFaceIntegration(_mMesh, triIndices(t), invJac, integCoe, normal,
				mappingIndex3, bfIndex3, x2, y2, z2, x3, y3, z3, xv, yv, zv))
				continue;

			int mappingIndex[8], bfIndex[8];
			int nLocal = 3;
			if (elementOrder == 2)
			{
				Eigen::Vector3i conn;
				_mMesh->GetCoonOfTri(triIndices(t), conn);
				Nedelec3D::buildSbcSecondOrderDofMap(_mMesh, conn(0), conn(1) + 1, mappingIndex);
				Nedelec3D::sbcFaceSecondOrderBfIndex(conn(1) + 1, bfIndex);
				nLocal = 8;
			}
			else
			{
				for (int i = 0; i < 3; i++)
				{
					mappingIndex[i] = mappingIndex3[i];
					bfIndex[i] = bfIndex3[i];
				}
			}

			const int triDomain = _mMesh->GetDomainOfTri(triIndices(t));
			Vector3i conn;
			_mMesh->GetCoonOfTri(triIndices(t), conn);
			const int tetDomain = _mMesh->GetDomainOfTet(conn(0));
			const double eps = _mMatLib->GetEpsr(tetDomain)(0, 0).real();
			const std::complex<double> nn = std::sqrt(std::complex<double>(eps, 0.0));

			Vector3d meshNorm;
			_mMesh->GetNormOfFace(triDomain, meshNorm);
			const Vector3d faceP0(x3(0), y3(0), z3(0));
			const Vector3d faceP1(x3(1), y3(1), z3(1));
			const Vector3d faceP2(x3(2), y3(2), z3(2));
			normal = OpticsFEM::computeScatterSBCNormal(
				false, triDomain, xv, yv, faceP0, faceP1, faceP2, &meshNorm);

			std::vector<std::vector<Vector3d>> Egp(static_cast<size_t>(nbrGP),
				std::vector<Vector3d>(static_cast<size_t>(nLocal)));
			for (int gp = 0; gp < nbrGP; gp++)
			{
				const double wgp = 1.0 - u[gp] - v[gp];
				const double u2 = x2(0) * u[gp] + x2(1) * v[gp] + x2(2) * wgp;
				const double v2 = y2(0) * u[gp] + y2(1) * v[gp] + y2(2) * wgp;
				const double w2 = z2(0) * u[gp] + z2(1) * v[gp] + z2(2) * wgp;
				for (int j = 0; j < nLocal; j++)
				{
					bfN.GetValueBF(bfIndex[j], u2, v2, w2, Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)]);
					Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)] =
						invJac * Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)];
				}
			}

			VectorXcd bt = VectorXcd::Zero(nLocal);
			for (int i = 0; i < nLocal; i++)
			{
				for (int gp = 0; gp < nbrGP; gp++)
				{
					double px, py, pz;
					physicalPointOnFace(u[gp], v[gp], x2, y2, z2, x3, y3, z3, px, py, pz);
					const Vector3cd H0 = evalVec3At(parser, xx, yy, zz, H0x, H0y, H0z, px, py, pz);
					const Vector3cd Ms = normal.cross(H0);
					bt(i) -= iUnit * zFactor * k0 * nn * integCoe * wght[gp]
						* Egp[static_cast<size_t>(gp)][static_cast<size_t>(i)].dot(Ms) * 2.0;
				}
			}

			for (int i = 0; i < nLocal; i++)
				_mB_complex(mappingIndex[i]) += bt(i);
		}
	}

	delete[] u; delete[] v; delete[] w; delete[] wght;
	(void)nBfFace;
}

void OpticsFEM_3D_Scatter::Assemble_SCD()
{
	const double k0 = 2.0 * Pi / _mSolver->GetLda0();
	const std::complex<double> iUnit(0.0, 1.0);
	const double zFactor = std::sqrt(mu0 / epsilon0);
	const int elementOrder = _mPhy->GetElementOrder();
	const int bfOrder = Nedelec3D::bfOrderParam(elementOrder);
	const int gaussTri = Nedelec3D::gaussOrderTri(elementOrder);
	const int nBfFace = (elementOrder == 2) ? 8 : 3;

	Gauss gauss;
	const int nbrGP = gauss.GetNbrGaussPoints(TWODIM, TRIANGLE, gaussTri);
	double* u = new double[nbrGP];
	double* v = new double[nbrGP];
	double* w = new double[nbrGP];
	double* wght = new double[nbrGP];
	gauss.GetGaussPoints(TWODIM, TRIANGLE, u, v, w, wght);

	BF bfN;
	bfN.GetNbrBF(THREEDIM, TETRAHEDRON, BF_NEDELEC, bfOrder);

	mup::ParserX parser(mup::pckALL_COMPLEX);
	parser.EnableAutoCreateVar(true);
	mup::Value xx, yy, zz;
	parser.DefineVar(_T("x"), mup::Variable(&xx));
	parser.DefineVar(_T("y"), mup::Variable(&yy));
	parser.DefineVar(_T("z"), mup::Variable(&zz));

	for (int n = 0; n < _mPhy->GetNbrSCD(); n++)
	{
		std::string J0x, J0y, J0z;
		_mPhy->GetJ0(J0x, J0y, J0z, n);

		// faceFactor / srcFactor:
		//   Order1: empirical COMSOL SurfaceCurrent match: 2*k0/(π*max(ε_r)).
		//   Order2: same *2.0 as MAG/SBC face sources (no extra k0). Reusing the
		//   order1 faceFactor over-scales |E| on the SCD plane (~4.4 vs COMSOL ~2.1).
		double srcFactor = 2.0;
		if (elementOrder != 2)
		{
			double epsScdNorm = 1.0;
			for (int t = 0; t < _mMesh->GetNbrTet(); ++t)
			{
				const int dom = _mMesh->GetDomainOfTet(t);
				epsScdNorm = std::max(epsScdNorm, _mMatLib->GetEpsr(dom)(0, 0).real());
			}
			srcFactor = 2.0 * k0 / Pi / epsScdNorm;
		}

		VectorXi triIndices;
		_mMesh->GetTriIndicesOfDomain(_mPhy->GetSCDDomain(n), triIndices);
		for (int t = 0; t < triIndices.size(); t++)
		{
			Matrix3d invJac;
			double integCoe = 0.0;
			Vector3d normal = Vector3d::Zero();
			int mappingIndex3[3], bfIndex3[3];
			Vector3d x2, y2, z2, x3, y3, z3;
			double xv[4], yv[4], zv[4];
			if (!setupFaceIntegration(_mMesh, triIndices(t), invJac, integCoe, normal,
				mappingIndex3, bfIndex3, x2, y2, z2, x3, y3, z3, xv, yv, zv))
				continue;

			int mappingIndex[8], bfIndex[8];
			int nLocal = 3;
			if (elementOrder == 2)
			{
				Eigen::Vector3i conn;
				_mMesh->GetCoonOfTri(triIndices(t), conn);
				Nedelec3D::buildSbcSecondOrderDofMap(_mMesh, conn(0), conn(1) + 1, mappingIndex);
				Nedelec3D::sbcFaceSecondOrderBfIndex(conn(1) + 1, bfIndex);
				nLocal = 8;
			}
			else
			{
				for (int i = 0; i < 3; i++)
				{
					mappingIndex[i] = mappingIndex3[i];
					bfIndex[i] = bfIndex3[i];
				}
			}

			const int triDomain = _mMesh->GetDomainOfTri(triIndices(t));
			Vector3i conn;
			_mMesh->GetCoonOfTri(triIndices(t), conn);

			Vector3d meshNorm;
			_mMesh->GetNormOfFace(triDomain, meshNorm);
			const Vector3d faceP0(x3(0), y3(0), z3(0));
			const Vector3d faceP1(x3(1), y3(1), z3(1));
			const Vector3d faceP2(x3(2), y3(2), z3(2));
			normal = OpticsFEM::computeScatterSBCNormal(
				false, triDomain, xv, yv, faceP0, faceP1, faceP2, &meshNorm);

			const int tetDomain = _mMesh->GetDomainOfTet(conn(0));
			const double eps = _mMatLib->GetEpsr(tetDomain)(0, 0).real();
			const std::complex<double> nn = std::sqrt(std::complex<double>(eps, 0.0));

			std::vector<std::vector<Vector3d>> Egp(static_cast<size_t>(nbrGP),
				std::vector<Vector3d>(static_cast<size_t>(nLocal)));
			for (int gp = 0; gp < nbrGP; gp++)
			{
				const double wgp = 1.0 - u[gp] - v[gp];
				const double u2 = x2(0) * u[gp] + x2(1) * v[gp] + x2(2) * wgp;
				const double v2 = y2(0) * u[gp] + y2(1) * v[gp] + y2(2) * wgp;
				const double w2 = z2(0) * u[gp] + z2(1) * v[gp] + z2(2) * wgp;
				for (int j = 0; j < nLocal; j++)
				{
					bfN.GetValueBF(bfIndex[j], u2, v2, w2, Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)]);
					Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)] =
						invJac * Egp[static_cast<size_t>(gp)][static_cast<size_t>(j)];
				}
			}

			VectorXcd bt = VectorXcd::Zero(nLocal);
			for (int i = 0; i < nLocal; i++)
			{
				for (int gp = 0; gp < nbrGP; gp++)
				{
					double px, py, pz;
					physicalPointOnFace(u[gp], v[gp], x2, y2, z2, x3, y3, z3, px, py, pz);
					const Vector3cd J0 = evalVec3At(parser, xx, yy, zz, J0x, J0y, J0z, px, py, pz);
					const Vector3cd Jt = crossNormal(normal, J0);
					bt(i) -= iUnit * zFactor * k0 * nn * integCoe * wght[gp]
						* Egp[static_cast<size_t>(gp)][static_cast<size_t>(i)].dot(Jt) * srcFactor;
				}
			}

			for (int i = 0; i < nLocal; i++)
				_mB_complex(mappingIndex[i]) += bt(i);
		}
	}

	delete[] u; delete[] v; delete[] w; delete[] wght;
	(void)nBfFace;
}

void OpticsFEM_3D_Scatter::Assemble_MPD()
{
	const double k0 = 2.0 * Pi / _mSolver->GetLda0();
	const double zFactor = std::sqrt(mu0 / epsilon0);
	const int elementOrder = _mPhy->GetElementOrder();
	const int bfOrder = Nedelec3D::bfOrderParam(elementOrder);
	const int gaussVol = Nedelec3D::gaussOrderVol(elementOrder);

	Eigen::VectorXi mpd;
	Eigen::MatrixXd m;
	_mPhy->GetMPD(mpd);
	_mPhy->GetMPDData(m);

	Gauss gauss;
	const int nbrGP = gauss.GetNbrGaussPoints(THREEDIM, TETRAHEDRON, gaussVol);
	double* u = new double[nbrGP];
	double* v = new double[nbrGP];
	double* w = new double[nbrGP];
	double* wght = new double[nbrGP];
	gauss.GetGaussPoints(THREEDIM, TETRAHEDRON, u, v, w, wght);

	BF bfCurlN;
	const int vdof = bfCurlN.GetNbrBF(THREEDIM, TETRAHEDRON, BF_CURL_NEDELEC, bfOrder);
	Vector3d** curlE = new Vector3d*[nbrGP];
	for (int i = 0; i < nbrGP; i++)
		curlE[i] = new Vector3d[vdof];

	Vector3d vertex[4];
	for (int dip = 0; dip < mpd.size(); dip++)
	{
		const Vector3d mCurrent = m.row(dip);
		for (int n = 0; n < _mMesh->GetNbrTet(); n++)
		{
			if (!tetContainsVertex(_mMesh, n, mpd(dip)))
				continue;

			for (int i = 0; i < 4; i++)
				_mMesh->GetVertex(_mMesh->GetTet(n, i), vertex[i]);

			Matrix3d Jac, TJac;
			Jac(0, 0) = vertex[0](0) - vertex[3](0); Jac(0, 1) = vertex[0](1) - vertex[3](1); Jac(0, 2) = vertex[0](2) - vertex[3](2);
			Jac(1, 0) = vertex[1](0) - vertex[3](0); Jac(1, 1) = vertex[1](1) - vertex[3](1); Jac(1, 2) = vertex[1](2) - vertex[3](2);
			Jac(2, 0) = vertex[2](0) - vertex[3](0); Jac(2, 1) = vertex[2](1) - vertex[3](1); Jac(2, 2) = vertex[2](2) - vertex[3](2);
			const double detJac = std::abs(Jac.determinant());
			if (detJac < 1e-30)
				continue;
			TJac = Jac.transpose() / Jac.determinant();

			for (int gp = 0; gp < nbrGP; gp++)
			{
				for (int j = 0; j < vdof; j++)
				{
					bfCurlN.GetValueBF(j + 1, u[gp], v[gp], w[gp], curlE[gp][j]);
					curlE[gp][j] = TJac * curlE[gp][j];
				}
			}

			VectorXcd Tt = VectorXcd::Zero(vdof);
			for (int i = 0; i < vdof; i++)
			{
				for (int gp = 0; gp < nbrGP; gp++)
				{
					Tt(i) += std::complex<double>(0.0, -1.0) * zFactor * k0 * wght[gp] * detJac * curlE[gp][i].dot(mCurrent);
				}
			}

			std::vector<int> mapBuf(static_cast<size_t>(vdof), 0);
			Nedelec3D::buildTetDofMap(_mMesh, n, elementOrder, mapBuf.data(), vdof);
			for (int i = 0; i < vdof; i++)
				_mB_complex(mapBuf[static_cast<size_t>(i)]) += Tt(i);
		}
	}

	delete[] u; delete[] v; delete[] w; delete[] wght;
	for (int i = 0; i < nbrGP; i++)
		delete[] curlE[i];
	delete[] curlE;
}

void OpticsFEM_3D_Scatter::Assemble_EPD()
{
	const double k0 = 2.0 * Pi / _mSolver->GetLda0();
	const double zFactor = std::sqrt(mu0 / epsilon0);
	const int elementOrder = _mPhy->GetElementOrder();
	const int bfOrder = Nedelec3D::bfOrderParam(elementOrder);
	const int gaussVol = Nedelec3D::gaussOrderVol(elementOrder);

	Eigen::VectorXi epd;
	Eigen::MatrixXd p;
	_mPhy->GetEPD(epd);
	_mPhy->GetEPDData(p);

	Gauss gauss;
	const int nbrGP = gauss.GetNbrGaussPoints(THREEDIM, TETRAHEDRON, gaussVol);
	double* u = new double[nbrGP];
	double* v = new double[nbrGP];
	double* w = new double[nbrGP];
	double* wght = new double[nbrGP];
	gauss.GetGaussPoints(THREEDIM, TETRAHEDRON, u, v, w, wght);

	BF bfN;
	const int vdof = bfN.GetNbrBF(THREEDIM, TETRAHEDRON, BF_NEDELEC, bfOrder);
	Vector3d** E = new Vector3d*[nbrGP];
	for (int i = 0; i < nbrGP; i++)
		E[i] = new Vector3d[vdof];

	Vector3d vertex[4];
	for (int dip = 0; dip < epd.size(); dip++)
	{
		const Vector3d pCurrent = p.row(dip);
		for (int n = 0; n < _mMesh->GetNbrTet(); n++)
		{
			if (!tetContainsVertex(_mMesh, n, epd(dip)))
				continue;

			for (int i = 0; i < 4; i++)
				_mMesh->GetVertex(_mMesh->GetTet(n, i), vertex[i]);

			Matrix3d Jac, InvJac;
			Jac(0, 0) = vertex[0](0) - vertex[3](0); Jac(0, 1) = vertex[0](1) - vertex[3](1); Jac(0, 2) = vertex[0](2) - vertex[3](2);
			Jac(1, 0) = vertex[1](0) - vertex[3](0); Jac(1, 1) = vertex[1](1) - vertex[3](1); Jac(1, 2) = vertex[1](2) - vertex[3](2);
			Jac(2, 0) = vertex[2](0) - vertex[3](0); Jac(2, 1) = vertex[2](1) - vertex[3](1); Jac(2, 2) = vertex[2](2) - vertex[3](2);
			const double detJac = std::abs(Jac.determinant());
			if (detJac < 1e-30)
				continue;
			InvJac = Jac.inverse();

			for (int gp = 0; gp < nbrGP; gp++)
			{
				for (int j = 0; j < vdof; j++)
				{
					bfN.GetValueBF(j + 1, u[gp], v[gp], w[gp], E[gp][j]);
					E[gp][j] = InvJac * E[gp][j];
				}
			}

			VectorXcd Tt = VectorXcd::Zero(vdof);
			for (int i = 0; i < vdof; i++)
			{
				for (int gp = 0; gp < nbrGP; gp++)
				{
					Tt(i) += std::complex<double>(0.0, -1.0) * zFactor * k0 * wght[gp] * detJac * E[gp][i].dot(pCurrent);
				}
			}

			std::vector<int> mapBuf(static_cast<size_t>(vdof), 0);
			Nedelec3D::buildTetDofMap(_mMesh, n, elementOrder, mapBuf.data(), vdof);
			for (int i = 0; i < vdof; i++)
				_mB_complex(mapBuf[static_cast<size_t>(i)]) += Tt(i);
		}
	}

	delete[] u; delete[] v; delete[] w; delete[] wght;
	for (int i = 0; i < nbrGP; i++)
		delete[] E[i];
	delete[] E;
}
