using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using HPJ.Simulation.Map;
using System.Threading;
using System.Threading.Tasks;
using HPJ.Simulation.Pathing;
using HPJ.Simulation.Enums;
using UnityEditor;
using System;
using HPJ.Simulation;
using System.IO;
//using Unity.Plastic.Newtonsoft.Json;
using UnityEngine.SceneManagement;
using HPJ.Presentation.Obstacle;
using HPJ.Presentation.Agents;
using HPJ.Presentation.NavigationManagerSettings;
using Newtonsoft.Json;
#if UNITY_EDITOR
using Unity.EditorCoroutines.Editor;
#endif

namespace HPJ.Presentation
{
    /// <summary>
    /// The Manager used to manage the Map sets, Obstacles as Gameobjects, Navigation Agents as Gameobjects
    /// </summary>
    public class NavigationManager : MonoBehaviour
    {
        #region Maps

        [SerializeField]
        protected List<MapSet> _mapSets = new List<MapSet>();
        /// <summary>
        /// A List of the maps
        /// </summary>
        public List<MapSet> MapSets { get { if (ApplicationPlaying) { return _activeSets; } return _mapSets; } }
        [NonSerialized]
        protected List<MapSet> _activeSets;
        protected bool ApplicationPlaying { get; set; }
        /// <summary>
        /// A List of all the bridges
        /// </summary>
        [SerializeField]
        protected List<MapBridge> _bridges = new List<MapBridge>();
        public List<MapBridge> Bridges { get { return _bridges; } }

        /// <summary>
        /// A List of all the bridges
        /// </summary>
        [SerializeField]
        protected List<MapSeam> _seams = new List<MapSeam>();
        public List<MapSeam> Seams { get { return _seams; } }

        /// <summary>
        /// The Navigation Manager Settings
        /// </summary>
        public ManagerSetting ManagerSettings = new ManagerSetting();

        /// <summary>
        /// Seam settings for the Navigation Manager
        /// </summary>
        public SeamSettings MapSeamSettings = new SeamSettings();

        /// <summary>
        /// Used to send a Navigation Job Request to the simulation
        /// </summary>
        /// <param name="Job"></param>
        public void SetDestination(NavigationJob Job)
        {
            for (int i = 0; i < MapSets.Count; i++)
            {
                MapSet Set = MapSets[i];
                if (Set.JobCompatible(Job))
                {
                    Set.SetDestination(Job);
                    return;
                }
            }

            if (Job.CurrentPath != null)
            {
                Job.CurrentPath.PreviousStatus = "Was not in bounds of a Map";
                Job.CurrentPath.ValidPath = false;
                Job.CurrentStep = PathfindingCalculationStep.Complete;
            }
        }

        /// <summary>
        /// Adds a navigation order for the Navigation Agents
        /// </summary>
        /// <param name="newOrder"></param>
        internal void AddNavigationOrder(NavigationOrder newOrder)
        {
            newOrder.StartingMap.SetDestination(newOrder.NavJob);
        }

        /// <summary>
        /// Initilaizes all the map bridges
        /// </summary>
        public void SetMapBridges()
        {
            for (int i = 0; i < MapSets.Count; i++)
            {
                MapSet Map = MapSets[i];
                Map.ClearMaps();
                foreach (MapBridge Bridge in Bridges)
                {
                    if (Bridge.Contains(Map))
                    {
                        Bridge.RefreshWithNew(Map);
                        Map.AddBridge(Bridge);
                    }
                }
                foreach (MapSeam Seam in Seams)
                {
                    if (Seam.Contains(Map))
                    {
                        Seam.RefreshWithNew(Map);
                        Map.AddSeam(Seam);
                    }
                }
            }
        }

        /// <summary>
        /// Gets a desired bridge or seam for a position
        /// </summary>
        /// <param name="start"></param>
        /// <param name="startingMap"></param>
        /// <param name="endingMap"></param>
        /// <param name="ClosestBridge"></param>
        /// <param name="ClosestSeam"></param>
        /// <returns></returns>
        public bool GetDesiredBridgeOrSeam(IntVector3 start, MapSet startingMap, MapSet endingMap, out MapBridge ClosestBridge, out MapSeam ClosestSeam, out int BridgeScore, out int SeamScore)
        {
            ClosestBridge = null;
            ClosestSeam = null;
            BridgeScore = int.MaxValue;
            SeamScore = int.MaxValue;

            // Make sure that these maps HAVE a way out of them
            if ((endingMap.MapBridges.Count == 0 && endingMap.MapSeams.Count == 0) || (startingMap.MapBridges.Count == 0 && startingMap.MapSeams.Count == 0))
            {
                Debug.Log($"No Seam or Bridge for Starting or Ending Map {startingMap.SetSettings.Name}-> S#{startingMap.MapSeams.Count} - B#{startingMap.MapBridges.Count} / {endingMap.SetSettings.Name}-> S#{endingMap.MapSeams.Count} - B#{endingMap.MapBridges.Count}");
                return false;
            }

            int ClosestScore = int.MaxValue;
            // Check for direct bridges first
            for (int i = 0; i < Bridges.Count; i++)
            {
                MapBridge Bridge = Bridges[i];
                if (!Bridge.ValidBridge)
                {
                    continue;
                }

                if (Bridge.CanWalkInDirection(startingMap, endingMap))
                {
                    IntVector3 BridgePoint = Bridge.GetPoint(startingMap);
                    int Distance = IntVector3.DistanceSquared(BridgePoint, start);
                    if (Distance < ClosestScore)
                    {
                        ClosestBridge = Bridge;
                        ClosestScore = Distance;
                    }
                }
            }

            ClosestScore = int.MaxValue;
            // Check for direct bridges first
            for (int i = 0; i < Seams.Count; i++)
            {
                MapSeam Seam = Seams[i];
                if (!Seam.ValidSeam)
                {
                    continue;
                }

                if (Seam.DirectConnects(startingMap, endingMap))
                {
                    IntVector3 SeamPoint = Seam.GetPoint(startingMap, start);
                    int Distance = IntVector3.DistanceSquared(SeamPoint, start);
                    if (Distance < ClosestScore)
                    {
                        ClosestSeam = Seam;
                        ClosestScore = Distance;
                    }
                }
            }

            // Ok adjacent map does not directly lead to the end map, so lets do node based pathfinding to try and get there
            if (ClosestBridge == null && ClosestSeam == null)
            {
                ClosestBridge = AStarBridgeNode(startingMap, endingMap, start);

                ClosestSeam = AStarSeamNode(startingMap, endingMap, start);
            }

            if (ClosestBridge != null)
            {
                IntVector3 BridgePoint = ClosestBridge.GetPoint(startingMap);
                BridgeScore = IntVector3.DistanceSquared(BridgePoint, start);
            }

            if (ClosestSeam != null)
            {
                IntVector3 SeamPoint = ClosestSeam.GetPoint(startingMap, start);
                SeamScore = IntVector3.DistanceSquared(SeamPoint, start);
            }

            return ClosestBridge != null || ClosestSeam != null;
        }

        /// <summary>
        /// Gets a desired bridge for a position
        /// </summary>
        /// <param name="start"></param>
        /// <param name="startingMap"></param>
        /// <param name="endingMap"></param>
        /// <returns></returns>
        public MapBridge GetDesiredBridge(IntVector3 start, MapSet startingMap, MapSet endingMap)
        {
            if (endingMap.MapBridges.Count == 0 || startingMap.MapBridges.Count == 0)
            {
                return null;
            }

            MapBridge ClosestBridge = null;
            int ClosestScore = int.MaxValue;
            // Check for direct bridges first
            foreach (MapBridge Bridge in Bridges)
            {
                if (!Bridge.ValidBridge)
                {
                    continue;
                }

                if (Bridge.CanWalkInDirection(startingMap, endingMap))
                {
                    IntVector3 BridgePoint = Bridge.GetPoint(startingMap);
                    int Distance = IntVector3.DistanceSquared(BridgePoint, start);
                    if (Distance < ClosestScore)
                    {
                        ClosestBridge = Bridge;
                        ClosestScore = Distance;
                    }
                }
            }

            // Ok adjacent map does not directly lead to the end map, so lets do node based pathfinding to try and get there
            if (ClosestBridge == null)
            {
                ClosestBridge = AStarBridgeNode(startingMap, endingMap, start);
                //ClosestScore = int.MaxValue;
                //List<MapSet> ClosedMaps = new List<MapSet>();
                //foreach (MapBridge Bridge in startingMap.MapBridges)
                //{
                //    if (!Bridge.ValidBridge)
                //    {
                //        continue;
                //    }

                //    IntVector3 BridgePoint = Bridge.GetPoint(startingMap);
                //    int Distance = IntVector3.DistanceSquared(BridgePoint, start);

                //    ClosedMaps.Add(startingMap);
                //    if (BridgeEventuallyLeadsToMap(Bridge, startingMap, endingMap, ClosedMaps))
                //    {
                //        if (Distance < ClosestScore)
                //        {
                //            ClosestBridge = Bridge;
                //        }
                //    }
                //    ClosedMaps.Clear();
                //}
            }

            return ClosestBridge;
        }

        /// <summary>
        /// Gets a desired seam for a position
        /// </summary>
        /// <param name="start"></param>
        /// <param name="startingMap"></param>
        /// <param name="endingMap"></param>
        /// <returns></returns>
        public MapSeam GetDesiredSeam(IntVector3 start, MapSet startingMap, MapSet endingMap)
        {
            if (endingMap.MapBridges.Count == 0 || startingMap.MapBridges.Count == 0)
            {
                return null;
            }

            MapSeam ClosestSeam = null;
            int ClosestScore = int.MaxValue;
            // Check for direct bridges first
            foreach (MapSeam Seam in Seams)
            {
                if (!Seam.ValidSeam)
                {
                    continue;
                }

                if (Seam.DirectConnects(startingMap, endingMap))
                {
                    IntVector3 BridgePoint = Seam.GetPoint(startingMap, start);
                    int Distance = IntVector3.DistanceSquared(BridgePoint, start);
                    if (Distance < ClosestScore)
                    {
                        ClosestSeam = Seam;
                        ClosestScore = Distance;
                    }
                }
            }

            // Ok adjacent map does not directly lead to the end map, so lets do node based pathfinding to try and get there
            if (ClosestSeam == null)
            {
                ClosestSeam = AStarSeamNode(startingMap, endingMap, start);
                //ClosestScore = int.MaxValue;
                //List<MapSet> ClosedMaps = new List<MapSet>();
                //foreach (MapBridge Bridge in startingMap.MapBridges)
                //{
                //    if (!Bridge.ValidBridge)
                //    {
                //        continue;
                //    }

                //    IntVector3 BridgePoint = Bridge.GetPoint(startingMap);
                //    int Distance = IntVector3.DistanceSquared(BridgePoint, start);

                //    ClosedMaps.Add(startingMap);
                //    if (BridgeEventuallyLeadsToMap(Bridge, startingMap, endingMap, ClosedMaps))
                //    {
                //        if (Distance < ClosestScore)
                //        {
                //            ClosestBridge = Bridge;
                //        }
                //    }
                //    ClosedMaps.Clear();
                //}
            }

            return ClosestSeam;
        }


        #region Bridge AStar Region

        #region Map Bridges
        /// <summary>
        /// Used A* Node Pathfinding to find a set of bridge to a ending map
        /// </summary>
        /// <param name="StartMap"></param>
        /// <param name="EndMap"></param>
        /// <param name="Start"></param>
        /// <returns></returns>
        public MapBridge AStarBridgeNode(MapSet StartMap, MapSet EndMap, IntVector3 Start)
        {
            List<int> OpenList = new List<int>();
            List<int> ClosedList = new List<int>();
            Dictionary<int, AStarBridgeData> SortedList = new Dictionary<int, AStarBridgeData>();

            OpenList.Add(StartMap.InstanceID);
            AStarBridgeData StartingTileData = new AStarBridgeData(-1, StartMap.InstanceID, GetMapDistance(StartMap, EndMap));
            StartingTileData.GCost = 0;
            StartingTileData.FCost = StartingTileData.GCost + StartingTileData.HCost;
            SortedList.Add(StartMap.InstanceID, StartingTileData);

            MapSet CurrentTile = StartMap;
            int Iterations = 0;

            bool MaxIterationsMade = false;
            int MaxIterations = 3000;

            while (OpenList.Count > 0)
            {
                // Give a safty range of 2 times the range in terms of iterations, if it cannot find a path by then, it's failed. can probably cannot be reached
                if (Iterations > MaxIterations)
                {
                    MaxIterationsMade = true;
                    break;
                }
                Iterations++;

                int CurrentNodeIndex = GetLowestFCostMapInstanceID(OpenList, EndMap);
                // Remove current
                CurrentTile = MapSets[OpenList[CurrentNodeIndex]];
                OpenList.RemoveAt(CurrentNodeIndex);
                AStarBridgeData CurrentData = SortedList[CurrentTile.InstanceID];
                CurrentTile = MapSets[CurrentData.ID];
                ClosedList.Add(CurrentTile.InstanceID);

                if (EndMap.InstanceID == CurrentTile.InstanceID)
                {
                    // Reached the end
                    //Debug.Log("Found the End - Should be Path Complete");
                    break;
                }

                for (int j= 0; j < CurrentTile.MapBridges.Count; j++)
                {
                    MapBridge Bridge = CurrentTile.MapBridges[j];
                    MapSet OtherMap = Bridge.Other(CurrentTile);
                    //Debug.Log($"Other Map {OtherMap.SetSettings.Name} ({OtherMap.InstanceID}) -> Bridges {OtherMap.MapBridges.Count}");

                    //if (OtherMap.InstanceID == EndMap.InstanceID)
                    //{
                    //    Debug.Log("Found Last Map");
                    //}

                    if (OtherMap == null)
                    {
                        continue;
                    }

                    if (!Bridge.CanWalkInDirection(CurrentTile, OtherMap))
                    {
                        continue;
                    }

                    AStarBridgeData NeighbourData;
                    if (!SortedList.ContainsKey(OtherMap.InstanceID))
                    {
                        NeighbourData = new AStarBridgeData(CurrentData.ID, OtherMap.InstanceID, GetMapDistance(OtherMap, EndMap));
                        SortedList.Add(OtherMap.InstanceID, NeighbourData);
                        if (!ClosedList.Contains(OtherMap.InstanceID))
                        {
                            //Debug.Log($"Add Map {OtherMap.SetSettings.Name} for ({StartMap.SetSettings.Name} -> {EndMap.SetSettings.Name})");
                            OpenList.Add(OtherMap.InstanceID);
                        }
                    }
                    else
                    {
                        NeighbourData = SortedList[OtherMap.InstanceID];
                    }

                    int TentativeGCost = CurrentData.GCost + GetMapDistance(CurrentTile, OtherMap);
                    if (TentativeGCost < NeighbourData.GCost)
                    {
                        NeighbourData.FromMapInstanceID = CurrentData.ID;
                        NeighbourData.GCost = TentativeGCost;
                        NeighbourData.FCost = NeighbourData.GCost + NeighbourData.HCost;
                        SortedList[OtherMap.InstanceID] = NeighbourData;
                    }
                }

                for (int j = 0; j < CurrentTile.MapSeams.Count; j++)
                {
                    MapSeam Seam = CurrentTile.MapSeams[j];
                    MapSet OtherMap = Seam.Other(CurrentTile);
                    //Debug.Log($"Other Map {OtherMap.SetSettings.Name} ({OtherMap.InstanceID}) -> Bridges {OtherMap.MapBridges.Count}");

                    //if (OtherMap.InstanceID == EndMap.InstanceID)
                    //{
                    //    Debug.Log("Found Last Map");
                    //}

                    if (OtherMap == null)
                    {
                        continue;
                    }

                    AStarBridgeData NeighbourData;
                    if (!SortedList.ContainsKey(OtherMap.InstanceID))
                    {
                        NeighbourData = new AStarBridgeData(CurrentData.ID, OtherMap.InstanceID, GetMapDistance(OtherMap, EndMap));
                        SortedList.Add(OtherMap.InstanceID, NeighbourData);
                        if (!ClosedList.Contains(OtherMap.InstanceID))
                        {
                            //Debug.Log($"Add Map {OtherMap.SetSettings.Name} for ({StartMap.SetSettings.Name} -> {EndMap.SetSettings.Name})");
                            OpenList.Add(OtherMap.InstanceID);
                        }
                    }
                    else
                    {
                        NeighbourData = SortedList[OtherMap.InstanceID];
                    }

                    int TentativeGCost = CurrentData.GCost + GetMapDistance(CurrentTile, OtherMap);
                    if (TentativeGCost < NeighbourData.GCost)
                    {
                        NeighbourData.FromMapInstanceID = CurrentData.ID;
                        NeighbourData.GCost = TentativeGCost;
                        NeighbourData.FCost = NeighbourData.GCost + NeighbourData.HCost;
                        SortedList[OtherMap.InstanceID] = NeighbourData;
                    }
                }
            }

            AStarBridgeData EndTileData;
            if (MaxIterationsMade)
            {
                Debug.Log("Max Iterations Made");
                return null;
            }
            else if (!SortedList.TryGetValue(EndMap.InstanceID, out EndTileData))
            {
                // No Valid Path Found
                Debug.Log($"No Valid Path Found in sorted list ({ClosedList.Count} / {OpenList.Count} / {SortedList.Count})");
                return null;
            }
            else if (EndTileData.FromMapInstanceID == -1)
            {
                // No Valid Path Found
                Debug.Log("End does not have a from ID");
                return null;
            }
            else
            {
                int NextMapIndex = GetNextMap(SortedList, EndMap, StartMap);
                if (NextMapIndex != -1)
                {
                    MapSet NextMap = MapSets[NextMapIndex];

                    MapBridge ClosestMap = StartMap.GetClosestBridge(NextMap, Start);
                    return ClosestMap;
                }
                Debug.Log("This should not hit");
            }

            return null;
        }

        private int GetNextMap(Dictionary<int, AStarBridgeData> sortedList, MapSet EndMap, MapSet StartMap)
        {
            AStarBridgeData CurrentTile = sortedList[EndMap.InstanceID];

            // The Route From Index of the starting tile should be the only one with a (-1, -1) index
            while (CurrentTile.FromMapInstanceID != -1)
            {
                if (CurrentTile.FromMapInstanceID == StartMap.InstanceID)
                {
                    return CurrentTile.ID;
                }
                CurrentTile = sortedList[CurrentTile.FromMapInstanceID];
            }

            return -1;
        }

        private int GetLowestFCostMapInstanceID(List<int> openList, MapSet endMap)
        {
            int LowestCostIndex = 0;

            // Typical AStar pathfinding
            int CurrentLowestCost = GetMapDistance(MapSets[openList[0]], endMap);
            for (int i = 1; i < openList.Count; i++)
            {
                int testCost = GetMapDistance(MapSets[openList[i]], endMap);
                if (testCost < CurrentLowestCost)
                {
                    CurrentLowestCost = testCost;
                    LowestCostIndex = i;
                }
            }

            return LowestCostIndex;
        }

        private int GetMapDistance(MapSet Map1, MapSet Map2)
        {
            if (Map1.InstanceID == Map2.InstanceID)
            {
                return 0;
            }

            int LowestDistance = int.MaxValue;

            for (int i = 0; i < Map1.KeyBorderPositions.Count; i++)
            {
                IntVector3 KeyPosition = Map1.KeyBorderPositions[i];
                for (int j = 0; j < Map2.KeyBorderPositions.Count; j++)
                {
                    IntVector3 OtherMapKeyPosition = Map2.KeyBorderPositions[j];
                    int NewScore = IntVector3.Displacement(KeyPosition, OtherMapKeyPosition);
                    if (NewScore < LowestDistance)
                    {
                        LowestDistance = NewScore;
                    }
                }
            }

            return LowestDistance;
        }

        private struct AStarBridgeData
        {
            public int ID;
            public int FromMapInstanceID;
            public int FCost;
            public int GCost;
            public int HCost;

            public AStarBridgeData(int MapInstanceID, int ThisID, int EndCost)
            {
                ID = ThisID;
                FromMapInstanceID = MapInstanceID;
                FCost = int.MaxValue;
                GCost = int.MaxValue;
                HCost = EndCost;
            }
        }
        public bool BridgeEventuallyLeadsToMap(MapBridge Bridge, MapSet CurrentMap, MapSet EndingMap, List<MapSet> ClosedList)
        {
            MapSet OtherMap = Bridge.Other(CurrentMap);
            if (OtherMap == null)
            {
                return false;
            }

            if (OtherMap == EndingMap)
            {
                return true;
            }

            if (ClosedList.Contains(OtherMap))
            {
                return false;
            }

            ClosedList.Add(OtherMap);
            for (int i = 0; i < OtherMap.MapBridges.Count; i++)
            {
                MapBridge OtherBridge = OtherMap.MapBridges[i];
                MapSet NextOtherMap = OtherBridge.Other(OtherMap);
                if (NextOtherMap == EndingMap)
                {
                    return true;
                }

                if (NextOtherMap == null)
                {
                    continue;
                }

                if (ClosedList.Contains(NextOtherMap))
                {
                    continue;
                }

                if (BridgeEventuallyLeadsToMap(OtherBridge, OtherMap, EndingMap, ClosedList))
                {
                    return true;
                }
            }

            for (int i = 0; i < OtherMap.MapSeams.Count; i++)
            {
                MapSeam OtherSeam = OtherMap.MapSeams[i];
                MapSet NextOtherMap = OtherSeam.Other(OtherMap);
                if (NextOtherMap == EndingMap)
                {
                    return true;
                }

                if (NextOtherMap == null)
                {
                    continue;
                }

                if (ClosedList.Contains(NextOtherMap))
                {
                    continue;
                }

                if (SeamEventuallyLeadsToMap(OtherSeam, OtherMap, EndingMap, ClosedList))
                {
                    return true;
                }
            }

            return false;
        }

        #endregion

        #region Map Seams
        /// <summary>
        /// Used A* Node Pathfinding to find a set of seams to a ending map
        /// </summary>
        /// <param name="StartMap"></param>
        /// <param name="EndMap"></param>
        /// <param name="Start"></param>
        /// <returns></returns>
        public MapSeam AStarSeamNode(MapSet StartMap, MapSet EndMap, IntVector3 Start)
        {
            List<int> OpenList = new List<int>();
            List<int> ClosedList = new List<int>();
            Dictionary<int, AStarBridgeData> SortedList = new Dictionary<int, AStarBridgeData>();

            OpenList.Add(StartMap.InstanceID);
            AStarBridgeData StartingTileData = new AStarBridgeData(-1, StartMap.InstanceID, GetMapDistance(StartMap, EndMap));
            StartingTileData.GCost = 0;
            StartingTileData.FCost = StartingTileData.GCost + StartingTileData.HCost;
            SortedList.Add(StartMap.InstanceID, StartingTileData);

            MapSet CurrentTile = StartMap;
            int Iterations = 0;

            bool MaxIterationsMade = false;
            int MaxIterations = 3000;

            while (OpenList.Count > 0)
            {
                // Give a safty range of 2 times the range in terms of iterations, if it cannot find a path by then, it's failed. can probably cannot be reached
                if (Iterations > MaxIterations)
                {
                    MaxIterationsMade = true;
                    break;
                }
                Iterations++;

                int CurrentNodeIndex = GetLowestFCostMapInstanceID(OpenList, EndMap);
                // Remove current
                CurrentTile = MapSets[OpenList[CurrentNodeIndex]];
                OpenList.RemoveAt(CurrentNodeIndex);
                AStarBridgeData CurrentData = SortedList[CurrentTile.InstanceID];
                CurrentTile = MapSets[CurrentData.ID];
                ClosedList.Add(CurrentTile.InstanceID);

                if (EndMap.InstanceID == CurrentTile.InstanceID)
                {
                    // Reached the end
                    //Debug.Log("Found the End - Should be Path Complete");
                    break;
                }

                foreach (MapBridge Bridge in CurrentTile.MapBridges)
                {
                    MapSet OtherMap = Bridge.Other(CurrentTile);
                    //Debug.Log($"Other Map {OtherMap.SetSettings.Name} ({OtherMap.InstanceID}) -> Bridges {OtherMap.MapBridges.Count}");

                    //if (OtherMap.InstanceID == EndMap.InstanceID)
                    //{
                    //    Debug.Log("Found Last Map");
                    //}

                    if (OtherMap == null)
                    {
                        continue;
                    }

                    AStarBridgeData NeighbourData;
                    if (!SortedList.ContainsKey(OtherMap.InstanceID))
                    {
                        NeighbourData = new AStarBridgeData(CurrentData.ID, OtherMap.InstanceID, GetMapDistance(OtherMap, EndMap));
                        SortedList.Add(OtherMap.InstanceID, NeighbourData);
                        if (!ClosedList.Contains(OtherMap.InstanceID))
                        {
                            //Debug.Log($"Add Map {OtherMap.SetSettings.Name} for ({StartMap.SetSettings.Name} -> {EndMap.SetSettings.Name})");
                            OpenList.Add(OtherMap.InstanceID);
                        }
                    }
                    else
                    {
                        NeighbourData = SortedList[OtherMap.InstanceID];
                    }

                    int TentativeGCost = CurrentData.GCost + GetMapDistance(CurrentTile, OtherMap);
                    if (TentativeGCost < NeighbourData.GCost)
                    {
                        NeighbourData.FromMapInstanceID = CurrentData.ID;
                        NeighbourData.GCost = TentativeGCost;
                        NeighbourData.FCost = NeighbourData.GCost + NeighbourData.HCost;
                        SortedList[OtherMap.InstanceID] = NeighbourData;
                    }
                }

                foreach (MapSeam Seam in CurrentTile.MapSeams)
                {
                    MapSet OtherMap = Seam.Other(CurrentTile);
                    //Debug.Log($"Other Map {OtherMap.SetSettings.Name} ({OtherMap.InstanceID}) -> Bridges {OtherMap.MapBridges.Count}");

                    //if (OtherMap.InstanceID == EndMap.InstanceID)
                    //{
                    //    Debug.Log("Found Last Map");
                    //}

                    if (OtherMap == null)
                    {
                        continue;
                    }

                    AStarBridgeData NeighbourData;
                    if (!SortedList.ContainsKey(OtherMap.InstanceID))
                    {
                        NeighbourData = new AStarBridgeData(CurrentData.ID, OtherMap.InstanceID, GetMapDistance(OtherMap, EndMap));
                        SortedList.Add(OtherMap.InstanceID, NeighbourData);
                        if (!ClosedList.Contains(OtherMap.InstanceID))
                        {
                            //Debug.Log($"Add Map {OtherMap.SetSettings.Name} for ({StartMap.SetSettings.Name} -> {EndMap.SetSettings.Name})");
                            OpenList.Add(OtherMap.InstanceID);
                        }
                    }
                    else
                    {
                        NeighbourData = SortedList[OtherMap.InstanceID];
                    }

                    int TentativeGCost = CurrentData.GCost + GetMapDistance(CurrentTile, OtherMap);
                    if (TentativeGCost < NeighbourData.GCost)
                    {
                        NeighbourData.FromMapInstanceID = CurrentData.ID;
                        NeighbourData.GCost = TentativeGCost;
                        NeighbourData.FCost = NeighbourData.GCost + NeighbourData.HCost;
                        SortedList[OtherMap.InstanceID] = NeighbourData;
                    }
                }
            }

            AStarBridgeData EndTileData;
            if (MaxIterationsMade)
            {
                Debug.Log("Max Iterations Made");
                return null;
            }
            else if (!SortedList.TryGetValue(EndMap.InstanceID, out EndTileData))
            {
                // No Valid Path Found
                Debug.Log($"No Valid Path Found in sorted list ({ClosedList.Count} / {OpenList.Count} / {SortedList.Count})");
                return null;
            }
            else if (EndTileData.FromMapInstanceID == -1)
            {
                // No Valid Path Found
                Debug.Log("End does not have a from ID");
                return null;
            }
            else
            {
                int NextMapIndex = GetNextMap(SortedList, EndMap, StartMap);
                if (NextMapIndex != -1)
                {
                    MapSet NextMap = MapSets[NextMapIndex];

                    MapSeam ClosestMap = StartMap.GetClosestSeam(NextMap, Start);
                    return ClosestMap;
                }
                Debug.Log("This should not hit");
            }

            return null;
        }

        public bool SeamEventuallyLeadsToMap(MapSeam Seam, MapSet CurrentMap, MapSet EndingMap, List<MapSet> ClosedList)
        {
            MapSet OtherMap = Seam.Other(CurrentMap);
            if (OtherMap == null)
            {
                return false;
            }

            if (OtherMap == EndingMap)
            {
                return true;
            }

            if (ClosedList.Contains(OtherMap))
            {
                return false;
            }

            ClosedList.Add(OtherMap);
            foreach (MapSeam OtherSeam in OtherMap.MapSeams)
            {
                MapSet NextOtherMap = OtherSeam.Other(OtherMap);
                if (NextOtherMap == EndingMap)
                {
                    return true;
                }

                if (NextOtherMap == null)
                {
                    continue;
                }

                if (ClosedList.Contains(NextOtherMap))
                {
                    continue;
                }

                if (SeamEventuallyLeadsToMap(OtherSeam, OtherMap, EndingMap, ClosedList))
                {
                    return true;
                }
            }

            foreach (MapBridge OtherBridge in OtherMap.MapBridges)
            {
                MapSet NextOtherMap = OtherBridge.Other(OtherMap);
                if (NextOtherMap == EndingMap)
                {
                    return true;
                }

                if (NextOtherMap == null)
                {
                    continue;
                }

                if (ClosedList.Contains(NextOtherMap))
                {
                    continue;
                }

                if (BridgeEventuallyLeadsToMap(OtherBridge, OtherMap, EndingMap, ClosedList))
                {
                    return true;
                }
            }

            return false;
        }

        #endregion

        #endregion


        /// <summary>
        /// Checks whether a specific bridge is valid
        /// </summary>
        /// <param name="Bridge"></param>
        /// <returns></returns>
        public bool ValidBridge(MapBridge Bridge)
        {
            if (Bridge == null || Bridge.MidPoints == null)
            {
                return false;
            }

            if (Bridge.MidPoints.Count <= 0)
            {
                return false;
            }

            Bridge.MapB = GetMapAtTileWorldPosition(Bridge.MidPoints[Bridge.MidPoints.Count - 1]);
            Bridge.MapA = GetMapAtTileWorldPosition(Bridge.MidPoints[0]);

            if (Bridge.MapB == Bridge.MapA)
            {
                return false;
            }

            if (Bridge.MapB == null || Bridge.MapA == null)
            {
                return false;
            }

            return true;
        }

        private void ValidateBridge(MapBridge bridge)
        {
            bridge.MapB = GetMapAtTileWorldPosition(bridge.MidPoints[bridge.MidPoints.Count - 1]);
            bridge.MapA = GetMapAtTileWorldPosition(bridge.MidPoints[0]);
        }

        #endregion

        #region Threads

        /// <summary>
        /// Map Simulatin Threads
        /// </summary>
        public Thread[] Threads;
        /// <summary>
        /// Whether the map simulation threads are running
        /// </summary>
        public bool Running { get; protected set; }

        /// <summary>
        /// Extra Simulation Thrad
        /// </summary>
        public Thread ExtraSimulatinThread;
        /// <summary>
        /// Whether the map simulation helper thread is running
        /// </summary>
        public bool HelperRunning { get; protected set; }

        /// <summary>
        /// The options for parrellism in the map simulation threads
        /// </summary>
        protected ParallelOptions _parallelOptions;

        /// <summary>
        /// Log list for callback from the map simulation Threads
        /// </summary>
        public List<string> Logs { get; protected set; }
        public List<string> Warnings { get; protected set; }
        public List<string> Errors { get; protected set; }

        /// <summary>
        /// Logs all the callbacks from the simulation
        /// </summary>
        /// <param name="Log"></param>
        /// <param name="LogType"></param>
        public void AddDebugLog(string Log, SimulationDebugLog LogType)
        {
            if (LogType == SimulationDebugLog.Error)
            {
                lock (Errors)
                {
                    Errors.Add(Log);
                }
            }
            else if (LogType == SimulationDebugLog.Warning)
            {
                lock (Warnings)
                {
                    Warnings.Add(Log);
                }
            }
            else
            {
                lock (Logs)
                {
                    Logs.Add(Log);
                }
            }
        }

        #endregion

        #region Painter

        /// <summary>
        /// Painting Settings
        /// </summary>
        public PainterSetting Painter = new PainterSetting();
        /// <summary>
        /// Auto-Painter Settings
        /// </summary>
        public AutoPainterSetting AutoPainter = new AutoPainterSetting();

        /// <summary>
        /// Autopaints a givent map by the auto painter settings
        /// </summary>
        public void AutoPaint()
        {
            if (AutoPainter.AutoPaintMapIndex < 0 || AutoPainter.AutoPaintMapIndex >= _mapSets.Count)
            {
                return;
            }

#if UNITY_EDITOR
            if (!Application.isPlaying)
            {
                EditorCoroutineUtility.StartCoroutine(BakeTiles(), this);
            }
            else
            {
                StartCoroutine(BakeTiles());
            }
#endif
        }

        /// <summary>
        /// Whether the Auto-Painter is baking in the tiles right now or not
        /// </summary>
        public bool Baking { get; set; }

        /// <summary>
        /// Bakes all the tiles from the Auto-Painter
        /// </summary>
        /// <returns></returns>
        protected virtual IEnumerator BakeTiles()
        {
            MapSet Map = _mapSets[AutoPainter.AutoPaintMapIndex];
            Debug.Log($"Baking Map {Map.SetSettings.Name} (Index:{AutoPainter.AutoPaintMapIndex})");
            ClearMap(AutoPainter.AutoPaintMapIndex);

            RaycastHit ForRayHit;
            RaycastHit AgainstRayHit;
            Baking = true;
            float RaycastDistance = AutoPainter.AutoPainterElavationIncrease + AutoPainter.AutoPainterElavationIDecrease + Map.SetSettings.MapSettings.Offset.y / 100f;
            while (Baking)
            {
                for (int x = 0; x < Map.SetSettings.MapSettings.MapWidth; x++)
                {
                    for (int y = 0; y < Map.SetSettings.MapSettings.MapHeight; y++)
                    {
                        // Raycast
                        Vector3 TileWorldPosition = GetTileWorldIndexCenter(x, y, Map);
                        // Water Check
                        if (Physics.Raycast(TileWorldPosition + Vector3.up * AutoPainter.AutoPainterElavationIncrease, Vector3.down * RaycastDistance, out ForRayHit, 800, AutoPainter.AutoPainterCheckForLayers))
                        {
                            if (Physics.Raycast(TileWorldPosition + Vector3.up * AutoPainter.AutoPainterElavationIncrease, Vector3.down * RaycastDistance, out AgainstRayHit, 800, AutoPainter.AutoPainterCheckAgainstLayers))
                            {
                                // If the for is closer then count it
                                if (ForRayHit.distance < AgainstRayHit.distance)
                                {
                                    Map.ChangeDefaultTileType(x, y, AutoPainter.AutoPainterType);
                                }
                                else
                                {
                                    // Distance Check
                                    float HeightDifference = AgainstRayHit.point.y - Map.SetSettings.MapSettings.Offset.y / 100f;
                                    if (HeightDifference > AutoPainter.AboveMapTypeChangeHeight)
                                    {
                                        Map.ChangeDefaultTileType(x, y, AutoPainter.AutoPainterAboveHeightType);
                                    }
                                    else if (HeightDifference < -AutoPainter.BelowMapTypeChangeHeight)
                                    {
                                        Map.ChangeDefaultTileType(x, y, AutoPainter.AutoPainterBelowHeightType);
                                    }
                                }
                            }
                            else
                            {
                                Map.ChangeDefaultTileType(x, y, AutoPainter.AutoPainterType);
                            }
                        }
                        else
                        {
                            if (Physics.Raycast(TileWorldPosition + Vector3.up * AutoPainter.AutoPainterElavationIncrease, Vector3.down * RaycastDistance, out AgainstRayHit, 800, AutoPainter.AutoPainterCheckAgainstLayers))
                            {
                                // Distance Check
                                float HeightDifference = AgainstRayHit.point.y - Map.SetSettings.MapSettings.Offset.y / 100f;
                                if (HeightDifference > AutoPainter.AboveMapTypeChangeHeight)
                                {
                                    Map.ChangeDefaultTileType(x, y, AutoPainter.AutoPainterAboveHeightType);
                                }
                                else if (HeightDifference < -AutoPainter.BelowMapTypeChangeHeight)
                                {
                                    Map.ChangeDefaultTileType(x, y, AutoPainter.AutoPainterBelowHeightType);
                                }
                            }
                        }

                        if (!Baking)
                        {
                            break;
                        }
                    }

                    if (!Baking)
                    {
                        break;
                    }

                    yield return null;
                }

                Baking = false;
            }

            SaveMaps();
            Debug.Log($"Bake Complete");
        }

        /// <summary>
        /// Creates a seam from the seam settings. Editor Only
        /// </summary>
        public void CreateSeam()
        {
            if (Application.isPlaying)
            {
                return;
            }

            if (MapSeamSettings.MapAInsertSettings.MapIndex < 0 || MapSeamSettings.MapAInsertSettings.MapIndex >= MapSets.Count)
            {
                return;
            }

            if (MapSeamSettings.MapBInsertSettings.MapIndex < 0 || MapSeamSettings.MapBInsertSettings.MapIndex >= MapSets.Count)
            {
                return;
            }

            MapSet MapA = MapSets[MapSeamSettings.MapAInsertSettings.MapIndex];
            MapSet MapB = MapSets[MapSeamSettings.MapBInsertSettings.MapIndex];
            MapSeam NewSeam = new MapSeam(MapA, MapB, MapSeamSettings.MapAInsertSettings.ConnectionSide, MapSeamSettings.MapBInsertSettings.ConnectionSide);
        }

        /// <summary>
        /// Clears all maps of all the painted tiles
        /// </summary>
        public void ClearMap()
        {
            if (Painter.CleanMapIndex < 0 || Painter.CleanMapIndex >= _mapSets.Count)
            {
                return;
            }

            MapSet Map = _mapSets[AutoPainter.AutoPaintMapIndex];
            Map.CleanMapTiles();
            SaveMaps();
        }

        /// <summary>
        /// Clears a map of all the painted tiles
        /// </summary>
        public void ClearMap(int MapIndex)
        {
            if (MapIndex < 0 || MapIndex >= _mapSets.Count)
            {
                return;
            }

            MapSet Map = _mapSets[MapIndex];
            Map.CleanMapTiles();
            SaveMaps();
        }

        #endregion

        #region Unity

        public static NavigationManager Instance = null;
        public bool Initialized { get; set; }
        protected void Awake()
        {
            ApplicationPlaying = Application.IsPlaying(this);
            if (Instance == null)
            {
                Instance = this;
            }
            else
            {
                Debug.Log($"Duplicate Navigation Manager in Scene. Destroying Navigation Manager {name}#{GetInstanceID()}");
                Destroy(gameObject);
            }

            if (RandomValidPosition == null)
            {
                RandomValidPosition = new System.Random(33399927);
            }

            _activeSets = new List<MapSet>();
            LoadMaps(_activeSets);           

            if (_activeSets.Count <= 0)
            {
                enabled = false;
                return;
            }

            Logs = new List<string>();
            Warnings = new List<string>();
            Errors = new List<string>();
            SetMapBridges();

            Running = true;
            SimulationAgents = new Dictionary<ushort, SimAgent>();
            SimulationAgentList = new List<SimAgent>();
            Obstacles = new List<NavigationObstacle>();
            Agents = new List<NavigationAgent>();
            _parallelOptions = new ParallelOptions() { MaxDegreeOfParallelism = ManagerSettings.PathfindingParrellism };

            Threads = new Thread[ManagerSettings.MapRedundancy + 1];

            bool CreateAgentMap = ManagerSettings.SimulationSettings.UseExtraSimulation && ManagerSettings.SimulationSettings.UseAgentAvoidance;
            foreach (MapSet Set in _activeSets)
            {
                Set.Initialize(ManagerSettings.PathfindingParrellism, ManagerSettings.MapRedundancy, CreateAgentMap,  AddDebugLog);
            }

            foreach(MapBridge Bridge in Bridges)
            {
                Bridge.Verify();
                //Debug.Log($"{Bridge.MapA.SetSettings.Name} to {Bridge.MapB.SetSettings.Name} is Valid = {Bridge.ValidBridge}");
            }

            foreach (MapSeam Seam in Seams)
            {
                Seam.Verify();
                //Debug.Log($"{Bridge.MapA.SetSettings.Name} to {Bridge.MapB.SetSettings.Name} is Valid = {Bridge.ValidBridge}");
            }

            if (ManagerSettings.SimulationSettings.UseExtraSimulation)
            {
                ExtraSimulatinThread = new Thread(SimulationHelperUpdate);
                ExtraSimulatinThread.Start();
            }

            for (int i = 0; i < ManagerSettings.MapRedundancy + 1; i++)
            {
                if (i == 0)
                {
                    Threads[i] = new Thread(UpdateThread0);
                }
                else if (i == 1)
                {
                    Threads[i] = new Thread(UpdateThread1);
                }
                else if (i == 2)
                {
                    Threads[i] = new Thread(UpdateThread2);
                }
                else if (i == 3)
                {
                    Threads[i] = new Thread(UpdateThread3);
                }
                else if (i == 4)
                {
                    Threads[i] = new Thread(UpdateThread4);
                }
                else if (i == 5)
                {
                    Threads[i] = new Thread(UpdateThread5);
                }
                else if (i == 6)
                {
                    Threads[i] = new Thread(UpdateThread6);
                }
                else if (i == 7)
                {
                    Threads[i] = new Thread(UpdateThread7);
                }
                else if (i == 8)
                {
                    Threads[i] = new Thread(UpdateThread8);
                }
                else if (i == 9)
                {
                    Threads[i] = new Thread(UpdateThread9);
                }

                Threads[i].Start();
            }

            Initialized = true;
            Debug.Log($"Navigation Manager Initialized");
        }

        /// <summary>
        /// List of all the obstacles that are currently active
        /// </summary>
        public List<NavigationObstacle> Obstacles { get; protected set; }
        /// <summary>
        /// List of all the agents that are currently active
        /// </summary>
        public List<NavigationAgent> Agents { get; protected set; }

        private void Update()
        {
            for (int i = 0; i < Obstacles.Count; i++)
            {
                Obstacles[i].UpdateObstacle();
            }

            for (int i = 0; i < Agents.Count; i++)
            {
                Agents[i].AutoUpdateAgent();
            }
        }

        public void FixedUpdate()
        {
            lock (Logs)
            {
                if (Logs.Count > 0)
                {
                    for (int i = 0; i < Logs.Count; i++)
                    {
                        string log = Logs[i];
                        if (!string.IsNullOrEmpty(log))
                        {
                            Debug.Log(log);
                        }
                    }
                    Logs.Clear();
                }
            }

            lock (Warnings)
            {
                if (Warnings.Count > 0)
                {
                    for (int i = 0; i < Warnings.Count; i++)
                    {
                        string log = Warnings[i];
                        if (!string.IsNullOrEmpty(log))
                        {
                            Debug.LogWarning(log);
                        }
                    }
                    Warnings.Clear();
                }
            }

            lock (Errors)
            {
                if (Errors.Count > 0)
                {
                    for (int i = 0; i < Errors.Count; i++)
                    {
                        string log = Errors[i];
                        if (!string.IsNullOrEmpty(log))
                        {
                            Debug.LogError(log);
                        }
                    }
                    Errors.Clear();
                }
            }            
        }

        private void OnDestroy()
        {
            foreach(MapSet Map in MapSets)
            {
                Map.OnDestroy();
            }
            if (Running)
            {
                Running = false;
                foreach(Thread thread in Threads)
                {
                    thread?.Abort();
                }
            }
            if (HelperRunning)
            {
                HelperRunning = false;
                ExtraSimulatinThread?.Abort();
            }
            Instance = null;
        }

        /// <summary>
        /// Navigation Manager Gizmos Settings
        /// </summary>
        public GizmosSetting GizmosSettings;

        /// <summary>
        /// Current Bridge that is being attempted
        /// </summary>
        public MapBridge AttemptedBridge { get; set; }

        private void OnDrawGizmosSelected()
        {
        #if UNITY_EDITOR

            if (!Application.isEditor)
            {
                return;
            }

            if (!Application.IsPlaying(this))
            {
                for (int i = 0; i < MapSets.Count; i++)
                {
                    if (MapSets[i].SetSettings.MapSettings.MapInfo.MakeSureSizeIsRight())
                    {
                        //Debug.Log($"Had to Correct Map {MapSets[i].SetSettings.Name}");
                    }
                    MapSets[i].InstanceID = i;
                }
            }

            if (!GizmosSettings.DisplayGizmos)
            {
                return;
            }

            int Index = -1;
            foreach(MapSet Map in MapSets)
            {
                Index++;
                if (GizmosSettings.DisplayMapGizmos_ForMapIndecies.Count > 0 && !GizmosSettings.DisplayMapGizmos_ForMapIndecies.Contains(Index))
                {
                    continue;
                }

                Vector3 MapOffset = new Vector3(Map.SetSettings.MapSettings.Offset.x, Map.SetSettings.MapSettings.Offset.y, Map.SetSettings.MapSettings.Offset.z);
                Vector3 MapSize = new Vector3();
                MapSize.x = Map.SetSettings.MapSettings.TileSize * Map.SetSettings.MapSettings.MapWidth / 100f;
                MapSize.y = 0.1f;
                MapSize.z = Map.SetSettings.MapSettings.TileSize * Map.SetSettings.MapSettings.MapHeight / 100f;
                Vector3 MapCenter = new Vector3();
                MapCenter.x = MapSize.x / 2 + Map.SetSettings.MapSettings.Offset.x / 100f;
                MapCenter.y = Map.SetSettings.MapSettings.Offset.y / 100f;
                MapCenter.z = MapSize.z / 2 + Map.SetSettings.MapSettings.Offset.z / 100f;

                if (GizmosSettings.DisplayMapBase)
                {
                    if (GizmosSettings.DisplayMapEdge)
                    {
                        Color color = Color.black;
                        color.a = GizmosSettings.DisplayMapAlpha;
                        Gizmos.color = color;
                        Gizmos.DrawCube(MapCenter - Vector3.down * 0.4f, new Vector3(MapSize.x + 5, MapSize.y, MapSize.z + 5));
                    }

                    Color basecolor = Color.white;
                    basecolor.a = GizmosSettings.DisplayMapAlpha;
                    Gizmos.color = basecolor;
                    Gizmos.DrawCube(MapCenter, MapSize);
                }

                if (GizmosSettings.DisplayTiles)
                {
                    Gizmos.color = Color.black;
                    MapOffset.y += 0.1f;
                    for (int x = 0; x <= Map.SetSettings.MapSettings.MapWidth; x++)
                    {
                        Vector3 Start = MapOffset + new Vector3(Map.SetSettings.MapSettings.TileSize, 0, 0) * x;
                        Start /= 100f;
                        Gizmos.DrawLine(Start, Start + Vector3.forward * MapSize.z);
                    }
                    for (int y = 0; y <= Map.SetSettings.MapSettings.MapHeight; y++)
                    {
                        Vector3 Start = MapOffset + new Vector3(0, 0, Map.SetSettings.MapSettings.TileSize) * y;
                        Start /= 100f;
                        Gizmos.DrawLine(Start, Start + Vector3.right * MapSize.x);
                    }
                }

                if (GizmosSettings.DisplayPaintedTilesMapIndex == -1 || GizmosSettings.DisplayPaintedTilesMapIndex == Index)
                {
                    if (GizmosSettings.DisplayPaintedTiles)
                    {
                        if (Application.IsPlaying(this))
                        {
                            if (Map.Initialized)
                            {
                                for (int x = 0; x < Map.SetSettings.MapSettings.MapWidth; x++)
                                {
                                    for (int y = 0; y < Map.SetSettings.MapSettings.MapHeight; y++)
                                    {
                                        try
                                        {
                                            TileTypes GetType = Map.BaseMap.GetTileType(x, y);

                                            if (!GizmosSettings.OnlyPaintTheseTypes.Contains(GetType))
                                            {
                                                continue;
                                            }

                                            if (GetType == TileTypes.Standard || GetType == TileTypes.Free)
                                            {
                                                Gizmos.color = Color.green;
                                            }
                                            else if (GetType == TileTypes.Blocked || GetType == TileTypes.Void)
                                            {
                                                Gizmos.color = Color.black;
                                            }
                                            else if (GetType == TileTypes.Obstacle)
                                            {
                                                Gizmos.color = Color.red;
                                            }
                                            else if (GetType == TileTypes.Water)
                                            {
                                                Gizmos.color = Color.blue;
                                            }

                                            Vector3 TilePosition = GetTileWorldIndexCenter(x, y, Map);
                                            TilePosition.y += 0.01f;
                                            Vector3 Size = Vector3.one * Map.SetSettings.MapSettings.TileSize / 100f;
                                            Size.y = 0.01f;
                                            Gizmos.DrawCube(TilePosition, Size);
                                        }
                                        catch (Exception ex)
                                        {
                                            Debug.LogException(ex);
                                        }
                                    }
                                }
                            }
                        }
                        else
                        {
                            Map.SetSettings.MapSettings.MapInfo.MakeSureSizeIsRight();
                            if (Map.SetSettings.MapSettings.MapWidth != Map.SetSettings.MapSettings.MapInfo.SavedTiles.GetLength(0) || Map.SetSettings.MapSettings.MapHeight != Map.SetSettings.MapSettings.MapInfo.SavedTiles.GetLength(1))
                            {
                                continue;
                            }
                            for (int x = 0; x < Map.SetSettings.MapSettings.MapWidth; x++)
                            {
                                for (int y = 0; y < Map.SetSettings.MapSettings.MapHeight; y++)
                                {
                                    try
                                    {
                                        TileTypes GetType = (TileTypes)Map.SetSettings.MapSettings.MapInfo.SavedTiles[x, y];

                                        if (!GizmosSettings.OnlyPaintTheseTypes.Contains(GetType))
                                        {
                                            continue;
                                        }

                                        if (GetType == TileTypes.Standard || GetType == TileTypes.Free)
                                        {
                                            Gizmos.color = Color.green;
                                        }
                                        else if (GetType == TileTypes.Blocked || GetType == TileTypes.Void)
                                        {
                                            Gizmos.color = Color.black;
                                        }
                                        else if (GetType == TileTypes.Obstacle)
                                        {
                                            Gizmos.color = Color.red;
                                        }
                                        else if (GetType == TileTypes.Water)
                                        {
                                            Gizmos.color = Color.blue;
                                        }

                                        Vector3 TilePosition = GetTileWorldIndexCenter(x, y, Map);
                                        TilePosition.y += 0.2f;
                                        Vector3 Size = Vector3.one * Map.SetSettings.MapSettings.TileSize / 100f;
                                        Size.y = 0.5f;
                                        Gizmos.DrawCube(TilePosition, Size);
                                    }
                                    catch (Exception ex)
                                    {
                                        Debug.LogException(ex);
                                    }
                                }
                            }
                        }
                    }

                    if (GizmosSettings.DisplayAgentOwnedTiles)
                    {
                        if (Application.IsPlaying(this))
                        {
                            if (Map.Initialized)
                            {
                                Gizmos.color = Color.cyan;
                                if (Map.BaseMap.AgentOwnershipMap != null)
                                {
                                    for (int x = 0; x < Map.SetSettings.MapSettings.MapWidth; x++)
                                    {
                                        for (int y = 0; y < Map.SetSettings.MapSettings.MapHeight; y++)
                                        {
                                            try
                                            {
                                                if (Map.BaseMap.AgentOwnershipMap.Tiles[x,y] != 0)
                                                {
                                                    Vector3 TilePosition = GetTileWorldIndexCenter(x, y, Map);
                                                    TilePosition.y += 0.01f;
                                                    Vector3 Size = Vector3.one * Map.SetSettings.MapSettings.TileSize / 100f;
                                                    Size.y = 0.01f;
                                                    Gizmos.DrawCube(TilePosition, Size);
                                                }
                                            }
                                            catch (Exception ex)
                                            {
                                                Debug.LogException(ex);
                                            }
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }

            if (GizmosSettings.DisplayBridges)
            {
                Gizmos.color = Color.blue;
                for (int i = 0; i < Bridges.Count; i++)
                {
                    if (ValidBridge(Bridges[i]))
                    {
                        Gizmos.color = Color.blue;
                    }
                    else
                    {
                        Gizmos.color = Color.red;
                    }

                    for (int j = 1; j < Bridges[i].MidPoints.Count; j++)
                    {
                        // Figure out positions
                        Vector3 MidPoint = new Vector3(Bridges[i].MidPoints[j].x, Bridges[i].MidPoints[j].y, Bridges[i].MidPoints[j].z) / 100f;
                        Vector3 PreviousMidPoint = new Vector3(Bridges[i].MidPoints[j - 1].x, Bridges[i].MidPoints[j - 1].y, Bridges[i].MidPoints[j - 1].z) / 100f;
                        // Draw Map Position
                        Gizmos.DrawLine(MidPoint, PreviousMidPoint);
                    }
                }

                if(GizmosSettings.DisplayBridgeDirections)
                {
                    for (int i = 0; i < Bridges.Count; i++)
                    {
                        if (!ValidBridge(Bridges[i]))
                        {
                            continue;
                        }

                        if (Bridges[i].WalkableDirections == BridgeDirections.A_To_B)
                        {
                            Vector3 StartPoint = new Vector3(Bridges[i].MidPoints[0].x, Bridges[i].MidPoints[0].y, Bridges[i].MidPoints[0].z) / 100f;
                            Vector3 EndPoint = new Vector3(Bridges[i].MidPoints[Bridges[i].MidPoints.Count - 1].x, Bridges[i].MidPoints[Bridges[i].MidPoints.Count - 1].y, Bridges[i].MidPoints[Bridges[i].MidPoints.Count - 1].z) / 100f;

                            Gizmos.color = Color.red;
                            Gizmos.DrawSphere(StartPoint, 0.5f);

                            Gizmos.color = Color.blue;
                            Gizmos.DrawSphere(EndPoint, 0.5f);
                        }
                        else if (Bridges[i].WalkableDirections == BridgeDirections.B_To_A)
                        {
                            Vector3 StartPoint = new Vector3(Bridges[i].MidPoints[0].x, Bridges[i].MidPoints[0].y, Bridges[i].MidPoints[0].z) / 100f;
                            Vector3 EndPoint = new Vector3(Bridges[i].MidPoints[Bridges[i].MidPoints.Count - 1].x, Bridges[i].MidPoints[Bridges[i].MidPoints.Count - 1].y, Bridges[i].MidPoints[Bridges[i].MidPoints.Count - 1].z) / 100f;

                            Gizmos.color = Color.blue;
                            Gizmos.DrawSphere(StartPoint, 0.5f);

                            Gizmos.color = Color.red;
                            Gizmos.DrawSphere(EndPoint, 0.5f);
                        }
                    }
                }

                if (AttemptedBridge != null)
                {
                    if (ValidBridge(AttemptedBridge))
                    {
                        Gizmos.color = Color.green;
                    }
                    else
                    {
                        Gizmos.color = Color.yellow;
                    }

                    for (int j = 1; j < AttemptedBridge.MidPoints.Count; j++)
                    {
                        // Figure out positions
                        Vector3 MidPoint = new Vector3(AttemptedBridge.MidPoints[j].x, AttemptedBridge.MidPoints[j].y, AttemptedBridge.MidPoints[j].z) / 100f;
                        Vector3 PreviousMidPoint = new Vector3(AttemptedBridge.MidPoints[j - 1].x, AttemptedBridge.MidPoints[j - 1].y, AttemptedBridge.MidPoints[j - 1].z) / 100f;
                        // Draw Map Position
                        Gizmos.DrawLine(MidPoint, PreviousMidPoint);
                    }
                }
            }
            else
            {
                if (Application.IsPlaying(this))
                {
                    return;
                }

                foreach (MapBridge Bridge in Bridges)
                {
                    ValidateBridge(Bridge);
                }
            }

            if (MapSeamSettings.DisplaySeams)
            {
                //Gizmos.color = Color.green;
                foreach(MapSeam Seam in Seams)
                {
                    Vector3 MapASize = new Vector3();
                    MapASize.y = 0.1f;
                    Vector3 MapAPosition = new Vector3();
                    MapAPosition.y = Seam.MapA.SetSettings.MapSettings.Offset.y / 100f;

                    if (Seam.MapA_ConectionSide == SeamConnectionSide.South || Seam.MapA_ConectionSide == SeamConnectionSide.North)
                    {
                        MapASize.z = Seam.MapA.SetSettings.MapSettings.TileSize / 100f;
                        MapASize.x = Seam.MapA.SetSettings.MapSettings.TileSize * Seam.MapA.SetSettings.MapSettings.MapWidth / 100f;
                        MapAPosition.x = MapASize.x / 2 + Seam.MapA.SetSettings.MapSettings.Offset.x / 100f;
                        MapAPosition.z = MapASize.z / 2 + Seam.MapA.SetSettings.MapSettings.Offset.z / 100f;
                        if (Seam.MapA_ConectionSide == SeamConnectionSide.North)
                        {
                            MapAPosition.z += Seam.MapA.SetSettings.MapSettings.TileSize * (Seam.MapB.SetSettings.MapSettings.MapHeight - 1) / 100f;
                        }
                    }
                    else
                    {
                        MapASize.x = Seam.MapA.SetSettings.MapSettings.TileSize / 100f;
                        MapASize.z = Seam.MapA.SetSettings.MapSettings.TileSize * Seam.MapA.SetSettings.MapSettings.MapHeight / 100f;
                        MapAPosition.x = MapASize.x / 2 + Seam.MapA.SetSettings.MapSettings.Offset.x / 100f;
                        MapAPosition.z = MapASize.z / 2 + Seam.MapA.SetSettings.MapSettings.Offset.z / 100f;
                        if (Seam.MapA_ConectionSide == SeamConnectionSide.East)
                        {
                            MapAPosition.x += Seam.MapA.SetSettings.MapSettings.TileSize * (Seam.MapA.SetSettings.MapSettings.MapWidth - 1) / 100f;
                        }
                    }

                    Vector3 MapBSize = new Vector3();
                    MapBSize.y = 0.1f;
                    Vector3 MapBPosition = new Vector3();

                    if (Seam.MapB_ConectionSide == SeamConnectionSide.South || Seam.MapB_ConectionSide == SeamConnectionSide.North)
                    {
                        MapBSize.z = Seam.MapB.SetSettings.MapSettings.TileSize / 100f;
                        MapBSize.x = Seam.MapB.SetSettings.MapSettings.TileSize * Seam.MapB.SetSettings.MapSettings.MapWidth / 100f;
                        MapBPosition.x = MapBSize.x / 2 + Seam.MapB.SetSettings.MapSettings.Offset.x / 100f;
                        MapBPosition.z = MapBSize.z / 2 + Seam.MapB.SetSettings.MapSettings.Offset.z / 100f;
                        if (Seam.MapB_ConectionSide == SeamConnectionSide.North)
                        {
                            MapBPosition.z += Seam.MapB.SetSettings.MapSettings.TileSize * (Seam.MapB.SetSettings.MapSettings.MapHeight - 1) / 100f;
                        }
                    }
                    else
                    {
                        MapBSize.x = Seam.MapB.SetSettings.MapSettings.TileSize / 100f;
                        MapBSize.z = Seam.MapB.SetSettings.MapSettings.TileSize * Seam.MapB.SetSettings.MapSettings.MapHeight / 100f;
                        MapBPosition.x = MapASize.x / 2 + Seam.MapB.SetSettings.MapSettings.Offset.x / 100f;
                        MapBPosition.z = MapASize.z / 2 + Seam.MapB.SetSettings.MapSettings.Offset.z / 100f;
                        if (Seam.MapB_ConectionSide == SeamConnectionSide.East)
                        {
                            MapBPosition.x += Seam.MapB.SetSettings.MapSettings.TileSize * (Seam.MapA.SetSettings.MapSettings.MapWidth - 1) / 100f;
                        }
                    }

                    Gizmos.color = Color.blue;
                    Gizmos.DrawCube(MapAPosition, MapASize);
                    Gizmos.color = Color.yellow;
                    Gizmos.DrawCube(MapBPosition, MapBSize);
                    Gizmos.color = Color.blue;
                    Gizmos.DrawLine(MapAPosition, MapBPosition);
                }
            }

            if (Application.IsPlaying(this))
            {
                return;
            }

            if (GizmosSettings.DisplayPaintBrush)
            {
                Event e = Event.current;
                Vector3 mousePos = e.mousePosition;
                float ppp = EditorGUIUtility.pixelsPerPoint;
                if (SceneView.currentDrawingSceneView == null || SceneView.currentDrawingSceneView.camera == null)
                {
                    return;
                }
                mousePos.y = SceneView.currentDrawingSceneView.camera.pixelHeight - mousePos.y * ppp;
                mousePos.x *= ppp;

                //Debug.Log($"Try To Paint");
                Ray ray = SceneView.currentDrawingSceneView.camera.ScreenPointToRay(mousePos);

                MapSet ClosestMapSet = null;
                float ClosestScore = float.MaxValue;
                Vector3 ClosestPosition = new Vector3();
                int NewIndex = 0;
                foreach (MapSet Set in MapSets)
                {
                    if (GizmosSettings.DisplayMapGizmos_ForMapIndecies.Count > 0 && !GizmosSettings.DisplayMapGizmos_ForMapIndecies.Contains(NewIndex))
                    {
                        continue;
                    }
                    NewIndex++;

                    Plane MapPlane = new Plane(Vector3.up, new Vector3(Set.SetSettings.MapSettings.Offset.x, Set.SetSettings.MapSettings.Offset.y, Set.SetSettings.MapSettings.Offset.z) / 100f);
                    if (MapPlane.Raycast(ray, out float Enter))
                    {
                        Vector3 HitPosition = ray.GetPoint(Enter);
                        if (ValidPoint(HitPosition, Set))
                        {
                            //Debug.Log($"Scored a Hit at Position {HitPosition} on map {Set.SetSettings.Name} - Score {Enter}");
                            if (Enter < ClosestScore)
                            {
                                ClosestMapSet = Set;
                                ClosestScore = Enter;
                                ClosestPosition = HitPosition;
                            }
                        }
                    }
                }

                if (ClosestMapSet != null)
                {
                    //Debug.Log("Show Paint Brush");
                    Vector3 Size = (ClosestMapSet.SetSettings.MapSettings.TileSize + ClosestMapSet.SetSettings.MapSettings.TileSize * Painter.CurrentPainterRadius * 2) * Vector3.one;
                    Size /= 100f;
                    Size.y = 1;
                    Size.x -= 0.2f * ClosestMapSet.SetSettings.MapSettings.TileSize / 100f;
                    Size.z -= 0.2f * ClosestMapSet.SetSettings.MapSettings.TileSize / 100f;
                    Vector3 PainterPosition = ClosestPosition + Vector3.up;
                    PainterPosition = GetTileCenterWorldPosition(PainterPosition, ClosestMapSet);

                    Color BrushColor = Color.white;

                    if (Painter.CurrentPainterType == TileTypes.Standard || Painter.CurrentPainterType == TileTypes.Free)
                    {
                        BrushColor = Color.green;
                    }
                    else if (Painter.CurrentPainterType == TileTypes.Blocked || Painter.CurrentPainterType == TileTypes.Void)
                    {
                        BrushColor = Color.black;
                    }
                    else if (Painter.CurrentPainterType == TileTypes.Obstacle)
                    {
                        BrushColor = Color.red;
                    }
                    else if (Painter.CurrentPainterType == TileTypes.Water)
                    {
                        BrushColor = Color.blue;
                    }

                    BrushColor.a = 0.5f;
                    Gizmos.color = BrushColor;

                    if (GizmosSettings.DebugPaintBrushPosition)
                    {
                        IntVector3 WorldTileIndex = GetTileWorldPosition(PainterPosition);
                        IntVector2 TileIndex = ClosestMapSet.GetMapTileIndex(WorldTileIndex);

                        Debug.Log($"Brush Position Data: Tile Center({PainterPosition.x}, {PainterPosition.y}, {PainterPosition.z}), Tile Index({TileIndex.x}, {TileIndex.y})");
                    }

                    if (Painter.PainterType == PainterTypes.Square)
                    {
                        Gizmos.DrawCube(PainterPosition, Size);
                        //if (SquarePaintBrush == null)
                        //{
                        //    GameObject PrimativeMeshObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
                        //    SquarePaintBrush = PrimativeMeshObject.GetComponent<MeshRenderer>();
                        //    _squareFilter = PrimativeMeshObject.GetComponent<MeshFilter>();
                        //    Collider collider = PrimativeMeshObject.GetComponent<Collider>();
                        //    collider.enabled = false;
                        //    PrimativeMeshObject.layer = 5;
                        //    SquarePaintBrush.transform.SetParent(transform);
                        //    SquarePaintBrush.gameObject.name = "Square Brush";
                        //    DestroyImmediate(collider);
                        //    SquarePaintBrush.transform.position = Vector3.up * 1000000;
                        //}
                        //SquarePaintBrush.sharedMaterial.color = BrushColor;
                        //SquarePaintBrush.transform.position = PainterPosition;
                        //SquarePaintBrush.transform.localScale = Size;
                        //Gizmos.DrawMesh(_squareFilter.sharedMesh, SquarePaintBrush.subMeshStartIndex, PainterPosition, Quaternion.identity, Size);
                    }
                    else
                    {
                        if (Painter.CirclePaintBrush == null)
                        {
                            GameObject PrimativeMeshObject = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
                            Painter.CirclePaintBrush = PrimativeMeshObject.GetComponent<MeshRenderer>();
                            Painter.CircleFilter = PrimativeMeshObject.GetComponent<MeshFilter>();
                            Collider collider = PrimativeMeshObject.GetComponent<Collider>();
                            PrimativeMeshObject.layer = 5;
                            //CirclePaintBrush.transform.SetParent(transform);
                            Painter.CirclePaintBrush.gameObject.name = "Circle Brush";
                            DestroyImmediate(collider);
                            Painter.CirclePaintBrush.transform.position = Vector3.up * 1000000;
                        }
                        //CirclePaintBrush.sharedMaterial.color = BrushColor;
                        //CirclePaintBrush.transform.position = PainterPosition;
                        //CirclePaintBrush.transform.localScale = Size;
                        Gizmos.DrawMesh(Painter.CircleFilter.sharedMesh, Painter.CirclePaintBrush.subMeshStartIndex, PainterPosition, Quaternion.identity, Size);
                    }
                }
            }
            #endif
        }

        /// <summary>
        /// Saves all the maps for the current scene
        /// </summary>
        public void SaveMaps()
        {
            if (Application.isPlaying)
            {
                return;
            }

            JsonSerializerSettings setting = new JsonSerializerSettings
            {
                Formatting = Formatting.None,
                ReferenceLoopHandling = ReferenceLoopHandling.Ignore
            };

            string BasePath = Application.dataPath + ManagerSettings.MapSaveDataFolder;
            if (string.IsNullOrEmpty(ManagerSettings.MapSaveDataFolder) || ManagerSettings.MapSaveDataFolder == "None" || !Directory.Exists(BasePath)) 
            {
                if (!Directory.Exists(BasePath) && ManagerSettings.MapSaveDataFolder != "None" && !string.IsNullOrEmpty(ManagerSettings.MapSaveDataFolder))
                {
                    Debug.Log($"Save -> Base Path not Valid: FULL: '{BasePath}' -Partial: '{ManagerSettings.MapSaveDataFolder}'");
                }

                BasePath = Application.streamingAssetsPath + "/Saved Maps/";
                if (!Directory.Exists(Application.streamingAssetsPath))
                {
                    Directory.CreateDirectory(Application.streamingAssetsPath);
                }

                if (!Directory.Exists(Application.streamingAssetsPath + "/Saved Maps/"))
                {
                    Directory.CreateDirectory(Application.streamingAssetsPath + "/Saved Maps/");
                }
            }

            foreach (MapSet MapSet in _mapSets)
            {
                MapSet.SetSettings.MapSettings.MapInfo.MakeSureSizeIsRight();

                string path = Path.Combine(BasePath, $"Scene_({SceneManager.GetActiveScene().name})-Map_({MapSet.SetSettings.Name}).json");
                string json = JsonConvert.SerializeObject(MapSet, setting);

                if (File.Exists(path))
                {
                    File.Delete(path);
                }
                File.WriteAllText(path, json);
            }
        }

        /// <summary>
        /// Load all the maps for the current scene
        /// </summary>
        public void LoadMaps()
        {
            if (Application.isPlaying)
            {
                return;
            }

            //Debug.Log("Load Maps");
            JsonSerializerSettings setting = new JsonSerializerSettings
            {
                Formatting = Formatting.None,
                ReferenceLoopHandling = ReferenceLoopHandling.Ignore
            };

            string BasePath = Application.dataPath + ManagerSettings.MapSaveDataFolder;
            if (string.IsNullOrEmpty(ManagerSettings.MapSaveDataFolder) || ManagerSettings.MapSaveDataFolder == "None" || !Directory.Exists(BasePath))
            {
                if (!Directory.Exists(BasePath) && ManagerSettings.MapSaveDataFolder != "None" && !string.IsNullOrEmpty(ManagerSettings.MapSaveDataFolder))
                {
                    Debug.Log($"Load -> Base Path not Valid: FULL: '{BasePath}' -Partial: '{ManagerSettings.MapSaveDataFolder}'");
                }
                BasePath = Application.streamingAssetsPath + "/Saved Maps/";
            }

            List<MapSet> NewMapSet = new List<MapSet>();
            foreach (MapSet MapSet in _mapSets)
            {
                string path = Path.Combine(BasePath, $"Scene_({SceneManager.GetActiveScene().name})-Map_({MapSet.SetSettings.Name}).json");

                if (File.Exists(path))
                {
                    string fileContent = File.ReadAllText(path);
                    MapSet SavedMap = JsonConvert.DeserializeObject<MapSet>(fileContent);
                    NewMapSet.Add(SavedMap);
                }
                else
                {
                    NewMapSet.Add(MapSet);
                }
            }
            _mapSets.Clear();
            _mapSets = NewMapSet;
        }

        /// <summary>
        /// Load all the maps for the current scene and puts them in the list
        /// </summary>
        /// <param name="NewList"></param>
        protected void LoadMaps(List<MapSet> NewList)
        {
            JsonSerializerSettings setting = new JsonSerializerSettings
            {
                Formatting = Formatting.None,
                ReferenceLoopHandling = ReferenceLoopHandling.Ignore
            };

            string BasePath = Application.dataPath + ManagerSettings.MapSaveDataFolder;

            if (string.IsNullOrEmpty(ManagerSettings.MapSaveDataFolder) || ManagerSettings.MapSaveDataFolder == "None" || !Directory.Exists(BasePath))
            {
                if (!Directory.Exists(BasePath) && ManagerSettings.MapSaveDataFolder != "None" && !string.IsNullOrEmpty(ManagerSettings.MapSaveDataFolder))
                {
                    Debug.Log($"Load -> Base Path not Valid: FULL: '{BasePath}' -Partial: '{ManagerSettings.MapSaveDataFolder}'");
                }
                BasePath = Application.streamingAssetsPath + "/Saved Maps/";
            }

            foreach (MapSet MapSet in _mapSets)
            {
                string path = Path.Combine(BasePath, $"Scene_({SceneManager.GetActiveScene().name})-Map_({MapSet.SetSettings.Name}).json");

                if (File.Exists(path))
                {
                    string fileContent = File.ReadAllText(path);
                    MapSet SavedMap = JsonConvert.DeserializeObject<MapSet>(fileContent);
                    NewList.Add(SavedMap);
                }
                else
                {
                    NewList.Add(MapSet);
                }
            }
        }

        #endregion

        #region Updates

        public void UpdateThread0()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update0);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update0(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[0].Update();
            }
        }

        public void UpdateThread1()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update1);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update1(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[1].Update();
            }
        }

        public void UpdateThread2()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update2);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update2(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[2].Update();
            }
        }

        public void UpdateThread3()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update3);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update3(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[3].Update();
            }
        }

        public void UpdateThread4()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update4);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update4(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[4].Update();
            }
        }

        public void UpdateThread5()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update5);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update5(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[5].Update();
            }
        }

        public void UpdateThread6()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update6);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update6(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[6].Update();
            }
        }

        public void UpdateThread7()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update7);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update7(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[7].Update();
            }
        }

        public void UpdateThread8()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update8);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update8(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[8].Update();
            }
        }

        public void UpdateThread9()
        {
            try
            {
                while (Running)
                {
                    Thread.Sleep(1);

                    Parallel.ForEach(_activeSets, _parallelOptions, Update9);
                }
            }
            catch (Exception e)
            {
                Debug.Log($"Error Caught {e}");
            }
        }

        public void Update9(MapSet Map)
        {
            if (Map.Initialized)
            {
                Map.RedundantMaps[9].Update();
            }
        }

        #endregion

        #region Extra Simulation

        /// <summary>
        /// Dictionary of all the simulation Agent
        /// </summary>
        public Dictionary<ushort, SimAgent> SimulationAgents { get; protected set; }

        /// <summary>
        /// List of all the simulation agents
        /// </summary>
        public List<SimAgent> SimulationAgentList { get; protected set; }

        /// <summary>
        /// Stopwatch to keep track of the helper update
        /// </summary>
        protected System.Diagnostics.Stopwatch HelperStopwatch;

        /// <summary>
        /// The Helper threads target period in ms
        /// </summary>
        public long HelperTargetPeriod { get; set; }

        /// <summary>
        /// The Helper threads target period in sec
        /// </summary>
        public float HelperTargetDeltaTime { get; set; }

        /// <summary>
        /// The Helper threads Delta Time of the last frame
        /// </summary>
        public float HelperDeltaTime { get; protected set; }

        /// <summary>
        /// This helper is used to run the simulation agents and the agent avoidence
        /// </summary>
        public void SimulationHelperUpdate()
        {
            HelperRunning = true;
            HelperStopwatch = new System.Diagnostics.Stopwatch();
            HelperTargetPeriod = (int)MathF.Ceiling(1000f / ManagerSettings.SimulationSettings.DesiredHelperTickrate);
            HelperTargetDeltaTime = HelperTargetPeriod / 1000f;

            while (HelperRunning)
            {
                try
                {
                    HelperStopwatch.Start();

                    // Update
                    HelperUpdate();

                    // Wait Section
                    HelperStopwatch.Stop();

                    HelperDeltaTime = HelperStopwatch.ElapsedMilliseconds / 1000f;
                    if (HelperStopwatch.ElapsedMilliseconds < HelperTargetPeriod)
                    {
                        HelperDeltaTime = HelperTargetDeltaTime;
                        int SleepPeriod = (int)(HelperTargetPeriod - HelperStopwatch.ElapsedMilliseconds);
                        Thread.Sleep(SleepPeriod);
                    }
                    else
                    {
                        HelperDeltaTime = HelperStopwatch.ElapsedMilliseconds / 1000f;
                    }

                    HelperStopwatch.Reset();
                }
                catch (Exception e)
                {
                    lock (Errors)
                    {
                        Errors.Add($"{e.Message} \n --------------------------------- \n {e.StackTrace}");
                    }
                }
            }
        }

        /// <summary>
        /// The main update section of the simulation helper update. Removed this section for clarity and ease of use
        /// </summary>
        protected virtual void HelperUpdate()
        {
            lock (SimulationAgents)
            {
                for (int i = 0; i < SimulationAgentList.Count; i++)
                {
                    SimulationAgentList[i].Update();
                }
            }

            for (int i = 0; i < MapSets.Count; i++)
            {
                MapSets[i].ClearClaims();
            }
        }

        #endregion

        #region Helpers

        /// <summary>
        /// Gets the tile world index center for a specific map
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="map"></param>
        /// <returns></returns>
        private Vector3 GetTileWorldIndexCenter(int x, int y, MapSet map)
        {
            Vector3 NewPosition = new Vector3();

            NewPosition.x = x * map.SetSettings.MapSettings.TileSize + map.SetSettings.MapSettings.TileSize / 2f + map.SetSettings.MapSettings.Offset.x;
            NewPosition.y = map.SetSettings.MapSettings.Offset.y;
            NewPosition.z = y * map.SetSettings.MapSettings.TileSize + map.SetSettings.MapSettings.TileSize / 2f + map.SetSettings.MapSettings.Offset.z;
            NewPosition /= 100f;

            return NewPosition;
        }

        /// <summary>
        /// Checks to see if any point of a circle is on a specific map
        /// </summary>
        /// <param name="set"></param>
        /// <param name="newPosition"></param>
        /// <param name="radius"></param>
        /// <returns></returns>
        internal bool CircleOnMap(MapSet set, Vector3 newPosition, float radius)
        {
            IntVector3 WorldTileIndex = GetTileWorldPosition(newPosition);
            IntVector2 TileIndex = set.GetMapTileIndex(WorldTileIndex);
            int IntRadius = Mathf.FloorToInt(radius * 100 / set.SetSettings.MapSettings.TileSize);
            IntVector2 RadiusVector = new IntVector2(IntRadius, IntRadius);
            IntVector2 BottomLeftTileIndex = set.GetMapTileIndex(set.BottomLeftPosition) - RadiusVector;
            IntVector2 TopRightTileIndex = set.GetMapTileIndex(set.TopRightPosition) + RadiusVector;
            
            if (TileIndex.x >= BottomLeftTileIndex.x && TileIndex.x <= TopRightTileIndex.x && TileIndex.y >= BottomLeftTileIndex.y && TileIndex.y <= TopRightTileIndex.y)
            {
                return true;
            }

            return false;
        }

        /// <summary>
        /// Checks to see if any point of a rectangle is on a specific map
        /// </summary>
        /// <param name="set"></param>
        /// <param name="newPositionRect"></param>
        /// <param name="HeightCheck"></param>
        /// <returns></returns>
        internal bool RectOnMap(MapSet set, PositionSquareRect newPositionRect, float HeightCheck)
        {
            if (newPositionRect.Corners[0].y * 100 < set.SetSettings.MapSettings.Offset.y || (newPositionRect.Corners[0].y - HeightCheck) * 100 > set.SetSettings.MapSettings.Offset.y)
            {
                return false;
            }

            foreach(Vector3 Corner in newPositionRect.Corners) 
            {
                if (set.PointInMap(GetTileWorldPosition(Corner)))
                {
                    return true;
                }
            }

            return false;
        }

        /// <summary>
        /// Get the best map at a world tile index. Can be NULL
        /// </summary>
        /// <param name="intVector3"></param>
        /// <returns></returns>
        public MapSet GetMapAtTileWorldPosition(IntVector3 intVector3)
        {
            List<MapSet> PotentialMaps = new List<MapSet>();
            foreach(MapSet set in MapSets)
            {
                if (set.PointInMap(intVector3))
                {
                    PotentialMaps.Add(set);
                }
            }

            if (PotentialMaps.Count == 0)
            {
                return null;
            }

            MapSet ClosestMap = PotentialMaps[0];
            int ClosestMapScore = Mathf.Abs(ClosestMap.SetSettings.MapSettings.Offset.y - intVector3.y);
            foreach (MapSet set in PotentialMaps)
            {
                int NewScore = Mathf.Abs(set.SetSettings.MapSettings.Offset.y - intVector3.y);
                if (NewScore < ClosestMapScore)
                {
                    ClosestMap = set;
                    ClosestMapScore = NewScore;
                }
            }

            return ClosestMap;
        }

        /// <summary>
        /// Get the best map at a world position. Can be NULL
        /// </summary>
        /// <param name="intVector3"></param>
        /// <returns></returns>
        public MapSet GetMapAtWorldPosition(Vector3 position)
        {
            IntVector3 intWorldPosition = GetTileWorldPosition(position);
            List<MapSet> PotentialMaps = new List<MapSet>();
            foreach (MapSet set in MapSets)
            {
                if (set.PointInMap(intWorldPosition))
                {
                    PotentialMaps.Add(set);
                }
            }

            if (PotentialMaps.Count == 0)
            {
                return null;
            }

            MapSet ClosestMap = PotentialMaps[0];
            int ClosestMapScore = Mathf.Abs(ClosestMap.SetSettings.MapSettings.Offset.y - intWorldPosition.y);
            foreach (MapSet set in PotentialMaps)
            {
                int NewScore = Mathf.Abs(set.SetSettings.MapSettings.Offset.y - intWorldPosition.y);
                if (NewScore < ClosestMapScore)
                {
                    ClosestMap = set;
                    ClosestMapScore = NewScore;
                }
            }

            return ClosestMap;
        }

        /// <summary>
        /// Adds the Obstacle to the Manager
        /// </summary>
        /// <param name="navigationObstacle"></param>
        public void AddObstacle(NavigationObstacle navigationObstacle)
        {
            if (!Obstacles.Contains(navigationObstacle))
            {
                Obstacles.Add(navigationObstacle);
            }
        }

        /// <summary>
        /// Removes the Obstacle to the Manager
        /// </summary>
        /// <param name="navigationObstacle"></param>
        public void RemoveObstacle(NavigationObstacle navigationObstacle)
        {
            if (Obstacles.Contains(navigationObstacle))
            {
                Obstacles.Remove(navigationObstacle);
            }
        }

        /// <summary>
        /// Adds the Agent to the Manager
        /// </summary>
        /// <param name="agent"></param>
        public void AddAgent(NavigationAgent agent)
        {
            if (!Agents.Contains(agent))
            {
                Agents.Add(agent);
            }
        }

        /// <summary>
        /// Adds the simulation Agent to the Manager
        /// </summary>
        /// <param name="agent"></param>
        public void AddSimAgent(SimNavigationAgent agent)
        {
            if (!Agents.Contains(agent))
            {
                Agents.Add(agent);

                lock(SimulationAgents)
                {
                    SimulationAgents.Add(agent.SimulationAgent.SimID, agent.SimulationAgent);
                    lock (SimulationAgentList)
                    {
                        SimulationAgentList.Add(agent.SimulationAgent);
                    }
                }
            }
        }

        /// <summary>
        /// Removes the Agent to the Manager
        /// </summary>
        /// <param name="agent"></param>
        public void RemoveAgent(NavigationAgent agent)
        {
            if (Agents.Contains(agent))
            {
                Agents.Remove(agent);
            }
        }

        /// <summary>
        /// Removes the simulation Agent to the Manager
        /// </summary>
        /// <param name="agent"></param>
        public void RemoveSimAgent(SimNavigationAgent agent)
        {
            if (!Running)
            {
                return;
            }
            if (Agents.Contains(agent))
            {
                Agents.Remove(agent);

                lock (SimulationAgents)
                {
                    SimulationAgents.Remove(agent.SimulationAgent.SimID);

                    lock (SimulationAgentList)
                    {
                        SimulationAgentList.Remove(agent.SimulationAgent);
                    }
                }
            }
        }

        /// <summary>
        /// Checks whether a point is valid on a map
        /// </summary>
        /// <param name="hitPosition"></param>
        /// <param name="set"></param>
        /// <returns></returns>
        public bool ValidPoint(Vector3 hitPosition, MapSet set)
        {
            if (set == null)
            {
                return false;
            }
            return set.PointInMap(new IntVector3(Mathf.FloorToInt(hitPosition.x * 100f), (int)(hitPosition.y * 100f), Mathf.FloorToInt(hitPosition.z * 100f)));
        }

        /// <summary>
        /// Checks whether a point is valid on a any map
        /// </summary>
        /// <param name="hitPosition"></param>
        /// <param name="set"></param>
        /// <returns></returns>
        public bool ValidPoint(Vector3 hitPosition, out MapSet HitMap)
        {
            HitMap = null;

            foreach(MapSet Map in MapSets)
            {
                if (Map.PointInMap(new IntVector3(Mathf.FloorToInt(hitPosition.x * 100f), (int)(hitPosition.y * 100f), Mathf.FloorToInt(hitPosition.z * 100f))))
                {
                    HitMap = Map;
                }
            }

            if (HitMap == null)
            {
                return false;
            }

            return true;
        }

        /// <summary>
        /// Checks whether a point is valid on a map
        /// </summary>
        /// <param name="hitPosition"></param>
        /// <param name="set"></param>
        /// <returns></returns>
        public bool ValidPoint(IntVector3 hitWorldTilePosition, MapSet set)
        {
            return set.PointInMap(hitWorldTilePosition);
        }

        /// <summary>
        /// Get the world tile index from a position
        /// </summary>
        /// <param name="hitPosition"></param>
        /// <returns></returns>
        public IntVector3 GetTileWorldPosition(Vector3 hitPosition)
        {
            return new IntVector3(Mathf.FloorToInt(hitPosition.x * 100), Mathf.FloorToInt(hitPosition.y * 100), Mathf.FloorToInt(hitPosition.z * 100));
        }

        /// <summary>
        /// Get the position world tile index from a world tile index
        /// </summary>
        /// <param name="hitPosition"></param>
        /// <returns></returns>
        public Vector3 GetTileWorldPosition(IntVector3 tilePosition)
        {
            return new Vector3(tilePosition.x / 100f, tilePosition.y / 100f, tilePosition.z / 100f);
        }

        /// <summary>
        /// Gets the tile index center position from a position
        /// </summary>
        /// <param name="hitRoughPosition"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public Vector3 GetTileCenterWorldPosition(Vector3 hitRoughPosition, MapSet Map)
        {
            IntVector3 GetTilePosition = GetTileWorldPosition(hitRoughPosition);
            IntVector2 GetTileIndex = Map.GetMapTileIndex(GetTilePosition);
            return GetTileCenterWorldPosition(GetTileIndex, Map);
        }

        /// <summary>
        /// Gets the tile index center position from a map tile index
        /// </summary>
        /// <param name="mapTileIndex"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public Vector3 GetTileCenterWorldPosition(IntVector2 mapTileIndex, MapSet Map)
        {
            return new Vector3(Map.SetSettings.MapSettings.Offset.x + mapTileIndex.x * Map.SetSettings.MapSettings.TileSize + Map.SetSettings.MapSettings.TileSize / 2f, Map.SetSettings.MapSettings.Offset.y, Map.SetSettings.MapSettings.Offset.z + mapTileIndex.y * Map.SetSettings.MapSettings.TileSize + Map.SetSettings.MapSettings.TileSize / 2f) / 100f;
        }

        /// <summary>
        /// Gets the tile index center position from a world tile index
        /// </summary>
        /// <param name="worldTileIndex"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public Vector3 GetTileCenterWorldPosition(IntVector3 worldTileIndex, MapSet Map)
        {
            IntVector3 FlattenedWorldTileIndex = Map.GetWorldTileIndex(Map.GetMapTileIndex(worldTileIndex), false); // Set it to the corner position
            return new Vector3(FlattenedWorldTileIndex.x + Map.SetSettings.MapSettings.TileSize / 2f, (int)FlattenedWorldTileIndex.y, FlattenedWorldTileIndex.z + Map.SetSettings.MapSettings.TileSize / 2f) / 100f;
        }

        /// <summary>
        /// Gets the world tile index center position from a world tile index
        /// </summary>
        /// <param name="worldTileIndex"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public Vector3 GetTileCenterWorldPosition(IntVector3 worldTileIndex)
        { 
            return new Vector3(Mathf.FloorToInt(worldTileIndex.x), (int)worldTileIndex.y, Mathf.FloorToInt(worldTileIndex.z)) / 100f;
        }

        /// <summary>
        /// Gets the tile index from a world tile index
        /// </summary>
        /// <param name="TileWorldIndex"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public IntVector2 GetTileIndexForMap(IntVector3 TileWorldIndex, MapSet Map)
        {
            if (Map == null)
            {
                return new IntVector2();
            }
            return Map.GetMapTileIndex(TileWorldIndex);
        }

        /// <summary>
        /// Gets the tile index from a position
        /// </summary>
        /// <param name="WorldPosition"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public IntVector2 GetTileIndexForMap(Vector3 WorldPosition, MapSet Map)
        {
            IntVector3 WorldTileIndex = GetTileWorldPosition(WorldPosition);
            return Map.GetMapTileIndex(WorldTileIndex);
        }

        /// <summary>
        /// Gets the tile type from a position
        /// </summary>
        /// <param name="WorldPosition"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public TileTypes GetTileTypeForMap(Vector3 WorldPosition, MapSet Map)
        {
            IntVector3 WorldTileIndex = GetTileWorldPosition(WorldPosition);
            return Map.GetTileType(Map.GetMapTileIndex(WorldTileIndex));
        }

        /// <summary>
        /// Gets the tile type from a tile index
        /// </summary>
        /// <param name="MapIndex"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public TileTypes GetTileTypeForMap(IntVector2 MapIndex, MapSet Map)
        {
            if (Map == null)
            {
                return TileTypes.OutOfBounds;
            }
            return Map.GetTileType(MapIndex);
        }

        /// <summary>
        /// Gets the tile type from a tile index
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public TileTypes GetTileTypeForMap(int x, int y, MapSet Map)
        {
            return Map.GetTileType(x, y);
        }

        /// <summary>
        /// Get the tile bounds of a map
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public TileBounds GetTileBoundsForMap(int x, int y, MapSet Map)
        {
            Vector3 TilePosition = GetTileWorldPosition(Map.GetWorldTileIndex(x, y));
            return new TileBounds(TilePosition, Map.SetSettings.MapSettings.TileSize / 100f);
        }

        /// <summary>
        /// Get the tile bounds of a map
        /// </summary>
        /// <param name="TileIndex"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public TileBounds GetTileBoundsForMap(IntVector2 TileIndex, MapSet Map)
        {
            Vector3 TilePosition = GetTileWorldPosition(Map.GetWorldTileIndex(TileIndex));
            return new TileBounds(TilePosition, Map.SetSettings.MapSettings.TileSize / 100f);
        }

        /// <summary>
        /// Get a random valid position in the scene
        /// </summary>
        /// <param name="ValidTypes"></param>
        /// <param name="PlaceAtCenter"></param>
        /// <returns></returns>
        public Vector3 GetRandomValidPosition(List<TileTypes> ValidTypes, bool PlaceAtCenter = false)
        {
            if (MapSets.Count > 0)
            {
                MapSet RandomMap = MapSets[RandomValidPosition.Next(0, MapSets.Count)];
                return GetRandomValidPosition(RandomMap, ValidTypes, PlaceAtCenter);
            }

            return new Vector3();
        }

        /// <summary>
        /// Get a random valid position oon this map
        /// </summary>
        /// <param name="ValidTypes"></param>
        /// <param name="PlaceAtCenter"></param>
        /// <param name="Map"></param>
        /// <returns></returns>
        public Vector3 GetRandomValidPosition(List<TileTypes> ValidTypes, MapSet Map, bool PlaceAtCenter = false)
        {
            return GetRandomValidPosition(Map, ValidTypes, PlaceAtCenter);
        }

        /// <summary>
        /// Get a random position in range of a position
        /// </summary>
        /// <param name="ValidTypes"></param>
        /// <param name="Position"></param>
        /// <param name="Range"></param>
        /// <param name="PlaceAtCenter"></param>
        /// <returns></returns>
        public Vector3 GetRandomValidPositionAtPosition(List<TileTypes> ValidTypes, IntVector2 Position, int Range = 30, bool PlaceAtCenter = false)
        {
            if (MapSets.Count > 0)
            {
                MapSet RandomMap = MapSets[RandomValidPosition.Next(0, MapSets.Count - 1)];
                return GetRandomValidPositionAtPosition(RandomMap, ValidTypes, Position, Range, PlaceAtCenter);
            }

            return new Vector3();
        }

        protected System.Random RandomValidPosition = new System.Random(33399927);
        /// <summary>
        /// get a random valid position for a map
        /// </summary>
        /// <param name="Map"></param>
        /// <param name="ValidTypes"></param>
        /// <param name="PlaceAtCenter"></param>
        /// <returns></returns>
        public Vector3 GetRandomValidPosition(MapSet Map, List<TileTypes> ValidTypes, bool PlaceAtCenter)
        {
            if (Map == null)
            {
                throw new Exception($"Map Is Null");
            }

            if (Map.SetSettings == null)
            {
                throw new Exception($"1. Map Settings Is Null");
            }

            if (Map.SetSettings.MapSettings == null)
            {
                throw new Exception($"2. Map Settings Is Null");
            }

            IntVector2 RandomIndex = new IntVector2(RandomValidPosition.Next(0, Map.SetSettings.MapSettings.MapWidth - 1), RandomValidPosition.Next(0, Map.SetSettings.MapSettings.MapHeight - 1));
            IntVector3 WorldIndex;

            if(!ValidTypes.Contains(Map.GetTileType(RandomIndex)))
            {
                int CurrentRadius = 0;
                IntVector2 NewIndex;
                while (!ValidTypes.Contains(Map.GetTileType(RandomIndex)) && CurrentRadius < 103)
                {
                    CurrentRadius++;

                    NewIndex = RandomIndex;
                    NewIndex.x += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.y += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.y -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x += CurrentRadius;
                    NewIndex.y += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x -= CurrentRadius;
                    NewIndex.y += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x -= CurrentRadius;
                    NewIndex.y -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x += CurrentRadius;
                    NewIndex.y -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }
                }
            }

            WorldIndex = Map.GetWorldTileIndex(RandomIndex, false);
            if (PlaceAtCenter)
            {
                Vector3 RandomPosition = new Vector3(WorldIndex.x + Map.SetSettings.MapSettings.TileSize / 2f, WorldIndex.y, WorldIndex.z + Map.SetSettings.MapSettings.TileSize / 2f) / 100f;
                //Debug.Log($"Random Index ({RandomIndex.x}, {RandomIndex.y}) -> World Index ({WorldIndex.x}, {WorldIndex.y}, {WorldIndex.z}) -> World Position ({RandomPosition.x}, {RandomPosition.y}, {RandomPosition.z})");
                return RandomPosition;
            }

            return new Vector3(WorldIndex.x, WorldIndex.y, WorldIndex.z) / 100f;
        }

        /// <summary>
        /// Get a random valid position at a position within a range
        /// </summary>
        /// <param name="Map"></param>
        /// <param name="ValidTypes"></param>
        /// <param name="Position"></param>
        /// <param name="Range"></param>
        /// <param name="PlaceAtCenter"></param>
        /// <returns></returns>
        public Vector3 GetRandomValidPositionAtPosition(MapSet Map, List<TileTypes> ValidTypes, IntVector2 Position, int Range = 30, bool PlaceAtCenter = false)
        {
            IntVector2 LowBound = Position - new IntVector2(Range, Range);
            if (LowBound.x < 0)
            {
                LowBound.x = 0;
            }
            if (LowBound.y < 0)
            {
                LowBound.y = 0;
            }
            IntVector2 HighBound = Position + new IntVector2(Range, Range);
            if (HighBound.x >= Map.SetSettings.MapSettings.MapWidth)
            {
                HighBound.x = Map.SetSettings.MapSettings.MapWidth - 1;
            }
            if (HighBound.y >= Map.SetSettings.MapSettings.MapHeight)
            {
                HighBound.y = Map.SetSettings.MapSettings.MapHeight - 1;
            }
            IntVector2 RandomIndex = new IntVector2(RandomValidPosition.Next(LowBound.x, HighBound.x), RandomValidPosition.Next(LowBound.y, HighBound.y));
            IntVector3 WorldIndex;

            if (!ValidTypes.Contains(Map.GetTileType(RandomIndex)))
            {
                int CurrentRadius = 0;
                IntVector2 NewIndex;
                while (!ValidTypes.Contains(Map.GetTileType(RandomIndex)) && CurrentRadius < 103)
                {
                    CurrentRadius++;

                    NewIndex = RandomIndex;
                    NewIndex.x += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.y += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.y -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x += CurrentRadius;
                    NewIndex.y += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x -= CurrentRadius;
                    NewIndex.y += CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x -= CurrentRadius;
                    NewIndex.y -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }

                    NewIndex = RandomIndex;
                    NewIndex.x += CurrentRadius;
                    NewIndex.y -= CurrentRadius;
                    if (ValidTypes.Contains(Map.GetTileType(NewIndex)))
                    {
                        RandomIndex = NewIndex;
                        break;
                    }
                }
            }

            WorldIndex = Map.GetWorldTileIndex(RandomIndex, false);
            if (PlaceAtCenter)
            {
                Vector3 RandomPosition = new Vector3(WorldIndex.x + Map.SetSettings.MapSettings.TileSize / 2f, WorldIndex.y, WorldIndex.z + Map.SetSettings.MapSettings.TileSize / 2f) / 100f;
                //Debug.Log($"Random Index ({RandomIndex.x}, {RandomIndex.y}) -> World Index ({WorldIndex.x}, {WorldIndex.y}, {WorldIndex.z}) -> World Position ({RandomPosition.x}, {RandomPosition.y}, {RandomPosition.z})");
                return RandomPosition;
            }

            return new Vector3(WorldIndex.x, WorldIndex.y, WorldIndex.z) / 100f;
        }

        public Vector3 GetRandomPosition()
        {
            if (MapSets.Count > 0)
            {
                MapSet RandomMap = MapSets[RandomValidPosition.Next(0, MapSets.Count)];
                return GetRandomPosition(RandomMap);
            }

            return new Vector3();
        }

        /// <summary>
        /// Get a random position on a map
        /// </summary>
        /// <param name="Map"></param>
        /// <returns></returns>
        public Vector3 GetRandomPosition(MapSet Map)
        {
            IntVector2 RandomIndex = new IntVector2(RandomValidPosition.Next(0, Map.SetSettings.MapSettings.MapWidth - 1), RandomValidPosition.Next(0, Map.SetSettings.MapSettings.MapHeight - 1));
            IntVector3 WorldIndex = Map.GetWorldTileIndex(RandomIndex);
            return new Vector3(WorldIndex.x, WorldIndex.y, WorldIndex.z);
        }

        /// <summary>
        /// Gets the best map from a position
        /// </summary>
        /// <param name="WorldPosition"></param>
        /// <returns></returns>
        public MapSet GetCurrentMap(Vector3 WorldPosition)
        {
            if (MapSets.Count <= 0)
            {
                return null;
            }

            IntVector3 WorltIileIndex = GetTileWorldPosition(WorldPosition);
            MapSet ClosestMap = null;
            int ClosestScore = int.MaxValue;
            foreach (MapSet map in MapSets)
            {
                if (map.PointInMap(WorltIileIndex))
                {
                    int NewScore = WorltIileIndex.y - map.SetSettings.MapSettings.Offset.y;
                    if (NewScore >= 0)
                    {
                        if (NewScore < ClosestScore)
                        {
                            ClosestScore = NewScore;
                            ClosestMap = map;
                        }
                    }
                }
            }

            return ClosestMap;
        }

        #endregion
    }

    [Serializable]
    public struct TileBounds
    {
        public Vector3 Center;
        public Vector3 BottomLeft;
        public Vector3 TopLeft;
        public Vector3 BottomRight;
        public Vector3 TopRight;

        public TileBounds(Vector3 TilePosition, float TileSize)
        {
            Center = TilePosition;
            Center.x += TileSize / 2;
            Center.z += TileSize / 2;

            TopRight = TilePosition;
            TopRight.x += TileSize;
            TopRight.z += TileSize; 

            BottomLeft = TilePosition;

            TopLeft = TilePosition;
            TopLeft.z += TileSize;

            BottomRight = TilePosition;
            BottomRight.x += TileSize;

            TopRight = TilePosition;
            TopRight.x += TileSize;
            TopRight.z += TileSize;
        }

        public bool InBounds(Vector3 Point)
        {
            if (Point.x > TopRight.x || Point.x < BottomLeft.x || Point.z > TopRight.z || Point.z < BottomLeft.z)
            {
                return false;
            }

            return true;
        }

        public bool IntersectsBounds(Vector3 PointA, Vector3 PointB)
        {
            if (InBounds(PointA) || InBounds(PointB))
            {
                return true;
            }

            if (LineLineCollision(PointA, PointB, BottomRight, BottomLeft) || LineLineCollision(PointA, PointB, BottomRight, TopRight) || LineLineCollision(PointA, PointB, BottomLeft, TopLeft) || LineLineCollision(PointA, PointB, TopRight, TopLeft))
            {
                return true;
            }

            return false;
        }

        public static bool LineLineCollision(Vector3 Position1Start, Vector3 Position1End, Vector3 Position2Start, Vector3 Position2End)
        {
            // Directions
            float uA = ((Position2End.x - Position2Start.x) * (Position1Start.z - Position2Start.z) - (Position2End.z - Position2Start.z) * (Position1Start.x - Position2Start.x)) / ((Position2End.z - Position2Start.z) * (Position1End.x - Position1Start.x) - (Position2End.x - Position2Start.x) * (Position1End.z - Position1Start.z));
            float uB = ((Position1End.x - Position1Start.x) * (Position1Start.z - Position2Start.z) - (Position1End.z - Position1Start.z) * (Position1Start.x - Position2Start.x)) / ((Position2End.z - Position2Start.z) * (Position1End.x - Position1Start.x) - (Position2End.x - Position2Start.x) * (Position1End.z - Position1Start.z));


            if (uA >= 0 && uA <= 1 && uB >= 0 && uB <= 1)
            {
                return true;
            }

            return false;
        }
    }
}

namespace HPJ.Presentation.NavigationManagerSettings
{
    /// <summary>
    /// The Manager settings of the Navigation Manager
    /// </summary>
    [Serializable]
    public class ManagerSetting
    {
        /// <summary>
        /// The amount of redundant maps generated - More Maps = Faster Pathfinding at cost of memory
        /// </summary>
        [SerializeField]
        [Range(0, 8)]
        [Tooltip("The amount of redundant maps generated - More Maps = Faster Pathfinding at cost of memory")]
        public int MapRedundancy = 4;

        /// <summary>
        /// Setting used for parrellism for each maps pathfinding
        /// </summary>
        [SerializeField]
        [Range(2, 16)]
        [Tooltip("Setting used for parrellism for each maps pathfinding")]
        public int PathfindingParrellism = 4;

        /// <summary>
        /// The size of the grid of maps you want to create
        /// </summary>
        [Tooltip("The size of the grid of maps you want to create")]
        public Vector2Int MapAutoCreationGridSize = new Vector2Int(1, 1);

        /// <summary>
        /// The number of tiles in the x and z of the generated grid maps
        /// </summary>
        [Tooltip("The number of tiles in the x and z of the generated grid maps")]
        public Vector2Int MapAutoGridTileWidthAndHeight = new Vector2Int(1, 1);

        /// <summary>
        /// The physical size of the autogenerated tiles on the generated maps
        /// </summary>
        [Tooltip("The physical size of the autogenerated tiles on the generated maps")]
        public int MapAutoGridTileSize = 100;

        /// <summary>
        /// The physical offset of the bottom left map
        /// </summary>
        [Tooltip("The physical offset of the bottom left map")]
        public IntVector3 MapAutoGridOffset = new IntVector3();

        /// <summary>
        /// Whether to create seams for each connected side of the autogrid
        /// </summary>
        [Tooltip("Whether to create seams for each connected side of the autogrid")]
        public bool MapAutoGridAutoSeam = true;

        /// <summary>
        /// The filepath for the save data for this scene
        /// </summary>
        [Tooltip("The filepath for the save data")]
        public string MapSaveDataFolder = "None";

        /// <summary>
        /// Extra Simulation Settings
        /// </summary>
        public ExtraSimulationSettings SimulationSettings = new ExtraSimulationSettings();
    }

    /// <summary>
    /// The Painter settings, used to paint tiles on the Maps
    /// </summary>
    [Serializable]
    public class PainterSetting
    {
        /// <summary>
        /// The Paint Brush Size
        /// </summary>
        [Range(0, 500)]
        public int CurrentPainterRadius = 1;
        /// <summary>
        /// The Tile Types we want to paint
        /// </summary>
        public TileTypes CurrentPainterType = TileTypes.Blocked;
        /// <summary>
        /// The Paint Brush Type
        /// </summary>
        public PainterTypes PainterType = PainterTypes.Square;
        /// <summary>
        /// Which Map do you want to clean (Change to Default Tile)
        /// </summary>
        [Tooltip("Which Map do you want to clean (Change to Default Tile)")]
        public int CleanMapIndex = 0;

        [NonSerialized] public MeshRenderer SquarePaintBrush = null;
        [NonSerialized] public MeshRenderer CirclePaintBrush = null;
        [NonSerialized] public MeshFilter SquareFilter = null;
        [NonSerialized] public MeshFilter CircleFilter = null;
    }

    /// <summary>
    /// The Auto-Painter settings, used to paint tiles on the Maps automatically
    /// </summary>
    [Serializable]
    public class AutoPainterSetting
    {
        /// <summary>
        /// What Tile Type the Auto painter turns the Tile Into
        /// </summary>
        [Tooltip("What Tile Type the Auto painter turns the Tile Into")]
        public TileTypes AutoPainterType = TileTypes.Water;
        /// <summary>
        /// What Layers the Raycast must HIT to turn that tile
        /// </summary>
        [Tooltip("What Layers the Raycast must HIT to turn that tile")]
        public LayerMask AutoPainterCheckForLayers;
        /// <summary>
        /// What Layers the Raycast must AVOID HITTING to turn that tile
        /// </summary>
        [Tooltip("What Layers the Raycast must AVOID HITTING to turn that tile")]
        public LayerMask AutoPainterCheckAgainstLayers;
        /// <summary>
        /// "How much the Raycast will increase its starting position from the height of the Map (Offset Y)"
        /// </summary>
        [Tooltip("How much the Raycast will increase its starting position from the height of the Map (Offset Y)")]
        public float AutoPainterElavationIncrease = 20;
        /// <summary>
        /// How much the Raycast will go below the height of the Map (Offset Y)
        /// </summary>
        [Tooltip("How much the Raycast will go below the height of the Map (Offset Y)")]
        public float AutoPainterElavationIDecrease = 100;
        /// <summary>
        /// Which Map do you want to Auto Paint
        /// </summary>
        [Tooltip("Which Map do you want to Auto Paint")]
        public int AutoPaintMapIndex = 0;
        /// <summary>
        /// How high above the map, where the tiles change type
        /// </summary>
        [Tooltip("How high above the map, where the tiles change type")]
        public float AboveMapTypeChangeHeight = 10;
        /// <summary>
        /// What Tile Type the Auto painter turns the Tile Into on height check above
        /// </summary>
        [Tooltip("What Tile Type the Auto painter turns the Tile Into on height check above")]
        public TileTypes AutoPainterAboveHeightType = TileTypes.Blocked;
        /// <summary>
        /// How far below the map, where the tiles change type
        /// </summary>
        [Tooltip("How far below the map, where the tiles change type")]
        public float BelowMapTypeChangeHeight = 10;
        /// <summary>
        /// What Tile Type the Auto painter turns the Tile Into on height check below
        /// </summary>
        [Tooltip("What Tile Type the Auto painter turns the Tile Into on height check below")]
        public TileTypes AutoPainterBelowHeightType = TileTypes.Blocked;
    }

    /// <summary>
    /// The all seam settings
    /// </summary>
    [Serializable]
    public class SeamSettings
    {
        /// <summary>
        /// Whether to display seams
        /// </summary>
        public bool DisplaySeams = false;

        /// <summary>
        /// Map 1 or A Seam Insert Settings
        /// </summary>
        public MapSeamInsertSettings MapAInsertSettings = new MapSeamInsertSettings();

        /// <summary>
        /// Map 2 or B Seam Insert Settings
        /// </summary>
        public MapSeamInsertSettings MapBInsertSettings = new MapSeamInsertSettings();
    }

    /// <summary>
    /// The map seam settings
    /// </summary>
    [Serializable]
    public class MapSeamInsertSettings
    {
        /// <summary>
        /// Map Index
        /// </summary>
        public int MapIndex = -1;

        /// <summary>
        /// Map Index
        /// </summary>
        public SeamConnectionSide ConnectionSide =  SeamConnectionSide.East;
    }

    /// <summary>
    /// Extra Simulation Settings
    /// </summary>
    [Serializable]
    public class ExtraSimulationSettings
    {
        /// <summary>
        /// Whether to use another thread for extra simulation. Used by Simulation Agents and Agent Avoidance
        /// </summary>
        public bool UseExtraSimulation = false;

        /// <summary>
        /// Whether to use agent avoidence for the simulation agents
        /// </summary>
        public bool UseAgentAvoidance = false;

        /// <summary>
        /// The Desired tickrate of the helper update
        /// </summary>
        public int DesiredHelperTickrate = 60;
    }

    /// <summary>
    /// The Gizmos settings of the Navigation Manager
    /// </summary>
    [Serializable]
    public class GizmosSetting
    {
        /// <summary>
        /// Whether to show the Gizmos
        /// </summary>
        [Header("GUI")]
        public bool DisplayGizmos = true;
        /// <summary>
        /// Whether to diplay the bridges
        /// </summary>
        public bool DisplayBridges = false;
        /// <summary>
        /// Shows the direction the bridge allows. Nothing for both ways, otherwise direction is from Red to Blue
        /// </summary>
        public bool DisplayBridgeDirections = false;
        /// <summary>
        /// The Bridge we want in inset a midpoint on
        /// </summary>
        public int InsertBridgePointIndex = -1;
        /// <summary>
        /// Whether to display tile grid
        /// </summary>
        public bool DisplayTiles = false;
        /// <summary>
        /// Whether to display transform handle for the map
        /// </summary>
        public bool DisplayMapHandles = true;
        /// <summary>
        /// Whether to display map nale label for the map
        /// </summary>
        public bool DisplayMapLabels = true;
        /// <summary>
        /// Whether to display the base of the Maps
        /// </summary>
        public bool DisplayMapBase = true;
        /// <summary>
        /// Whether to display the Map Edge
        /// </summary>
        public bool DisplayMapEdge = true;
        /// <summary>
        /// The Alpha of the Map Base and Edge
        /// </summary>
        [Range(0, 1)]
        public float DisplayMapAlpha = 1;
        /// <summary>
        /// Whether to display the Map arrows on the Maps
        /// </summary>
        public bool DisplayMapArrows = true;
        /// <summary>
        /// Adjusts the Map Arrow Size
        /// </summary>
        public float DisplayArrowSize = 12;
        /// <summary>
        /// Whether to display the paint brush or not
        /// </summary>
        public bool DisplayPaintBrush = false;
        /// <summary>
        /// Whether to debug the paint brush position
        /// </summary>
        public bool DebugPaintBrushPosition = false;
        /// <summary>
        /// Whether to display the painted tiles. Painter and Auto-Painter
        /// </summary>
        public bool DisplayPaintedTiles = false;
        /// <summary>
        /// The Tiles we want to see 
        /// </summary>
        public List<TileTypes> OnlyPaintTheseTypes = new List<TileTypes>();
        /// <summary>
        /// The Map index we want to show the painted tiles on. -1 for all
        /// </summary>
        [Tooltip("Which Map do you want the tiles rendered, (-1 for All)")]
        public int DisplayPaintedTilesMapIndex = -1;

        /// <summary>
        /// limits the amount of gizmos on screen by the desired map indecies. a empty list means every map
        /// </summary>
        public List<int> DisplayMapGizmos_ForMapIndecies = new List<int>();

        /// <summary>
        /// Whetehr to display the tiles owned by agents
        /// </summary>
        public bool DisplayAgentOwnedTiles = false;
    }
}