using UnityEngine;
using Unity.Profiling;
using System;
using System.Collections;
using System.Collections.Generic;

namespace Obi
{
    [ExecuteInEditMode]
    [RequireComponent(typeof(ObiRopeBase))]
    public class ObiPathSmoother : MonoBehaviour
    {
        static ProfilerMarker m_AllocateRawChunksPerfMarker = new ProfilerMarker("AllocateRawChunks");
        static ProfilerMarker m_GenerateSmoothChunksPerfMarker = new ProfilerMarker("GenerateSmoothChunks");

        private Matrix4x4 w2l;
        private Quaternion w2lRotation;

        [Range(0, 1)]
        [Tooltip("Curvature threshold below which the path will be decimated. A value of 0 won't apply any decimation. As you increase the value, decimation will become more aggresive.")]
        public float decimation = 0;

        [Range(0, 3)]
        [Tooltip("Smoothing iterations applied to the path. A smoothing value of 0 won't perform any smoothing at all. Note that smoothing is applied after decimation.")]
        public uint smoothing = 0;

        [Tooltip("Twist in degrees applied to each sucessive path section.")]
        public float twist = 0;

        public event ObiActor.ActorCallback OnCurveGenerated;

        protected float smoothLength = 0;
        protected int smoothSections = 0;

        [HideInInspector] public ObiList<ObiList<ObiPathFrame>> rawChunks = new ObiList<ObiList<ObiPathFrame>>();
        [HideInInspector] public ObiList<ObiList<ObiPathFrame>> smoothChunks = new ObiList<ObiList<ObiPathFrame>>();
        private Stack<Vector2Int> stack = new Stack<Vector2Int>();
        private BitArray decimateBitArray = new BitArray(0);

        public float SmoothLength
        {
            get { return smoothLength; }
        }

        public float SmoothSections
        {
            get { return smoothSections; }
        }

        private void OnEnable()
        {
            GetComponent<ObiRopeBase>().OnInterpolate += Actor_OnInterpolate;
        }

        private void OnDisable()
        {
            GetComponent<ObiRopeBase>().OnInterpolate -= Actor_OnInterpolate;
        }

        void Actor_OnInterpolate(ObiActor actor)
        {
            GenerateSmoothChunks(((ObiRopeBase)actor), smoothing);

            if (OnCurveGenerated != null)
                OnCurveGenerated(actor);
        }

        private void AllocateChunk(int sections)
        {
            if (sections > 1)
            {

                if (rawChunks.Data[rawChunks.Count] == null)
                {
                    rawChunks.Data[rawChunks.Count] = new ObiList<ObiPathFrame>();
                    smoothChunks.Data[smoothChunks.Count] = new ObiList<ObiPathFrame>();
                }

                rawChunks.Data[rawChunks.Count].SetCount(sections);

                rawChunks.SetCount(rawChunks.Count + 1);
                smoothChunks.SetCount(smoothChunks.Count + 1);
            }
        }

        private float CalculateChunkLength(ObiList<ObiPathFrame> chunk)
        {
            float length = 0;
            for (int i = 1; i < chunk.Count; ++i)
                length += Vector3.Distance(chunk[i].position, chunk[i - 1].position);
            return length;
        }

        /**
         * Generates raw curve chunks from the rope description.
         */
        private void AllocateRawChunks(ObiRopeBase actor)
        {
            using (m_AllocateRawChunksPerfMarker.Auto())
            {
                rawChunks.Clear();

                if (actor.path == null)
                    return;

                // Count particles for each chunk.
                int particles = 0;
                for (int i = 0; i < actor.elements.Count; ++i)
                {
                    particles++;
                    // At discontinuities, start a new chunk.
                    if (i < actor.elements.Count - 1 && actor.elements[i].particle2 != actor.elements[i + 1].particle1)
                    {
                        AllocateChunk(++particles);
                        particles = 0;
                    }
                }
                AllocateChunk(++particles);
            }
        }

        private void PathFrameFromParticle(ObiRopeBase actor, ref ObiPathFrame frame, int particleIndex, bool interpolateOrientation = true)
        {
            // Update current frame values from particles:
            frame.position = w2l.MultiplyPoint3x4(actor.GetParticlePosition(particleIndex));
            frame.thickness = actor.GetParticleMaxRadius(particleIndex);
            frame.color = actor.GetParticleColor(particleIndex);

            // Use particle orientation if possible:
            if (actor.usesOrientedParticles)
            {
                Quaternion current = actor.GetParticleOrientation(particleIndex);
                Quaternion previous = actor.GetParticleOrientation(Mathf.Max(0, particleIndex - 1));
                Quaternion average = w2lRotation * (interpolateOrientation ? Quaternion.SlerpUnclamped(current, previous, 0.5f) : current);
                frame.normal = average * Vector3.up;
                frame.binormal = average * Vector3.right;
                frame.tangent = average * Vector3.forward;
            }
        }

        /**
         * Generates smooth curve chunks.
         */
        public void GenerateSmoothChunks(ObiRopeBase actor, uint smoothingLevels)
        {
            using (m_GenerateSmoothChunksPerfMarker.Auto())
            {
                smoothChunks.Clear();
                smoothSections = 0;
                smoothLength = 0;

                if (!Application.isPlaying)
                    actor.RebuildElementsFromConstraints();

                AllocateRawChunks(actor);

                w2l = actor.transform.worldToLocalMatrix;
                w2lRotation = w2l.rotation;

                // keep track of the first element of each chunk
                int chunkStart = 0;

                ObiPathFrame frame_0 = new ObiPathFrame(); // "next" frame
                ObiPathFrame frame_1 = new ObiPathFrame(); // current frame
                ObiPathFrame frame_2 = new ObiPathFrame(); // previous frame

                // generate curve for each rope chunk:
                for (int i = 0; i < rawChunks.Count; ++i)
                {
                    int elementCount = rawChunks[i].Count - 1;

                    // Initialize frames:
                    frame_0.Reset();
                    frame_1.Reset();
                    frame_2.Reset();

                    PathFrameFromParticle(actor, ref frame_1, actor.elements[chunkStart].particle1, false);

                    frame_2 = frame_1;

                    for (int m = 1; m <= rawChunks[i].Count; ++m)
                    {

                        int index;
                        if (m >= elementCount)
                            // second particle of last element in the chunk.
                            index = actor.elements[chunkStart + elementCount - 1].particle2;
                        else
                            //first particle of current element.
                            index = actor.elements[chunkStart + m].particle1;

                        // generate curve frame from particle:
                        PathFrameFromParticle(actor, ref frame_0, index);

                        if (actor.usesOrientedParticles)
                        {
                            // copy frame directly.
                            frame_2 = frame_1;
                        }
                        else
                        {
                            // perform parallel transport, using forward / backward average to calculate tangent.
                            frame_1.tangent = ((frame_1.position - frame_2.position) + (frame_0.position - frame_1.position)).normalized;
                            frame_2.Transport(frame_1, twist);
                        }

                        // in case we wrapped around the rope, average first and last frames:
                        if (chunkStart + m > actor.activeParticleCount)
                        {
                            frame_2 = rawChunks[0][0] = 0.5f * frame_2 + 0.5f * rawChunks[0][0];
                        }

                        frame_1 = frame_0;

                        rawChunks[i][m - 1] = frame_2;
                    }

                    // increment chunkStart by the amount of elements in this chunk:
                    chunkStart += elementCount;

                    // adaptive curvature-based decimation:
                    if (Decimate(rawChunks[i], smoothChunks[i], decimation))
                    {
                        // if any decimation took place, swap raw and smooth chunks:
                        var aux = rawChunks[i];
                        rawChunks[i] = smoothChunks[i];
                        smoothChunks[i] = aux;
                    }

                    // get smooth curve points:
                    Chaikin(rawChunks[i], smoothChunks[i], smoothingLevels);

                    // count total curve sections and total curve length:
                    smoothSections += smoothChunks[i].Count;
                    smoothLength += CalculateChunkLength(smoothChunks[i]);
                }
            }
        }

        public ObiPathFrame GetSectionAt(float mu)
        {
            float edgeMu = smoothSections * Mathf.Clamp(mu,0,0.9999f);
            int index = (int)edgeMu;
            float sectionMu = edgeMu - index;

            int counter = 0;
            int chunkIndex = -1;
            int indexInChunk = -1;
            for (int i = 0; i < smoothChunks.Count; ++i)
            {
                if (counter + smoothChunks[i].Count > index)
                {
                    chunkIndex = i;
                    indexInChunk = index - counter;
                    break;
                }
                counter += smoothChunks[i].Count;
            }

            ObiList<ObiPathFrame> chunk = smoothChunks[chunkIndex];
            ObiPathFrame s1 = chunk[indexInChunk];
            ObiPathFrame s2 = chunk[Mathf.Min(indexInChunk + 1, chunk.Count - 1)];

            return (1 - sectionMu) * s1 + sectionMu * s2;
        }

        /**
         * Iterative version of the Ramer-Douglas-Peucker path decimation algorithm. 
         */
        private bool Decimate(ObiList<ObiPathFrame> input, ObiList<ObiPathFrame> output, float threshold)
        {
            // no decimation, no work to do, just return:
            if (threshold < 0.00001f || input.Count < 3)
                return false;

            float scaledThreshold = threshold * threshold * 0.01f;

            stack.Push(new Vector2Int(0, input.Count - 1));

            decimateBitArray.Length = Mathf.Max(decimateBitArray.Length, input.Count);
            decimateBitArray.SetAll(true);

            while (stack.Count > 0)
            {
                var range = stack.Pop();

                float dmax = 0;
                int index = range.x;
                float mu;

                for (int i = index + 1; i < range.y; ++i)
                {
                    if (decimateBitArray[i])
                    {
                        float d = Vector3.SqrMagnitude(ObiUtils.ProjectPointLine(input[i].position, input[range.x].position, input[range.y].position, out mu) - input[i].position);

                        if (d > dmax)
                        {
                            index = i;
                            dmax = d;
                        }
                    }
                }

                if (dmax > scaledThreshold)
                {
                    stack.Push(new Vector2Int(range.x, index));
                    stack.Push(new Vector2Int(index, range.y));
                }
                else
                {
                    for (int i = range.x + 1; i < range.y; ++i)
                        decimateBitArray[i] = false;
                }
            }

            output.Clear();
            for (int i = 0; i < input.Count; ++i)
                if (decimateBitArray[i])
                    output.Add(input[i]);

            return true;
        }

        /** 
        * This method uses a variant of Chainkin's algorithm to produce a smooth curve from a set of control points. It is specially fast
        * because it directly calculates subdivision level k, instead of recursively calculating levels 1..k.
        */
        private void Chaikin(ObiList<ObiPathFrame> input, ObiList<ObiPathFrame> output, uint k)
        {
            // no subdivision levels, no work to do. just copy the input to the output:
            if (k == 0 || input.Count < 3)
            {
                output.SetCount(input.Count);
                for (int i = 0; i < input.Count; ++i)
                    output[i] = input[i];
                return;
            }

            // calculate amount of new points generated by each inner control point:
            int pCount = (int)Mathf.Pow(2, k);

            // precalculate some quantities:
            int n0 = input.Count - 1;
            float twoRaisedToMinusKPlus1 = Mathf.Pow(2, -(k + 1));
            float twoRaisedToMinusK = Mathf.Pow(2, -k);
            float twoRaisedToMinus2K = Mathf.Pow(2, -2 * k);
            float twoRaisedToMinus2KMinus1 = Mathf.Pow(2, -2 * k - 1);

            // allocate ouput:
            output.SetCount((n0 - 1) * pCount + 2);

            // calculate initial curve points:
            output[0] = (0.5f + twoRaisedToMinusKPlus1) * input[0] + (0.5f - twoRaisedToMinusKPlus1) * input[1];
            output[pCount * n0 - pCount + 1] = (0.5f - twoRaisedToMinusKPlus1) * input[n0 - 1] + (0.5f + twoRaisedToMinusKPlus1) * input[n0];

            // calculate internal points:
            for (int j = 1; j <= pCount; ++j)
            {
                // precalculate coefficients:
                float F = 0.5f - twoRaisedToMinusKPlus1 - (j - 1) * (twoRaisedToMinusK - j * twoRaisedToMinus2KMinus1);
                float G = 0.5f + twoRaisedToMinusKPlus1 + (j - 1) * (twoRaisedToMinusK - j * twoRaisedToMinus2K);
                float H = (j - 1) * j * twoRaisedToMinus2KMinus1;

                for (int i = 1; i < n0; ++i)
                    ObiPathFrame.WeightedSum(F, G, H,
                                             ref input.Data[i - 1],
                                             ref input.Data[i],
                                             ref input.Data[i + 1],
                                             ref output.Data[(i - 1) * pCount + j]);
            }

            // make first and last curve points coincide with original points:
            output[0] = input[0];
            output[output.Count - 1] = input[input.Count - 1];
        }

    }
}