using UnityEngine;

namespace Cinemachine.Utility
{
    /// <summary>
    /// A collection of utilities relating to Bezier splines
    /// </summary>
    public static class SplineHelpers
    {
        /// <summary>Compute the value of a 4-point 3-dimensional bezier spline</summary>
        /// <param name="t">How far along the spline (0...1)</param>
        /// <param name="p0">First point</param>
        /// <param name="p1">First tangent</param>
        /// <param name="p2">Second point</param>
        /// <param name="p3">Second tangent</param>
        /// <returns>The spline value at t</returns>
        public static Vector3 Bezier3(
            float t, Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3)
        {
            t = Mathf.Clamp01(t);
            float d = 1f - t;
            return d * d * d * p0 + 3f * d * d * t * p1
                + 3f * d * t * t * p2 + t * t * t * p3;
        }

        /// <summary>Compute the tangent of a 4-point 3-dimensional bezier spline</summary>
        /// <param name="t">How far along the spline (0...1)</param>
        /// <param name="p0">First point</param>
        /// <param name="p1">First tangent</param>
        /// <param name="p2">Second point</param>
        /// <param name="p3">Second tangent</param>
        /// <returns>The spline tangent at t</returns>
        public static Vector3 BezierTangent3(
            float t, Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3)
        {
            t = Mathf.Clamp01(t);
            return (-3f * p0 + 9f * p1 - 9f * p2 + 3f * p3) * (t * t)
                +  (6f * p0 - 12f * p1 + 6f * p2) * t
                -  3f * p0 + 3f * p1;
        }

        /// <summary>Compute the weights for the tangent of a 4-point 3-dimensional bezier spline</summary>
        /// <param name="p0">First point</param>
        /// <param name="p1">First tangent</param>
        /// <param name="p2">Second point</param>
        /// <param name="p3">Second tangent</param>
        /// <param name="w0">First output weight</param>
        /// <param name="w1">Second output weight</param>
        /// <param name="w2">Third output weight</param>
        public static void BezierTangentWeights3(
            Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3,
            out Vector3 w0, out Vector3 w1, out Vector3 w2)
        {
            w0 = -3f * p0 + 9f * p1 - 9f * p2 + 3f * p3;
            w1 = 6f * p0 - 12f * p1 + 6f * p2;
            w2 = -3f * p0 + 3f * p1;
        }
        
        /// <summary>Compute the value of a 4-point 1-dimensional bezier spline</summary>
        /// <param name="t">How far along the spline (0...1)</param>
        /// <param name="p0">First point</param>
        /// <param name="p1">First tangent</param>
        /// <param name="p2">Second point</param>
        /// <param name="p3">Second tangent</param>
        /// <returns>The spline value at t</returns>
        public static float Bezier1(float t, float p0, float p1, float p2, float p3)
        {
            t = Mathf.Clamp01(t);
            float d = 1f - t;
            return d * d * d * p0 + 3f * d * d * t * p1
                + 3f * d * t * t * p2 + t * t * t * p3;
        }

        /// <summary>Compute the tangent of a 4-point 1-dimensional bezier spline</summary>
        /// <param name="t">How far along the spline (0...1)</param>
        /// <param name="p0">First point</param>
        /// <param name="p1">First tangent</param>
        /// <param name="p2">Second point</param>
        /// <param name="p3">Second tangent</param>
        /// <returns>The spline tangent at t</returns>
        public static float BezierTangent1(
            float t, float p0, float p1, float p2, float p3)
        {
            t = Mathf.Clamp01(t);
            return (-3f * p0 + 9f * p1 - 9f * p2 + 3f * p3) * t * t
                +  (6f * p0 - 12f * p1 + 6f * p2) * t
                -  3f * p0 + 3f * p1;
        }

        /// <summary>
        /// Use the Thomas algorithm to compute smooth tangent values for a spline.  
        /// Resultant tangents guarantee second-order smoothness of the curve.
        /// </summary>
        /// <param name="knot">The knots defining the curve</param>
        /// <param name="ctrl1">Output buffer for the first control points (1 per knot)</param>
        /// <param name="ctrl2">Output buffer for the second control points (1 per knot)</param>
        public static void ComputeSmoothControlPoints(
            ref Vector4[] knot, ref Vector4[] ctrl1, ref Vector4[] ctrl2)
        {
            int numPoints = knot.Length;
            if (numPoints <= 2)
            {
                if (numPoints == 2)
                {
                    ctrl1[0] = Vector4.Lerp(knot[0], knot[1], 0.33333f);
                    ctrl2[0] = Vector4.Lerp(knot[0], knot[1], 0.66666f);
                }
                else if (numPoints == 1)
                    ctrl1[0] = ctrl2[0] = knot[0];
                return;
            }

            var a = new float[numPoints];
            var b = new float[numPoints];
            var c = new float[numPoints];
            var r = new float[numPoints];
            for (int axis = 0; axis < 4; ++axis)
            {
                int n = numPoints - 1;

                // Linear into the first segment
                a[0] = 0;
                b[0] = 2;
                c[0] = 1;
                r[0] = knot[0][axis] + 2 * knot[1][axis];

                // Internal segments
                for (int i = 1; i < n - 1; ++i)
                {
                    a[i] = 1;
                    b[i] = 4;
                    c[i] = 1;
                    r[i] = 4 * knot[i][axis] + 2 * knot[i+1][axis];
                }

                // Linear out of the last segment
                a[n - 1] = 2;
                b[n - 1] = 7;
                c[n - 1] = 0;
                r[n - 1] = 8 * knot[n - 1][axis] + knot[n][axis];

                // Solve with Thomas algorithm
                for (int i = 1; i < n; ++i)
                {
                    float m = a[i] / b[i-1];
                    b[i] = b[i] - m * c[i-1];
                    r[i] = r[i] - m * r[i-1];
                }

                // Compute ctrl1
                ctrl1[n-1][axis] = r[n-1] / b[n-1];
                for (int i = n - 2; i >= 0; --i)
                    ctrl1[i][axis] = (r[i] - c[i] * ctrl1[i + 1][axis]) / b[i];

                // Compute ctrl2 from ctrl1
                for (int i = 0; i < n; i++)
                    ctrl2[i][axis] = 2 * knot[i + 1][axis] - ctrl1[i + 1][axis];
                ctrl2[n - 1][axis] = 0.5f * (knot[n][axis] + ctrl1[n - 1][axis]);
            }
        }

        /// <summary>
        /// Use the Thomas algorithm to compute smooth tangent values for a spline.  
        /// This method will assume that the spline is looped (i.e. that the last knot is followed by the first).
        /// Resultant tangents guarantee second-order smoothness of the curve.
        /// </summary>
        /// <param name="knot">The knots defining the curve</param>
        /// <param name="ctrl1">Output buffer for the first control points (1 per knot)</param>
        /// <param name="ctrl2">Output buffer for the second control points (1 per knot)</param>
        public static void ComputeSmoothControlPointsLooped(
            ref Vector4[] knot, ref Vector4[] ctrl1, ref Vector4[] ctrl2)
        {
            int numPoints = knot.Length;
            if (numPoints < 2)
            {
                if (numPoints == 1)
                    ctrl1[0] = ctrl2[0] = knot[0];
                return;
            }

            int margin = Mathf.Min(4, numPoints-1);
            Vector4[] knotLooped = new Vector4[numPoints + 2 * margin];
            Vector4[] ctrl1Looped = new Vector4[numPoints + 2 * margin];
            Vector4[] ctrl2Looped = new Vector4[numPoints + 2 * margin];
            for (int i = 0; i < margin; ++i)
            {
                knotLooped[i] = knot[numPoints-(margin-i)];
                knotLooped[numPoints+margin+i] = knot[i];
            }
            for (int i = 0; i < numPoints; ++i)
                knotLooped[i + margin] = knot[i];
            ComputeSmoothControlPoints(ref knotLooped, ref ctrl1Looped, ref ctrl2Looped);
            for (int i = 0; i < numPoints; ++i)
            {
                ctrl1[i] = ctrl1Looped[i + margin];
                ctrl2[i] = ctrl2Looped[i + margin];
            }
        }
    }
}