using System;
using System.Collections.Generic;
using System.Text;
using Brains.Framework.Map;


namespace Brains.Framework.PathFinding
{
    public class PathFinder
    {
        public List<int> BlockedTiles = new List<int>();

        internal PriorityQueueB<PathfinderNode> openQueue;

        public List<PathfinderNode> closeList;

        public bool found = false;

        public static Random rand = new Random();

        public HeuristicType HType = HeuristicType.Euclidean;

        
        protected bool _initialized = false;
        protected int[,] _grid;
        protected bool _forceStop = false;
        protected bool _stopped = false;
        protected Agent _agent;
        protected Grid _map;
        protected GridCell _startNode;
        protected GridCell _endNode;
        protected PathfinderNode _parentNode;
        protected int _heuristicEstimateValue = 4;
        protected PathFinderState _state;

        protected static sbyte[,] direction = new sbyte[8, 2]
            { 
                { 0,-1} , 
                { 1, 0}, 
                { 0, 1}, 
                {-1, 0}, 
                { 1,-1}, 
                { 1, 1}, 
                {-1, 1},
                {-1,-1}
            };

        public Agent Agent
        {
            get { return _agent; }
            set { _agent= value; }
        }
        
        public Grid Grid
        {
            get { return _map; }
            set
            {
                _map= value;
            }
        }
        
        public GridCell StartNode
        {
            get { return _startNode; }
            set { _startNode = value; }
        }
        
        public GridCell EndNode
        {
            get { return _endNode; }
            set { _endNode = value; }
        }

        public int HeuristicEstimateValue
        {
            get { return _heuristicEstimateValue; }
            set { _heuristicEstimateValue = value; }
        }
        
        public PathFinderState State
        {
            get { return _state; }
        }

        public PathFinder()
        {
            BlockedTiles.Add(99);
        }

        ~PathFinder()
        {
            BlockedTiles.Clear();
            if (openQueue != null)
                openQueue.Clear();
            if(closeList!=null)
                closeList.Clear();
        }

        public virtual void Initilize()
        {
            if (_grid == null)
            {
                _grid = new int[_map.Cols, _map.Rows];

                for (int w = 0; w < _map.Cols; w++)
                {
                    for (int h = 0; h < _map.Rows; h++)
                    {
                        if (_agent == null)
                        {
                            if (_map.GetCell(w, h).Type == 0)
                                _grid[w, h] = 99;
                            else
                                _grid[w, h] = _map.GetCell(w, h).Type;
                        }
                        else
                            _grid[w, h] = _agent.GetCostOfCellType(_map.GetCell(w, h).Type);
                    }
                }
            }

            _parentNode = new PathfinderNode();
            _parentNode.G = 0;
            _parentNode.H = _heuristicEstimateValue;
            _parentNode.F = _parentNode.G + _parentNode.H;
            _parentNode.X = _startNode.X;
            _parentNode.Y = _startNode.Y;
            _parentNode.PX = _parentNode.X;
            _parentNode.PY = _parentNode.Y;

            _initialized = true;
            this._state = PathFinderState.Idle;
        }

        public void FindPath()
        {
            if (!_initialized)
            {
                Exception err = new Exception("ERROR: AIPathfinder.FindPath() - Class not initilized yet!");
            }

            _forceStop = false;
            _stopped = false;
            found = false;
            if (openQueue == null)
                openQueue = new PriorityQueueB<PathfinderNode>(new NodeComparer());
            if (closeList == null)
                closeList = new List<PathfinderNode>(Grid.Cols* Grid.Rows);
            
            openQueue.Clear();
            closeList.Clear();

            openQueue.Push(_parentNode);
            this._state = PathFinderState.Working;
            HType = (HeuristicType)rand.Next(0, 3);
        }

        public virtual void Reset()
        {
            if (openQueue == null)
                openQueue = new PriorityQueueB<PathfinderNode>(new NodeComparer());
            if (closeList == null)
                closeList = new List<PathfinderNode>(Grid.Cols * Grid.Rows);
            this.found = false;
            this.closeList.Clear();
            this.openQueue.Clear();
            this._stopped = false;
            this._state = PathFinderState.Idle;
            this._grid = null;
            this._initialized = false;
            this.Initilize();
            this.FindPath();
        }
        
        public virtual void Iterate()
        {
            if (found)
            {
                return;
            }
            _heuristicEstimateValue = 16;
            int _count=_grid.GetUpperBound(0) * _grid.GetUpperBound(1);

            //Check the whole map isnt on the close list
            if (closeList.Count < _count) 
            {
                if (closeList.Count + openQueue.Count < _count)
                {
                    //Should be at least one item on the queue
                    while (openQueue.Count > 0 && !_forceStop) 
                    {
                        //Get the next open node
                        _parentNode = openQueue.Pop();
                        
                        //We've found our destination tile
                        if (_parentNode.X == _endNode.X && _parentNode.Y == _endNode.Y) 
                        {
                            closeList.Add(_parentNode);
                            found = true;
                            break;
                        }
                        //For each adjacent square //first 4 sides then corners
                        for (int index = 0; index < 8; index++) 
                        {
                            int newX = _parentNode.X + direction[index, 0];
                            int newY = _parentNode.Y + direction[index, 1];
                            // probing out of map
                            if (newX < 0 || newY < 0 ||
                                newX >= _map.Cols|| 
                                newY >= _map.Rows)
                                continue;

                            PathfinderNode _node = new PathfinderNode();
                            _node.X = newX;
                            _node.Y = newY;
                            //BLOCKED Tile
                            if (BlockedTiles.Contains(_grid[_node.X, _node.Y])) 
                                continue;

                            _node.G = _map.GetCell(_node.X, _node.Y).Type;                            
                            int newG = _parentNode.G + (_grid[_node.X, _node.Y]*10);
                            if (index > 3) //Corner Tiles More Expensive
                                newG = _parentNode.G + (_grid[_node.X, _node.Y] * 16);
                            
                            // else
                            //     newG += (map.Node(newNode.X, newNode.Y)).Type;

                            if (newG == _parentNode.G)
                                continue;

                            int foundInOpenIndex = FoundInOpenList(_node);
                            if (foundInOpenIndex != -1 &&
                                openQueue[foundInOpenIndex].G < newG)
                                continue;

                            bool foundinclosedindex = FoundInClosedList(_node);
                            if (foundinclosedindex == true)
                                continue;

                            _node.PX = _parentNode.X;
                            _node.PY = _parentNode.Y;
                            _node.G += newG;

                            float Heuristic = 0;
                            //Three different types of Heuristic calculation
                            //if(HType==HeuristicType.Euclidean)
                            //    Heuristic = (int)(_heuristicEstimateValue  * (Math.Sqrt(Math.Pow((_node.X - _endNode.X), 2) + Math.Pow((_node.Y - _endNode.Y), 2))));
                            //else //if(HType==HeuristicType.Manhattan)
                                Heuristic = _heuristicEstimateValue * (Math.Abs(_endNode.X - _node.X) + Math.Abs(_endNode.Y - _node.Y));
                            //else if(HType==HeuristicType.Diagonal)
                              //  Heuristic = _heuristicEstimateValue * Math.Max(Math.Abs(_node.X - _endNode.X), Math.Abs(_node.X- _endNode.Y));

                            _node.H = (int)Heuristic;

                            _node.F = _node.G + _node.H;
                            if (foundInOpenIndex > -1)
                                openQueue[foundInOpenIndex] = _node;
                            else
                                openQueue.Push(_node);
                        }
                        closeList.Add(_parentNode);
                        return;
                        //break;
                    }
                }
            }

            if (found)
            {
                PathfinderNode fNode = closeList[closeList.Count - 1];

                for (int i = closeList.Count - 1; i >= 0; i--)
                {
                    if (fNode.PX == closeList[i].X && fNode.PY == closeList[i].Y || i == closeList.Count - 1)
                    {
                        fNode = closeList[i];
                    }
                    else
                    {
                        closeList.RemoveAt(i);
                    }
                }
                _stopped = true;
                _state = PathFinderState.Finished;
                return;

                //return mClose;
            }
            _stopped = true;
            _state = PathFinderState.Failed;
            return;

        }

        private bool FoundInClosedList(PathfinderNode node)
        {
            int _foundinclosedindex = -1;
            for (int j = 0; j < closeList.Count; j++)
            {
                if (closeList[j].X == node.X &&
                    closeList[j].Y == node.Y)
                {
                    _foundinclosedindex = j;
                    break;
                }
            }
            return _foundinclosedindex!=-1;
        }

        private int FoundInOpenList(PathfinderNode node)
        {
            int _foundInOpenIndex = -1;
            for (int j = 0; j < openQueue.Count; j++)
            {
                if (openQueue[j].X == node.X &&
                    openQueue[j].Y == node.Y)
                {
                    _foundInOpenIndex = j;
                    break;
                }
            }
            return _foundInOpenIndex ;
        }
        
       
        

    }
}