using System.Collections.Generic;
using System.Linq;
using System.Numerics;

namespace PathFinding
{
  public class PathFinder2D<T> where T : IPlanarElement
  {
    /// <summary>
    /// </summary>
    /// <param name="element">Guaranteed to be non-null.</param>
    /// <param name="position"></param>
    public delegate void Callback(T collider, Vector2 position);


    private readonly IPlanarCollection<T> collection;
    private readonly int width;
    private readonly int height;
    public PathFinder2D(IPlanarCollection<T> collection)
    {
      this.collection = collection;
      width = collection.GetLength(0);
      height = collection.GetLength(1);
    }

    /// <summary>
    /// Navigate the collection such that each "step" is always towards the destination.
    /// </summary>
    /// <param name="element">The pathing element.</param>
    /// <param name="origin">The starting location.</param>
    /// <param name="destination">The destination.</param>
    /// <param name="callback">Do cool stuff here.</param>
    /// <returns>True if the element reached the destination.</returns>
    public bool PathTo(T element, Vector2 origin, Vector2 destination, Callback callback)
    {
      if (destination.X > width - 1 || destination.Y > height - 1 || destination.X < 0 || destination.Y < 0)
      {
        return false;
      }

      var path = FindDirectionTowardsDestination(element.GetPaths(), origin, destination);
      var next = Vector2.Add(origin, path.Direction);

      if (!IsPathable(origin, destination, next))
      {
        // Assumption: if a single best-choice step towards the destination cannot happen, no pathing can happen.
        return false;
      }

      var shouldPath = true;
      while (shouldPath)
      {
        var collider = collection[(int)next.X, (int)next.Y];
        if (collider != null) callback(collider, next);
        if (next == destination) return true;
        if (path.Distance == Distance.OneStep)
        {
          shouldPath = false;
        }
        next = Vector2.Add(next, path.Direction);
      }
      return true;
    }

    public void PathEvery(IPlanarElement element, Vector2 from, Callback callback)
    {
      foreach (var path in element.GetPaths())
      {
        var shouldPath = true;
        var next = Vector2.Add(from, path.Direction);
        while (shouldPath && next.X < width && next.Y < height && next.X >= 0 && next.Y >= 0)
        {
          var collider = collection[(int)next.X, (int)next.Y];
          if (collider != null)
          {
            callback(collider, next);
          }
          next = Vector2.Add(from, path.Direction);
          if (path.Distance == Distance.OneStep)
          {
            shouldPath = false;
          }
        }
      }
    }

    public Path FindDirectionTowardsDestination(ICollection<Path> paths, Vector2 origin, Vector2 destination) =>
      paths.Aggregate((a, b) => Vector2.Distance(destination, Vector2.Add(origin, a.Direction)) < Vector2.Distance(destination, Vector2.Add(origin, b.Direction)) ? a : b);


    public bool IsPathable(Vector2 origin, Vector2 destination, T element)
    {
      var path = FindDirectionTowardsDestination(element.GetPaths(), origin, destination);
      var next = Vector2.Add(origin, path.Direction);
      return IsPathable(origin, destination, next);
    }
    public bool IsPathable(Vector2 origin, Vector2 destination, Vector2 next)
    {
      if (Vector2.Distance(next, destination) < Vector2.Distance(origin, destination))
      {
        // y = mx + b
        // b = -mx + y
        var slope = (destination.Y - origin.Y) / (destination.X - origin.X);
        var yIntercept = -(slope * origin.X) + origin.Y;
        return float.IsInfinity(slope) || next.Y == slope * next.X + yIntercept;
      }
      return false;
    }
  }
}