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



namespace Rokojori
{
  public class SplinesDeformerMappingData
  {
    public Vector3 localPosition;
    public Vector3 localNormal;
    public float normalizedSplineParameter;
    public float weight;
  }

  public class SplinesDeformModifier
  {
    public SplineCurve[] sourceSplines;
    public SplineCurve[] targetSplines;

    public SplinesDeformerSettings settings;

    

    public MeshGeometry Modify( MeshGeometry mg )
    { 
      var mappings = CreateSourceMappings( mg );
      return CreateDeformed( mg, mappings ); 
    }

    SplinesDeformerMappingData CreateSourceMapping( SplineCurve curve, Vector3 worldPosition, Vector3 worldNormal )
    {
      var closestParameter = curve.GetClosestParameterTo( worldPosition, settings.splineMappingResolution, settings.splineMappingDepth );
      var pointIndex = curve.NormalizedToPointIndex( closestParameter );
      var pose = curve.GetPoseByPointIndex( pointIndex );
      
      var localPosition = pose.ApplyInverse( worldPosition );
      var localNormal   = pose.rotation.Inverse() * worldNormal;

      var mappingData = new SplinesDeformerMappingData();
      
      mappingData.localPosition = localPosition;
      mappingData.localNormal   = localNormal;
      mappingData.normalizedSplineParameter = closestParameter;
      mappingData.weight = 0;

      return mappingData;
    }

    SplinesDeformerMappingData[] CreateSourceMappings( MeshGeometry meshGeometry)
    {
      var mappingSize = meshGeometry.vertices.Count * sourceSplines.Length;
      
      var deformerMappings = new SplinesDeformerMappingData[ mappingSize];
       
      for ( int i = 0; i < meshGeometry.vertices.Count; i++ )
      {
        var weights = 0f;

        for ( int j = 0; j < sourceSplines.Length; j++ )
        {
          var vertex = meshGeometry.vertices[ i ];
          var normal = meshGeometry.normals[ i ];
          var curve = sourceSplines[ j ];
          var mapping = CreateSourceMapping( curve, vertex, normal );
          
          var distance = curve.PositionAt( mapping.normalizedSplineParameter ) - vertex;
          var inverseWeight = MathX.NormalizeClamped( distance.Length(), settings.splineMinDistance, settings.splineMaxDistance );
          mapping.weight = 1f - inverseWeight; 
          weights += mapping.weight;
          deformerMappings[ i * sourceSplines.Length + j  ] = mapping;
        }

        if ( weights > 0 && weights != 1f )
        {
          for ( int j = 0; j < sourceSplines.Length; j++ )
          {
            var mapping = deformerMappings[ i * sourceSplines.Length + j  ];
            mapping.weight /= weights;
          }
        }
      }

      return deformerMappings;
    }

    MeshGeometry CreateDeformed( MeshGeometry meshGeometry,  SplinesDeformerMappingData[] mappingData )
    {
      var cloned = meshGeometry.Clone();

      for ( int i = 0; i < cloned.vertices.Count; i++ )
      {
        var vertex = Vector3.Zero;
        var normal = Vector3.Zero;

        for ( int j = 0; j < targetSplines.Length; j++ )
        {
          var mapping = mappingData[ i * targetSplines.Length + j  ];
          var curve = targetSplines[ j ];

          if ( settings.targetSmoothing > 0 )
          {            
            var pose = curve.SmoothedPoseAt( mapping.normalizedSplineParameter, settings.targetSmoothing * 0.5f, 2, settings.targetSmoothing ); 
            pose.ApplyTwist( curve.TwistAt( mapping.normalizedSplineParameter ) );
            vertex += pose.Apply( mapping.localPosition ) * mapping.weight;
            normal += pose.rotation * mapping.localNormal * mapping.weight;

          }
          else
          { 
            var pose = curve.PoseAt( mapping.normalizedSplineParameter );
            pose.ApplyTwist( curve.TwistAt( mapping.normalizedSplineParameter ) );
            vertex += pose.Apply( mapping.localPosition ) * mapping.weight;
            normal += pose.rotation * mapping.localNormal * mapping.weight;
          } 
          
        }

        cloned.vertices[ i ] = vertex;
        cloned.normals[ i ] = normal.Normalized();
      }

      return cloned;
    }
    
  }
}