// Uncomment this to disable diagonal movemet.
//#define ALLOW_DIAGONAL_MOVEMENT

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using System.Threading;
using Microsoft.Xna.Framework;

namespace CarFire
{
    /// <summary>
    /// A class to navigate from here to there through a grid of
    /// open and closed cells.
    /// </summary>
    public class PathFinder
    {
        #region Public Types

        /// <summary>
        /// The heuristic function should return some value representing
        /// the distance between two points.  A common approach is to
        /// return the manhattan distance.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">The endpoint.</param>
        /// <returns>The heuristic.</returns>
        public delegate int Heuristic(Point a, Point b);

        /// <summary>
        /// The cost function should take two points representing two
        /// adjacent cells and return a cost measure of how expensive it
        /// is to move from one cell to the other.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">Another point.</param>
        /// <returns>The cost.</returns>
        public delegate int CostFunction(Point a, Point b);


        /// <summary>
        /// An async task object to manage a search task that has been
        /// put on the thread pool.
        /// </summary>
        public class AsyncTask
        {
            /// <summary>
            /// Construct an async task object.
            /// </summary>
            /// <param name="finder">A path finder.</param>
            /// <param name="start">The cell to start at.</param>
            /// <param name="finish">The desired destination.</param>
            /// <param name="heuristic">The heuristic function.</param>
            /// <param name="costFunction">The cost function.</param>
            public AsyncTask(PathFinder finder, Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
            {
                mPathFinder = finder;
                mStart = start;
                mFinish = finish;
                mHeuristic = heuristic;
                mCostFunction = costFunction;
            }


            /// <summary>
            /// Determine whether or not the task has completed.
            /// </summary>
            public bool IsCompleted { get { return mIsDone; } }

            /// <summary>
            /// Get the resulting path.
            /// </summary>
            public List<Point> Path { get { return mPath; } }


            #region Private Members

            public void Run(object context)
            {
                mPath = mPathFinder.GetPath(mStart, mFinish, mHeuristic, mCostFunction);
                mIsDone = true;
            }


            PathFinder mPathFinder;
            Point mStart;
            Point mFinish;
            Heuristic mHeuristic;
            CostFunction mCostFunction;

            List<Point> mPath;
            bool mIsDone;

            #endregion
        }

        #endregion


        #region Public Methods

        /// <summary>
        /// Construct a path finder with a grid.  The grid is a matrix
        /// of boolean values, true meaning the cell is walkable and false
        /// meaning the cell is closed.
        /// </summary>
        /// <param name="grid">The grid to find paths on.</param>
        public PathFinder(bool[,] grid)
        {
            Debug.Assert(grid != null);

            mGrid = grid;
            mGridWidth = mGrid.GetUpperBound(0) + 1;
            mGridHeight = mGrid.GetUpperBound(1) + 1;
        }


        /// <summary>
        /// The A* algorithm for finding the best path through a grid of cells.
        /// The manhattan distance heuristic and a simple distance-based cost
        /// function will be used.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <returns>A list of points representing the path through the grid,
        /// starting point not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish)
        {
            return GetPath(start, finish, GetManhattanDistance, GetCost);
        }

        /// <summary>
        /// The A* algorithm for finding the best path through a grid of cells.
        /// A simple distance-based cost function will be used.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <param name="heuristic">The heuristic function.</param>
        /// <returns>A list of points representing the path through the grid,
        /// starting point not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish, Heuristic heuristic)
        {
            return GetPath(start, finish, heuristic, GetCost);
        }

        /// <summary>
        /// The A* algorithm for finding the best path through a grid of cells.
        /// The manhattan distance heuristic will be used.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <param name="costFunction">The cost function</param>
        /// <returns>A list of points representing the path through the grid,
        /// starting point not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish, CostFunction costFunction)
        {
            return GetPath(start, finish, GetManhattanDistance, costFunction);
        }

        /// <summary>
        /// The A* algorithm for finding the best path through a grid of cells.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <param name="heuristic">The heuristic function.</param>
        /// <param name="costFunction">The cost function.</param>
        /// <returns>A list of points representing the path through the grid,
        /// starting point not included, or null if no path could be found.</return>
        public List<Point> GetPath(Point start, Point finish, Heuristic heuristic, CostFunction costFunction)
        {
            mFringe = new BinaryHeap<Cell>();
            mCells = new Cell[mGridWidth, mGridHeight];

            Cell startCell = new Cell(start, 0, heuristic(start, finish));
            mFringe.Add(startCell);
            mCells[start.X, start.Y] = startCell;
            while (mFringe.Count > 0)
            {
                Cell cell = mFringe.GetNext();
                cell.IsOpen = false;

                if (cell.Point == finish)
                {
                    List<Point> list = new List<Point>();
                    list.Add(cell.Point);

                    cell = cell.Parent;
                    if (cell != null) for (; cell.Point != start; cell = cell.Parent) list.Add(cell.Point);

                    list.Reverse();
                    return list;
                }

                List<Point> neighbors = new List<Point>(8);
                neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
                neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
                neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
                neighbors.Add(new Point(cell.Point.X, cell.Point.Y + 1));
#if ALLOW_DIAGONAL_MOVEMENT
                neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));
                neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));
                neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
                neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
#endif
                foreach (Point point in neighbors)
                {
                    if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight &&
                        mGrid[point.X, point.Y])
                    {
                        int cost = cell.G + costFunction(cell.Point, point);

                        Cell inQueue = mCells[point.X, point.Y];
                        if (inQueue == null)
                        {
                            Cell neighbor = new Cell(point, cost, heuristic(point, finish), cell);
                            mFringe.Add(neighbor);
                            mCells[point.X, point.Y] = neighbor;
                        }
                        else if (inQueue.IsOpen && cost < inQueue.G)
                        {
                            inQueue.G = cost;
                            inQueue.Parent = cell;
                            mFringe.Promote(inQueue);
                        }
                    }
                }
            }

            return null;
        }


        /// <summary>
        /// Get a shortest path by putting the task on the thread pool.
        /// </summary>
        /// <param name="start">The cell to start at.</param>
        /// <param name="finish">The desired destination.</param>
        /// <returns>The async task object.</returns>
        public AsyncTask GetPathAsync(Point start, Point finish)
        {
            AsyncTask task = new AsyncTask(this, start, finish, GetManhattanDistance, GetCost);
            ThreadPool.QueueUserWorkItem(new WaitCallback(task.Run));
            return task;
        }


        /// <summary>
        /// Find the closest open cell that is near another cell.
        /// </summary>
        /// <param name="coordinates">The coordinates.</param>
        /// <returns>An open cell at or near the given coordinates,
        /// or null if no open nearby cell could be found.</returns>
        public Point? GetNearbyOpenCell(Point coordinates)
        {
            if (0 <= coordinates.X && coordinates.X < mGridWidth && 0 <= coordinates.Y && coordinates.Y < mGridHeight &&
                mGrid[coordinates.X, coordinates.Y])
            {
                return coordinates;
            }

            mFringe = new BinaryHeap<Cell>();
            mCells = new Cell[mGridWidth, mGridHeight];

            Cell startCell = new Cell(coordinates, 0, 0);
            mFringe.Add(startCell);
            mCells[coordinates.X, coordinates.Y] = startCell;
            while (mFringe.Count > 0)
            {
                Cell cell = mFringe.GetNext();

                List<Point> neighbors = new List<Point>(8);
                neighbors.Add(new Point(cell.Point.X, cell.Point.Y - 1));
                neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y));
                neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y));
                neighbors.Add(new Point(cell.Point.X, cell.Point.Y + 1));
                neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y - 1));
                neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y - 1));
                neighbors.Add(new Point(cell.Point.X - 1, cell.Point.Y + 1));
                neighbors.Add(new Point(cell.Point.X + 1, cell.Point.Y + 1));
                foreach (Point point in neighbors)
                {
                    if (0 <= point.X && point.X < mGridWidth && 0 <= point.Y && point.Y < mGridHeight)
                    {
                        if (mGrid[point.X, point.Y])
                        {
                            return point;
                        }
                        else
                        {
                            int cost = cell.G + GetCost(cell.Point, point);

                            Cell inQueue = mCells[point.X, point.Y];
                            if (inQueue == null)
                            {
                                Cell neighbor = new Cell(point, cost, 0);
                                mFringe.Add(neighbor);
                                mCells[point.X, point.Y] = neighbor;
                            }
                        }
                    }
                }
            }

            return null;
        }


        /// <summary>
        /// Get the manhattan distance between two points.  This is a simple but
        /// effective and commonly-used heuristic.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">Another point.</param>
        /// <returns>The manhattan distance.</returns>
        public static int GetManhattanDistance(Point a, Point b)
        {
            int w = b.X - a.X;
            int h = b.Y - a.Y;
            if (w < 0) w = -w;
            if (h < 0) h = -h;
            return w + h;
        }

        /// <summary>
        /// Get the cost to travel from one point to another.  This is a simple
        /// cost function based purely on distance.  On a square grid, diagonal
        /// cells are further away than adjacent cells; therefore, adjacent moves
        /// are favored.
        /// </summary>
        /// <param name="a">A point.</param>
        /// <param name="b">Another point.</param>
        /// <returns>The cost.</returns>
        public static int GetCost(Point a, Point b)
        {
            if (a.X != b.X && a.Y != b.Y) return 14;
            return 10;
        }

        #endregion


        #region Private Types

        class Cell : IComparable<Cell>
        {
            public Point Point;
            public Cell Parent;
            public bool IsOpen;

            public int G
            {
                get { return mG; }
                set { mG = value; mF = mG + mH; }
            }

            public int H
            {
                get { return mH; }
                set { mH = value; mF = mG + mH; }
            }

            public int F { get { return mF; } }


            public Cell(Point point, int g, int h)
            {
                Point = point;
                IsOpen = true;
                mG = g;
                mH = h;
                mF = g + h;
            }

            public Cell(Point point, int g, int h, Cell parent)
            {
                Point = point;
                Parent = parent;
                IsOpen = true;
                mG = g;
                mH = h;
                mF = g + h;
            }

            public int CompareTo(Cell other)
            {
                return F - other.F;
            }


            int mG;
            int mH;
            int mF;
        }

        #endregion


        #region Private Variables

        bool[,] mGrid;
        int mGridWidth;
        int mGridHeight;

        IPriorityQueue<Cell> mFringe;
        Cell[,] mCells;

        #endregion
    }
}