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

#include <cmath>
#include <complex>
#include <iostream>
#include <vector>

using namespace Eigen;

namespace {

Vector3cd crossMuN(const Vector3d& n, const Vector3cd& a)
{
	return n.cross(a);
}

// MATLAB PortBCMatrixAssembly: sum(Alphaj .* temp) — no conjugate on either factor.
std::complex<double> matlabSumProduct(const Vector3d& aReal, const Vector3cd& b)
{
	return std::complex<double>(aReal(0), 0.0) * b(0)
		+ std::complex<double>(aReal(1), 0.0) * b(1)
		+ std::complex<double>(aReal(2), 0.0) * b(2);
}

void getTetFaceData(
	Mesh_3D* mesh, int numEle, int numFace,
	int faceNode[3], int faceBF[3],
	double x[4], double y[4], double z[4], double l[6])
{
	for (int i = 0; i < 4; i++)
	{
		Vector3d v;
		mesh->GetVertex(mesh->GetTet(numEle, i), v);
		x[i] = v(0); y[i] = v(1); z[i] = v(2);
	}
	auto edgeLen = [](double x1, double y1, double z1, double x2, double y2, double z2) {
		return std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));
	};
	l[0] = edgeLen(x[0], y[0], z[0], x[1], y[1], z[1]);
	l[1] = edgeLen(x[0], y[0], z[0], x[2], y[2], z[2]);
	l[2] = edgeLen(x[0], y[0], z[0], x[3], y[3], z[3]);
	l[3] = edgeLen(x[1], y[1], z[1], x[2], y[2], z[2]);
	l[4] = edgeLen(x[1], y[1], z[1], x[3], y[3], z[3]);
	l[5] = edgeLen(x[2], y[2], z[2], x[3], y[3], z[3]);

	if (numFace == 1) { faceNode[0] = 0; faceNode[1] = 1; faceNode[2] = 2; faceBF[0] = 0; faceBF[1] = 1; faceBF[2] = 3; }
	else if (numFace == 2) { faceNode[0] = 0; faceNode[1] = 1; faceNode[2] = 3; faceBF[0] = 0; faceBF[1] = 2; faceBF[2] = 4; }
	else if (numFace == 3) { faceNode[0] = 0; faceNode[1] = 2; faceNode[2] = 3; faceBF[0] = 1; faceBF[1] = 2; faceBF[2] = 5; }
	else { faceNode[0] = 1; faceNode[1] = 2; faceNode[2] = 3; faceBF[0] = 3; faceBF[1] = 4; faceBF[2] = 5; }
}

void assembleOnePortFace(
	Mesh_3D* mesh, MaterialLib* matLib,
	const PortNumericMode& mode, int faceIdx,
	VectorXcd& G, VectorXcd& S_or_T, VectorXcd& bEdge,
	std::complex<double>& P, std::complex<double>& b1,
	bool isInput)
{
	const int numEle = mode.facesConn(faceIdx, 0) - 1;
	const int numFace = mode.facesConn(faceIdx, 1);
	int faceNode[3], faceBF[3];
	double x[4], y[4], z[4], l[6];
	getTetFaceData(mesh, numEle, numFace, faceNode, faceBF, x, y, z, l);

	const int domain = mesh->GetDomainOfTet(numEle);
	const std::complex<double> mu = 1.0 / matLib->GetMur(domain)(0, 0);

	Matrix3d Jac;
	Jac(0, 0) = x[0] - x[3]; Jac(0, 1) = y[0] - y[3]; Jac(0, 2) = z[0] - z[3];
	Jac(1, 0) = x[1] - x[3]; Jac(1, 1) = y[1] - y[3]; Jac(1, 2) = z[1] - z[3];
	Jac(2, 0) = x[2] - x[3]; Jac(2, 1) = y[2] - y[3]; Jac(2, 2) = z[2] - z[3];
	const Matrix3d InvJac = Jac.inverse();

	double xx[3], yy[3], zz[3];
	for (int i = 0; i < 3; i++)
	{
		xx[i] = x[faceNode[i]];
		yy[i] = y[faceNode[i]];
		zz[i] = z[faceNode[i]];
	}

	Matrix3d fJac = Matrix3d::Zero();
	fJac(0, 0) = -xx[0] + xx[1]; fJac(0, 1) = -yy[0] + yy[1];
	fJac(1, 0) = -xx[0] + xx[2]; fJac(1, 1) = -yy[0] + yy[2]; fJac(2, 2) = 1.0;
	const Matrix3d InvfJac = fJac.inverse();
	Matrix3d fJacS = Matrix3d::Zero();
	fJacS(0, 0) = InvfJac(1, 1); fJacS(0, 1) = -InvfJac(1, 0);
	fJacS(1, 0) = -InvfJac(0, 1); fJacS(1, 1) = InvfJac(0, 0); fJacS(2, 2) = 1.0;
	const Matrix3d fTJac = fJac.transpose() / fJac.determinant();
	const double fDetJac = std::abs(fJac.determinant());

	Gauss gauss;
	BF bfEt, bfEz, bfEdge, bfCurlEt, bfCurlEz;
	bfEt.GetNbrBF(TWODIM, TRIANGLE, BF_NEDELEC, BF_LINEFUNC);
	bfEz.GetNbrBF(TWODIM, TRIANGLE, BF_LAGRANGE, BF_LINEFUNC);
	bfEdge.GetNbrBF(THREEDIM, TETRAHEDRON, BF_NEDELEC, BF_LINEFUNC);
	bfCurlEt.GetNbrBF(TWODIM, TRIANGLE, BF_CURL_NEDELEC, BF_LINEFUNC);
	bfCurlEz.GetNbrBF(TWODIM, TRIANGLE, BF_CURL_LAGRANGE, BF_LINEFUNC);

	const int nGP = gauss.GetNbrGaussPoints(TWODIM, TRIANGLE, BF_LINEFUNC * 2);
	std::vector<double> u(nGP), v(nGP), w(nGP), wght(nGP);
	gauss.GetGaussPoints(TWODIM, TRIANGLE, u.data(), v.data(), w.data(), wght.data());

	Matrix2d fJac2;
	fJac2(0, 0) = -xx[0] + xx[1]; fJac2(0, 1) = -yy[0] + yy[1];
	fJac2(1, 0) = -xx[0] + xx[2]; fJac2(1, 1) = -yy[0] + yy[2];
	const Matrix3d vJac = Jac.inverse();

	for (int gp = 0; gp < nGP; gp++)
	{
		Vector3cd fE = Vector3cd::Zero();
		Vector3cd fcurlE = Vector3cd::Zero();
		for (int i = 0; i < 3; i++)
		{
			Vector3d tempEt, tempGradEz, tempCurlEt, tempEz;
			bfEt.GetValueBF(i + 1, u[gp], v[gp], 0.0, tempEt);
			Vector3d et = InvfJac * tempEt * l[faceBF[i]];
			bfEz.GetValueBF(i + 1, u[gp], v[gp], 0.0, tempEz);
			Vector3d ezVec(0, 0, tempEz(2));

			bfCurlEt.GetValueBF(i + 1, u[gp], v[gp], 0.0, tempCurlEt);
			Vector3d curlEtVec = fTJac * Vector3d(0, 0, tempCurlEt(2)) * l[faceBF[i]];
			bfCurlEz.GetValueBF(i + 1, u[gp], v[gp], 0.0, tempGradEz);
			Vector3d curlEzVec = fJacS * Vector3d(tempGradEz(0), tempGradEz(1), 0.0);

			const int mapEt = mode.portEdgeOfFace(faceIdx, i);
			const int mapEz = mode.portNewFaces(faceIdx, i);
			fE += et.cast<std::complex<double>>() * mode.Et(mapEt)
				+ ezVec.cast<std::complex<double>>() * mode.Ez(mapEz);
			fcurlE += curlEtVec.cast<std::complex<double>>() * mode.Et(mapEt)
				+ curlEzVec.cast<std::complex<double>>() * mode.Ez(mapEz);
		}

		Vector3cd fN0, fcurlN0, fN1, fcurlN1, fN2, fcurlN2, fW;
		if (isInput)
		{
			fN0 = fE * mode.powerCoef;
			fcurlN0 = (fcurlE + mode.gamma * Vector3cd(fE(1), -fE(0), 0.0)) * mode.powerCoef;
			fN1 = fE;
			fcurlN1 = fcurlE - mode.gamma * Vector3cd(fE(1), -fE(0), 0.0);
		}
		else
		{
			fN2 = fE;
			fcurlN2 = fcurlE + mode.gamma * Vector3cd(fE(1), -fE(0), 0.0);
		}
		fW = fE;

		Vector3d alphaPhys[3];
		for (int i = 0; i < 3; i++)
		{
			// Match MATLAB: temp = [u,v]*fJac2 + [xx(1),yy(1),zz(1)]
			Vector3d refPt(
				fJac2(0, 0) * u[gp] + fJac2(1, 0) * v[gp] + xx[0],
				fJac2(0, 1) * u[gp] + fJac2(1, 1) * v[gp] + yy[0],
				zz[0]);
			// Match MATLAB: temp = [u,v]*fJac2 + xyz; ref3 = temp*vJac - x4*vJac (row-vector multiply)
			Vector3d ref3 = vJac.transpose() * refPt - vJac.transpose() * Vector3d(x[3], y[3], z[3]);
			Vector3d bf;
			bfEdge.GetValueBF(faceBF[i] + 1, ref3(0), ref3(1), ref3(2), bf);
			alphaPhys[i] = InvJac * bf * l[faceBF[i]];
		}

		const int mappingEdge[3] = {
			mesh->GetEdgeOfTet(numEle, faceBF[0]),
			mesh->GetEdgeOfTet(numEle, faceBF[1]),
			mesh->GetEdgeOfTet(numEle, faceBF[2]) };

		if (isInput)
		{
			for (int i = 0; i < 3; i++)
			{
				G(mappingEdge[i]) += wght[gp] * fDetJac * (fW(0) * alphaPhys[i](0) + fW(1) * alphaPhys[i](1));
				const Vector3cd temp = mu * crossMuN(mode.normal, fcurlN1);
				S_or_T(mappingEdge[i]) += wght[gp] * fDetJac * matlabSumProduct(alphaPhys[i], temp);
				const Vector3cd temp0 = mu * crossMuN(mode.normal, fcurlN0.conjugate());
				bEdge(mappingEdge[i]) += wght[gp] * fDetJac * matlabSumProduct(alphaPhys[i], temp0);
			}
			P += wght[gp] * fDetJac * (fW(0) * fN1(0) + fW(1) * fN1(1));
			b1 += wght[gp] * fDetJac * (fW(0) * fN0(0) + fW(1) * fN0(1));
		}
		else
		{
			for (int i = 0; i < 3; i++)
			{
				G(mappingEdge[i]) += wght[gp] * fDetJac * (fW(0) * alphaPhys[i](0) + fW(1) * alphaPhys[i](1));
				const Vector3cd temp = mu * crossMuN(mode.normal, fcurlN2);
				S_or_T(mappingEdge[i]) += wght[gp] * fDetJac * matlabSumProduct(alphaPhys[i], temp);
			}
			P += wght[gp] * fDetJac * (fW(0) * fN2(0) + fW(1) * fN2(1));
		}
	}
}

void blockAppendInputPort(
	SparseMatrix<std::complex<double>, RowMajor>& A,
	VectorXcd& b,
	int edgeDof,
	const VectorXcd& S, const VectorXcd& G,
	std::complex<double> P, std::complex<double> b1)
{
	const int n = static_cast<int>(b.size());
	SparseMatrix<std::complex<double>, RowMajor> Aold = A;
	A = SparseMatrix<std::complex<double>, RowMajor>(n + 1, n + 1);
	std::vector<Triplet<std::complex<double>>> trips;
	for (int k = 0; k < Aold.outerSize(); k++)
		for (SparseMatrix<std::complex<double>, RowMajor>::InnerIterator it(Aold, k); it; ++it)
			trips.emplace_back(it.row(), it.col(), it.value());
	for (int i = 0; i < edgeDof; i++)
		if (std::abs(S(i)) > 0)
			// Port column must match MATLAB PortBC (not Hermitian transpose).
			trips.emplace_back(i, n, std::conj(S(i)));
	for (int i = 0; i < edgeDof; i++)
		if (std::abs(G(i)) > 0)
			trips.emplace_back(n, i, G(i));
	trips.emplace_back(n, n, -P);
	A.setFromTriplets(trips.begin(), trips.end());
	VectorXcd bnew(n + 1);
	bnew.head(n) = -b;
	bnew(n) = b1;
	b = bnew;
}

void blockAppendOutputPort(
	SparseMatrix<std::complex<double>, RowMajor>& A,
	VectorXcd& b,
	int edgeDof,
	const VectorXcd& T, const VectorXcd& G,
	std::complex<double> P)
{
	const int n = static_cast<int>(b.size());
	SparseMatrix<std::complex<double>, RowMajor> Aold = A;
	A = SparseMatrix<std::complex<double>, RowMajor>(n + 1, n + 1);
	std::vector<Triplet<std::complex<double>>> trips;
	for (int k = 0; k < Aold.outerSize(); k++)
		for (SparseMatrix<std::complex<double>, RowMajor>::InnerIterator it(Aold, k); it; ++it)
			trips.emplace_back(it.row(), it.col(), it.value());
	for (int i = 0; i < edgeDof; i++)
		if (std::abs(T(i)) > 0)
			trips.emplace_back(i, n, std::conj(T(i)));
	for (int i = 0; i < edgeDof; i++)
		if (std::abs(G(i)) > 0)
			trips.emplace_back(n, i, G(i));
	trips.emplace_back(n, n, -P);
	A.setFromTriplets(trips.begin(), trips.end());
	VectorXcd bnew(n + 1);
	bnew.head(n) = b;
	bnew(n) = 0.0;
	b = bnew;
}

} // namespace

void OpticsFEM_3D_Scatter::Assemble_Port()
{
	if (!_mPhy->HasNumericPort())
		return;

	_mIsReal = false;
	const int dof = _mDof;

	for (int p = 0; p < _mPhy->GetNbrNumericPort(); p++)
	{
		const PortNumericMode& mode = _mPhy->GetNumericPort(p);
		const bool isInput = (mode.type == 1);
		const int nf = mode.facesConn.rows();

		VectorXcd G = VectorXcd::Zero(dof);
		VectorXcd S_or_T = VectorXcd::Zero(dof);
		VectorXcd bEdge = VectorXcd::Zero(dof);
		std::complex<double> P(0.0, 0.0);
		std::complex<double> b1(0.0, 0.0);

		for (int f = 0; f < nf; f++)
			assembleOnePortFace(_mMesh, _mMatLib, mode, f, G, S_or_T, bEdge, P, b1, isInput);

		if (isInput)
		{
			_mB_complex += bEdge;
			std::cout << "[OpticsFEM_3D_Scatter] Port input"
				<< " |S|=" << S_or_T.norm()
				<< " |G|=" << G.norm()
				<< " |bEdge|=" << bEdge.norm()
				<< " |P|=" << std::abs(P)
				<< " b1=" << b1 << std::endl;
			blockAppendInputPort(_mA_complex, _mB_complex, dof, S_or_T, G, P, b1);
		}
		else
		{
			std::cout << "[OpticsFEM_3D_Scatter] Port output"
				<< " |T|=" << S_or_T.norm()
				<< " |G|=" << G.norm()
				<< " |P|=" << std::abs(P) << std::endl;
			blockAppendOutputPort(_mA_complex, _mB_complex, dof, S_or_T, G, P);
		}
	}
	_mPortExtraDof = static_cast<int>(_mB_complex.size()) - dof;

	const int sysSize = static_cast<int>(_mB_complex.size());
	if (_mP_complex.rows() < sysSize)
	{
		Eigen::SparseMatrix<std::complex<double>, Eigen::RowMajor> Pnew(sysSize, sysSize);
		Pnew.setIdentity();
		_mP_complex = Pnew;
	}
}