using HPJ.Simulation;
using HPJ.Simulation.Enums;
using HPJ.Simulation.Map;
using HPJ.Simulation.Pathing;
using HPJ.Simulation.Utilities;
using System;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Events;

namespace HPJ.Presentation.Agents
{
    /// <summary>
    /// Navigation Agents are used to move and pathfind from its transform.position to a destination
    /// </summary>
    public class NavigationAgent : MonoBehaviour
    {
        #region Unity

        [SerializeField]
        protected AgentSettings _settings = new AgentSettings();
        /// <summary>
        /// The Agent Settings
        /// </summary>
        public AgentSettings Settings { get { return _settings; } }

        protected AgentStates _state;
        /// <summary>
        /// The Current State of the Agent
        /// </summary>
        public AgentStates State { get { return _state; } internal set { if (value != _state) { _previousState = _state; } _state = value; } }
        protected AgentStates _previousState;
        /// <summary>
        /// The Current State of the Agent
        /// </summary>
        public AgentStates PreviousState { get { return _previousState; } }
        /// <summary>
        /// The Current Navigation Order of the Agent
        /// </summary>
        public NavigationOrder CurrentOrder { get; protected set; }
        /// <summary>
        /// The Current Map the Agent is On. Can be NULL
        /// </summary>
        public MapSet CurrentMap { get; internal set; }
        /// <summary>
        /// The Starting Map the Agent is started On. Can be NULL
        /// </summary>
        public MapSet StartingMap { get; internal set; }

        /// <summary>
        /// The Current Path of the Agent
        /// </summary>
        public List<Vector3> Path { get; internal set; }

        /// <summary>
        /// The Destination this agent wants to move to
        /// </summary>
        public Vector3 WantedDestination { get; set; }

        /// <summary>
        /// The Ground Cast Component on the gameobject
        /// </summary>
        public AgentGroundCast GroundCast { get; internal set; }

        [SerializeField]
        protected Transform _rotateTransform = null;

        protected virtual void Awake()
        {
            WantedDestination = transform.position;
            if (_rotateTransform == null)
            {
                _rotateTransform = transform;
            }

            State = AgentStates.Idle;
            _previousState = _state;
            Path = new List<Vector3>();
            GroundCast = GetComponent<AgentGroundCast>();
            CurrentOrder = new NavigationOrder(this);
            TraversableTilesKey = TraversableTiles.TilesToString();
            PrefferedTilesKey = PrefferedTiles.TilesToString();

            Invoke(nameof(AddToManager), 0.1f);
        }

        /// <summary>
        /// Adds the Agent to the Navigation Manager
        /// </summary>
        protected virtual void AddToManager()
        {
            if (NavigationManager.Instance == null)
            {
                Invoke(nameof(AddToManager), 0.1f);
                return;
            }

            if (!NavigationManager.Instance)
            {
                Invoke(nameof(AddToManager), 0.1f);
                return;
            }

            if (NavigationManager.Instance.MapSets.Count <= 0)
            {
                return;
            }

            IntVector3 StartPosition = NavigationManager.Instance.GetTileWorldPosition(transform.position);
            CurrentMap = NavigationManager.Instance.GetMapAtTileWorldPosition(StartPosition);
            StartingMap = NavigationManager.Instance.GetMapAtTileWorldPosition(StartPosition);

            NavigationManager.Instance?.AddAgent(this);
        }

        /// <summary>
        /// Sets the current map of the agent, if its not on the map, it will move to the nearest point on this map
        /// </summary>
        /// <param name="map"></param>
        public void SetCurrentMap(MapSet map)
        {
            MapSet currentMap = NavigationManager.Instance.GetCurrentMap(transform.position);
            if (map != currentMap)
            {
                IntVector3 CurrentPosition = NavigationManager.Instance.GetTileWorldPosition(transform.position);
                IntVector3 ClosestPosition = map.GetClosestPoint(CurrentPosition);
                Vector3 TileCenter = NavigationManager.Instance.GetTileCenterWorldPosition(ClosestPosition, map);
                transform.position = TileCenter;
            }

            if (StartingMap == null)
            {
                StartingMap = map;
            }
        }

        /// <summary>
        /// Finds a random valid point within a range of the agents position
        /// </summary>
        /// <param name="randomRange"></param>
        /// <returns></returns>
        internal virtual IntVector3 GetRandomDestination(int randomRange)
        {
            IntVector3 RandomDestination = NavigationManager.Instance.GetTileWorldPosition(transform.position);

            if (CurrentMap == null)
            {
                return RandomDestination;
            }

            int tries = 0;
            while (tries < 30)
            {
                tries++;
                int Variance = randomRange * CurrentMap.SetSettings.MapSettings.TileSize;
                int RandomVarianceX = UnityEngine.Random.Range(-Variance, Variance + 1);
                int RandomVarianceY = UnityEngine.Random.Range(-Variance, Variance + 1);

                IntVector3 RandomPoint = RandomDestination + new IntVector3(RandomVarianceX, 0, RandomVarianceY);
                if (NavigationManager.Instance.ValidPoint(RandomPoint, CurrentMap))
                {
                    if (TraversableTiles.Contains(CurrentMap.GetTileType(CurrentMap.GetMapTileIndex(RandomPoint))))
                    {
                        RandomDestination = RandomPoint;
                        break;
                    }
                }
            }

            return RandomDestination;
        }

        /// <summary>
        /// Finds a random valid point away the wanted location
        /// </summary>
        /// <param name="hitPoint"></param>
        /// <param name="randomTileDistanceVariance"></param>
        /// <returns></returns>
        internal virtual IntVector3 GetRandomDestinationAwayFromPoint(Vector3 hitPoint, int randomTileDistanceVariance)
        {
            // Reverse Position
            hitPoint.x *= -1;
            hitPoint.z *= -1;

            IntVector3 RandomDestination = NavigationManager.Instance.GetTileWorldPosition(hitPoint);

            if (CurrentMap == null)
            {
                return RandomDestination;
            }

            int tries = 0;
            while (tries < 30)
            {
                tries++;
                int Variance = randomTileDistanceVariance * CurrentMap.SetSettings.MapSettings.TileSize;
                int RandomVarianceX = UnityEngine.Random.Range(-Variance, Variance + 1);
                int RandomVarianceY = UnityEngine.Random.Range(-Variance, Variance + 1);

                IntVector3 RandomPoint = RandomDestination + new IntVector3(RandomVarianceX, 0, RandomVarianceY);
                if (NavigationManager.Instance.ValidPoint(RandomPoint, CurrentMap))
                {
                    if (TraversableTiles.Contains(CurrentMap.GetTileType(CurrentMap.GetMapTileIndex(RandomPoint))))
                    {
                        RandomDestination = RandomPoint;
                        break;
                    }
                }
            }

            return RandomDestination;
        }

        /// <summary>
        /// Finds a random valid point near the wanted location
        /// </summary>
        /// <param name="hitPoint"></param>
        /// <param name="randomTileDistanceVariance"></param>
        /// <returns></returns>
        public virtual IntVector3 GetRandomDestinationNearPoint(Vector3 hitPoint, int randomTileDistanceVariance)
        {
            IntVector3 RandomDestination = NavigationManager.Instance.GetTileWorldPosition(hitPoint);

            if (CurrentMap == null)
            {
                return RandomDestination;
            }

            int tries = 0;
            while(tries < 30)
            {
                tries++;
                int Variance = randomTileDistanceVariance * CurrentMap.SetSettings.MapSettings.TileSize;
                int RandomVarianceX = UnityEngine.Random.Range(-Variance, Variance + 1);
                int RandomVarianceY = UnityEngine.Random.Range(-Variance, Variance + 1);

                IntVector3 RandomPoint = RandomDestination + new IntVector3(RandomVarianceX, 0, RandomVarianceY);
                if (NavigationManager.Instance.ValidPoint(RandomPoint, CurrentMap))
                {
                    if (TraversableTiles.Contains(CurrentMap.GetTileType(CurrentMap.GetMapTileIndex(RandomPoint))))
                    {
                        RandomDestination = RandomPoint;
                        break;
                    }
                }
            }

            return RandomDestination;
        }

        protected virtual void OnDestroy()
        {
            NavigationManager.Instance?.RemoveAgent(this);
        }

        protected virtual void OnDrawGizmosSelected()
        {
            if (!Application.IsPlaying(gameObject))
            {
                return;
            }

            if (!GizmosSettings.ShowGizmos)
            {
                return;
            }

            if (GizmosSettings.ShowPath)
            {
                Gizmos.color = Color.red;
                for(int i = 1; i < Path.Count; i++)
                {
                    Gizmos.DrawLine(Path[i - 1], Path[i]);
                }
            }

            if (GizmosSettings.ShowStartAndEnd)
            {
                Gizmos.color = Color.blue;
                Vector3 Start = new Vector3(CurrentOrder.Start.x, CurrentOrder.Start.y, CurrentOrder.Start.z) / 100f;
                Gizmos.DrawSphere(Start + Vector3.up, 1);
                Gizmos.color = Color.red;
                Vector3 Destination = new Vector3(CurrentOrder.Destination.x, CurrentOrder.Destination.y, CurrentOrder.Destination.z) / 100f;
                Gizmos.DrawSphere(Destination + Vector3.up, 1);
            }

            if (GizmosSettings.ShowValidTilesInRange)
            {
                if (CurrentOrder.StartingMap != null)
                {
                    Gizmos.color = Color.yellow;
                    List<IntVector2> TraversableTiles = GetTraversableTilesInRange(GizmosSettings.ValidTileRange);

                    Vector3 SquareSize = Vector3.one * CurrentOrder.StartingMap.SetSettings.MapSettings.TileSize / 100;
                    SquareSize.x -= 0.05f;
                    SquareSize.y = 0.1f;
                    SquareSize.z -= 0.05f;
                    foreach (IntVector2 TileIndex in TraversableTiles)
                    {
                        Vector3 TilePosition = NavigationManager.Instance.GetTileCenterWorldPosition(TileIndex, CurrentOrder.StartingMap);
                        Gizmos.DrawCube(TilePosition, SquareSize);
                    }
                }
            }

            if (GizmosSettings.ShowTraverableRange)
            {
                if (CurrentOrder.StartingMap != null)
                {
                    Gizmos.color = Color.green;
                    List<IntVector2> TraversableTiles = GetTraversableTilesInRangeBySteps(GizmosSettings.TraversableRange);

                    Vector3 SquareSize = Vector3.one * CurrentOrder.StartingMap.SetSettings.MapSettings.TileSize / 100;
                    SquareSize.x -= 0.05f;
                    SquareSize.y = 0.1f;
                    SquareSize.z -= 0.05f;
                    foreach (IntVector2 TileIndex in TraversableTiles)
                    {
                        Vector3 TilePosition = NavigationManager.Instance.GetTileCenterWorldPosition(TileIndex, CurrentOrder.StartingMap);
                        Gizmos.DrawCube(TilePosition, SquareSize);
                    }
                }
            }

            if (GizmosSettings.ShowClosestValidTileToWantedPosition)
            {
                if (NavigationManager.Instance != null && CurrentMap != null && !(this is SimNavigationAgent))
                {
                    Gizmos.color = Color.cyan;
                    Vector3 SquareSize = Vector3.one * CurrentMap.SetSettings.MapSettings.TileSize / 100;
                    SquareSize.x -= 0.05f;
                    SquareSize.y = 0.1f;
                    SquareSize.z -= 0.05f;

                    IntVector2 WantedIndex = NavigationManager.Instance.GetTileIndexForMap(WantedDestination, CurrentMap);
                    WantedIndex = CurrentMap.GetClosestValidIndex(WantedIndex, _settings.TraversableTiles);

                    Vector3 TilePosition = NavigationManager.Instance.GetTileCenterWorldPosition(WantedIndex, CurrentOrder.StartingMap);
                    Gizmos.DrawCube(TilePosition, SquareSize);
                }
            }
        }

        #endregion

        #region Agent

        /// <summary>
        /// The Movement Speed of the Agent
        /// </summary>
        public float MovementSpeed { get { return _settings.MovementSpeed; } set { _settings.MovementSpeed = value;  } }
        /// <summary>
        /// If the Agent should validate its current path "Local Avoidance"
        /// </summary>
        public bool AutoValidateCurrentPath { get { return _settings.AutoValidateCurrentPath; } set { _settings.AutoValidateCurrentPath = value; } }
        /// <summary>
        /// The auto validation interval
        /// </summary>
        public float AutoValidateTime { get { return _settings.AutoValidateTime; } set { _settings.AutoValidateTime = value; } }
        /// <summary>
        /// How Far the local avoidance will check for
        /// </summary>
        public int LocalAvoidanceRange { get { return _settings.LocalAvoidanceRange; } set { _settings.LocalAvoidanceRange = value; } }
        /// <summary>
        /// If the agent should validate the tile the agent is currently on
        /// </summary>
        public bool ValidateCurrentTile { get { return _settings.ValidateCurrentTile; } set { _settings.ValidateCurrentTile = value; } }
        /// <summary>
        /// If the agent should try and recalculate its path if it fails during the movement of the agent. like during "Local Avoidance"
        /// </summary>
        public bool KeepAttemptingOnFail { get { return _settings.KeepAttemptingOnFail; } set { _settings.KeepAttemptingOnFail = value; } }
        /// <summary>
        /// How often the agent should keep trying to recalculate its path
        /// </summary>
        public float KeepAttemptingTime { get { return _settings.KeepAttemptingTime; } set { _settings.KeepAttemptingTime = value; } }
        /// <summary>
        /// The navigation type this agent uses
        /// </summary>
        public NavigationTypes NavigationType { get { return _settings.NavigationType; } }
        /// <summary>
        /// The Traversable tile types this agent uses
        /// </summary>
        public List<TileTypes> TraversableTiles { get { return _settings.TraversableTiles; } set { lock (_settings.TraversableTiles) { _settings.TraversableTiles = value; TraversableTilesKey = _settings.TraversableTiles.TilesToString(); } } }
        /// <summary>
        /// The Tiles this agent preffers during pathfinding. Only used for pathfinding types that uses it. Like A*. Not JPS
        /// </summary>
        public List<TileTypes> PrefferedTiles { get { return _settings.PrefferedTiles; } set { lock (_settings.PrefferedTiles) { _settings.PrefferedTiles = value; PrefferedTilesKey = _settings.PrefferedTiles.TilesToString(); } } }
        /// <summary>
        /// The Key for the traversable tile types used to get Byte Maps
        /// </summary>
        public string TraversableTilesKey { get; set; }
        public string PrefferedTilesKey { get; set; }

        /// <summary>
        /// Update is called once per frame
        /// </summary>
        public virtual void AutoUpdateAgent()
        {
            if (_settings.AutomaticUpdate)
            {
                UpdateAgentState();
                if (_settings.GroundCheck)
                {
                    GroundCast?.PhysicsCheck();
                }
                if (_settings.RotateTowardsMovingDirection)
                {
                    if (State == AgentStates.Moving)
                    {
                        if (CurrentDirection != Vector3.zero)
                        {
                            if (_settings.RotateSpeed <= 0)
                            {
                                _rotateTransform.forward = _currentDirection;
                            }
                            else
                            {
                                _rotateTransform.forward = Vector3.Slerp(_rotateTransform.forward, _currentDirection, Settings.RotateSpeed * Time.deltaTime);
                            }
                        }
                    }
                }
            }
        }

        protected int _currentMovementPathIndex;
        /// <summary>
        /// Current Path Index the agent is on
        /// </summary>
        public int CurrentPathingIndex { get { return _currentMovementPathIndex; } internal set { _currentMovementPathIndex = value; } }

        protected Vector3 _currentDirection;
        public float _lerpPercentage { get; internal set; }
        public float _lerpTotal { get; internal set; }
        public float _autoValidateTimer { get; internal set; }
        public float _keepAttemptingTimer { get; internal set; }
        public TileTypes CurrentTileType { get; internal set; }
        public IntVector2 CurrentTileIndex { get; internal set; }

        /// <summary>
        /// The Current Direction the agent is going on
        /// </summary>
        public Vector3 CurrentDirection { get { return _currentDirection; } internal set { _currentDirection = value; } }

        /// <summary>
        /// If the agent failed its last pathfinding
        /// </summary>
        public bool FailedLastPath { get; protected set; }

        /// <summary>
        /// Updates the Agent
        /// </summary>
        public virtual void UpdateAgentState()
        {
            if (State == AgentStates.Moving)
            {
                _lerpPercentage += Time.deltaTime * MovementSpeed / _lerpTotal;
                transform.position = Vector3.Lerp(Path[_currentMovementPathIndex], Path[_currentMovementPathIndex + 1], _lerpPercentage);

                // Make sure the current tile for this agent is Valid
                if (ValidateCurrentTile)
                {
                    if (!ValidateStandingTile())
                    {
                        // This Path is invalid, so needs to be recalculated
                        if (CurrentOrder.NavJob.CurrentPath != null)
                        {
                            lock (CurrentOrder.NavJob.CurrentPath)
                            {
                                CurrentOrder.NavJob.CurrentPath.ValidPath = false;
                            }
                        }

                        // Move to closest Valid Tile and repath
                        CurrentTileIndex = CurrentOrder.StartingMap.GetClosestValidIndex(CurrentTileIndex, TraversableTiles);
                        CurrentTileType = NavigationManager.Instance.GetTileTypeForMap(CurrentTileIndex, CurrentOrder.StartingMap);
                        transform.position = NavigationManager.Instance.GetTileCenterWorldPosition(CurrentTileIndex, CurrentOrder.StartingMap);
                        SetDestination(CurrentOrder.Destination);
                        return;
                    }
                }

                if (_lerpPercentage >= 1f)
                {
                    // Go to NEXT index
                    _currentMovementPathIndex++;
                    transform.position = Path[_currentMovementPathIndex];
                    if (_currentMovementPathIndex >= Path.Count - 1)
                    {
                        // Completed
                        if (CurrentOrder.Destination == CurrentOrder.CheckPointDestination)
                        {
                            //Debug.Log("At Destination");
                            OnMovingComplete?.Invoke(this);
                            State = AgentStates.Idle;
                        }
                        // Find Destination 
                        else
                        {
                            //Debug.Log("At Current Destination");
                            SetDestination(CurrentOrder.Destination);
                        }
                    }
                    else
                    {
                        _currentDirection = Path[_currentMovementPathIndex + 1] - Path[_currentMovementPathIndex];
                        _currentDirection.y = 0;
                        _lerpPercentage = 0;
                        _lerpTotal = _currentDirection.magnitude;
                    }
                }

                // Local Advoidance
                if (AutoValidateCurrentPath)
                {
                    _autoValidateTimer += Time.deltaTime;
                    if (_autoValidateTimer > AutoValidateTime)
                    {
                        // By this point the pathing may be over
                        if (State == AgentStates.Moving)
                        {
                            if (!CurrentPathIsValid(LocalAvoidanceRange))
                            {
                                _autoValidateTimer = 0;
                                if (CurrentOrder.NavJob.CurrentPath != null)
                                {
                                    lock(CurrentOrder.NavJob.CurrentPath)
                                    {
                                        CurrentOrder.NavJob.CurrentPath.ValidPath = false;
                                    }
                                }
                                SetDestination(CurrentOrder.Destination);
                            }
                        }
                    }
                }
            }
            else if (CurrentOrder.PathfindingStep == PathfindingCalculationStep.Complete)
            {
                RecievePathingInfoCallback();
            }
            else if (State != AgentStates.Paused && State != AgentStates.Stopped)
            {
                // Keep the agent in a valid position
                if (CurrentOrder.StartingMap == null)
                {
                    CurrentOrder.StartingMap = NavigationManager.Instance.GetCurrentMap(transform.position);
                    CurrentMap = CurrentOrder.StartingMap;
                }
                if (CurrentOrder.StartingMap == null)
                {
                    return;
                }

                if (ValidateCurrentTile)
                {
                    if (!ValidateStandingTile())
                    {
                        // Move to closest Valid Tile and repath
                        CurrentTileIndex = CurrentOrder.StartingMap.GetClosestValidIndex(CurrentTileIndex, TraversableTiles);
                        CurrentTileType = NavigationManager.Instance.GetTileTypeForMap(CurrentTileIndex, CurrentOrder.StartingMap);
                        transform.position = NavigationManager.Instance.GetTileCenterWorldPosition(CurrentTileIndex, CurrentOrder.StartingMap);
                    }
                }

                if (FailedLastPath)
                {
                    if (KeepAttemptingOnFail)
                    {
                        _keepAttemptingTimer += Time.deltaTime;
                        if (_keepAttemptingTimer >= KeepAttemptingTime)
                        {
                            _keepAttemptingTimer = 0;
                            if (Settings.KeepAttemptingOnFail)
                            {
                                ReadjustToNearestPoint();
                            }
                            else
                            {                                
                                SetDestination(WantedDestination);
                            }
                        }
                    }
                }
            }

        }

        /// <summary>
        /// Stops the agent, continue makes this go into the idle state
        /// </summary>
        public virtual void Stop()
        {
            State = AgentStates.Stopped;
            CurrentOrder.Clear();
        }

        /// <summary>
        /// Stops the Agent, Can be continued
        /// </summary>
        public virtual void Pause()
        {
            if (State != AgentStates.Stopped && State != AgentStates.Idle)
            {
                State = AgentStates.Paused;
            }
        }

        /// <summary>
        /// Makes the agent continue its path or stop being stopped (Paused to Moving or Stopped to Idle)
        /// </summary>
        public virtual void Continue()
        {
            if (State == AgentStates.Paused)
            {
                State = AgentStates.Moving;
            }
            else if (State == AgentStates.Stopped)
            {
                State = AgentStates.Idle;
            }
        }

        /// <summary>
        /// Idles the Agent
        /// </summary>
        public virtual void Idle()
        {
            State = AgentStates.Idle;
            CurrentOrder.Clear();
        }

        /// <summary>
        /// Tells you if the current tile the unit is on is valid
        /// </summary>
        /// <returns></returns>
        public virtual bool ValidateStandingTile()
        {
            if (CurrentOrder.DestinationState == DestinationStates.NextMap_FromBridge || CurrentOrder.DestinationState == DestinationStates.NextMap_FromSeam)
            {
                return true;
            }

            if (CurrentMap != null)
            {
                CurrentTileIndex = NavigationManager.Instance.GetTileIndexForMap(transform.position, CurrentMap);
                CurrentTileType = NavigationManager.Instance.GetTileTypeForMap(CurrentTileIndex, CurrentMap);
                if (!TraversableTiles.Contains(CurrentTileType))
                {
                    return false;
                }
            }

            return true;
        }

        /// <summary>
        /// Checks the tiles in the path to see if they are still valid
        /// </summary>
        /// <param name="Range"></param>
        /// <returns></returns>
        public virtual bool CurrentPathIsValid(int Range = -1)
        {
            // You are on a bridge, so it will automatically be true
            if (CurrentOrder.DestinationState == DestinationStates.NextMap_FromBridge || CurrentOrder.DestinationState == DestinationStates.NextMap_FromSeam)
            {
                return true;
            }
            else if (CurrentOrder.DestinationState == DestinationStates.SameMap || CurrentOrder.DestinationState == DestinationStates.Bridge || CurrentOrder.DestinationState == DestinationStates.ClosestPoint)
            {
                // Reaffirm current Index
                if (!ValidateStandingTile())
                {
                    return false;
                }

                // This is to keep track of the path
                int validationPathIndex = _currentMovementPathIndex + 1;
                if (Range == -1)
                {
                    Range = int.MaxValue;
                }
                int RangeCounter = 0;

                // Calculate this section
                IntVector2 NextDestinationIndexPosition = NavigationManager.Instance.GetTileIndexForMap(Path[validationPathIndex], CurrentOrder.StartingMap);
                IntVector2 Direction = new IntVector2();
                if (CurrentTileIndex.x < NextDestinationIndexPosition.x)
                {
                    Direction.x = 1;
                }
                else if (CurrentTileIndex.x > NextDestinationIndexPosition.x)
                {
                    Direction.x = -1;
                }
                else
                {
                    Direction.x = 0;
                }
                if (CurrentTileIndex.y < NextDestinationIndexPosition.y)
                {
                    Direction.y = 1;
                }
                else if (CurrentTileIndex.y > NextDestinationIndexPosition.y)
                {
                    Direction.y = -1;
                }
                else
                {
                    Direction.y = 0;
                }

                // Set the tracking tile
                IntVector2 TrackingTile = CurrentTileIndex;
                while (TrackingTile != NextDestinationIndexPosition)
                {
                    TrackingTile += Direction;
                    if (!ValidateTileIndex(TrackingTile, CurrentOrder.StartingMap))
                    {
                        return false;
                    }

                    RangeCounter++;
                    if (RangeCounter > Range)
                    {
                        return true;
                    }
                }

                // Check the next sections of the path
                validationPathIndex++;
                while (validationPathIndex < Path.Count)
                {
                    NextDestinationIndexPosition = NavigationManager.Instance.GetTileIndexForMap(Path[validationPathIndex], CurrentOrder.StartingMap);
                    if (TrackingTile.x < NextDestinationIndexPosition.x)
                    {
                        Direction.x = 1;
                    }
                    else if (TrackingTile.x > NextDestinationIndexPosition.x)
                    {
                        Direction.x = -1;
                    }
                    else
                    {
                        Direction.x = 0;
                    }
                    if (TrackingTile.y < NextDestinationIndexPosition.y)
                    {
                        Direction.y = 1;
                    }
                    else if (TrackingTile.y > NextDestinationIndexPosition.y)
                    {
                        Direction.y = -1;
                    }
                    else
                    {
                        Direction.y = 0;
                    }

                    // Set the tracking tile
                    while (TrackingTile != NextDestinationIndexPosition)
                    {
                        TrackingTile += Direction;
                        if (!ValidateTileIndex(TrackingTile, CurrentOrder.StartingMap))
                        {
                            return false;
                        }

                        RangeCounter++;
                        if (RangeCounter > Range)
                        {
                            return true;
                        }
                    }

                    validationPathIndex++;
                }

                return true;
            }

            return false;

        }

        /// <summary>
        /// Used to have the Agent calculate its path and move towards this desintation
        /// </summary>
        /// <param name="Destination"></param>
        /// <param name="RecenterOnTile"></param>
        /// <param name="SetWantedDestination"></param>
        public virtual void SetDestination(Vector3 Destination, bool RecenterOnTile = false, bool SetWantedDestination = true)
        {
            //Debug.Log("Pathfinding");
            if (CurrentOrder.PathfindingStep == PathfindingCalculationStep.Pathfinding || State == AgentStates.Stopped)
            {
                return;
            }
            if (SetWantedDestination)
            {
                WantedDestination = Destination;
            }
            State = AgentStates.Pathfinding;
            CurrentOrder.SetPrevious();
            CurrentOrder.Clear();
            CurrentOrder.SetDestination(Destination, RecenterOnTile);
        }

        /// <summary>
        /// Used to have the Agent calculate its path and move towards this desintation
        /// </summary>
        /// <param name="WorldTileIndexDestination"></param>
        /// <param name="RecenterOnTile"></param>
        /// <param name="SetWantedDestination"></param>
        internal virtual void SetDestination(IntVector3 WorldTileIndexDestination, bool RecenterOnTile = false, bool SetWantedDestination = false)
        {
            if (CurrentOrder.PathfindingStep == PathfindingCalculationStep.Pathfinding || State == AgentStates.Stopped)
            {
                return;
            }

            if (SetWantedDestination)
            {
                WantedDestination = NavigationManager.Instance.GetTileWorldPosition(WorldTileIndexDestination);
            }

            State = AgentStates.Pathfinding;
            CurrentOrder.SetPrevious();
            CurrentOrder.Clear();
            CurrentOrder.SetDestination(WorldTileIndexDestination, RecenterOnTile);
        }

        /// <summary>
        /// Readjusts the adjust so it sets it destination to the nearest tile to it's destination that is valid
        /// </summary>
        public virtual void ReadjustToNearestPoint()
        {
            SetDestinationToNearestPoint(WantedDestination, true);
        }

        /// <summary>
        /// Sets the destination to the nearest valid point near the desired destination
        /// </summary>
        /// <param name="Destination"></param>
        /// <param name="RecenterOnTile"></param>
        public virtual void SetDestinationToNearestPoint(Vector3 Destination, bool RecenterOnTile = false)
        {
            if (CurrentOrder.PathfindingStep == PathfindingCalculationStep.Pathfinding || State == AgentStates.Stopped)
            {
                return;
            }
            State = AgentStates.Pathfinding;
            CurrentOrder.SetPrevious();
            CurrentOrder.Clear();

            IntVector3 WorldTilePosition = NavigationManager.Instance.GetTileWorldPosition(Destination);
            MapSet EndingMap = NavigationManager.Instance.GetMapAtTileWorldPosition(NavigationManager.Instance.GetTileWorldPosition(Destination));
            if (NavigationManager.Instance.ManagerSettings.SimulationSettings.UseAgentAvoidance)
            {
                IntVector2 TileIndex = EndingMap.GetMapTileIndex(WorldTilePosition);
                if (!_settings.TraversableTiles.Contains(EndingMap.GetTileType(TileIndex)))
                {
                    SetDestination(EndingMap.GetWorldTileIndex(EndingMap.GetClosestValidIndex(TileIndex, _settings.TraversableTiles)), true);
                }
                else
                {
                    SetDestination(Destination);
                }
            }
            else
            {
                if (!_settings.TraversableTiles.Contains(EndingMap.GetTileType(WorldTilePosition)))
                {
                    SetDestination(EndingMap.GetWorldTileIndex(EndingMap.GetClosestValidIndex(EndingMap.GetMapTileIndex(WorldTilePosition), _settings.TraversableTiles)), true);
                }
                else
                {
                    SetDestination(Destination);
                }
            }
        }

        /// <summary>
        /// Sets the destination to the nearest valid point near the desired destination
        /// </summary>
        /// <param name="Destination"></param>
        /// <param name="RecenterOnTile"></param>
        protected virtual void SetDestinationToNearestPoint(IntVector3 Destination, bool RecenterOnTile = false)
        {
            if (CurrentOrder.PathfindingStep == PathfindingCalculationStep.Pathfinding || State == AgentStates.Stopped)
            {
                return;
            }
            State = AgentStates.Pathfinding;
            CurrentOrder.SetPrevious();
            CurrentOrder.Clear();

            MapSet EndingMap = NavigationManager.Instance.GetMapAtTileWorldPosition(Destination);

            if (NavigationManager.Instance.ManagerSettings.SimulationSettings.UseAgentAvoidance)
            {
                IntVector2 TileIndex = EndingMap.GetMapTileIndex(Destination);
                if (!_settings.TraversableTiles.Contains(EndingMap.GetTileType(TileIndex)))
                {
                    SetDestination(EndingMap.GetWorldTileIndex(EndingMap.GetClosestValidIndex(TileIndex, _settings.TraversableTiles)), true);
                }
                else
                {
                    SetDestination(Destination);
                }
            }
            else
            {
                if (!_settings.TraversableTiles.Contains(EndingMap.GetTileType(Destination)))
                {
                    SetDestination(EndingMap.GetWorldTileIndex(EndingMap.GetClosestValidIndex(EndingMap.GetMapTileIndex(Destination), _settings.TraversableTiles)), true);
                }
                else
                {
                    SetDestination(Destination);
                }
            }
        }

        /// <summary>
        /// The Callback for when the agent recieves its path
        /// </summary>
        protected virtual void RecievePathingInfoCallback()
        {
            if (State == AgentStates.Stopped)
            {
                return;
            }
            // Ignore Previous orders if there is a duplication or previous uncompleted order
            CurrentOrder.SetPath(Path);
            if (CurrentOrder.NavJob.CurrentPath != null && CurrentOrder.NavJob.CurrentPath.ValidPath)
            { 
                State = AgentStates.SucceededPathing;         
            }
            else if (CurrentOrder.DestinationState == DestinationStates.NextMap_FromBridge || CurrentOrder.DestinationState == DestinationStates.NextMap_FromSeam)
            {
                State = AgentStates.SucceededPathing;
            }
            else
            {
                State = AgentStates.FailedPathing;
            }

            FailedLastPath = false;
            CurrentOrder.PathfindingStep = PathfindingCalculationStep.Finalized;
            if (State == AgentStates.SucceededPathing)
            {
                OnPathfindingSucceed?.Invoke(this);
                _currentMovementPathIndex = 0;
                SetInitialPositionToPath();
                if (CurrentOrder.Recenter)
                {
                    transform.position = Path[_currentMovementPathIndex];
                }
                if (_settings.GroundCheck)
                {
                    GroundCast?.PhysicsCheck();
                }
                _currentDirection = Path[_currentMovementPathIndex + 1] - Path[_currentMovementPathIndex];
                _currentDirection.y = 0;
                _lerpPercentage = 0;
                _lerpTotal = _currentDirection.magnitude;
                if (PreviousState != AgentStates.Paused)
                {
                    State = AgentStates.Moving;
                }
                else
                {
                    State = AgentStates.Paused;
                }
            }
            else if (State == AgentStates.FailedPathing)
            {
                FailedLastPath = true;
                State = AgentStates.Idle;
                OnPathfindingFailed?.Invoke(this);
            }
        }

        /// <summary>
        /// Adds the initial position to the list. Replaces first index if starting position in closer to end
        /// </summary>
        protected virtual void SetInitialPositionToPath()
        {
            float DistanceToEnd = (Path[Path.Count - 1] - Path[0]).magnitude;
            float CurrentDistanceToEnd = (Path[Path.Count - 1] - transform.position).magnitude;

            if (CurrentDistanceToEnd < DistanceToEnd)
            {
                Path[0] = transform.position;
            }
            else
            {
                Path.Insert(0, transform.position);
            }
        }

        #endregion

        #region Helpers

        /// <summary>
        /// Helps get the current position for this agent. Sim agents override this to use the simagent position
        /// </summary>
        /// <returns></returns>
        /// <exception cref="NotImplementedException"></exception>
        internal virtual Vector3 GetCurrentPosition()
        {
            return transform.position;
        }

        /// <summary>
        /// This tells you if the current tile index is valid for this unit
        /// </summary>
        /// <param name="TileIndex"></param>
        /// <returns></returns>
        public virtual bool ValidateTileIndex(IntVector2 TileIndex, MapSet Map)
        {
            return TraversableTiles.Contains(NavigationManager.Instance.GetTileTypeForMap(TileIndex, Map));
        }

        /// <summary>
        /// Gives a list of tile indicies that are traverable in range
        /// </summary>
        /// <param name="Range"></param>
        /// <returns></returns>
        public List<IntVector2> GetTraversableTilesInRange(int Range, DistanceCheck checkType = DistanceCheck.Distance)
        {
            List<IntVector2> Tiles = new List<IntVector2>();

            if (CurrentMap != null)
            {
                CurrentTileIndex = NavigationManager.Instance.GetTileIndexForMap(transform.position, CurrentMap);
                IntVector2 UpperBounds = new IntVector2(CurrentTileIndex.x + Range, CurrentTileIndex.y + Range);
                IntVector2 LowerBounds = new IntVector2(CurrentTileIndex.x - Range, CurrentTileIndex.y - Range);
                int SQRRange = Range * Range;

                for (int x = LowerBounds.x; x <= UpperBounds.x; x++)
                {
                    for (int y = LowerBounds.y; y <= UpperBounds.y; y++)
                    {
                        if (checkType == DistanceCheck.Manhatten)
                        {
                            int ManhattenDistance = Mathf.Abs(x - CurrentTileIndex.x) + Mathf.Abs(y - CurrentTileIndex.y);
                            if (ManhattenDistance > Range)
                            {
                                continue;
                            }

                            if (ManhattenDistance == 0)
                            {
                                continue;
                            }
                        }
                        else if (checkType == DistanceCheck.Distance)
                        {
                            IntVector2 Distance = new IntVector2(x, y);
                            Distance.x -= CurrentTileIndex.x;
                            Distance.y -= CurrentTileIndex.y;

                            float DistanceMagnitude = Distance.Distance();
                            if (DistanceMagnitude > Range)
                            {
                                continue;
                            }

                            if (DistanceMagnitude == 0)
                            {
                                continue;
                            }
                        }
                        else if (checkType == DistanceCheck.SqrDistance)
                        {
                            IntVector2 Distance = new IntVector2(x, y);
                            Distance.x -= CurrentTileIndex.x;
                            Distance.y -= CurrentTileIndex.y;

                            float DistanceMagnitude = Distance.SqrDistance();
                            if (DistanceMagnitude > SQRRange)
                            {
                                continue;
                            }

                            if (DistanceMagnitude == 0)
                            {
                                continue;
                            }
                        }

                        TileTypes TileIndexType = NavigationManager.Instance.GetTileTypeForMap(x, y, CurrentMap);
                        if (TileIndexType == TileTypes.OutOfBounds)
                        {
                            continue;
                        }

                        if (TraversableTiles.Contains(TileIndexType))
                        {
                            Tiles.Add(new IntVector2(x, y));
                        }
                    }
                }
            }

            return Tiles;
        }

        /// <summary>
        /// Gives a list of tile indicies that are traverable in the number of steps required to get there. Suggest not running this every frame
        /// </summary>
        /// <param name="Range"></param>
        /// <returns></returns>
        public List<IntVector2> GetTraversableTilesInRangeBySteps(int Steps, MovementType MovementType = MovementType.EightDirection)
        {
            List<IntVector2> Tiles = new List<IntVector2>();

            if (CurrentMap != null)
            {
                HashSet<IntVector2> ClosedTiles = new HashSet<IntVector2>();
                Dictionary<IntVector2, BreadthFirstData> SortedTiles = new Dictionary<IntVector2, BreadthFirstData>();

                CurrentTileIndex = NavigationManager.Instance.GetTileIndexForMap(transform.position, CurrentMap);

                List<BreadthFirstData> OpenList = new List<BreadthFirstData>();
                BreadthFirstData CurrentData;

                BreadthFirstData StartingData = new BreadthFirstData(CurrentTileIndex, 0);
                SortedTiles.Add(CurrentTileIndex, StartingData);
                OpenList.Add(StartingData);

                int StepScore = Steps * 10;
                //Debug.Log($"Step Score {StepScore}");

                while (OpenList.Count > 0)
                {
                    CurrentData = OpenList[0];
                    OpenList.RemoveAt(0);
                    ClosedTiles.Add(CurrentData.Index);

                    for (int x = -1; x <= 1; x++)
                    {
                        for (int y = -1; y <= 1; y++)
                        {
                            if (x == 0 && y == 0)
                            {
                                continue;
                            }

                            if (MovementType == MovementType.FourDirection)
                            {
                                if (x != 0 && y != 0)
                                {
                                    continue;
                                }
                            }

                            IntVector2 NewIndex = CurrentData.Index + new IntVector2(x, y);

                            TileTypes TileIndexType = NavigationManager.Instance.GetTileTypeForMap(NewIndex, CurrentMap);
                            if (TileIndexType == TileTypes.OutOfBounds)
                            {
                                continue;
                            }

                            if (TraversableTiles.Contains(TileIndexType))
                            {
                                int AdditionalScore = 10;
                                if (x != 0 && y != 0)
                                {
                                    AdditionalScore = 14;
                                }

                                if (!SortedTiles.ContainsKey(NewIndex))
                                {
                                    int newScore = CurrentData.StepScore + AdditionalScore;

                                    if (newScore <= StepScore)
                                    {
                                        BreadthFirstData NewData = new BreadthFirstData(NewIndex, newScore);
                                        Tiles.Add(NewIndex);
                                        SortedTiles.Add(NewIndex, NewData);
                                        OpenList.Add(NewData);
                                    }
                                }
                                else
                                {
                                    int newScore = CurrentData.StepScore + AdditionalScore;
                                    int PreviousScore = SortedTiles[NewIndex].StepScore;
                                    if (newScore < PreviousScore)
                                    {
                                        //Debug.Log($"Reduce Score {PreviousScore} -> {newScore} @ ({CurrentTileIndex.x - NewIndex.x}, {CurrentTileIndex.y - NewIndex.y})");
                                        SortedTiles[NewIndex].StepScore = newScore;

                                        if (ClosedTiles.Contains(NewIndex))
                                        {
                                            //Debug.Log($"Brought Back {PreviousScore} -> {newScore}");

                                            ClosedTiles.Remove(NewIndex);
                                            OpenList.Add(SortedTiles[NewIndex]);
                                        }
                                    }
                                }
                            }
                        }
                    }
                }
            }

            return Tiles;
        }

        private class BreadthFirstData
        {
            public IntVector2 Index;
            public int StepScore;

            public BreadthFirstData(IntVector2 index, int score)
            {
                Index = index;
                StepScore = score;
            }
        }

        #endregion

        #region Events

        /// <summary>
        /// The Event that is called when the Pathfinding is completed correctly
        /// </summary>
        public UnityEvent<NavigationAgent> OnPathfindingSucceed;
        /// <summary>
        /// The Event that is called when the Pathfinding is completed incorrectly
        /// </summary>
        public UnityEvent<NavigationAgent> OnPathfindingFailed;
        /// <summary>
        /// The Event that is called when the agent is done moving to it's destination
        /// </summary>
        public UnityEvent<NavigationAgent> OnMovingComplete;

        #endregion

        /// <summary>
        /// The Gizmos settings for the Agent
        /// </summary>
        public AgentGizmosSettings GizmosSettings = new AgentGizmosSettings();
    }

    /// <summary>
    /// The Navigation Order for the agent to bridge itself to the simulation
    /// </summary>
    [Serializable]
    public class NavigationOrder
    {
        /// <summary>
        /// Agent for this Order
        /// </summary>
        public NavigationAgent Agent;

        /// <summary>
        /// Navigation Start
        /// </summary>
        public IntVector3 Start;
        /// <summary>
        /// Navigation Current End
        /// </summary>
        public IntVector3 Destination;
        /// <summary>
        /// Navigation Desired End
        /// </summary>
        public IntVector3 FinalDestination;

        /// <summary>
        /// Navigation Previous Movement Start
        /// </summary>
        public IntVector3 PreviousStart;
        /// <summary>
        /// Navigation Movement End
        /// </summary>
        public IntVector3 PreviousDestination;

        /// <summary>
        /// Navigation Current Destination
        /// </summary>
        public IntVector3 CheckPointDestination;
        /// <summary>
        /// The Current State the Navigation is at
        /// </summary>
        public DestinationStates DestinationState;

        /// <summary>
        /// The Navigation Job request used to pathfind for the agent
        /// </summary>
        public NavigationJob NavJob;

        /// <summary>
        /// Current pathfinding step of the agent
        /// </summary>
        public PathfindingCalculationStep PathfindingStep;
        /// <summary>
        /// Starting map of the order
        /// </summary>
        public MapSet StartingMap;
        /// <summary>
        /// Ending map of the order
        /// </summary>
        public MapSet EndingMap;
        /// <summary>
        /// The bridge that was found and needed to be move onto to get to the destination
        /// </summary>
        public MapBridge FoundBridge;
        /// <summary>
        /// The seam that was found and needed to be move onto to get to the destination
        /// </summary>
        public MapSeam FoundSeam;
        /// <summary>
        /// Whether to move on the center of the tile or not
        /// </summary>
        public bool Recenter;

        public NavigationOrder(NavigationAgent agent)
        {
            PreviousStart = new IntVector3();
            PreviousDestination = new IntVector3();
            Agent = agent;
            NavJob = new NavigationJob(NavJobCallback);
            Clear();
        }

        /// <summary>
        /// Clears the order to default
        /// </summary>
        public void Clear()
        {
            PathfindingStep = PathfindingCalculationStep.Incomplete;
            Start = new IntVector3();
            Destination = new IntVector3();
            CheckPointDestination = new IntVector3();
            DestinationState = DestinationStates.None;
            EndingMap = null;
            StartingMap = null;
            FoundBridge = null;
            FoundSeam = null;
            NavJob.Clear();
        }

        /// <summary>
        /// Sets the destination for the agent
        /// </summary>
        /// <param name="Position"></param>
        /// <param name="RecenterOnTile"></param>
        public virtual void SetDestination(Vector3 Position, bool RecenterOnTile = false)
        {
            IntVector3 TileWorldIndex = NavigationManager.Instance.GetTileWorldPosition(Position);
            SetDestination(TileWorldIndex, RecenterOnTile);
        }

        /// <summary>
        /// Sets the destination for the agent
        /// </summary>
        /// <param name="Position"></param>
        /// <param name="RecenterOnTile"></param>
        internal virtual void SetDestination(IntVector3 Position, bool RecenterOnTile = false)
        {
            Recenter = RecenterOnTile;

            PathfindingStep = PathfindingCalculationStep.Pathfinding;
            Start = NavigationManager.Instance.GetTileWorldPosition(Agent.GetCurrentPosition());
            Destination = Position;
            FinalDestination = Destination;

            StartingMap = NavigationManager.Instance.GetMapAtTileWorldPosition(Start);
            EndingMap = NavigationManager.Instance.GetMapAtTileWorldPosition(Destination);
            if (StartingMap != null)
            {
                if (!Agent.TraversableTiles.Contains(StartingMap.GetTileType(Start)))
                {
                    Start = StartingMap.GetClosestValidPoint(Start, Agent.TraversableTiles);
                }
                IntVector2 TileIndex = NavigationManager.Instance.GetTileIndexForMap(Start, StartingMap);
                Start = StartingMap.GetWorldTileIndex(TileIndex);
            }

            if (EndingMap != null)
            {
                if (!Agent.TraversableTiles.Contains(EndingMap.GetTileType(Destination)))
                {
                    Destination = EndingMap.GetClosestValidPoint(Destination, Agent.TraversableTiles);
                }
                IntVector2 TileIndex = NavigationManager.Instance.GetTileIndexForMap(Destination, EndingMap);
                Destination = EndingMap.GetWorldTileIndex(TileIndex);
            }

            if (StartingMap == null)
            {
                //Debug.Log("No Starting Map");
                CheckPointDestination = new IntVector3();
                DestinationState = DestinationStates.Null; // Start Not presently on a map so not path or "Null"
                NavJobCallback();
            }
            else if (EndingMap == null)
            {
                //Debug.Log("No End Map");
                CheckPointDestination = StartingMap.GetClosestPoint(Destination);
                if (!Agent.TraversableTiles.Contains(StartingMap.GetTileType(StartingMap.GetMapTileIndex(CheckPointDestination))))
                {
                    CheckPointDestination = StartingMap.GetClosestValidPoint(CheckPointDestination, Agent.TraversableTiles);
                }
                Destination = CheckPointDestination;
                NavJob.SetDestinationInfo(Start, CheckPointDestination, Agent.TraversableTiles, Agent.PrefferedTiles, Agent.NavigationType, StartingMap);
                DestinationState = DestinationStates.ClosestPoint; // End Not presently on a map so go to "Closest Point"
                EndingMap = StartingMap;
                NavigationManager.Instance.AddNavigationOrder(this);
            }
            else if (StartingMap.InstanceID == EndingMap.InstanceID)
            {
                //Debug.Log("Same Map");
                CheckPointDestination = Destination;
                if (!Agent.TraversableTiles.Contains(StartingMap.GetTileType(StartingMap.GetMapTileIndex(CheckPointDestination))))
                {
                    CheckPointDestination = StartingMap.GetClosestValidPoint(CheckPointDestination, Agent.TraversableTiles);
                }
                NavJob.SetDestinationInfo(Start, CheckPointDestination, Agent.TraversableTiles, Agent.PrefferedTiles, Agent.NavigationType, StartingMap);
                if (CheckPointDestination == Start)
                {
                    DestinationState = DestinationStates.SamePosition; // Start and End are at the same spot
                    NavJobCallback();
                    return;
                }
                DestinationState = DestinationStates.SameMap; // Start and End are on the "Same Map"
                NavigationManager.Instance.AddNavigationOrder(this);
            }
            else
            {
                if (NavigationManager.Instance.GetDesiredBridgeOrSeam(Start, StartingMap, EndingMap, out FoundBridge, out FoundSeam, out int BridgeScore, out int SeamScore))
                {
                    if (FoundSeam == null || BridgeScore < SeamScore)
                    {
                        IntVector2 StartTileIndex = NavigationManager.Instance.GetTileIndexForMap(Start, StartingMap);
                        IntVector3 BridgePoint = FoundBridge.GetPoint(StartingMap);
                        IntVector2 BridgeStartTileIndex = NavigationManager.Instance.GetTileIndexForMap(BridgePoint, StartingMap);

                        if (StartTileIndex == BridgeStartTileIndex)
                        {
                            CheckPointDestination = FoundBridge.GetOpposingPoint(StartingMap);
                            DestinationState = DestinationStates.NextMap_FromBridge; // Start is at bridge point so just move across bridge to the "Next Map"
                            NavJobCallback();
                        }
                        else
                        {
                            CheckPointDestination = BridgePoint;
                            NavJob.SetDestinationInfo(Start, CheckPointDestination, Agent.TraversableTiles, Agent.PrefferedTiles, Agent.NavigationType, StartingMap);
                            DestinationState = DestinationStates.Bridge; // Not close enough to the bridge, so move to the "Bridge"
                            NavigationManager.Instance.AddNavigationOrder(this);
                        }
                    }
                    else
                    {
                        IntVector2 StartTileIndex = NavigationManager.Instance.GetTileIndexForMap(Start, StartingMap);
                        IntVector3 SeamPoint = FoundSeam.GetPoint(StartingMap, Start);
                        IntVector2 BridgeStartTileIndex = NavigationManager.Instance.GetTileIndexForMap(SeamPoint, StartingMap);

                        if (StartTileIndex == BridgeStartTileIndex)
                        {
                            CheckPointDestination = FoundSeam.GetOppositePoint(StartingMap, Start);
                            DestinationState = DestinationStates.NextMap_FromSeam; // Start is at seam point so just move across seam to the "Next Map"
                            NavJobCallback();
                        }
                        else
                        {
                            CheckPointDestination = SeamPoint;
                            NavJob.SetDestinationInfo(Start, CheckPointDestination, Agent.TraversableTiles, Agent.PrefferedTiles, Agent.NavigationType, StartingMap);
                            DestinationState = DestinationStates.Seam; // Not close enough to the bridge, so move to the "Bridge"
                            NavigationManager.Instance.AddNavigationOrder(this);
                        }
                    }
                }
                else
                {
                    Debug.Log("Could not find Bridge");
                    CheckPointDestination = StartingMap.GetClosestPoint(Destination);
                    if (!Agent.TraversableTiles.Contains(StartingMap.GetTileType(StartingMap.GetMapTileIndex(CheckPointDestination))))
                    {
                        CheckPointDestination = StartingMap.GetClosestValidPoint(CheckPointDestination, Agent.TraversableTiles);
                    }
                    NavJob.SetDestinationInfo(Start, CheckPointDestination, Agent.TraversableTiles, Agent.PrefferedTiles, Agent.NavigationType, StartingMap);
                    DestinationState = DestinationStates.ClosestPoint; // End Not presently on a map so go to "Closest Point"
                    Destination = CheckPointDestination;
                    NavigationManager.Instance.AddNavigationOrder(this);
                }
            }
        }

        /// <summary>
        /// The callback when the pathfinding is completed
        /// </summary>
        private void NavJobCallback()
        {
            PathfindingStep = PathfindingCalculationStep.Complete;
        }

        /// <summary>
        /// Sorts the path for the agent
        /// </summary>
        /// <param name="path"></param>
        public void SetPath(List<Vector3> path)
        {
            path.Clear();
            // Use Bridge to get to next map, figure out which direction the bridge path is
            if (DestinationState == DestinationStates.NextMap_FromBridge)
            { 
                if (FoundBridge.MapA.InstanceID == StartingMap.InstanceID)
                {
                    path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(FoundBridge.MidPoints[0], StartingMap)); // Point should be tile center by the starting maps size
                    for(int i = 1; i < FoundBridge.MidPoints.Count - 1; i++)
                    {
                        path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(FoundBridge.MidPoints[i])); // Should be by the world scale of 1
                    }
                    path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(FoundBridge.MidPoints[FoundBridge.MidPoints.Count - 1], FoundBridge.MapB)); // Point should be tile center by the next maps size
                }
                // Reverse because the bridge is sorted by A side
                else if (FoundBridge.MapB.InstanceID == StartingMap.InstanceID)
                {
                    path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(FoundBridge.MidPoints[FoundBridge.MidPoints.Count - 1], StartingMap)); // Point should be tile center by the next maps size
                    for (int i = FoundBridge.MidPoints.Count - 2; i > 0; i--)
                    {
                        path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(FoundBridge.MidPoints[i])); // Should be by the world scale of 1
                    }
                    path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(FoundBridge.MidPoints[0], FoundBridge.MapA)); // Point should be tile center by the starting maps size
                }
            }
            else if (DestinationState == DestinationStates.NextMap_FromSeam)
            {
                IntVector3 AgentWorldPosition = NavigationManager.Instance.GetTileWorldPosition(Agent.transform.position);
                IntVector3 EndingMapIndex = FoundSeam.GetOppositePoint(StartingMap, AgentWorldPosition);

                path.Add(Agent.transform.position);
                path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(EndingMapIndex));
            }
            // Use the navigation Path to populate the path list
            else if (DestinationState != DestinationStates.None && DestinationState != DestinationStates.Null)
            {
                if (NavJob.CurrentPath != null)
                {
                    if (!NavJob.CurrentPath.ValidPath)
                    {
                        return;
                    }
                }
                else
                {
                    return;
                }

                //IntVector3 Start1 = StartingMap.GetWorldTileIndex(NavJob.CurrentPath.Path[0]);
                //IntVector3 End1 = StartingMap.GetWorldTileIndex(NavJob.CurrentPath.Path[NavJob.CurrentPath.Path.Count - 1]);

                //IntVector2 StartTileIndex = StartingMap.GetMapTileIndex(Start);
                //IntVector2 EndTileIndex = StartingMap.GetMapTileIndex(CheckPointDestination);

                //Debug.Log($"Destination Values: [({NavJob.CurrentPath.Path[0].x}, {NavJob.CurrentPath.Path[0].y}) / ({StartTileIndex.x}, {StartTileIndex.y}) -> ({NavJob.CurrentPath.Path[NavJob.CurrentPath.Path.Count - 1].x}, {NavJob.CurrentPath.Path[NavJob.CurrentPath.Path.Count - 1].y}) / ({EndTileIndex.x}, {EndTileIndex.y})]");
                if (NavJob.CurrentPath.NeedToReverse(Start))
                {
                    //Debug.Log($"Need To Reverse");
                    for (int i = NavJob.CurrentPath.Path.Count - 1; i >= 0; i--)
                    {
                        path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(NavJob.CurrentPath.Path[i], StartingMap));
                    }
                }
                else
                {
                    //Debug.Log($"Dont To Reverse");
                    for (int i = 0; i < NavJob.CurrentPath.Path.Count; i++)
                    {
                        path.Add(NavigationManager.Instance.GetTileCenterWorldPosition(NavJob.CurrentPath.Path[i], StartingMap));
                    }
                }
            }
            else if (DestinationState == DestinationStates.SamePosition)
            {

            }
        }

        /// <summary>
        /// Sets the previous pathing info
        /// </summary>
        internal void SetPrevious()
        {
            PreviousStart = Start;
            PreviousDestination = Destination;
        }
    }

    /// <summary>
    /// The gizmos settings for the agent
    /// </summary>
    [Serializable]
    public class AgentGizmosSettings
    {
        /// <summary>
        /// Whether to show the Gizmos
        /// </summary>
        public bool ShowGizmos = false;
        /// <summary>
        /// Whether to show the path
        /// </summary>
        public bool ShowPath = false;
        /// <summary>
        /// Whether to show the start and end positions
        /// </summary>
        public bool ShowStartAndEnd = false;
        /// <summary>
        /// Whether to show a traversable range around the agent
        /// </summary>
        public bool ShowTraverableRange = false;
        /// <summary>
        /// The traversable range we want to show
        /// </summary>
        public int TraversableRange = 10;
        /// <summary>
        /// whether to show all the surrounding tiles within range of the aganet
        /// </summary>
        public bool ShowValidTilesInRange = false;
        /// <summary>
        /// The Valid tile range we want to show
        /// </summary>
        public int ValidTileRange = 10;

        /// <summary>
        /// Whether to show the closest tile to the wanted position
        /// </summary>
        public bool ShowClosestValidTileToWantedPosition = false;
    }

    /// <summary>
    /// The Agent settings for the Agent
    /// </summary>
    [Serializable]
    public class AgentSettings
    {
        /// <summary>
        /// Whether to automatically update the agent with the given systems
        /// </summary>
        public bool AutomaticUpdate = true;
        /// <summary>
        /// whether to check the ground beneath the agent 
        /// </summary>
        public bool GroundCheck = false;
        /// <summary>
        /// whether the agent rotates towards pathing direction
        /// </summary>
        public bool RotateTowardsMovingDirection = true;
        /// <summary>
        /// Rotation speed
        /// </summary>
        public float RotateSpeed = 5;
        /// <summary>
        /// Movement speed of the agent
        /// </summary>
        [SerializeField]
        private float _movementSpeed = 5f;
        /// <summary>
        /// whether to auto validate the current path
        /// </summary>
        [SerializeField]
        private bool _autoValidateCurrentPath = false;
        /// <summary>
        /// Auto validate interval
        /// </summary>
        [SerializeField]
        private float _autoValidateTime = 1.5f;
        /// <summary>
        /// Auto validate range
        /// </summary>
        [SerializeField]
        private int _localAvoidanceRange = -1;
        /// <summary>
        /// Whether to validate the current tile the agent is on
        /// </summary>
        [SerializeField]
        private bool _validateCurrentTile = false;
        /// <summary>
        /// Whether to keep attempting to find a path when the current path is invalidated
        /// </summary>
        [SerializeField]
        private bool _keepAttemptingOnFail = false;
        /// <summary>
        /// Whether to try to move to the nearest point of your destination
        /// </summary>
        [SerializeField]
        private bool _onFailAttemptToNearestPoint = false;
        /// <summary>
        /// Whether to try and keep to readjust to wanted position
        /// </summary>
        [SerializeField]
        private bool _keepTryingToReadjustToWantedPosition = false;
        /// <summary>
        /// Keep checking on fail interval
        /// </summary>
        [SerializeField]
        private float _keepAttemptingTime = 1.5f;
        /// <summary>
        /// The Agent navigation type
        /// </summary>
        [SerializeField]
        private NavigationTypes _navigationType = NavigationTypes.JumpPointSearch;
        /// <summary>
        /// The Traversable tiles for the agent
        /// </summary>
        [SerializeField]
        private List<TileTypes> _traversableTiles = new List<TileTypes>() { TileTypes.Standard, TileTypes.Free };
        /// <summary>
        /// the preffered pathing tiles for the agent
        /// </summary>
        [SerializeField]
        private List<TileTypes> _prefferedTiles = new List<TileTypes>();

        public float MovementSpeed { get { return _movementSpeed; } set { _movementSpeed = value; if (_movementSpeed < 0) { _movementSpeed = 0; } } }
        public bool AutoValidateCurrentPath { get { return _autoValidateCurrentPath; } set { _autoValidateCurrentPath = value; } }
        public float AutoValidateTime { get { return _autoValidateTime; } set { _autoValidateTime = value; } }
        public int LocalAvoidanceRange { get { return _localAvoidanceRange; } set { _localAvoidanceRange = value; } }
        public bool ValidateCurrentTile { get { return _validateCurrentTile; } set { _validateCurrentTile = value; } }
        public bool KeepAttemptingOnFail { get { return _keepAttemptingOnFail; } set { _keepAttemptingOnFail = value; } }
        public bool OnFailAttemptToNearestPoint { get { return _onFailAttemptToNearestPoint; } set { _onFailAttemptToNearestPoint = value; } }
        public bool KeepTryingToReadjustToWantedPosition { get { return _keepTryingToReadjustToWantedPosition; } set { _keepTryingToReadjustToWantedPosition = value; } }
        public float KeepAttemptingTime { get { return _keepAttemptingTime; } set { _keepAttemptingTime = value; } }
        public NavigationTypes NavigationType { get { return _navigationType; } }
        public List<TileTypes> TraversableTiles { get { return _traversableTiles; } set { lock (_traversableTiles) { _traversableTiles = value; TraversableTilesKey = _traversableTiles.TilesToString(); } } }
        public List<TileTypes> PrefferedTiles { get { return _prefferedTiles; } set { lock (_prefferedTiles) { _prefferedTiles = value; PrefferedTilesKey = _prefferedTiles.TilesToString(); } } }
        public string TraversableTilesKey { get; set; }
        public string PrefferedTilesKey { get; set; }
    }
}