SSMDevice: Corrected typos, cleaned debug, and recognizing earlier fo…

…und conflict location as the relevant one.

src/microsim/devices/MSDevice_SSM.cpp

@@ -2372,22 +2372,22 @@ MSDevice_SSM::findSurroundingVehicles(const MSVehicle& veh, double range, FoeInf
     // Init pos with vehicle's current position. Below pos is set to zero to denote
     // the beginning position of the currently considered edge
     double pos = veh.getPositionOnLane();
-    // remainingRange is the range minus the distance that is already scanned downstream along the vehicles route
+    // remainingDownstreamRange is the range minus the distance that is already scanned downstream along the vehicles route
     double remainingDownstreamRange = range;
     // distToConflictLane is the distance of the ego vehicle to the start of the currently considered potential conflict lane (can be negative for its current lane)
     double distToConflictLane = -pos;
     // junctions that were already scanned (break search in recurrent nets)
     std::set<const MSJunction*> seenJunctions;
 
-    // if the current edge is internal, collect all vehicles from the junction and below range upstream (except on the vehicles own edge),
+    // if the current edge is internal, collect all vehicles from the junction and within upstream range (except on the vehicles own edge),
     // this is analogous to the code treating junctions in the loop below. Note that the distance on the junction itself is not included into
     // range, so vehicles farther away than range can be collected, too.
     if (lane->isInternal()) {
         edge = &(lane->getEdge());
 
 #ifdef DEBUG_SSM_SURROUNDING
-        std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' is on internal edge " << edge->getID() << "'.\n"
-                  << "Previous edge of its route: '" << (*edgeIter)->getID() << "'" << std::endl;
+        std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' is on internal edge " << edge->getID() << "'." << std::endl;
+//                  << "Previous edge of its route: '" << (*edgeIter)->getID() << "'" << std::endl;
 #endif
 
         assert(edge->getToJunction() == edge->getFromJunction());
@@ -2548,6 +2548,10 @@ MSDevice_SSM::getUpstreamVehicles(const MSEdge* edge, double pos, double range,
         const MSLane::VehCont& vehicles = lane->getVehiclesSecure();
         for (MSLane::VehCont::const_iterator vi = vehicles.begin(); vi != vehicles.end(); ++vi) {
             MSVehicle* veh = *vi;
+            if (foeCollector.find(veh) != foeCollector.end()) {
+                // vehicle already recognized, earlier recognized conflict has priority
+                continue;
+            }
             if (veh->getPositionOnLane() - veh->getLength() <= pos && veh->getPositionOnLane() >= pos - range) {
 #ifdef DEBUG_SSM
                 std::cout << veh->getID()  << "\n";