using Godot;
using Rokojori;
using System.Collections.Generic;

using Rokojori.Extensions;
namespace Rokojori
{
  /** Arc-length reparametrization of a SplineCurve.

      A spline's own parameter is the point index, and a rational cubic bezier
      does not travel at a constant rate along it - so stepping the point index
      evenly gives rows that bunch up in the tight parts of the curve and spread
      out in the straight ones. Anything that lays a texture, a mesh or a
      placement along a spline wants to step by <marked>distance</marked>
      instead, and there is no closed form for that: the only way is to sample
      the curve, accumulate chord lengths and invert the table.

      This class is that table. Create it once per curve state, then convert
      freely in both directions.

      `samplesPerSegment` is the accuracy knob. The table's length is always a
      slight <marked>under</marked>estimate, because chords cut the corner off
      the curve they approximate - 16 samples per segment is well under a
      percent for anything that is not a hairpin.
  */
  public class SplineArcLengthTable
  {
    SplineCurve _curve;
    int _samplesPerSegment;

    /** Cumulative distance at sample i, which sits at point index
        i / samplesPerSegment. Monotonically non-decreasing by construction,
        which is what makes the binary search in PointIndexAtDistance valid.
    */
    float[] _distances;

    public SplineCurve curve => _curve;

    public float length => _distances.Length == 0 ? 0 : _distances[ _distances.Length - 1 ];

    public int numSegments => Mathf.Max( 0, _curve.points.Count - 1 );

    public static SplineArcLengthTable Create( SplineCurve curve, int samplesPerSegment = 16 )
    {
      var table = new SplineArcLengthTable();

      table._curve = curve;
      table._samplesPerSegment = Mathf.Max( 1, samplesPerSegment );
      table.Bake();

      return table;
    }

    void Bake()
    {
      var segments = numSegments;

      if ( segments <= 0 )
      {
        _distances = new float[]{ 0 };

        return;
      }

      var numSamples = segments * _samplesPerSegment + 1;

      _distances = new float[ numSamples ];

      var last = _curve.GetPositionByPointIndex( 0 );
      var distance = 0f;

      _distances[ 0 ] = 0;

      for ( var i = 1; i < numSamples; i++ )
      {
        var pointIndex = i / (float) _samplesPerSegment;
        var position = _curve.GetPositionByPointIndex( pointIndex );

        distance += ( position - last ).Length();
        _distances[ i ] = distance;

        last = position;
      }
    }

    /** Distance along the curve at a point index, interpolated between the two
        surrounding samples.
    */
    public float DistanceAtPointIndex( float pointIndex )
    {
      var maxIndex = _distances.Length - 1;
      var sample = Mathf.Clamp( pointIndex * _samplesPerSegment, 0, maxIndex );

      var lower = Mathf.FloorToInt( sample );
      var upper = Mathf.Min( lower + 1, maxIndex );

      return Mathf.Lerp( _distances[ lower ], _distances[ upper ], sample - lower );
    }

    /** The point index a distance along the curve lands on - the inverse of
        DistanceAtPointIndex, by binary search plus a linear step inside the
        sample it lands in.

        Distances outside the curve clamp to its ends rather than extrapolating.
    */
    public float PointIndexAtDistance( float distance )
    {
      var maxIndex = _distances.Length - 1;

      if ( maxIndex <= 0 || distance <= 0 )
      {
        return 0;
      }

      if ( distance >= length )
      {
        return maxIndex / (float) _samplesPerSegment;
      }

      var low = 0;
      var high = maxIndex;

      // Invariant: _distances[ low ] <= distance < _distances[ high ].
      while ( high - low > 1 )
      {
        var middle = ( low + high ) / 2;

        if ( _distances[ middle ] <= distance )
        {
          low = middle;
        }
        else
        {
          high = middle;
        }
      }

      var span = _distances[ high ] - _distances[ low ];

      // Two coincident samples - a zero-length span has no inside to land in.
      var fraction = span <= 0.000001f ? 0 : ( distance - _distances[ low ] ) / span;

      return ( low + fraction ) / _samplesPerSegment;
    }

    public Vector3 PositionAtDistance( float distance )
    {
      return _curve.GetPositionByPointIndex( PointIndexAtDistance( distance ) );
    }

    /** The curve's direction at a distance, as a central difference over
        `delta` meters. Falls back to a forward or backward difference at the
        ends, and to zero on a degenerate curve.
    */
    public Vector3 DirectionAtDistance( float distance, float delta )
    {
      var total = length;

      if ( total <= 0.000001f )
      {
        return Vector3.Zero;
      }

      var step = Mathf.Min( Mathf.Max( delta, 0.0001f ), total * 0.5f );

      var before = Mathf.Max( 0, distance - step );
      var after = Mathf.Min( total, distance + step );

      var direction = PositionAtDistance( after ) - PositionAtDistance( before );

      return direction.LengthSquared() < 0.000000001f ? Vector3.Zero : direction.Normalized();
    }
  }
}