diff --git a/Moose Development/Moose/Ops/ArmyGroup.lua b/Moose Development/Moose/Ops/ArmyGroup.lua
index c566c5b1c..c063d3c18 100644
--- a/Moose Development/Moose/Ops/ArmyGroup.lua	
+++ b/Moose Development/Moose/Ops/ArmyGroup.lua	
@@ -140,6 +140,18 @@ function ARMYGROUP:GetClosestRoad()
   return self:GetCoordinate():GetClosestPointToRoad()
 end
 
+--- Get 2D distance to the closest road.
+-- @param #ARMYGROUP self
+-- @return #number Distance in meters to the closest road.
+function ARMYGROUP:GetClosestRoadDist()
+  local road=self:GetClosestRoad()
+  if road then
+    local dist=road:Get2DDistance(self:GetCoordinate())
+    return dist
+  end
+  return math.huge
+end
+
 
 --- Add a *scheduled* task to fire at a given coordinate.
 -- @param #ARMYGROUP self
@@ -372,80 +384,75 @@ function ARMYGROUP:onafterUpdateRoute(From, Event, To, n, Speed, Formation)
   -- Waypoints.
   local waypoints={}
   
-  -- Total number of waypoints
-  local N=#self.waypoints
-
-  -- Add remaining waypoints to route.
-  for i=n, N do
+  -- Next waypoint.
+  local wp=UTILS.DeepCopy(self.waypoints[n]) --Ops.OpsGroup#OPSGROUP.Waypoint
   
-    -- Copy waypoint.
-    local wp=UTILS.DeepCopy(self.waypoints[i]) --Ops.OpsGroup#OPSGROUP.Waypoint
-    
-    if i==n then
-    
-      ---
-      -- Next Waypoint
-      ---
-    
-      if Speed then
-        wp.speed=UTILS.KnotsToMps(Speed)
-      else
-        -- Take default waypoint speed.
-      end
-      
-      if self.formationPerma then
-        --if self.formationPerma==ENUMS.Formation.Vehicle.OnRoad then
-          wp.action=self.formationPerma
-        --end
-      elseif Formation then 
-        wp.action=Formation
-      end
-      
-      -- Current set formation.
-      self.option.Formation=wp.action
-      
-      -- Current set speed in m/s.
-      self.speedWp=wp.speed
-    
-    else
+  -- Do we want to drive on road to the next wp?
+  local onroad=wp.action==ENUMS.Formation.Vehicle.OnRoad  
 
-      ---
-      -- Later Waypoint(s)
-      ---
-    
-      if self.formationPerma then
-        wp.action=self.formationPerma
-      else
-        -- Take default waypoint speed.
-      end
-      
-    end
-    
-    if wp.roaddist>100 and wp.action==ENUMS.Formation.Vehicle.OnRoad then
-    
-      -- Waypoint is actually off road!
-      wp.action=ENUMS.Formation.Vehicle.OffRoad
+  -- Current point.
+  local current=self:GetCoordinate():WaypointGround(UTILS.MpsToKmph(self.speedWp), ENUMS.Formation.Vehicle.OffRoad)
+  table.insert(waypoints, current)
+
+  -- Insert a point on road.
+  if onroad then
+    local current=self:GetClosestRoad():WaypointGround(UTILS.MpsToKmph(self.speedWp), ENUMS.Formation.Vehicle.OnRoad)
+    table.insert(waypoints, current)
+  end
 
-      -- Add "On Road" waypoint in between.
-      local wproad=wp.roadcoord:WaypointGround(wp.speed, ENUMS.Formation.Vehicle.OnRoad)
-      table.insert(waypoints, wproad)     
-    end    
-     
-    -- Debug info.
-    self:T(string.format("WP %d %s: Speed=%d m/s, alt=%d m, Action=%s", i, wp.type, wp.speed, wp.alt, wp.action))
         
-    -- Add waypoint.
-    table.insert(waypoints, wp)
+  -- Speed.
+  if Speed then
+    wp.speed=UTILS.KnotsToMps(Speed)
+  else
+    -- Take default waypoint speed.
+  end
+  
+  -- Formation.
+  if self.formationPerma then
+    wp.action=self.formationPerma
+  elseif Formation then 
+    wp.action=Formation
+  end
+  
+  -- Current set formation.
+  self.option.Formation=wp.action
+  
+  -- Current set speed in m/s.
+  self.speedWp=wp.speed
+
+  -- Add waypoint in between because this waypoint is "On Road" but lies "Off Road".
+  if onroad then
+  
+    -- The real waypoint is actually off road.
+    wp.action=ENUMS.Formation.Vehicle.OffRoad
+
+    -- Add "On Road" waypoint in between.
+    local wproad=wp.roadcoord:WaypointGround(wp.speed, ENUMS.Formation.Vehicle.OnRoad) --Ops.OpsGroup#OPSGROUP.Waypoint
+    
+    -- Insert road waypoint.
+    table.insert(waypoints, wproad)
+  end    
+        
+  -- Add waypoint.
+  table.insert(waypoints, wp)
+  
+  -- Debug output.
+  if false then
+    for i,_wp in pairs(waypoints) do
+      local wp=_wp
+      local text=string.format("WP #%d UID=%d type=%s: Speed=%d m/s, alt=%d m, Action=%s", i, wp.uid and wp.uid or 0, wp.type, wp.speed, wp.alt, wp.action)
+      self:I(text)
+      if wp.coordinate then
+        wp.coordinate:MarkToAll(text)
+      end
+    end
   end
 
 
-  -- Current waypoint.
-  local current=self:GetCoordinate():WaypointGround(UTILS.MpsToKmph(self.speedWp), self.option.Formation)
-  table.insert(waypoints, 1, current)
-  table.insert(waypoints, 1, current)  -- Seems to be better to add this twice. Otherwise, the passing waypoint functions is triggered to early!
-
-  if #waypoints>2 then
+  if #waypoints>=2 then
   
+    -- Debug info.
     self:T(self.lid..string.format("Updateing route: WP %d-->%d-->%d (#%d), Speed=%.1f knots, Formation=%s", 
     self.currentwp, n, #self.waypoints, #waypoints-2, UTILS.MpsToKnots(self.speedWp), tostring(self.option.Formation)))
 
@@ -646,7 +653,7 @@ end
 
 --- Add an a waypoint to the route.
 -- @param #ARMYGROUP self
--- @param Core.Point#COORDINATE Coordinate The coordinate of the waypoint. Use COORDINATE:SetAltitude(altitude) to define the altitude.
+-- @param Core.Point#COORDINATE Coordinate The coordinate of the waypoint.
 -- @param #number Speed Speed in knots. Default is default cruise speed or 70% of max speed.
 -- @param #number AfterWaypointWithID Insert waypoint after waypoint given ID. Default is to insert as last waypoint.
 -- @param #number Formation Formation the group will use.
@@ -683,7 +690,7 @@ function ARMYGROUP:AddWaypoint(Coordinate, Speed, AfterWaypointWithID, Formation
   end
   
   -- Debug info.
-  self:T(self.lid..string.format("Adding waypoint UID=%d (index=%d), Speed=%.1f knots, Dist2Road=%d m, Action=%s", waypoint.uid, wpnumber, Speed, waypoint.roaddist, waypoint.action))
+  self:I(self.lid..string.format("Adding waypoint UID=%d (index=%d), Speed=%.1f knots, Dist2Road=%d m, Action=%s", waypoint.uid, wpnumber, Speed, waypoint.roaddist, waypoint.action))
   
   -- Update route.
   if Updateroute==nil or Updateroute==true then
diff --git a/Moose Development/Moose/Ops/OpsGroup.lua b/Moose Development/Moose/Ops/OpsGroup.lua
index 67b6335b1..2d0010b78 100644
--- a/Moose Development/Moose/Ops/OpsGroup.lua	
+++ b/Moose Development/Moose/Ops/OpsGroup.lua	
@@ -1133,18 +1133,21 @@ end
 --- Get next waypoint index.
 -- @param #OPSGROUP self
 -- @param #boolean cyclic If true, return first waypoint if last waypoint was reached. Default is patrol ad infinitum value set.
+-- @param #number i Waypoint index from which the next index is returned. Default is the last waypoint passed.
 -- @return #number Next waypoint index.
-function OPSGROUP:GetWaypointIndexNext(cyclic)
+function OPSGROUP:GetWaypointIndexNext(cyclic, i)
 
   if cyclic==nil then
     cyclic=self.adinfinitum
   end
   
   local N=#self.waypoints
-
-  local n=math.min(self.currentwp+1, N)
   
-  if cyclic and self.currentwp==N then
+  i=i or self.currentwp
+
+  local n=math.min(i+1, N)
+  
+  if cyclic and i==N then
     n=1
   end
   
@@ -2677,18 +2680,18 @@ function OPSGROUP:onafterPassingWaypoint(From, Event, To, Waypoint)
     -- Set switch to true.    
     self.passedfinalwp=true
     
-    -- Check if all tasks/mission are done? If so, RTB or WAIT.
-    -- Note, we delay it for a second to let the OnAfterPassingwaypoint function to be executed in case someone wants to add another waypoint there.
-    if ntasks==0 then
-      self:_CheckGroupDone(1)
-    end
+  end
 
+  -- Check if all tasks/mission are done? If so, RTB or WAIT.
+  -- Note, we delay it for a second to let the OnAfterPassingwaypoint function to be executed in case someone wants to add another waypoint there.
+  if ntasks==0 then
+    self:_CheckGroupDone()
   end
 
   -- Debug info.
   local text=string.format("Group passed waypoint %s/%d ID=%d: final=%s detour=%s astar=%s", 
   tostring(wpindex), #self.waypoints, Waypoint.uid, tostring(self.passedfinalwp), tostring(Waypoint.detour), tostring(Waypoint.astar))
-  self:T(self.lid..text)
+  self:I(self.lid..text)
   
 end
 
@@ -3058,92 +3061,60 @@ function OPSGROUP:_CheckGroupDone(delay)
       self:ScheduleOnce(delay, self._CheckGroupDone, self)
     else
     
-      if self.passedfinalwp then
-      
-        ---
-        -- Passed FINAL waypoint
-        ---
-      
-        if #self.waypoints>1 then
-        
-          if self.adinfinitum then
-          
-            -- Get positive speed to first waypoint.
-            local speed=self:GetSpeedToWaypoint(1)
-          
-            -- Start route at first waypoint.
-            self:__UpdateRoute(-1, 1, speed)
-            
-            self:T(self.lid..string.format("Passed final WP, #WP>1, adinfinitum=TRUE ==> Goto WP 1 at speed>0"))
-                        
-            self.passedfinalwp=false
-            
-          else
-            -- No further waypoints. Command a full stop.
-            self:__FullStop(-1)
-            
-            self:T(self.lid..string.format("Passed final WP, #WP>1, adinfinitum=FALSE ==> Full Stop"))
-          end
-          
-        elseif #self.waypoints==1 then
-        
-          --- Only one WP left
-        
-          -- The last waypoint.
-          local waypoint=self.waypoints[1] --Ops.OpsGroup#OPSGROUP.Waypoint
-          
-          local dist=self:GetCoordinate():Get2DDistance(waypoint.coordinate)          
-          
-          
-          if self.adinfinitum and dist>1000 then  -- Note that dist>100 caused the same wp to be passed a lot of times.
-          
-            self:T(self.lid..string.format("Passed final WP, #WP=1, adinfinitum=TRUE dist>1000 ==> Goto WP 1 at speed>0"))
-
-            -- Get positive speed to first waypoint.
-            local speed=self:GetSpeedToWaypoint(1)
-          
-            -- Start route at first waypoint.
-            self:__UpdateRoute(-1, 1, speed)
-            
-            self.passedfinalwp=false
-            
-          else
-          
-            self:T(self.lid..string.format("Passed final WP, #WP=1, adinfinitum=FALSE or dist<1000 ==> Full Stop"))
-          
-            self:__FullStop(-1)
-            
-          end
-          
-        else
-        
-          --- No waypoints left
-
-          -- No further waypoints. Command a full stop.
-          self:T(self.lid..string.format("No waypoints left ==> Full Stop"))
-          
-          self:__FullStop(-1)
-          
-        end
     
+      if self.adinfinitum then
+      
+        ---
+        -- Parol Ad Infinitum
+        ---
+      
+        -- Next waypoint index.
+        local i=self:GetWaypointIndexNext(true)
+        
+        -- Get positive speed to first waypoint.
+        local speed=self:GetSpeedToWaypoint(i)
+        
+        -- Start route at first waypoint.
+        self:UpdateRoute(i, speed)
+        
+        self:T(self.lid..string.format("Adinfinitum=TRUE ==> Goto WP index=%d at speed=%d knots", i, speed))
+
       else
       
         ---
-        -- Final waypoint NOT passed yet
+        -- Finite Patrol
         ---
-      
-        if #self.waypoints>0 then
-          self:T(self.lid..string.format("NOT Passed final WP, #WP>0 ==> Update Route"))
-          self:__UpdateRoute(-1)
-        else
-          self:E(self.lid..string.format("WARNING: No waypoints left! Commanding a Full Stop"))
+    
+        if self.passedfinalwp then
+        
+          ---
+          -- Passed FINAL waypoint
+          ---
+  
+          -- No further waypoints. Command a full stop.
           self:__FullStop(-1)
+              
+          self:T(self.lid..string.format("Passed final WP, #WP>1, adinfinitum=FALSE ==> Full Stop"))
+  
+        else
+        
+          ---
+          -- Final waypoint NOT passed yet
+          ---
+        
+          if #self.waypoints>0 then
+            self:T(self.lid..string.format("NOT Passed final WP, #WP>0 ==> Update Route"))
+            self:UpdateRoute()
+          else
+            self:E(self.lid..string.format("WARNING: No waypoints left! Commanding a Full Stop"))
+            self:__FullStop(-1)
+          end
+          
         end
         
       end
-    
-    end
-    
+      
+    end    
   end
   
 end
@@ -3368,7 +3339,7 @@ function OPSGROUP:_UpdateWaypointTasks(n)
     if i>=n or nwaypoints==1 then
     
       -- Debug info.
-      self:T(self.lid..string.format("Updating waypoint task for waypoint %d/%d ID=%d. Last waypoint passed %d", i, nwaypoints, wp.uid, self.currentwp))
+      self:T2(self.lid..string.format("Updating waypoint task for waypoint %d/%d ID=%d. Last waypoint passed %d", i, nwaypoints, wp.uid, self.currentwp))
   
       -- Tasks of this waypoint
       local taskswp={}
@@ -3401,12 +3372,12 @@ function OPSGROUP._PassingWaypoint(group, opsgroup, uid)
   
   if waypoint then
   
+    -- Current wp.
+    local currentwp=opsgroup.currentwp
+  
     -- Get the current waypoint index.
     opsgroup.currentwp=opsgroup:GetWaypointIndex(uid)
-    
-    -- Increase passing counter.
-    waypoint.npassed=waypoint.npassed+1
-    
+        
     -- Set expected speed and formation from the next WP.
     local wpnext=opsgroup:GetWaypointNext()  
     if wpnext then
@@ -3450,13 +3421,18 @@ function OPSGROUP._PassingWaypoint(group, opsgroup, uid)
   
     -- Debug message.
     local text=string.format("Group passing waypoint uid=%d", uid)
-    opsgroup:T2(opsgroup.lid..text)
-  
+    opsgroup:T(opsgroup.lid..text)
+    
     -- Trigger PassingWaypoint event.
     if not (waypoint.astar or waypoint.detour) then
+
+      -- Increase passing counter.
+      waypoint.npassed=waypoint.npassed+1    
+      
+      -- Call event function.
       opsgroup:PassingWaypoint(waypoint)
     end
-    
+
   end
 
 end