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

namespace Rokojori
{
  public class SplineCurveCreator
  {
    bool closed;   

    public SplineCurve Create( List<SplinePoint> splinePoints, bool close = false )
    {
      closed = close;
      var points = new List<SplineCurvePoint>();

      for ( int i = 0; i < splinePoints.Count; i++ )
      {
        points.Add( CreatePoint( splinePoints, i ) );
      }

      if ( closed )
      {
        points.Add( CreatePoint( splinePoints, 0 ) );
      }

      return SplineCurve.From( points );
    }

    public SplineCurve Create( List<Vector3> splinePoints, bool close = false )
    {
      closed = close;
      var points = new List<SplineCurvePoint>();

      for ( int i = 0; i < splinePoints.Count; i++ )
      {
        points.Add( CreatePoint( splinePoints, i ) );
      }

      if ( closed )
      {
        points.Add( CreatePoint( splinePoints, 0 ) );
      }

      return SplineCurve.From( points );
    }

    SplineCurvePoint CreatePoint( List<SplinePoint> splinePoints, int index )
    {
      var splineCurvePoint = new SplineCurvePoint();
      var splinePoint = splinePoints[ index ];
      splineCurvePoint.position = splinePoint.GlobalPosition;
      splineCurvePoint.rotation = splinePoint.GetGlobalQuaternion();
      splineCurvePoint.scale  = splinePoint.Scale;
      splineCurvePoint.twist = splinePoint.twist;
      splineCurvePoint.weight = 1f;
      
      splineCurvePoint.tangentBefore.position = GetTangentPosition( splinePoints, index, true, closed );
      splineCurvePoint.tangentNext.position   = GetTangentPosition( splinePoints, index, false, closed );

      splineCurvePoint.tangentBefore.weight = splinePoint.tangentBeforeWeight;
      splineCurvePoint.tangentNext.weight = splinePoint.tangentNextWeight;

      return splineCurvePoint;
    }


    SplineCurvePoint CreatePoint( List<Vector3> splinePoints, int index )
    {
      var splineCurvePoint = new SplineCurvePoint();
      var splinePoint = splinePoints[ index ];
      splineCurvePoint.position = splinePoint;
      splineCurvePoint.rotation = Quaternion.Identity;
      splineCurvePoint.scale  = Vector3.One;
      splineCurvePoint.twist = 0;
      splineCurvePoint.weight = 1f;
      
      splineCurvePoint.tangentBefore.position = GetTangentPosition( splinePoints, index, true, closed );
      splineCurvePoint.tangentNext.position   = GetTangentPosition( splinePoints, index, false, closed );

      splineCurvePoint.tangentBefore.weight = 1;
      splineCurvePoint.tangentNext.weight = 1;

      return splineCurvePoint;
    }

    public static Vector3 GetTangentDirectionSmoothed( float smoothing, List<SplinePoint> splinePoints, int index, bool before, bool closed )
    {
      var previousIndex = MathX.SafeIndex( index - 1, splinePoints.Count, closed );
      var currentIndex  = index;
      var nextIndex     = MathX.SafeIndex( index + 1, splinePoints.Count, closed );

      var previousDirection = GetTangentDirection( splinePoints, previousIndex, before, closed );
      var currentDirection  = GetTangentDirection( splinePoints, currentIndex, before, closed );
      var nextDirection     = GetTangentDirection( splinePoints, nextIndex, before, closed );

      var smoothed = ( previousDirection + currentDirection + nextDirection ) / 3.0f;   

      return currentDirection + smoothing * ( smoothed - currentDirection );

    }


    public static Vector3 GetTangentDirectionSmoothed( int numSamples, float smoothing, List<SplinePoint> splinePoints, int index, bool before, bool closed )
    {
      var smoothedTangent = Vector3.Zero;
      var unsmoothedTangent = Vector3.Zero;

      for ( int i = -numSamples; i <= numSamples; i++ )
      {
        var sampleIndex = MathX.SafeIndex( index + i, splinePoints.Count, closed );
        var direction = GetTangentDirection( splinePoints, sampleIndex, before, closed );
        smoothedTangent += direction;

        if ( i == 0 )
        {
          unsmoothedTangent = direction;
        }
      }

      smoothedTangent /= ( numSamples * 2 + 1 );

      return unsmoothedTangent + smoothing * ( smoothedTangent - unsmoothedTangent );
    }

    public static Vector3 GetTangentDirectionSmoothed( int numSamples, float smoothing, List<SplineCurvePoint> splinePoints, int index, bool before, bool closed )
    {
      var smoothedTangent = Vector3.Zero;
      var unsmoothedTangent = Vector3.Zero;

      for ( int i = -numSamples; i <= numSamples; i++ )
      {
        var sampleIndex = MathX.SafeIndex( index + i, splinePoints.Count, closed );
        var direction = GetTangentDirection( splinePoints, sampleIndex, before, closed );
        smoothedTangent += direction;

        if ( i == 0 )
        {
          unsmoothedTangent = direction;
        }
      }

      smoothedTangent /= ( numSamples * 2 + 1 );

      return unsmoothedTangent + smoothing * ( smoothedTangent - unsmoothedTangent );
    }


    public static Vector3 GetTangentDirection( List<SplinePoint> splinePoints, int index, bool before, bool closed )
    {
      var splinePoint = splinePoints[ index ];

      return GetTangentPosition( splinePoints, index, before, closed ) - splinePoint.GlobalPosition;
    } 

    public static Vector3 GetTangentDirection( List<SplineCurvePoint> splinePoints, int index, bool before, bool closed,
    float overshootPrevention = 0, float tangentScale = 1, float symmetricTangentLength = 0 )
    {
      var splinePoint = splinePoints[ index ];

      return GetTangentPosition( splinePoints, index, before, closed, overshootPrevention, tangentScale, symmetricTangentLength ) - splinePoint.position;
    } 

    

    public static Vector3 GetTangentPosition( List<SplineCurvePoint> splinePoints, int index, bool before, bool closed,
    float overshootPrevention = 0, float tangentScale = 1, float symmetricTangentLength = 0 )
    {
      var splinePoint = splinePoints[ index ];
      
      var previousIndex = index - 1;
      
      if ( previousIndex == -1 )      
      {
        previousIndex = closed ? splinePoints.Count -1  : 0;
      }

      var nextIndex = index + 1;

      if ( nextIndex == splinePoints.Count )  
      {
        nextIndex = closed ? 0 : splinePoints.Count - 1;
      }

      var previous = splinePoints[ previousIndex ];
      var next     = splinePoints[ nextIndex ];

      var previousPosition = previous.position;
      var nextPosition     = next.position;

      var position =  splinePoint.position;

      return GetTangentPosition( position, previousPosition, nextPosition, before, overshootPrevention, tangentScale, symmetricTangentLength );

    }

    public static Vector3 GetTangentPosition( List<Vector3> splinePoints, int index, bool before, bool closed,
    float overshootPrevention = 0, float tangentScale = 1, float symmetricTangentLength = 0 )
    {
      var splinePoint = splinePoints[ index ];
      
      var previousIndex = index - 1;
      
      if ( previousIndex == -1 )      
      {
        previousIndex = closed ? splinePoints.Count -1  : 0;
      }

      var nextIndex = index + 1;

      if ( nextIndex == splinePoints.Count )  
      {
        nextIndex = closed ? 0 : splinePoints.Count - 1;
      }

      var previous = splinePoints[ previousIndex ];
      var next     = splinePoints[ nextIndex ];

      var previousPosition = previous;
      var nextPosition     = next;

      var position =  splinePoint;

      return GetTangentPosition( position, previousPosition, nextPosition, before, overshootPrevention, tangentScale, symmetricTangentLength );

    }

    public static Vector3 GetTangentPosition( List<SplinePoint> splinePoints, int index, bool before, bool closed )
    {
      var splinePoint = splinePoints[ index ];

      if ( splinePoint.tangentMode == SplinePointTangentMode.Custom )
      {
        return before ? splinePoint.tangentBefore.GlobalPosition : 
                        splinePoint.tangentNext.GlobalPosition;
      }
      
      var previousIndex = index - 1;
      
      if ( previousIndex == -1 )      
      {
        previousIndex = closed ? splinePoints.Count -1  : 0;
      }

      var nextIndex = index + 1;

      if ( nextIndex == splinePoints.Count )  
      {
        nextIndex = closed ? 0 : splinePoints.Count - 1;
      }

      var previous = splinePoints[ previousIndex ];
      var next     = splinePoints[ nextIndex ];

      var previousPosition = previous.GlobalPosition;
      var nextPosition     = next.GlobalPosition;

      

      var position =  splinePoint.GlobalPosition;

      var overshootPrevention = splinePoint.overshootPrevention;
      var tangentScale = splinePoint.tangentScale;
      var symmetricTangentLength = splinePoint.symmetricTangentLength;

      return GetTangentPosition( position, previousPosition, nextPosition, before, overshootPrevention, tangentScale, symmetricTangentLength );

    }

    
    public static Vector3 GetTangentPosition( Vector3 point, Vector3 previousPosition, Vector3 nextPosition, bool before,
                                              float overshootPrevention = 0, float tangentScale = 1, float symmetricTangentLength = 0 )
    {
      

      if ( overshootPrevention > 0 )
      {
        var previousDirection = ( previousPosition - point ) ;
        var nextDirection    = ( nextPosition - point );

        var previousLength = previousDirection.Length();
        var nextLength     = nextDirection.Length();

        if ( previousLength > nextLength )
        {
          previousPosition = Math3D.Lerp( 
            previousPosition, point + previousDirection.Normalized() * nextLength, 
            overshootPrevention
          );
        }
        else
        {
          nextPosition = Math3D.Lerp( 
            nextPosition, point + nextDirection.Normalized() * previousLength, 
            overshootPrevention
          );
        }
      } 

      var direction = nextPosition - previousPosition;
      var length = 0f;

      var lengthBefore = ( point - previousPosition ).Length();
      var lengthNext   = ( point - nextPosition ).Length();

      var lengthAverage = ( lengthBefore + lengthNext ) * 0.5f;
      
      if ( symmetricTangentLength > 0 )
      {
        lengthBefore = Mathf.Lerp( lengthBefore, lengthAverage, symmetricTangentLength );
        lengthNext   = Mathf.Lerp( lengthNext, lengthAverage, symmetricTangentLength ); 
      }

      if ( before )
      {
        length = -lengthBefore;
      }
      else
      {
        length = lengthNext;
      }

      return point + direction.Normalized() * length * 0.33333f * tangentScale;
    }
  }

}