using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Numerics;
using System.Reflection;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace CommonUsage.Mathematics
{

    using T3 = Tuple<float, float, float>;
    using D3 = Tuple<double, double, double>;

    public class CommonMath
    {
	    public class PrimeEnumerator<T>
	    {
		    public PrimeEnumerator(List<T> items, Func<T, bool> process)
		    {
                _n = items.Count;
                _items = items;
                _process = process;

                foreach (var pNum in _primes)
                {
	                if (_n % pNum != 0)
	                {
		                _a = pNum;
		                _b = 11;
		                break;
	                }
                }
			}

		    public void Enumerate()
		    {
			    using (var enumerator = Get().GetEnumerator())
			    {
                    while (enumerator.MoveNext()) { }
			    }
		    }

		    private readonly int _n, _a, _b;
		    private readonly int[] _primes = new[] { 29, 23, 19, 17, 13 };
		    private List<T> _items;
            private readonly Func<T, bool> _process;

		    private IEnumerable<bool> Get()
		    {
			    for (var i = 0; i < _n; ++i)
			    {
				    var id = (i * _a + _b) % _n;
				    yield return _process(_items[id]);
			    }
			}
	    }

        private static IEnumerable<IEnumerable<T>> GetPermutationsInternal<T>(IEnumerable<T> list, int length)
        {
            if (length == 1) return list.Select(t => new T[] { t });

            return GetPermutationsInternal(list, length - 1)
                .SelectMany(t => list.Where(e => !t.Contains(e)),
                    (t1, t2) => t1.Concat(new T[] { t2 }));
        }

        /// <summary>
        /// 得到一组数据的所有排列。
        /// </summary>
        /// <typeparam name="T">元素数据类型</typeparam>
        /// <param name="list">所有待选元素</param>
        /// <param name="selectNum">所选出的元素数量</param>
        /// <returns></returns>
        public static List<List<T>> GetPermutations<T>(List<T> list, int selectNum)
        {
            return GetPermutationsInternal(list, selectNum).Select(ll => ll.ToList()).ToList();
        }

        public static (float bias, Vector2 hPnt, float d) Project2DLine(Vector2 pnt, Vector2 segSt,
            Vector2 segEnd)
        {
            var dir = Vector2.Normalize(segEnd - segSt);
            var fd = Vector2.Dot(pnt - segSt, dir);
            var hPnt = segSt + fd * dir;
            var bias = dir.X * (pnt.Y-segSt.Y) - (pnt.X-segSt.X) * dir.Y;
            return (bias, hPnt, fd);
        }

        public static (float bias, Vector2 hPnt, float fd) Project2DLine(Vector2 pnt, Vector2 segSt, float tangent)
        {
	        var dir = new Vector2((float)System.Math.Cos(tangent / 180 * System.Math.PI), (float)System.Math.Sin(tangent / 180 * System.Math.PI));
	        var fd = Vector2.Dot(pnt - segSt, dir);
	        var hPnt = segSt + fd * dir;
	        var bias = dir.X * (pnt.Y - segSt.Y) - (pnt.X - segSt.X) * dir.Y;
	        return (bias, hPnt, fd);
		}

        public class LineEqu
        {
            public double A, B, C, ln, dAB;
            public double px1, px2, py1, py2;
            public float midX;
            public float midY;
        }
        // Fit line with PCA.
        public LineEqu CalcLine(IEnumerable<Vector2> tls)
        {
            var lidarPoint2Ds = tls as Vector2[] ?? tls.ToArray();
            float fx = lidarPoint2Ds.Average(f => f.X);
            float fy = lidarPoint2Ds.Average(f => f.Y);
            float fxx = lidarPoint2Ds.Average(f => f.X * f.X);
            float fxy = lidarPoint2Ds.Average(f => f.X * f.Y);
            float fyy = lidarPoint2Ds.Average(f => f.Y * f.Y);
            float a = fxx - fx * fx, b = fxy - fx * fy, c = fyy - fy * fy;
            double sqt = System.Math.Sqrt((a - c) * (a - c) + 4 * b * b);
            double l1 = a + c + sqt;
            double l2 = a + c - sqt;
            double dx, dy;
            if (System.Math.Abs(a - l1 / 2) > System.Math.Abs(c - l1 / 2))
            {
                dy = l1 / 2 - a; dx = b;
            }
            else
            {
                dx = l1 / 2 - c; dy = b;
            }
            double norm = System.Math.Sqrt(dx * dx + dy * dy);
            dx /= norm; dy /= norm;
            double A = dy, B = -dx, C = dx * fy - dy * fx;
            double dAB = System.Math.Sqrt(A * A + B * B);

            return new CommonMath.LineEqu
            {
                A = A,
                B = B,
                C = C,
                ln = lidarPoint2Ds.Average(p => System.Math.Abs(p.X * A + p.Y * B + C) / dAB),
                midX = fx,
                midY = fy
            };
        }

        public static double QuadInterp3(double[] confsF)
        {
            if (confsF[0] > confsF[1] && confsF[0] > confsF[2])
            {
                //printf("left overflow...\n");
                return -1;
            }

            if (confsF[1] > confsF[0] && confsF[1] > confsF[2])
            {
                return (-(confsF[2] - confsF[0]) / 2.0f / (confsF[0] + confsF[2] - 2.0f * confsF[1] + 0.0001f));
            }
            if (confsF[2] > confsF[0] && confsF[2] > confsF[1])
            {
                //printf("right overflow...\n");
                return 1;
            }
            return 0;
        }

        public static double cross(PointF O, PointF A, PointF B)
        {
            return (A.X - O.X) * (B.Y - O.Y) - (A.Y - O.Y) * (B.X - O.X);
        }

        public static List<PointF> GetConvexHull(List<PointF> points)
        {
            if (points == null)
                return null;

            if (points.Count() <= 1)
                return points;

            int n = points.Count(), k = 0;
            List<PointF> H = new List<PointF>(new PointF[2 * n]);

            points.Sort((a, b) =>
                a.X == b.X ? a.Y.CompareTo(b.Y) : a.X.CompareTo(b.X));

            // Build lower hull
            for (int i = 0; i < n; ++i)
            {
                while (k >= 2 && cross(H[k - 2], H[k - 1], points[i]) <= 0)
                    k--;
                H[k++] = points[i];
            }

            // Build upper hull
            for (int i = n - 2, t = k + 1; i >= 0; i--)
            {
                while (k >= t && cross(H[k - 2], H[k - 1], points[i]) <= 0)
                    k--;
                H[k++] = points[i];
            }

            return H.Take(k - 1).ToList();
        }

        public static bool IsPointInPolygon4(PointF[] polygon, PointF testPoint)
        {
            // ray casting odd even test.
            bool result = false;
            int j = polygon.Count() - 1;
            for (int i = 0; i < polygon.Count(); i++)
            {
                if (polygon[i].Y < testPoint.Y && polygon[j].Y >= testPoint.Y ||
                    polygon[j].Y < testPoint.Y && polygon[i].Y >= testPoint.Y)
                {
                    if (polygon[i].X + (testPoint.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) *
                        (polygon[j].X - polygon[i].X) < testPoint.X)
                    {
                        result = !result;
                    }
                }

                j = i;
            }

            return result;
        }

        public static double Exp(double val)
        {
            if (val < -20) return 0.0000001;
            if (val > 20) return 99999999999999;
            long tmp = (long)(1512775 * val + 1072632447);
            return BitConverter.Int64BitsToDouble(tmp << 32);
        }
        public static double gaussmf(double x, double sig, double c)
        {
            return Exp(-(x - c) * (x - c) / (2 * sig * sig));
        }

        public static float Exp(float x)
        {
            if (x < -10) return 0;
            if (x > 10) return 99999999999999;
            x = 1.0f + x / 64f;
            x *= x;
            x *= x;
            x *= x;
            x *= x;
            x *= x;
            x *= x;
            return x;
        }
        public static float gaussmf(float x, float sig, float c)
        {
            return Exp(-(x - c) * (x - c) / (2 * sig * sig));
        }

        public static D3 Transform2D(D3 src, D3 t)
        {
            var rth = src.Item3 / 180.0 * System.Math.PI;
            var p1dtx = (src.Item1 + System.Math.Cos(rth) * t.Item1 -
                                System.Math.Sin(rth) * t.Item2);
            var p1dty = (src.Item2 + System.Math.Sin(rth) * t.Item1 +
                                System.Math.Cos(rth) * t.Item2);
            var p1dtth = src.Item3 + t.Item3;
            return Tuple.Create(p1dtx, p1dty, p1dtth);
        }

        public struct LngLatToXY
        {
            public double scale;
            public double rad;
            public double biasX, biasY;
        }

        public static LngLatToXY GetTransformLngLatToXY(Vector2 lnglat1, Vector2 xy1, Vector2 lnglat2, Vector2 xy2)
        {
            var scale = (xy1 - xy2).Length() / (lnglat1 - lnglat2).Length();
            var dxy = (xy1 - xy2);
            var dlnglat = lnglat1 - lnglat2;
            var rad = System.Math.Atan2(dxy.X, dxy.Y) - System.Math.Atan2(dlnglat.X, dlnglat.Y);
            var intm = lnglat1 * scale;
            var biasX = xy1.X - (intm.X * System.Math.Cos(rad) - intm.Y * System.Math.Sin(rad));
            var biasY = xy1.Y - (intm.X * System.Math.Sin(rad) + intm.Y * System.Math.Cos(rad));
            return new LngLatToXY {rad = rad, biasX = biasX, biasY = biasY, scale = scale};
        }

        public Vector2 TransformLngLatToXY(Vector2 lnglat, LngLatToXY t)
        {
            var intm = lnglat * (float) t.scale;
            return new Vector2((float) (intm.X * System.Math.Cos(t.rad) - intm.Y * System.Math.Sin(t.rad) + t.biasX),
                (float) (intm.X * System.Math.Sin(t.rad) + intm.Y * System.Math.Cos(t.rad) + t.biasY));
        }

        public static D3 ReverseTransform(D3 dest, D3 t)
        {
            var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
            var nxT = (dest.Item1 - System.Math.Cos(rth) * t.Item1 +
                              System.Math.Sin(rth) * t.Item2);
            var nyT = (dest.Item2 - System.Math.Sin(rth) * t.Item1 -
                              System.Math.Cos(rth) * t.Item2);
            var pth = dest.Item3 - t.Item3;
            return Tuple.Create(nxT, nyT, pth);
        }

        public static D3 SolveTransform2D(D3 src, D3 dest)
        {
            var th = dest.Item3 - src.Item3;
            th = (th - System.Math.Round((th) / 360.0f) * 360);
            var rth = src.Item3 / 180.0 * System.Math.PI;
            var x = ((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
                            (dest.Item2 - src.Item2) * System.Math.Sin(rth));
            var y = (-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
                            (dest.Item2 - src.Item2) * System.Math.Cos(rth));
            return Tuple.Create(x, y, th);
        }

        public static T3 Transform2D(T3 src, T3 t)
        {
            var rth = src.Item3 / 180.0 * System.Math.PI;
            var p1dtx = (float)(src.Item1 + System.Math.Cos(rth) * t.Item1 -
                                System.Math.Sin(rth) * t.Item2);
            var p1dty = (float)(src.Item2 + System.Math.Sin(rth) * t.Item1 +
                                System.Math.Cos(rth) * t.Item2);
            var p1dtth = src.Item3 + t.Item3;
            return Tuple.Create(p1dtx, p1dty, p1dtth);
        }

        public static Vector2 Transform2D(Vector3 src, Vector3 t)
        {
	        var tup = Transform2D(Tuple.Create(src.X, src.Y, src.Z), Tuple.Create(t.X, t.Y, t.Z));
	        return new Vector2(tup.Item1, tup.Item2);
        }

        public static Vector2 Transform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
        {
	        var tup = Transform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
	        return new Vector2(tup.Item1, tup.Item2);
		}

		public static T3 ReverseTransform(T3 dest, T3 t)
        {
            var rth = (dest.Item3 - t.Item3) / 180.0 * System.Math.PI;
            var nxT = (float)(dest.Item1 - System.Math.Cos(rth) * t.Item1 +
                              System.Math.Sin(rth) * t.Item2);
            var nyT = (float)(dest.Item2 - System.Math.Sin(rth) * t.Item1 -
                              System.Math.Cos(rth) * t.Item2);
            var pth = dest.Item3 - t.Item3;
            return Tuple.Create(nxT, nyT, pth);
        }

        public static T3 SolveTransform2D(T3 src, T3 dest)
        {
            var th = dest.Item3 - src.Item3;
            th = (float)(th - System.Math.Round((th) / 360.0f) * 360);
            var rth = src.Item3 / 180.0 * System.Math.PI;
            var x = (float)((dest.Item1 - src.Item1) * System.Math.Cos(rth) +
                            (dest.Item2 - src.Item2) * System.Math.Sin(rth));
            var y = (float)(-(dest.Item1 - src.Item1) * System.Math.Sin(rth) +
                            (dest.Item2 - src.Item2) * System.Math.Cos(rth));
            return Tuple.Create(x, y, th);
        }

		public static Vector2 SolveTransform2D(Vector2 srcPos, float srcTh, Vector2 dt, float dth = 0)
		{
			var tup = SolveTransform2D(Tuple.Create(srcPos.X, srcPos.Y, srcTh), Tuple.Create(dt.X, dt.Y, dth));
			return new Vector2(tup.Item1, tup.Item2);
		}

		public static double dist(double x1, double y1, double x2, double y2)
        {
            return System.Math.Sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
        }

        [StructLayout(LayoutKind.Explicit)]
        private struct FloatIntUnion
        {
            [FieldOffset(0)] public float f;

            [FieldOffset(0)] public int tmp;
        }

        public static float Sqrt(float z)
        {
            FloatIntUnion u;
            u.tmp = 0;
            u.f = z;
            u.tmp -= 1 << 23; /* Subtract 2^m. */
            u.tmp >>= 1; /* Divide by 2. */
            u.tmp += 1 << 29; /* Add ((b + 1) / 2) * 2^m. */
            return u.f;
        }

        public static float dist2(float x1, float y1, float x2, float y2)
        {
            return ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
        }


        public static float d2(float x1, float y1, float x2, float y2)
        {
            return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2);
        }

        public static float ThAverage(List<float> angles)
        {
	        var anchor = angles[0];
	        var diff = 0f;
	        foreach (var angle in angles)
		        diff += ThDiff(angle, anchor);
	        return RoundTh(anchor + diff / angles.Count);
        }

        public static float ThDiff(float th1, float th2)
        {
            return (float)(th1 - th2 -
                           System.Math.Round((th1 - th2) / 360.0f) * 360);
        }

        public static double ThDiff(double th1, double th2)
        {
            return th1 - th2 -
                           System.Math.Round((th1 - th2) / 360.0f) * 360;
        }

        public static double refine(double x)
        {
            if (x < 1 && x > -1) return x;
            if (x > 1)
                return (2 / (1 + System.Math.Exp(-((x - 1) * 2))));
            return (2 / (1 + System.Math.Exp(-((x + 1) * 2)))) - 2;
        }

        /// <summary>
        /// 求点p到两点式直线p1p2的距离
        /// </summary>
        /// <param name="x">点p的x坐标</param>
        /// <param name="y">点p的y坐标</param>
        /// <param name="x1">直线点p1的x坐标</param>
        /// <param name="y1">直线点p1的y坐标</param>
        /// <param name="x2">直线点p2的x坐标</param>
        /// <param name="y2">直线点p2的y坐标</param>
        /// <returns></returns>
        public static double Point2LineDist(double x, double y, double x1, double y1, double x2, double y2)
        {
            double a1 = -(y1 - y2) / 10;
            double b1 = (x1 - x2) / 10;
            double c1 = (x1 * (y1 - y2) - y1 * (x1 - x2)) / 10;
            return System.Math.Abs(a1 * x + b1 * y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
        }

        public static double Point2LineDist(Vector2 p, LineSegment ll)
        {
            double a1 = -(ll.Src.Y - ll.Dst.Y) / 10;
            double b1 = (ll.Src.X - ll.Dst.X) / 10;
            double c1 = (ll.Src.X * (ll.Src.Y - ll.Dst.Y) - ll.Src.Y * (ll.Src.X - ll.Dst.X)) / 10;
            return System.Math.Abs(a1 * p.X + b1 * p.Y + c1) / System.Math.Sqrt(a1 * a1 + b1 * b1);
        }

        /// <summary>
        /// 两条两点式直线间的夹角
        /// </summary>
        /// <param name="x1"></param>
        /// <param name="y1"></param>
        /// <param name="x2"></param>
        /// <param name="y2"></param>
        /// <param name="x3"></param>
        /// <param name="y3"></param>
        /// <param name="x4"></param>
        /// <param name="y4"></param>
        /// <returns>角度制</returns>
        public static double AngleBetweenLines(double x1, double y1, double x2, double y2, double x3, double y3,
            double x4, double y4)
        {
            var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
            var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
            return System.Math.Acos(System.Math.Abs(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length())) / System.Math.PI * 180;
        }

        public static double AngleBetweenLines(LineSegment ls1, LineSegment ls2)
        {
	        return AngleBetweenLines(ls1.Src.X, ls1.Src.Y, ls1.Dst.X, ls1.Dst.Y, ls2.Src.X, ls2.Src.Y, ls2.Dst.X,
		        ls2.Dst.Y);
        }

		/// <summary>
		/// 两向量间夹角
		/// </summary>
		/// <param name="x1"></param>
		/// <param name="y1"></param>
		/// <param name="x2"></param>
		/// <param name="y2"></param>
		/// <param name="x3"></param>
		/// <param name="y3"></param>
		/// <param name="x4"></param>
		/// <param name="y4"></param>
		/// <returns>角度制</returns>
		public static double AngleBetweenVectors(double x1, double y1, double x2, double y2, double x3, double y3,
            double x4, double y4)
        {
            var vec1 = new Vector2((float)(x2 - x1), (float)(y2 - y1));
            var vec2 = new Vector2((float)(x4 - x3), (float)(y4 - y3));
            return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
        }

        /// <summary>
        /// 两向量间夹角.
        /// </summary>
        /// <param name="vec1"></param>
        /// <param name="vec2"></param>
        /// <returns>角度制</returns>
        public static double AngleBetweenVectors(Vector2 vec1, Vector2 vec2)
        {
            return System.Math.Acos(Vector2.Dot(vec1, vec2) / vec1.Length() / vec2.Length()) / System.Math.PI * 180;
        }

        public static double AngleBetweenVectors(Vector3 vector1, Vector3 vector2)
        {
	        float dotProduct = Vector3.Dot(vector1, vector2);
	        float magnitude1 = vector1.Length();
	        float magnitude2 = vector2.Length();
	        float cosine = dotProduct / (magnitude1 * magnitude2);

	        return System.Math.Acos(cosine) / System.Math.PI * 180;
        }

		/// <summary>
		/// 求点到直线的垂足
		/// </summary>
		/// <param name="x"></param>
		/// <param name="y"></param>
		/// <param name="x1"></param>
		/// <param name="y1"></param>
		/// <param name="x2"></param>
		/// <param name="y2"></param>
		/// <returns></returns>
		public static (double, double) PerpendicularPoint(double x, double y, double x1, double y1, double x2, double y2)
        {
            double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
            var u = ((x - x1) * lx + (y - y1) * ly) / dAB;
            return new(x1 + u * lx, y1 + u * ly);
        }

        public static Vector2 PerpendicularPoint(Vector2 p, LineSegment ls)
        {
            double lx = ls.Dst.X - ls.Src.X, ly = ls.Dst.Y - ls.Src.Y, dAB = lx * lx + ly * ly;
            var u = ((p.X - ls.Src.X) * lx + (p.Y - ls.Src.Y) * ly) / dAB;
            return new Vector2((float)(ls.Src.X + u * lx), (float)(ls.Src.Y + u * ly));
        }

        public static double PerpendicularPosition(double x, double y, double x1, double y1, double x2, double y2)
        {
            double lx = x2 - x1, ly = y2 - y1;
            var dAB = CommonMath.Sqrt((float)(lx * lx + ly * ly));
            lx /= dAB;
            ly /= dAB;
            return (x - x1) * lx + (y - y1) * ly;
        }


        /// <summary>
        /// 最小二乘法拟合直线，得到两点式。
        /// </summary>
        /// <param name="pts">待拟合的点集，应至少有2个点。</param>
        /// <param name="maxDist2Line">检查是否所有点距直线的距离均小于maxDist2Line，若为-1则不检查。</param>
        /// <returns>返回两点式的两个端点坐标。若坐标为全0，则拟合失败。</returns>
        public static (bool, Vector2, Vector2) FitLineSegment(List<Vector2> pts, double maxDist2Line = -1)
        {
            if (pts.Count < 2)
            {
                Console.WriteLine($"Points too Few! {pts.Count}! Cannot perform line fitting!",
                    MethodBase.GetCurrentMethod()?.Name ?? "FitLine");
                return (false, Vector2.Zero, Vector2.Zero);
            };

            // y = kx + b
            double A = 0, B = 0, C = 0, D = 0;
            foreach (var p in pts)
            {
                A += p.X * p.X;
                B += p.X;
                C += p.X * p.Y;
                D += p.Y;
            }

            var tmp = A * pts.Count - B * B;
            var k = (C * pts.Count - B * D) / tmp;
            var b = (A * D - C * B) / tmp;

            double x1 = 0,
                y1 = k * x1 + b,
                x2 = 1000,
                y2 = k * x2 + b;

            double CalcDist(ref bool fail, ref Vector2 endP, ref Vector2 endQ)
            {
                double distSum = 0;
                double lx = x2 - x1, ly = y2 - y1, dAB = lx * lx + ly * ly;
                double minU = double.MaxValue, maxU = double.MinValue;

                foreach (var p in pts)
                {
                    var u = ((p.X - x1) * lx + (p.Y - y1) * ly) / dAB;
                    var perp = new Vector2((float)(x1 + u * lx), (float)(y1 + u * ly));
                    if (u < minU)
                    {
                        endP = perp;
                        minU = u;
                    }
                    if (u > maxU)
                    {
                        endQ = perp;
                        maxU = u;
                    }

                    var curDist = dist(perp.X, perp.Y, p.X, p.Y);
                    if (maxDist2Line > -1 && curDist > maxDist2Line) fail = true;
                    distSum += curDist;
                }

                return distSum;
            }

            var kbFail = false;
            Vector2 endP1 = new Vector2(), endQ1 = new Vector2();
            double kbDist = CalcDist(ref kbFail, ref endP1, ref endQ1);
            
            // x = my + n
            A = 0;
            B = 0;
            C = 0;
            D = 0;
            foreach (var p in pts)
            {
                A += p.X * p.Y;
                B += p.Y * p.Y;
                C += p.Y;
                D += p.X;
            }

            tmp = C * C - B * pts.Count;
            var m = (C * D - A * pts.Count) / tmp;
            var n = (A * C - B * D) / tmp;
            y1 = 0;
            x1 = m * y1 + n;
            y2 = 1000;
            x2 = m * y2 + n;

            var mnFail = false;
            Vector2 endP2 = new Vector2(), endQ2 = new Vector2();
            double mnDist = CalcDist(ref mnFail, ref endP2, ref endQ2);

            Vector2 endP = endP1, endQ = endQ1;
            if (mnDist < kbDist)
            {
                if (mnFail) return (false, new Vector2(), new Vector2());
                endP = endP2;
                endQ = endQ2;
            }
            else if (kbFail) return (false, new Vector2(), new Vector2());

            return (true, endP, endQ);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static int toId(int x, int y, int z)
        {
            return (x * 1140671485 + 12820163) ^ (y * 134775813 + 1) ^ (z * 1103515245 + 12345);
        }

        public class Clustering<T>
        {
	        public int numIteration = 3;
	        public int itemNumThreshold = 10;

	        public Func<T, T, bool> inRange;
	        public Func<List<T>, T> average;
	        private readonly List<T> _inputData;
	        private Dictionary<T, List<T>> _clusters = new Dictionary<T, List<T>>();

	        public Clustering(List<T> data, Func<T, T, bool> inRange, Func<List<T>, T> average)
	        {
		        _inputData = data;
		        this.inRange = inRange;
		        this.average = average;
	        }

	        public Dictionary<T, List<T>> GetClusters()
	        {
		        var tmp = new List<(T center, List<T> items)>();

		        for (var iter = 0; iter < numIteration; iter++)
		        {
			        tmp = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
				        .Select(cluster => (average(cluster.items), new List<T>())).ToList();

			        foreach (var data in _inputData)
			        {
				        var added = false;
				        foreach (var cluster in tmp)
				        {
					        if (inRange(cluster.center, data))
					        {
						        cluster.items.Add(data);
						        added = true;
						        break;
					        }
				        }

				        if (!added)
					        tmp.Add((data, new List<T>() { data }));
			        }
		        }

		        _clusters = tmp.Where(cluster => cluster.items.Count > itemNumThreshold)
			        .ToDictionary(cluster => cluster.center, cluster => cluster.items);
		        return _clusters;
	        }
        }

        public static (bool, Vector2) TwoLinesIntersection(Vector2 A, Vector2 B, Vector2 C, Vector2 D)
        {
	        // Line AB represented as a1x + b1y = c1
	        double a1 = B.Y - A.Y;
	        double b1 = A.X - B.X;
	        double c1 = a1 * (A.X) + b1 * (A.Y);

	        // Line CD represented as a2x + b2y = c2
	        double a2 = D.Y - C.Y;
	        double b2 = C.X - D.X;
	        double c2 = a2 * (C.X) + b2 * (C.Y);

	        double determinant = a1 * b2 - a2 * b1;

	        if (determinant == 0)
	        {
		        // The lines are parallel. This is simplified
		        // by returning a pair of FLT_MAX
		        return new(false, new Vector2());
	        }
	        else
	        {
		        double x = (b2 * c1 - b1 * c2) / determinant;
		        double y = (a1 * c2 - a2 * c1) / determinant;
		        return (true, new Vector2((float)x, (float)y));
	        }
        }

        public static bool IsAtLeft(Vector2 anchor, Vector2 dest, Vector2 p)
        {
	        var v1 = new Vector3(anchor - p, 0);
	        var v2 = new Vector3(dest - p, 0);
	        return Vector3.Cross(v1, v2).Z > 0;
        }

        /// <summary>
        /// 将角度转化至-180到180度的范围内。
        /// </summary>
        /// <param name="th"></param>
        /// <returns></returns>
        public static double RoundTh(double th)
        {
	        return th - System.Math.Round(th / 360) * 360;
        }

        /// <summary>
        /// 将角度转化至-180到180度的范围内。
        /// </summary>
        /// <param name="th"></param>
        /// <returns></returns>
        public static float RoundTh(float th)
        {
	        return th - (float)System.Math.Round(th / 360f) * 360f;
        }
	}
}