namespace Dreamteck.Splines
{
    using System.Collections;
    using System.Collections.Generic;
    using UnityEngine;

    [System.Serializable]
    public class SampleCollection
    {
        [HideInInspector]
        [UnityEngine.Serialization.FormerlySerializedAs("samples")]
        public SplineSample[] samples = new SplineSample[0];

        public int length
        {
            get { return samples.Length; }
        }
        public int[] optimizedIndices = new int[0];
        bool hasSamples
        {
            get { return samples.Length > 0; }
        }
        public SplineComputer.SampleMode sampleMode = SplineComputer.SampleMode.Default;
        private SplineSample _workSample = new SplineSample();


        public SampleCollection()
        {
        }

        public SampleCollection(SampleCollection input)
        {
            samples = input.samples;
            optimizedIndices = input.optimizedIndices;
            sampleMode = input.sampleMode;
        }

        public int GetClippedSampleCount(double clipFrom, double clipTo, out int startIndex, out int endIndex)
        {
            startIndex = endIndex = 0;
            if (sampleMode == SplineComputer.SampleMode.Default)
            {
                startIndex = DMath.FloorInt((samples.Length - 1) * clipFrom);
                endIndex = DMath.CeilInt((samples.Length - 1) * clipTo);
            }
            else
            {
                double clipFromLerp = 0.0, clipToLerp = 0.0;
                GetSamplingValues(clipFrom, out startIndex, out clipFromLerp);
                GetSamplingValues(clipTo, out endIndex, out clipToLerp);
                if (clipToLerp > 0.0 && endIndex < samples.Length - 1) endIndex++;
            }

            if (clipTo < clipFrom) //Handle looping segments
            {
                int toSamples = endIndex + 1;
                int fromSamples = samples.Length - startIndex;
                return toSamples + fromSamples;
            }
            return endIndex - startIndex + 1;
        }

        public void GetSamplingValues(double percent, out int sampleIndex, out double lerp)
        {
            lerp = 0.0;
            if (sampleMode == SplineComputer.SampleMode.Optimized)
            {
                double indexValue = percent * (optimizedIndices.Length - 1);
                int index = DMath.FloorInt(indexValue);
                sampleIndex = optimizedIndices[index];
                double lerpPercent = 0.0;
                if (index < optimizedIndices.Length - 1)
                {
                    //Percent 0-1 between the sampleIndex and the next sampleIndex
                    double indexLerp = indexValue - index;
                    double sampleIndexPercent = (double)index / (optimizedIndices.Length - 1);
                    double nextSampleIndexPercent = (double)(index + 1) / (optimizedIndices.Length - 1);
                    //Percent 0-1 of the sample between the sampleIndices' percents
                    lerpPercent = DMath.Lerp(sampleIndexPercent, nextSampleIndexPercent, indexLerp);
                }
                if (sampleIndex < samples.Length - 1)
                {
                    lerp = DMath.InverseLerp(samples[sampleIndex].percent, samples[sampleIndex + 1].percent, lerpPercent);
                }
                return;
            }

            sampleIndex = DMath.FloorInt(percent * (samples.Length - 1));
            lerp = (samples.Length - 1) * percent - sampleIndex;
        }

        /// <summary>
        /// Same as Spline.EvaluatePosition but the result is transformed by the computer's transform
        /// </summary>
        /// <param name="percent">Evaluation percent</param>
        /// <param name="mode">Mode to use the method in. Cached uses the cached samples while Calculate is more accurate but heavier</param>
        /// <returns></returns>
        public Vector3 EvaluatePosition(double percent)
        {
            if (!hasSamples) return Vector3.zero;
            int index;
            double lerp;
            GetSamplingValues(percent, out index, out lerp);
            if (lerp > 0.0)
            {
                return Vector3.Lerp(samples[index].position, samples[index + 1].position, (float)lerp);
            }
            return samples[index].position;
        }

        /// <summary>
        /// Same as Spline.Evaluate but the result is transformed by the computer's transform
        /// </summary>
        /// <param name="percent">Evaluation percent</param>
        /// <param name="mode">Mode to use the method in. Cached uses the cached samples while Calculate is more accurate but heavier</param>
        /// <returns></returns>
        public SplineSample Evaluate(double percent)
        {
            SplineSample result = new SplineSample();
            Evaluate(percent, ref result);
            return result;
        }

        /// <summary>
        /// Evaluates the sample collection and transforms the result by the <see cref="localToWorldMatrix"/>
        /// </summary>
        /// <param name="result"></param>
        /// <param name="percent"></param>
        public void Evaluate(double percent, ref SplineSample result)
        {
            if (!hasSamples)
            {
                result = new SplineSample();
                return;
            }
            int index;
            double lerp;
            GetSamplingValues(percent, out index, out lerp);
            if (lerp > 0.0)
            {
                SplineSample.Lerp(ref samples[index], ref samples[index + 1], lerp, ref result);
            }
            else
            {
                result.FastCopy(ref samples[index]);
            }
        }

        /// <summary>
        /// Evaluates the sample collection and transforms the results by the <see cref="localToWorldMatrix"/>
        /// </summary>
        /// <param name="from">Start position [0-1]</param>
        /// <param name="to">Target position [from-1]</param>
        /// <returns></returns>
        public void Evaluate(ref SplineSample[] results, double from = 0.0, double to = 1.0)
        {
            if (!hasSamples)
            {
                results = new SplineSample[0];
                return;
            }
            Spline.FormatFromTo(ref from, ref to);
            int fromIndex, toIndex;
            double lerp;
            GetSamplingValues(from, out fromIndex, out lerp);
            GetSamplingValues(to, out toIndex, out lerp);
            if (lerp > 0.0 && toIndex < samples.Length - 1)
            {
                toIndex++;
            }
            int clippedIterations = toIndex - fromIndex + 1;
            if (results == null)
            {
                results = new SplineSample[clippedIterations];
            }
            else if (results.Length != clippedIterations)
            {
                results = new SplineSample[clippedIterations];
            }

            results[0] = Evaluate(from);
            results[results.Length - 1] = Evaluate(to);
            for (int i = 1; i < results.Length - 1; i++)
            {
                results[i].FastCopy(ref samples[i + fromIndex]);
            }
        }

        /// <summary>
        /// Same as Spline.EvaluatePositions but the results are transformed by the computer's transform
        /// </summary>
        /// <param name="from">Start position [0-1]</param>
        /// <param name="to">Target position [from-1]</param>
        /// <returns></returns>
        public void EvaluatePositions(ref Vector3[] positions, double from = 0.0, double to = 1.0)
        {
            if (!hasSamples)
            {
                positions = new Vector3[0];
                return;
            }

            Spline.FormatFromTo(ref from, ref to);
            int fromIndex, toIndex;
            double lerp;
            GetSamplingValues(from, out fromIndex, out lerp);
            GetSamplingValues(to, out toIndex, out lerp);
            if (lerp > 0.0 && toIndex < samples.Length - 1)
            {
                toIndex++;
            }
            int clippedIterations = toIndex - fromIndex + 1;

            if (positions == null)
            {
                positions = new Vector3[clippedIterations];
            }
            else if (positions.Length != clippedIterations)
            {
                positions = new Vector3[clippedIterations];
            }

            positions[0] = EvaluatePosition(from);
            positions[positions.Length - 1] = EvaluatePosition(to);
            for (int i = 1; i < positions.Length - 1; i++)
            {
                positions[i] = samples[i + fromIndex].position;
            }
        }

        /// <summary>
        /// Returns the percent from the spline at a given distance from the start point
        /// </summary>
        /// <param name="start">The start point</param>
        /// <param name="distance">The distance to travel</param>
        /// <param name="direction">The direction towards which to move</param>
        /// <returns></returns>
        public double Travel(double start, float distance, Spline.Direction direction, out float moved, double clipFrom = 0.0, double clipTo = 1.0)
        {
            moved = 0f;
            if (!hasSamples) return 0.0;
            if (direction == Spline.Direction.Forward && start >= 1.0) return clipTo;
            else if (direction == Spline.Direction.Backward && start <= 0.0) return clipFrom;

            double lastPercent = start;
            if (distance == 0f) return lastPercent;
            Vector3 lastPos = EvaluatePosition(start);
            int sampleIndex;
            double lerp;
            GetSamplingValues(lastPercent, out sampleIndex, out lerp);
            if (direction == Spline.Direction.Forward && lerp > 0.0) sampleIndex++;
            float lastDistance = 0f;
            int minIndex = 0;
            int maxIndex = samples.Length - 1;

            bool samplesAreLooped = clipTo < clipFrom;

            if (samplesAreLooped)
            {
                GetSamplingValues(clipFrom, out minIndex, out lerp);
                GetSamplingValues(clipTo, out maxIndex, out lerp);
                if (lerp > 0.0) maxIndex++;
            }

            while (moved < distance)
            {
                Vector3 transformedPos = samples[sampleIndex].position;
                lastDistance = Vector3.Distance(transformedPos, lastPos);
                moved += lastDistance;
                if (moved >= distance) break;
                lastPos = transformedPos;
                lastPercent = samples[sampleIndex].percent;
                if (direction == Spline.Direction.Forward)
                {
                    if (sampleIndex == samples.Length - 1)
                    {
                        if (samplesAreLooped)
                        {
                            lastPos = samples[0].position;
                            lastPercent = samples[0].percent;
                            sampleIndex = 1;
                        }
                        else break;
                    }
                    if (samplesAreLooped && sampleIndex == maxIndex) break;
                    sampleIndex++;
                }
                else
                {
                    if (sampleIndex == 0)
                    {
                        if (samplesAreLooped)
                        {
                            lastPos = samples[samples.Length - 1].position;
                            lastPercent = samples[samples.Length - 1].percent;
                            sampleIndex = samples.Length - 2;
                        }
                        else break;
                    }
                    if (samplesAreLooped && sampleIndex == minIndex) break;
                    sampleIndex--;
                }
            }
            float moveExcess = 0f;
            if (moved > distance)
            {
                moveExcess = moved - distance;
            }


            double lerpPercent = 0.0;
            if(lastDistance > 0.0)
            {
                lerpPercent = moveExcess / lastDistance;
            }
            double p = DMath.Lerp(lastPercent, samples[sampleIndex].percent, 1f - lerpPercent);
            moved -= moveExcess;
            return p;
        }

        /// <summary>
        /// Returns the percent from the spline at a given distance from the start point while applying a local <paramref name="offset"/> to each sample
        /// The offset is multiplied by the sample sizes
        /// </summary>
        /// <param name="start">The start point</param>
        /// /// <param name="distance">The distance to travel</param>
        /// <param name="direction">The direction towards which to move</param>
        /// <returns></returns>
        public double TravelWithOffset(double start, float distance, Spline.Direction direction, Vector3 offset, out float moved, double clipFrom = 0.0, double clipTo = 1.0)
        {
            moved = 0f;
            if (!hasSamples) return 0.0;
            if (direction == Spline.Direction.Forward && start >= 1.0) return clipTo;
            else if (direction == Spline.Direction.Backward && start <= 0.0) return clipFrom;

            double lastPercent = start;
            if (distance == 0f) return lastPercent;

            Evaluate(start, ref _workSample);
            Vector3 lastPos = _workSample.position + _workSample.up * (offset.y * _workSample.size) + _workSample.right * (offset.x * _workSample.size) + _workSample.forward * (offset.z * _workSample.size);

            int sampleIndex;
            double lerp;
            GetSamplingValues(lastPercent, out sampleIndex, out lerp);
            if (direction == Spline.Direction.Forward && lerp > 0.0) sampleIndex++;
            float lastDistance = 0f;
            int minIndex = 0;
            int maxIndex = length - 1;

            bool samplesAreLooped = clipTo < clipFrom;

            if (samplesAreLooped)
            {
                GetSamplingValues(clipFrom, out minIndex, out lerp);
                GetSamplingValues(clipTo, out maxIndex, out lerp);
                if (lerp > 0.0) maxIndex++;
            }

            while (moved < distance)
            {
                Vector3 newPos = samples[sampleIndex].position + 
                    samples[sampleIndex].up * (offset.y * samples[sampleIndex].size) + 
                    samples[sampleIndex].right * (offset.x * samples[sampleIndex].size) + 
                    samples[sampleIndex].forward * (offset.z * samples[sampleIndex].size);
                lastDistance = Vector3.Distance(newPos, lastPos);
                moved += lastDistance;
                if (moved >= distance)
                {
                    break;
                }
                lastPos = newPos;
                lastPercent = samples[sampleIndex].percent;
                if (direction == Spline.Direction.Forward)
                {
                    if (sampleIndex == length - 1)
                    {
                        if (samplesAreLooped)
                        {
                            lastPos = samples[0].position + 
                                samples[0].up * (offset.y * samples[0].size) + 
                                samples[0].right * (offset.x * samples[0].size) + 
                                samples[0].forward * (offset.z * samples[0].size);
                            lastPercent = samples[0].percent;
                            sampleIndex = 1;
                        }
                        else break;
                    }
                    if (samplesAreLooped && sampleIndex == maxIndex) break;
                    sampleIndex++;
                }
                else
                {
                    if (sampleIndex == 0)
                    {
                        if (samplesAreLooped)
                        {
                            int lastIndex = samples.Length - 1;
                            lastPos = samples[lastIndex].position + 
                                samples[lastIndex].up * (offset.y * samples[lastIndex].size) + 
                                samples[lastIndex].right * (offset.x * samples[lastIndex].size) + 
                                samples[lastIndex].forward * (offset.z * samples[lastIndex].size);
                            lastPercent = samples[lastIndex].percent;
                            sampleIndex = samples.Length - 2;
                        }
                        else break;
                    }
                    if (samplesAreLooped && sampleIndex == minIndex) break;
                    sampleIndex--;
                }
            }
            float moveExcess = 0f;
            if (moved > distance)
            {
                moveExcess = moved - distance;
            }

            double p = DMath.Lerp(lastPercent, samples[sampleIndex].percent, 1f - moveExcess / lastDistance);
            moved -= moveExcess;
            return p;
        }

        public double Travel(double start, float distance, Spline.Direction direction = Spline.Direction.Forward)
        {
            float moved;
            return Travel(start, distance, direction, out moved);
        }

        /// <summary>
        /// Same as Spline.Project but the point is transformed by the computer's transform.
        /// </summary>
        /// <param name="position">Point in space</param>
        /// <param name="subdivide">Subdivisions default: 4</param>
        /// <param name="from">Sample from [0-1] default: 0f</param>
        /// <param name="to">Sample to [0-1] default: 1f</param>
        /// <param name="mode">Mode to use the method in. Cached uses the cached samples while Calculate is more accurate but heavier</param>
        /// <param name="subdivisions">Subdivisions for the Calculate mode. Don't assign if not using Calculated mode.</param>
        /// <returns></returns>
        public void Project(Vector3 position, int controlPointCount, ref SplineSample result, double from = 0.0, double to = 1.0)
        {
            if (!hasSamples) return;
            if (samples.Length == 1)
            {
                result.FastCopy(ref samples[0]);
                return;
            }
            Spline.FormatFromTo(ref from, ref to);
            //First make a very rough sample of the from-to region 
            int steps = (controlPointCount - 1) * 4; //Sampling four points per segment is enough to find the closest point range
            int step = samples.Length / steps;
            if (step < 1) step = 1;
            float minDist = (position - samples[0].position).sqrMagnitude;
            int fromIndex = 0;
            int toIndex = samples.Length - 1;
            double lerp;
            if (from != 0.0) GetSamplingValues(from, out fromIndex, out lerp);
            if (to != 1.0)
            {
                GetSamplingValues(to, out toIndex, out lerp);
                if (lerp > 0.0 && toIndex < samples.Length - 1) toIndex++;
            }
            int checkFrom = fromIndex;
            int checkTo = toIndex;

            //Find the closest point range which will be checked in detail later
            for (int i = fromIndex; i < toIndex; i += step)
            {
                if (i >= toIndex) i = toIndex-1;
                Vector3 projected = LinearAlgebraUtility.ProjectOnLine(samples[i].position, samples[Mathf.Min(i + step, toIndex)].position, position);
                float dist = (position - projected).sqrMagnitude;
                if (dist < minDist)
                {
                    minDist = dist;
                    checkFrom = Mathf.Max(i - step, 0);
                    checkTo = Mathf.Min(i + step, samples.Length - 1);
                }
                if (i == toIndex) break;
            }
            minDist = (position - samples[checkFrom].position).sqrMagnitude;

            int index = checkFrom;
            //Find the closest result within the range
            for (int i = checkFrom + 1; i <= checkTo; i++)
            {
                float dist = (position - samples[i].position).sqrMagnitude;
                if (dist < minDist)
                {
                    minDist = dist;
                    index = i;
                }
            }
            //Project the point on the line between the two closest samples
            int backIndex = index - 1;
            if (backIndex < 0) backIndex = 0;
            int frontIndex = index + 1;
            if (frontIndex > samples.Length - 1) frontIndex = samples.Length - 1;
            Vector3 back = LinearAlgebraUtility.ProjectOnLine(samples[backIndex].position, samples[index].position, position);
            Vector3 front = LinearAlgebraUtility.ProjectOnLine(samples[index].position, samples[frontIndex].position, position);
            float backLength = (samples[index].position - samples[backIndex].position).magnitude;
            float frontLength = (samples[index].position - samples[frontIndex].position).magnitude;
            float backProjectDist = (back - samples[backIndex].position).magnitude;
            float frontProjectDist = (front - samples[frontIndex].position).magnitude;
            if (backIndex < index && index < frontIndex)
            {
                if ((position - back).sqrMagnitude < (position - front).sqrMagnitude)
                {
                    SplineSample.Lerp(ref samples[backIndex], ref samples[index], backProjectDist / backLength, ref result);
                    if (sampleMode == SplineComputer.SampleMode.Uniform) result.percent = DMath.Lerp(GetSamplePercent(backIndex), GetSamplePercent(index), backProjectDist / backLength);
                }
                else
                {
                    SplineSample.Lerp(ref samples[frontIndex], ref samples[index], frontProjectDist / frontLength, ref result);
                    if (sampleMode == SplineComputer.SampleMode.Uniform) result.percent = DMath.Lerp(GetSamplePercent(frontIndex), GetSamplePercent(index), frontProjectDist / frontLength);
                }
            }
            else if (backIndex < index)
            {
                SplineSample.Lerp(ref samples[backIndex], ref samples[index], backProjectDist / backLength, ref result);
                if (sampleMode == SplineComputer.SampleMode.Uniform) result.percent = DMath.Lerp(GetSamplePercent(backIndex), GetSamplePercent(index), backProjectDist / backLength);
            }
            else
            {
                SplineSample.Lerp(ref samples[frontIndex], ref samples[index], frontProjectDist / frontLength, ref result);
                if (sampleMode == SplineComputer.SampleMode.Uniform) result.percent = DMath.Lerp(GetSamplePercent(frontIndex), GetSamplePercent(index), frontProjectDist / frontLength);
            }

            if (samples.Length > 1 && from == 0.0 && to == 1.0 && result.percent < samples[1].percent) //Handle looped splines
            {
                Vector3 projected = LinearAlgebraUtility.ProjectOnLine(samples[samples.Length - 1].position, samples[samples.Length - 2].position, position);
                if ((position - projected).sqrMagnitude < (position - result.position).sqrMagnitude)
                {
                    double l = LinearAlgebraUtility.InverseLerp(samples[samples.Length - 1].position, samples[samples.Length - 2].position, projected);
                    SplineSample.Lerp(ref samples[samples.Length - 1], ref samples[samples.Length - 2], l, ref result);
                    if (sampleMode == SplineComputer.SampleMode.Uniform) result.percent = DMath.Lerp(GetSamplePercent(samples.Length - 1), GetSamplePercent(samples.Length - 2), l);
                }
            } 
        }

        private double GetSamplePercent(int sampleIndex)
        {
            if (sampleMode == SplineComputer.SampleMode.Optimized)
            {
                return samples[optimizedIndices[sampleIndex]].percent;
            }
            return (double)sampleIndex / (samples.Length - 1);
        }

        /// <summary>
        /// Same as Spline.CalculateLength but this takes the computer's transform into account when calculating the length.
        /// </summary>
        /// <param name="from">Calculate from [0-1] default: 0f</param>
        /// <param name="to">Calculate to [0-1] default: 1f</param>
        /// <param name="resolution">Resolution [0-1] default: 1f</param>
        /// <param name="address">Node address of junctions</param>
        /// <returns></returns>
        public float CalculateLength(double from = 0.0, double to = 1.0)
        {
            if (!hasSamples) return 0f;
            Spline.FormatFromTo(ref from, ref to);
            float length = 0f;
            Vector3 lastPos = EvaluatePosition(from);
            int fromIndex, toIndex;
            double lerp;
            GetSamplingValues(from, out fromIndex, out lerp);
            GetSamplingValues(to, out toIndex, out lerp);
            if (lerp > 0.0 && toIndex < this.length - 1)
            {
                toIndex++;
            }
            for (int i = fromIndex+1; i < toIndex; i++)
            {
                Vector3 currentPos = samples[i].position;
                length += Vector3.Distance(currentPos, lastPos);
                lastPos = currentPos;
            }
            length += Vector3.Distance(EvaluatePosition(to), lastPos);
            return length;
        }

        /// <summary>
        /// Calculates the length between <paramref name="from"/> and <paramref name="to"/> with applied local offset to to the samples
        /// The offset is multiplied by the sample sizes
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <param name="offset"></param>
        /// <returns></returns>
        public float CalculateLengthWithOffset(Vector3 offset, double from = 0.0, double to = 1.0)
        {
            if (!hasSamples) return 0f;
            Spline.FormatFromTo(ref from, ref to);
            float length = 0f;
            Evaluate(from, ref _workSample);
            Vector3 lastPos = _workSample.position + _workSample.up * (offset.y * _workSample.size) + _workSample.right * (offset.x * _workSample.size) + _workSample.forward * (offset.z * _workSample.size);
            int fromIndex, toIndex;
            double lerp;
            GetSamplingValues(from, out fromIndex, out lerp);
            GetSamplingValues(to, out toIndex, out lerp);

            if (lerp > 0.0 && toIndex < this.length - 1)
            {
                toIndex++;
            }

            for (int i = fromIndex + 1; i < toIndex; i++)
            {
                Vector3 newPos = samples[i].position + samples[i].up * (offset.y * samples[i].size) + samples[i].right * (offset.x * samples[i].size) + samples[i].forward * (offset.z * samples[i].size);
                length += Vector3.Distance(newPos, lastPos);
                lastPos = newPos;
            }

            Evaluate(to, ref _workSample);
            _workSample.position += _workSample.up * (offset.y * _workSample.size) + _workSample.right * (offset.x * _workSample.size) + _workSample.forward * (offset.z * _workSample.size);
            length += Vector3.Distance(_workSample.position, lastPos);
            return length;
        }

    }
}