using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Numerics;
using System.Reflection;
using CommonUsage.Mathematics;

namespace CommonUsage.Geometries
{
    public class BezierCurve : AbstractGeometry
    {

        public BezierCurve(List<Vector2> controlPoints, int resolution = 100)
        {
            // Console.WriteLine($"BezierCurve1");
            // Console.WriteLine(string.Join("  ",controlPoints.Select(p=>$"{p.X:f2},{p.Y:f2}")));
            _controlPoints = controlPoints;
            _resolution = resolution;
            InitializeBezier();
        }

        public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine, bool visExtendedPart = false)
        {
            if (VisualizeOption.DrawAuxiliary)
                for (var i = 0; i < _controlPoints.Count - 1; ++i)
                {
                    processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1],
                        false, false, VisualizeOption.AuxiliaryColor));
                    if (i == 0) continue;
                    processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor));
                }

            for (var i = 0; i < _bezierPoints.Count - 1; ++i)
            {
                if (Direction == -1)
                {
                    processLine(new VisLine(_bezierPoints[i + 1], _bezierPoints[i],
                        false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
                }
                else
                {
                    processLine(new VisLine(_bezierPoints[i], _bezierPoints[i + 1],
                        false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
                }


            }
        }

        public (Vector2 Point, int Id) QueryPoint(Vector2 point)
        {
            var p = new Vector2();
            var id = -1;
            var bestDistance = float.MaxValue;

            var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList();

            void TryQuery(Dictionary<uint, List<(Vector2 Point, int Id)>> dict, List<uint> hashList)
            {
                foreach (var hash in hashList)
                {
                    if (!dict.TryGetValue(hash, out var ll)) continue;
                    foreach (var (q, qId) in ll)
                    {
                        var d = Vector2.Distance(q, point);
                        if (d < bestDistance)
                        {
                            p = q;
                            id = qId;
                            bestDistance = d;
                        }
                    }
                }
            }
            //map目前有bug,取消cpu占用也不严重，必要时候在优化
            // TryQuery(_pointsMappingSmall, hashes);
            //
            // if (id == -1)
            // {
            // 	hashes = _bias.Select(bb => CalculateHash(point, 1000, bb.X, bb.Y)).ToList();
            // 	TryQuery(_pointsMappingBig, hashes);
            // }

            if (id == -1)
            {
                // todo: improve the way to find closest point if mappings fail
                (p, id) = _bezierPoints.Select((p, i) => (p, i))
                    .OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First();
            }

            return (p, id);
        }

        public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
        {
            var (p, id) = QueryPoint(point);
            var tangent = _tangents[id];
            var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent);
            var next = fd > 0 ? id + 1 : id - 1;
            if (id == 0) next = 1;
            // Console.WriteLine($"id:{id} next:{next} tangent:{tangent} _tangents.Count:{_tangents.Count}");
            if (next > 0 && next < _tangents.Count)//线性插值
            {
                var (_, _, t) = CommonMath.Project2DLine(point, _bezierPoints[id], _bezierPoints[next]);
                var partial = t / Vector2.Distance(_bezierPoints[id], _bezierPoints[next]);
                if (partial >= 0 && partial <= 1)
                {
                    tangent = CommonMath.RoundTh(_tangents[id] +
                                                 partial * CommonMath.RoundTh(_tangents[next] - _tangents[id]));
                    if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5)
                        Console.WriteLine($"bezier tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
                }
                // else Console.WriteLine("bezier tangents bug");
            }
            return (lp, tangent, bias, fd + _sumDistances[id]);
        }

        public override float Length()
        {
            return _length;
        }

        public override float QueryCurvature(float position)
        {
            int id = _sumDistances.Count - 1;
            if (position <= 0) id = 0;
            else
            {
                for (int i = 1; i < _sumDistances.Count; i++)
                {
                    if (position > _sumDistances[i - 1] && position <= _sumDistances[i])
                    {
                        id = i;
                        break;
                    }
                }
            }

            var result = _curvatures[id];
            if (id > 0 && id < _sumDistances.Count - 1)//插值
            {
                var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]);
                if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id];
                else Console.WriteLine("bezier curvature bug");
            }

            return result;
        }

        public Vector3 QueryBezierPointsById(int id)
        {
            if (id < 0 || id > Resolution)
            {
                Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Resolution},id:{id}.");
                return new Vector3(0, 0, 0);
            }

            return new Vector3(_bezierPoints[id].X, _bezierPoints[id].Y, _tangents[id]);
        }

        public List<Vector2> ControlPoints => _controlPoints;

        public int Resolution => _resolution;
        /// <summary>
        /// 仅用于simple显示路径方向
        /// </summary>
        public int Direction = 1;
        public int Order => _order;

        // public List<float> Tangents => _tangents;

        public void UpdateControlPoint(int id, Vector2 point)
        {
            _controlPoints[id] = point;
            InitializeBezier();
        }

        public void AddControlPoint(int id, Vector2 point)
        {
            _controlPoints.Insert(id, point);
            InitializeBezier();
        }

        public void RemoveControlPoint(int id)
        {
            _controlPoints.RemoveAt(id);
            InitializeBezier();
        }

        public Vector2 GetMidPoint()
        {
            return _bezierPoints[(int)Math.Ceiling(_resolution / 2d)];
        }

        private void InitializeBezier()
        {
            _order = _controlPoints.Count - 1;
            // _bezierPoints = new List<Vector2>();
            var delta = 1.0f / _resolution;
            // for (int t = 0; t <= _resolution; t += 1)//下面循环算了，没必要先递归算一遍
            // 	_bezierPoints.Add(new Vector2(DeCasteljauX(_order, 0, t*delta), DeCasteljauY(_order, 0, t*delta)));
            var allPoints = new List<List<List<Vector2>>>();
            for (var i = 0; i < _order; i++)
            {
                var size = allPoints.Count;
                var morePoints = new List<List<Vector2>>();
                for (var j = 0; j < _order - i; j++)
                {
                    var points = new List<Vector2>();
                    for (int t = 0; t <= _resolution; t += 1)
                    {
                        float p0x;
                        float p1x;
                        float p0y;
                        float p1y;
                        var z = t;
                        if (size > 0)
                        {
                            p0x = allPoints[i - 1][j][z].X;
                            p1x = allPoints[i - 1][j + 1][z].X;
                            p0y = allPoints[i - 1][j][z].Y;
                            p1y = allPoints[i - 1][j + 1][z].Y;
                        }
                        else
                        {
                            p0x = _controlPoints[j].X;
                            p1x = _controlPoints[j + 1].X;
                            p0y = _controlPoints[j].Y;
                            p1y = _controlPoints[j + 1].Y;
                        }

                        var part = t * delta;
                        points.Add(new Vector2((1 - part) * p0x + part * p1x, (1 - part) * p0y + part * p1y));
                    }
                    morePoints.Add(points);
                }
                allPoints.Add(morePoints);
            }

            _bezierPoints = allPoints.Last().Last();
            _tangentInfo = allPoints;
            _tangents = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
            _curvatures = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
            var p2 = allPoints[Order - 2];
            for (var id = 0; id < _bezierPoints.Count; ++id)
            {
                _tangents[id] =
                    (float)(Math.Atan2(p2[1][id].Y - p2[0][id].Y, p2[1][id].X - p2[0][id].X) / Math.PI * 180);
                if (id != 0) _curvatures[id] = (float)((CommonMath.ThDiff(_tangents[id], _tangents[id - 1]) / 180 * Math.PI)
                    / (Vector2.Distance(_bezierPoints[id], _bezierPoints[id - 1]) / 1000));
            }
            // Console.WriteLine($"{string.Join("\n", _tangents.Select((val, i) => $"{i}: {val}"))}");
            _tangents[0] = _tangents[1]; // todo: here is temporary fix
            _curvatures[0] = _curvatures[1];
            for (var id = 1; id < _bezierPoints.Count - 1; ++id)//前移0.5
                _curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;

            _remainDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
            _sumDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
            for (var i = _bezierPoints.Count - 2; i >= 0; --i)
            {
                _remainDistances[i] =
                    _remainDistances[i + 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i + 1]);
            }
            for (var i = 1; i < _bezierPoints.Count; ++i)
            {
                _sumDistances[i] =
                    _sumDistances[i - 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i - 1]);
            }
            _length = _sumDistances.Last();

            _minX = _bezierPoints.Min(pp => pp.X);
            _minY = _bezierPoints.Min(pp => pp.Y);

            var tmpList = _bezierPoints.Select((point, index) => (point, index)).ToList();
            return;
            void GenerateGridMapping(ref Dictionary<uint, List<(Vector2 Point, int Id)>> dict, float gSize)
            {
                dict = new Dictionary<uint, List<(Vector2 Point, int Id)>>();
                foreach (var (point, index) in tmpList)
                {
                    var hash = CalculateHash(point, gSize);
                    if (dict.TryGetValue(hash, out var ll))
                        ll.Add((point, index));
                    else dict[hash] = new List<(Vector2 Point, int Id)>() { (point, index) };
                }
            }

            GenerateGridMapping(ref _pointsMappingSmall, 100);
            GenerateGridMapping(ref _pointsMappingBig, 1000);
        }

        private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0)
        {
            return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff));
        }

        private readonly List<(int X, int Y)> _bias = new()
        {
            new(-1, -1), new(-1, 0), new(-1, 1),
            new(0, -1), new(0, 0), new(0, 1),
            new(1, -1), new(1, 0), new(1, 1),
        };

        private float DeCasteljauX(int i, int j, float t)
        {
            if (i == 1)
                return (1 - t) * _controlPoints[j].X + t * _controlPoints[j + 1].X;
            return (1 - t) * DeCasteljauX(i - 1, j, t) + t * DeCasteljauX(i - 1, j + 1, t);
        }

        private float DeCasteljauY(int i, int j, float t)
        {
            if (i == 1)
                return (1 - t) * _controlPoints[j].Y + t * _controlPoints[j + 1].Y;
            return (1 - t) * DeCasteljauY(i - 1, j, t) + t * DeCasteljauY(i - 1, j + 1, t);
        }

        private int _order;
        private int _resolution;
        private List<Vector2> _controlPoints;
        private List<Vector2> _bezierPoints;
        private List<List<List<Vector2>>> _tangentInfo;
        private List<float> _tangents;
        private List<float> _remainDistances;
        private List<float> _sumDistances;
        private List<float> _curvatures;
        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
        private float _minX, _minY;
        private float _length;
    }
}