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

namespace CommonUsage.Geometries
{
    public class NurbsCurve : AbstractGeometry
    {

        public NurbsCurve(List<Vector2> controlPoints, List<float> weights, List<float> knotVector, int frame = 100)
        {
            _controlPoints = controlPoints;
            _weights = weights;
            _knotVector = knotVector;
            _frame = frame;
            InitializeNurbs();
        }

        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 < _nurbsPoints.Count - 1; ++i)
            {
                if (Direction == -1)
                {
                    processLine(new VisLine(_nurbsPoints[i + 1], _nurbsPoints[i],
                        false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2));
                }
                else
                {
                    processLine(new VisLine(_nurbsPoints[i], _nurbsPoints[i + 1],
                        false, i == (int)(_nurbsPoints.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;
                        }
                    }
                }
            }

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

            return (p, id);
        }

        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),
        };

        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 (next > 0 && next < _tangents.Count)//线性插值
            {
                var (_, _, t) = CommonMath.Project2DLine(point, _nurbsPoints[id], _nurbsPoints[next]);
                var partial = t / Vector2.Distance(_nurbsPoints[id], _nurbsPoints[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($"Nurbs tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
                }
                else Console.WriteLine("Nurbs tangents bug");
            }
            return (lp, tangent, bias, fd + _sumDistances[id]);
        }

        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("Nurbs curvature bug");
            }

            return result;
        }

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

            return new Vector3(_nurbsPoints[id].X, _nurbsPoints[id].Y, _tangents[id]);
        }
        public override float Length()
        {
            return _length;
        }

        public int Order => _order;
        public List<Vector2> ControlPoints => _controlPoints;
        public List<float> Weights => _weights;
        public List<float> KnotVector => _knotVector;
        public int Frame => _frame;

        public int Direction = 1;


        public void UpdateControlPoint(int id, Vector2 point)
        {
            _controlPoints[id] = point;
            InitializeNurbs();
        }
        public void UpdateNurbsWeihgts(int id, float weight)
        {
            _weights[id] = weight;
            InitializeNurbs();

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

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

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

        private void InitializeNurbs()
        {
            _order = _controlPoints.Count - 1;
            List<List<Vector2>> allpoints = new List<List<Vector2>>();
            List<Vector2> nurbsCurvePoints = new List<Vector2>();
            float delta = 1.0f / Frame;

            for (float t = 0; t <= 1; t += delta)
            {
                var (point, tangent) = DeBoorAlgorithm(t);
                var points = new List<Vector2>
                {
                    point,
                    point + tangent // Tangent endpoint
                };
                allpoints.Add(points);
                nurbsCurvePoints.Add(point); // Store the curve point separately
            }

            _nurbsPoints = nurbsCurvePoints;
            _tangents = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
            _curvatures = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();

            for (var id = 0; id < _nurbsPoints.Count - 1; ++id)
            {
                Vector2 p1 = _nurbsPoints[id];
                Vector2 p2 = _nurbsPoints[id + 1];

                float tangentAngle = (float)Math.Atan2(p2.Y - p1.Y, p2.X - p1.X) * 180 / (float)Math.PI;
                _tangents[id] = tangentAngle;

                // Calculate curvature using finite differences of tangent (second derivative approximation)
                if (id > 0)
                {
                    float previousTangent = _tangents[id - 1];
                    float curvature = (float)(CommonMath.ThDiff(tangentAngle, previousTangent) * Math.PI / 180) /
                                      (Vector2.Distance(p1, p2) / 1000);
                    _curvatures[id] = curvature;
                }
            }

            _curvatures.Insert(0, _curvatures[0]);
            for (var id = 1; id < _curvatures.Count - 1; ++id)
            {
                _curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;
            }

            _remainDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
            _sumDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
            _remainDistances[_nurbsPoints.Count - 1] = 0;

            for (var i = _nurbsPoints.Count - 2; i >= 0; --i)
            {
                _remainDistances[i] = _remainDistances[i + 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i + 1]);
            }

            _sumDistances[0] = 0;
            for (var i = 1; i < _nurbsPoints.Count; ++i)
            {
                _sumDistances[i] = _sumDistances[i - 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i - 1]);
            }

            _length = _sumDistances.Last();

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

            var tmpList = _nurbsPoints.Select((point, index) => (point, index)).ToList();
            // GenerateGridMapping(ref _pointsMappingSmall, 100, tmpList);
            // GenerateGridMapping(ref _pointsMappingBig, 1000, tmpList);
        }


       

        private float CalculateLength()
        {
            return _nurbsPoints.Zip(_nurbsPoints.Skip(1), Vector2.Distance).Sum();
        }

        private (Vector2, Vector2) DeBoorAlgorithm(float t)
        {
            Vector2 numerator = Vector2.Zero;
            Vector2 tangentNumerator = Vector2.Zero;
            float denominator = 0f;

            // Calculate the point on the curve
            for (int i = 0; i < ControlPoints.Count; ++i)
            {
                float basis = BasisFunction(i, _order, t) * Weights[i];
                numerator += basis * ControlPoints[i];
                denominator += basis;
            }

            Vector2 point = numerator / denominator;

            // Calculate the tangent vector using the analytical derivative
            for (int i = 0; i < ControlPoints.Count; ++i)
            {
                float basisDerivative = BasisFunctionDerivative(i, _order, t) * Weights[i];
                tangentNumerator += basisDerivative * ControlPoints[i];
            }
            Vector2 tangent = tangentNumerator / denominator;

            return (point, tangent);
        }

        private float BasisFunction(int i, int p, float t)
        {
            if (p == 0)
                return (KnotVector[i] <= t && t < KnotVector[i + 1]) ? 1.0f : 0.0f;

            float denom1 = KnotVector[i + p] - KnotVector[i];
            float term1 = denom1 == 0 ? 0 : ((t - KnotVector[i]) / denom1) * BasisFunction(i, p - 1, t);

            float denom2 = KnotVector[i + p + 1] - KnotVector[i + 1];
            float term2 = denom2 == 0 ? 0 : ((KnotVector[i + p + 1] - t) / denom2) * BasisFunction(i + 1, p - 1, t);

            return term1 + term2;
        }

        private float BasisFunctionDerivative(int i, int k, float t)
        {
            if (k == 0) return 0;

            float denom1 = KnotVector[i + k] - KnotVector[i];
            float denom2 = KnotVector[i + k + 1] - KnotVector[i + 1];

            float term1 = denom1 != 0 ? BasisFunction(i, k - 1, t) / denom1 : 0;
            float term2 = denom1 != 0 ? (t - KnotVector[i]) * BasisFunctionDerivative(i, k - 1, t) / denom1 : 0;

            float term3 = denom2 != 0 ? -BasisFunction(i + 1, k - 1, t) / denom2 : 0;
            float term4 = denom2 != 0 ? (KnotVector[i + k + 1] - t) * BasisFunctionDerivative(i + 1, k - 1, t) / denom2 : 0;

            return term1 + term2 + term3 + term4;
        }
       
        private int _order;
        private List<Vector2> _controlPoints;
        private List<float> _weights;
        private List<float> _knotVector;
        private int _frame;
        private List<Vector2> _nurbsPoints;
        private List<List<List<Vector2>>> _tangentPoints;
        private List<float> _curvatures;
        private List<float> _sumDistances;
        private List<float> _remainDistances;
        private float _minX, _minY;
        private float _length;
        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
        private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
        private List<float> _tangents;

    }
}