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


namespace Rokojori
{  
  public class Grid2D<T>
  { 
    float _gridX;
    float _gridY;
    MapList<Vector2I,T> _cells = new MapList<Vector2I, T>();
    Dictionary<T,Vector2I> _objectsInCell = new Dictionary<T, Vector2I>();

    Func<T,Vector2> _getPosition;
    
    public Grid2D( float sizeX, float sizeY, Func<T,Vector2> getPosition )
    {
      _gridX = sizeX;
      _gridY = sizeY;
      _getPosition = getPosition;
    } 

    public static Grid2D<N> XZfromNode3D<N>( float x, float z ) where N:MeshInstance3D
    {
      return new Grid2D<N>( x, z, t => Math2D.XZ( t.GlobalPosition ) );
    }

    public float WorldToGridX( float x )
    {
      return x / _gridX;
    }

    public float WorldToGridY( float y )
    {
      return y / _gridY;
    }

    public int WorldToGridIDX( float x )
    {
      return Mathf.FloorToInt( WorldToGridX( x ) );
    }

    public int WorldToGridIDY( float y )
    {
     return Mathf.FloorToInt( WorldToGridY( y ) );
    }

    public Vector2I WorldToGridID( Vector2 worldPosition )
    {
      var idX = WorldToGridIDX( worldPosition.X );
      var idY = WorldToGridIDY( worldPosition.Y );
      return new Vector2I( idX, idY ); 
    }

    public Vector2 GridIDtoWorld( Vector2I id )
    {
      return new Vector2( id.X * _gridX, id.Y * _gridY );
    }

    public bool Has( T obj )
    {
      return _objectsInCell.ContainsKey( obj );
    }

    public Vector2I? GetRegistratedCell( T t )
    {
      if ( ! _objectsInCell.ContainsKey( t ) )
      {
        return null;
      }

      return _objectsInCell[ t ];
    }

    public void Remove( T obj )
    {
      if ( ! Has( obj ) )
      {
        return;
      }

      var cell = _objectsInCell[ obj ];
      _objectsInCell.Remove( obj );
      _cells.Remove( cell, obj );

    }

    public void AddAll( List<T> objects )
    {
      objects.ForEach( o => Add( o ) );
    }

    public void Add( T obj )
    {
      Remove( obj );

      var id = WorldToGridID( _getPosition( obj ) );

      _objectsInCell[ obj ] = id;
      _cells.Add( id, obj );
    }

    public List<T> GetAllAtID( Vector2I id )
    {
      if ( ! _cells.ContainsKey( id ) )
      {
        return _cells[ id ];
      }

      return new List<T>();
    }

    public List<T> GetAtWorldPosition( Vector2 worldPosition )
    {
      return GetAllAtID( WorldToGridID( worldPosition ) );
    }

    public void ForEachCell( Action<Vector2I,List<T>> action )
    {
      foreach ( var id_objects in _cells )
      {
        action( id_objects.Key, id_objects.Value );
      }
    }

    public List<T> GetInBox( Box2 box, List<T> outputList = null )
    {      
      var start = box.min;
      var end   = box.max;

      var minID = WorldToGridID( start );
      var maxID = WorldToGridID( end ) + Vector2I.One;

      outputList = outputList == null ? new List<T>() : outputList;

      for ( int i = minID.X; i < maxID.X; i++ )
      {
        for ( int j = minID.Y; j < maxID.Y; j++ )
        {
          var id = new Vector2I( i, j );
          
          if ( ! _cells.ContainsKey( id ) )
          {
            continue;
          }

          outputList.AddRange( _cells[ id ] );
        }
      }

      return outputList;

    }

  }
}