using UnityEngine;
using Unity.Mathematics;
using Pathfinding.Pooling;

namespace Pathfinding {
	/// <summary>
	/// Finds a path in a random direction from the start node.
	///
	/// Terminates and returns when G \>= length (passed to the constructor) + RandomPath.spread or when there are no more nodes left to search.
	///
	/// <code>
	///
	/// // Call a RandomPath call like this, assumes that a Seeker is attached to the GameObject
	///
	/// // The path will be returned when the path is over a specified length (or more accurately when the traversal cost is greater than a specified value).
	/// // A score of 1000 is approximately equal to the cost of moving one world unit.
	/// int theGScoreToStopAt = 50000;
	///
	/// // Create a path object
	/// RandomPath path = RandomPath.Construct(transform.position, theGScoreToStopAt);
	/// // Determines the variation in path length that is allowed
	/// path.spread = 5000;
	///
	/// // Get the Seeker component which must be attached to this GameObject
	/// Seeker seeker = GetComponent<Seeker>();
	///
	/// // Start the path and return the result to MyCompleteFunction (which is a function you have to define, the name can of course be changed)
	/// seeker.StartPath(path, MyCompleteFunction);
	///
	/// </code>
	///
	/// [Open online documentation to see videos]
	///
	/// See: wander (view in online documentation for working links)
	/// </summary>
	public class RandomPath : ABPath {
		/// <summary>
		/// G score to stop searching at.
		/// The G score is rougly the distance to get from the start node to a node multiplied by 1000 (per default, see Pathfinding.Int3.Precision), plus any penalties.
		///
		/// [Open online documentation to see videos]
		/// </summary>
		public int searchLength;

		/// <summary>
		/// All G scores between <see cref="searchLength"/> and <see cref="searchLength"/>+<see cref="spread"/> are valid end points, a random one of them is chosen as the final point.
		/// On grid graphs a low spread usually works (but keep it higher than nodeSize*1000 since that it the default cost of moving between two nodes), on NavMesh graphs
		/// I would recommend a higher spread so it can evaluate more nodes.
		///
		/// [Open online documentation to see videos]
		/// </summary>
		public int spread = 5000;

		/// <summary>
		/// If an <see cref="aim"/> is set, the higher this value is, the more it will try to reach <see cref="aim"/>.
		///
		/// Recommended values are between 0 and 10.
		/// </summary>
		public float aimStrength;

		/// <summary>Currently chosen end node</summary>
		uint chosenPathNodeIndex;
		uint chosenPathNodeGScore;

		/// <summary>
		/// The node with the highest G score which is still lower than <see cref="searchLength"/>.
		/// Used as a backup if a node with a G score higher than <see cref="searchLength"/> could not be found
		/// </summary>
		uint maxGScorePathNodeIndex;

		/// <summary>The G score of <see cref="maxGScorePathNodeIndex"/></summary>
		uint maxGScore;

		/// <summary>
		/// An aim can be used to guide the pathfinder to not take totally random paths.
		/// For example you might want your AI to continue in generally the same direction as before, then you can specify
		/// aim to be transform.postion + transform.forward*10 which will make it more often take paths nearer that point
		/// See: <see cref="aimStrength"/>
		/// </summary>
		public Vector3 aim;

		int nodesEvaluatedRep;

		/// <summary>
		/// Random number generator source.
		///
		/// You may supply a custom random number generator here in order to get reproducible paths by using a fixed seed.
		/// If not, a new System.Random instance will be created when the path is constructed.
		///
		/// Keep in mind that multiple paths should never share the same System.Random instance, since paths are calculated in parallel.
		/// </summary>
		public System.Random rnd;

		protected override bool hasEndPoint => false;

		public override bool endPointKnownBeforeCalculation => false;

		protected override void Reset () {
			base.Reset();

			searchLength = 5000;
			spread = 5000;

			aimStrength = 0.0f;
			chosenPathNodeIndex = uint.MaxValue;
			maxGScorePathNodeIndex = uint.MaxValue;
			chosenPathNodeGScore = 0;
			maxGScore = 0;
			aim = Vector3.zero;
			rnd = new System.Random();

			nodesEvaluatedRep = 0;
		}

		public RandomPath () {}

		public static RandomPath Construct (Vector3 start, int length, OnPathDelegate callback = null) {
			var p = PathPool.GetPath<RandomPath>();

			p.Setup(start, length, callback);
			return p;
		}

		protected RandomPath Setup (Vector3 start, int length, OnPathDelegate callback) {
			this.callback = callback;

			searchLength = length;

			originalStartPoint = start;
			originalEndPoint = Vector3.zero;

			startPoint = start;
			endPoint = Vector3.zero;

			return this;
		}

		/// <summary>
		/// Refreshes the start point of the path.
		/// Useful if the agent has moved significantly since the path was requested.
		///
		/// If the path has not yet been processed by the pathfinding system, the start point will be updated immediately.
		/// If the path is being calculated or has been calculated, this function will do nothing.
		///
		/// A common way is to call this function once per frame until the path has been calculated,
		/// with the most up-to-date position of the agent.
		///
		/// Note: For a RandomPath, this method only updates the start point, since the end point is determined by the pathfinding result.
		/// </summary>
		public override void RefreshPathEndpoints (Vector3 start, Vector3 end) {
			lock (this) {
				if (PipelineState == PathState.PathQueue || PipelineState == PathState.Created) {
					originalStartPoint = startPoint = start;
				}
			}
		}

		/// <summary>
		/// Calls callback to return the calculated path.
		/// See: <see cref="callback"/>
		/// </summary>
		protected override void ReturnPath () {
			if (path != null && path.Count > 0) {
				originalEndPoint = endPoint;
			}
			if (callback != null) {
				callback(this);
			}
		}

		protected override void Prepare (ref SearchContext ctx) {
			var startNNInfo  = AstarPath.active.GetNearest(startPoint, nearestNodeConstraint);

			startPoint = startNNInfo.position;
			endPoint = startPoint;

#if ASTARDEBUG
			Debug.DrawLine((Vector3)startNNInfo.node.position, startPoint, Color.blue);
#endif

			if (startNNInfo.node == null) {
				FailWithError("Couldn't find close nodes to the start point");
				return;
			}

			UnityEngine.Assertions.Assert.IsTrue(traversalConstraint.CanTraverse(startNNInfo.node));

			heuristicScale = aimStrength;

			ctx.pathHandler.AddTemporaryNode(new TemporaryNode {
				type = TemporaryNodeType.Start,
				position = (Int3)startNNInfo.position,
				associatedNode = startNNInfo.node.NodeIndex,
			});
			ctx.heuristicObjective = new HeuristicObjective((int3)(Int3)aim, heuristic, heuristicScale);
			ctx.traversalConstraint = traversalConstraint;
			ctx.traversalCosts = traversalCosts;
			ctx.AddStartNodesToHeap();
		}

		protected override void OnHeapExhausted (ref SearchContext ctx) {
			if (chosenPathNodeIndex == uint.MaxValue && maxGScorePathNodeIndex != uint.MaxValue) {
				chosenPathNodeIndex = maxGScorePathNodeIndex;
				chosenPathNodeGScore = maxGScore;
			}

			if (chosenPathNodeIndex != uint.MaxValue) {
				OnFoundEndNode(ref ctx, chosenPathNodeIndex, 0, chosenPathNodeGScore);
			} else {
				FailWithError("Not a single node found to search");
			}
		}

		protected override void OnFoundEndNode (ref SearchContext ctx, uint pathNode, uint hScore, uint gScore) {
			if (ctx.pathHandler.IsTemporaryNode(pathNode)) {
				base.OnFoundEndNode(ref ctx, pathNode, hScore, gScore);
			} else {
				// The target node is a normal node.
				var node = ctx.pathHandler.GetNode(pathNode);
				endPoint = node.RandomPointOnSurface();

				cost = gScore;
				CompleteState = PathCompleteState.Complete;
				Trace(ref ctx, pathNode);
			}
		}

		public override void OnVisitNode (ref SearchContext ctx, uint pathNode, uint hScore, uint gScore) {
			// This method may be called multiple times without checking if the path is complete yet.
			if (CompleteState != PathCompleteState.NotCalculated) return;

			if (gScore >= searchLength) {
				if (gScore <= searchLength+spread) {
					nodesEvaluatedRep++;

					// Use reservoir sampling to pick a node from the ones with the highest G score
					if (rnd.NextDouble() <= 1.0f/nodesEvaluatedRep) {
						chosenPathNodeIndex = pathNode;
						chosenPathNodeGScore = gScore;
					}
				} else {
					// If no node was in the valid range of G scores, then fall back to picking one right outside valid range
					if (chosenPathNodeIndex == uint.MaxValue) {
						chosenPathNodeIndex = pathNode;
						chosenPathNodeGScore = gScore;
					}

					OnFoundEndNode(ref ctx, chosenPathNodeIndex, 0, chosenPathNodeGScore);
				}
			} else if (gScore > maxGScore) {
				maxGScore = gScore;
				maxGScorePathNodeIndex = pathNode;
			}
		}
	}
}
