using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;

using UnityEngine;

using Framework.Geometry.Lines;

namespace Framework.Navigation.Platformer {

  /// <summary>Calculates paths by which actors can get from one place to another</summary>
  public class Navigator {

    #region class VisitedGround

    /// <summary>Stores the navigation leading up to a specific area</summary>
    private class VisitedGround {

      /// <summary>Initializes a new area navigation record</summary>
      /// <param name="parent">Parent area through which the area was reached</param>
      /// <param name="cost">Accumulated cost for reaching this area</param>
      /// <param name="entryX">Point at which this area was entered</param>
      public VisitedGround(Ground parent, float cost, float entryX) {
        this.Parent = parent;
        this.Cost = cost;
        this.EntryX = entryX;
      }

      /// <summary>Node from which the searcher came</summary>
      public Ground Parent;
      /// <summary>Cost with which the walkable area was reached</summary>
      /// <remarks>
      ///   This allows other path searchers to replace the node's parent if they
      ///   found a shorter way to it.
      /// </remarks>
      public float Cost;
      /// <summary>X coordinate where the agent entered the area</summary>
      public float EntryX;

    }

    #endregion // class VisitedGround;

    #region class PathSearch

    /// <summary>Searches for a path from the source to the target</summary>
    private class PathSearch {

      /// <summary>Initializes a new path search manager</summary>
      /// <param name="constraints">Constraints limiting the agent's movement</param>
      public PathSearch(Constraints constraints) {
        this.openNodesLookup = new Dictionary<Ground, VisitedGround>();
        this.closedNodesLookup = new Dictionary<Ground, VisitedGround>();
        this.instructions = new List<Instruction>();

        this.constraints = constraints;
      }

      /// <summary>Movement instructions that have resulted from the path search</summary>
      public IList<Instruction> Instructions {
        get { return this.instructions; }
      }

      /// <summary>Whether the path search has been cancelled</summary>
      public bool IsCancelled {
        get { return this.cancelRequested; }
      }

      /// <summary>Finds a path from the origin to the destination ground</summary>
      /// <param name="from">Ground on which the agent is starting</param>
      /// <param name="entryX">Current X coordinate of the agent</param>
      /// <param name="to">Ground the agent wants to reach</param>
      /// <returns>Navigation instructions from the origin to the destination</returns>
      public IList<Instruction> FindPath(Ground from, float entryX, Ground to) {
        runExhaustiveSearch(from, entryX);
        if(this.cancelRequested) {
          return null;
        }

        generateNavigationInstructions(from, entryX, to);
        if(this.cancelRequested) {
          return null;
        }

        return this.instructions;
      }

      /// <summary>Cancels the path search</summary>
      public void Cancel() {
        this.cancelRequested = true;
      }

      /// <summary>
      ///   Runs an exhaustive search (visiting all nodes) on the navigation graph
      /// </summary>
      /// <param name="from">Walkable area the search will start with</param>
      /// <param name="entryX">Current position on the starting walking able</param>
      /// <returns>The calculated costs for each node in the navigation graph</returns>
      private void runExhaustiveSearch(Ground from, float entryX) {
        this.openNodesLookup.Add(from, new VisitedGround(null, 0, entryX));
        float averageJumpDistance = this.constraints.MaximumJumpDistance / 2.0f;

        while(this.openNodesLookup.Count > 0) {
          KeyValuePair<Ground, VisitedGround> node = this.openNodesLookup.First();
          this.openNodesLookup.Remove(node.Key);
          this.closedNodesLookup.Add(node);

          // If the agent is no longer interested in the result, abort
          if(this.cancelRequested) {
            break;
          }

          // Check all other grounds that can be reached from this one and calculate
          // the cost of moving there
          for(int index = 0; index < node.Key.JumpTargets.Length; ++index) {
            Ground targetGround = node.Key.JumpTargets[index];
            entryX = node.Value.EntryX;
            Ground sourceGround = node.Key;

            // Look for the optimal jump to reach the other platform from this one.
            Segment2? optimalJump;
            {
              Segment2? leftJump = JumpCalculator.GetLeftJumpPath(
                sourceGround, targetGround, averageJumpDistance, this.constraints
              );
              Segment2? rightJump = JumpCalculator.GetRightJumpPath(
                sourceGround, targetGround, averageJumpDistance, this.constraints
              );

              // Prefer the side on which the agent has the shorter way
              if(leftJump.HasValue) {
                float distanceToLeftJumpOff = Math.Abs(entryX - leftJump.Value.From.x);
                if(rightJump.HasValue) {
                  float distanceToRightJumpOff = Math.Abs(entryX - rightJump.Value.From.x);
                  if(distanceToRightJumpOff < distanceToLeftJumpOff) {
                    optimalJump = rightJump;
                  }
                }
                optimalJump = leftJump;
              } else {
                optimalJump = rightJump;
              }
            }

            // It may not be possible for the agent to make this particular jump,
            // so only process it if a valid jump solution was found.
            if(optimalJump.HasValue) {

              // Estimate the movement cost of getting to the other ground
              float cost = node.Value.Cost;
              cost += Math.Abs(optimalJump.Value.From.x - entryX);
              cost += optimalJump.Value.Length;

              // Check whether we processed the target ground already
              VisitedGround target;
              if(!this.closedNodesLookup.TryGetValue(targetGround, out target)) {
                this.openNodesLookup.TryGetValue(targetGround, out target);
              }

              // Create a new target ground or update the existing one in case we
              // found a shorter route to it.
              if(target == null) {
                target = new VisitedGround(node.Key, cost, entryX);
                this.openNodesLookup.Add(targetGround, target);
              } else {
                if(cost < target.Cost) {
                  target.Parent = targetGround;
                  target.Cost = cost;
                  target.EntryX = entryX;
                }
              }

            } // if jump to jump target possible
          } // for each jump target in current open node
        } // while open nodes present
      }

      /// <summary>Generates the navigation instructions after the search finished</summary>
      /// <param name="from">Ground the agent starts on</param>
      /// <param name="entryX">Position the agent starts at</param>
      /// <param name="to">Ground the agent wants to reach</param>
      private void generateNavigationInstructions(Ground from, float entryX, Ground to) {
        var path = new Stack<Ground>();

        // Follow the parent nodes back to the starting point, building the path that
        // the agent has to follow in reverse
        Ground current = to;
        while(current != from) {
          path.Push(current);

          VisitedGround visitedGround;
          if(this.closedNodesLookup.TryGetValue(current, out visitedGround)) {
            current = visitedGround.Parent;
          } else {
            return; // No path from source to target
          }
        }

        // Now convert the path into movement instructions
        while(path.Count > 0) {
          Ground intermediate = path.Pop();

          if(this.cancelRequested) {
            return;
          }

          // Calculate a realistic jump trajectory for the left side
          Segment2? leftJump;
          {
            float leftBorder = Math.Max(from.Left, intermediate.Left);
            float leftJumpFloorX = Math.Min(
              Math.Max(entryX, leftBorder - this.constraints.MaximumJumpDistance),
              leftBorder
            );
            float leftOptimalDistance = (leftBorder - leftJumpFloorX);

            leftJump = JumpCalculator.GetLeftJumpPath(
              from, intermediate,
              leftOptimalDistance / this.constraints.MaximumJumpDistance, this.constraints
            );
          }

          // Calculate a realistic jump trajectory for the right side
          Segment2? rightJump;
          {
            float rightBorder = Math.Min(from.Right, intermediate.Right);
            float rightJumpFloorX = Math.Min(
              Math.Max(entryX, rightBorder),
              rightBorder + this.constraints.MaximumJumpDistance
            );
            float rightOptimalDistance = (rightJumpFloorX - rightBorder);

            rightJump = JumpCalculator.GetRightJumpPath(
              from, intermediate,
              rightOptimalDistance / this.constraints.MaximumJumpDistance, this.constraints
            );
          }

          // Prefer the side on which the agent has the shorter way
          Segment2? jump = null;
          if(leftJump.HasValue) {
            float distanceToLeftJumpOff = Math.Abs(entryX - leftJump.Value.From.x);
            if(rightJump.HasValue) {
              float distanceToRightJumpOff = Math.Abs(entryX - rightJump.Value.From.x);
              if(distanceToRightJumpOff < distanceToLeftJumpOff) {
                jump = rightJump;
              }
            }
            jump = leftJump;
          }
          if(rightJump.HasValue) {
            jump = rightJump;
          }

          // Add a movement instruction to the list if the jump off point requires moving
          float distanceToJumpOffPoint = Math.Abs(jump.Value.From.x - entryX);
          if(distanceToJumpOffPoint > 0.01f) {
            this.instructions.Add(
              new Instruction() {
                Ground = from,
                Action = Action.Walk,
                From = new Vector3(entryX, from.GetGroundHeightAt(entryX), 0.0f),
                To = new Vector3(jump.Value.From.x, jump.Value.From.y, 0.0f)
              }
            );
          }

          // Add the jump itself
          this.instructions.Add(
            new Instruction() {
              Ground = from,
              Action = Action.Jump,
              From = new Vector3(jump.Value.From.x, jump.Value.From.y, 0.0f),
              To = new Vector3(jump.Value.To.x, jump.Value.To.y, 0.0f)
            }
          );

          entryX = jump.Value.To.x;
          from = intermediate;
        }
      }

      /// <summary>Reachable nodes that have yet to be checked</summary>
      private IDictionary<Ground, VisitedGround> openNodesLookup;
      /// <summary>Reachable nodes that were already checked</summary>
      private IDictionary<Ground, VisitedGround> closedNodesLookup;
      /// <summary>Constraints by which the agent's movement is limited</summary>
      private Constraints constraints;
      /// <summary>Whether the search has been cancelled</summary>
      private volatile bool cancelRequested;
      /// <summary>Navigation instructions the agent has to follow</summary>
      private IList<Instruction> instructions;

    }

    #endregion // class PathSearch

    #region class PathSearchParameters

    /// <summary>Carries the parameters of a path search</summary>
    private class PathSearchParameters {

      /// <summary>Initializes a new path search parameter container</summary>
      /// <param name="from">Ground from which on the agent is starting</param>
      /// <param name="entryX">Position the agent is starting at</param>
      /// <param name="to">Ground to which the agent wants to move</param>
      public PathSearchParameters(Ground from, float entryX, Ground to) {
        this.From = from;
        this.EntryX = entryX;
        this.To = to;
      }

      /// <summary>Ground from which on the agent is starting</summary>
      public Ground From;
      /// <summary>Position the agent is starting at</summary>
      public float EntryX;
      /// <summary>Ground to which the agent wants to move</summary>
      public Ground To;

    }

    #endregion // class PathSearchParameters

    #region class AsynchronousPathSearch

    /// <summary>Performs a path search asynchronously in a background thread</summary>
    private class AsynchronousPathSearch : PathSearch, IAsyncResult {

      /// <summary>Initializes a new asynchronous path search</summary>
      /// <param name="constraints">
      ///   Constraints by which the movement of the agent is limited
      /// </param>
      /// <param name="completionCallback">
      ///   Callback that will be invoked when the path search has finished
      /// </param>
      /// <param name="state">Custom object that will be passed to the callback</param>
      public AsynchronousPathSearch(
        Constraints constraints, WaitCallback completionCallback, object state
      ) : base(constraints) {
        this.completionCallback = completionCallback;
        this.state = state;

        this.findPathInThreadDelegate = new WaitCallback(findPathInThread);
      }

      /// <summary>Starts a path search in a background thread</summary>
      /// <param name="from">Ground the agent is starting on</param>
      /// <param name="entryX">Current position of the agent on the ground</param>
      /// <param name="to">Ground the agent wants to get to</param>
      public void Start(Ground from, float entryX, Ground to) {
        ThreadPool.QueueUserWorkItem(
          this.findPathInThreadDelegate, new PathSearchParameters(from, entryX, to)
        );
      }

      /// <summary>Exception that has occurred in the background thread</summary>
      public Exception Exception {
        get { return this.exception; }
      }

      /// <summary>Searches a path from the origin to the destination</summary>
      /// <param name="state">
      ///   Path search parameter container with informations about the path search
      /// </param>
      private void findPathInThread(object state) {
        string oldName = Thread.CurrentThread.Name;
        Thread.CurrentThread.Name = "Navigator Path Searching Thread";
        try {
          var parameters = (PathSearchParameters)state;
          FindPath(parameters.From, parameters.EntryX, parameters.To);

          // Let the caller know that the search is finished
          lock(this) {
            this.isCompleted = true;
            if(this.completionEvent != null) {
              this.completionEvent.Set();
            }
          }

          // If the caller wanted to be notified upon completion, run the callback
          if(this.completionCallback != null) {
            this.completionCallback(this.state);
          }
        }
        catch(Exception exception) {
          this.exception = exception;
        }
        finally {
          Thread.CurrentThread.Name = oldName;
        }
      }

      /// <summary>Not supported. Always null.</summary>
      public object AsyncState {
        get { return this.state; }
      }

      /// <summary>
      ///   Returns a wait handle that can be used to wait until the path search is finished
      /// </summary>
      public WaitHandle AsyncWaitHandle {
        get {
          if(this.completionEvent == null) {
            lock(this) {
              if(this.completionEvent == null) {
                this.completionEvent = new ManualResetEvent(this.isCompleted);
              }
            }
          }

          return this.completionEvent;
        }
      }

      /// <summary>
      ///   Whether the path search has completed in the same thread that started it
      /// </summary>
      public bool CompletedSynchronously {
        get { return false; }
      }

      /// <summary>True if the path search has finished</summary>
      public bool IsCompleted {
        get { return this.isCompleted; }
      }

      /// <summary>Only created on demand; cleared when the path search finished</summary>
      private ManualResetEvent completionEvent;
      /// <summary>Whether the path search has finished</summary>
      private bool isCompleted;
      /// <summary>Delegate for the findPathInThread() method</summary>
      private WaitCallback findPathInThreadDelegate;
      /// <summary>Callback that will be invoked when the search has finished</summary>
      private WaitCallback completionCallback;
      /// <summary>User-defined state that will be passed to the callback</summary>
      private object state;
      /// <summary>Exception that occurred in the worker thread, if any</summary>
      private Exception exception;

    }

    #endregion // class AsynchronousPathSearch

    /// <summary>Calculates the best path for an agent to go to a target position</summary>
    /// <param name="from">Location the agent is starting from</param>
    /// <param name="to">Location the agent wants to move to</param>
    /// <param name="constraints">The agent's abilities - jump height, distance, etc.</param>
    /// <returns>A list of instructions the agent can follow to get to the target</returns>
    public static IList<Instruction> FindPath(
      Locator from, Locator to, Constraints constraints
    ) {
      return new PathSearch(constraints).FindPath(
        from.CurrentGround, from.transform.position.x, to.CurrentGround
      );
    }

    /// <summary>Calculates the best path for an agent to go to a target position</summary>
    /// <param name="from">Location the agent is starting from</param>
    /// <param name="to">Location the agent wants to move to</param>
    /// <param name="constraints">The agent's abilities - jump height, distance, etc.</param>
    /// <returns>
    ///   An asynchronous result handle that can be used to wait for
    ///   completion of the path search
    /// </returns>
    public static IAsyncResult BeginFindPath(
      Locator from, Locator to, Constraints constraints
    ) {
      var pathSearch = new AsynchronousPathSearch(constraints, null, null);
      pathSearch.Start(
        from.CurrentGround, from.transform.position.x, to.CurrentGround
      );
      return pathSearch;
    }

    /// <summary>
    ///   Waits for an asynchronous path search to end and returns the movement instructions
    ///   that have resulted from the path search
    /// </summary>
    /// <param name="asyncResult">Async result returned by the BeginFindPath() method</param>
    /// <returns>A lsit of movement instructions that lead from start to finish</returns>
    public static IList<Instruction> EndFindPath(IAsyncResult asyncResult) {
      var pathSearch = asyncResult as AsynchronousPathSearch;
      if(pathSearch == null) {
        throw new ArgumentException(
          "The asyncResult does not belong to this class", "asyncResult"
        );
      }

      // If the path search is not yet finished, wait until it is
      if(!pathSearch.IsCompleted) {
        pathSearch.AsyncWaitHandle.WaitOne();
      }

      // If the path search was cancelled, end with the appropriate exception
      if(pathSearch.IsCancelled) {
        throw new OperationCanceledException("Path search was cancelled");
      }

      // If an error occurred during the path search, rethrow it
      if(pathSearch.Exception != null) {
        throw pathSearch.Exception;
      }

      // Everything seems to have worked, return the movement instructions
      return pathSearch.Instructions;
    }

    /// <summary>Cancels the path search running in the background</summary>
    /// <param name="asyncResult">Async result returned by the BeginFindPath() method</param>
    public static void CancelFindPath(IAsyncResult asyncResult) {
      var pathSearch = asyncResult as AsynchronousPathSearch;
      if(pathSearch == null) {
        throw new ArgumentException(
          "The asyncResult does not belong to this class", "asyncResult"
        );
      }

      pathSearch.Cancel();
    }

  }

} // namespace Framework.Navigation.Platformer