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

using System.Threading.Tasks;

namespace Rokojori.PointClouds
{  
  public class PointCloudOcTree:OcTree<Point>
  { 
    List<Vector3> compressedNormals = new List<Vector3>()
    {
      Vector3.Up,
      Vector3.Down,
      Vector3.Forward,
      Vector3.Back,
      Vector3.Right,
      Vector3.Left
    };

    public bool normalSeperation = false;

    public PointCloudOcTree( Vector3 min, Vector3 max, float rootCellSize, int maxDepth  )
    :base( null, min, max, rootCellSize, maxDepth  )
    {
      this._getPosition = GetPointPosition;
      this._combinePoints = CombinePoints;
      this._smoothPoints = SmoothPoints;
    }

    protected Vector3 GetPointPosition( Point p )
    {
      return p.position;
    }

    protected int GetNormalIndex( Vector3 normal )
    {
      var closest = -1;
      var closestDistance = 100f;
      
      for ( int i = 0; i < compressedNormals.Count; i++ )
      {
        var d = compressedNormals[ i ].DistanceSquaredTo( normal );

        if ( d < closestDistance )
        {
          closestDistance = d;
          closest = i;
        }

      }

      return closest;

    }

    protected List<Point> CombinePoints( List<Point> points )
    {
      if ( normalSeperation )
      {
        var lists = compressedNormals.Map( c => new List<Point>() );

        points.ForEach(
          ( p )=>
          {
            var normalIndex = GetNormalIndex( p.normal );
            lists[ normalIndex ].Add( p );
          }
        );

        lists = lists.Filter( n => n.Count > 0 );


        return lists.Map( l => Point.AsAverage( l ) );
      }

      return new List<Point>(){ Point.AsAverage( points ) };
    }

    protected List<Point> SmoothPoints( List<Point> targets, List<Point> points, float amount )
    {
      if ( targets == null || points == null )
      {
        return targets;
      }

      var average = Point.AsAverage( points );
      return targets.Map( t => t.Lerp( average, amount ) );;
    }

    
  }
}