working on pathfinding

Enemy.cs

@@ -1,15 +1,22 @@
 using Godot;
 using System;
+using System.Collections.Generic;
+using System.Linq;
 
 public partial class Enemy : StaticBody2D
 {
     [Signal]
     public delegate void DeathEventHandler(Enemy THIS);
-    public int _damage = 1, _health = 2;
+    public int _damage = 1, _health = 2, _speed;
-    public Vector2 _speed = Vector2.Up, _range = Vector2.Up;
+    public Vector2I _range = Vector2I.Up;
     public float _movement = 0;
     public GridMarker _gridMarker;
     public Commander _commander;
+
+    public override void _Ready()
+    {
+        base._Ready();
+    }
     public override void _PhysicsProcess(double delta)
     {
         base._PhysicsProcess(delta);
@@ -25,6 +32,76 @@ public partial class Enemy : StaticBody2D
         
     }
 
+    public List<GridMarker> Pathfinding(List<GridMarker> DESTINATIONS, bool INCLUDE_SELF = false)
+    {
+        PriorityQueue<GridMarker, float> queue = new();
+        queue.Enqueue(_gridMarker, 0f);
+        Dictionary<GridMarker, GridMarker> cameFrom = new(), best = new();
+        Dictionary<GridMarker, float> costSoFar = new();
+        cameFrom[_gridMarker] = null;
+        costSoFar[_gridMarker] = 0f;
+        GridMarker current;
+        float newCost, priority;
+
+        for (int i = 0; i < DESTINATIONS.Count; i++)
+        {
+            GridMarker destination = DESTINATIONS[i];
+            while (queue.Count > 0)
+            {
+                current = queue.Dequeue();
+                if (current == destination)
+                {
+                    best = cameFrom;
+                    GD.Print("breaking at line 56");
+                    continue;
+                }
+
+                List<GridMarker> neighbors = current.GetMarkersInRange(1.5f);
+                for (int j = 0; j < neighbors.Count; j++)
+                {
+                    GridMarker next = neighbors[j];
+                    if (next._occupant != null)
+                    {
+                        continue;
+                    }
+                    newCost = costSoFar[current] + 1;
+                    if (best.Count > 0 && newCost >= best.Count)
+                    {
+                        GD.Print("breaking at line 70");
+                        break;
+                    }
+                    if (!costSoFar.TryGetValue(next, out float value) || newCost < value)
+                    {
+                        costSoFar[next] = newCost;
+                        priority = newCost;
+                        queue.Enqueue(next, priority);
+                        cameFrom[next] = current;
+                    }
+                }
+            }
+        }
+
+        GD.Print(best.Last().Value._address);
+        List<GridMarker> path = new();
+        GridMarker pathMarker = best.Last().Value;
+
+        if (best.TryGetValue(pathMarker, out GridMarker marker))
+        {
+            while (pathMarker != null)
+            {
+                path.Add(pathMarker);
+                pathMarker = marker;
+            }
+            path.Reverse();
+            if (!INCLUDE_SELF)
+            {
+                path.Remove(_gridMarker);
+            }
+        }
+
+        return path;
+    }
+
     public void TakeDamage(int DAMAGE, Commander ATTACKER)
     {
         GD.Print(_health, _health - DAMAGE);

EnemyController.cs

@@ -19,6 +19,8 @@ public partial class EnemyController : TurnController
             List<GridMarker> lastRow = _grid._gridMarkers.Last();
             List<GridMarker> unoccupiedMarkers = [.. lastRow.Where(m => m._occupant == null).Cast<GridMarker>()];
             GridMarker randomMarker = unoccupiedMarkers[Globals._rng.Next(unoccupiedMarkers.Count)];
+            newEnemy._speed = Globals._rng.Next(2,4+1);
+            newEnemy.Modulate = new Color(newEnemy._speed == 2 ? "#FF0000" : newEnemy._speed == 3 ? "#00FF00" : "#0000FF");
             WarpEnemy(newEnemy, randomMarker);
             _enemies.Add(newEnemy);
             AddChild(newEnemy);
@@ -30,24 +32,43 @@ public partial class EnemyController : TurnController
         AddEnemies(3);
     }
 
-    public void MoveEnemy(Enemy ENEMY)
+    // public void MoveEnemy(Enemy ENEMY)
-    {
+    // {
-        ENEMY._gridMarker = _grid.GetMarkerByAddress((Vector2I)(ENEMY._gridMarker._address + ENEMY._speed));
+    //     ENEMY._gridMarker = _grid.GetMarkerFromOffset(ENEMY._gridMarker, ENEMY._speed);
-        Tween tween = CreateTween();
+    //     Tween tween = CreateTween();
-        tween.TweenProperty(ENEMY, "global_position", ENEMY._gridMarker.GlobalPosition, 1.0f);
+    //     tween.TweenProperty(ENEMY, "global_position", ENEMY._gridMarker.GlobalPosition, 1.0f);
-    }
+    // }
 
     public void MoveEnemies(List<Enemy> ENEMIES = null)
     {
         ENEMIES ??= _enemies;
+        ENEMIES = [.. ENEMIES.OrderByDescending(e => e._gridMarker._address.Y).ThenByDescending(e => e._gridMarker._address.X)];
         Tween tween = CreateTween();
         tween.SetParallel();
+        Dictionary<Enemy, List<GridMarker>> enemyPaths = new();
+
         for (int i = 0; i < ENEMIES.Count; i++)
         {
-            ENEMIES[i]._gridMarker = _grid.GetMarkerByAddress((Vector2I)(ENEMIES[i]._gridMarker._address + ENEMIES[i]._speed));
+            enemyPaths[ENEMIES[i]] = ENEMIES[i].Pathfinding(_grid._gridMarkers.ToList());
-            tween.TweenProperty(ENEMIES[i], "global_position", ENEMIES[i]._gridMarker.GlobalPosition, 0.4f);
+        }
+
+        int maxSteps = enemyPaths.Select(p => p.Value.Count).Max();
+        GD.Print(enemyPaths.Count, " ", maxSteps);
+
+        for (int i = 0; i < maxSteps; i++)
+        {
+            Dictionary<Enemy, List<GridMarker>> qualifyingPaths = (Dictionary<Enemy, List<GridMarker>>)enemyPaths.Where(p => p.Value.Count <= maxSteps);
+            for (int j = 0; j < qualifyingPaths.Count; j++)
+            {
+                Enemy enemy = qualifyingPaths.ElementAt(j).Key;
+                GridMarker marker = qualifyingPaths.ElementAt(j).Value.ElementAt(i);
+                if (j == 0)
+                {
+                    tween.Chain();
+                }
+                tween.TweenProperty(enemy, "global_position", enemy._gridMarker.GlobalPosition, 0.4f);
+            }
         }
-        tween.TweenCallback(Callable.From(() => EmitSignal(SignalName.TurnDone)));
     }
 
     public void RemoveEnemy(Enemy ENEMY_TO_REMOVE)

GridMap2D.cs

@@ -34,7 +34,7 @@ public partial class GridMap2D : Node2D
             for (int j = 0; j < _sizeInCells.X; j++)
             {
                 GridMarker newGridMarker = (GridMarker)_gridMarker.Duplicate();
-                newGridMarker._address = new Vector2(j, i);
+                newGridMarker._address = new Vector2I(j, i);
                 newGridMarker.Position = new Vector2(_playArea.Position.X - playAreaBounds.Shape.GetRect().Size.X / 2 + (j+.5f)*_cellSize, _playArea.Position.Y - playAreaBounds.Shape.GetRect().Size.Y / 2 + (i+.5f)*_cellSize);
                 _gridMarkers[i].Add(newGridMarker);
                 AddChild(newGridMarker);
@@ -67,8 +67,13 @@ public partial class GridMap2D : Node2D
         return _gridMarkers[COORDINATES.Y][COORDINATES.X];
     }
 
-    public GridMarker GetGridMarkerRelativeToAnother(GridMarker GRID_MARKER, Vector2I STEP)
+    public GridMarker GetMarkerFromOffset(GridMarker GRID_MARKER, Vector2I STEP)
     {
-        return _gridMarkers[(int)GRID_MARKER._address.Y + STEP.Y][(int)GRID_MARKER._address.X + STEP.X];
+        return _gridMarkers[GRID_MARKER._address.Y + STEP.Y][GRID_MARKER._address.X + STEP.X];
+    }
+
+    public List<GridMarker> GetMarkerInRange(GridMarker GRID_MARKER, int RANGE)
+    {
+        return [.. _gridMarkers.SelectMany(m => m).Where(m => (GRID_MARKER._address - m._address).Length() <= RANGE).Cast<GridMarker>()];
     }
 }

GridMarker.cs

@@ -1,10 +1,25 @@
 using Godot;
 using System;
+using System.Collections.Generic;
+using System.Linq;
 
 public partial class GridMarker : Marker2D
 {
-    public Vector2 _address;
+    public Vector2I _address;
     public Node _occupant;
 
-    // public GridMarker Get
+    public List<GridMarker> _gridMarkers;
+
+    public override void _Ready()
+    {
+        base._Ready();
+    }
+
+    public List<GridMarker> GetMarkersInRange(float RANGE = 1.0f){
+        if (_gridMarkers == null || _gridMarkers.Count == 0)
+        {   
+            _gridMarkers = [.. GetTree().GetNodesInGroup("GridMarkers").Cast<GridMarker>()];
+        }
+        return [.. _gridMarkers.Where(m => m._address.Length() <= RANGE).Cast<GridMarker>()];
+    }
 }

grid_marker.tscn

@@ -2,5 +2,5 @@
 
 [ext_resource type="Script" uid="uid://cob0pwghnubxa" path="res://GridMarker.cs" id="1_u0re5"]
 
-[node name="GridMarker" type="Marker2D" unique_id=1390656323]
+[node name="GridMarker" type="Marker2D" unique_id=1390656323 groups=["GridMarkers"]]
 script = ExtResource("1_u0re5")

project.godot

@@ -25,6 +25,10 @@ window/size/mode=3
 
 project/assembly_name="PeggleRoguelike"
 
+[global_group]
+
+GridMarkers=""
+
 [input]
 
 leftClick={