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

namespace Rokojori
{
  public class SplineCurveTangent
  {
    public Vector3 position;
    public float weight = 1f;

    public SplineCurveTangent Clone()
    {
      var t = new SplineCurveTangent();
      t.position = position;
      t.weight = weight;

      return t;
    }

  } 

  public class SplineCurvePoint
  {
    public Vector3 position;
    public Quaternion rotation;
    public Vector3 scale;
    public float twist = 0f;
    public float weight = 1f; 

    public SplineCurveTangent tangentBefore = new SplineCurveTangent();
    public SplineCurveTangent tangentNext = new SplineCurveTangent();

    public SplineCurvePoint Clone()
    {
      var scp = new SplineCurvePoint();
      scp.position = position;
      scp.rotation = rotation;
      scp.scale = scale;
      scp.weight = weight;
      scp.twist = twist;
      scp.tangentBefore = tangentBefore.Clone();
      scp.tangentNext = tangentNext.Clone();

      return scp;
    }

    public SplineCurvePoint CloneForXZ( float y = 0 )
    {
      var cloned = Clone();
      cloned.position.Y = y;
      cloned.tangentBefore.position.Y = 0;
      cloned.tangentNext.position.Y = 0;

      return cloned;
    }

    public SplineCurvePoint CloneForXY( float z = 0 )
    {
      var cloned = Clone();
      cloned.position.Z = z;
      cloned.tangentBefore.position.Z = 0;
      cloned.tangentNext.position.Z = 0;

      return cloned;
    }

    public SplineCurvePoint ApplyPose( Pose p )
    {
      var cloned = Clone();
      cloned.position = p.Apply( cloned.position );
      cloned.rotation = cloned.rotation * p.rotation;
      cloned.tangentBefore.position = p.Apply( cloned.tangentBefore.position );
      cloned.tangentNext.position = p.Apply( cloned.tangentNext.position );

      return cloned;
    }

    

  }

  public class SplineCurve: Curve3
  {
    List<SplineCurvePoint> _points = new List<SplineCurvePoint>();
    
    public List<SplineCurvePoint> points => _points;

    public static SplineCurve From( List<SplineCurvePoint> points )
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = points;
      return splineCurve;
    }

    public Vector3 MinPointPosition()
    {
      if ( _points.Count == 0 )
      {
        return Vector3.Zero;
      }

      var min = _points[ 0 ].position;

      points.ForEach( p => min = min.Min( p.position ) );

      return min;
    }

    public Vector3 MaxPointPosition()
    {
      if ( _points.Count == 0 )
      {
        return Vector3.Zero;
      }

      var max = _points[ 0 ].position;

      points.ForEach( p => max = max.Max( p.position ) );

      return max;
    }


    public SplineCurve Clone()
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = Lists.Map( points, p => p.Clone() );
      return splineCurve;
    }

    public SplineCurve ApplyPose( Pose pose )
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = Lists.Map( points, p => p.ApplyPose( pose ) );
      return splineCurve;
    }

    public SplineCurve CloneForXZ( float y = 0 )
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = Lists.Map( points, p => p.CloneForXZ( y ) );
      return splineCurve;
    }

    public SplineCurve CloneForXY( float z  = 0 )
    {
      var splineCurve = new SplineCurve();
      splineCurve._points = Lists.Map( points, p => p.CloneForXY( z ) );
      return splineCurve;
    }

    

    
    public Vector3 GetPositionByPointIndex( float pointIndex )
    {
      if ( pointIndex <= 0 ||  pointIndex >= ( _points.Count - 1 ) )
      {
        var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );

        return _points[ index ].position;
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      return RationalCubicBezier.Compute( 
        lerpAmount,

        _points[ lower ].position, 
        _points[ lower ].tangentNext.position,
        _points[ higher ].tangentBefore.position,
        _points[ higher ].position,

        _points[ lower ].weight, 
        _points[ lower ].tangentNext.weight,
        _points[ higher ].tangentBefore.weight,
        _points[ higher ].weight
        
      );
    }

    public Pose GetPoseByPointIndex( float pointIndex )
    {
      if ( pointIndex <= 0 ||  pointIndex >= ( _points.Count - 1 ) )
      {
        var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );

        return Pose.Create( _points[ index ].position, _points[ index ].rotation );
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      var pLower  = _points[ lower ];
      var pHigher = _points[ higher ];

      var position = RationalCubicBezier.Compute( 
        lerpAmount,

        pLower.position, 
        pLower.tangentNext.position,
        pHigher.tangentBefore.position,
        pHigher.position,

        pLower.weight, 
        pLower.tangentNext.weight,
        pHigher.tangentBefore.weight,
        pHigher.weight
        
      );

      var rotation =  pLower.rotation.Slerp( pHigher.rotation, lerpAmount );

      return Pose.Create( position, rotation );
    }

    public Quaternion GetRotationByPointIndex( float pointIndex )
    {
      if ( pointIndex <= 0 ||  pointIndex >= ( _points.Count - 1 ) )
      {
        var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );

        return _points[ index ].rotation;
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      return _points[ lower ].rotation.Slerp( _points[ higher ].rotation, lerpAmount );
    } 

    public Transform3D GetTransformAt( float normalized )
    {
      return GetTransformByPointIndex( NormalizedToPointIndex( normalized ) );
    }

    public Transform3D GetTransformByPointIndex( float pointIndex )
    {
      var pose  = GetPoseByPointIndex( pointIndex );
      var scale = GetScaleByPointIndex( pointIndex );

      return Math3D.TRS( pose, scale ); 
    }

    public Vector3 GetScaleByPointIndex( float pointIndex )
    {
      if ( pointIndex <= 0 ||  pointIndex >= ( _points.Count - 1 ) )
      {
        var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );

        return _points[ index ].scale;
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      return _points[ lower ].scale.Lerp( _points[ higher ].scale, lerpAmount );
    }

    public float TwistAt( float normalized )
    {
      return SmoothStepTwistByPointIndex( NormalizedToPointIndex( normalized ) );
    }

    public float GetTwistByPointIndex( float pointIndex )
    {
      if ( pointIndex <= 0 ||  pointIndex >= ( _points.Count - 1 ) )
      {
        var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );

        return _points[ index ].twist;
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      return Mathf.Lerp( _points[ lower ].twist, _points[ higher ].twist, lerpAmount );
    }

    public T LerpByPointIndex<T>( float pointIndex, Func<int,T> get, Func<T,T,float,T> lerp )
    {
      if ( pointIndex <= 0 ||  pointIndex >= ( _points.Count - 1 ) )
      {
        var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );

        return get( index );
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      return lerp( get( lower ), get( higher ), lerpAmount );
    }

    public T SmoothStepByPointIndex<T>( float pointIndex, Func<int,T> get, Func<T,T,float,T> lerp )
    {
      if ( pointIndex <= 0 ||  pointIndex >= ( _points.Count - 1 ) )
      {
        var index = pointIndex <= 0 ? 0 : ( _points.Count - 1 );

        return get( index );
      }

      var lower  = Mathf.FloorToInt( pointIndex );
      var higher = Mathf.CeilToInt( pointIndex );

      var lerpAmount = pointIndex - lower;

      var smoothStepAmount = Mathf.SmoothStep( 0, 1, lerpAmount );
      
      return lerp( get( lower ), get( higher ), smoothStepAmount );
    }

    public Vector3 SmoothStepScaleByPointIndex( float pointIndex )
    {
      return SmoothStepByPointIndex<Vector3>( pointIndex, 
             ( id ) => _points[ id ].scale, 
             ( a, b, t ) => a.Lerp( b, t )
      );
    }

    public float SmoothStepTwistByPointIndex( float pointIndex )
    {
      return SmoothStepByPointIndex<float>( pointIndex, 
             ( id ) => _points[ id ].twist, 
             ( a, b, t ) => Mathf.Lerp( a, b, t )
      );
    }

    public override Vector3 PositionAt( float t )
    {
      if ( _points.Count <= 0 )
      {
        return Vector3.Zero;
      }

      var index = NormalizedToPointIndex( t );

      return GetPositionByPointIndex( index );
    }

    public override Quaternion RotationAt( float t )
    {
      
      if ( _points.Count <= 0 )
      {
        return Quaternion.Identity;
      }

      var index = NormalizedToPointIndex( t );

      return GetRotationByPointIndex( index );
    }

    public float NormalizedToPointIndex( float normalized )
    {
      return normalized * ( _points.Count - 1 );
    }

    public float PointIndexToNormalized( float pointIndex )
    {
      return pointIndex / ( _points.Count - 1 );
    }
  }
}