Monster will stop beside soldier and not on top

DepthsBelow/DepthsBelow/KeyboardInput.cs

@@ -43,6 +43,7 @@ namespace DepthsBelow
                 }
                 else
                 {
+                    core.TestMonster.step = 0;
                     core.PlayerTurn = false;
                     Point target = Point.Zero;
                     int distance = 7000;

DepthsBelow/DepthsBelow/SmallEnemy.cs

@@ -18,6 +18,18 @@ namespace DepthsBelow
 
         private PathFinder.Node nextNode;
 
+        public int numberOfSteps = 5;
+        public int currentStep = 0;
+
+        public int step
+        {
+            get { return currentStep; }
+            set
+            {
+                currentStep = value;
+            }
+        }
+
         public SmallEnemy(Core core, ref List<SmallEnemy> swarm)
             : base(core)
         {
@@ -81,6 +93,19 @@ namespace DepthsBelow
                     }
                 }
 
+                foreach (var soldier in core.Squad)
+                {
+                    var pathFinder = soldier.GetComponent<PathFinder>();
+                    var position = nextNode.Position;
+                    if (soldier.Transform.Grid == position)
+                    {
+                        soldierCollisions.Add(soldier.Transform.Grid);
+                        GetComponent<PathFinder>().RecreatePath(soldierCollisions);
+                        nextNode = GetComponent<PathFinder>().Next();
+                        break;
+                    }
+                }
+
                 if (nextNode != null)
                 {
                     var nodeWorldPos = Grid.GridToWorld(nextNode.Position);
@@ -96,7 +121,19 @@ namespace DepthsBelow
                         Transform.World.Y -= speed;
 
                     if (Transform.World == nodeWorldPos)
-                        nextNode = GetComponent<PathFinder>().Next();
+                    {
+                        if (currentStep < numberOfSteps)
+                        {
+                            currentStep++;
+                            //lastLastNode = lastNode;
+                            //lastNode = nextNode;
+                            nextNode = GetComponent<PathFinder>().Next();
+                        }
+                        else
+                        {
+                            GetComponent<PathFinder>().Stop();
+                        }
+                    }
                 }
 
             }