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

namespace Rokojori
{
  public class FresnetFrame
  {
    public Vector3 tangent;
    public Vector3 normal;
    public Vector3 binormal;
  }

	public abstract class Curve3
	{
    float _tangentSamplingRange = Mathf.Pow( 10f, -8f );
    public abstract Vector3 PositionAt( float t );

    public virtual Quaternion RotationAt( float t )
    {
      return RotationFromTangent( t );
    }
    

    public Quaternion RotationFromTangent( float t )
    {
      // RJLog.Log( "Sampling Rotation From Tangent" );
      var gradient = TangentAt( t );
      return Math3D.LookRotation( gradient, Vector3.Up );
    }

    public virtual Pose PoseAt( float t )
    {
      return Pose.Create( PositionAt( t ), RotationAt( t ) );
    }

    public virtual Pose SmoothedPoseAt( float t, float stepSize = 0.01f, int steps = 1, float fallOff = 0.5f )
    {
      var poses = new List<Pose>();

      poses.Add( PoseAt( t ) );

      var weigths = new List<float>();

      var weight = 1f;
      weigths.Add( weight );

      var sumWeigths = weight;

      for ( int i = 0; i < steps; i++ )
      {
        var poseBefore = PoseAt( t - stepSize * i );
        var poseAfter  = PoseAt( t + stepSize * i );

        poses.Add( poseBefore );
        poses.Add( poseAfter );

        weight *= fallOff;
        weigths.Add( weight );
        weigths.Add( weight );

        sumWeigths += weight * 2;
      }

      for ( int i = 0; i < weigths.Count; i++ )
      {
        weigths[ i ] = weigths[ i ] / sumWeigths;
      }

      return Pose.Merge( poses, weigths );
    }
    
    public virtual void SampleMultiple( int numSamples, List<Vector3> output )
    {
      SampleMultiple( new Range( 0, 1 ), numSamples, output );
    }
    
    public virtual void SampleMultiple( Range range, int numSamples, List<Vector3> output )
    {
      var diff = range.length / ( numSamples - 1 ) ;
      
      for ( var i = 0 ; i < numSamples; i++ )
      {
        var t = range.min + i * diff;
        output.Add( PositionAt( t ) );
      }
    }

    public virtual Path2 SampleXZPath( int numSamples )
    {
      return SampleXZPath( new Range( 0, 1 ), numSamples );
    }
    
    public virtual Path2 SampleXZPath( Range range, int numSamples )
    {
      var diff = range.length / ( numSamples - 1 ) ;
      var points = new List<Vector2>();
      
      for ( var i = 0 ; i < numSamples; i++ )
      {
        var t = range.min + i * diff;
        var p = PositionAt( t );
        points.Add( new Vector2( p.X, p.Z ) );
        
      }

      return new Path2( points );
    }

    public virtual Path2 SampleXYPath( int numSamples )
    {
      return SampleXYPath( new Range( 0, 1 ), numSamples );
    }
    
    public virtual Path2 SampleXYPath( Range range, int numSamples )
    {
      var diff = range.length / ( numSamples - 1 ) ;
      var points = new List<Vector2>();
      
      for ( var i = 0 ; i < numSamples; i++ )
      {
        var t = range.min + i * diff;
        var p = PositionAt( t );
        points.Add( new Vector2( p.X, p.Y ) );
        
      }

      return new Path2( points );
    }

    public const int CurveLengthDivisions = 20;

    List<float> _curveLengths = null;
    
    public void ClearCurveLengths()
    {
      _curveLengths = null;
    }


    List<float> _getCurveLengths( int divisions = CurveLengthDivisions ) 
    {

      if ( _curveLengths != null && _curveLengths.Count == ( divisions + 1 ) ) 
      {
        return _curveLengths;
      }


      _curveLengths = new List<float>();
      _curveLengths.Add( 0f );

      var before = PositionAt( 0 );
      var sum = 0f;

      for ( int i = 1; i <= divisions; i ++ ) {

        var p = PositionAt( (float) i / divisions );
        sum += p.DistanceTo( before );

        _curveLengths.Add( sum );
        before = p;

      }

      return _curveLengths;
    }

    public float UtoT( float normalizedCurveLength, int numCurveLengths = -1 ) 
    { 
      return ComputeTforNormalizedCurveLength( normalizedCurveLength, numCurveLengths );
    }

    public float ComputeTforNormalizedCurveLength( float normalizedCurveLength, int numCurveLengths = -1 ) 
    {

      var curveLengths = numCurveLengths < 0 ? _getCurveLengths() : _getCurveLengths( numCurveLengths );

      var i = 0;
      numCurveLengths = curveLengths.Count;

      var targetCurveLength = normalizedCurveLength * curveLengths[ numCurveLengths - 1 ];

      var low = 0;
      var high = numCurveLengths - 1;
      var comparison = 0f;

      while ( low <= high ) 
      {
        i = Mathf.FloorToInt( low + ( high - low ) / 2 ); 

        comparison = curveLengths[ i ] - targetCurveLength;

        if ( comparison < 0 ) 
        {
          low = i + 1;
        } 
        else if ( comparison > 0 ) 
        {
          high = i - 1;
        } 
        else 
        {
          high = i;
          break;
        }

      }

      i = high;

      if ( curveLengths[ i ] == targetCurveLength ) 
      {
        return i / ( numCurveLengths - 1 );
      }


      var lengthBefore = curveLengths[ i ];
      var lengthAfter  = curveLengths[ i + 1 ];

      var segmentLength = lengthAfter - lengthBefore;

      var segmentFraction = ( targetCurveLength - lengthBefore ) / segmentLength;


      var t = ( i + segmentFraction ) / ( numCurveLengths - 1 );

      return t;

    }

    public virtual Vector3 TangentAt( float t )
    {
      return TangentAt( t, _tangentSamplingRange );
    } 

    public virtual Vector3 TangentAt( float t, float samplingRange )
    {
      return PositionAt( t + samplingRange ) - PositionAt( t - samplingRange );
    } 

    public virtual float ComputeLength( int numSamples )
    {
      return ComputeSubLength( Range.Of_01 , numSamples );
    }    

    public List<FresnetFrame> ComputeFresnetFrames( int segments, bool closed )
    {
      var frames = _GetFresnetFramesWithTangents( segments, closed );

      _ComputeInitialNormalVector( frames );
      _ComputeNormalsAndBinormals( frames, closed );

      return frames;
    }

    List<FresnetFrame> _GetFresnetFramesWithTangents( int segments, bool closed )
    {
      var frames = new List<FresnetFrame>();

      for ( int i = 0; i <= segments; i++ ) 
      {
        var fresnetFrame = new FresnetFrame();
        var u = (float) i / segments;
        var t = ComputeTforNormalizedCurveLength( u );
        fresnetFrame.tangent = TangentAt( t ) ;
        frames.Add( fresnetFrame );
      }

      return frames;
    }

    void _ComputeInitialNormalVector( List<FresnetFrame> frames )
    {
      frames[ 0 ].normal = new Vector3();
      frames[ 0 ].binormal = new Vector3();

      var min = float.MaxValue;

      var tx = Math.Abs( frames[ 0 ].tangent.X );
      var ty = Math.Abs( frames[ 0 ].tangent.Y );
      var tz = Math.Abs( frames[ 0 ].tangent.Z );

      var normal = Vector3.Zero;
      
      if ( tx <= min ) 
      {
        min = tx;
        normal = new Vector3( 1, 0, 0 );

      }

      if ( ty <= min ) 
      {
        min = ty;
        normal = new Vector3( 0, 1, 0 );

      }

      if ( tz <= min ) 
      {
        normal = new Vector3( 0, 0, 1 );
      }

      var vec = frames[ 0 ].tangent.Cross( normal ).Normalized();

      frames[ 0 ].normal   = frames[ 0 ].tangent.Cross( vec );
      frames[ 0 ].binormal = frames[ 0 ].tangent.Cross( frames[ 0 ].normal );
    }

    void _ComputeNormalsAndBinormals( List<FresnetFrame> frames, bool closed )
    {
      for ( int i = 1; i < frames.Count; i++ ) 
      {
        frames[ i ].normal   = frames[ i - 1 ].normal;
        frames[ i ].binormal = frames[ i - 1 ].binormal;

        var vec = frames[ i - 1 ].tangent.Cross( frames[ i ].tangent );

        if ( vec.Length() > Mathf.Epsilon ) 
        {
          vec = vec.Normalized();

          var dot = frames[ i - 1 ].tangent.Dot( frames[ i ].tangent );
          var angle = Mathf.Acos( Mathf.Clamp( dot, - 1, 1 ) ); 

          var rotationMatrix = new Transform3D().Rotated( vec, angle );
          frames[ i ].normal = frames[ i ].normal * rotationMatrix;
        }

        frames[ i ].binormal = frames[ i ].tangent.Cross( frames[ i ].normal ); 

	  	}

      if ( ! closed  ) 
      {
        return;
      }


      var d = frames[ 0 ].normal.Dot( frames[ frames.Count - 1 ].normal );

			var theta = Mathf.Acos( Mathf.Clamp( d, - 1, 1 ) );
			theta /= frames.Count;

			if ( frames[ 0 ].tangent.Dot( frames[ 0 ].normal.Cross( frames[ frames.Count - 1 ].normal ) ) > 0 ) {

				theta = -theta;
			}

			for ( int i = 1; i < frames.Count; i ++ ) 
      {
        var rm = new Transform3D().Rotated( frames[ i ].tangent, theta * i );
        frames[ i ].normal = frames[ i ].normal * rm;
				frames[ i ].binormal = frames[ i ].tangent.Cross( frames[ i ].normal );
			}
      
    }



    public virtual float ComputeSubLength( Range range, int numSamples )
    {
      var diff = range.length / ( numSamples - 1 ) ;
      var it = PositionAt( range.min );
      var length = 0f;

      for ( var i = 1 ; i < numSamples; i++ )
      {
        var t = range.min + i * diff;
        var next = PositionAt( t );
        length += ( next - it ).Length();
        it = next;
      }

      return length;
    }
    
    public virtual int ComputeNumSamplesFor( Range range,
    float minimumDistance, int minSamples = 2, 
    int numSamplesForLengthComputation = -1 )
    {
      if ( minSamples < 2 )
      { minSamples = 2; }
      
      if ( numSamplesForLengthComputation <= 0  )
      { numSamplesForLengthComputation = minSamples * 4; }

      float distance = ComputeSubLength( range, numSamplesForLengthComputation );
      float numFloatingCuts = distance / minimumDistance;
      int numSamples = Mathf.CeilToInt( numFloatingCuts );

      return Mathf.Max( minSamples, numSamples );
    }

    public Vector3 GetClosestPositionTo( Vector3 position, int numSegments = 20, int depth = 0 )
    {
      var parameter = GetClosestParameterTo( position, numSegments, depth );

      return PositionAt( parameter );
    }

    public float GetClosestParameterTo( Vector3 point, int numSegments = 20, int depth = 3 )
    {
      return GetClosestParameterTo( 0, 1, point, numSegments, depth, 0 );
    }

    public float GetDistanceTo( Vector3 point, int numSegments = 20, int depth = 3 )
    {
      var p = GetClosestPositionTo( point, numSegments, depth );
      return ( p - point ).Length();
    }

    protected float GetClosestParameterTo( float tStart, float tEnd, Vector3 position, int numSegments, int maxDepth, int currentDepth = 0 )
    {
      var tNormalizer = ( tEnd - tStart ) / (float) ( numSegments - 1 );

      var closestIndex = -1;      
      var closestDistance = float.MaxValue;
      var minT = 0f;
      var maxT = 1f;
      var startPoint = PositionAt( tStart );

      var line = new Line3();
      var lastT = tStart;

      for ( int i = 1; i < numSegments; i++ )
      {
        var t = tNormalizer * i + tStart;
        var nextCurvePoint = PositionAt( t );

        line.Set( startPoint, nextCurvePoint );
        
        var closestPoint = line.ClosestPointToPoint( position );

        var distance = ( position - closestPoint ).LengthSquared();

        if ( distance < closestDistance )
        {
          closestDistance = distance;
          closestIndex = i - 1;
          minT = lastT;
          maxT = t;
        }

        startPoint = nextCurvePoint;
        lastT = t;
      }

      if ( maxDepth != currentDepth )
      { 
        return GetClosestParameterTo( minT, maxT, position, numSegments, maxDepth, currentDepth + 1 );
      }

      var tNormalizer2 = ( maxT - minT ) / (float)(numSegments - 1 );
      
      closestDistance = float.MaxValue;
      var closestParameter = 0.5f;

      for ( int i = 0; i < numSegments; i++ )
      {
        var detailedT = i * tNormalizer2 + minT;
        var sampledPoint = PositionAt( detailedT );

        var distance = ( sampledPoint - position ).LengthSquared();

        if ( distance < closestDistance )
        {
          closestParameter = detailedT;
          closestDistance = distance;
        }
      }

      return closestParameter;      
      
    }

        
  }
}