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

namespace PathFinding
{
	public class PathFinder2D<T> where T : IPlanarElement
	{
		/// <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;

		/// <param name="width">Horizontal size, in steps, of the pathable plane.</param>
		/// <param name="height">Vertical size, in steps, of the pathable plane.</param>
		public PathFinder2D(IPlanarCollection<T> collection, int width, int height)
		{
			this.collection = collection;
			this.width = width;
			this.height = height;
		}

		/// <summary>
		/// Navigate the collection such that each "step" is always towards the destination, respecting the Paths available to the element at origin.
		/// </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(Vector2 origin, Vector2 destination, Callback? callback = null)
		{
			if (destination.X > width - 1 || destination.Y > height - 1 || destination.X < 0 || destination.Y < 0)
			{
				return false;
			}
			var element = collection[origin];
			if (element == null) return false;

			var path = FindDirectionTowardsDestination(element.MoveSet.GetMoves(), origin, destination);
			if (!IsPathable(origin, destination, path.Direction))
			{
				// Assumption: if a single best-choice step towards the destination cannot happen, no pathing can happen.
				return false;
			}

			var shouldPath = true;
			var next = origin;
			while (shouldPath && next != destination)
			{
				next = Vector2.Add(next, path.Direction);
				var collider = collection[next];
				if (collider != null)
				{
					callback?.Invoke(collider, next);
					shouldPath = false;
				}
				else if (path.Distance == Distance.OneStep)
				{
					shouldPath = false;
				}
			}
			return next == destination;
		}

		public void PathEvery(Vector2 from, Callback callback)
		{
			var element = collection[from];
			if (element == null)
			{
				return;
			}
			foreach (var path in element.MoveSet.GetMoves())
			{
				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.Y, (int)next.X];
					if (collider != null)
					{
						callback(collider, next);
						shouldPath = false;
					}
					if (path.Distance == Distance.OneStep)
					{
						shouldPath = false;
					}
					next = Vector2.Add(next, path.Direction);
				}
			}
		}

		/// <summary>
		/// Path the line from origin to destination, ignoring any Paths defined by the element at origin.
		/// </summary>
		public void LinePathTo(Vector2 origin, Vector2 direction, Callback callback)
		{
			direction = Vector2.Normalize(direction);

			var next = Vector2.Add(origin, direction);
			while (next.X >= 0 && next.X < width && next.Y >= 0 && next.Y < height)
			{
				var element = collection[next];
				if (element != null) callback(element, next);
				next = Vector2.Add(next, direction);
			}
		}

		public static Move FindDirectionTowardsDestination(ICollection<Move> paths, Vector2 origin, Vector2 destination) =>
			paths.Aggregate((a, b) =>
			{
				var distanceA = Vector2.Distance(destination, Vector2.Add(origin, a.Direction));
				var distanceB = Vector2.Distance(destination, Vector2.Add(origin, b.Direction));
				return distanceA < distanceB ? a : b;
			});

		public static bool IsPathable(Vector2 origin, Vector2 destination, Vector2 direction)
		{
			var next = Vector2.Add(origin, direction);
			if (Vector2.Distance(next, destination) >= Vector2.Distance(origin, destination)) return false;

			var slope = (destination.Y - origin.Y) / (destination.X - origin.X);
			if (float.IsInfinity(slope))
			{
				return next.X == destination.X;
			}
			else
			{
				// b = -mx + y
				var yIntercept = -slope * origin.X + origin.Y;
				// y = mx + b
				return next.Y == slope * next.X + yIntercept;
			}
		}
	}
}