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


namespace Rokojori
{
  public class LODLerpingData
  {
    public Curve lowerLevelWeights  = MathX.Curve( 0, 1 );
    public Curve higherLevelWeights = MathX.Curve( 0, 1 );

    public int lerpSteps = 3;

    public void Sample( float weight, ListView<int> source, List<int> output )
    { 
      var numSamples = Mathf.Round( weight * source.Count );

      var sourceCopy = source.SubList();
      sourceCopy.Shuffle( 7 );
      sourceCopy.Shuffle( 3 );
      sourceCopy.Shuffle( 5 );
      sourceCopy.ShuffleMultiple( 5 );

      for ( int i = 0; i < numSamples; i++ )
      {
        output.Add( sourceCopy[ i ]  );
      }
    }

    public float Sample( ListView<int> lower, ListView<int> higher, List<int> output, int step, float lowerSize, float higherSize )
    {
      var t = lerpSteps == 1 ? 0.5f : ( ( step + 1 )/ ( ( lerpSteps + 2 )  - 1f ) );

      // 4
      // 0 => ( 0 + 1 ) / ( ( 4 + 2 ) -1 ) => 1 / ( 5 ) => 0.2
      // 1 => ( 1 + 1 ) / ( ( 4 + 2 ) -1 ) => 2 / ( 5 ) => 0.4
      // 2 => ( 2 + 1 ) / ( ( 4 + 2 ) -1 ) => 3 / ( 5 ) => 0.6
      // 3 => ( 3 + 1 ) / ( ( 4 + 2 ) -1 ) => 4 / ( 5 ) => 0.8 

      var lowerWeight = lowerLevelWeights.Sample( 1f - t );
      var higherWeight = higherLevelWeights.Sample( t );

      Sample( lowerWeight, lower, output );
      Sample( higherWeight, higher, output );       

      return Mathf.Lerp( lowerSize, higherSize, t );
    }
  }

  public class CustomMeshAttributeList
  {
    protected int customIndex = 0;
    public int index => customIndex;

    protected ArrayMesh.ArrayCustomFormat customFormat;
    public ArrayMesh.ArrayCustomFormat format => customFormat;
    
    public CustomMeshAttributeList( int customIndex, ArrayMesh.ArrayCustomFormat customFormat )
    {
      this.customIndex = customIndex;
      this.customFormat = customFormat;
    }

    public Mesh.ArrayFormat GetFormatFlag()
    {
      var channel = GetChannelFormat();
      var shift = GetShift();

      return channel | (Mesh.ArrayFormat)( ( (long)customFormat ) << (int)shift );
    }

    public virtual CustomMeshAttributeList Clone()
    {
      return null;
    }


    public SurfaceTool.CustomFormat GetSurfaceToolFormat()
    {
      if ( ArrayMesh.ArrayCustomFormat.RgbaFloat == customFormat )
      {
        return SurfaceTool.CustomFormat.RgbaFloat;
      }

      if ( ArrayMesh.ArrayCustomFormat.RgbFloat == customFormat )
      {
        return SurfaceTool.CustomFormat.RgbFloat;
      }
      
      if ( ArrayMesh.ArrayCustomFormat.RgFloat == customFormat )
      {
        return SurfaceTool.CustomFormat.RgFloat;
      }

      if ( ArrayMesh.ArrayCustomFormat.RFloat == customFormat )
      {
        return SurfaceTool.CustomFormat.RFloat;
      }

      return SurfaceTool.CustomFormat.Max;
    }

    public ArrayMesh.ArrayType GetTypeSlot()
    {
      if ( customIndex == 0 )
      {
        return ArrayMesh.ArrayType.Custom0;
      }

      if ( customIndex == 1 )
      {
        return ArrayMesh.ArrayType.Custom1;
      }

      if ( customIndex == 2 )
      {
        return ArrayMesh.ArrayType.Custom2;
      }

      if ( customIndex == 3 )
      {
        return ArrayMesh.ArrayType.Custom3;
      }

      return ArrayMesh.ArrayType.Max;

    }

    public ArrayMesh.ArrayFormat GetChannelFormat()
    {
      if ( customIndex == 0 )
      {
        return ArrayMesh.ArrayFormat.FormatCustom0;
      }

      if ( customIndex == 1 )
      {
        return ArrayMesh.ArrayFormat.FormatCustom1;
      }

      if ( customIndex == 2 )
      {
        return ArrayMesh.ArrayFormat.FormatCustom2;
      }

      if ( customIndex == 3 )
      {
        return ArrayMesh.ArrayFormat.FormatCustom3;
      }

      return 0;

    }

    public long GetShift()
    {
      if ( customIndex == 0 )
      {
        return (long)ArrayMesh.ArrayFormat.FormatCustom0Shift;
      }

      if ( customIndex == 1 )
      {
        return (long)ArrayMesh.ArrayFormat.FormatCustom1Shift;
      }

      if ( customIndex == 2 )
      {
        return (long)ArrayMesh.ArrayFormat.FormatCustom2Shift;
      }

      if ( customIndex == 3 )
      {
        return (long)ArrayMesh.ArrayFormat.FormatCustom3Shift;
      }

      return 0;
    }

    public virtual void WriteData( Godot.Collections.Array array )
    {

    }

    public virtual void Write( SurfaceTool surfaceTool, int index )
    {
      
    }

    public virtual void AddTo( CustomMeshAttributeList list, int sourceIndex )
    {

    }
  }

  public abstract class CustomMeshAttributeList<T> : CustomMeshAttributeList
  {    
    public List<T> values = new List<T>();
    public CustomMeshAttributeList( int customIndex, ArrayMesh.ArrayCustomFormat customFormat ):base( customIndex, customFormat )
    {}
  }


  public class MeshAttributeVector4List : CustomMeshAttributeList<Vector4>
  {
    public MeshAttributeVector4List( int customIndex ):base( customIndex, ArrayMesh.ArrayCustomFormat.RgbaFloat )
    {}

    public override CustomMeshAttributeList Clone()
    {
      var v4 = new MeshAttributeVector4List( customIndex );
      v4.values = Lists.Clone( values );

      return v4;
    }

    public override void WriteData( Godot.Collections.Array array )
    {
      var colors = new List<float>();


      values.ForEach(
        ( v ) =>
        {
          colors.AddRange( new float[]{ v.X, v.Y, v.Z, v.W } );
        }
      );

      RJLog.Log( GetTypeSlot(), customFormat, colors.Count );
      array[ (int) GetTypeSlot() ] = colors.ToArray();


    }

      
    public override void Write( SurfaceTool tool, int index )
    {
      tool.SetCustom( this.index, new Color( values[ index ].X, values[ index ].Y, values[ index ].Z, values[ index ].W) );
    }

    public override void AddTo( CustomMeshAttributeList list, int sourceIndex )
    {
      var typeList = list as MeshAttributeVector4List;

      typeList.values.Add( values[ sourceIndex ] );
    }

  }



  public class MeshGeometry
  {
    public string name;
    
    public List<Vector3> vertices = new List<Vector3>();
    public int numVertices => Lists.Size( vertices );
    
    public List<int> indices = new List<int>();

    public List<Vector3> normals;
    public int numNormals => Lists.Size( normals );
    
    public List<Vector2> uvs;
    public int numUVs => Lists.Size( uvs );

    public List<Vector2> uv2s;
    public int numUV2s => Lists.Size( uv2s );

    public List<Color> colors;
    public int numColors => Lists.Size( colors );

    public float lodEdgeLength = 0;

    public List<CustomMeshAttributeList> customMeshAttributes = new List<CustomMeshAttributeList>();

    public int numTriangles => indices.Count / 3;
    

    public static MeshGeometry From( MeshInstance3D meshInstance3D )
    {
      return From( (ArrayMesh)meshInstance3D.Mesh, meshInstance3D.GlobalTransform );
    }

    public static MeshGeometry From( ArrayMesh mesh, Transform3D? trsf = null, int index = 0 )
    {
      var mg = new MeshGeometry();

      var arrays = mesh.SurfaceGetArrays( index );

      var vertices = arrays[ (int) Mesh.ArrayType.Vertex ];

      if ( Variant.Type.Nil != vertices.VariantType  )
      {
        mg.vertices = new List<Vector3>( vertices.AsVector3Array() );
      }

      var normals = arrays[ (int) Mesh.ArrayType.Normal ];

      if ( Variant.Type.Nil != normals.VariantType  )
      {
        mg.normals = new List<Vector3>( normals.AsVector3Array() );
      }

      var uvs = arrays[ (int) Mesh.ArrayType.TexUV ];

      if ( Variant.Type.Nil != uvs.VariantType  )
      {
        mg.uvs = new List<Vector2>( uvs.AsVector2Array() );
      }

      var uv2s = arrays[ (int) Mesh.ArrayType.TexUV2 ];

      if ( Variant.Type.Nil != uv2s.VariantType  )
      {
        mg.uv2s = new List<Vector2>( uv2s.AsVector2Array() );
      }

      var colors = arrays[ (int) Mesh.ArrayType.Color ];

      if ( Variant.Type.Nil != colors.VariantType  )
      {
        mg.colors = new List<Color>( colors.AsColorArray() );
      }

      var indices = arrays[ (int) Mesh.ArrayType.Index ];

      if ( Variant.Type.Nil != indices.VariantType  )
      {
        mg.indices = new List<int>( indices.AsInt32Array() );
      }

      if ( trsf != null )
      {
        mg.ApplyTransform( (Transform3D)trsf );
      }

      return mg;
    }

    public void ApplyTransform( Vector3 translation, Quaternion rotation, Vector3 scale, int start = 0, int length = -1 )
    {
      ApplyTransform( Math3D.TRS( translation, rotation, scale ), start, length );
    }

    public void ApplyTranslation( Vector3 translation, int start = 0, int length = -1 )
    {
      if ( start < 0 )
      {
        start = vertices.Count + start;
      }

      length = length < 0 ? ( vertices.Count - start ) : length;

      var end = start + length;
      
      for ( int i = start; i < end; i++ )
      {
        vertices[ i ] = vertices[ i ] + translation;
      }
    }


   public void ApplyUVTransform( Transform2D transform, int start = 0, int length = -1 )
    {
      if ( start < 0 )
      {
        start = vertices.Count + start;
      }

      length = length < 0 ? ( vertices.Count - start ) : length;

      var end = start + length;

      for ( int i = start; i < end; i++ )
      {
        uvs[ i ] = transform * uvs[ i ];
      }
    }


    public void ApplyTransform( Transform3D trsf, int start = 0, int length = -1 )
    {
      if ( start < 0 )
      {
        start = vertices.Count + start;
      }

      length = length < 0 ? ( vertices.Count - start ) : length;

      var end = start + length;

      for ( int i = start; i < end; i++ )
      {
        vertices[ i ] = trsf * vertices[ i ];
        normals[ i ] = trsf.Basis.GetRotationQuaternion() * normals[ i ];
      }
    }

    public void ApplyTransformWithPivot( Transform3D trsf, Vector3 pivot, int start = 0, int length = -1 )
    {
      if ( start < 0 )
      {
        start = vertices.Count + start;
      }

      length = length < 0 ? ( vertices.Count - start ) : length;

      var end = start + length;

      for ( int i = start; i < end; i++ )
      {
        vertices[ i ] = ( trsf * ( vertices[ i ] - pivot ) ) + pivot;
        normals[ i ] = trsf.Basis.GetRotationQuaternion() * normals[ i ];
      }
    }

    public void NormalsLookAt( Vector3 point, float amount )
    {
      for ( int i = 0; i < normals.Count; i++ )
      {
        var direction = ( vertices[ i ] - point ).Normalized();
        normals[ i ] = Math3D.BlendNormals( normals[ i ], direction, amount );
      }
    }

    public void BlendNormals( Vector3 direction, float amount )
    {
      if ( amount <= 0 )
      {
        return;
      }

      if ( amount >= 1 )
      {
        for ( int i = 0; i < normals.Count; i++ )
        {
          normals[ i ] = direction;
        }
        
        return;
      }

      for ( int i = 0; i < normals.Count; i++ )
      {
        normals[ i ] = Math3D.BlendNormals( normals[ i ], direction, amount );
      }
    }

    public Range GetRangeY()
    {
      var minY = float.MaxValue;
      var maxY = -float.MaxValue;

      for ( int i = 0; i < vertices.Count; i++ )
      {
        minY = Mathf.Min( minY, vertices[ i ].Y );
        maxY = Mathf.Max( maxY, vertices[ i ].Y );
      } 

      return new Range( minY, maxY );
    }

    public void BlendNormalsOverY( Vector3 direction, float amount, float startY, float endY, Curve curve )
    {
      if ( amount <= 0 )
      {
        return;
      }

      for ( int i = 0; i < normals.Count; i++ )
      {
        var yPositionAmount = MathX.RemapClamped( vertices[ i ].Y, startY, endY, 0, 1 );
        var blendAmount = curve.Sample( yPositionAmount ) * amount; 
        blendAmount = MathX.Clamp01( blendAmount );
        normals[ i ] = Math3D.BlendNormals( normals[ i ], direction, blendAmount );
      }
    }

    public void Offset( Vector3 offset )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        vertices[ i ] = vertices[ i ] + offset;
      }
    }

    public void ForEachTriangle( Action<int,int,int,int> callback )
    {
      var index = 0;

      for ( int i = 0; i < indices.Count; i += 3 )
      {
        var a = indices[ i + 0 ];
        var b = indices[ i + 1 ];
        var c = indices[ i + 2 ];

        callback( index, a, b, c );

        index ++;
      }
    }

    static int GetUVIndex( int u, int v, int segments )
    {
      return u * ( segments + 1 ) + v;
    }

   
    public static MeshGeometry CreateFromUVFunction( Func<Vector2,Pose> uv, int uSegments, int vSegments, bool fullUVQuads = false )
    {
      var mg = new MeshGeometry();

      if ( fullUVQuads )
      {
        var uv00 = new Vector2( 0, 0 );
        var uv10 = new Vector2( 1, 0 );
        var uv01 = new Vector2( 0, 1 );
        var uv11 = new Vector2( 1, 1 );

        for ( int i = 0; i < uSegments; i++ )
        {
          var u0 = (float)i / uSegments;
          var u1 = (float)(i + 1 ) / uSegments; 

          for ( int j = 0; j < vSegments; j++ )
          {
            var v0 = (float)j / vSegments;
            var v1 = (float)(j + 1 ) / vSegments; 

            var p00 = uv( new Vector2( u0, v0 ) );
            var p10 = uv( new Vector2( u1, v0 ) );
            var p01 = uv( new Vector2( u0, v1 ) );
            var p11 = uv( new Vector2( u1, v1 ) );
            var n = ( p00.up + p10.up + p01.up + p11.up ) / 4f;

            mg.AddQuad(
              p00.position, p10.position, p11.position, p01.position,
              n, n, n, n,
              uv00, uv10, uv11, uv01
            );
          } 
        }

        return mg;
      }
      
      for ( int i = 0; i <= uSegments; i++ )
      {
        var uI = (float)i / uSegments;

        for ( int j = 0; j <= vSegments; j++ )
        {
          var vJ = (float)j / vSegments;

          var uvIJ = new Vector2( uI, vJ );

          var pose = uv( uvIJ );

          mg.vertices.Add( pose.position );
          mg.normals.Add( pose.up );
          mg.uvs.Add( uvIJ );
        }
      }


      for ( int i = 0; i < uSegments; i++ )
      {
        var u0 = i;
        var u1 = i + 1;

        for ( int j = 0; j < vSegments; j++ )
        {
          var v0 = j;
          var v1 = j + 1;

          var a = GetUVIndex( u0, v0, vSegments );
          var b = GetUVIndex( u1, v0, vSegments );
          var c = GetUVIndex( u1, v1, vSegments );
          var d = GetUVIndex( u0, v1, vSegments );

          mg.AddTriangle( a, b, d );
          mg.AddTriangle( b, c, d );

          // mg.AddTriangle( d, b, a );
          // mg.AddTriangle( d, c, b );
        }
      }

      return mg;

    }


    public MeshGeometry UniqueTriangles()
    {
      var mg = new MeshGeometry();

      mg.Initialize( true, true, colors != null, uv2s != null );

      if ( colors != null )
      {
        mg.colors = new List<Color>();
      }

      if ( uv2s != null )
      {
        mg.uv2s = new List<Vector2>();
      }      

      ForEachTriangle(
        ( index, a, b, c )=>
        {
          var vA = vertices[ a ];
          var vB = vertices[ b ];
          var vC = vertices[ c ];

          var uvA = uvs[ a ];
          var uvB = uvs[ b ];
          var uvC = uvs[ c ];

          var nA = normals[ a ];
          var nB = normals[ b ];
          var nC = normals[ c ];

          mg.AddTriangle( 
            vA, vB, vC,
            nA, nB, nC,
            uvA, uvB, uvC
          );

          if ( colors != null )
          {
            Lists.Add( mg.colors, colors[ a ], colors[ b ], colors[ c ] );
          }

          if ( uv2s != null )
          {
            Lists.Add( mg.uv2s, uv2s[ a ], uv2s[ b ], uv2s[ c ] );
          }

        }
      );

      return mg;
    }

    public void SetTriangleUVs( int triangleIndex, Vector2 uv )
    {
      var i = triangleIndex * 3;

      var a = indices[ i + 0 ];
      var b = indices[ i + 1 ];
      var c = indices[ i + 2 ];

      uvs[ a ] = uv;
      uvs[ b ] = uv;
      uvs[ c ] = uv;
    }

    public void SetTriangleNormal( int triangleIndex, Vector3 normal )
    {
      var i = triangleIndex * 3;

      var a = indices[ i + 0 ];
      var b = indices[ i + 1 ];
      var c = indices[ i + 2 ];

      normals[ a ] = normal;
      normals[ b ] = normal;
      normals[ c ] = normal;
    }

    public void SetTriangleColor( int triangleIndex, Color color )
    {
      var i = triangleIndex * 3;

      var a = indices[ i + 0 ];
      var b = indices[ i + 1 ];
      var c = indices[ i + 2 ];

      colors[ a ] = color;
      colors[ b ] = color;
      colors[ c ] = color;
    }

    public MeshGeometry( bool normals = true, bool uvs = true, bool colors = false, bool uvs2 = false )
    {
      Initialize( normals, uvs, colors, uvs2 );
    } 

    public MeshGeometry( string name, bool normals = true, bool uvs = true, bool colors = false, bool uvs2 = false )
    {
      this.name = name;
      Initialize( normals, uvs, colors, uvs2 );
    }   

    public static MeshGeometry Combine( List<MeshGeometry> meshes )
    {
      var mg = new MeshGeometry();
      meshes.ForEach( m => mg.Add( m ) );
      return mg;
    }

    public static MeshGeometry BillboardQuad( float size = 1 )
    {
      var hs = size / 2f;

      var mg = new MeshGeometry();
      mg.AddQuad(
        new Vector3( -hs, -hs, 0 ), new Vector3( hs, -hs, 0 ),  
        new Vector3(  hs,  hs, 0 ), new Vector3( -hs,  hs, 0 ),

        Vector3.Forward, Vector3.Forward, Vector3.Forward, Vector3.Forward, 

        new Vector2( 0, 0 ), new Vector2( 1, 0 ),  
        new Vector2( 1, 1 ), new Vector2( 0,  1 )
      );

      return mg;
    }

    public void Initialize( bool normals = true, bool uvs = true, bool colors = false, bool uvs2 = false )
    {      
      this.normals = normals ? new List<Vector3>() : null;
      this.uvs = uvs ? new List<Vector2>() : null;
      this.uv2s = uvs2 ? new List<Vector2>() : null;
      this.colors = colors ? new List<Color>() : null;      
    }

    public Vector3 GetRawNormal( int triangleIndex )
    { 
      var va = vertices[ indices[ triangleIndex * 3 ] ];
      var vb = vertices[ indices[ triangleIndex * 3 + 1] ]; 
      var vc = vertices[ indices[ triangleIndex * 3 + 2 ] ]; 

      return Math3D.ComputeNormal( va, vb, vc );
    }

    public void ComputeNormals()
    {
      var dl = new MapList<int,Vector3>();

      ForEachTriangle(
        ( t, va, vb, vc)=>
        {
          var normal = GetRawNormal( t );

          dl.Add( va, normal );
          dl.Add( vb, normal );
          dl.Add( vc, normal );
        }
      );

      foreach ( var e in dl )
      {
        var normalIndex = e.Key;
        var normal = Math3D.Average( e.Value ); 

        normals[ normalIndex ] = normal; 
      }
    }

    public MeshGeometry Clone()
    {
      var mg = new MeshGeometry();

      mg.vertices = Lists.Clone( vertices );
      mg.indices  = Lists.Clone( indices );
      mg.normals  = Lists.Clone( normals );

      mg.uvs      = Lists.Clone( uvs );
      mg.uv2s     = Lists.Clone( uv2s );
      mg.colors   = Lists.Clone( colors );

      mg.lodEdgeLength = lodEdgeLength;

      if ( customMeshAttributes != null )
      {
        mg.customMeshAttributes = new List<CustomMeshAttributeList>();
        customMeshAttributes.ForEach(
          ( m )=>
          {
            mg.customMeshAttributes.Add( m.Clone() );
          }
        );
      }

      return mg;
    }

    public void FlipNormalDirection()
    {
      for ( int i = 0; i < normals.Count; i++ )
      {
        normals[ i ] = -normals[ i ];
      }

      for ( int i = 0 ; i < indices.Count; i += 3 )
      {
        var b = indices[ i ];
        indices[ i ] = indices[ i + 2 ];
        indices[ i + 2 ] = b;
      }
      
    }

    public void AddPoint( Vector3 position, Color color, Vector3 normal, int normalIndex )
    {
      vertices.Add( position );
      colors.Add( color );
      ( (MeshAttributeVector4List) customMeshAttributes[ normalIndex ] ).values.Add( new Vector4( normal.X, normal.Y, normal.Z, 0.0f ) );
      
      indices.Add( indices.Count );
    }  

    public void AddPoint( Vector3 position, Color color, Vector3 normal, int normalIndex, Vector2 uv, int uvIndex )
    {
      vertices.Add( position );
      colors.Add( color );

      ( (MeshAttributeVector4List) customMeshAttributes[ normalIndex ] ).values.Add( new Vector4( normal.X, normal.Y, normal.Z, 0.0f ) );
      ( (MeshAttributeVector4List) customMeshAttributes[ uvIndex ] ).values.Add( new Vector4( uv.X, uv.Y, 0.0f, 0.0f ) );
      
      indices.Add( indices.Count );
    }  

    public void AddPoint( Vector3 position, Vector3 normal, int normalIndex, Vector2 uv, int uvIndex )
    {
      vertices.Add( position );
      ( (MeshAttributeVector4List) customMeshAttributes[ normalIndex ] ).values.Add( new Vector4( normal.X, normal.Y, normal.Z, 0.0f ) );
      ( (MeshAttributeVector4List) customMeshAttributes[ uvIndex ] ).values.Add( new Vector4( uv.X, uv.Y, 0.0f, 0.0f ) );
      
      indices.Add( indices.Count );
    }  

    public void Add( MeshGeometry sourceGeometry, Transform3D? optionalTransform = null, List<int> indicesTarget = null )
    {
      var mappedIndices = new Dictionary<int,int>();

      var rotation = optionalTransform == null ? Quaternion.Identity : ( (Transform3D)optionalTransform ).Basis.GetRotationQuaternion();
      var transform = optionalTransform == null ? Transform3D.Identity : (Transform3D)optionalTransform;

      var outputIndices = indices;
      
      if ( indicesTarget != null )
      {
        outputIndices = indicesTarget;
      }

      for ( int i = 0; i < sourceGeometry.indices.Count; i++ )
      {
        var sourceIndex = sourceGeometry.indices[ i ];

        if ( ! mappedIndices.ContainsKey( sourceIndex ) )
        {
          var newIndex = vertices.Count;

          if ( sourceIndex >= sourceGeometry.vertices.Count || sourceIndex < 0 )
          {
            RJLog.Log( "Out of range:", i, ">>", sourceIndex, sourceGeometry.vertices.Count, sourceGeometry.indices );
          }

          var v = sourceGeometry.vertices[ sourceIndex ]; 

          if ( optionalTransform == null )
          {
            vertices.Add( v );
          }
          else
          {
            vertices.Add( transform * v );
          }

          if ( normals != null && sourceGeometry.numNormals > 0 )
          {
            if ( sourceIndex < 0 || sourceIndex >= sourceGeometry.normals.Count )
            {
              RJLog.Log( "Normals index bad:", sourceIndex, sourceGeometry.normals.Count );
            }

            if ( optionalTransform == null )
            {
              normals.Add( sourceGeometry.normals[ sourceIndex ] );
            }
            else
            {
              normals.Add( rotation * sourceGeometry.normals[ sourceIndex ] );
            }
            
          }

          if ( uvs != null && sourceGeometry.numUVs > 0 )
          {
            uvs.Add( sourceGeometry.uvs[ sourceIndex ] );
          }

          if ( uv2s != null && sourceGeometry.numUV2s > 0 )
          {
            uv2s.Add( sourceGeometry.uv2s[ sourceIndex ] );
          }

          if ( colors != null && sourceGeometry.numColors > 0 )
          {
            colors.Add( sourceGeometry.colors[ sourceIndex ] );
          }


          if ( Lists.Size( customMeshAttributes ) == Lists.Size( sourceGeometry.customMeshAttributes ) )
          {
            for ( int j = 0; j < customMeshAttributes.Count; j++ )
            {
              var customList = customMeshAttributes[ j ];
              var sourceCustomList = sourceGeometry.customMeshAttributes[ j ];
              sourceCustomList.AddTo( customList, sourceIndex );
            }
          }

          mappedIndices[ sourceIndex ] = newIndex;
        }

        outputIndices.Add( mappedIndices[ sourceIndex ] );
      }
    }


    public void AddTriangle( int a, int b, int c, bool flip = false )    
    {
      if ( flip )
      {
        Lists.Add( indices, c, b, a );
      }
      else
      {
        Lists.Add( indices, a, b, c );
      }
      
    }


    public void AddTriangle( 
      Vector3 va, Vector3 vb, Vector3 vc, 
      Vector3 na, Vector3 nb, Vector3 nc,
      Vector2 uva, Vector2 uvb, Vector2 uvc
    )
    {
      var index = vertices.Count;

      Lists.Add( vertices, va, vb, vc );
      Lists.Add( normals, na, nb, nc );
      Lists.Add( uvs, uva, uvb, uvc );
      Lists.Add( indices, index, index + 1, index + 2 );
    }

   

    public void AddTriangle( 
      Vector3 va, Vector3 vb, Vector3 vc, 
      Vector2 uva, Vector2 uvb, Vector2 uvc
    )
    {
      var n = Vector3.Up;

      AddTriangle( va, vb, vc, 
                   n, n, n,
                   uva, uvb, uvc );
    }

    

    public void AddTriangle( Vector3 va, Vector3 vb, Vector3 vc )    
    {
      var n  = Vector3.Up;
      var uv = Vector2.Zero;

      AddTriangle( va, vb, vc,  n, n, n, uv, uv, uv );
    }

    public void AddQuad( 
      VertexAttributes a, 
      VertexAttributes b,
      VertexAttributes c,
      VertexAttributes d 
    )
    {
      AddQuad(
        a.position, b.position, c.position, d.position,
        (Vector3) a.normal, (Vector3) b.normal, (Vector3) c.normal, (Vector3) d.normal,
        (Vector2) a.uv, (Vector2) b.uv, (Vector2) c.uv, (Vector2) d.uv        
      );

    }

    public void AddQuad(
      Vector3 va, Vector3 vb, Vector3 vc, Vector3 vd,
      Vector3 na, Vector3 nb, Vector3 nc, Vector3 nd, 
      Vector2 uva, Vector2 uvb, Vector2 uvc, Vector2 uvd
    )
    {
      
      /*
      
        0:a -- 1:b
         |      | 
        2:c -- 3:d

        faces:
        1: 0 1 2
        2: 2 3 0
      
      */

      var index = vertices.Count;

      Lists.Add( vertices, va, vb, vc, vd );
      Lists.Add( normals, na, nb, nc, nd );
      Lists.Add( uvs, uva, uvb, uvc, uvd );

      Lists.Add( indices, index, index + 1, index + 2 );
      Lists.Add( indices, index + 2, index + 3, index );
     
    }


    public void AddQuad( Quaternion rotation, float size, Box2 rectangle )
    {
      AddQuad( rotation, size, rectangle.min, rectangle.max );
    }

   
    public void DuplicateRange( int verticesStart, int verticesLength, int indexStart, int indicesLength )
    {
      for ( int i = verticesStart; i < verticesStart + verticesLength; i++ )
      {
        var v = vertices[ i ];
        var n = normals[ i ];
        var uv = uvs[ i ];

        vertices.Add( v ) ;
        normals.Add( n );
        uvs.Add( uv );
      }

      for ( int i = indexStart; i < indexStart + indicesLength; i++ )
      {
        indices.Add( indices[ i ] );
      }
    }

    public void AddQuadWithCustomDivisions( Quaternion rotation, float size, Vector2 uv00, Vector2 uv11, List<float> uDivisions, List<float> vDivisions )
    {
      if ( uDivisions.Count == 0 && vDivisions.Count == 0 )
      {
        AddQuad( rotation, size, uv00, uv11 ); 
        return;
      }

      var l = size * 0.5f;

      var normal = Vector3.Back * rotation;
      var points = new List<Vector3>
      {
        new Vector3(  l, -l, 0 ), new Vector3( -l, -l, 0 ),
        new Vector3( -l,  l, 0 ), new Vector3(  l,  l, 0 )
      };

      for ( int i = 0; i < points.Count; i++ )
      {
        points[ i ] = points[ i ] * rotation; 
      }

      var uv10 = new Vector2( uv11.X, uv00.Y );
      var uv01 = new Vector2( uv00.X, uv11.Y );

      var v10 = VertexAttributes.Create( points[ 0 ], uv10, normal );
      var v00 = VertexAttributes.Create( points[ 1 ], uv00, normal );      
      var v01 = VertexAttributes.Create( points[ 2 ], uv01, normal );
      var v11 = VertexAttributes.Create( points[ 3 ], uv11, normal );

      var uSegments = new List<float>();
      uSegments.Add( 0 );
      uSegments.AddRange( uDivisions );
      uSegments.Add( 1 );

      var vSegments = new List<float>();
      vSegments.Add( 0 );
      vSegments.AddRange( vDivisions );
      vSegments.Add( 1 );

      for ( int i = 0; i < ( uSegments.Count - 1 ); i++ )
      {
        var i0 = uSegments[ i ];
        var i1 = uSegments[ i + 1 ];

        var t0 = v10.Lerp( v00, i0 );
        var t1 = v10.Lerp( v00, i1 ); 

        var b0 = v01.Lerp( v11, i0 );
        var b1 = v01.Lerp( v11, i1 );  


        for ( int j = 0; j < ( vSegments.Count - 1 ); j++ )
        {
          var j0 = vSegments[ j ];
          var j1 = vSegments[ j + 1 ];

          var tb00 = t0.Lerp( b0, j0 );
          var tb10 = t1.Lerp( b1, j0 );

          var tb01 = t0.Lerp( b0, j1 );
          var tb11 = t1.Lerp( b1, j1 );

          AddQuad( tb10, tb00, tb01, tb11 );
        }
      }


    }

    public void AddQuadSubdivided( Quaternion rotation, float size, Vector2 uv00, Vector2 uv11, int uDivisions = 0, int vDivisions = 0 )
    {
      uDivisions = Mathf.Max( 0, uDivisions );
      vDivisions = Mathf.Max( 0, vDivisions );

      if ( uDivisions == 0 && vDivisions == 0 )
      {
        AddQuad( rotation, size, uv00, uv11 );
        return;
      }

      var mg = CreateFromUVFunction( 
        ( uv ) => 
        {
          var xy = uv * size - Vector2.One * size / 2f;

          var pose = new Pose();
          pose.position = Math3D.XY( xy );
          pose.rotation = rotation;

          return pose;

        },

        uDivisions + 2, vDivisions + 2
      );

      var uvTransform = new Transform2D();
      uvTransform.Origin = uv00;
      uvTransform = uvTransform.ScaledLocal( uv11 - uv00 );

      mg.ApplyUVTransform( uvTransform );

      Add( mg );
    }

    public void AddQuad( Quaternion rotation, float size, Vector2 uv00, Vector2 uv11 )
    {
      var l = size * 0.5f;

      var normal = Vector3.Back * rotation;
      var points = new List<Vector3>
      {
        new Vector3(  l, -l, 0 ), new Vector3( -l, -l, 0 ),
        new Vector3( -l,  l, 0 ), new Vector3(  l,  l, 0 )
      };

      for ( int i = 0; i < points.Count; i++ )
      {
        points[ i ] = points[ i ] * rotation; 
      }

      var uv10 = new Vector2( uv11.X, uv00.Y );
      var uv01 = new Vector2( uv00.X, uv11.Y );

      AddQuad(
        points[ 0 ], points[ 1 ], points[ 2 ], points[ 3 ],
        normal, normal, normal, normal,
        uv10, uv00, uv01, uv11
      );

    }

    public void AddQuad( int lt, int rt, int lb, int rb, bool flip = false )    
    {
      if ( flip )
      {
        AddQuad( rt, lt, rb, lb, false );
      }
      else
      {
        Lists.Add( indices, lb, rt, lt, rt, lb, rb );
      }
     
    }


    public void AddQuad(
      Vector3 va, Vector3 vb, Vector3 vc, Vector3 vd,
      Vector2 uva, Vector2 uvb, Vector2 uvc, Vector2 uvd
    )
    {

      AddTriangle( va, vb, vc, uva, uvb, uvc );
      AddTriangle( vc, vd, va, uvc, uvd, uva );
    }

    public void AddQuad(  Vector3 va, Vector3 vb, Vector3 vc, Vector3 vd  )
    {
      AddQuad(
        va, vb, vc, vd,
        new Vector2( 0, 0 ),
        new Vector2( 0, 1 ),
        new Vector2( 1, 1 ),
        new Vector2( 1, 0 )
      );
    }

    public void AddQuad( Vector3 offset, float size  )
    {
      var center = new Vector3( 0.5f, 0.5f, 0 );

      /*
      
      [-0.5, -0.5]  [0.5, -0.5 ]  
      [-0.5,  0.5]  [0.5,  0.5 ]  

      */

      AddQuad(        
        new Vector3( -0.5f, -0.5f, 0 ) * size + offset ,
        new Vector3( -0.5f,  0.5f, 0 ) * size + offset ,
        new Vector3(  0.5f,  0.5f, 0 ) * size + offset ,
        new Vector3(  0.5f, -0.5f, 0 ) * size + offset 
      );
    } 

    public void AddConvex2( Convex2 convex )
    {
      var points = convex.points;
      var tris = points.Count - 2;

      for ( int i = 0; i < tris; i++ )
      {
        var p0 = Math3D.XYasXZ( points[ 0 ] );
        var p1 = Math3D.XYasXZ( points[ i + 1 ] );
        var p2 = Math3D.XYasXZ( points[ i + 2 ] );

        AddTriangle( p0, p1, p2 );
      }
    }

    public void Turbulence( float amount, Vector3 noiseScale, Vector3 noiseOffset )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        var v = vertices[ i ];
        var offset = Noise.PerlinPolar3( v * noiseScale + noiseOffset ) * amount;
        vertices[ i ] = v + offset;
      } 
    }

    public void RemapX( Curve curve )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        var v = vertices[ i ];
        v.X = curve.Sample( v.X );
        vertices[ i ] = v;
      }      
    }

    public void RemapY( Curve curve )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        var v = vertices[ i ];
        v.Y = curve.Sample( v.Y );
        vertices[ i ] = v;
      }      
    }

    public void RemapZ( Curve curve )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        var v = vertices[ i ];
        v.Z = curve.Sample( v.Z );
        vertices[ i ] = v;
      }     
    }



    public void ScaleXForY( Curve curve )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        var v = vertices[ i ];
        v.X = v.X * curve.Sample( v.Y );
        vertices[ i ] = v;
      }     
    }

    public void ScaleZForY( Curve curve )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        var v = vertices[ i ];
        v.Z = v.Z * curve.Sample( v.Y );
        vertices[ i ] = v;
      }     
    }

    public void ScaleXZForY( Curve curve )
    {
      for ( int i = 0; i < vertices.Count; i++ )
      {
        var v = vertices[ i ];
        var s = curve.Sample( v.Y );
        vertices[ i ] = new Vector3( v.X * s, v.Y, v.Z * s );
      }     
    }

    public void CenterMesh( bool onX = true, bool onY = true, bool onZ = true, Box3 centerCalcluationBox = null )
    {
      var offset = new Vector3( 0, 0, 0 );
      var numPoints = 0;

      for ( int i = 0; i < vertices.Count; i++ )
      {
        if ( centerCalcluationBox != null && ! centerCalcluationBox.ContainsPoint( vertices[ i ] ) )
        {
          continue;
        }

        offset += vertices[ i ];
        numPoints ++;
      }

      offset /= numPoints;
      
      if ( ! onX )
      {
        offset.X = 0;
      } 

      if ( ! onY )
      {
        offset.Y = 0;
      }

      if ( ! onZ )
      {
        offset.Z = 0;
      }


      ApplyTranslation( - offset );


    }

    
    public static ArrayMesh GenerateLODMesh( List<MeshGeometry> 
        mgs, Mesh.PrimitiveType type = Mesh.PrimitiveType.Triangles, ArrayMesh arrayMesh = null, 
        bool generateTangents = true, LODLerpingData lerpingData = null )
    {
      return mgs[ 0 ].GenerateMesh( type, arrayMesh, generateTangents, mgs.Sub( 1 ), lerpingData );
    }
    
    public static ArrayMesh GeneratePointsLODMesh( List<MeshGeometry> mgs, LODLerpingData lerpingData = null, bool generateTangents = true )
    {
      return GenerateLODMesh( mgs, Mesh.PrimitiveType.Points, null, generateTangents, lerpingData );
    }

    public static ArrayMesh GenerateLinesLODMesh( List<MeshGeometry> mgs, LODLerpingData lerpingData = null, bool generateTangents = true )
    {
      return GenerateLODMesh( mgs, Mesh.PrimitiveType.Lines, null, generateTangents, lerpingData );
    }

    public static ArrayMesh GenerateTrianglesLODMesh( List<MeshGeometry> mgs, LODLerpingData lerpingData = null, bool generateTangents = true )
    {
      return GenerateLODMesh( mgs, Mesh.PrimitiveType.Triangles, null, generateTangents, lerpingData );
    }

    public ArrayMesh GenerateMesh( 
      Mesh.PrimitiveType type = Mesh.PrimitiveType.Triangles, ArrayMesh arrayMesh = null, 
      bool generateTangents = true, List<MeshGeometry> lods = null, LODLerpingData lerpingData = null  )
    {
      if ( arrayMesh == null )
      {
        arrayMesh = new ArrayMesh();
      }

      var lodDictionary = new Godot.Collections.Dictionary();
      

      _GenerateLODs( type, lods, lodDictionary, lerpingData );
      
      var surfaceArray = new Godot.Collections.Array();
      surfaceArray.Resize( (int) Mesh.ArrayType.Max );

      var flags = Mesh.ArrayFormat.FormatVertex | Mesh.ArrayFormat.FormatIndex;

      if ( vertices != null && vertices.Count != 0 )
      {
        surfaceArray[ (int) Mesh.ArrayType.Vertex ] = vertices.ToArray();
      }

      if ( normals != null && normals.Count != 0 )
      {
        surfaceArray[ (int) Mesh.ArrayType.Normal ] = normals.ToArray();
        flags |= Mesh.ArrayFormat.FormatNormal;
      }

      if ( uvs != null && uvs.Count != 0 )
      {
        surfaceArray[ (int) Mesh.ArrayType.TexUV ] = uvs.ToArray();
        flags |= Mesh.ArrayFormat.FormatTexUV;
      }

      if ( uv2s != null && uv2s.Count != 0 )
      {
        surfaceArray[ (int) Mesh.ArrayType.TexUV2 ] = uv2s.ToArray();
        flags |= Mesh.ArrayFormat.FormatTexUV2;
      }

      if ( colors != null && colors.Count != 0 )
      {
        surfaceArray[ (int) Mesh.ArrayType.Color ] = colors.ToArray();
        flags |= Mesh.ArrayFormat.FormatColor;
      }

      if ( indices != null && indices.Count != 0 )
      {
        surfaceArray[ (int) Mesh.ArrayType.Index ] = indices.ToArray();
        flags |= Mesh.ArrayFormat.FormatIndex;
      }      

      customMeshAttributes.ForEach( 
        c => 
        { 
          flags |= c.GetFormatFlag();
          c.WriteData( surfaceArray );
        }
      );

      if ( lodDictionary != null && lodDictionary.Count > 0 )
      {
        var keys = Lists.From( lodDictionary.Keys );
        RJLog.Log( "LODS:", keys );
      }

      RJLog.Log( "Flags:", flags );
      arrayMesh.AddSurfaceFromArrays( type, surfaceArray, null, lodDictionary, flags ); 

      if ( generateTangents )
      {
        arrayMesh.RegenNormalMaps();
      }

      return arrayMesh;
      
    }

    void _GenerateLODs( Mesh.PrimitiveType type, List<MeshGeometry> lods, 
                        Godot.Collections.Dictionary lodDictionary, LODLerpingData lerpingData )
    {
      if ( lods == null )
      {
        return;
      }

      RJLog.Log( "Creating mesh with LODs", vertices.Count );

      var higherIndices = indices;
      var higherIndicesLength = indices.Count;
      var higherEdgeLength = this.lodEdgeLength;

      var primitiveLength = 3;

      if ( Mesh.PrimitiveType.Lines == type )
      {
        primitiveLength = 2;
      }
      else if ( Mesh.PrimitiveType.Points == type )
      {
        primitiveLength = 1;
      }


      var lodIndex = -1;
      lods.ForEach(
        ( lod )=>
        {
          lodIndex ++;

          
          var lodIndices = new List<int>();
          Add( lod, null, lodIndices );

          if ( lerpingData != null )
          {
            var lowOffsetIndex = lodIndices.Count / primitiveLength;
            var lowIndices  = Lists.Create( lowOffsetIndex, i => i );
            var highIndices = Lists.Create( higherIndicesLength / primitiveLength, i => i + lowOffsetIndex );             

            for ( int s = 0; s < lerpingData.lerpSteps; s ++ )
            {
              var i = ( lerpingData.lerpSteps - 1 ) - s; 
              var lerpedGenericOutput = new List<int>();

              var lerpedEdgeLength = lerpingData.Sample( lowIndices.View(), highIndices.View(), 
                                      lerpedGenericOutput, i, lod.lodEdgeLength, higherEdgeLength );

              var lerpedPrimitiveIndices = new List<int>();

              var evaluatedJ = 0;
              var evaluatedK = 0;
              var isLow = false;
              var offset = 0;

              try
              {
                for ( int j = 0; j < lerpedGenericOutput.Count; j++ )
                {
                  evaluatedJ = j;
                  var primitiveIndex = lerpedGenericOutput[ j ];
                  isLow = primitiveIndex < lowOffsetIndex;

                  if ( primitiveIndex < lowOffsetIndex )
                  { 
                    offset = primitiveIndex * primitiveLength;

                    for ( int k = 0; k < primitiveLength; k ++ )
                    {
                      evaluatedK = k;
                      lerpedPrimitiveIndices.Add( lodIndices[ offset + k ] );
                    }
                  }
                  else
                  {
                    offset = ( primitiveIndex - lowOffsetIndex ) * primitiveLength;

                    for ( int k = 0; k < primitiveLength; k ++ )
                    {
                      evaluatedK = k;
                      lerpedPrimitiveIndices.Add( higherIndices[ offset + k ] );
                    }
                  }
                  
                }
              }
              catch ( System.Exception e )
              {
                RJLog.Log(
                  "LodIndex", lodIndex,
                  "Lerped Output Size:", lerpedGenericOutput.Count, 
                  "j", evaluatedJ,
                  "k", evaluatedK, 
                  "isLow", isLow,
                  "offset", offset,
                  "indices", indices.Count,
                  "indices", higherIndices.Count,
                  "primitiveLength", primitiveLength,
                  "lowOffsetIndex", lowOffsetIndex,
                  "Low Size", lodIndices.Count, 
                  "High Size", higherIndicesLength
                );
                RJLog.Error( e );
              }

              RJLog.Log( "Adding lod", lods.IndexOf( lod ), i, "size:", lerpedEdgeLength, "verts:", lerpedPrimitiveIndices.Count );
              lodDictionary[ lerpedEdgeLength ] = lerpedPrimitiveIndices.ToArray();

            }
          }

          higherIndices = lodIndices;
          higherIndicesLength = lodIndices.Count;
          higherEdgeLength = lod.lodEdgeLength;


          RJLog.Log( "Adding lod", lods.IndexOf( lod ), "full", "size:", lod.lodEdgeLength, "verts:", lodIndices.Count );
          lodDictionary[ lod.lodEdgeLength ] = lodIndices.ToArray();
        }
      );

      RJLog.Log( "Added vertices", vertices.Count );
    }
    

  }
}