Refactor pathfinding updates and add AccessReader support (#1183)

There was some extra bloat in the path graph updates.
Now the queue should also just run if it gets too big regardless.
Un-anchored physics objects are no longer a hard fail for pathfinding.
Add AccessReader support so open / close doors show up for pathfinding
AI also ensure they call the operator's shutdown when they're shutdown so that should cancel the pathfinding job.

I tried to split these into 2 commits but they were kinda coupled together

Co-authored-by: Metal Gear Sloth <metalgearsloth@gmail.com>

Content.Server/GameObjects/EntitySystems/AI/Pathfinding/PathfindingNode.cs

@@ -1,6 +1,12 @@
 using System;
 using System.Collections.Generic;
+using Content.Server.GameObjects.Components.Access;
+using Content.Server.GameObjects.Components.Doors;
 using Content.Server.GameObjects.EntitySystems.AI.Pathfinding;
+using Robust.Server.GameObjects;
+using Robust.Shared.GameObjects;
+using Robust.Shared.GameObjects.Components;
+using Robust.Shared.Interfaces.GameObjects;
 using Robust.Shared.Map;
 using Robust.Shared.Maths;
 
@@ -8,27 +14,34 @@ namespace Content.Server.GameObjects.EntitySystems.Pathfinding
 {
     public class PathfindingNode
     {
-        // TODO: Add access ID here
         public PathfindingChunk ParentChunk => _parentChunk;
         private readonly PathfindingChunk _parentChunk;
-        public TileRef TileRef { get; private set; }
-        public List<int> CollisionLayers { get; }
-        public int CollisionMask { get; private set; }
+        
         public Dictionary<Direction, PathfindingNode> Neighbors => _neighbors;
         private Dictionary<Direction, PathfindingNode> _neighbors = new Dictionary<Direction, PathfindingNode>();
+        
+        public TileRef TileRef { get; private set; }
+        
+        /// <summary>
+        /// Whenever there's a change in the collision layers we update the mask as the graph has more reads than writes
+        /// </summary>
+        public int BlockedCollisionMask { get; private set; }
+        private readonly Dictionary<EntityUid, int> _blockedCollidables = new Dictionary<EntityUid, int>(0);
 
-        public PathfindingNode(PathfindingChunk parent, TileRef tileRef, List<int> collisionLayers = null)
+        public IReadOnlyCollection<EntityUid> PhysicsUids => _physicsUids;
+        private readonly HashSet<EntityUid> _physicsUids = new HashSet<EntityUid>(0);
+
+        /// <summary>
+        /// The entities on this tile that require access to traverse
+        /// </summary>
+        /// We don't store the ICollection, at least for now, as we'd need to replicate the access code here
+        public IReadOnlyCollection<AccessReader> AccessReaders => _accessReaders.Values;
+        private readonly Dictionary<EntityUid, AccessReader> _accessReaders = new Dictionary<EntityUid, AccessReader>(0);
+
+        public PathfindingNode(PathfindingChunk parent, TileRef tileRef)
         {
             _parentChunk = parent;
             TileRef = tileRef;
-            if (collisionLayers == null)
-            {
-                CollisionLayers = new List<int>();
-            }
-            else
-            {
-                CollisionLayers = collisionLayers;
-            }
             GenerateMask();
         }
 
@@ -105,25 +118,70 @@ namespace Content.Server.GameObjects.EntitySystems.Pathfinding
             TileRef = newTile;
         }
 
-        public void AddCollisionLayer(int layer)
+        /// <summary>
+        /// Call if this entity is relevant for the pathfinder
+        /// </summary>
+        /// <param name="entity"></param>
+        /// TODO: These 2 methods currently don't account for a bunch of changes (e.g. airlock unpowered, wrenching, etc.)
+        public void AddEntity(IEntity entity)
         {
-            CollisionLayers.Add(layer);
-            GenerateMask();
+            // If we're a door
+            if (entity.HasComponent<AirlockComponent>() || entity.HasComponent<ServerDoorComponent>())
+            {
+                // If we need access to traverse this then add to readers, otherwise no point adding it (except for maybe tile costs in future)
+                // TODO: Check for powered I think (also need an event for when it's depowered
+                // AccessReader calls this whenever opening / closing but it can seem to get called multiple times
+                // Which may or may not be intended?
+                if (entity.TryGetComponent(out AccessReader accessReader) && !_accessReaders.ContainsKey(entity.Uid))
+                {
+                    _accessReaders.Add(entity.Uid, accessReader);
+                }
+                return;
+            }
+            
+            if (entity.TryGetComponent(out CollidableComponent collidableComponent))
+            {
+                if (entity.TryGetComponent(out PhysicsComponent physicsComponent) && !physicsComponent.Anchored)
+                {
+                    _physicsUids.Add(entity.Uid);
+                }
+                else
+                {
+                    _blockedCollidables.TryAdd(entity.Uid, collidableComponent.CollisionLayer);
+                    GenerateMask();
+                }
+            }
         }
 
-        public void RemoveCollisionLayer(int layer)
+        public void RemoveEntity(IEntity entity)
         {
-            CollisionLayers.Remove(layer);
-            GenerateMask();
+            if (_accessReaders.ContainsKey(entity.Uid))
+            {
+                _accessReaders.Remove(entity.Uid);
+                return;
+            }
+
+            if (entity.HasComponent<CollidableComponent>())
+            {
+                if (entity.TryGetComponent(out PhysicsComponent physicsComponent) && physicsComponent.Anchored)
+                {
+                    _blockedCollidables.Remove(entity.Uid);
+                    GenerateMask();
+                }
+                else
+                {
+                    _physicsUids.Remove(entity.Uid);
+                }
+            }
         }
 
         private void GenerateMask()
         {
-            CollisionMask = 0x0;
+            BlockedCollisionMask = 0x0;
 
-            foreach (var layer in CollisionLayers)
+            foreach (var layer in _blockedCollidables.Values)
             {
-                CollisionMask |= layer;
+                BlockedCollisionMask |= layer;
             }
         }
     }