/// <summary>
        /// Gives the approximate closest point to point p. dt controls how accurate it will be.
        /// </summary>
        /// <param name="p">Point we are getting closest point on curve to</param>
        /// <param name="bc">Bezier Component</param>
        /// <param name="dt">delta controls number of iterations taken
        /// and is therefore important to understand efficiency is reduced
        /// as dt gets smaller...
        /// </param>
        /// <returns>The closesst point along curve to p</returns>
        public static Vector3 ClosestPoint(Vector3 p, BezierComponent bc, float dt = 0.01f)
        {
            if (dt < 0.000001f)
            {
                return(Vector3.positiveInfinity);
            }

            int C = bc.controlPoints.Length;
            int N = (C - C % 4) / 4;

            Vector3 ccp = Evaluate(0, bc);
            float   csd = (p - ccp).sqrMagnitude;
            float   t   = dt;
            Vector3 cp;
            float   d;

            while (t < N)
            {
                cp = Evaluate(t, bc);
                d  = (p - cp).sqrMagnitude;
                if (d < csd)
                {
                    ccp = cp;
                    csd = d;
                }
                t += dt;
            }

            return(ccp);
        }
        public static void DrawGizmoForCurve(BezierComponent bc, GizmoType gizmoType)
        {
            BezierCPComponent[] CPS = bc.controlPoints;
            if (CPS == null || CPS.Length == 0)
            {
                return;
            }

            int N = CurveCount(bc);

            // Draw curve
            Vector3 cp;
            Vector3 pp = BezierSystem.Evaluate(0, bc);

            Gizmos.color = bc.curveColor;
            for (float t = bc.drawDelta; t < N; t += bc.drawDelta)
            {
                cp = BezierSystem.Evaluate(t, bc);
                Gizmos.DrawLine(pp, cp);
                pp = cp;
            }
            cp = BezierSystem.Evaluate(N - 0.0002f, bc);
            Gizmos.DrawLine(pp, cp);

            // Draw CP lines
            Gizmos.color = bc.tangentColor;
            for (int i = 0; i < N; i++)
            {
                int s = 4 * i;
                Gizmos.DrawLine(CPS[s].transform.position, CPS[s + 1].transform.position);
                Gizmos.DrawLine(CPS[s + 2].transform.position, CPS[s + 3].transform.position);
            }
        }
        /// <summary>
        /// Gives the next param after t such that the the Length along curve
        /// going from t to retured value is approximately arc length length.
        /// i.e for getting a uniform speed along curve.
        /// </summary>
        /// <param name="t">param</param>
        /// <param name="length">length to travel along curve</param>
        /// <param name="crv">BezierComponent to evaluate</param>
        /// <returns>new uniform length parameter</returns>
        public static float UniformSpeedParam(float t, float length, BezierComponent crv)
        {
            Vector3 p       = Evaluate(t, crv);
            Vector3 tangent = TangentVector(t, crv);

            t += length / tangent.magnitude;
            return(t);
        }
        /// <summary>
        /// Gives the position of the curve in World Space at parameter t
        /// </summary>
        /// <param name="t">
        /// parameter. Domain -> [0, N)
        /// where N is the number of curves appended together
        /// to give the full curve. The BezierCPComponent[]
        /// length must be of form: 4*i where is a natural number...
        /// </param>
        /// <param name="bc">The BezierComponent to evaluate</param>
        /// <returns>point on curve at param t</returns>
        public static Vector3 Evaluate(float t, BezierComponent bc)
        {
            int startIdx = -1;

            t = Param(t, bc, out startIdx);
            float p = 1 - t;

            BezierCPComponent[] cps = bc.controlPoints;

            Vector3 p0 = cps[startIdx].transform.position;
            Vector3 p1 = cps[startIdx + 1].transform.position;
            Vector3 p2 = cps[startIdx + 2].transform.position;
            Vector3 p3 = cps[startIdx + 3].transform.position;

            return((p0 * p * p * p) + (3 * p1 * t * p * p) + (3 * p2 * p * t * t) + (p3 * t * t * t));
        }
        /// <summary>
        /// Transforms the parameter to be between [0,1]
        /// and sets the startIdx so that we can index into
        /// BezierCurve array and get parameter
        /// </summary>
        /// <param name="t">param from [0, CurveCount(BezierComponent) )</param>
        /// <param name="bc">BezierComponent to eval</param>
        /// <param name="startIdx">the start index into the BezierComponent.controlPoints array...</param>
        /// <returns>returns normalized parameter between [0,1]</returns>
        public static float Param(float t, BezierComponent bc, out int startIdx)
        {
            int p = (int)Mathf.Floor(t);
            int N = CurveCount(bc);

            if (p >= N)
            {
                startIdx = -1;
                return(-1);
            }
            else
            {
                startIdx = p * 4;
            }

            return(t - p);
        }
        /// <summary>
        /// Better to call this when evaluating the arclength for the entire curve...
        /// </summary>
        /// <param name="bezierData">BezierComponent data to evaluate</param>
        /// <returns>Arc Length</returns>
        public static float FastArcLength(BezierComponent bc)
        {
            float L         = 0.0f;
            int   N         = (bc.controlPoints.Length - bc.controlPoints.Length % 4) / 4;
            float arclength = 0f;

            for (int i = 0; i < N; i++)
            {
                Vector3 A = bc.controlPoints[4 * i].transform.position;
                Vector3 B = bc.controlPoints[4 * i + 1].transform.position;
                Vector3 C = bc.controlPoints[4 * i + 2].transform.position;
                Vector3 D = bc.controlPoints[4 * i + 3].transform.position;
                L = 0;
                ArcLengthUtil(A, B, C, D, 5, ref L);
                arclength += L;
            }
            return(arclength);
        }
        /// <summary>
        /// Gives the Tangent vector to the curve
        /// </summary>
        /// <param name="t">param</param>
        /// <param name="bc">BezierComponent to evaluate</param>
        /// <returns></returns>
        public static Vector3 TangentVector(float t, BezierComponent bc)
        {
            int startIdx = 0;

            t = Param(t, bc, out startIdx);

            BezierCPComponent[] cps = bc.controlPoints;
            Vector3             p0  = cps[startIdx].transform.position;
            Vector3             p1  = cps[startIdx + 1].transform.position;
            Vector3             p2  = cps[startIdx + 2].transform.position;
            Vector3             p3  = cps[startIdx + 3].transform.position;

            Vector3 A       = -3 * p0 + 9 * p1 - 9 * p2 + 3 * p3;
            Vector3 B       = 6 * p0 + -12 * p1 + 6 * p2;
            Vector3 C       = -3 * p0 + 3 * p1;
            Vector3 tangent = (t * t * A) + (t * B) + C;

            return(tangent);
        }
        /// <summary>
        /// Returns the Arc Length of the curve.
        /// Currently, its mostly a bruteforce approach and therefore
        /// the paramter dt is important for efficiency. If you want
        /// to evaulate the entire curve, use FastArcLength instead.
        /// </summary>
        /// <param name="t0">starting param</param>
        /// <param name="t1">end param</param>
        /// <param name="bc">BezierComponent to eval.</param>
        /// <param name="dt">delta param. This controls the steps algo takes.
        /// Its important to understand the lower the number, the more accurate
        /// but the more expensive it becomes.
        /// </param>
        /// <returns>The Arclength of curve between t0 and t1</returns>
        public static float ArcLength(float t0, float t1, BezierComponent bc, float dt = 0.01f)
        {
            float   length = 0;
            float   t      = t0 + dt;
            Vector3 pp     = BezierSystem.Evaluate(t0, bc);
            Vector3 cp;

            while (t < t1)
            {
                cp      = BezierSystem.Evaluate(t, bc);
                length += (cp - pp).magnitude;
                pp      = cp;
                t      += dt;
            }

            cp      = BezierSystem.Evaluate(t1, bc);
            length += (cp - pp).magnitude;
            return(length);
        }
 /// <summary>
 /// Gives the current count of curves.
 /// Entire curve is made up of individual
 /// cubic bezier curves.
 /// </summary>
 /// <param name="bc">BezierComponent to eval</param>
 /// <returns>Curve count</returns>
 public static int CurveCount(BezierComponent bc)
 {
     return((bc.controlPoints.Length - bc.controlPoints.Length % 4) / 4);
 }