Fixed TODOs in "OSMBehaviorController"

VadereSimulator/src/org/vadere/simulator/models/osm/OSMBehaviorController.java

@@ -15,6 +15,7 @@ import org.vadere.state.scenario.Topography;
 import org.vadere.state.simulation.FootStep;
 import org.vadere.util.geometry.shapes.VPoint;
 import org.vadere.util.geometry.shapes.Vector2D;
+import org.vadere.util.logging.Logger;
 
 import java.util.LinkedList;
 import java.util.List;
@@ -23,7 +24,9 @@ import java.util.stream.Collectors;
 /**
  * A class to encapsulate the behavior of a single {@link PedestrianOSM}.
  *
- * This class can be used by {@link OptimalStepsModel} to react on stimuli.
+ * This class can be used by {@link OptimalStepsModel} to react to
+ * environmental stimuli (see {@link Stimulus}) and how an agent
+ * has categorized itself in regard to other agents (see {@link SelfCategory}).
  *
  * For instance:
  * <pre>
@@ -36,6 +39,8 @@ import java.util.stream.Collectors;
  */
 public class OSMBehaviorController {
 
+    private static Logger logger = Logger.getLogger(OSMBehaviorController.class);
+
     /**
      * Prepare move of pedestrian inside the topography. The pedestrian object already has the new
      * location (Vpoint to) stored within its position attribute. This method only informs the
@@ -109,7 +114,9 @@ public class OSMBehaviorController {
             pedestrian.setTargets(nextTarget);
             pedestrian.setCombinedPotentialStrategy(CombinedPotentialStrategy.TARGET_DISTRACTION_STRATEGY);
         } else {
-            // TODO: Maybe, log to console.
+            logger.debug(String.format("Expected: %s, Received: %s"),
+                    Bang.class.getSimpleName(),
+                    mostImportantStimulus.getClass().getSimpleName());
         }
     }
 
@@ -121,7 +128,9 @@ public class OSMBehaviorController {
             pedestrian.setTargets(changeTarget.getNewTargetIds());
 
         } else {
-            // TODO: Maybe, log to console.
+            logger.debug(String.format("Expected: %s, Received: %s"),
+                    ChangeTarget.class.getSimpleName(),
+                    mostImportantStimulus.getClass().getSimpleName());
         }
     }
 
@@ -153,6 +162,7 @@ public class OSMBehaviorController {
 			    }
 		    }
 	    }
+
 	    return null;
     }
 
@@ -163,7 +173,7 @@ public class OSMBehaviorController {
         List<Pedestrian> closestPedestrians = topography.getSpatialMap(Pedestrian.class)
                 .getObjects(positionOfPedestrian, pedestrian.getAttributes().getSearchRadius());
 
-        // Filter out "me" and pedestrians which are farer away from target than "me".
+        // Filter out "me" and pedestrians which are further away from target than "me".
         closestPedestrians = closestPedestrians.stream()
                 .filter(candidate -> pedestrian.getId() != candidate.getId())
                 .filter(candidate -> pedestrian.getTargetPotential(candidate.getPosition()) < pedestrian.getTargetPotential(pedestrian.getPosition()))
@@ -172,7 +182,10 @@ public class OSMBehaviorController {
         // Sort by distance away from "me".
         closestPedestrians = closestPedestrians.stream()
                 .sorted((pedestrian1, pedestrian2) ->
-                Double.compare(positionOfPedestrian.distance(pedestrian1.getPosition()), positionOfPedestrian.distance(pedestrian2.getPosition())))
+                        Double.compare(
+                                positionOfPedestrian.distance(pedestrian1.getPosition()),
+                                positionOfPedestrian.distance(pedestrian2.getPosition())
+                        ))
                 .collect(Collectors.toList());
 
         return closestPedestrians;
@@ -250,6 +263,13 @@ public class OSMBehaviorController {
         return angleInRadian;
     }
 
+    /**
+     * Swap two pedestrians.
+     *
+     * Watch out: This method manipulates pedestrian2 which is contained in a queue
+     * sorted by timeOfNextStep! The calling code must re-add pedestrian2 after
+     * invoking this method.
+     */
     public void swapPedestrians(PedestrianOSM pedestrian1, PedestrianOSM pedestrian2, Topography topography) {
         VPoint newPosition = pedestrian2.getPosition().clone();
         VPoint oldPosition = pedestrian1.getPosition().clone();
@@ -257,22 +277,20 @@ public class OSMBehaviorController {
         pedestrian1.setNextPosition(newPosition);
         pedestrian2.setNextPosition(oldPosition);
 
-        // Use "makeStep()" to swap both pedestrians to avoid "java.lang.AssertionError:
-        // Number of pedestrians in LinkedCellGrid does not match number of pedestrians in topography".
+        // Synchronize movement of both pedestrians
 	    double startTimeStep = pedestrian1.getTimeOfNextStep();
 	    double durationStep = Math.max(pedestrian1.getDurationNextStep(), pedestrian2.getDurationNextStep());
 	    double endTimeStep = startTimeStep + durationStep;
 
-	    // here we interrupt the current footstep of pedestrian 2 to sync it to pedestrian 1
-	    // this is only required for the sequential update scheme since pedestrian 2 might have done some steps in this time step and
-	    // is ahead (with respect to the time) of pedestrian 1. We remove those steps which is not a good solution!
+	    // We interrupt the current footstep of pedestrian 2 to sync it with
+        // pedestrian 1. It is only required for the sequential update scheme
+        // since pedestrian 2 might have done some steps in this time step and
+	    // is ahead (with respect to the time) of pedestrian 1.
+        // We remove those steps which is not a good solution!
 	    if(!pedestrian2.getTrajectory().isEmpty()) {
-
-	    	// the agent was standing still, otherwise this should not be allowed
 		    pedestrian2.getTrajectory().adjustEndTime(startTimeStep);
 	    }
 
-	    // Note: Here we manipulate a pedestrian which is contained in the queue sorted by timeOfNextStep!
 	    pedestrian1.setTimeOfNextStep(startTimeStep);
 	    pedestrian2.setTimeOfNextStep(startTimeStep);
 
@@ -280,12 +298,11 @@ public class OSMBehaviorController {
         makeStep(pedestrian2, topography, durationStep);
 
         // TODO The experiment showed that speed decreased (to half of free-flow velocity).
-        //   Therefore, use "pedestrian.getDurationNextStep() * 2".
+        //   Maybe, use "pedestrian.getDurationNextStep() * 2".
         pedestrian1.setTimeOfNextStep(endTimeStep);
         pedestrian2.setTimeOfNextStep(endTimeStep);
 
 	    pedestrian1.setTimeCredit(pedestrian1.getTimeCredit() - durationStep);
-//	    assert pedestrian1.getTimeCredit() >= 0.0;
 	    pedestrian2.setTimeCredit(pedestrian1.getTimeCredit());
     }
 }