#pragma once
#include "discretization_helpers.hpp"
#include "FillBoundary_fwd.hpp"
#include "KDTree.hpp"
#include "KDTreeMutable.hpp"
#include <iomanip>
#include <random>
#include <map>

namespace meshless {

	DomainDiscretization<Eigen::Vector3d> discretizeBoundaryWithDensity(const OccShape& shape, const std::function<double(Eigen::Vector3d)>& h, int type) {
		if(type == 0)type = -1;
		std::mt19937 gen(shape.seed_);
		DomainDiscretization<Eigen::Vector3d> domainGlobal(shape);
		KDTreeMutable treeGlobal;
		TopExp_Explorer expFace(shape.myShape, TopAbs_FACE);
		int numFace = 0;

		std::cout.precision(10);

		for(; expFace.More(); expFace.Next()) {
			std::cout << "正在离散化第" << ++numFace << "张曲面" << std::endl;
			//if(numFace != 5)continue;
			bool reverseNormal = false;
			const TopoDS_Face& face = TopoDS::Face(expFace.Current());
			Handle(Geom_Surface) geomSurface = BRep_Tool::Surface(face);
			if(face.Orientable() == TopAbs_REVERSED) {
				std::cout << "该面法线需反向\n";
				reverseNormal = true;
			}
			DomainDiscretization<Eigen::Vector2d> domainParamLocalPatch(shape);

			//开始离散化每个wire
			auto outerWire = BRepTools::OuterWire(face);
			int numWire = 0;
			TopExp_Explorer expWire(face, TopAbs_WIRE);
			for(; expWire.More(); expWire.Next()) {
				std::cout << "正在离散化第" << ++numWire << "个Wire" << std::endl;
				const TopoDS_Wire& wire = TopoDS::Wire(expWire.Current());
				if(wire == outerWire) {
					std::cout << "此wire为外环\n";
				}
				TopExp_Explorer expEdge(wire, TopAbs_EDGE);
				int numEdge = 0;
				int genPoints = 0;
				for(; expEdge.More(); expEdge.Next()) {

					std::cout << "正在离散化第" << ++numEdge << "条Edge" << std::endl;
					//if(numEdge != 5)continue;
					const TopoDS_Edge& edge = TopoDS::Edge(expEdge.Current());

					KDTreeMutable treeLocal;
					Standard_Real firstParam, lastParam;
					Handle(Geom_Curve) geomCurve = BRep_Tool::Curve(edge, firstParam, lastParam);
					//std::cout << "param: " << firstParam << " -> " << lastParam << '\n';

					//曲线参数点在曲面参数域上的坐标
					Standard_Real pCurveFirstParam, pCurveLastParam;
					Handle(Geom2d_Curve) geomPCurve = BRep_Tool::CurveOnSurface(edge, face, pCurveFirstParam, pCurveLastParam);
					Handle(Geom2d_TrimmedCurve) geomTrimmedCurve = new Geom2d_TrimmedCurve(geomPCurve, pCurveFirstParam, pCurveLastParam);

					//GeomTools::Dump(geomCurve, std::cout);
					//std::cout << "In param Pcurve:\n";
					//GeomTools::Dump(geomPCurve, std::cout);
					std::cout << "GeomPcurve P:" << pCurveFirstParam << ',' << pCurveLastParam << '\n';

					std::uniform_real_distribution<> dis(pCurveFirstParam, pCurveLastParam);
					//曲线参数点
					double tSeedInParam = dis(gen);
					gp_Pnt2d tSeedInPCurve;
					geomPCurve->D0(tSeedInParam, tSeedInPCurve);


					//std::cout << "t in param: " << tSeedInParam << "\n";
					//std::cout << "t in PCurve: " << tSeedInPCurve.Coord().X() << "," << tSeedInPCurve.Coord().Y() << "\n";

					//!这里两种计算方法得到的结果不一致，第一种更准确
					gp_Pnt tSeedInCurve;
					geomCurve->D0(tSeedInParam, tSeedInCurve);
					gp_Pnt tSeedInSurf;
					geomSurface->D0(tSeedInPCurve.X(), tSeedInPCurve.Y(), tSeedInSurf);
					//! Tip: use GeomLib::NormEstim() to calculate surface normal at specified (U, V) point.

					std::cout << "t in Surface:#1 " << tSeedInCurve.Coord().X() << "," << tSeedInCurve.Coord().Y() << "," << tSeedInCurve.Coord().Z() << "\n";
					std::cout << "t in Surface:#2 " << tSeedInSurf.Coord().X() << "," << tSeedInSurf.Coord().Y() << "," << tSeedInSurf.Coord().Z() << "\n";

					Eigen::Vector3d eigenSeedPnt(tSeedInCurve.X(), tSeedInCurve.Y(), tSeedInCurve.Z());
					Eigen::Vector2d eigenSeedPntParam(tSeedInPCurve.X(), tSeedInPCurve.Y());
					domainParamLocalPatch.addInternalNodeWithT(transPnt2d(tSeedInPCurve), tSeedInParam, 1);
					treeLocal.insert(transPnt(tSeedInCurve));


					//Insert into global structures.
					double checkRadiusSeed = h(eigenSeedPnt);

					double d_sq = treeGlobal.size() == 0 ? 10 * checkRadiusSeed * checkRadiusSeed : treeGlobal.query(eigenSeedPnt).second[0];
					if(d_sq >= (shape.zeta * checkRadiusSeed) * (shape.zeta * checkRadiusSeed)) {
						//domainGlobal.addBoundaryNode(eigenSeedPnt, -1);
						gp_Dir tSeedNormal;
						auto retStatus = GeomLib::NormEstim(geomSurface, tSeedInPCurve, 1e-6, tSeedNormal);
						if(retStatus >= 2) {
							std::cout << "calculate wrong!\n";
							exit(-1);
						}
						if(reverseNormal) tSeedNormal = -tSeedNormal;
						Eigen::Vector3d eigenNormal(tSeedNormal.X(), tSeedNormal.Y(), tSeedNormal.Z());
						domainGlobal.addBoundaryNode(eigenSeedPnt, type, eigenNormal);
						treeGlobal.insert(eigenSeedPnt);
					}

					int curNode = domainParamLocalPatch.size() - 1;
					int endNode = domainParamLocalPatch.size();
					while(curNode < endNode && endNode < shape.max_points_boundary) {
						genPoints++;
						//std::cout << "curNode / endNode : " << curNode << '/' << endNode << '\n';
						auto param = domainParamLocalPatch.pos(curNode);
						double t = domainParamLocalPatch.getT(curNode);
						std::vector<double> candidates{ 1.0, -1.0 };

						gp_Pnt pt;
						gp_Vec der;
						geomCurve->D1(t, pt, der);
						//std::cout << "pt: " << pt.X() << "," << pt.Y() << "," << pt.Z() << '\n';
						//std::cout << "der: " << der.X() << "," << der.Y() << "," << der.Z() << '\n';
						//std::cout << "der.Mag:" << der.Magnitude() << '\n';
						double alpha = h(transPnt(pt)) / der.Magnitude();
						for(const auto& uCan : candidates) {
							//参数点必须在规定的参数域内
							double tNew = t + alpha * uCan;
							if(tNew > pCurveLastParam || tNew < pCurveFirstParam) {
								//std::cout << "排除\n";
								continue;
							}
							//std::cout << "tNew:" << tNew << "\n";
							gp_Pnt2d tNewSeedInPCurve;
							geomPCurve->D0(tNew, tNewSeedInPCurve);
							gp_Pnt ptNew;
							geomCurve->D0(tNew, ptNew);
							gp_Pnt ptNew2;
							geomSurface->D0(tNewSeedInPCurve.X(), tNewSeedInPCurve.Y(), ptNew2);
							//std::cout << "ptNew: " << ptNew.X() << "," << ptNew.Y() << "," << ptNew.Z() << '\n';
							//std::cout << "ptNew2: " << ptNew2.X() << "," << ptNew2.Y() << "," << ptNew2.Z() << '\n';
							double checkradius = pt.SquareDistance(ptNew);
							//std::cout << "CheckRadius: " << checkradius << '\n';


							//距离已有的点不能太近
							double d_sq_loc = treeLocal.query(transPnt(ptNew)).second[0];
							//std::cout << "d_sq_loc: " << d_sq_loc << '\n';
							if(d_sq_loc >= (shape.zeta * shape.zeta * checkradius)) {
								domainParamLocalPatch.addInternalNodeWithT(transPnt2d(tNewSeedInPCurve), tNew, 1);
								treeLocal.insert(transPnt(ptNew));
								endNode++;
								double d_sq_global = treeGlobal.query(transPnt(ptNew)).second[0];
								if(d_sq_global >= (shape.zeta * shape.zeta * checkradius)) {
									gp_Dir tSeedNormalNew;
									auto retStatus = GeomLib::NormEstim(geomSurface, tNewSeedInPCurve, 1e-6, tSeedNormalNew);
									if(retStatus >= 2) {
										std::cout << "calculate wrong!\n";
										exit(-1);
									}
									if(reverseNormal) {
										tSeedNormalNew = -tSeedNormalNew;
									}
									domainGlobal.addBoundaryNode(transPnt(ptNew), type, transVec(tSeedNormalNew));
									treeGlobal.insert(transPnt(ptNew));
								} else {
									//std::cout << "全局不添加点\n";
								}
							} else {
								//std::cout << "局部不添加点\n";
							}
						}
						curNode++;
					}
					std::cout << "该边已经生成：" << genPoints << "个点\n";
				}
			}
			//fill boundary surface!
			KDTree paramBoundarySearch;
			int curNode = 0, endNode = domainParamLocalPatch.size();
			while(curNode < endNode && endNode < shape.max_points_boundary) {
				auto paramPoint = domainParamLocalPatch.pos(curNode);
				gp_Pnt initPt;
				gp_Vec derU, derV;
				//geomSurface->D0(paramPoint.x(), paramPoint.y(), initPt);
				geomSurface->D1(paramPoint.x(), paramPoint.y(), initPt, derU, derV);
				Eigen::MatrixXd jacobMat;
				jacobMat.resize(3, 2);
				jacobMat(0, 0) = derU.X();
				jacobMat(0, 1) = derV.X();
				jacobMat(1, 0) = derU.Y();
				jacobMat(1, 1) = derV.Y();
				jacobMat(2, 0) = derU.Z();
				jacobMat(2, 1) = derV.Z();

				auto unitCandidates = SphereDiscretization<double, 2>::construct(1, shape.n_samples, gen);

				for(auto& uCand : unitCandidates) {
					double alpha = h(transPnt(initPt)) / (jacobMat * uCand).norm();
					auto node = paramPoint + alpha * uCand;
					gp_Pnt2d nodePnt(node.x(), node.y());

					//该参数点必须在参数域内
					BRepClass_FaceClassifier brepF;
					brepF.Perform(face, nodePnt, 1e-6);
					if(brepF.State() == TopAbs_State::TopAbs_IN) {
						//std::cout << "TopAbs_IN\n";
					}
					if(brepF.State() == TopAbs_State::TopAbs_ON) {
						//std::cout << "TopAbs_ON\n";
						continue;
					}
					if(brepF.State() == TopAbs_State::TopAbs_OUT) {
						//std::cout << "TopAbs_OUT\n";
						continue;
					}
					if(brepF.State() == TopAbs_State::TopAbs_UNKNOWN) {
						//std::cout << "TopAbs_UNKNOWN\n";
						continue;
					}

					gp_Pnt newPt;
					gp_Vec derU, derV;
					geomSurface->D1(node.x(), node.y(), newPt, derU, derV);

					double d_sq = treeGlobal.query(transPnt(newPt)).second[0];
					double checkradius = newPt.SquareDistance(initPt);
					if(d_sq < (shape.zeta * shape.zeta * checkradius)) {
						continue;
					}
					domainParamLocalPatch.addInternalNode(node, 1);
					treeGlobal.insert(transPnt(newPt));
					gp_Dir normal;
					auto retStatus = GeomLib::NormEstim(geomSurface, nodePnt, 1e-6, normal);
					if(retStatus >= 2) {
						std::cout << "calculate wrong!\n";
						exit(-1);
					}
					if(reverseNormal) {
						normal = -normal;
					}
					domainGlobal.addBoundaryNode(transPnt(newPt), type, transVec(normal));
					endNode++;
				}
				curNode++;
			}
			if(endNode >= shape.max_points_boundary) {
				std::cerr << "Maximum number of points reached, fill may be incomplete." << std::endl;
			}
		}
		return domainGlobal;
	}
};