using System;
using System.Drawing;
using System.Collections.Generic;

using Uber.HabboHotel.Rooms;

namespace Uber.HabboHotel.Pathfinding
{
    public struct PathFinderNode
    {
        public int F;
        public int G;
        public int H;
        public int X;
        public int Y;
        public int PX;
        public int PY;
    }

    public enum PathFinderNodeType
    {
        Start = 1,
        End = 2,
        Open = 4,
        Close = 8,
        Current = 16,
        Path = 32
    }

    public enum HeuristicFormula
    {
        Manhattan = 1,
        MaxDXDY = 2,
        DiagonalShortCut = 3,
        Euclidean = 4,
        EuclideanNoSQR = 5,
        Custom1 = 6
    }

    public enum MatrixState
    {
        BLOCKED = 0,
        WALKABLE = 1,
        WALKABLE_LASTSTEP = 2
    }

    public delegate void PathFinderDebugHandler(int fromX, int fromY, int x, int y, PathFinderNodeType type, int totalCost, int cost);

    public class PathFinder : IPathFinder
    {
        private MatrixState[,] mGrid = null;
        private double[,] mHeightmap = null;
        private int mapSizeX;
        private int mapSizeY;
        private float mMaximumAscend = 1.5f;
        private float mMaximumDescend = 4;
        private bool mAccountHeights = true;
        private PriorityQueueB<PathFinderNode> mOpen = new PriorityQueueB<PathFinderNode>(new ComparePFNode());
        private List<PathFinderNode> mClose = new List<PathFinderNode>();
        private bool mStop = false;
        private bool mStopped = true;
        private HeuristicFormula mFormula = HeuristicFormula.MaxDXDY;
        private int mHEstimate = 2;
        private bool mTieBreaker = false;
        private int mSearchLimit = 25000;
        private double mCompletedTime = 0;

        public PathFinder(MatrixState[,] grid, double[,] height, int sizeX, int sizeY)
        {
            if (grid == null) throw new Exception("Pathfinder: Grid cannot be null");
            if (height == null) throw new Exception("Heightmap: Height cannot be null");

            mGrid = grid;
            mHeightmap = height;
            mapSizeX = sizeX;
            mapSizeY = sizeY;

            mSearchLimit = mapSizeX * mapSizeY;
        }

        public bool AccountHeights
        {
            get { return mAccountHeights; }
            set { mAccountHeights = value; }
        }

        public float MaxAscend
        {
            get { return mMaximumAscend; }
            set { mMaximumAscend = value; }
        }

        public float MaxDescend
        {
            get { return mMaximumDescend; }
            set { mMaximumDescend = value; }
        }

        public double[,] Heightmap
        {
            get { return mHeightmap; }
            set { mHeightmap = value; }
        }

        public bool Stopped
        {
            get { return mStopped; }
        }

        public HeuristicFormula Formula
        {
            get { return mFormula; }
            set { mFormula = value; }
        }

        public int HeuristicEstimate
        {
            get { return mHEstimate; }
            set { mHEstimate = value; }
        }

        public bool TieBreaker
        {
            get { return mTieBreaker; }
            set { mTieBreaker = value; }
        }

        public int SearchLimit
        {
            get { return mSearchLimit; }
            set { mSearchLimit = value; }
        }

        public double CompletedTime
        {
            get { return mCompletedTime; }
            set { mCompletedTime = value; }
        }

        public void FindPathStop()
        {
            mStop = true;
        }

        public List<PathFinderNode> FindPath(Coord Start, Coord End)
        {
            PathFinderNode parentNode;
            bool found = false;
            int gridX = mapSizeX;
            int gridY = mapSizeY;

            mStop = false;
            mStopped = false;
            mOpen.Clear();
            mClose.Clear();

            sbyte[,] direction;
            direction = new sbyte[4, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }};

            parentNode.G = 0;
            parentNode.H = mHEstimate;
            parentNode.F = parentNode.G + parentNode.H;
            parentNode.X = Start.x;
            parentNode.Y = Start.y;
            parentNode.PX = parentNode.X;
            parentNode.PY = parentNode.Y;
            mOpen.Push(parentNode);

            while (mOpen.Count > 0 && !mStop)
            {
                parentNode = mOpen.Pop();

                if (parentNode.X == End.x && parentNode.Y == End.y)
                {
                    mClose.Add(parentNode);
                    found = true;
                    break;
                }

                if (mClose.Count > mSearchLimit)
                {
                    mStopped = true;
                    return null;
                }

                // Lets calculate each successors
                for (int i = 0; i < 4; i++)
                {
                    PathFinderNode newNode;
                    newNode.X = parentNode.X + direction[i, 0];
                    newNode.Y = parentNode.Y + direction[i, 1];

                    if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
                    {
                        continue;
                    }

                    int newG = parentNode.G + (int)mGrid[newNode.X, newNode.Y];

                    if (newG == parentNode.G)
                    {
                        continue;
                    }

                    // Take into account heights
                    if (mAccountHeights)
                    {
                        if (mMaximumDescend > -1)
                        {
                            if (mHeightmap[parentNode.X, parentNode.Y] - mMaximumDescend > mHeightmap[newNode.X, newNode.Y])
                            {
                                continue;
                            }
                        }

                        if (mMaximumAscend > -1)
                        {
                            if (mHeightmap[parentNode.X, parentNode.Y] + mMaximumAscend < mHeightmap[newNode.X, newNode.Y])
                            {
                                continue;
                            }
                        }
                    }

                    int foundInOpenIndex = -1;
                    for (int j = 0; j < mOpen.Count; j++)
                    {
                        if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
                        {
                            foundInOpenIndex = j;
                            break;
                        }
                    }

                    if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
                    {
                        continue;
                    }

                    int foundInCloseIndex = -1;
                    for (int j = 0; j < mClose.Count; j++)
                    {
                        if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
                        {
                            foundInCloseIndex = j;
                            break;
                        }
                    }

                    if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
                    {
                        continue;
                    }

                    newNode.PX = parentNode.X;
                    newNode.PY = parentNode.Y;
                    newNode.G = newG;

                    switch (mFormula)
                    {
                        default:
                        case HeuristicFormula.Manhattan:
                            newNode.H = mHEstimate * (Math.Abs(newNode.X - End.x) + Math.Abs(newNode.Y - End.y));
                            break;
                        case HeuristicFormula.MaxDXDY:
                            newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - End.x), Math.Abs(newNode.Y - End.y)));
                            break;
                        case HeuristicFormula.DiagonalShortCut:
                            int h_diagonal = Math.Min(Math.Abs(newNode.X - End.x), Math.Abs(newNode.Y - End.y));
                            int h_straight = (Math.Abs(newNode.X - End.x) + Math.Abs(newNode.Y - End.y));
                            newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
                            break;
                        case HeuristicFormula.Euclidean:
                            newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - End.x), 2) + Math.Pow((newNode.Y - End.y), 2)));
                            break;
                        case HeuristicFormula.EuclideanNoSQR:
                            newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - End.x), 2) + Math.Pow((newNode.Y - End.y), 2)));
                            break;
                        case HeuristicFormula.Custom1:
                            Point dxy = new Point(Math.Abs(End.x - newNode.X), Math.Abs(End.y - newNode.Y));
                            int Orthogonal = Math.Abs(dxy.X - dxy.Y);
                            int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
                            newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
                            break;
                    }

                    if (mTieBreaker)
                    {
                        int dx1 = parentNode.X - End.x;
                        int dy1 = parentNode.Y - End.y;
                        int dx2 = Start.x - End.x;
                        int dy2 = Start.y - End.y;
                        int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
                        newNode.H = (int)(newNode.H + cross * 0.001);
                    }

                    newNode.F = newNode.G + newNode.H;
                    mOpen.Push(newNode);
                }

                mClose.Add(parentNode);
            }

            if (found)
            {
                PathFinderNode fNode = mClose[mClose.Count - 1];
                for (int i = mClose.Count - 1; i >= 0; i--)
                {
                    if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
                    {
                        fNode = mClose[i];
                    }
                    else
                    {
                        mClose.RemoveAt(i);
                    }
                }

                mStopped = true;
                return mClose;
            }

            mStopped = true;
            return null;
        }

        internal class ComparePFNode : IComparer<PathFinderNode>
        {
            public int Compare(PathFinderNode x, PathFinderNode y)
            {
                if (x.F > y.F)
                {
                    return 1;
                }
                else if (x.F < y.F)
                {
                    return -1;
                }

                return 0;
            }
        }
    }
}