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

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

using namespace Eigen;

// Port of MATLAB assembly_out.m and assembly_inc.m

namespace {

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

double getScalarEps(int domainOfTet, MaterialLib* matLib)
{
	return matLib->GetEpsr(domainOfTet)(0, 0).real();
}

void assembleSBCFace(
	Mesh_3D* mesh,
	MaterialLib* matLib,
	std::vector<Triplet<std::complex<double>>>& tripleA,
	VectorXcd& B,
	double k0,
	double* u, double* v, double* wght, int nbrGP,
	BF& bfN,
	bool isInc,
	const Vector3cd& Einc,
	int triIdx)
{
	const int domain = mesh->GetDomainOfTri(triIdx);

	Vector3i conn;
	mesh->GetCoonOfTri(triIdx, conn);
	const int numTet = conn(0);
	const int numFace = conn(1) + 1;

	double xv[4], yv[4], zv[4];
	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);
	}

	Vector3d x2, y2, z2;
	Vector3d x3, y3, z3;
	int bfIndex[3];
	int mappingIndex[3];

	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;
	}

	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;
	const Matrix3d 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;
	const double integCoe = 0.25 * std::sqrt(heron);

	Vector3d normal;
	mesh->GetNormOfFace(domain, normal);

	Vector3d E[4][3];
	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 < 3; j++)
		{
			bfN.GetValueBF(bfIndex[j], u2, v2, w2, E[gp][j]);
			E[gp][j] = InvJac * E[gp][j];
		}
	}

	const int tetDomain = mesh->GetDomainOfTet(numTet);
	const double eps = getScalarEps(tetDomain, matLib);
	const std::complex<double> nn = std::sqrt(std::complex<double>(eps, 0.0));
	const std::complex<double> iUnit(0.0, 1.0);

	Matrix3cd Ae = Matrix3cd::Zero();
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			for (int gp = 0; gp < nbrGP; gp++)
			{
				const Vector3d tEj = crossNormal(normal, E[gp][j]);
				Ae(i, j) += iUnit * k0 * nn * integCoe * wght[gp]
					* E[gp][i].dot(tEj) * 2.0;
			}
		}
	}

	std::complex<double> Be[3] = { 0.0, 0.0, 0.0 };
	if (isInc)
	{
		const Vector3d EincReal(Einc.real()(0), Einc.real()(1), Einc.real()(2));
		const Vector3d tEinc = crossNormal(normal, EincReal);
		for (int i = 0; i < 3; i++)
		{
			for (int gp = 0; gp < nbrGP; gp++)
			{
				Be[i] -= iUnit * k0 * nn * 2.0 * integCoe * wght[gp]
					* E[gp][i].dot(tEinc) * 2.0;
			}
		}
	}

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
			tripleA.emplace_back(mappingIndex[i], mappingIndex[j], Ae(i, j));
		if (isInc)
			B(mappingIndex[i]) += Be[i];
	}
}

void assembleSBCByType(
	Mesh_3D* mesh,
	MaterialLib* matLib,
	Phy_WaveOpticsModel* phy,
	std::vector<Triplet<std::complex<double>>>& tripleA,
	VectorXcd& B,
	double k0,
	int sbcTypeFilter,
	bool isInc,
	const Vector3cd& Einc)
{
	Gauss gauss;
	const int nbrGP = gauss.GetNbrGaussPoints(TWODIM, TRIANGLE, BF_LINEFUNC * 2);
	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, BF_LINEFUNC);
	const int nbrSBC = phy->GetNbrSBC();
	for (int s = 0; s < nbrSBC; s++)
	{
		if (phy->GetSBCType(s) != sbcTypeFilter)
			continue;

		VectorXi triIndices;
		mesh->GetTriIndicesOfDomain(phy->GetSBCDomain(s), triIndices);
		for (int n = 0; n < triIndices.size(); n++)
		{
			assembleSBCFace(mesh, matLib, tripleA, B, k0, u, v, wght, nbrGP, bfN,
				isInc, Einc, triIndices(n));
		}
	}

	delete[] u;
	delete[] v;
	delete[] w;
	delete[] wght;
}

} // namespace

void OpticsFEM_3D_Scatter::Assemble_SBC()
{
	const double k0 = 2.0 * Pi / _mSolver->GetLda0();

	assembleSBCByType(_mMesh, _mMatLib, _mPhy, _mTripleA_complex, _mB_complex,
		k0, 0, false, Vector3cd::Zero());

	Vector3cd Einc = Vector3cd::Zero();
	for (int s = 0; s < _mPhy->GetNbrSBC(); s++)
	{
		if (_mPhy->GetSBCType(s) != 1)
			continue;
		std::string sx, sy, sz;
		_mPhy->GetEinc(s, sx, sy, sz);
		Einc(0) = std::complex<double>(std::stod(sx), 0.0);
		Einc(1) = std::complex<double>(std::stod(sy), 0.0);
		Einc(2) = std::complex<double>(std::stod(sz), 0.0);
		break;
	}
	assembleSBCByType(_mMesh, _mMatLib, _mPhy, _mTripleA_complex, _mB_complex,
		k0, 1, true, Einc);
}

void OpticsFEM_3D_Scatter::Assemble_PEC_ELE()
{
}