/**************************************************************************
*   Copyright (c) Kresimir Tusek         (kresimir.tusek@gmail.com) 2018  *
*   This file is part of the FreeCAD CAx development system.              *
*                                                                         *
*   This library is free software; you can redistribute it and/or         *
*   modify it under the terms of the GNU Library General Public           *
*   License as published by the Free Software Foundation; either          *
*   version 2 of the License, or (at your option) any later version.      *
*                                                                         *
*   This library  is distributed in the hope that it will be useful,      *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
*   GNU Library General Public License for more details.                  *
*                                                                         *
*   You should have received a copy of the GNU Library General Public     *
*   License along with this library; see the file COPYING.LIB. If not,    *
*   write to the Free Software Foundation, Inc., 59 Temple Place,         *
*   Suite 330, Boston, MA  02111-1307, USA                                *
*                                                                         *
***************************************************************************/

#include "Adaptive.hpp"
#include <iostream>
#include <cmath>
#include <cstring>
#include <ctime>
#include <algorithm>

namespace ClipperLib
{
void TranslatePath(const Path &input, Path &output, IntPoint delta);
}

namespace AdaptivePath
{
using namespace ClipperLib;
using namespace std;
#define SAME_POINT_TOL_SQRD_SCALED 4.0
#define UNUSED(expr) (void)(expr)

//*****************************************
// Utils - inline
//*****************************************

inline double DistanceSqrd(const IntPoint &pt1, const IntPoint &pt2)
{
	double Dx = double(pt1.X - pt2.X);
	double dy = double(pt1.Y - pt2.Y);
	return (Dx * Dx + dy * dy);
}

inline bool SetSegmentLength(const IntPoint &pt1, IntPoint &pt2, double new_length)
{
	double Dx = double(pt2.X - pt1.X);
	double dy = double(pt2.Y - pt1.Y);
	double l = sqrt(Dx * Dx + dy * dy);
	if (l > 0.0)
	{
		pt2.X = long(pt1.X + new_length * Dx / l);
		pt2.Y = long(pt1.Y + new_length * dy / l);
		return true;
	}
	return false;
}

inline bool HasAnyPath(const Paths &paths)
{
	for (Paths::size_type i = 0; i < paths.size(); i++)
	{
		if (paths[i].size() > 0)
			return true;
	}
	return false;
}

inline double averageDV(const vector<double> &vec)
{
	double s = 0;
	std::size_t size = vec.size();
	if (size == 0)
		return 0;
	for (std::size_t i = 0; i < size; i++)
		s += vec[i];
	return s / double(size);
}

inline DoublePoint rotate(const DoublePoint &in, double rad)
{
	double c = cos(rad);
	double s = sin(rad);
	return DoublePoint(c * in.X - s * in.Y, s * in.X + c * in.Y);
}

// calculates path length for open path
inline double PathLength(const Path &path)
{
	double len = 0;
	if (path.size() < 2)
		return len;
	for (size_t i = 1; i < path.size(); i++)
	{
		len += sqrt(DistanceSqrd(path[i - 1], path[i]));
	}
	return len;
}

inline double PointSideOfLine(const IntPoint &p1, const IntPoint &p2, const IntPoint &pt)
{
	return double((pt.X - p1.X) * (p2.Y - p1.Y) - (pt.Y - p2.Y) * (p2.X - p1.X));
}

inline double Angle3Points(const DoublePoint &p1, const DoublePoint &p2, const DoublePoint &p3)
{
	double t1 = atan2(p2.Y - p1.Y, p2.X - p1.X);
	double t2 = atan2(p3.Y - p2.Y, p3.X - p2.X);
	double a = fabs(t2 - t1);
	return min(a, 2 * M_PI - a);
}

inline DoublePoint DirectionV(const IntPoint &pt1, const IntPoint &pt2)
{
	double DX = double(pt2.X - pt1.X);
	double DY = double(pt2.Y - pt1.Y);
	double l = sqrt(DX * DX + DY * DY);
	if (l < NTOL)
		return DoublePoint(0, 0);
	return DoublePoint(DX / l, DY / l);
}

inline void NormalizeV(DoublePoint &pt)
{
	double len = sqrt(pt.X * pt.X + pt.Y * pt.Y);
	if (len > NTOL)
	{
		pt.X /= len;
		pt.Y /= len;
	}
}

inline DoublePoint GetPathDirectionV(const Path &pth, size_t pointIndex)
{
	if (pth.size() < 2)
		return DoublePoint(0, 0);
	const IntPoint &p1 = pth.at(pointIndex > 0 ? pointIndex - 1 : pth.size() - 1);
	const IntPoint &p2 = pth.at(pointIndex);
	return DirectionV(p1, p2);
}

//*****************************************
// Utils
//*****************************************

class BoundBox
{
  public:
	BoundBox()
	{
		minX = 0;
		maxX = 0;
		minY = 0;
		maxY = 0;
	}

	// generic: first point
	BoundBox(const IntPoint &p1)
	{
		minX = p1.X;
		maxX = p1.X;
		minY = p1.Y;
		maxY = p1.Y;
	}

	void SetFirstPoint(const IntPoint &p1)
	{
		minX = p1.X;
		maxX = p1.X;
		minY = p1.Y;
		maxY = p1.Y;
	}

	// generic: subsequent points
	void AddPoint(const IntPoint &pt)
	{
		minX = min(pt.X, minX);
		maxX = max(pt.X, maxX);
		minY = min(pt.Y, minY);
		maxY = max(pt.Y, maxY);
	}

	// line segment: two points
	BoundBox(const IntPoint &p1, const IntPoint &p2)
	{
		if (p1.X < p2.X)
		{
			minX = p1.X;
			maxX = p2.X;
		}
		else
		{
			minX = p2.X;
			maxX = p1.X;
		}
		if (p1.Y < p2.Y)
		{
			minY = p1.Y;
			maxY = p2.Y;
		}
		else
		{
			minY = p2.Y;
			maxY = p1.Y;
		}
	}

	// for circle: center and radius
	BoundBox(const IntPoint &center, long radius)
	{
		minX = center.X - radius;
		maxX = center.X + radius;
		minY = center.Y - radius;
		maxY = center.Y + radius;
	}

	// bounds check - intersection
	inline bool CollidesWith(const BoundBox &bb2)
	{
		return minX <= bb2.maxX && maxX >= bb2.minX && minY <= bb2.maxY && maxY >= bb2.minY;
	}

	// bounds check -  contains
	inline bool Contains(const BoundBox &bb2)
	{
		return minX <= bb2.minX && maxX >= bb2.maxX && minY <= bb2.minY && maxY >= bb2.maxY;
	}

	ClipperLib::cInt minX;
	ClipperLib::cInt maxX;
	ClipperLib::cInt minY;
	ClipperLib::cInt maxY;
};

std::ostream &operator<<(std::ostream &s, const BoundBox &p)
{
	s << "(" << p.minX << "," << p.minY << ") - (" << p.maxX << "," << p.maxY << ")";
	return s;
}

int getPathNestingLevel(const Path &path, const Paths &paths)
{
	int nesting = 0;
	for (const auto &other : paths)
	{
		if (path.size() > 0 && PointInPolygon(path.front(), other) != 0)
			nesting++;
	}
	return nesting;
}

void appendDirectChildPaths(Paths &outPaths, const Path &path, const Paths &paths)
{
	int nesting = getPathNestingLevel(path, paths);
	for (const auto &other : paths)
	{
		if (path.size() > 0 && other.size() > 0 && PointInPolygon(other.front(), path) != 0)
		{
			if (getPathNestingLevel(other, paths) == nesting + 1)
				outPaths.push_back(other);
		}
	}
}

void AverageDirection(const vector<DoublePoint> &unityVectors, DoublePoint &output)
{
	std::size_t size = unityVectors.size();
	output.X = 0;
	output.Y = 0;
	// sum vectors
	for (std::size_t i = 0; i < size; i++)
	{
		DoublePoint v = unityVectors[i];
		output.X += v.X;
		output.Y += v.Y;
	}
	// normalize
	double magnitude = sqrt(output.X * output.X + output.Y * output.Y);
	output.X /= magnitude;
	output.Y /= magnitude;
}

double DistancePointToLineSegSquared(const IntPoint &p1, const IntPoint &p2, const IntPoint &pt,
									 IntPoint &closestPoint, double &ptParameter, bool clamp = true)
{
	double D21X = double(p2.X - p1.X);
	double D21Y = double(p2.Y - p1.Y);
	double DP1X = double(pt.X - p1.X);
	double DP1Y = double(pt.Y - p1.Y);
	double lsegLenSqr = D21X * D21X + D21Y * D21Y;
	if (lsegLenSqr == 0)
	{ // segment is zero length, return point to point distance
		closestPoint = p1;
		ptParameter = 0;
		return DP1X * DP1X + DP1Y * DP1Y;
	}
	double parameter = DP1X * D21X + DP1Y * D21Y;
	if (clamp)
	{
		// clamp the parameter
		if (parameter < 0)
			parameter = 0;
		else if (parameter > lsegLenSqr)
			parameter = lsegLenSqr;
	}
	// point on line at parameter
	ptParameter = parameter / lsegLenSqr;
	closestPoint.X = long(p1.X + ptParameter * D21X);
	closestPoint.Y = long(p1.Y + ptParameter * D21Y);
	// calculate distance from point on line to pt
	double DX = double(pt.X - closestPoint.X);
	double DY = double(pt.Y - closestPoint.Y);
	return DX * DX + DY * DY; // return distance squared
}

void ScaleUpPaths(Paths &paths,long scaleFactor) {
	for(auto &pth:paths) {
		for(auto &pt:pth) {
			pt.X*=scaleFactor;
			pt.Y*=scaleFactor;
		}
	}
}

void ScaleDownPaths(Paths &paths,long scaleFactor) {
	for(auto &pth:paths) {
		for(auto &pt:pth) {
			pt.X/=scaleFactor;
			pt.Y/=scaleFactor;
		}
	}
}




double DistancePointToPathsSqrd(const Paths &paths, const IntPoint &pt, IntPoint &closestPointOnPath,
								size_t &clpPathIndex,
								size_t &clpSegmentIndex,
								double &clpParameter)
{
	double minDistSq = __DBL_MAX__;
	IntPoint clp;
	// iterate though paths
	for (Path::size_type i = 0; i < paths.size(); i++)
	{
		const Path *path = &paths[i];
		Path::size_type size = path->size();
		// iterate through segments
		for (Path::size_type j = 0; j < size; j++)
		{
			double ptPar;
			double distSq = DistancePointToLineSegSquared(path->at(j > 0 ? j - 1 : size - 1), path->at(j), pt, clp, ptPar);
			if (distSq < minDistSq)
			{
				clpPathIndex = i;
				clpSegmentIndex = j;
				clpParameter = ptPar;
				closestPointOnPath = clp;
				minDistSq = distSq;
			}
		}
	}
	return minDistSq;
}

// joins collinear segments (within the tolerance)
void CleanPath(const Path &inp, Path &outpt, double tolerance)
{
	if (inp.size() < 3)
	{
		outpt = inp;
		return;
	}
	outpt.clear();
	Path tmp;
	CleanPolygon(inp, tmp, tolerance);
	long size=long(tmp.size());

	// CleanPolygon will have empty result if all points are collinear,
	// 	need to add first and last point to the output
	if(size<=2) {
		outpt.push_back(inp.front());
		outpt.push_back(inp.back());
		return;
	}

	// restore starting point
	double clpPar =  0;
	size_t clpSegmentIndex=0;
	size_t clpPathIndex=0;
	Paths tmpPaths;
	tmpPaths.push_back(tmp);
	IntPoint clp;
	// find point on cleaned poly that is closest to original starting point
	DistancePointToPathsSqrd(tmpPaths,inp.front(),clp,clpPathIndex,clpSegmentIndex,clpPar);


	// if closes point is not one of the polygon points, add it as separate first point
	if(DistanceSqrd(clp,tmp.at(clpSegmentIndex)) > 0 &&
		DistanceSqrd(clp,tmp.at(clpSegmentIndex>0 ? clpSegmentIndex-1 : size-1)) > 0) outpt.push_back(clp);

	// add remaining points starting from closest
	long index;
	for (long i = 0; i < size; i++)
	{
		index = static_cast<long>(clpSegmentIndex + i);
		if (index >= size) index -= size;
		outpt.push_back(tmp.at(index));
	}


	if(DistanceSqrd(outpt.front(),inp.front()) > SAME_POINT_TOL_SQRD_SCALED)
		outpt.insert(outpt.begin(), inp.front());

	if(DistanceSqrd(outpt.back(),inp.back()) > SAME_POINT_TOL_SQRD_SCALED)
		outpt.push_back( inp.back());

}

bool Circle2CircleIntersect(const IntPoint &c1, const IntPoint &c2, double radius, pair<DoublePoint, DoublePoint> &intersections)
{
	double DX = double(c2.X - c1.X);
	double DY = double(c2.Y - c1.Y);
	double d = sqrt(DX * DX + DY * DY);
	if (d < NTOL)
		return false; // same center
	if (d >= radius)
		return false; // do not intersect, or intersect in one point (this case not relevant here)
	double a_2 = sqrt(4 * radius * radius - d * d) / 2.0;
	intersections.first = DoublePoint(0.5 * (c1.X + c2.X) - DY * a_2 / d, 0.5 * (c1.Y + c2.Y) + DX * a_2 / d);
	intersections.second = DoublePoint(0.5 * (c1.X + c2.X) + DY * a_2 / d, 0.5 * (c1.Y + c2.Y) - DX * a_2 / d);
	return true;
}

bool Line2CircleIntersect(const IntPoint &c, double radius, const IntPoint &p1, const IntPoint &p2, vector<DoublePoint> &result, bool clamp = true)
{
	// if more intersections returned, first is closer to p1

	//box  check for performance
	if (clamp)
	{
		BoundBox cBB(c, (ClipperLib::cInt)radius + 1); // circle bound box
		BoundBox sBB(p1, p2);
		if (!sBB.CollidesWith(cBB))
			return false;
	}

	double dx = double(p2.X - p1.X);
	double dy = double(p2.Y - p1.Y);
	double lcx = double(p1.X - c.X);
	double lcy = double(p1.Y - c.Y);
	double a = dx * dx + dy * dy;
	double b = 2 * dx * lcx + 2 * dy * lcy;
	double C = lcx * lcx + lcy * lcy - radius * radius;
	double sq = b * b - 4 * a * C;
	if (sq < 0)
		return false; // no solution
	sq = sqrt(sq);
	double t1 = (-b - sq) / (2 * a);
	double t2 = (-b + sq) / (2 * a);
	result.clear();
	if (clamp)
	{
		if (t1 >= 0.0 && t1 <= 1.0)
			result.push_back(DoublePoint(p1.X + t1 * dx, p1.Y + t1 * dy));
		if (t2 >= 0.0 && t2 <= 1.0)
			result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy));
	}
	else
	{
		result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy));
		result.push_back(DoublePoint(p1.X + t2 * dx, p1.Y + t2 * dy));
	}
	return result.size() > 0;
}

// calculate center point of polygon
IntPoint Compute2DPolygonCentroid(const Path &vertices)
{
	DoublePoint centroid(0, 0);
	double signedArea = 0.0;
	double x0 = 0.0; // Current vertex X
	double y0 = 0.0; // Current vertex Y
	double x1 = 0.0; // Next vertex X
	double y1 = 0.0; // Next vertex Y
	double a = 0.0;  // Partial signed area

	// For all vertices
	size_t i = 0;
	Path::size_type size = vertices.size();
	for (i = 0; i < size; ++i)
	{
		x0 = double(vertices[i].X);
		y0 = double(vertices[i].Y);
		x1 = double(vertices[(i + 1) % size].X);
		y1 = double(vertices[(i + 1) % size].Y);
		a = x0 * y1 - x1 * y0;
		signedArea += a;
		centroid.X += (x0 + x1) * a;
		centroid.Y += (y0 + y1) * a;
	}

	signedArea *= 0.5;
	centroid.X /= (6.0 * signedArea);
	centroid.Y /= (6.0 * signedArea);
	return IntPoint(long(centroid.X), long(centroid.Y));
}

// point must be within first path (boundary) and must not be within all other paths (holes)
bool IsPointWithinCutRegion(const Paths &toolBoundPaths, const IntPoint &point)
{
	for (size_t i = 0; i < toolBoundPaths.size(); i++)
	{
		int pip = PointInPolygon(point, toolBoundPaths[i]);
		if (i == 0 && pip == 0)
			return false; // is outside or on boundary
		if (i > 0 && pip != 0)
			return false; // is inside hole
	}
	return true;
}

/* finds intersection of line segment with line segment */
bool IntersectionPoint(const IntPoint &s1p1,
					   const IntPoint &s1p2,
					   const IntPoint &s2p1,
					   const IntPoint &s2p2,
					   IntPoint &intersection)
{
	double S1DX = double(s1p2.X - s1p1.X);
	double S1DY = double(s1p2.Y - s1p1.Y);
	double S2DX = double(s2p2.X - s2p1.X);
	double S2DY = double(s2p2.Y - s2p1.Y);
	double d = S1DY * S2DX - S2DY * S1DX;
	if (fabs(d) < NTOL)
		return false; // lines are parallel

	double LPDX = double(s1p1.X - s2p1.X);
	double LPDY = double(s1p1.Y - s2p1.Y);
	double p1d = S2DY * LPDX - S2DX * LPDY;
	double p2d = S1DY * LPDX - S1DX * LPDY;
	if ((d < 0) && (p1d < d || p1d > 0 || p2d < d || p2d > 0))
		return false; // intersection not within segment1
	if ((d > 0) && (p1d < 0 || p1d > d || p2d < 0 || p2d > d))
		return false; // intersection not within segment2
	double t = p1d / d;
	intersection = IntPoint(long(s1p1.X + S1DX * t), long(s1p1.Y + S1DY * t));
	return true;
}

/* finds one/first intersection of line segment with paths */
bool IntersectionPoint(const Paths &paths, const IntPoint &p1, const IntPoint &p2, IntPoint &intersection)
{
	BoundBox segBB(p1, p2);
	for (size_t i = 0; i < paths.size(); i++)
	{
		const Path *path = &paths[i];
		size_t size = path->size();
		if (size < 2)
			continue;
		BoundBox pathBB(path->front());
		for (size_t j = 0; j < size; j++)
		{

			const IntPoint *pp2 = &path->at(j);

			// box check for performance
			pathBB.AddPoint(*pp2);
			if (!pathBB.CollidesWith(segBB))
				continue;

			const IntPoint *pp1 = &path->at(j > 0 ? j - 1 : size - 1);
			double LDY = double(p2.Y - p1.Y);
			double LDX = double(p2.X - p1.X);
			double PDX = double(pp2->X - pp1->X);
			double PDY = double(pp2->Y - pp1->Y);
			double d = LDY * PDX - PDY * LDX;
			if (fabs(d) < NTOL)
				continue; // lines are parallel

			double LPDX = double(p1.X - pp1->X);
			double LPDY = double(p1.Y - pp1->Y);
			double p1d = PDY * LPDX - PDX * LPDY;
			double p2d = LDY * LPDX - LDX * LPDY;
			if ((d < 0) && (p1d < d || p1d > 0 || p2d < d || p2d > 0))
				continue; // intersection not within segment
			if ((d > 0) && (p1d < 0 || p1d > d || p2d < 0 || p2d > d))
				continue; // intersection not within segment
			double t = p1d / d;
			intersection = IntPoint(long(p1.X + LDX * t), long(p1.Y + LDY * t));
			return true;
		}
	}
	return false;
}

void SmoothPaths(Paths &paths, double stepSize, long pointCount, long iterations)
{
	Paths output;
	output.resize(paths.size());
	const long scale=1000;
	const double stepScaled = stepSize*scale;

	ScaleUpPaths(paths,scale);
	vector<pair<size_t /*path index*/, IntPoint>> points;
	for (size_t i = 0; i < paths.size(); i++)
	{
		for (const auto &pt : paths[i])
		{
			if (points.empty())
			{
				points.push_back(pair<size_t /*path index*/, IntPoint>(i, pt));
				continue;
			}
			const auto &back=points.back();
			const IntPoint & lastPt = back.second;


			const double l = sqrt(DistanceSqrd(lastPt, pt));

			if (l < 0.5*stepScaled )
			{
				if(points.size()>1) points.pop_back();
				points.push_back(pair<size_t /*path index*/, IntPoint>(i, pt));
				continue;
			}
			size_t lastPathIndex = back.first;
			const long steps = max(long(l / stepScaled),1L);
			const long left=pointCount*iterations*2;
			const long right =steps-pointCount*iterations*2;
			for (long idx = 0; idx <= steps; idx++)
			{
				if(idx>left && idx<right) {
					idx=right;
					continue;
				}
				const double p = double(idx) / steps;
				const IntPoint ptx(long(lastPt.X + double(pt.X - lastPt.X) * p),
							 long(lastPt.Y + double(pt.Y - lastPt.Y) * p));

				if(idx==0 && DistanceSqrd(back.second,ptx)<scale && points.size()>1) points.pop_back();

				if (p < 0.5)
					points.push_back(pair<size_t /*path index*/, IntPoint>(lastPathIndex, ptx));
				else
					points.push_back(pair<size_t /*path index*/, IntPoint>(i, ptx));
			}
		}
	}
	if (points.empty())
		return;
	const long size=long(points.size());
	for(long iter=0;iter<iterations; iter++) {
		for (long i = 1; i < size-1; i++)
		{
			IntPoint &cp = points[i].second;
			IntPoint avgPoint(cp);
			long cnt = 1;

			long ptsToAverage = pointCount;
			if (i <= ptsToAverage)
			 	ptsToAverage = max(i-1,0L);
			else if(i + ptsToAverage >= size-1)
			 	ptsToAverage = size-1-i;
			for (long j = i-ptsToAverage; j <= i+ptsToAverage; j++)
			{
				if (j == i) continue;
				long index=j;
				if(index<0)	index=0;
				if(index>=size) index=size-1;
				IntPoint &p = points[index].second;
				avgPoint.X += p.X ;
				avgPoint.Y += p.Y ;
				cnt++;
			}
			cp.X=avgPoint.X/cnt;
			cp.Y=avgPoint.Y/cnt;
		}
	}

	for (const auto &pr:points)
	{
		output[pr.first].push_back(pr.second);
	}
	for (size_t i = 0; i < paths.size(); i++)
	{
		CleanPath(output[i], paths[i], 1.4*scale);
	}
	ScaleDownPaths(paths,scale);
}

bool PopPathWithClosestPoint(Paths &paths /*closest path is removed from collection and shifted to start with closest point */
							 ,
							 IntPoint p1, Path &result)
{

	if (paths.size() == 0)
		return false;

	double minDistSqrd = __DBL_MAX__;
	size_t closestPathIndex = 0;
	long closestPointIndex = 0;
	for (size_t pathIndex = 0; pathIndex < paths.size(); pathIndex++)
	{
		Path &path = paths.at(pathIndex);
		for (size_t i = 0; i < path.size(); i++)
		{
			double dist = DistanceSqrd(p1, path.at(i));
			if (dist < minDistSqrd)
			{
				minDistSqrd = dist;
				closestPathIndex = pathIndex;
				closestPointIndex = long(i);
			}
		}
	}

	result.clear();
	// make new path starting with that point
	Path &closestPath = paths.at(closestPathIndex);
	for (size_t i = 0; i < closestPath.size(); i++)
	{
		long index = closestPointIndex + long(i);
		if (index >= long(closestPath.size()))
			index -= long(closestPath.size());
		result.push_back(closestPath.at(index));
	}
	// remove the closest path
	paths.erase(paths.begin() + closestPathIndex);
	return true;
}

void DeduplicatePaths(const Paths &inputs, Paths &outputs)
{
	outputs.clear();
	for (const auto &new_pth : inputs)
	{
		bool duplicate = false;
		// if all points of new path exist on some of the old paths, path is considered duplicate
		for (const auto &old_pth : outputs)
		{
			bool all_points_exists = true;
			for (const auto pt1 : new_pth)
			{
				bool pointExists = false;
				for (const auto pt2 : old_pth)
				{
					if (DistanceSqrd(pt1, pt2) < SAME_POINT_TOL_SQRD_SCALED)
					{
						pointExists = true;
						break;
					}
				}
				if (!pointExists)
				{
					all_points_exists = false;
					break;
				}
			}
			if (all_points_exists)
			{
				duplicate = true;
				break;
			}
		}

		if (!duplicate && new_pth.size() > 0)
		{
			outputs.push_back(new_pth);
		}
	}
}

void ConnectPaths(Paths input, Paths &output)
{
	output.clear();
	bool newPath = true;
	Path joined;
	while (input.size() > 0)
	{
		if (newPath)
		{
			if (joined.size() > 0)
				output.push_back(joined);
			joined.clear();
			for (auto pt : input.front())
			{
				joined.push_back(pt);
			}
			input.erase(input.begin());
			newPath = false;
		}
		bool anyMatch = false;
		for (size_t i = 0; i < input.size(); i++)
		{
			Path &n = input.at(i);
			if (DistanceSqrd(n.front(), joined.back()) < SAME_POINT_TOL_SQRD_SCALED)
			{
				for (auto pt : n)
					joined.push_back(pt);
				input.erase(input.begin() + i);
				anyMatch = true;
				break;
			}
			else if (DistanceSqrd(n.back(), joined.back()) < SAME_POINT_TOL_SQRD_SCALED)
			{
				ReversePath(n);
				for (auto pt : n)
					joined.push_back(pt);
				input.erase(input.begin() + i);
				anyMatch = true;
				break;
			}
			else if (DistanceSqrd(n.front(), joined.front()) < SAME_POINT_TOL_SQRD_SCALED)
			{
				for (auto pt : n)
					joined.insert(joined.begin(), pt);
				input.erase(input.begin() + i);
				anyMatch = true;
				break;
			}
			else if (DistanceSqrd(n.back(), joined.front()) < SAME_POINT_TOL_SQRD_SCALED)
			{
				ReversePath(n);
				for (auto pt : n)
					joined.insert(joined.begin(), pt);
				input.erase(input.begin() + i);
				anyMatch = true;
				break;
			}
		}
		if (!anyMatch)
			newPath = true;
	}
	if (joined.size() > 0)
		output.push_back(joined);
}

// helper class for measuring performance
class PerfCounter
{
  public:
	PerfCounter(string p_name)
	{
		name = p_name;
		count = 0;
		running = false;
		start_ticks = 0;
		total_ticks = 0;
	}
	inline void Start()
	{
#ifdef DEV_MODE
		start_ticks = clock();
		if (running)
		{
			cerr << "PerfCounter already running:" << name << endl;
		}
		running = true;
#endif
	}
	inline void Stop()
	{
#ifdef DEV_MODE
		if (!running)
		{
			cerr << "PerfCounter not running:" << name << endl;
		}
		total_ticks += clock() - start_ticks;
		start_ticks = clock();
		count++;
		running = false;
#endif
	}
	void DumpResults()
	{
		double total_time = double(total_ticks) / CLOCKS_PER_SEC;
		cout << "Perf: " << name.c_str() << " total_time: " << total_time << " sec, call_count:" << count << " per_call:" << double(total_time / count) << endl;
		start_ticks = clock();
		total_ticks = 0;
		count = 0;
	}

  private:
	string name;
	clock_t start_ticks;
	clock_t total_ticks;
	size_t count;
	bool running = false;
};

PerfCounter Perf_ProcessPolyNode("ProcessPolyNode");
PerfCounter Perf_CalcCutAreaCirc("CalcCutArea");
PerfCounter Perf_CalcCutAreaClip("CalcCutAreaClip");
PerfCounter Perf_NextEngagePoint("NextEngagePoint");
PerfCounter Perf_PointIterations("PointIterations");
PerfCounter Perf_ExpandCleared("ExpandCleared");
PerfCounter Perf_DistanceToBoundary("DistanceToBoundary");
PerfCounter Perf_AppendToolPath("AppendToolPath");
PerfCounter Perf_IsAllowedToCutTrough("IsAllowedToCutTrough");
PerfCounter Perf_IsClearPath("IsClearPath");

//***********************************
// Cleared area bounding support
//***********************************
class ClearedArea
{
  public:
	ClearedArea(ClipperLib::cInt p_toolRadiusScaled)
	{
		toolRadiusScaled = p_toolRadiusScaled;
	};

	void SetClearedPaths(const Paths &paths)
	{
		clearedPaths = paths;
		bboxPathsInvalid = true;
		bboxClippedInvalid = true;
	}
	void ExpandCleared(const Path toClearToolPath)
	{
		if (toClearToolPath.empty())
			return;
		Perf_ExpandCleared.Start();
		clipof.Clear();
		clipof.AddPath(toClearToolPath, JoinType::jtRound, EndType::etOpenRound);
		Paths toolCoverPoly;
		clipof.Execute(toolCoverPoly, toolRadiusScaled + 1);
		clip.Clear();
		clip.AddPaths(clearedPaths, PolyType::ptSubject, true);
		clip.AddPaths(toolCoverPoly, PolyType::ptClip, true);
		clip.Execute(ClipType::ctUnion, clearedPaths);
		CleanPolygons(clearedPaths);
		bboxPathsInvalid = true;
		bboxClippedInvalid = true;
		Perf_ExpandCleared.Stop();
	}

	// gets the path sections inside the ext. tool bounding box
	Paths &GetBoundedClearedPaths(const IntPoint &toolPos)
	{
		BoundBox toolBB(toolPos, toolRadiusScaled);
		if (!bboxPathsInvalid && clearedBBPathsInFocus.Contains(toolBB))
		{
			return clearedBoundedPaths;
		}
		ClipperLib::cInt delta = focusBBFactor1 * toolRadiusScaled;
		clearedBBPathsInFocus.SetFirstPoint(IntPoint(toolPos.X - delta, toolPos.Y - delta));
		clearedBBPathsInFocus.AddPoint(IntPoint(toolPos.X + delta, toolPos.Y + delta));

		BoundBox bb(toolPos, focusBBFactor2 * toolRadiusScaled);
		clearedBoundedPaths.clear();
		for (const auto &pth : clearedPaths)
		{
			if (pth.size() < 2)
				continue;
			Path bPath;
			size_t size = pth.size();
			for (size_t i = 0; i < size + 1; i++)
			{
				IntPoint last = (i > 0 ? pth[i - 1] : pth.back());
				IntPoint next = i < size ? pth[i] : pth.front();
				BoundBox ptbox(last, next);
				if (ptbox.CollidesWith(bb))
				{
					if (bPath.empty() || bPath.back() != last)
						bPath.push_back(last);
					bPath.push_back(next);
				}
				else
				{
					if (!bPath.empty())
					{
						clearedBoundedPaths.push_back(bPath);
						bPath.clear();
					}
				}
			}
			if (!bPath.empty())
			{
				clearedBoundedPaths.push_back(bPath);
				bPath.clear();
			}
		}
		bboxPathsInvalid = false;
		return clearedBoundedPaths;
	}

	// get cleared area/poly bounded to toolbox
	Paths &GetBoundedClearedAreaClipped(const IntPoint &toolPos)
	{
		BoundBox toolBB(toolPos, toolRadiusScaled);
		if (!bboxClippedInvalid && clearedBBClippedInFocus.Contains(toolBB))
		{
			return clearedBoundedClipped;
		}
		ClipperLib::cInt delta = focusBBFactor1 * toolRadiusScaled;
		clearedBBClippedInFocus.SetFirstPoint(IntPoint(toolPos.X - delta, toolPos.Y - delta));
		clearedBBClippedInFocus.AddPoint(IntPoint(toolPos.X + delta, toolPos.Y + delta));

		// a little larger area is bounded than checked
		ClipperLib::cInt delta2 = focusBBFactor2 * toolRadiusScaled;
		Path bbPath;
		bbPath.push_back(IntPoint(toolPos.X - delta2, toolPos.Y - delta2));
		bbPath.push_back(IntPoint(toolPos.X + delta2, toolPos.Y - delta2));
		bbPath.push_back(IntPoint(toolPos.X + delta2, toolPos.Y + delta2));
		bbPath.push_back(IntPoint(toolPos.X - delta2, toolPos.Y + delta2));
		clip.Clear();
		clip.AddPath(bbPath, PolyType::ptSubject, true);
		clip.AddPaths(clearedPaths, PolyType::ptClip, true);
		clip.Execute(ClipType::ctIntersection, clearedBoundedClipped);
		bboxClippedInvalid = false;
		return clearedBoundedClipped;
	}

	// get full cleared area
	Paths &GetCleared()
	{
		return clearedPaths;
	}

  private:
	Clipper clip;
	ClipperOffset clipof;
	Paths clearedPaths;
	Paths clearedBoundedClipped;
	Paths clearedBoundedPaths;

	ClipperLib::cInt toolRadiusScaled;
	BoundBox clearedBBClippedInFocus;
	BoundBox clearedBBPathsInFocus;

	bool bboxClippedInvalid = false;
	bool bboxPathsInvalid = false;
	// size of the focus BB
	const ClipperLib::cInt focusBBFactor1 = 8;
	const ClipperLib::cInt focusBBFactor2 = 9;
};

//***************************************
// Linear Interpolation - area vs angle
//***************************************
class Interpolation
{
  public:
	const double MIN_ANGLE = -M_PI / 4;
	const double MAX_ANGLE = M_PI / 4;

	void clear()
	{
		angles.clear();
		areas.clear();
	}
	// adds point keeping the incremental order of areas for interpolation to work correctly
	void addPoint(double area, double angle)
	{
		std::size_t size = areas.size();
		if (size == 0 || area > areas[size - 1] + NTOL)
		{ // first point or largest area point
			areas.push_back(area);
			angles.push_back(angle);
			return;
		}

		for (std::size_t i = 0; i < size; i++)
		{
			if (area < areas[i] - NTOL && (i == 0 || area > areas[i - 1] + NTOL))
			{
				areas.insert(areas.begin() + i, area);
				angles.insert(angles.begin() + i, angle);
			}
		}
	}

	double interpolateAngle(double targetArea)
	{
		std::size_t size = areas.size();
		if (size < 2 || targetArea > areas[size - 1])
			return MIN_ANGLE; //max engage angle - convenient value to initially measure cut area
		if (targetArea < areas[0])
			return MAX_ANGLE; // min engage angle

		for (size_t i = 1; i < size; i++)
		{
			// find 2 subsequent points where target area is between
			if (areas[i - 1] <= targetArea && areas[i] > targetArea)
			{
				// linear interpolation
				double af = (targetArea - areas[i - 1]) / (areas[i] - areas[i - 1]);
				double a = angles[i - 1] + af * (angles[i] - angles[i - 1]);
				return a;
			}
		}
		return MIN_ANGLE;
	}

	double clampAngle(double angle)
	{
		if (angle < MIN_ANGLE)
			return MIN_ANGLE;
		if (angle > MAX_ANGLE)
			return MAX_ANGLE;
		return angle;
	}

	double getRandomAngle()
	{
		return MIN_ANGLE + (MAX_ANGLE - MIN_ANGLE) * double(rand()) / double(RAND_MAX);
	}
	size_t getPointCount()
	{
		return areas.size();
	}

  private:
	vector<double> angles;
	vector<double> areas;
};

//***************************************
// Engage Point
//***************************************

class EngagePoint
{
  public:
	struct EngageState
	{
		size_t currentPathIndex;
		size_t currentSegmentIndex;
		double segmentPos = 0;
		double totalDistance = 0;
		double currentPathLength = 0;
		int passes = 0;

		double metric; // engage point metric

		bool operator<(const EngageState &other) const
		{
			return (metric < other.metric);
		}
	};
	EngagePoint(const Paths &p_toolBoundPaths)
	{
		SetPaths(p_toolBoundPaths);

		state.currentPathIndex = 0;
		state.currentSegmentIndex = 0;
		state.segmentPos = 0;
		state.totalDistance = 0;
		calculateCurrentPathLength();
	}

	void SetPaths(const Paths &paths)
	{
		toolBoundPaths = paths;
		state.currentPathIndex = 0;
		state.currentSegmentIndex = 0;
		state.segmentPos = 0;
		state.totalDistance = 0;
		state.passes = 0;
		calculateCurrentPathLength();
	}

	EngageState GetState()
	{
		return state;
	}

	void SetState(const EngageState &new_state)
	{
		state = new_state;
	}

	void ResetPasses()
	{
		state.passes = 0;
	}
	void moveToClosestPoint(const IntPoint &pt, double step)
	{

		Path result;
		IntPoint current = pt;
		// chain paths according to distance in between
		Paths toChain = toolBoundPaths;
		toolBoundPaths.clear();
		// if(toChain.size()>0) {
		// 	toolBoundPaths.push_back(toChain.front());
		// 	toChain.erase(toChain.begin());
		// }
		while (PopPathWithClosestPoint(toChain, current, result))
		{
			toolBoundPaths.push_back(result);
			if (result.size() > 0)
				current = result.back();
		}

		double minDistSq = __DBL_MAX__;
		size_t minPathIndex = state.currentPathIndex;
		size_t minSegmentIndex = state.currentSegmentIndex;
		double minSegmentPos = state.segmentPos;
		state.totalDistance = 0;
		for (;;)
		{
			while (moveForward(step))
			{
				double distSqrd = DistanceSqrd(pt, getCurrentPoint());
				if (distSqrd < minDistSq)
				{
					minDistSq = distSqrd;
					minPathIndex = state.currentPathIndex;
					minSegmentIndex = state.currentSegmentIndex;
					minSegmentPos = state.segmentPos;
				}
			}
			if (!nextPath())
				break;
		}
		state.currentPathIndex = minPathIndex;
		state.currentSegmentIndex = minSegmentIndex;
		state.segmentPos = minSegmentPos;
		calculateCurrentPathLength();
		ResetPasses();
	}
	bool nextEngagePoint(Adaptive2d *parent, ClearedArea &clearedArea, double step, double minCutArea, double maxCutArea, int maxPases = 2)
	{
		Perf_NextEngagePoint.Start();
		double prevArea = 0; // we want to make sure that we catch the point where the area is on raising slope
		IntPoint initialPoint(-1000000000, -1000000000);
		for (;;)
		{
			if (!moveForward(step))
			{
				if (!nextPath())
				{
					state.passes++;
					if (state.passes >= maxPases)
					{
						Perf_NextEngagePoint.Stop();
						return false; // nothing more to cut
					}
					prevArea = 0;
				}
			}
			IntPoint cpt = getCurrentPoint();
			double area = parent->CalcCutArea(clip, initialPoint, cpt, clearedArea);
			if (area > minCutArea && area < maxCutArea && area > prevArea)
			{
				Perf_NextEngagePoint.Stop();
				return true;
			}
			prevArea = area;
		}
	}
	IntPoint getCurrentPoint()
	{
		const Path *pth = &toolBoundPaths.at(state.currentPathIndex);
		const IntPoint *p1 = &pth->at(state.currentSegmentIndex > 0 ? state.currentSegmentIndex - 1 : pth->size() - 1);
		const IntPoint *p2 = &pth->at(state.currentSegmentIndex);
		double segLength = sqrt(DistanceSqrd(*p1, *p2));
		return IntPoint(long(p1->X + state.segmentPos * double(p2->X - p1->X) / segLength), long(p1->Y + state.segmentPos * double(p2->Y - p1->Y) / segLength));
	}

	DoublePoint getCurrentDir()
	{
		const Path *pth = &toolBoundPaths.at(state.currentPathIndex);
		const IntPoint *p1 = &pth->at(state.currentSegmentIndex > 0 ? state.currentSegmentIndex - 1 : pth->size() - 1);
		const IntPoint *p2 = &pth->at(state.currentSegmentIndex);
		double segLength = sqrt(DistanceSqrd(*p1, *p2));
		return DoublePoint(double(p2->X - p1->X) / segLength, double(p2->Y - p1->Y) / segLength);
	}

	bool moveForward(double distance)
	{
		const Path *pth = &toolBoundPaths.at(state.currentPathIndex);
		if (distance < NTOL)
			throw std::invalid_argument("distance must be positive");
		state.totalDistance += distance;
		double segmentLength = currentSegmentLength();
		while (state.segmentPos + distance > segmentLength)
		{
			state.currentSegmentIndex++;
			if (state.currentSegmentIndex >= pth->size())
			{
				state.currentSegmentIndex = 0;
			}
			distance = distance - (segmentLength - state.segmentPos);
			state.segmentPos = 0;
			segmentLength = currentSegmentLength();
		}
		state.segmentPos += distance;
		return state.totalDistance <= 1.2 * state.currentPathLength;
	}

	bool nextPath()
	{
		state.currentPathIndex++;
		state.currentSegmentIndex = 0;
		state.segmentPos = 0;
		state.totalDistance = 0;
		if (state.currentPathIndex >= toolBoundPaths.size())
		{
			state.currentPathIndex = 0;
			calculateCurrentPathLength();
			return false;
		}
		calculateCurrentPathLength();
		return true;
	}

  private:
	Paths toolBoundPaths;
	EngageState state;
	Clipper clip;
	void calculateCurrentPathLength()
	{
		const Path *pth = &toolBoundPaths.at(state.currentPathIndex);
		size_t size = pth->size();
		state.currentPathLength = 0;
		for (size_t i = 0; i < size; i++)
		{
			const IntPoint *p1 = &pth->at(i > 0 ? i - 1 : size - 1);
			const IntPoint *p2 = &pth->at(i);
			state.currentPathLength += sqrt(DistanceSqrd(*p1, *p2));
		}
	}

	double currentSegmentLength()
	{
		const Path *pth = &toolBoundPaths.at(state.currentPathIndex);
		const IntPoint *p1 = &pth->at(state.currentSegmentIndex > 0 ? state.currentSegmentIndex - 1 : pth->size() - 1);
		const IntPoint *p2 = &pth->at(state.currentSegmentIndex);
		return sqrt(DistanceSqrd(*p1, *p2));
	}
};

//***************************************
// Adaptive2d main class - implementation
//***************************************

Adaptive2d::Adaptive2d()
{
}

double Adaptive2d::CalcCutArea(Clipper &clip, const IntPoint &c1, const IntPoint &c2, ClearedArea &clearedArea, bool preventConventional)
{

	double dist = DistanceSqrd(c1, c2);
	if (dist < NTOL)
		return 0;

	Perf_CalcCutAreaCirc.Start();

	/// new alg
	double rsqrd = toolRadiusScaled * toolRadiusScaled;
	double area = 0;
	Paths interPaths;
	IntPoint clp;				// to hold closest point
	vector<DoublePoint> inters; // to hold intersection results
	BoundBox c2BB(c2, toolRadiusScaled);
	BoundBox c1BB(c1, toolRadiusScaled);
	Paths &clearedBounded = clearedArea.GetBoundedClearedAreaClipped(c2);
	for (const Path &path : clearedBounded)
	{
		size_t size = path.size();
		if (size == 0)
			continue;

		//** bound box check
		// construct bound box for path
		BoundBox pathBB(path.front());
		for (const auto &pt : path)
			pathBB.AddPoint(pt);
		if (!c2BB.CollidesWith(c2))
			continue; // this path cannot colide with tool
		//** end of BB check

		size_t curPtIndex = 0;
		bool found = false;
		// step 1: we find the starting point on the cleared path that is outside new tool shape (c2)
		for (size_t i = 0; i < size; i++)
		{
			if (DistanceSqrd(path[curPtIndex], c2) > rsqrd)
			{
				found = true;
				break;
			}
			curPtIndex++;
			if (curPtIndex >= size)
				curPtIndex = 0;
		}
		if (!found)
			continue; // try another path

		// step 2: iterate through path from starting point and find the part of the path inside the c2
		size_t prevPtIndex = curPtIndex;
		Path *interPath = NULL;
		bool prev_inside = false;
		const IntPoint *p1 = &path[prevPtIndex];
		double par; // to hold parameter output
		for (size_t i = 0; i < size; i++)
		{
			curPtIndex++;
			if (curPtIndex >= size)
				curPtIndex = 0;
			const IntPoint *p2 = &path[curPtIndex];
			BoundBox segBB(*p1, *p2);
			if (!prev_inside)
			{ // prev state: outside, find first point inside C2
				if (segBB.CollidesWith(c2BB) &&
					DistancePointToLineSegSquared(*p1, *p2, c2, clp, par) <= rsqrd)
				{ // current segment inside, start
					prev_inside = true;
					interPaths.push_back(Path());
					if (interPaths.size() > 1)
						break; // we will use poly clipping alg. if there are more intersecting paths
					interPath = &interPaths.back();
					// current segment inside c2, prev point outside, find intersection:
					if (Line2CircleIntersect(c2, toolRadiusScaled, *p1, *p2, inters))
					{
						interPath->push_back(IntPoint(long(inters[0].X), long(inters[0].Y)));
						if (inters.size() > 1)
						{
							interPath->push_back(IntPoint(long(inters[1].X), long(inters[1].Y)));
							prev_inside = false;
						}
						else
						{
							interPath->push_back(IntPoint(*p2));
						}
					}
					else
					{ // no intersection - must be edge case, add p2
						interPath->push_back(IntPoint(*p2));
					}
				}
			}
			else if (interPath != NULL)
			{ // state: inside
				if ((DistanceSqrd(c2, *p2) <= rsqrd))
				{ // next point still inside, add it and continue, no state change
					interPath->push_back(IntPoint(*p2));
				}
				else
				{ // prev point inside, current point outside, find instersection
					if (Line2CircleIntersect(c2, toolRadiusScaled, *p1, *p2, inters))
					{
						if (inters.size() > 1)
						{
							interPath->push_back(IntPoint(long(inters[1].X), long(inters[1].Y)));
						}
						else
						{
							interPath->push_back(IntPoint(long(inters[0].X), long(inters[0].Y)));
						}
					}
					prev_inside = false;
				}
			}
			prevPtIndex = curPtIndex;
			p1 = p2;
		}
		if (interPaths.size() > 1)
			break; // we will use poly clipping alg. if there are more intersecting paths with the tool (rare case)
	}
	Perf_CalcCutAreaCirc.Stop();
	if (interPaths.size() == 1 && interPaths.front().size() > 1)
	{
		Perf_CalcCutAreaCirc.Start();
		Path *interPath = &interPaths.front();
		// interPath - now contains the part of cleared path inside the C2
		size_t ipc2_size = interPath->size();
		const IntPoint &fpc2 = interPath->front(); // first point
		const IntPoint &lpc2 = interPath->back();  // last point
		// path length
		double interPathLen = 0;
		for (size_t j = 1; j < ipc2_size; j++)
			interPathLen += sqrt(DistanceSqrd(interPath->at(j - 1), interPath->at(j)));

		Paths inPaths;
		inPaths.reserve(200);
		inPaths.push_back(*interPath);
		Path pthToSubtract;
		pthToSubtract.push_back(fpc2);

		double fi1 = atan2(fpc2.Y - c2.Y, fpc2.X - c2.X);
		double fi2 = atan2(lpc2.Y - c2.Y, lpc2.X - c2.X);
		double minFi = fi1;
		double maxFi = fi2;
		if (maxFi < minFi)
			maxFi += 2 * M_PI;

		if (preventConventional && interPathLen >= RESOLUTION_FACTOR)
		{
			// detect conventional mode cut - we want only climb mode
			IntPoint midPoint(long(c2.X + toolRadiusScaled * cos(0.5 * (maxFi + minFi))), long(c2.Y + toolRadiusScaled * sin(0.5 * (maxFi + minFi))));
			if (PointSideOfLine(c1, c2, midPoint) < 0)
			{
				area = __DBL_MAX__;
				Perf_CalcCutAreaCirc.Stop();
				// #ifdef DEV_MODE
				// 	cout << "Break: @(" << double(c2.X)/scaleFactor << "," << double(c2.Y)/scaleFactor  << ") conventional mode" << endl;
				// #endif
				return area;
			}
		}

		double scanDistance = 2.5 * toolRadiusScaled;
		// stepping through path discretized to stepDistance
		double stepDistance = min(double(RESOLUTION_FACTOR), interPathLen / 24) + 1;
		const IntPoint *prevPt = &interPath->front();
		double distance = 0;
		for (size_t j = 1; j < ipc2_size; j++)
		{
			const IntPoint *cpt = &interPath->at(j);
			double segLen = sqrt(DistanceSqrd(*cpt, *prevPt));
			if (segLen < NTOL)
				continue; // skip point - segment too short
			for (double pos_unclamped = 0.0; pos_unclamped < segLen + stepDistance; pos_unclamped += stepDistance)
			{
				double pos = pos_unclamped;
				if (pos > segLen)
				{
					distance += stepDistance - (pos - segLen);
					pos = segLen; // make sure we get exact end point
				}
				else
				{
					distance += stepDistance;
				}
				double dx = double(cpt->X - prevPt->X);
				double dy = double(cpt->Y - prevPt->Y);
				IntPoint segPoint(long(prevPt->X + dx * pos / segLen), long(prevPt->Y + dy * pos / segLen));
				IntPoint scanPoint(long(c2.X + scanDistance * cos(minFi + distance * (maxFi - minFi) / interPathLen)),
								   long(c2.Y + scanDistance * sin(minFi + distance * (maxFi - minFi) / interPathLen)));

				IntPoint intersC2(segPoint.X, segPoint.Y);
				IntPoint intersC1(segPoint.X, segPoint.Y);

				// there should be intersection with C2
				if (Line2CircleIntersect(c2, toolRadiusScaled, segPoint, scanPoint, inters))
				{
					if (inters.size() > 1)
					{
						intersC2.X = long(inters[1].X);
						intersC2.Y = long(inters[1].Y);
					}
					else
					{
						intersC2.X = long(inters[0].X);
						intersC2.Y = long(inters[0].Y);
					}
				}
				else
				{
					pthToSubtract.push_back(segPoint);
				}

				if (Line2CircleIntersect(c1, toolRadiusScaled, segPoint, scanPoint, inters))
				{
					if (inters.size() > 1)
					{
						intersC1.X = long(inters[1].X);
						intersC1.Y = long(inters[1].Y);
					}
					else
					{
						intersC1.X = long(inters[0].X);
						intersC1.Y = long(inters[0].Y);
					}
					if (DistanceSqrd(segPoint, intersC2) < DistanceSqrd(segPoint, intersC1))
					{
						pthToSubtract.push_back(intersC2);
					}
					else
					{
						pthToSubtract.push_back(intersC1);
					}
				}
				else
				{ // add the segpoint if no intersection with C1
					pthToSubtract.push_back(segPoint);
				}
			}
			prevPt = cpt;
		}

		pthToSubtract.push_back(lpc2); // add last point
		pthToSubtract.push_back(c2);

		double segArea = Area(pthToSubtract);
		double A = (maxFi - minFi) * rsqrd / 2; // sector area
		area += A - fabs(segArea);
		Perf_CalcCutAreaCirc.Stop();
	}
	else if (interPaths.size() > 1)
	{
		Perf_CalcCutAreaClip.Start();
		// old way of calculating cut area based on polygon clipping
		// used in case when there are multiple intersections of tool with cleared poly (very rare case, but important)
		// 1. find differene between old and new tool shape
		Path oldTool;
		Path newTool;
		TranslatePath(toolGeometry, oldTool, c1);
		TranslatePath(toolGeometry, newTool, c2);
		clip.Clear();
		clip.AddPath(newTool, PolyType::ptSubject, true);
		clip.AddPath(oldTool, PolyType::ptClip, true);
		Paths toolDiff;
		clip.Execute(ClipType::ctDifference, toolDiff);

		// 2. difference to cleared
		clip.Clear();
		clip.AddPaths(toolDiff, PolyType::ptSubject, true);
		clip.AddPaths(clearedBounded, PolyType::ptClip, true);
		Paths cutAreaPoly;
		clip.Execute(ClipType::ctDifference, cutAreaPoly);

		// calculate resulting area
		area = 0;
		for (Path &path : cutAreaPoly)
		{
			area += fabs(Area(path));
		}
		Perf_CalcCutAreaClip.Stop();
	}
	return area;
}

void Adaptive2d::ApplyStockToLeave(Paths &inputPaths)
{
	ClipperOffset clipof;
	if (stockToLeave > NTOL)
	{
		clipof.Clear();
		clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon);
		if (opType == OperationType::otClearingOutside || opType == OperationType::otProfilingOutside)
			clipof.Execute(inputPaths, stockToLeave * scaleFactor);
		else
			clipof.Execute(inputPaths, -stockToLeave * scaleFactor);
	}
	else
	{
		// fix for clipper glitches
		clipof.Clear();
		clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon);
		clipof.Execute(inputPaths, -1);
		clipof.Clear();
		clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon);
		clipof.Execute(inputPaths, 1);
	}
}

//********************************************
// Adaptive2d - Execute
//********************************************

std::list<AdaptiveOutput> Adaptive2d::Execute(const DPaths &stockPaths, const DPaths &paths, std::function<bool(TPaths)> progressCallbackFn)
{
	//**********************************
	// Initializations
	//**********************************

	// keep the tolerance in workable range
	if (tolerance < 0.01)
		tolerance = 0.01;
	if (tolerance > 0.2)
		tolerance = 0.2;

	scaleFactor = RESOLUTION_FACTOR / tolerance;
	if (stepOverFactor * toolDiameter < 1.0)
		scaleFactor *= 1.0 / (stepOverFactor * toolDiameter);
	if (scaleFactor > 1000)
		scaleFactor = 1000;

	//scaleFactor = round(scaleFactor);

	current_region=0;
	cout << "Tool Diameter: " << toolDiameter << endl;
	cout << "Accuracy: " << round(10000.0/scaleFactor)/10 << " um" << endl;
	cout << flush;

	toolRadiusScaled = long(toolDiameter * scaleFactor / 2);
	stepOverScaled = toolRadiusScaled * stepOverFactor;
	progressCallback = &progressCallbackFn;
	lastProgressTime = clock();
	stopProcessing = false;

	if(helixRampDiameter<NTOL)
		helixRampDiameter=0.75*toolDiameter;
	if (helixRampDiameter > toolDiameter)
		helixRampDiameter = toolDiameter;
	if (helixRampDiameter < toolDiameter / 8)
		helixRampDiameter = toolDiameter / 8;

	helixRampRadiusScaled = long(helixRampDiameter * scaleFactor / 2);
	finishPassOffsetScaled = long(stepOverScaled / 10);

	ClipperOffset clipof;
	Clipper clip;

	// generate tool shape
	clipof.Clear();
	Path p;
	p << IntPoint(0, 0);
	clipof.AddPath(p, JoinType::jtRound, EndType::etOpenRound);
	Paths toolGeometryPaths;
	clipof.Execute(toolGeometryPaths, toolRadiusScaled);
	toolGeometry = toolGeometryPaths[0];
	//calculate reference area
	Path slotCut;
	TranslatePath(toolGeometryPaths[0], slotCut, IntPoint(toolRadiusScaled / 2, 0));
	clip.Clear();
	clip.AddPath(toolGeometryPaths[0], PolyType::ptSubject, true);
	clip.AddPath(slotCut, PolyType::ptClip, true);
	Paths crossing;
	clip.Execute(ClipType::ctDifference, crossing);
	referenceCutArea = fabs(Area(crossing[0]));
	optimalCutAreaPD = 2 * stepOverFactor * referenceCutArea / toolRadiusScaled;
#ifdef DEV_MODE
	cout << "optimalCutAreaPD:" << optimalCutAreaPD << " scaleFactor:" << scaleFactor << " toolRadiusScaled:" << toolRadiusScaled << " helixRampRadiusScaled:" << helixRampRadiusScaled << endl;
#endif
	//******************************
	// Convert input paths to clipper
	//******************************
	Paths converted;
	for (size_t i = 0; i < paths.size(); i++)
	{
		Path cpth;
		for (size_t j = 0; j < paths[i].size(); j++)
		{
			std::pair<double, double> pt = paths[i][j];
			cpth.push_back(IntPoint(long(pt.first * scaleFactor), long(pt.second * scaleFactor)));
		}
		Path cpth2;
		CleanPath(cpth,cpth2,FINISHING_CLEAN_PATH_TOLERANCE);
		converted.push_back(cpth2);
	}

	DeduplicatePaths(converted, inputPaths);
	ConnectPaths(inputPaths, inputPaths);
	SimplifyPolygons(inputPaths);
	ApplyStockToLeave(inputPaths);

	//*************************
	// convert stock paths
	//*************************
	stockInputPaths.clear();
	for (size_t i = 0; i < stockPaths.size(); i++)
	{
		Path cpth;
		for (size_t j = 0; j < stockPaths[i].size(); j++)
		{
			std::pair<double, double> pt = stockPaths[i][j];
			cpth.push_back(IntPoint(long(pt.first * scaleFactor), long(pt.second * scaleFactor)));
		}

		stockInputPaths.push_back(cpth);
	}

	SimplifyPolygons(stockInputPaths);
	//CleanPolygons(stockInputPaths,0.707);

	//***************************************
	//	Resolve hierarchy and run processing
	//***************************************
	double cornerRoundingOffset = 0.15 * toolRadiusScaled / 2;
	if (opType == OperationType::otClearingInside || opType == OperationType::otClearingOutside)
	{

		// prepare stock boundary overshooted paths
		clipof.Clear();
		clipof.AddPaths(stockInputPaths, JoinType::jtSquare, EndType::etClosedPolygon);
		double overshootDistance = 4 * toolRadiusScaled + stockToLeave * scaleFactor;
		if (forceInsideOut)
			overshootDistance = 0;
		Paths stockOvershoot;
		clipof.Execute(stockOvershoot, overshootDistance);
		ReversePaths(stockOvershoot);

		if (opType == OperationType::otClearingOutside)
		{
			// add stock paths, with overshooting
			for (auto p : stockOvershoot)
				inputPaths.push_back(p);
		}
		else if (opType == OperationType::otClearingInside)
		{
			// potential TODO: check if there are open paths, and try to close it through overshooted stock boundary
		}

		clipof.Clear();
		clipof.AddPaths(inputPaths, JoinType::jtRound, EndType::etClosedPolygon);
		Paths paths;
		clipof.Execute(paths, -toolRadiusScaled - finishPassOffsetScaled - cornerRoundingOffset);
		for (const auto &current : paths)
		{
			int nesting = getPathNestingLevel(current, paths);
			if (nesting % 2 != 0 && (polyTreeNestingLimit == 0 || nesting <= polyTreeNestingLimit))
			{
				Paths toolBoundPaths;
				toolBoundPaths.push_back(current);
				if (polyTreeNestingLimit != nesting)
				{
					appendDirectChildPaths(toolBoundPaths, current, paths);
				}

				// offset back outwards - corner rounding
				clipof.Clear();
				clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon);
				clipof.Execute(toolBoundPaths, cornerRoundingOffset);

				// restore original bound paths
				// bounding paths - i.e. area that must be cleared inside
				// it's not the same as input paths due to filtering (nesting logic) and corner rounding
				Paths boundPaths;
				clipof.Clear();
				clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon);
				clipof.Execute(boundPaths, toolRadiusScaled + finishPassOffsetScaled);
				ProcessPolyNode(boundPaths, toolBoundPaths);
			}
		}
	}

	if (opType == OperationType::otProfilingInside || opType == OperationType::otProfilingOutside)
	{
		double offset = opType == OperationType::otProfilingInside ? -2 * (helixRampRadiusScaled + toolRadiusScaled) - RESOLUTION_FACTOR : 2 * (helixRampRadiusScaled + toolRadiusScaled) + RESOLUTION_FACTOR;
		for (const auto &current : inputPaths)
		{
			int nesting = getPathNestingLevel(current, inputPaths);
			if (nesting % 2 != 0 && (polyTreeNestingLimit == 0 || nesting <= polyTreeNestingLimit))
			{
				Paths profilePaths;
				profilePaths.push_back(current);
				if (polyTreeNestingLimit != nesting)
				{
					appendDirectChildPaths(profilePaths, current, inputPaths);
				}
				for (size_t i = 0; i < profilePaths.size(); i++)
				{
					double efOffset = i == 0 ? offset : -offset;
					clipof.Clear();
					clipof.AddPath(profilePaths[i], JoinType::jtSquare, EndType::etClosedPolygon);
					Paths off1;
					clipof.Execute(off1, efOffset);
					// make poly between original path and offset path
					Paths boundPaths;
					clip.Clear();
					if (efOffset < 0)
					{
						clip.AddPath(profilePaths[i], PolyType::ptSubject, true);
						clip.AddPaths(off1, PolyType::ptClip, true);
					}
					else
					{
						clip.AddPaths(off1, PolyType::ptSubject, true);
						clip.AddPath(profilePaths[i], PolyType::ptClip, true);
					}
					clip.Execute(ClipType::ctDifference, boundPaths, PolyFillType::pftEvenOdd);

					/** tool bounds */
					Paths toolBoundPaths;
					clipof.Clear();
					clipof.AddPaths(boundPaths, JoinType::jtRound, EndType::etClosedPolygon);
					clipof.Execute(toolBoundPaths, -toolRadiusScaled - finishPassOffsetScaled - cornerRoundingOffset);

					/** offset back outwards - corner rounding */
					clipof.Clear();
					clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon);
					clipof.Execute(toolBoundPaths, cornerRoundingOffset);

					// restore original bound paths
					// bounding paths - i.e. area that must be cleared inside
					// it's not the same as above due to corner rounding
					clipof.Clear();
					clipof.AddPaths(toolBoundPaths, JoinType::jtRound, EndType::etClosedPolygon);
					clipof.Execute(boundPaths, toolRadiusScaled + finishPassOffsetScaled);

					ProcessPolyNode(boundPaths, toolBoundPaths);
				}
			}
		}
	}
	return results;
}

bool Adaptive2d::FindEntryPoint(TPaths &progressPaths, const Paths &toolBoundPaths, const Paths &boundPaths,
								ClearedArea &clearedArea /*output-initial cleared area by helix*/,
								IntPoint &entryPoint /*output*/,
								IntPoint &toolPos, DoublePoint &toolDir)
{
	Paths incOffset;
	Paths lastValidOffset;
	Clipper clip;
	ClipperOffset clipof;
	bool found = false;
	Paths clearedPaths;
	Paths checkPaths = toolBoundPaths;
	for (int iter = 0; iter < 10; iter++)
	{
		clipof.Clear();
		clipof.AddPaths(checkPaths, JoinType::jtSquare, EndType::etClosedPolygon);
		double step = RESOLUTION_FACTOR;
		double currentDelta = -1;
		clipof.Execute(incOffset, currentDelta);
		while (incOffset.size() > 0)
		{
			clipof.Execute(incOffset, currentDelta);
			if (incOffset.size() > 0)
				lastValidOffset = incOffset;
			currentDelta -= step;
		}
		for (size_t i = 0; i < lastValidOffset.size(); i++)
		{
			if (lastValidOffset[i].size() > 0)
			{
				entryPoint = Compute2DPolygonCentroid(lastValidOffset[i]);
				found = true;
				break;
			}
		}
		// check if the start point is in any of the holes
		// this may happen in case when toolBoundPaths are symmetric (boundary + holes)
		// we need to break simetry and try again
		for (size_t j = 0; j < checkPaths.size(); j++)
		{
			int pip = PointInPolygon(entryPoint, checkPaths[j]);
			if ((j == 0 && pip == 0) || (j > 0 && pip != 0))
			{
				found = false;
				break;
			}
		}
		// check if helix fits
		if (found)
		{
			// make initial polygon cleared by helix ramp
			clipof.Clear();
			Path p1;
			p1.push_back(entryPoint);
			clipof.AddPath(p1, JoinType::jtRound, EndType::etOpenRound);
			clipof.Execute(clearedPaths, helixRampRadiusScaled + toolRadiusScaled);
			CleanPolygons(clearedPaths);
			// we got first cleared area - check if it is crossing boundary
			clip.Clear();
			clip.AddPaths(clearedPaths, PolyType::ptSubject, true);
			clip.AddPaths(boundPaths, PolyType::ptClip, true);
			Paths crossing;
			clip.Execute(ClipType::ctDifference, crossing);
			if (crossing.size() > 0)
			{
				// helix does not fit to the cutting area
				found = false;
			}
			else
			{
				clearedArea.SetClearedPaths(clearedPaths);
			}
		}

		if (!found)
		{ // break simetry and try again
			clip.Clear();
			clip.AddPaths(checkPaths, PolyType::ptSubject, true);
			auto bounds = clip.GetBounds();
			clip.Clear();
			Path rect;
			rect << IntPoint(bounds.left, bounds.bottom);
			rect << IntPoint(bounds.left, (bounds.top + bounds.bottom) / 2);
			rect << IntPoint((bounds.left + bounds.right) / 2, (bounds.top + bounds.bottom) / 2);
			rect << IntPoint((bounds.left + bounds.right) / 2, bounds.bottom);
			clip.AddPath(rect, PolyType::ptSubject, true);
			clip.AddPaths(checkPaths, PolyType::ptClip, true);
			clip.Execute(ClipType::ctIntersection, checkPaths);
		}
		if (found)
			break;
	}

	if (!found)
		cerr << "Start point not found!" << endl;
	if (found)
	{
		// visualize/progress for helix
		clipof.Clear();
		Path hp;
		hp << entryPoint;
		clipof.AddPath(hp, JoinType::jtRound, EndType::etOpenRound);
		Paths hps;
		clipof.Execute(hps, helixRampRadiusScaled);
		AddPathsToProgress(progressPaths, hps);

		toolPos = IntPoint(entryPoint.X, entryPoint.Y - helixRampRadiusScaled);
		toolDir = DoublePoint(1.0, 0.0);
	}
	return found;
}
bool Adaptive2d::FindEntryPointOutside(TPaths &progressPaths, const Paths &toolBoundPaths, const Paths &boundPaths,
									   ClearedArea &clearedArea /*output-initial cleared area by helix*/,
									   IntPoint &entryPoint /*output*/,
									   IntPoint &toolPos, DoublePoint &toolDir)
{

	UNUSED(progressPaths); // to silence compiler warning
	UNUSED(boundPaths);	// to silence compiler warning

	Clipper clip;
	ClipperOffset clipof;
	Paths clearedPaths;
	// check if boundary shape to cut is outside the stock
	for (const auto &pth : toolBoundPaths)
	{
		for (size_t i = 0; i < pth.size(); i++)
		{
			IntPoint checkPoint = pth[i];
			IntPoint lastPoint = i > 0 ? pth[i - 1] : pth.back();
			// if point is outside the stock
			if (PointInPolygon(checkPoint, stockInputPaths.front()) == 0)
			{

				clipof.Clear();
				clipof.AddPaths(stockInputPaths, JoinType::jtSquare, EndType::etClosedPolygon);
				clipof.Execute(clearedPaths, 1000 * toolRadiusScaled);

				clip.Clear();
				clip.AddPaths(clearedPaths, PolyType::ptSubject, true);
				clip.AddPaths(stockInputPaths, PolyType::ptClip, true);
				clip.Execute(ClipType::ctDifference, clearedPaths);
				CleanPolygons(clearedPaths);
				SimplifyPolygons(clearedPaths);
				clearedArea.SetClearedPaths(clearedPaths);
				entryPoint = checkPoint;
				toolPos = entryPoint;
				// find tool dir
				double len = sqrt(DistanceSqrd(lastPoint, checkPoint));
				toolDir = DoublePoint((checkPoint.X - lastPoint.X) / len, (checkPoint.Y - lastPoint.Y) / len);
				return true;
			}
		}
	}
	return false;
}

//************************************************************
//  IsClearPath - returns true if path is clear from obstacles
//***********************************************************
bool Adaptive2d::IsClearPath(const Path &tp, ClearedArea &cleared, double safetyClearance)
{
	Perf_IsClearPath.Start();
	Clipper clip;
	ClipperOffset clipof;
	clipof.AddPath(tp, JoinType::jtRound, EndType::etOpenRound);
	Paths toolShape;
	clipof.Execute(toolShape, toolRadiusScaled + safetyClearance);
	clip.AddPaths(toolShape, PolyType::ptSubject, true);
	clip.AddPaths(cleared.GetCleared(), PolyType::ptClip, true);
	Paths crossing;
	clip.Execute(ClipType::ctDifference, crossing);
	double collisionArea = 0;
	for (auto &p : crossing)
	{
		collisionArea += fabs(Area(p));
	}
	Perf_IsClearPath.Stop();
	return collisionArea < 1.0;
}

bool Adaptive2d::IsAllowedToCutTrough(const IntPoint &p1, const IntPoint &p2, ClearedArea &cleared, const Paths &toolBoundPaths, double areaFactor, bool skipBoundsCheck)
{
	Perf_IsAllowedToCutTrough.Start();

	if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, p2))
	{
		// last point outside boundary - its not clear to cut
		Perf_IsAllowedToCutTrough.Stop();
		return false;
	}
	else if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, p1))
	{
		// first point outside boundary - its not clear to cut
		Perf_IsAllowedToCutTrough.Stop();
		return false;
	}
	else
	{
		Clipper clip;
		double distance = sqrt(DistanceSqrd(p1, p2));
		double stepSize = min(0.5 * stepOverScaled, 8 * RESOLUTION_FACTOR);
		if (distance < stepSize / 2)
		{ // not significant cut
			Perf_IsAllowedToCutTrough.Stop();
			return true;
		}
		if (distance < stepSize)
		{ // adjust for numeric instability with small distances
			areaFactor *= 2;
		}

		IntPoint toolPos1 = p1;
		long steps = long(distance / stepSize) + 1;
		stepSize = distance / steps;
		for (long i = 1; i <= steps; i++)
		{
			double p = double(i) / steps;
			IntPoint toolPos2(long(p1.X + double(p2.X - p1.X) * p), long(p1.Y + double(p2.Y - p1.Y) * p));
			double area = CalcCutArea(clip, toolPos1, toolPos2, cleared, false);
			// if we are cutting above optimal -> not clear to cut
			if (area > areaFactor * stepSize * optimalCutAreaPD)
			{
				Perf_IsAllowedToCutTrough.Stop();
				return false;
			}
			//if tool is outside boundary -> its not clear to cut
			if (!skipBoundsCheck && !IsPointWithinCutRegion(toolBoundPaths, toolPos2))
			{
				Perf_IsAllowedToCutTrough.Stop();
				return false;
			}
			toolPos1 = toolPos2;
		}
	}
	Perf_IsAllowedToCutTrough.Stop();
	return true;
}

bool Adaptive2d::ResolveLinkPath(const IntPoint &startPoint, const IntPoint &endPoint, ClearedArea &clearedArea, Path &output)
{
	vector<pair<IntPoint, IntPoint>> queue;
	queue.push_back(pair<IntPoint, IntPoint>(startPoint, endPoint));
	Path checkPath;
	double totalLength = 0;
	double directDistance = sqrt(DistanceSqrd(startPoint, endPoint));
	Paths linkPaths;

	double scanStep = 2 * RESOLUTION_FACTOR;
	if (scanStep > scaleFactor * 0.1)
		scanStep = scaleFactor * 0.1;
	if (scanStep < scaleFactor * 0.01)
		scanStep = scaleFactor * 0.01;
	long limit = 10000;

	double clearance = stepOverScaled;
	double offClearance = 2 * stepOverScaled;
	if (offClearance > directDistance / 2)
	{
		offClearance = directDistance / 2;
		clearance = 0;
	}

	long cnt = 0;

	// to hold CLP results
	IntPoint clp;
	size_t pindex;
	size_t sindex;
	double par;

	// put a time limit on the resolving the link path
	clock_t time_limit = (clock_t)(max(keepToolDownDistRatio, 3.0) * CLOCKS_PER_SEC / 6);

	clock_t time_out = clock() + time_limit;

	while (!queue.empty())
	{
		if (stopProcessing)
			return false;
		if (clock() > time_out)
		{
			cout << "Unable to resolve tool down linking path (limit reached)." << endl;
			return false;
		}

		cnt++;
		if (cnt > limit)
		{
			cout << "Unable to resolve tool down linking path @(" << endPoint.X / scaleFactor << "," << endPoint.Y / scaleFactor << ") (" << limit << " points limit reached)." << endl;
			return false;
		}
		pair<IntPoint, IntPoint> pointPair = queue.back();
		queue.pop_back();

		// check for self intersections - if found discard the link path
		for (size_t i = 0; i < linkPaths.size(); i++)
		{
			if (linkPaths[i].front() != pointPair.first && linkPaths[i].back() != pointPair.first && linkPaths[i].front() != pointPair.second && linkPaths[i].back() != pointPair.second && IntersectionPoint(linkPaths[i].front(), linkPaths[i].back(), pointPair.first, pointPair.second, clp))
			{
				cout << "Unable to resolve tool down linking path (self-intersects)." << endl;
				return false;
			}
		}

		DoublePoint direction = DirectionV(pointPair.first, pointPair.second);
		checkPath.clear();
		if (pointPair.first == startPoint)
		{
			checkPath.push_back(IntPoint(pointPair.first.X + offClearance * direction.X, pointPair.first.Y + offClearance * direction.Y));
		}
		else
		{
			checkPath.push_back(pointPair.first);
		}
		if (pointPair.second == endPoint)
		{
			checkPath.push_back(IntPoint(pointPair.second.X - offClearance * direction.X, pointPair.second.Y - offClearance * direction.Y));
		}
		else
		{
			checkPath.push_back(pointPair.second);
		}

		if (IsClearPath(checkPath, clearedArea, clearance))
		{
			totalLength += sqrt(DistanceSqrd(pointPair.first, pointPair.second));
			if (totalLength > keepToolDownDistRatio * directDistance)
			{
				return false;
			}
			Path link;
			link.push_back(pointPair.first);
			link.push_back(pointPair.second);
			linkPaths.push_back(link);
		}
		else
		{
			if (sqrt(DistanceSqrd(pointPair.first, pointPair.second)) < 4)
			{
				//segment became too short but still not clear
				return false;
			}
			DoublePoint pDir(-direction.Y, direction.X);
			// find mid point
			IntPoint midPoint(0.5 * double(pointPair.first.X + pointPair.second.X), 0.5 * double(pointPair.first.Y + pointPair.second.Y));
			for (long i = 1;; i++)
			{
				if (stopProcessing)
					return false;
				double offset = i * scanStep;
				IntPoint checkPoint1(midPoint.X + offset * pDir.X, midPoint.Y + offset * pDir.Y);
				IntPoint checkPoint2(midPoint.X - offset * pDir.X, midPoint.Y - offset * pDir.Y);

				if (DistancePointToPathsSqrd(clearedArea.GetCleared(), checkPoint1, clp, pindex, sindex, par) <
					DistancePointToPathsSqrd(clearedArea.GetCleared(), checkPoint2, clp, pindex, sindex, par))
				{
					// exchange points
					IntPoint tmp = checkPoint2;
					checkPoint2 = checkPoint1;
					checkPoint1 = tmp;
				}

				checkPath.clear();
				checkPath.push_back(checkPoint1);
				if (IsClearPath(checkPath, clearedArea, clearance + 1))
				{ // check if point clear
					queue.push_back(pair<IntPoint, IntPoint>(pointPair.first, checkPoint1));
					queue.push_back(pair<IntPoint, IntPoint>(checkPoint1, pointPair.second));
					break;
				}
				else
				{ // check the other side

					checkPath.clear();
					checkPath.push_back(checkPoint2);
					if (IsClearPath(checkPath, clearedArea, clearance + 1))
					{
						queue.push_back(pair<IntPoint, IntPoint>(pointPair.first, checkPoint2));
						queue.push_back(pair<IntPoint, IntPoint>(checkPoint2, pointPair.second));
						break;
					}
				}
				if (offset > keepToolDownDistRatio * directDistance)
					return false; // can't find keep tool down link
			}
		}
	}
	if (linkPaths.empty())
		return false;
	ConnectPaths(linkPaths, linkPaths);
	output = linkPaths[0];
	return true;
}

bool Adaptive2d::MakeLeadPath(bool leadIn, const IntPoint &startPoint, const DoublePoint &startDir, const IntPoint &beaconPoint,
							  ClearedArea &clearedArea, const Paths &toolBoundPaths, Path &output)
{
	IntPoint currentPoint = startPoint;
	DoublePoint targetDir = DirectionV(currentPoint, beaconPoint);
	double distanceToBeacon = sqrt(DistanceSqrd(startPoint, beaconPoint));
	double stepSize = 0.2 * stepOverScaled + 1;
	double maxPathLen = stepOverScaled;
	DoublePoint nextDir = startDir;
	IntPoint nextPoint = IntPoint(currentPoint.X + nextDir.X * stepSize, currentPoint.Y + nextDir.Y * stepSize);
	Path checkPath;
	double adaptFactor = 0.4;
	double alfa = M_PI / 64;
	double pathLen = 0;
	checkPath.push_back(nextPoint);
	for (int i = 0; i < 10000; i++)
	{
		if (IsAllowedToCutTrough(IntPoint(currentPoint.X + RESOLUTION_FACTOR * nextDir.X, currentPoint.Y + RESOLUTION_FACTOR * nextDir.Y), nextPoint, clearedArea, toolBoundPaths))
		{
			if (output.empty())
				output.push_back(currentPoint);
			output.push_back(nextPoint);
			currentPoint = nextPoint;
			pathLen += stepSize;
			targetDir = DirectionV(currentPoint, beaconPoint);
			nextDir = DoublePoint(nextDir.X + adaptFactor * targetDir.X, nextDir.Y + adaptFactor * targetDir.Y);
			NormalizeV(nextDir);
			if (pathLen > maxPathLen)
				break;
			if (pathLen > distanceToBeacon / 2)
				break;
		}
		else
		{
			nextDir = rotate(nextDir, leadIn ? -alfa : alfa);
		}
		nextPoint = IntPoint(currentPoint.X + nextDir.X * stepSize, currentPoint.Y + nextDir.Y * stepSize);
	}
	if (output.empty())
		output.push_back(startPoint);
	return true;
}
void Adaptive2d::AppendToolPath(TPaths &progressPaths, AdaptiveOutput &output,
								const Path &passToolPath, ClearedArea &clearedBefore,
								ClearedArea &clearedAfter, const Paths &toolBoundPaths)
{
	if (passToolPath.size() < 2)
		return;
	Perf_AppendToolPath.Start();
	UNUSED(progressPaths); // to silence compiler warning,var is occasionally used in dev. for debugging

	IntPoint endPoint(passToolPath[0]);
	// if there is a previous path - need to resolve linking move to new path
	if (output.AdaptivePaths.size() > 0 && output.AdaptivePaths.back().second.size() > 1)
	{
		auto &lastTPath = output.AdaptivePaths.back();

		auto &lastPrevTPoint = lastTPath.second.at(lastTPath.second.size() - 2);
		auto &lastTPoint = lastTPath.second.back();

		IntPoint startPrevPoint(long(lastPrevTPoint.first * scaleFactor), long(lastPrevTPoint.second * scaleFactor));
		IntPoint startPoint(long(lastTPoint.first * scaleFactor), long(lastTPoint.second * scaleFactor));

		ClipperOffset clipof;
		//first we try to cut through the linking move for short distances
		bool linkFound = false;
		double linkDistance = sqrt(DistanceSqrd(startPoint, endPoint));
		if (linkDistance < NTOL)
			linkFound = true;

		if (!linkFound)
		{
			size_t clpPathIndex;
			size_t clpSegmentIndex;
			double clpParameter;
			IntPoint clp;

			double beaconOffset = stepOverScaled;
			if (beaconOffset > linkDistance)
			{
				beaconOffset = linkDistance;
			}

			double pathLen = PathLength(passToolPath);
			if (beaconOffset > pathLen / 2)
			{
				beaconOffset = pathLen / 2;
			}
			if (beaconOffset > linkDistance / 2)
			{
				beaconOffset = linkDistance / 2;
			}

			DistancePointToPathsSqrd(toolBoundPaths, startPoint, clp, clpPathIndex, clpSegmentIndex, clpParameter);
			DoublePoint startDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex);

			DistancePointToPathsSqrd(toolBoundPaths, endPoint, clp, clpPathIndex, clpSegmentIndex, clpParameter);
			DoublePoint endDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex);

			IntPoint startBeacon(startPoint.X - beaconOffset * (startDir.Y - startDir.X), startPoint.Y + beaconOffset * (startDir.X + startDir.Y));
			IntPoint endBeacon(endPoint.X - beaconOffset * (endDir.X + endDir.Y), endPoint.Y + beaconOffset * (endDir.X - endDir.Y));
			Path leadOutPath;
			MakeLeadPath(false, startPoint, startDir, startBeacon, clearedBefore, toolBoundPaths, leadOutPath);

			Path leadInPath;
			MakeLeadPath(true, endPoint, DoublePoint(-endDir.X, -endDir.Y), endBeacon, clearedBefore, toolBoundPaths, leadInPath);
			ReversePath(leadInPath);

			Path linkPath;
			MotionType linkType = MotionType::mtCutting;

			// this is not needed:
			//clearedBefore.ExpandCleared(leadInPath);
			//clearedBefore.ExpandCleared(leadOutPath);

			if (ResolveLinkPath(leadOutPath.back(), leadInPath.front(), clearedBefore, linkPath))
			{
				linkType = MotionType::mtLinkClear;
				double remainingLeadInExtension = stepOverScaled / 2;
				while (linkPath.size() >= 2 && remainingLeadInExtension > NTOL)
				{
					IntPoint p1 = linkPath.at(linkPath.size() - 2);
					IntPoint p2 = linkPath.at(linkPath.size() - 1);
					double l = sqrt(DistanceSqrd(p1, p2));
					if (l >= remainingLeadInExtension)
					{
						IntPoint splitPoint(p1.X + (p2.X - p1.X) * (l - remainingLeadInExtension) / l, p1.Y + (p2.Y - p1.Y) * (l - remainingLeadInExtension) / l);
						linkPath.pop_back();
						linkPath.push_back(splitPoint);
						leadInPath.insert(leadInPath.begin(), splitPoint);
						remainingLeadInExtension = 0;
						Path checkPath;
						checkPath.push_back(p2);
						checkPath.push_back(splitPoint);
						if (!IsClearPath(checkPath, clearedBefore, 0))
						{
							remainingLeadInExtension = stepOverScaled / 2;
						}
					}
					else
					{
						linkPath.pop_back();
						leadInPath.insert(leadInPath.begin(), p1);
						remainingLeadInExtension -= l;
						if (remainingLeadInExtension < NTOL)
						{
							Path checkPath;
							checkPath.push_back(p2);
							checkPath.push_back(p1);
							if (!IsClearPath(checkPath, clearedBefore, 0))
							{
								remainingLeadInExtension = stepOverScaled / 2;
							}
						}
					}
				}
			}
			else
			{
				linkType = MotionType::mtLinkNotClear;
				double dist = sqrt(DistanceSqrd(leadOutPath.back(), leadInPath.front()));
				if (dist < 2 * stepOverScaled && IsAllowedToCutTrough(
													 IntPoint(leadOutPath.back().X + (leadInPath.front().X - leadOutPath.back().X) / dist, leadOutPath.back().Y + (leadInPath.front().Y - leadOutPath.back().Y) / dist),
													 IntPoint(leadInPath.front().X - (leadInPath.front().X - leadOutPath.back().X) / dist, leadInPath.front().Y - (leadInPath.front().Y - leadOutPath.back().Y) / dist),
													 clearedBefore, toolBoundPaths))
					linkType = MotionType::mtCutting;
				// add direct linking move at clear height

				linkPath.clear();
				linkPath.push_back(leadOutPath.back());
				linkPath.push_back(leadInPath.front());
			}

			/* paths smoothing*/
			Paths linkPaths;
			linkPaths.push_back(leadOutPath);
			linkPaths.push_back(linkPath);
			linkPaths.push_back(leadInPath);

			if (linkType == MotionType::mtLinkClear)
			{
				SmoothPaths(linkPaths, 0.1*stepOverScaled,1,4);
			}

			leadOutPath = linkPaths[0];
			linkPath = linkPaths[1];
			leadInPath = linkPaths[2];

			// add lead-out move
			TPath linkPath1;
			linkPath1.first = MotionType::mtCutting;
			for (const auto &pt : leadOutPath)
			{
				linkPath1.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
			}
			output.AdaptivePaths.push_back(linkPath1);

			// add linking path
			TPath linkPath2;
			linkPath2.first = linkType;
			for (const auto &pt : linkPath)
			{
				linkPath2.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
			}
			output.AdaptivePaths.push_back(linkPath2);

			// add lead-in move
			TPath linkPath3;
			linkPath3.first = MotionType::mtCutting;
			for (const auto &pt : leadInPath)
			{
				linkPath3.second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
			}

			output.AdaptivePaths.push_back(linkPath3);

			clearedAfter.ExpandCleared(leadInPath);
			clearedAfter.ExpandCleared(leadOutPath);

			linkFound = true;
		}
		if (!linkFound)
		{ // nothing clear so far - check direct link with no interim points - either this is clear or we need to raise the tool
			Path tp;
			tp << startPoint;
			tp << endPoint;
			MotionType mt = IsClearPath(tp, clearedBefore) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear;

			// make cutting move through small clear links
			if (mt == MotionType::mtLinkClear && linkDistance < toolRadiusScaled)
			{
				mt = MotionType::mtCutting;
				clearedAfter.ExpandCleared(tp);
			}

			TPath linkPath;
			linkPath.first = mt;
			linkPath.second.push_back(DPoint(double(startPoint.X) / scaleFactor, double(startPoint.Y) / scaleFactor));
			linkPath.second.push_back(DPoint(double(endPoint.X) / scaleFactor, double(endPoint.Y) / scaleFactor));
			output.AdaptivePaths.push_back(linkPath);
		}
	}
	TPath cutPath;
	cutPath.first = MotionType::mtCutting;
	for (const auto &p : passToolPath)
	{
		DPoint nextT;
		nextT.first = double(p.X) / scaleFactor;
		nextT.second = double(p.Y) / scaleFactor;
		cutPath.second.push_back(nextT);
	}

	if (cutPath.second.size() > 0)
		output.AdaptivePaths.push_back(cutPath);
	Perf_AppendToolPath.Stop();
}

void Adaptive2d::CheckReportProgress(TPaths &progressPaths, bool force)
{
	if (!force && (clock() - lastProgressTime < PROGRESS_TICKS))
		return; // not yet
	lastProgressTime = clock();
	if (progressPaths.size() == 0)
		return;
	if (progressCallback)
		if ((*progressCallback)(progressPaths))
			stopProcessing = true; // call python function, if returns true signal stop processing
	// clean the paths - keep the last point
	if (progressPaths.back().second.size() == 0)
		return;
	TPath *lastPath = &progressPaths.back();
	DPoint *lastPoint = &lastPath->second.back();
	DPoint next(lastPoint->first, lastPoint->second);
	while (progressPaths.size() > 1)
		progressPaths.pop_back();
	while (progressPaths.front().second.size() > 0)
		progressPaths.front().second.pop_back();
	progressPaths.front().first = MotionType::mtCutting;
	progressPaths.front().second.push_back(next);
}

void Adaptive2d::AddPathsToProgress(TPaths &progressPaths, Paths paths, MotionType mt)
{
	for (const auto &pth : paths)
	{
		if (pth.size() > 0)
		{
			progressPaths.push_back(TPath());
			progressPaths.back().first = mt;
			for (const auto pt : pth)
				progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
			progressPaths.back().second.push_back(DPoint(double(pth.front().X) / scaleFactor, double(pth.front().Y) / scaleFactor));
		}
	}
}

void Adaptive2d::AddPathToProgress(TPaths &progressPaths, const Path pth, MotionType mt)
{
	if (pth.size() > 0)
	{
		progressPaths.push_back(TPath());
		progressPaths.back().first = mt;
		for (const auto pt : pth)
			progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
	}
}

void Adaptive2d::ProcessPolyNode(Paths boundPaths, Paths toolBoundPaths)
{
	Perf_ProcessPolyNode.Start();
	current_region++;
	cout << "** Processing region: " << current_region << endl;

	// node paths are already constrained to tool boundary path for adaptive path before finishing pass
	Clipper clip;
	ClipperOffset clipof;

	IntPoint entryPoint;
	TPaths progressPaths;
	progressPaths.reserve(10000);

	CleanPolygons(toolBoundPaths);
	SimplifyPolygons(toolBoundPaths);

	CleanPolygons(boundPaths);
	SimplifyPolygons(boundPaths);

	AddPathsToProgress(progressPaths, toolBoundPaths, MotionType::mtLinkClear);

	IntPoint toolPos;
	DoublePoint toolDir;
	ClearedArea cleared(toolRadiusScaled);
	bool outsideEntry = false;
	bool firstEngagePoint = true;
	Paths engageBounds = toolBoundPaths;

	if (!forceInsideOut && FindEntryPointOutside(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos, toolDir))
	{
		if (Orientation(engageBounds[0]) == false)
			ReversePath(engageBounds[0]);
		// add initial offset of cleared area to engage paths
		Paths outsideEngage;
		clipof.Clear();
		clipof.AddPaths(stockInputPaths, JoinType::jtRound, EndType::etClosedPolygon);
		clipof.Execute(outsideEngage, toolRadiusScaled - stepOverFactor * toolRadiusScaled);
		CleanPolygons(outsideEngage);
		ReversePaths(outsideEngage);
		for (auto p : outsideEngage)
			engageBounds.push_back(p);
		outsideEntry = true;
	}
	else
	{
		if (!FindEntryPoint(progressPaths, toolBoundPaths, boundPaths, cleared, entryPoint, toolPos, toolDir))
		{
			Perf_ProcessPolyNode.Stop();
			return;
		}
	}

	EngagePoint engage(engageBounds); // engage point stepping instance

	if (outsideEntry)
	{
		engage.moveToClosestPoint(toolPos, 2 * RESOLUTION_FACTOR);
		engage.moveForward(RESOLUTION_FACTOR);
		toolPos = engage.getCurrentPoint();
		toolDir = engage.getCurrentDir();
		entryPoint = toolPos;
	}

	//cout << "Entry point:" << double(entryPoint.X)/scaleFactor << "," << double(entryPoint.Y)/scaleFactor << endl;

	AdaptiveOutput output;
	output.HelixCenterPoint.first = double(entryPoint.X) / scaleFactor;
	output.HelixCenterPoint.second = double(entryPoint.Y) / scaleFactor;

	long stepScaled = long(RESOLUTION_FACTOR);
	IntPoint engagePoint;

	IntPoint newToolPos;
	DoublePoint newToolDir;

	CheckReportProgress(progressPaths, true);

	IntPoint startPoint = toolPos;
	output.StartPoint = DPoint(double(startPoint.X) / scaleFactor, double(startPoint.Y) / scaleFactor);

	Path passToolPath; // to store pass toolpath
	Path toClearPath;
	IntPoint clp;				 // to store closest point
	vector<DoublePoint> gyro;	// used to average tool direction
	vector<double> angleHistory; // use to predict deflection angle
	double angle = M_PI;
	engagePoint = toolPos;
	Interpolation interp; // interpolation instance

	long total_iterations = 0;
	long total_points = 0;
	long total_exceeded = 0;
	long total_output_points = 0;
	long over_cut_count = 0;
	long bad_engage_count = 0;
	double prevDistFromStart = 0;
	double refinement_factor = 1;
	bool prevDistTrend = false;

	double perf_total_len = 0;
#ifdef DEV_MODE
	clock_t start_clock = clock();
#endif
	ClearedArea clearedBeforePass(toolRadiusScaled);
	clearedBeforePass.SetClearedPaths(cleared.GetCleared());

	//*******************************
	// LOOP - PASSES
	//*******************************
	for (long pass = 0; pass < PASSES_LIMIT; pass++)
	{
		if (stopProcessing)
			break;

		passToolPath.clear();
		toClearPath.clear();
		angleHistory.clear();

		// append a new path to progress info paths
		if (progressPaths.size() == 0)
		{
			progressPaths.push_back(TPath());
		}
		else
		{
			// append new path if previous not empty
			if (progressPaths.back().second.size() > 0)
				progressPaths.push_back(TPath());
		}

		angle = M_PI / 4; // initial pass angle
		bool recalcArea = false;
		double cumulativeCutArea = 0;
		// init gyro
		gyro.clear();
		for (int i = 0; i < DIRECTION_SMOOTHING_BUFLEN; i++)
			gyro.push_back(toolDir);

		size_t clpPathIndex;
		size_t clpSegmentIndex;
		double clpParamter;
		double passLength = 0;
		double noCutDistance=0;
		clearedBeforePass.SetClearedPaths(cleared.GetCleared());
		//*******************************
		// LOOP - POINTS
		//*******************************
		for (long point_index = 0; point_index < POINTS_PER_PASS_LIMIT; point_index++)
		{
			if (stopProcessing)
				break;

			total_points++;
			AverageDirection(gyro, toolDir);
			Perf_DistanceToBoundary.Start();

			double distanceToBoundary = sqrt(DistancePointToPathsSqrd(toolBoundPaths, toolPos, clp, clpPathIndex, clpSegmentIndex, clpParamter));
			DoublePoint boundaryDir = GetPathDirectionV(toolBoundPaths[clpPathIndex], clpSegmentIndex);
			double distBoundaryPointToEngage = sqrt(DistanceSqrd(clp, engagePoint));

			Perf_DistanceToBoundary.Stop();
			double distanceToEngage = sqrt(DistanceSqrd(toolPos, engagePoint));

			double targetAreaPD = optimalCutAreaPD;

			// set the step size
			double slowDownDistance = max(double(toolRadiusScaled) / 4, RESOLUTION_FACTOR * 8);
			if (distanceToBoundary < slowDownDistance || distanceToEngage < slowDownDistance)
			{
				stepScaled = long(RESOLUTION_FACTOR);
			}
			else if (fabs(angle) > NTOL)
			{
				stepScaled = long(RESOLUTION_FACTOR / fabs(angle));
			}
			else
			{
				stepScaled = long(RESOLUTION_FACTOR * 4);
			}

			// clamp the step size - for stability
			if (stepScaled > min(long(toolRadiusScaled / 4), long(RESOLUTION_FACTOR * 8)))
				stepScaled = min(long(toolRadiusScaled / 4), long(RESOLUTION_FACTOR * 8));
			if (stepScaled < RESOLUTION_FACTOR)
				stepScaled = long(RESOLUTION_FACTOR);

			//*****************************
			// ANGLE vs AREA ITERATIONS
			//*****************************
			double predictedAngle = averageDV(angleHistory);
			double maxError = AREA_ERROR_FACTOR * optimalCutAreaPD;
			double area = 0;
			double areaPD = 0;
			interp.clear();
			/******************************/
			Perf_PointIterations.Start();
			int iteration;
			double prev_error = __DBL_MAX__;
			for (iteration = 0; iteration < MAX_ITERATIONS; iteration++)
			{
				total_iterations++;
				if (iteration == 0)
					angle = predictedAngle;
				else if (iteration == 1)
					angle = interp.MIN_ANGLE; // max engage
				else if (iteration == 3)
					angle = interp.MAX_ANGLE; // min engage
				else if (interp.getPointCount() < 2)
					angle = interp.getRandomAngle();
				else
					angle = interp.interpolateAngle(targetAreaPD);
				angle = interp.clampAngle(angle);

				newToolDir = rotate(toolDir, angle);
				newToolPos = IntPoint(long(toolPos.X + newToolDir.X * stepScaled), long(toolPos.Y + newToolDir.Y * stepScaled));

				area = CalcCutArea(clip, toolPos, newToolPos, cleared);

				areaPD = area / double(stepScaled); // area per distance
				interp.addPoint(areaPD, angle);
				double error = areaPD - targetAreaPD;
				//	cout << " iter:" << iteration << " angle:" << angle << " area:" << areaPD << " target:" << targetAreaPD << " error:" << error << " max:"<< maxError << endl;
				if (fabs(error) < maxError)
				{
					angleHistory.push_back(angle);
					if (angleHistory.size() > ANGLE_HISTORY_POINTS)
						angleHistory.erase(angleHistory.begin());
					break;
				}
				if (iteration > 5 && fabs(error - prev_error) < 0.001)
					break;
				if (iteration == MAX_ITERATIONS - 1)
					total_exceeded++;
				prev_error = error;
			}
			Perf_PointIterations.Stop();

			recalcArea = false;
			// approach end boundary tangentially
			double relDistToBoundary = 4 * distanceToBoundary / stepOverScaled;
			if (relDistToBoundary <= 1.0 && passLength > 2 * stepOverFactor && distanceToEngage > 2 * stepOverScaled && distBoundaryPointToEngage > 2 * stepOverScaled)
			{
				double wb = 1 - relDistToBoundary;
				newToolDir = DoublePoint(newToolDir.X + wb * boundaryDir.X, newToolDir.Y + wb * boundaryDir.Y);
				NormalizeV(newToolDir);
				newToolPos = IntPoint(long(toolPos.X + newToolDir.X * stepScaled), long(toolPos.Y + newToolDir.Y * stepScaled));
				recalcArea = true;
			}

			//**********************************************
			// CHECK AND RECORD NEW TOOL POS
			//**********************************************
			long rotateStep = 0;
			while (!IsPointWithinCutRegion(toolBoundPaths, newToolPos) && rotateStep < 180)
			{
				rotateStep++;
				// if new tool pos. outside boundary rotate until back in
				recalcArea = true;
				newToolDir = rotate(newToolDir, M_PI / 90);
				newToolPos = IntPoint(long(toolPos.X + newToolDir.X * stepScaled), long(toolPos.Y + newToolDir.Y * stepScaled));
			}
			if (rotateStep >= 180)
			{
				#ifdef DEV_MODE
					cerr << "Warning: unexpected number of rotate iterations." << endl;
				#endif
				break;
			}

			if (recalcArea)
			{
				area = CalcCutArea(clip, toolPos, newToolPos, cleared);
			}

			// safety condition
			if (area > stepScaled * optimalCutAreaPD && areaPD > 2 * optimalCutAreaPD)
			{
				over_cut_count++;
				break;
			}

			// update cleared paths when trend of distance from start point changes sign (starts to get closer, or start to get farther)
			double distFromStart = sqrt(DistanceSqrd(toolPos, startPoint));
			bool distanceTrend = distFromStart > prevDistFromStart ? true : false;

			if (distanceTrend != prevDistTrend)
			{
				cleared.ExpandCleared(toClearPath);
				toClearPath.clear();
			}
			prevDistTrend = distanceTrend;
			prevDistFromStart = distFromStart;

			if (area > 0.5 * MIN_CUT_AREA_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR)
			{ // cut is ok - record it
				noCutDistance=0;
				if (toClearPath.size() == 0)
					toClearPath.push_back(toolPos);
				toClearPath.push_back(newToolPos);

				cumulativeCutArea += area;

				// append to toolpaths
				if (passToolPath.size() == 0)
				{
					// in outside entry first successful cut defines the "helix center" and start point
					// in this case helix diameter is 0 (straight line downwards)
					if (output.AdaptivePaths.size() == 0 && outsideEntry)
					{
						entryPoint = toolPos;
						output.HelixCenterPoint.first = double(entryPoint.X) / scaleFactor;
						output.HelixCenterPoint.second = double(entryPoint.Y) / scaleFactor;
						output.StartPoint = DPoint(double(entryPoint.X) / scaleFactor, double(entryPoint.Y) / scaleFactor);
					}
					passToolPath.push_back(toolPos);
				}
				passToolPath.push_back(newToolPos);
				perf_total_len += stepScaled;
				passLength += stepScaled;
				toolPos = newToolPos;

				// append to progress info paths
				if (progressPaths.size() == 0)
					progressPaths.push_back(TPath());
				progressPaths.back().second.push_back(DPoint(double(newToolPos.X) / scaleFactor, double(newToolPos.Y) / scaleFactor));

				// append gyro
				gyro.push_back(newToolDir);
				gyro.erase(gyro.begin());
				CheckReportProgress(progressPaths);
			}
			else
			{
#ifdef DEV_MODE
				// if(point_index==0) {
				// 	engage_no_cut_count++;
				// 	cout<<"Break:no cut #" << engage_no_cut_count << ", bad engage, pass:" << pass << " over_cut_count:" << over_cut_count << endl;
				// }
#endif
				//cout<<"Break: no cut @" << point_index << endl;
				if(noCutDistance>stepOverScaled) break;
				noCutDistance+=stepScaled;
			}
		} /* end of points loop*/

		if (toClearPath.size() > 0)
		{
			cleared.ExpandCleared(toClearPath);
			toClearPath.clear();
		}
		if (cumulativeCutArea > MIN_CUT_AREA_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR)
		{
			Path cleaned;
			CleanPath(passToolPath, cleaned, CLEAN_PATH_TOLERANCE);
			total_output_points += long(cleaned.size());
			AppendToolPath(progressPaths, output, cleaned, clearedBeforePass, cleared, toolBoundPaths);
			CheckReportProgress(progressPaths);
			bad_engage_count = 0;
			engage.ResetPasses();
		}
		else
		{
			bad_engage_count++;
		}

		if (bad_engage_count > 10000)
		{
			cerr << "Break (next valid engage point not found)." << endl;
			break;
		}

		/*****NEXT ENGAGE POINT******/
		if (firstEngagePoint)
		{
			engage.moveToClosestPoint(newToolPos, stepScaled + 1);
			firstEngagePoint = false;
		}
		else
		{
			double moveDistance = ENGAGE_SCAN_DISTANCE_FACTOR * stepOverScaled * refinement_factor;

			if (!engage.nextEngagePoint(this, cleared, moveDistance,
										ENGAGE_AREA_THR_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR,
										4 * referenceCutArea * stepOverFactor))
			{
				// check if there are any uncleared area left
				Paths remaining;
				for (const auto &p : cleared.GetCleared())
				{
					if (!p.empty() && IsPointWithinCutRegion(toolBoundPaths, p.front()) && DistancePointToPathsSqrd(boundPaths, p.front(), clp, clpPathIndex, clpSegmentIndex, clpParamter) > 4 * toolRadiusScaled * toolRadiusScaled)
					{
						remaining.push_back(p);
					}
				};
				if (remaining.empty())
				{
					cout << "All cleared." << endl;
					break;
				}
				else
				{
					cout << "Clearing " << remaining.size() << " remaining internal path(s)." << endl;
				}

				// try to find new engage point along the remaining
				clipof.Clear();
				clipof.AddPaths(remaining, JoinType::jtRound, EndType::etClosedPolygon);
				clipof.Execute(remaining, toolRadiusScaled - 0.5 * stepOverScaled);

				ReversePaths(remaining);
				engage.SetPaths(remaining);
				engage.moveToClosestPoint(newToolPos, stepScaled + 1);
				if (!engage.nextEngagePoint(this, cleared, moveDistance,
											ENGAGE_AREA_THR_FACTOR * optimalCutAreaPD * RESOLUTION_FACTOR,
											4 * referenceCutArea * stepOverFactor))
					break;
			}
		}
		toolPos = engage.getCurrentPoint();
		toolDir = engage.getCurrentDir();
	}

	//**********************************
	//*  FINISHING PASS                *
	//**********************************

	Paths finishingPaths;
	clipof.Clear();
	clipof.AddPaths(boundPaths, JoinType::jtRound, EndType::etClosedPolygon);
	clipof.Execute(finishingPaths, -toolRadiusScaled);

	clipof.Clear();
	clipof.AddPaths(finishingPaths, JoinType::jtRound, EndType::etClosedPolygon);
	clipof.Execute(toolBoundPaths, -1);

	IntPoint lastPoint = toolPos;
	Path finShiftedPath;

	bool allCutsAllowed = true;
	while (!stopProcessing && PopPathWithClosestPoint(finishingPaths, lastPoint, finShiftedPath))
	{
		if (finShiftedPath.empty())
			continue;
		// skip finishing passes outside the stock boundary - no sense to cut where is no material
		bool allPointsOutside = true;
		IntPoint p1 = finShiftedPath.front();
		for (const auto &pt : finShiftedPath)
		{

			// midpoint
			if (IsPointWithinCutRegion(stockInputPaths, IntPoint((p1.X + pt.X) / 2, (p1.Y + pt.Y) / 2)))
			{
				allPointsOutside = false;
				break;
			}
			//current point
			if (IsPointWithinCutRegion(stockInputPaths, pt))
			{
				allPointsOutside = false;
				break;
			}

			p1 = pt;
		}
		if (allPointsOutside)
			continue;

		progressPaths.push_back(TPath());
		// show in progress cb
		for (auto &pt : finShiftedPath)
		{
			progressPaths.back().second.push_back(DPoint(double(pt.X) / scaleFactor, double(pt.Y) / scaleFactor));
		}

		if (!finShiftedPath.empty())
			finShiftedPath << finShiftedPath.front(); // make sure its closed

		Path finCleaned;
		CleanPath(finShiftedPath, finCleaned, FINISHING_CLEAN_PATH_TOLERANCE);

		// sanity check for finishing paths - check the area of finishing cut
		for (size_t i = 1; i < finCleaned.size(); i++)
		{
			if (!IsAllowedToCutTrough(finCleaned.at(i - 1), finCleaned.at(i), cleared, toolBoundPaths, 2.0, true))
			{
				allCutsAllowed = false;
			}
		}

		// make sure it's closed
		finCleaned.push_back(finCleaned.front());
		AppendToolPath(progressPaths, output, finCleaned, cleared, cleared, toolBoundPaths);

		cleared.ExpandCleared(finCleaned);

		if (!finCleaned.empty())
		{
			lastPoint.X = finCleaned.back().X;
			lastPoint.Y = finCleaned.back().Y;
		}
	}

	Path returnPath;
	returnPath << lastPoint;
	returnPath << entryPoint;
	output.ReturnMotionType = IsClearPath(returnPath, cleared) ? MotionType::mtLinkClear : MotionType::mtLinkNotClear;

// dump performance results
#ifdef DEV_MODE
	Perf_ProcessPolyNode.Stop();
	Perf_ProcessPolyNode.DumpResults();
	Perf_PointIterations.DumpResults();
	Perf_CalcCutAreaCirc.DumpResults();
	Perf_CalcCutAreaClip.DumpResults();
	Perf_NextEngagePoint.DumpResults();
	Perf_ExpandCleared.DumpResults();
	Perf_DistanceToBoundary.DumpResults();
	Perf_AppendToolPath.DumpResults();
	Perf_IsAllowedToCutTrough.DumpResults();
	Perf_IsClearPath.DumpResults();
#endif
	CheckReportProgress(progressPaths, true);
#ifdef DEV_MODE
	double duration = ((double)(clock() - start_clock)) / CLOCKS_PER_SEC;
	cout << "PolyNode perf:" << perf_total_len / double(scaleFactor) / duration << " mm/sec"
		 << " processed_points:" << total_points
		 << " output_points:" << total_output_points
		 << " total_iterations:" << total_iterations
		 << " iter_per_point:" << (double(total_iterations) / ((double(total_points) + 0.001)))
		 << " total_exceeded:" << total_exceeded << " (" << 100 * double(total_exceeded) / double(total_points) << "%)"
		 << endl;
#endif

	// warn about invalid paths being detected
	if (!allCutsAllowed)
	{
		cerr << "Warning: some cuts may be above optimal step-over. Please double check the results." << endl
			 << "Hint: try to modify accuracy and/or step-over." << endl;
	}

	results.push_back(output);
}

} // namespace AdaptivePath
