using System;
using System.Collections;
using System.Linq;
using System.Text;

namespace Holo.Virtual.Rooms.Pathfinding
{
    public class IHIPathfinder
    {
        /// <summary>
        /// Stores the state of the tiles.
        /// </summary>
        private byte[,] CollisionMap;
        /// <summary>
        /// Stores the height of the tiles.
        /// </summary>
        private float[,] Height;

        public IHIPathfinder()
        {

        }

        /// <summary>
        /// Set the tile map.
        /// </summary>
        /// <param name="Map">The state of the tiles.</param>
        /// <param name="Height">The height of the tiles.</param>
        public void ApplyCollisionMap(byte[,] Map, float[,] Height)
        {
            // Is this replacing an existing tile map
            if (CollisionMap != null)
                // Yes, ensure thread safety.
                lock (CollisionMap)
                {
                    // Set the tile states.
                    CollisionMap = Map;
                    // Set the tile heights
                    this.Height = Height;
                }
            else
            {
                // No, don't worry about other threads.

                // Set the tile states.
                CollisionMap = Map;
                // Set the tile heights
                this.Height = Height;
            }
        }

        // TODO: Finish commenting

        /// <summary>
        /// Get the next step on a path.
        /// </summary>
        /// <param name="StartX">PointA X</param>
        /// <param name="StartY">PointA Y</param>
        /// <param name="EndX">PointB X</param>
        /// <param name="EndY">PointB Y</param>
        /// <returns></returns>
        public int[] Path(byte StartX, byte StartY, byte EndX, byte EndY, bool[,] UnitMap)
        {
            IHIPathfinderValues Values;
            lock (CollisionMap) // Thread Safety
            {
                Values = new IHIPathfinderValues(CollisionMap, Height, UnitMap);


                if (EndX >= CollisionMap.GetLength(0) || EndY >= CollisionMap.GetLength(1) || CollisionMap[EndX, EndY] == 0 || (StartX == EndX && StartY == EndY)) // Does a path need to be found and if so, the end on the map.
                    return new int[0]; // No, no path possible/required.

                #region Init
                if (StartX > CollisionMap.GetUpperBound(0) || StartY > CollisionMap.GetUpperBound(1)) // Is the start on the map
                    return new int[0]; // No, no path possible.

                Values.LastID++;
                Values.Count++;
                Values.BinaryHeap[Values.Count] = Values.LastID;
                Values.X[Values.LastID] = StartX;
                Values.Y[Values.LastID] = StartY;
                Values.H[Values.LastID] = (ushort)GetH(StartX, StartY, EndX, EndY);
                Values.Parent[Values.LastID] = 0;
                Values.G[Values.LastID] = 0;
                Values.F[Values.LastID] = (ushort)(Values.G[Values.LastID] + Values.H[Values.LastID]);
                #endregion


                while (Values.Count != 0)
                {
                    Values.Location = Values.BinaryHeap[1];

                    if (Values.X[Values.Location] == EndX && Values.Y[Values.Location] == EndY)
                        break;

                    Move(Values);

                    Add(-1, 0, EndX, EndY, Values);
                    Add(0, -1, EndX, EndY, Values);
                    Add(1, 0, EndX, EndY, Values);
                    Add(0, 1, EndX, EndY, Values);

                    Add(-1, -1, EndX, EndY, Values);
                    Add(-1, 1, EndX, EndY, Values);
                    Add(1, -1, EndX, EndY, Values);
                    Add(1, 1, EndX, EndY, Values);
                }
            }

            if (Values.Count == 0)
                return new int[0];

            while (Values.X[Values.Parent[Values.Location]] != StartX || Values.Y[Values.Parent[Values.Location]] != StartY)
            {
                Values.Location = Values.Parent[Values.Location];
            }

            return new int[] { Values.X[Values.Location], Values.Y[Values.Location] };
        }



        int GetH(int X, int Y, int EndX, int EndY)
        {
            return (Math.Abs(X + EndX) + Math.Abs(Y + EndY));
        }


        private void Add(sbyte iX, sbyte iY, byte EndX, byte EndY, IHIPathfinderValues Values)
        {
            byte X = (byte)(Values.X[Values.Location] + iX);
            byte Y = (byte)(Values.Y[Values.Location] + iY);
            ushort Parent = Values.Location;

            if (X > CollisionMap.GetUpperBound(0) || Y > CollisionMap.GetUpperBound(1))
                return;

            if (Values.Tiles[X, Y] == 2)
                return;
            if ((CollisionMap[X, Y] == 0 || (CollisionMap[X, Y] == 2 && (X != EndX || Y != EndY))))
                return;
            if (Values.UnitMap[X, Y])
                return;

            float Z = Values.Z[X, Y];
            float pZ = Values.Z[Values.X[Parent], Values.Y[Parent]];
            if (Z > pZ + 1.2 || Z < pZ - 3.0) // TODO: Max/Min jump height from database
                return;

            if (Parent > 0)
            {
                if (Values.X[Parent] != X && Values.Y[Parent] != Y)
                {
                    if (CollisionMap[X, Values.Y[Parent]] == 0 || CollisionMap[X, Values.Y[Parent]] == 2)
                        return;
                    if (CollisionMap[Values.X[Parent], Y] == 0 || CollisionMap[Values.X[Parent], Y] == 2)
                        return;
                }
            }



            if (Values.Tiles[X, Y] == 1)
            {
                ushort i = 1;
                for (; i <= Values.Count; i++)
                {
                    if (Values.X[i] == X && Values.Y[i] == Y)
                        break;
                }

                if (Values.X[i] == EndX || Values.Y[i] == EndY)
                {
                    if (10 + Values.G[Parent] < Values.G[i])
                        Values.Parent[i] = Parent;
                }
                else
                    if (14 + Values.G[Parent] < Values.G[i])
                        Values.Parent[i] = Parent;
                return;
            }

            Values.LastID++;
            Values.Count++;
            Values.BinaryHeap[Values.Count] = Values.LastID;
            Values.X[Values.LastID] = X;
            Values.Y[Values.LastID] = Y;
            Values.H[Values.LastID] = (ushort)GetH(X, Y, EndX, EndY);
            Values.Parent[Values.LastID] = Parent;

            if (X == Values.X[Parent] || Y == Values.Y[Parent])
                Values.G[Values.LastID] = (ushort)(10 + Values.G[Parent]);
            else
                Values.G[Values.LastID] = (ushort)(14 + Values.G[Parent]);
            Values.F[Values.LastID] = (ushort)(Values.G[Values.LastID] + Values.H[Values.LastID]);

            ushort Temp;
            for (ushort C = Values.Count; C != 1; C /= 2)
            {
                if (Values.F[Values.BinaryHeap[C]] <= Values.F[Values.BinaryHeap[C / 2]])
                {
                    Temp = Values.BinaryHeap[C / 2];
                    Values.BinaryHeap[C / 2] = Values.BinaryHeap[C];
                    Values.BinaryHeap[C] = Temp;
                }
                else
                    break;
            }
            Values.Tiles[X, Y] = 1;
        }
        private static void Move(IHIPathfinderValues Values)
        {
            Values.Tiles[Values.X[Values.BinaryHeap[1]], Values.Y[Values.BinaryHeap[1]]] = 2;



            Values.BinaryHeap[1] = Values.BinaryHeap[Values.Count];
            Values.Count--;

            ushort Location = 1;
            ushort High;
            while (true)
            {
                High = Location;
                if (2 * High + 1 <= Values.Count)
                {
                    if (Values.F[Values.BinaryHeap[High]] >= Values.F[Values.BinaryHeap[2 * High]]) Location = (ushort)(2 * High);
                    if (Values.F[Values.BinaryHeap[Location]] >= Values.F[Values.BinaryHeap[2 * High + 1]]) Location = (ushort)(2 * High + 1);
                }
                else if (2 * High <= Values.Count)
                {
                    if (Values.F[Values.BinaryHeap[High]] >= Values.F[Values.BinaryHeap[2 * High]]) Location = (ushort)(2 * High);
                }

                if (High != Location)
                {
                    ushort Temp = Values.BinaryHeap[High];
                    Values.BinaryHeap[High] = Values.BinaryHeap[Location];
                    Values.BinaryHeap[Location] = Temp;
                }
                else
                    break;
            }
        }
    }

    public class IHIPathfinderValues
    {
        public byte[,] Tiles;
        public float[,] Z;


        public byte[] X;
        public byte[] Y;
        public ushort[] H;
        public ushort[] G;
        public ushort[] F;

        public ushort Count;
        public ushort LastID;

        public ushort[] BinaryHeap;
        public ushort[] Parent;

        public ushort Location;

        public bool[,] UnitMap;

        public IHIPathfinderValues(byte[,] CollisionMap, float[,] Height, bool[,] UnitMap)
        {
            Tiles = new byte[CollisionMap.GetLength(0), CollisionMap.GetLength(1)];

            X = new byte[Tiles.Length];
            Y = new byte[X.Length];
            Z = Height;
            H = new ushort[X.Length];
            G = new ushort[X.Length];
            F = new ushort[X.Length];

            Count = 0;
            LastID = 0;

            BinaryHeap = new ushort[X.Length];
            Parent = new ushort[X.Length];

            Location = 0;

            this.UnitMap = UnitMap;
        }
    }
}