using HPJ.Simulation.Enums;
using HPJ.Simulation.Map;
using System;
using System.Collections.Generic;

namespace HPJ.Simulation.Pathing
{
    /// <summary>
    /// A Jump point search script used by Navigation Jobs to calculate a path based on the jump point search (JPS) Algorithm
    /// </summary>
    public class JumpPointSearch : PathingCalculation
    {
        /* MIT License Information
           
           "The Jump Point Search algorithm used in this product is based on the work of Daniel Harabor and Alban Grastien, 
            and is released under the MIT License. Copyright (c) [year] Daniel Harabor and Alban Grastien.
            Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 
            to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, subject to the following conditions:
            The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software."
        
        */

        public static JumpPointSearch Instance = null;

        public override void Initialize()
        {
            NavigationType = Enums.NavigationTypes.JumpPointSearch;
            if (!PathingTypes.ContainsKey(NavigationType))
            {
                PathingTypes.Add(NavigationType, this);
            }
        }

        public override void CalculatePath(NavigationPath Path, Map.Map OccuringMap, out string Status, out bool Succeeded)
        {
            JPS_VariablePackage MemoryPackage = JPS_VariablePackage.GetPackage(OccuringMap.AgentAvoidence);
            try
            {
                IntVector2 StartTile = Path.Info.PointA;
                IntVector2 EndTile = Path.Info.PointB;

                if (StartTile.x >= OccuringMap.Settings.MapWidth || StartTile.y >= OccuringMap.Settings.MapHeight || StartTile.x < 0 || StartTile.y < 0)
                {
                    Path.ValidPath = false;
                    Succeeded = false;
                    Status = $"Starting Index is out of Range ({StartTile.x}, {StartTile.y}) -Map Size ({OccuringMap.Settings.MapWidth}, {OccuringMap.Settings.MapHeight})";
                    throw new Exception($"Starting Index is out of Range ({ StartTile.x }, { StartTile.y}) -Map Size({ OccuringMap.Settings.MapWidth}, { OccuringMap.Settings.MapHeight})");
                    return;
                }

                if (EndTile.x >= OccuringMap.Settings.MapWidth || EndTile.y >= OccuringMap.Settings.MapHeight || EndTile.x < 0 || EndTile.y < 0)
                {
                    Path.ValidPath = false;
                    Succeeded = false;
                    Status = $"Ending Index is out of Range ({EndTile.x}, {EndTile.y}) -Map Size ({OccuringMap.Settings.MapWidth}, {OccuringMap.Settings.MapHeight})";
                    throw new Exception($"Ending Index is out of Range ({EndTile.x}, {EndTile.y}) -Map Size ({OccuringMap.Settings.MapWidth}, {OccuringMap.Settings.MapHeight})");
                    return;
                }

                JPSTile StartingTileData = new JPSTile(StartTile, CalculateDistanceCost(StartTile.x, StartTile.y, EndTile.x, EndTile.y));
                StartingTileData.GCost = 0;
                MemoryPackage.OpenList.Add(StartingTileData);
                MemoryPackage.SortedList.Add(StartTile, StartingTileData);

                JPSTile CurrentTile;

                // Get BitMap
                BytePointMap BitMap = OccuringMap.GetBytePointMap(Path);
                MemoryPackage.SetAgentID(Path.AgentID);

                // JumpData
                MemoryPackage.LoadJumpData(StartTile, EndTile);

                int MaxIterations = 3000;
                int MaxIterationCounter = 0;

                while (MemoryPackage.OpenList.Count > 0)
                {
                    MaxIterationCounter++;
                    if (MaxIterationCounter >= MaxIterations)
                    {
                        break;
                    }

                    int CurrentNodeIndex = GetLowestFCostTileIndex(MemoryPackage.OpenList, EndTile.x, EndTile.y);
                    CurrentTile = MemoryPackage.OpenList[CurrentNodeIndex];
                    MemoryPackage.OpenList.RemoveAt(CurrentNodeIndex);
                    if (!MemoryPackage.SortedList.ContainsKey(CurrentTile.Position))
                    {
                        MemoryPackage.SortedList.Add(CurrentTile.Position, CurrentTile);
                    }

                    if (EndTile == CurrentTile.Position)
                    {
                        // Reached the end
                        break;
                    }

                    // Jump Up
                    JPSTile CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    JPSTile NextJumpPoint = VerticalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.UpRightData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }

                    // Jump Right
                    CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    NextJumpPoint = HorizontalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.UpRightData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }

                    // Jump Left
                    CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    NextJumpPoint = HorizontalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.DownLeftData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }

                    // Jump Down
                    CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    NextJumpPoint = VerticalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.DownLeftData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }

                    // Jump Up Right
                    CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    NextJumpPoint = DiagonalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.UpRightData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }

                    // Jump Down Right
                    CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    NextJumpPoint = DiagonalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.DownRightData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }

                    // Jump Down Left
                    CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    NextJumpPoint = DiagonalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.DownLeftData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }

                    // Jump Up Left
                    CurrentClone = CurrentTile.Clone(MemoryPackage);
                    CurrentClone.JumpFromTile = CurrentTile;
                    NextJumpPoint = DiagonalJump(BitMap, OccuringMap.AgentOwnershipMap, CurrentClone, MemoryPackage.UpLeftData);
                    if (NextJumpPoint != null)
                    {
                        if (!MemoryPackage.SortedList.ContainsKey(NextJumpPoint.Position))
                        {
                            MemoryPackage.SortedList.Add(NextJumpPoint.Position, NextJumpPoint);
                            MemoryPackage.OpenList.Add(NextJumpPoint);
                        }
                        else
                        {
                            JPSTile AlreadyExistedVersion = MemoryPackage.SortedList[NextJumpPoint.Position];
                            if (NextJumpPoint.GCost < AlreadyExistedVersion.GCost)
                            {
                                AlreadyExistedVersion.GCost = NextJumpPoint.GCost;
                                AlreadyExistedVersion.JumpFromTile = CurrentTile;
                            }
                            MemoryPackage.ReturnTile(NextJumpPoint);
                        }
                    }
                    else
                    {
                        MemoryPackage.ReturnTile(CurrentClone);
                    }
                }

                JPSTile EndTileData;
                if (!MemoryPackage.SortedList.TryGetValue(EndTile, out EndTileData) || EndTileData.JumpFromTile == null)
                {
                    // No Valid Path Found
                    Succeeded = false;
                    Status = $"Open list emptied - But did NOT reach the End, Likely Trapped (Open:{MemoryPackage.OpenList.Count} / Closed:{MemoryPackage.SortedList.Count}) ({OccuringMap.GetTileType(StartTile)} [{StartTile.x}, {StartTile.y}] -> {OccuringMap.GetTileType(EndTile)} [{EndTile.x}, {EndTile.y}]) -> Map: ({OccuringMap.Name} [{OccuringMap.Settings.MapWidth}, {OccuringMap.Settings.MapHeight}])";
                }
                else
                {
                    Succeeded = true;
                    // Path should be valid
                    SortPath(MemoryPackage.SortedList, Path, EndTile);
                    Status = $"Found a Valid Path -> 'List Counts (Open:{MemoryPackage.OpenList.Count} / Closed:{MemoryPackage.SortedList.Count})'";
                }

                JPS_VariablePackage.ReturnPackage(MemoryPackage);
            }
            catch (Exception ex)
            {
                JPS_VariablePackage.ReturnPackage(MemoryPackage);
                Status = "Faled";
                OccuringMap.LogError($"JPS Error: {ex.Message}: {ex.Source}: {ex.StackTrace}");
                Succeeded = false;
            }
        }

        private JPSTile DiagonalJump(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            PlaceholderTile.Position.x += JumpData.JumpDirection.x;
            PlaceholderTile.Position.y += JumpData.JumpDirection.y;

            // Out of Bounds
            if (PlaceholderTile.Position.x < 0 || PlaceholderTile.Position.x >= BitMap.Width)
            {
                return null;
            }

            if (PlaceholderTile.Position.y < 0 || PlaceholderTile.Position.y >= BitMap.Length)
            {
                return null;
            }

            // Is an Obstacle
            if (IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
            {
                return null;
            }

            if (!BitMap.CornerCutting)
            {
                // DO BOT ALLOW FOR JUMPS TO GO THROUGH THE CORNERS
                if (IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    return null;
                }
                else if (IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    return null;
                }
            }

            // Is the Goal
            if (PlaceholderTile.Position == JumpData.EndIndex)
            {
                return PlaceholderTile;
            }

            PlaceholderTile.GCost += 14;
            if (IsDiagonalForced(BitMap, TileOwnership, PlaceholderTile, JumpData))
            {
                PlaceholderTile.HCost = CalculateDistanceCost(PlaceholderTile.Position.x, PlaceholderTile.Position.y, JumpData.EndIndex.x, JumpData.EndIndex.y);
                return PlaceholderTile;
            }

            // Diagonal
            PlaceholderTile.SplitPosition = PlaceholderTile.Position;
            HorizontalJump_FromDiagonal(BitMap, TileOwnership, PlaceholderTile, JumpData);

            if (!PlaceholderTile.HitAdjacentJump)
            {
                PlaceholderTile.SplitPosition = PlaceholderTile.Position;
                VerticalJump_FromDiagonal(BitMap, TileOwnership, PlaceholderTile, JumpData);
            }

            if (PlaceholderTile.HitAdjacentJump)
            {
                PlaceholderTile.HCost = CalculateDistanceCost(PlaceholderTile.Position.x, PlaceholderTile.Position.y, JumpData.EndIndex.x, JumpData.EndIndex.y);
                return PlaceholderTile;
            }

            return DiagonalJump(BitMap, TileOwnership, PlaceholderTile, JumpData);
        }

        private JPSTile HorizontalJump(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            PlaceholderTile.Position.x += JumpData.JumpDirection.x;

            // Out of Bounds
            if (PlaceholderTile.Position.x < 0 || PlaceholderTile.Position.x >= BitMap.Width)
            {
                return null;
            }

            // Is an Obstacle
            if (IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
            {
                return null;
            }

            // Is the Goal
            if (PlaceholderTile.Position == JumpData.EndIndex)
            {
                return PlaceholderTile;
            }

            PlaceholderTile.GCost += 10;
            if (IsHorizontalForced(BitMap, TileOwnership, PlaceholderTile, JumpData))
            {
                return PlaceholderTile;
            }

            return HorizontalJump(BitMap, TileOwnership, PlaceholderTile, JumpData);
        }

        private JPSTile VerticalJump(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            PlaceholderTile.Position.y += JumpData.JumpDirection.y;

            // Out of Bounds
            if (PlaceholderTile.Position.y < 0 || PlaceholderTile.Position.y >= BitMap.Length)
            {
                return null;
            }

            // Is an Obstacle
            if (IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
            {
                return null;
            }

            // Is the Goal
            if (PlaceholderTile.Position == JumpData.EndIndex)
            {
                return PlaceholderTile;
            }

            PlaceholderTile.GCost += 10;
            if (IsVerticalForced(BitMap, TileOwnership, PlaceholderTile, JumpData))
            {
                return PlaceholderTile;
            }

            return VerticalJump(BitMap, TileOwnership, PlaceholderTile, JumpData);
        }

        private void HorizontalJump_FromDiagonal(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            PlaceholderTile.SplitPosition.x += JumpData.JumpDirection.x;

            // Out of Bounds
            if (PlaceholderTile.SplitPosition.x < 0 || PlaceholderTile.SplitPosition.x >= BitMap.Width)
            {
                return;
            }

            // Is an Obstacle
            if (IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
            {
                return;
            }

            // Is the Goal
            if (PlaceholderTile.SplitPosition == JumpData.EndIndex)
            {
                PlaceholderTile.HitAdjacentJump = true;
                return;
            }

            if (IsHorizontalForced_Diagonal(BitMap, TileOwnership, PlaceholderTile, JumpData))
            {
                PlaceholderTile.HitAdjacentJump = true;
                return;
            }

            HorizontalJump_FromDiagonal(BitMap, TileOwnership, PlaceholderTile, JumpData);
        }

        private void VerticalJump_FromDiagonal(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            PlaceholderTile.SplitPosition.y += JumpData.JumpDirection.y;

            // Out of Bounds
            if (PlaceholderTile.SplitPosition.y < 0 || PlaceholderTile.SplitPosition.y >= BitMap.Length)
            {
                return;
            }

            // Is an Obstacle
            if (IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
            {
                return;
            }

            // Is the Goal
            if (PlaceholderTile.SplitPosition == JumpData.EndIndex)
            {
                PlaceholderTile.HitAdjacentJump = true;
                return;
            }

            if (IsVerticalForced_Diagonal(BitMap, TileOwnership, PlaceholderTile, JumpData))
            {
                PlaceholderTile.HitAdjacentJump = true;
                return;
            }

            VerticalJump_FromDiagonal(BitMap, TileOwnership, PlaceholderTile, JumpData);
        }

        protected bool IsDiagonalForced(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            if (BitMap.CornerCutting)
            {
                // Opposite x direction is there a block
                if (IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    // Is the next block in the opposite x direction free
                    if (!IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y + JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        return true;
                    }
                }
                // Opposite y direction is there a block
                if (IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    // Is the next block in the opposite y direction free
                    if (!IsBlocked(PlaceholderTile.Position.x + JumpData.JumpDirection.x, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        return true;
                    }
                }
            }
            else
            {
                // Horizontal
                // Othogonal is Clear
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }

                // Vertical
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x + 1, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    // Othogonal is Clear
                    if (!IsBlocked(PlaceholderTile.Position.x + 1, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x - 1, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.Position.x - 1, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }

            return false;
        }

        protected bool IsHorizontalForced(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            if (BitMap.CornerCutting)
            {
                // Horizontal
                // Forwards is clear
                if (!IsBlocked(PlaceholderTile.Position.x + JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    // Is there Orthogonal block
                    if (IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.Position.x + JumpData.JumpDirection.x, PlaceholderTile.Position.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                    if (IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.Position.x + JumpData.JumpDirection.x, PlaceholderTile.Position.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }
            else
            {
                // Horizontal
                // Othogonal is Clear
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x - JumpData.JumpDirection.x, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }

            return false;
        }

        protected bool IsVerticalForced(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            if (BitMap.CornerCutting)
            {
                // Vertical
                // Forwards is clear
                if (!IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y + JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    // Is there Orthogonal block
                    if (IsBlocked(PlaceholderTile.Position.x + 1, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.Position.x + 1, PlaceholderTile.Position.y + JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                    // Is there Orthogonal block
                    else if (IsBlocked(PlaceholderTile.Position.x - 1, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.Position.x - 1, PlaceholderTile.Position.y + JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }
            else
            {
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x + 1, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.Position.x + 1, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.Position.x - 1, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.Position.x - 1, PlaceholderTile.Position.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.Position.x, PlaceholderTile.Position.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }

            return false;
        }

        protected bool IsHorizontalForced_Diagonal(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            if (BitMap.CornerCutting)
            {
                // Horizontal
                // Forwards is clear
                if (!IsBlocked(PlaceholderTile.SplitPosition.x + JumpData.JumpDirection.x, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    // Is there Orthogonal block
                    if (!IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x + JumpData.JumpDirection.x, PlaceholderTile.SplitPosition.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                    if (!IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x + JumpData.JumpDirection.x, PlaceholderTile.SplitPosition.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }
            else
            {
                // Horizontal
                // Othogonal is Clear
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.SplitPosition.x - JumpData.JumpDirection.x, PlaceholderTile.SplitPosition.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y + 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x - JumpData.JumpDirection.x, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.SplitPosition.x - JumpData.JumpDirection.x, PlaceholderTile.SplitPosition.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y - 1, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x - JumpData.JumpDirection.x, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }

            return false;
        }

        protected bool IsVerticalForced_Diagonal(BytePointMap BitMap, AgentMap TileOwnership, JPSTile PlaceholderTile, CurrentJunpData JumpData)
        {
            if (BitMap.CornerCutting)
            {
                // Vertical
                // Forwards is clear
                if (!IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y + JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    // Is there Orthogonal block
                    if (IsBlocked(PlaceholderTile.SplitPosition.x + 1, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x + 1, PlaceholderTile.SplitPosition.y + JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                    // Is there Orthogonal block
                    if (IsBlocked(PlaceholderTile.SplitPosition.x - 1, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        // Is there Orthogonal free on next
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x - 1, PlaceholderTile.SplitPosition.y + JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }
            else
            {
                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.SplitPosition.x + 1, PlaceholderTile.SplitPosition.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.SplitPosition.x + 1, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }

                // Is there Orthogonal back blocked
                if (IsBlocked(PlaceholderTile.SplitPosition.x - 1, PlaceholderTile.SplitPosition.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                {
                    if (!IsBlocked(PlaceholderTile.SplitPosition.x - 1, PlaceholderTile.SplitPosition.y, BitMap, TileOwnership, JumpData.AgentID))
                    {
                        if (!IsBlocked(PlaceholderTile.SplitPosition.x, PlaceholderTile.SplitPosition.y - JumpData.JumpDirection.y, BitMap, TileOwnership, JumpData.AgentID))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }

        protected bool IsBlocked(int x, int y, BytePointMap ByteMap, AgentMap TileOwnership, ushort ID)
        {
            if (ByteMap.GetTileSpeed(x, y) == 0)
            {
                return true;
            }

            if (TileOwnership != null)
            {
                if (TileOwnership.IsBlocked(x, y, ID))
                {
                    return true;
                }
            }

            return false;
        }

        protected int GetLowestFCostTileIndex(List<JPSTile> OpenList, int TileEndX, int TileEndY)
        {
            int LowestCostIndex = 0;

            // Typical AStar pathfinding
            int CurrentLowestCost = CalculateDistanceCost(OpenList[0].Position.x, OpenList[0].Position.y, TileEndX, TileEndY) + OpenList[0].GCost;
            for (int i = 1; i < OpenList.Count; i++)
            {
                int testCost = CalculateDistanceCost(OpenList[i].Position.x, OpenList[i].Position.y, TileEndX, TileEndY) + OpenList[i].GCost;
                if (testCost < CurrentLowestCost)
                {
                    CurrentLowestCost = testCost;
                    LowestCostIndex = i;
                }
            }

            return LowestCostIndex;
        }

        protected void SortPath(Dictionary<IntVector2, JPSTile> SortedData, NavigationPath Path, IntVector2 EndTileIndex)
        {
            JPSTile CurrentTile = SortedData[EndTileIndex];
            int InsertIndex = Path.Path.Count;
            //Path.Path.Insert(InsertIndex, EndTileIndex);

            // The Route From Index of the starting tile should be the only one with a (-1, -1) index
            while (CurrentTile.JumpFromTile != null)
            {
                Path.Path.Insert(InsertIndex, CurrentTile.Position);
                CurrentTile = SortedData[CurrentTile.JumpFromTile.Position];
                InsertIndex = Path.Path.Count;
            }
            Path.Path.Insert(InsertIndex, CurrentTile.Position);
        }

        internal int CalculateDistanceCost(int x1, int y1, int x2, int y2)
        {
            int X_Distance = x1 > x2 ? x1 - x2 : x2 - x1;
            int Y_Distance = y1 > y2 ? y1 - y2 : y2 - y1;

            if (Y_Distance < X_Distance)
            {
                int remaining = X_Distance - Y_Distance;
                return 14 * Y_Distance + 10 * remaining;
            }
            else
            {
                int remaining = Y_Distance - X_Distance;
                return 14 * X_Distance + 10 * remaining;
            }
        }

        #region JPS Classes

        public class JPS_VariablePackage
        {
            public List<JPSTile> OpenList = new List<JPSTile>();
            public Dictionary<IntVector2, JPSTile> SortedList = new Dictionary<IntVector2, JPSTile>();

            public CurrentJunpData UpRightData;
            public CurrentJunpData UpLeftData;
            public CurrentJunpData DownRightData;
            public CurrentJunpData DownLeftData;

            public JPS_VariablePackage(bool AgentAvoidence)
            {
                UpRightData = new CurrentJunpData(new IntVector2(1, 1), AgentAvoidence);
                UpLeftData = new CurrentJunpData(new IntVector2(-1, 1), AgentAvoidence);
                DownRightData = new CurrentJunpData(new IntVector2(1, -1), AgentAvoidence);
                DownLeftData = new CurrentJunpData(new IntVector2(-1, -1), AgentAvoidence);
            }

            public void LoadJumpData(IntVector2 Start, IntVector2 End)
            {
                UpRightData.SetData(Start, End);
                UpLeftData.SetData(Start, End);
                DownRightData.SetData(Start, End);
                DownLeftData.SetData(Start, End);
            }

            public void Clear()
            {
                OpenList.Clear();
                SortedList.Clear();
            }

            #region Pools

            public static List<JPS_VariablePackage> Pool = new List<JPS_VariablePackage>();

            public static JPS_VariablePackage GetPackage(bool Avoidence)
            {
                lock (Pool)
                {
                    if (Pool.Count > 0)
                    {
                        JPS_VariablePackage Package = Pool[0];
                        Pool.RemoveAt(0);
                        return Package;
                    }
                    else
                    {
                        return new JPS_VariablePackage(Avoidence);
                    }
                }
            }

            public static void ReturnPackage(JPS_VariablePackage Package)
            {
                foreach (JPSTile Tile in Package.SortedList.Values)
                {
                    Package.ReturnTile(Tile);
                }

                Package.Clear();
                lock (Pool)
                {
                    Pool.Add(Package);
                }
            }

            public List<JPSTile> TilePool = new List<JPSTile>();

            public JPSTile GetTile(IntVector2 position, int EndCost)
            {
                if (TilePool.Count > 0)
                {
                    JPSTile Package = TilePool[0];
                    TilePool.RemoveAt(0);
                    Package.Clear();
                    Package.Reset(position, EndCost);
                    return Package;
                }
                else
                {
                    return new JPSTile(position, EndCost);
                }
            }

            public void ReturnTile(JPSTile Package)
            {
                TilePool.Add(Package);
            }

            internal void SetAgentID(ushort agentID)
            {
                UpRightData.AgentID = agentID;
                UpLeftData.AgentID = agentID;
                DownLeftData.AgentID = agentID;
                DownRightData.AgentID = agentID;
            }

            #endregion
        }

        [Serializable]
        public class JPSTile
        {
            public IntVector2 Position;
            public IntVector2 SplitPosition;
            public JPSTile JumpFromTile;
            public int HCost;
            public int GCost;
            public int SplitGCost;
            public bool HitAdjacentJump;

            public JPSTile(IntVector2 position, int EndCost)
            {
                SplitPosition = new IntVector2();
                Position = position;
                JumpFromTile = null;
                HCost = EndCost;
                GCost = 0;
                SplitGCost = 0;
                HitAdjacentJump = false;
            }

            public void Clear()
            {
                JumpFromTile = null;
                GCost = 0;
                SplitGCost = 0;
                HitAdjacentJump = false;
            }

            public void Reset(IntVector2 position, int EndCost)
            {
                Position = position;
                HCost = EndCost;
            }

            public JPSTile Clone()
            {
                JPSTile NewTile = new JPSTile(Position, HCost);
                NewTile.GCost = GCost;

                return NewTile;
            }

            public JPSTile Clone(JPS_VariablePackage Package)
            {
                JPSTile NewTile = Package.GetTile(Position, HCost);
                NewTile.GCost = GCost;

                return NewTile;
            }
        }

        [Serializable]
        public class CurrentJunpData
        {
            public IntVector2 JumpDirection;
            public IntVector2 StartIndex;
            public IntVector2 EndIndex;
            public bool AgentAvoidence;

            public ushort AgentID { get; internal set; }

            public CurrentJunpData(IntVector2 Direciton, IntVector2 Start, IntVector2 End, bool Avoidence)
            {
                JumpDirection = Direciton;
                StartIndex = Start;
                EndIndex = End;
                AgentAvoidence = Avoidence;
            }

            public CurrentJunpData(IntVector2 Direciton, bool Avoidence)
            {
                JumpDirection = Direciton;
                AgentAvoidence = Avoidence;
            }

            public void SetData(IntVector2 Start, IntVector2 End)
            {
                StartIndex = Start;
                EndIndex = End;
            }
        }

        #endregion
    }
}