#include "StdAfx.h"
#include <algorithm>
#include "keyholing.h"
#include "clipper2/clipper.core.h"

using namespace Clipper2Lib;

inline size_t GetLowestPtIdx(const Path64& path)
{
	size_t result = 0;
	Point64 resPt = path[0];
	for (size_t i = 1; i < path.size(); ++i)
		if (path[i].y < resPt.y) continue;
		else if (path[i].y > resPt.y || path[i].x < resPt.x)
		{
			result = i;
			resPt = path[i];
		}
	return result;
}

//inline size_t GetNearestPtIdx(const Path64& path, const Point64& pt)
//{
//	size_t result = 0;
//	double distSqr = DistanceSqr(path[0], pt);
//	for (size_t i = 1; i < path.size(); ++i)
//	{
//		double ds = DistanceSqr(path[i], pt);
//		if (ds >= distSqr) continue;
//		distSqr = ds;
//		result = i;
//	}
//	return result;
//}

static Point64 GetClosestPtOnSegment(const Point64& seg1, const Point64& seg2, const Point64& pt)
{
	double dx = (seg2.x - seg1.x);
	double dy = (seg2.y - seg1.y);
	if (dx == 0.0 && dy == 0.0) return  seg1;
	double q = (dx * (pt.x - seg1.x) + dy * (pt.y - seg1.y)) / (Sqr(dx) + Sqr(dy));
	// and restrict the point to the segment ...
	q = std::fmax(0, std::fmin(1, q));
	return Point64((1 - q) * seg1.x + q * seg2.x, (1 - q) * seg1.y + q * seg2.y);
}
// 旋转点 (x, y) 逆时针旋转角度 theta (使用预先计算的 cosTheta 和 sinTheta)
inline Point64 rotatePoint(const Point64& p, double cosTheta, double sinTheta) {
	return { p.x * cosTheta + p.y * sinTheta, -p.x * sinTheta + p.y * cosTheta };
}
//// 将整个坐标系旋转，使线段水平
//inline Point64 rotatePointToHorizontal(const Point64& p, const Point64& origin, double cosTheta, double sinTheta) {
//	// 平移点到原点，然后应用旋转
//	double translatedX = p.x - origin.x;
//	double translatedY = p.y - origin.y;
//	return {
//		translatedX * cosTheta + translatedY * sinTheta, // X轴方向
//		-translatedX * sinTheta + translatedY * cosTheta // Y轴方向
//	};
//}
//// 点与X轴平行的两个点的最近距离点
//static Point64 GetClosestPtOnSegmentX(const Point64& seg1, const Point64& seg2, const Point64& pt)
//{
//	int64_t dx = (seg2.x - seg1.x);
//	int64_t dy = (seg2.y - seg1.y);
//	if (dx == 0.0 && dy == 0.0) return  seg1;
//
//	int64_t segMinX = std::fmin(seg1.x, seg2.x);
//
//	//if(pt.x<std::fmin(seg1.x, seg2.x))
//	double q = (dx * (pt.x - seg1.x) + dy * (pt.y - seg1.y)) / (Sqr(dx) + Sqr(dy));
//	// and restrict the point to the segment ...
//	q = std::fmax(0, std::fmin(1, q));
//	return Point64((1 - q) * seg1.x + q * seg2.x, (1 - q) * seg1.y + q * seg2.y);
//}

inline Point64 GetYIntersectPt(const Point64& ln1, const Point64& ln2, int64_t Y)
{
	double dx = static_cast<double>(ln2.x - ln1.x);
	if (dx == 0.0) return Point64(ln1.x, Y);
	double dy = static_cast<double>(ln2.y - ln1.y);
	// y = dy/dx * x + b
	// b = y1 - dy/dx * x1;
	double b = ln1.y - dy / dx * ln1.x;
	// x = (Y - b) * dx/dy 
	return Point64(static_cast<int64_t>(round((Y - b) * dx / dy)), Y);
}

static void JoinAtClosestPtBelowHole(Path64& outer, Path64 hole)
{
	// precondition: hole[0] is the hole's lowest vertex
	// find the closest point on the outer path (cpOuter) to hole[0],
	// while ensuring cpOuter is **below** holePt. (Joining with an 
	// 'outer' segment that's above holePt (ie y < holePt.y) risks the 
	// new join passing through a hole that hasn't yet been keyholed.)

	int highI = static_cast<int>(outer.size()) - 1;
	Point64 holePt = hole[0];
	Point64 cpOuter;
	int cpOuterIdx = -1;
	double distSqrd = MAX_DBL;
	for (size_t i = 0; i < highI; ++i)
	{
		Point64 nextPt = (i == highI) ? outer[0] : outer[i + 1];
		// Since cpOuter.y must be below holePt.y ...
		if (outer[i].y <= holePt.y && nextPt.y <= holePt.y) continue;

		// 'outer' may have overlapping collinear segments due to previous 
		// hole joins. Therefore it's imperative when the closest segment 
		// happens to be one of these overlapping collinear segments, that 
		// the new join is with the correct one. For example, when a hole 
		// is on the left of an outer segment, that outer segment **must** 
		// be descending. Likewise, when a hole is on the right of an
		// outer segment, then that outer segment must be descending.
		if (CrossProduct(holePt, outer[i], nextPt) < 0) continue;

		// Again make sure cpOuter.y is below holePt.y ...
		Point64 cp;
		if (outer[i].y <= holePt.y)
		{
			Point64 pt = GetYIntersectPt(outer[i], nextPt, holePt.y + 1);
			cp = GetClosestPtOnSegment(pt, nextPt, holePt);
		}
		else if (nextPt.y <= holePt.y)
		{
			Point64 pt = GetYIntersectPt(outer[i], nextPt, holePt.y + 1);
			cp = GetClosestPtOnSegment(outer[i], pt, holePt);
		}
		else
			cp = GetClosestPtOnSegment(outer[i], nextPt, holePt);

		double ds = DistanceSqr(cp, holePt);
		if (cpOuterIdx >= 0 && ds >= distSqrd) continue;

		cpOuter = cp;
		distSqrd = ds;
		cpOuterIdx = (cp == nextPt) ? i + 1 : i;
		if (ds == 0) break;
	}

	// insert duplicate points into both outer and hole paths
	if (cpOuter != outer[cpOuterIdx])
	{
		++cpOuterIdx;
		if (cpOuterIdx > highI) cpOuterIdx = 0;
		outer.insert(outer.begin() + cpOuterIdx, cpOuter);
	}
	outer.insert(outer.begin() + cpOuterIdx, cpOuter);
	++cpOuterIdx;
	if (cpOuterIdx >= outer.size()) cpOuterIdx = 0;

	hole.insert(hole.begin(), holePt);

	// finally join the hole with its outer path
	outer.reserve(outer.size() + hole.size());
	outer.insert(outer.begin() + cpOuterIdx, hole.begin(), hole.begin() + 1);
	outer.insert(outer.begin() + cpOuterIdx, hole.begin() + 1, hole.end());
}

struct PolySorter {
	inline bool operator()(const Path64& path1, const Path64& path2)
	{
		return path2[0].y < path1[0].y;
	}
};

static bool KeyHoleOuter(const PolyPath64* outer, Paths64& paths)
{
	if (!outer->Count())
	{
		paths.push_back(outer->Polygon());
		return true;
	}
	Paths64 tmp;
	tmp.reserve(outer->Count());
	for (const auto& hole : *outer)
	{
		Path64 p = hole->Polygon();
		// rearrange p so the lower vertex is first
		size_t i = GetLowestPtIdx(p);
		for (size_t j = 0; j < i; ++j)
		{
			Path64::iterator it = p.begin();
			std::rotate(it, it + 1, p.end());
		}
		tmp.push_back(p);
	}

	// sort holes so the lowest ones are first
	std::sort(tmp.begin(), tmp.end(), PolySorter());

	Path64 merged = outer->Polygon();
	// merge all holes into the outer path
	for (Paths64::iterator it = tmp.begin(); it != tmp.end(); ++it)
	{
		JoinAtClosestPtBelowHole(merged, *it);
		//std::cout << merged;
	}
	paths.push_back(merged);

	// finally do any nested outer polygons
	for (const auto& hole : *outer)
	{ 
		for (const auto& nested_outer : *hole)
		{
			if (!KeyHoleOuter(nested_outer.get(), paths))
			{
				return false;
			}
		}
	}
	return true;
}

bool KeyHole(const PolyTree64& polytree, Paths64& solution)
{
	for (const auto& outer : polytree)
		if (!KeyHoleOuter(outer.get(), solution)) return false;
	return true;
}
//////////////////////////////////////////////////////////////////////////////////////

struct BoundingBox64 {
	int64_t minX, minY, maxX, maxY;
	BoundingBox64() {}
	BoundingBox64(int64_t minX, int64_t minY, int64_t maxX, int64_t maxY)
		: minX(minX), minY(minY), maxX(maxX), maxY(maxY) {}
	// 判断两个包围盒是否有交集
	bool Intersects(const BoundingBox64& other) const {
		return !(other.maxX < this->minX || other.minX > this->maxX ||
			other.maxY < this->minY || other.minY > this->maxY);
	}
	// 计算两个包围盒之间的最小距离（粗略估算）
	double DistanceTo(const BoundingBox64& other) const {
		// if (Intersects(other)) return 0;
		int64_t dx = max(int64_t(0), max(minX - other.maxX, other.minX - maxX));
		int64_t dy = max(int64_t(0), max(minY - other.maxY, other.minY - maxY));
		return sqrt(dx * dx + dy * dy);
		// return min(dx, dy, )
	}
	void CreateBoundary(const Path64 path) {
		minX = INT64_MAX;
		maxX = INT64_MIN;
		minY = INT64_MAX;
		maxY = INT64_MIN;
		for (const auto& p : path) {
			minX = min(minX, p.x);
			minY = min(minY, p.y);
			maxX = max(maxX, p.x);
			maxY = max(maxY, p.y);
		}
	}
};
// 计算两点之间的距离
inline double distance(const Point64& p1, const Point64& p2) {
	return sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y));
}
double Clipper64Factor = 10000;
Point64 toPoint64(double x, double y) {
	int64_t nX = static_cast<int64_t>(x * Clipper64Factor);
	int64_t nY = static_cast<int64_t>(y* Clipper64Factor);
	return Point64(nX, nY);
}
// 将多边形转换为 Clipper 使用的 IntPoint 格式
Path64 toPolygon64(const PathD& poly) {
	Path64 clipperPoly;
	for (const PointD& p : poly) {
		clipperPoly.push_back(Point64(static_cast<int64_t>(p.x * Clipper64Factor), static_cast<int64_t>(p.y * Clipper64Factor)));
	}
	return clipperPoly;
}

// 将 Clipper 的 IntPoint 结果转换回原始 Polygon 格式
PathD fromPolygon64(const Path64& clipperPoly) {
	vector<PointD> vertices;
	for (const Point64& ip : clipperPoly) {
		vertices.emplace_back(ip.x / Clipper64Factor, ip.y/ Clipper64Factor);
	}
	return PathD(vertices);
}
// 找出旋转后距离线段最近的点
tuple<Point64, Point64, int, int, double> findClosestPointWithRotation(const Path64& poly1, const Path64& poly2, double& minDist) {
	Point64 closest1, closest2;
	//double minDist = distInit;
	int index1 = -1, index2 = -1;

	// 遍历第一个多边形的每一条边
	for (int i = 0; i < poly1.size(); i++)
	{
		Point64 p1 = poly1[i];
		Point64 p2 = poly1[(i + 1) % poly1.size()];

		// 计算线段 a-b 与 X 轴的夹角
		double dx = p2.x - p1.x;
		double dy = p2.y - p1.y;
		double theta = atan2(dy, dx);  // 不需要特别处理 dx == 0，atan2 自动处理
			// 预先计算 cos 和 sin，避免在旋转时多次计算
		double cosTheta = cos(theta);
		double sinTheta = sin(theta);
		// 坐标轴旋转后的坐标
		Point64 rotatedA = rotatePoint(p1, cosTheta, sinTheta);
		Point64 rotatedB = rotatePoint(p2, cosTheta, sinTheta);

		// 遍历第二个多边形的每个顶点，计算到每条边的距离
		for (int j = 0; j < poly2.size(); j++)
		{
			Point64 point = poly2[j];
			Point64 rotatedP = rotatePoint(point, cosTheta, sinTheta);
			double distY = fabs(rotatedP.y - rotatedA.y);
			if (distY > minDist)
			{
				continue;
			}
			// 如果旋转后的点的 x 坐标在线段的范围内，则计算垂直距离
			if (rotatedP.x >= rotatedA.x && rotatedP.x <= rotatedB.x) {
				//double dist = fabs(rotatedP.y - rotatedA.y); // 垂直距离为 y 坐标之差
				minDist = distY;
				//closestPoint = point; // 保存原坐标系中的点

				index1 = i;
				index2 = j;

				closest1 = Point64{ rotatedP.x , rotatedA.y };
				closest1 = rotatePoint(closest1, cosTheta, -sinTheta); // 旋转回原坐标系
				closest2 = point;
			}
			else {
				// 如果不在范围内，则计算到线段端点的距离
				size_t dSpaceXA = abs(rotatedP.x - rotatedA.x);
				size_t dSpaceXB = abs(rotatedP.x - rotatedB.x);
				size_t dSpaceXMin = min(dSpaceXA, dSpaceXB);
				if (dSpaceXA > minDist && dSpaceXB > minDist)
				{
					continue;
				}
				if (dSpaceXA < dSpaceXB) {
					double dDist = distance(rotatedA, rotatedP);
					if (dDist < minDist)
					{
						minDist = dDist;
						index1 = i;
						index2 = j;

						//closest1 = Point64{ rotatedA.x , rotatedA.y };
						//closest1 = rotatePoint(closest1, cosTheta, -sinTheta); // 旋转回原坐标
						closest1 = p1;
						closest2 = point;
					}
				}
				else {
					double dDist = distance(rotatedB, rotatedP);
					if (dDist < minDist)
					{
						minDist = dDist;
						index1 = i;
						index2 = j;

						//closest1 = Point64{ rotatedB.x , rotatedB.y };
						//closest1 = rotatePoint(closest1, cosTheta, -sinTheta); // 旋转回原坐标
						closest1 = p2;
						closest2 = point;
					}
				}
			}
		}
	}

	// 遍历第二个多边形的每一条边
	for (int i = 0; i < poly2.size(); i++)
	{
		Point64 p1 = poly2[i];
		Point64 p2 = poly2[(i + 1) % poly2.size()];

		// 计算线段 a-b 与 X 轴的夹角
		double dx = p2.x - p1.x;
		double dy = p2.y - p1.y;
		double theta = atan2(dy, dx);  // 不需要特别处理 dx == 0，atan2 自动处理
			// 预先计算 cos 和 sin，避免在旋转时多次计算
		double cosTheta = cos(theta);
		double sinTheta = sin(theta);
		// 将线段端点旋转到与 X 轴平行的坐标系
		Point64 rotatedA = rotatePoint(p1, cosTheta, sinTheta);
		Point64 rotatedB = rotatePoint(p2, cosTheta, sinTheta);

		// 遍历第一个多边形的每个顶点，计算到每条边的距离
		for (int j = 0; j < poly1.size(); j++)
		{
			Point64 point = poly1[j];
			Point64 rotatedP = rotatePoint(point, cosTheta, sinTheta);
			double distY = fabs(rotatedP.y - rotatedA.y);
			if (distY > minDist)
			{
				continue;
			}

			// 如果旋转后的点的 x 坐标在线段的范围内，则计算垂直距离
			if (rotatedP.x >= rotatedA.x && rotatedP.x <= rotatedB.x) {
				//double dist = fabs(rotatedP.y - rotatedA.y); // 垂直距离为 y 坐标之差
				minDist = distY;
				//closestPoint = point; // 保存原坐标系中的点
				index2 = i;
				index1 = j;

				closest2 = Point64{ rotatedP.x , rotatedA.y };
				closest2 = rotatePoint(closest2, cosTheta, -sinTheta); // 旋转回原坐标
				closest1 = point;
			}
			else
			{
				// 如果不在范围内，则计算到线段端点的距离
				size_t dSpaceXA = abs(rotatedP.x - rotatedA.x);
				size_t dSpaceXB = abs(rotatedP.x - rotatedB.x);
				size_t dSpaceXMin = min(dSpaceXA, dSpaceXB);
				if (dSpaceXA > minDist && dSpaceXB > minDist)
				{
					continue;
				}
				if (dSpaceXA < dSpaceXB) {
					double dDist = distance(rotatedA, rotatedP);
					if (dDist < minDist)
					{
						minDist = dDist;
						index2 = i;
						index1 = j;
						closest2 = p1;
						closest1 = point;
					}
				}
				else {
					double dDist = distance(rotatedB, rotatedP);
					if (dDist < minDist)
					{
						minDist = dDist;
						index2 = i;
						index1 = j;
						closest2 = p2;
						closest1 = point;
					}
				}
			}
		}
	}
	return { closest1, closest2, index1, index2, minDist };
}
// 计算两个多边形的最近顶点对
tuple<Point64, Point64, int, int, double> findClosestPoints(const Path64& poly1, const Path64& poly2) {
	Point64 closest1, closest2;
	double minDist = 1e30;
	int index1 = -1, index2 = -1;

	// 遍历第一个多边形的每一条边
	for (int i = 0; i < poly1.size(); i++)
	{
		Point64 p1 = poly1[i];
		Point64 p2 = poly1[(i + 1) % poly1.size()];

		// 遍历第二个多边形的每个顶点，计算到每条边的距离
		for (int j = 0; j < poly2.size(); j++)
		{
			Point64 point = poly2[j];
			Point64 ptConn = GetClosestPtOnSegment(p1, p2, point);
			double dDisConn = distance(ptConn, point);
			if (dDisConn < minDist) {
				minDist = dDisConn;

				index1 = i;
				index2 = j;

				closest1 = ptConn;
				closest2 = point;
			}
		}
	}

	// 遍历第二个多边形的每一条边
	for (int i = 0; i < poly2.size(); i++)
	{
		Point64 p1 = poly2[i];
		Point64 p2 = poly2[(i + 1) % poly2.size()];

		// 遍历第一个多边形的每个顶点，计算到每条边的距离
		for(int j=0;j<poly1.size();j++)
		{
			Point64 point = poly1[j];
			Point64 ptConn = GetClosestPtOnSegment(p1, p2, point);

			double dDisConn = distance(ptConn, point);
			if (dDisConn < minDist)
			{
				minDist = dDisConn;
				index2 = i;
				index1 = j;

				closest2 = ptConn;
				closest1 = point;
			}
		}
	}

	//for (int i = 0; i < poly1.size();i++) {
	//	Point64 p1 = poly1[i];
	//	for (int j = 0; j < poly2.size();j++) {
	//		Point64 p2 = poly2[j];
	//		double dist = distance(p1, p2);
	//		if (dist < minDist) {
	//			minDist = dist;
	//			closest1 = p1;
	//			closest2 = p2;

	//			index1 = i;
	//			index2 = j;
	//		}
	//	}
	//}
	return { closest1, closest2, index1, index2, minDist };
}
tuple<int, Point64, Point64, int, int, double> findClosestPath(const Path64& path, const Paths64& source, const std::vector< BoundingBox64> boundings)
{
	vector<int> vecSearchIndexs;
	BoundingBox64 bounding;
	bounding.CreateBoundary(path);
	double dDistMin = INT64_MAX;
	int nSearchNeareast = -1;
	for (int i = 0; i < boundings.size(); i++) {
		BoundingBox64* pBdCur = (BoundingBox64*)(&boundings[i]);
		if (pBdCur->Intersects(bounding)) {
			vecSearchIndexs.emplace_back(i);
			continue;
		}
		double dDist = bounding.DistanceTo(*pBdCur);
		if (dDist < dDistMin) {
			nSearchNeareast = i;
			dDistMin = dDist;
		}
	}
	if (nSearchNeareast > -1) {
		vecSearchIndexs.emplace_back(nSearchNeareast);
	}

	Point64 closest1, closest2;
	int idxFind = -1;
	int idxEdge1 = -1, idxEdge2 = -1;
	dDistMin = INT64_MAX;
	for (const auto& index : vecSearchIndexs)
	{
		Path64 item = source[index];
		//auto[p1, p2, index1, index2, distance] = findClosestPoints(path, item);
		double dDistOld = dDistMin;
		auto[p1, p2, index1, index2, distance] = findClosestPointWithRotation(path, item, dDistMin);
		if (dDistMin <= 1E-6) {
			idxFind = index;
			dDistMin = distance;
			closest1 = p1;
			closest2 = p2;
			idxEdge1 = index1;
			idxEdge2 = index2;
			break;
		}
		else if (distance < dDistOld) {
			idxFind = index;
			dDistMin = distance;
			closest1 = p1;
			closest2 = p2;
			idxEdge1 = index1;
			idxEdge2 = index2;
		}
	}
	//for (int i = 0; i < source.size();i++) {
	//	Path64 item = source[i];
	//	auto[p1, p2, index1, index2, distance] = findClosestPoints(path, item);
	//	if (distance < minDist) {
	//		idxFind = i;
	//		minDist = distance;
	//		closest1 = p1;
	//		closest2 = p2;
	//		idxEdge1 = index1;
	//		idxEdge2 = index2;
	//	}
	//}
	return { idxFind, closest1, closest2,  idxEdge1,  idxEdge2,  dDistMin };
}

// 将多个多边形连通
Path64 ConnectPolygons(const Paths64& polygons) {
	if (polygons.size() == 0) {
		return Path64();
	}
	Paths64 lstSource;
	std::vector< BoundingBox64> boundings;
	for (const auto& polygon : polygons) {
		lstSource.emplace_back(polygon);
		BoundingBox64 bounding;
		bounding.CreateBoundary(polygon);
		boundings.emplace_back(bounding);
	}

	Path64 pathResult = lstSource[0];
	lstSource.erase(lstSource.begin());
	boundings.erase(boundings.begin());
	while (lstSource.size() > 0) {
		// 查找最近的多边形
		auto[findIndex, p1, p2, index1, index2, findDist] = findClosestPath(pathResult, lstSource, boundings);

		// 插入连通点
		pathResult.insert(pathResult.begin() + index1+1, p1);

		pathResult.insert(pathResult.begin() + index1+2, p2);

		// 合并最近的多边形
		Path64 pathFind = lstSource[findIndex];
		std::rotate(pathFind.begin(), pathFind.begin() + index2+1, pathFind.end());
		pathResult.insert(pathResult.begin() + index1+3, pathFind.begin(), pathFind.end());

		// 插入连通点
		pathResult.insert(pathResult.begin() + index1 + 3 + pathFind.size(), p2);
		pathResult.insert(pathResult.begin() + index1+3 + pathFind.size()+1, p1);
		lstSource.erase(lstSource.begin() + findIndex);

		boundings.erase(boundings.begin() + findIndex);
	}

	return pathResult;
}