#include"Assemble_Base.h"
#include"../function/BF.h"
#include"../function/Gauss.h"
#include"../common/define.h"
#include"../Eigen/Sparse"

#include <vector>
#include <algorithm>
#include <string>

using namespace std;
using namespace Eigen;

void OpticsFEM_2D_EigenMode::Assemble_WaveEquation()
{
	//physic
	double k0 = 2 * Pi / _mSolver->GetLda0();

	//init of Gauss point
	Gauss gauss;
	int NbrGuassPoints;
	double* u, * v, * w, * wght;
	NbrGuassPoints = gauss.GetNbrGaussPoints(TWODIM, TRIANGLE, BF_LINEFUNC * 2);
	u = new double[NbrGuassPoints];
	v = new double[NbrGuassPoints];
	w = new double[NbrGuassPoints];
	wght = new double[NbrGuassPoints];
	gauss.GetGaussPoints(TWODIM, TRIANGLE, u, v, w, wght);

	//init of geo
	Vector3d* vertex = new Vector3d[3];
	Matrix3d Jac, InvJac, TJac;

	//init of basis function
	BF BF_Lagrange, BF_Grad_Lagrange, BF_Nedelec, BF_Curl_Nedelec;
	int sdof, vdof;
	Vector3d** Et, ** curlEt, ** Ez, ** gradEz;
	sdof = BF_Lagrange.GetNbrBF(TWODIM, TRIANGLE, BF_LAGRANGE, BF_LINEFUNC);
	BF_Grad_Lagrange.GetNbrBF(TWODIM, TRIANGLE, BF_GRAD_LAGRANGE, BF_LINEFUNC);
	vdof = BF_Nedelec.GetNbrBF(TWODIM, TRIANGLE, BF_NEDELEC, BF_LINEFUNC);
	BF_Curl_Nedelec.GetNbrBF(TWODIM, TRIANGLE, BF_CURL_NEDELEC, BF_LINEFUNC);
	Et = new Vector3d * [NbrGuassPoints];
	curlEt = new Vector3d * [NbrGuassPoints];
	Ez = new Vector3d * [NbrGuassPoints];
	gradEz = new Vector3d * [NbrGuassPoints];
	for (int i = 0; i < NbrGuassPoints; i++)
	{
		Et[i] = new Vector3d[vdof];
		curlEt[i] = new Vector3d[vdof];
		Ez[i] = new Vector3d[sdof];
		gradEz[i] = new Vector3d[sdof];
	}
	
	//loop over tri
	int NbrTri = _mMesh->GetNbrTri();
	for (int n = 0; n < NbrTri; n++)
	{
		//coordinate of vertex
		for (int i = 0; i < 3; i++)
			_mMesh->GetVertex(_mMesh->GetTri(n, i), vertex[i]);

		//Jac
		Jac(0, 0) = vertex[1](0) - vertex[0](0); Jac(0, 1) = vertex[1](1) - vertex[0](1); Jac(0, 2) = 0.;
		Jac(1, 0) = vertex[2](0) - vertex[0](0); Jac(1, 1) = vertex[2](1) - vertex[0](1); Jac(1, 2) = 0.;
		Jac(2, 0) = 0.;			Jac(2, 1) = 0.;			Jac(2, 2) = 1.;
		InvJac = Jac.inverse();
		double DetJac = fabs(Jac.determinant());
		TJac = Jac.transpose() / Jac.determinant();

		//basis function
		for (int i = 0; i < NbrGuassPoints; i++)
		{
			for (int j = 0; j < sdof; j++)
			{
				BF_Lagrange.GetValueBF(j + 1, u[i], v[i], w[i], Ez[i][j]);
				BF_Grad_Lagrange.GetValueBF(j + 1, u[i], v[i], w[i], gradEz[i][j]);
				gradEz[i][j] = InvJac * gradEz[i][j];
			}
			for (int j = 0; j < vdof; j++)
			{
				BF_Nedelec.GetValueBF(j + 1, u[i], v[i], w[i], Et[i][j]);
				Et[i][j] = InvJac * Et[i][j];
				BF_Curl_Nedelec.GetValueBF(j + 1, u[i], v[i], w[i], curlEt[i][j]);
				curlEt[i][j] = TJac * curlEt[i][j];
			}
		}

		//material
		int domain = _mMesh->GetDomainOfTri(n);
		Matrix3cd epsr = _mMatLib->GetEpsr(domain);
		Matrix3d sigma = _mMatLib->GetSigma(domain);
		epsr = epsr - sigma * complex<double>(0.0, 1.0 / k0 * 120.0 * Pi);
		Matrix3cd Mur = _mMatLib->GetMur(domain);
		//PML
		Matrix3cd invMur;
		Eigen::VectorXd PMLData;
		double lambda;
		int PMLType;
		if (_mPhy->GetPML(domain, PMLType, PMLData, lambda))
		{
			int R0 = 10;
			double averX = (vertex[0](0) + vertex[1](0) + vertex[2](0)) / 3.0;
			double averY = (vertex[0](1) + vertex[1](1) + vertex[2](1)) / 3.0;
			Matrix3cd Lambda = Matrix3cd::Zero();
			if (PMLType == 0)
			{
				complex<double>  sx{ 1,-fabs((averX - PMLData(0)) / PMLData(1)) * R0 / PMLData(1) / k0 };
				complex<double>  sy{ 1,-fabs((averY - PMLData(2)) / PMLData(3)) * R0 / PMLData(3) / k0 };
				Lambda(0, 0) = sy / sx; Lambda(1, 1) = sx / sy; Lambda(2, 2) = sx * sy;
			}
			else if (PMLType == 1)
			{
				double rho = sqrt(averX * averX + averY * averY);
				double sigma = pow((rho - PMLData(1)) / PMLData(3), 2) * R0 / PMLData(3) / k0;
				complex<double> s1{ 1, -PMLData(3) / 2 / rho * sigma };
				complex<double> s2{ 1, -sigma };
				complex<double> aa = s1 / s2; complex<double> bb = s2 / s1; complex<double> cc = s1 * s2;
				Lambda(0, 0) = (aa * averX * averX + bb * averY * averY) / rho / rho;
				Lambda(0, 1) = (aa - bb) * averX * averY / rho / rho;
				Lambda(1, 0) = (aa - bb) * averX * averY / rho / rho;
				Lambda(1, 1) = (bb * averX * averX + aa * averY * averY) / rho / rho;
				Lambda(2, 2) = cc;
			}
			epsr = epsr * Lambda;
			invMur = (Mur * Lambda).inverse();
		}
		else
		{
			invMur = Mur.inverse();
		}
		//treat for weak form
		Matrix3cd epst = epsr;
		epst(2, 2) = 0.0;
		complex<double> epsz = epsr(2, 2);
		complex<double> murz = invMur(2, 2);
		Matrix3cd murt = Matrix3cd::Zero();
		murt(0, 0) = invMur(1, 1); murt(0, 1) = -invMur(1, 0);
		murt(1, 0) = -invMur(0, 1); murt(1, 1) = invMur(0, 0);

		//mapping 
		VectorXi MappingIndexS = VectorXi::Zero(sdof);
		VectorXi MappingIndexV = VectorXi::Zero(vdof);
		for (int i = 0; i < sdof; i++)
			MappingIndexS(i) = _mMesh->GetTri(n, i);
		for (int i = 0; i < vdof; i++)
			MappingIndexV(i) = _mMesh->GetEdgeOfTri(n, i) + _mMesh->GetNbrVertex();

		//submatrix
		MatrixXcd St, Sz, Tte, Tz, Ttu, G, Gt;
		St = MatrixXcd::Zero(vdof, vdof);
		Sz = MatrixXcd::Zero(sdof, sdof);
		Tte = MatrixXcd::Zero(vdof, vdof);
		Tz = MatrixXcd::Zero(sdof, sdof);
		Ttu = MatrixXcd::Zero(vdof, vdof);
		G = MatrixXcd::Zero(sdof, vdof);
		Gt = MatrixXcd::Zero(vdof, sdof);
		for (int i=0;i<sdof;i++)
			for (int j=0;j<sdof;j++)
				for (int k = 0; k < NbrGuassPoints; k++)
				{
					Sz(i, j) = Sz(i, j) + wght[k] * DetJac * (murt * gradEz[k][j]).dot(gradEz[k][i]);
					Tz(i, j) = Tz(i, j) + wght[k] * DetJac * k0 * k0 * epsz * Ez[k][i].dot(Ez[k][j]);
				}
		for (int i = 0; i < vdof; i++)
			for (int j = 0; j < vdof; j++)
				for (int k = 0; k < NbrGuassPoints; k++)
				{
					St(i, j) = St(i, j) + wght[k] * DetJac * murz * curlEt[k][i].dot(curlEt[k][j]);
					Tte(i, j) = Tte(i, j) + wght[k] * DetJac * k0 * k0 * Et[k][i].dot(epst * Et[k][j]);
					Ttu(i, j) = Ttu(i, j) + wght[k] * DetJac * Et[k][i].dot(murt * Et[k][j]);
				}
		for (int i = 0; i < sdof; i++)
			for (int j = 0; j < vdof; j++)
				for (int k = 0; k < NbrGuassPoints; k++)
				{
					G(i, j) = G(i, j) + wght[k] * DetJac * gradEz[k][i].dot(murt * Et[k][j]);
					Gt(j, i) = Gt(j, i) + wght[k] * DetJac * Et[k][j].dot(murt * gradEz[k][i]);
				}


		//store in triplet
		if (_mIsReal)
		{
			for (int i = 0; i < sdof; i++)
			{
				for (int j = 0; j < sdof; j++)
				{
					_mTripleB_real.push_back(Triplet<double>(MappingIndexS(i), MappingIndexS(j), Sz(i, j).real() - Tz(i, j).real()));
				}
			}
			for (int i = 0; i < vdof; i++)
			{
				for (int j = 0; j < vdof; j++)
				{
					_mTripleA_real.push_back(Triplet<double>(MappingIndexV(i), MappingIndexV(j), St(i, j).real() - Tte(i, j).real()));
					_mTripleB_real.push_back(Triplet<double>(MappingIndexV(i), MappingIndexV(j), Ttu(i, j).real()));
				}
			}
			for (int i = 0; i < sdof; i++)
			{
				for (int j = 0; j < vdof; j++)
				{
					_mTripleB_real.push_back(Triplet<double>(MappingIndexS(i), MappingIndexV(j), G(i, j).real()));
					_mTripleB_real.push_back(Triplet<double>(MappingIndexV(j), MappingIndexS(i), Gt(j, i).real()));
				}
			}
		}
		else
		{
			for (int i = 0; i < sdof; i++)
			{
				for (int j = 0; j < sdof; j++)
				{
					_mTripleB_complex.push_back(Triplet<complex<double>>(MappingIndexS(i), MappingIndexS(j), Sz(i, j) - Tz(i, j)));
				}
			}
			for (int i = 0; i < vdof; i++)
			{
				for (int j = 0; j < vdof; j++)
				{
					_mTripleA_complex.push_back(Triplet<complex<double>>(MappingIndexV(i), MappingIndexV(j), St(i, j) - Tte(i, j)));
					_mTripleB_complex.push_back(Triplet<complex<double>>(MappingIndexV(i), MappingIndexV(j), Ttu(i, j)));
				}
			}
			for (int i = 0; i < sdof; i++)
			{
				for (int j = 0; j < vdof; j++)
				{
					_mTripleB_complex.push_back(Triplet<complex<double>>(MappingIndexS(i), MappingIndexV(j), G(i,j)));
					_mTripleB_complex.push_back(Triplet<complex<double>>(MappingIndexV(j), MappingIndexS(i), Gt(j, i)));
				}
			}

		}
	}

	delete[] u, v, w, wght;

	delete[] vertex;

	for (int i = 0; i < NbrGuassPoints; i++)
	{
		delete[] Et[i], curlEt[i], Ez[i], gradEz[i];
	}
	delete[] Et, curlEt, Ez, gradEz;

}