using System;

using UnityEngine;

namespace Framework.Support {

  /// <summary>A cubic bezier curve based on 4 points</summary>
  public struct CubicBezierCurve {

    /// <summary>Point the curve will start at</summary>
    public Vector3 P0;
    /// <summary>Point towards which the curve leaves the starting point</summary>
    public Vector3 P1;
    /// <summary>Point away from which the curve reaches the ending point</summary>
    public Vector3 P2;
    /// <summary>Point the curve ends at</summary>
    public Vector3 P3;

    /// <summary>
    ///   Initializes a new cubic bezier curve from the provided support points
    /// </summary>
    /// <param name="p0">Point the curve will start at</param>
    /// <param name="p1">
    ///   Point in whose direction the curve will leave the starting point
    /// </param>
    /// <param name="p2">
    ///   Point from whose direction the curve will reach the ending point
    /// </param>
    /// <param name="p3">Point the curve will end at</param>
    public CubicBezierCurve(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3) {
      this.P0 = p0;
      this.P1 = p1;
      this.P2 = p2;
      this.P3 = p3;
    }

    /// <summary>Approximates the length of the bezier curve</summary>
    /// <returns>The approximate length of the berzier curve</returns>
    public float ApproximateLength() {
      const int SegmentCount = 10;
      float approximateLength = 0.0f;

      // Break the curve into line segments and sum the length of those line segments.
      // I couldn't find any code that actually calculates the length based on the input
      // parameters to the bezier formula.
      Vector3 previous;
      Vector3 current = this.P0;
      for(int index = 1; index < SegmentCount; ++index) {
        previous = current;
        current = Interpolate(
          this.P0, this.P1, this.P2, this.P3, (float)index / (float)SegmentCount
        );
        approximateLength += (current - previous).magnitude;
      }

      return approximateLength + (this.P3 - current).magnitude;
    }

#if false
    internal static float MaybeGetLength(Vector3 b0, Vector3 b1, Vector3 b2, Vector3 b3) {
      Vector3 p0 = (b0 - b1);
      Vector3 p1 = (b2 - b1);
      Vector3 p2;
      Vector3 p3 = (b3 - b2);

      float l0 = p0.magnitude;
      float l1 = p1.magnitude;
      float l3 = p3.magnitude;
      if(l0 > 0.0f) {
        p0 /= l0;
      }
      if(l1 > 0.0f) {
        p1 /= l1;
      }
      if(l3 > 0.0f) {
        p3 /= l3;
      }
      p2 = -p1;

      float a = Mathf.Abs(Vector3.Dot(p0, p1)) + Mathf.Abs(Vector3.Dot(p2, p3));
      if((a > 1.98f) || ((l0 + l1 + l3) < ((4.0f - a) * 8.0f))) {
        return l0 + l1 + l3;
      }

      Vector3 bl0 = b0;
      Vector3 bl1 = (b0 + b1) * 0.5f;
      Vector3 mid = (b1 + b2) * 0.5f;
      Vector3 bl2 = (bl1 + mid) * 0.5f;
      Vector3 br3 = b3;
      Vector3 br2 = (b2 + b3) * 0.5f;
      Vector3 br1 = (br2 + mid) * 0.5f;
      Vector3 br0 = (br1 + bl2) * 0.5f;
      Vector3 bl3 = br0;

      return MaybeGetLength(bl0, bl1, bl2, bl3) + MaybeGetLength(br0, br1, br2, br3);
    }
#endif

    /// <summary>Calculates the point at the specified interpolation time</summary>
    /// <param name="t">Time at which the point will be calculated</param>
    /// <returns>The interpolated point at the specified time</returns>
    public Vector3 GetPointAtTime(float t) {
      return Interpolate(this.P0, this.P1, this.P2, this.P3, t);
    }

    /// <summary>
    ///   Calculates a point on the berzier curve at the specified distance
    ///   (following the curve) from the start
    /// </summary>
    /// <param name="distanceFromStart">Distance from the start of the curve</param>
    /// <returns>The interpolated location</returns>
    /// <remarks>
    ///   <para>
    ///     This is a very rough approximation since the velocity along the curve
    ///     can be less than one (eg. at a tight bend), so using this method does not
    ///     guarantee equal distance traveled for the resulting point when linearly
    ///     increasing the distance.
    ///   </para>
    ///   <para>
    ///     If you need a more accurate version of this method, consider splitting
    ///     the curve into line segments, caching the length of those line segments
    ///     and interpolating the points along the curve this way. Or finding an
    ///     equation that calculates T from distance (I couldn't find one.)
    ///   </para>
    /// </remarks>
    public Vector3 GetPointAtDistance(float distanceFromStart) {
      return Interpolate(
        this.P0, this.P1, this.P2, this.P3, distanceFromStart / ApproximateLength()
      );
    }

    /// <summary>Interpolates a point along a cubic bezier curve</summary>
    /// <param name="p0">Location from which the curve starts</param>
    /// <param name="p1">Point towards which the direction of the curve starts out</param>
    /// <param name="p2">
    ///   Point from which towards the end the direction of the curve ends
    /// </param>
    /// <param name="p3">Location at which the curve ends</param>
    /// <param name="t">Interpolation interval in the range 0.0 .. 1.0</param>
    /// <returns>The interpolated position</returns>
    public static Vector3 Interpolate(
      Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t
    ) {
      return
        (float)Math.Pow(1 - t, 3) * p0 +
        3 * (float)Math.Pow(1 - t, 2) * t * p1 +
        3 * (1 - t) * (float)Math.Pow(t, 2) * p2 +
        (float)Math.Pow(t, 3) * p3;
    }

  }

} // namespace Framework.Support