1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file    MSE2Collector.cpp
11 /// @author  Christian Roessel
12 /// @author  Daniel Krajzewicz
13 /// @author  Sascha Krieg
14 /// @author  Michael Behrisch
15 /// @author  Robbin Blokpoel
16 /// @author  Jakob Erdmann
17 /// @author  Leonhard Luecken
18 /// @date    Mon Feb 03 2014 10:13 CET
19 /// @version $Id$
20 ///
21 // An areal detector covering a sequence of consecutive lanes
22 /****************************************************************************/
23 
24 
25 /* TODO:
26  * tests:
27  *  - subsecond variant, ballistic variant
28  * allow omitting jam processing (?)
29  *
30  * Meso-compatibility? (esp. enteredLane-argument for MSBaseVehicle::notifyEnter() is not treated)
31  * Compatibility without internal lanes?
32  * Include leftVehicles into output?
33 */
34 
35 // ===========================================================================
36 // included modules
37 // ===========================================================================
38 #include <config.h>
39 
40 #include <cassert>
41 #include <algorithm>
42 #include "MSE2Collector.h"
43 #include <microsim/MSLane.h>
44 #include <microsim/MSNet.h>
45 #include <microsim/MSVehicle.h>
46 #include <microsim/MSVehicleType.h>
47 
48 //#define DEBUG_E2_CONSTRUCTOR
49 //#define DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
50 //#define DEBUG_E2_NOTIFY_MOVE
51 //#define DEBUG_E2_MAKE_VEHINFO
52 //#define DEBUG_E2_DETECTOR_UPDATE
53 //#define DEBUG_E2_TIME_ON_DETECTOR
54 //#define DEBUG_E2_JAMS
55 //#define DEBUG_E2_XML_OUT
56 //#define DEBUG_COND (true)
57 //#define DEBUG_COND (getID()=="e2Detector_e5.601A_1_SAa")
58 //#define DEBUG_COND (getID()=="702057")
59 //#define DEBUG_COND (getID()=="det0")
60 
MSE2Collector(const std::string & id,DetectorUsage usage,MSLane * lane,double startPos,double endPos,double length,SUMOTime haltingTimeThreshold,double haltingSpeedThreshold,double jamDistThreshold,const std::string & vTypes)61 MSE2Collector::MSE2Collector(const std::string& id,
62                              DetectorUsage usage, MSLane* lane, double startPos, double endPos, double length,
63                              SUMOTime haltingTimeThreshold, double haltingSpeedThreshold, double jamDistThreshold,
64                              const std::string& vTypes) :
65     MSMoveReminder(id, lane, false),
66     MSDetectorFileOutput(id, vTypes),
67     myUsage(usage),
68     myJamHaltingSpeedThreshold(haltingSpeedThreshold),
69     myJamHaltingTimeThreshold(haltingTimeThreshold),
70     myJamDistanceThreshold(jamDistThreshold),
71     myNumberOfEnteredVehicles(0),
72     myNumberOfSeenVehicles(0),
73     myNumberOfLeftVehicles(0) {
74     reset();
75 
76 #ifdef DEBUG_E2_CONSTRUCTOR
77     if DEBUG_COND {
78     std::cout << "\n" << "Creating MSE2Collector " << id
79               << " with lane = " << lane->getID()
80                   << " startPos = " << startPos
81                   << " endPos = " << endPos
82                   << " length = " << length
83                   << " haltingTimeThreshold = " << haltingTimeThreshold
84                   << " haltingSpeedThreshold = " << haltingSpeedThreshold
85                   << " jamDistThreshold = " << jamDistThreshold
86                   << std::endl;
87     }
88 #endif
89 
90     assert(lane != 0);
91 
92     // check that exactly one of length, startPos, endPos is invalid
93     bool lengthInvalid = length == std::numeric_limits<double>::max() || length <= 0;
94     bool endPosInvalid = endPos == std::numeric_limits<double>::max();
95     bool posInvalid = startPos == std::numeric_limits<double>::max();
96 
97     // check and normalize positions (assure positive values for pos and endPos, snap to lane-ends)
98     if (lengthInvalid) {
99         // assume that the detector is only located on a single lane
100         if (posInvalid) {
101             WRITE_WARNING("No valid detector length and start position given. Assuming startPos = 0 and length = end position");
102             startPos = 0;
103         }
104         if (endPosInvalid) {
105             WRITE_WARNING("No valid detector length and end position given. Assuming endPos = lane length and length = endPos-startPos");
106             endPos = lane->getLength();
107         }
108         endPos = endPos < 0 ? lane->getLength() + endPos : endPos;
109         startPos = startPos < 0 ? lane->getLength() + startPos : startPos;
110         bool valid = endPos <= lane->getLength() && 0 <= startPos && startPos < endPos;
111         if (!valid) {
112             throw InvalidArgument("Error in specification for E2Detector '" + id + "'. Positional argument is malformed. 0 <= pos < endPos <= lane.getLength() is required.");
113         }
114         // snap detector ends to lane ends
115         endPos = snap(endPos, lane->getLength(), POSITION_EPS);
116         startPos = snap(startPos, 0., POSITION_EPS);
117         length = endPos - startPos;
118     } else if (posInvalid) {
119         // endPosInvalid == false
120         endPos = endPos < 0 ? lane->getLength() + endPos : endPos;
121         endPos = snap(endPos, lane->getLength(), POSITION_EPS);
122     } else {
123         // posInvalid == false
124         startPos = startPos < 0 ? lane->getLength() + startPos : startPos;
125         startPos = snap(startPos, 0., POSITION_EPS);
126     }
127 
128     myStartPos = startPos;
129     myEndPos = endPos;
130 
131     std::vector<MSLane*> lanes;
132     if (posInvalid) {
133         lanes = selectLanes(lane, length, "bw");
134     } else if (endPosInvalid) {
135         lanes = selectLanes(lane, length, "fw");
136     } else {
137         // assuming detector is only located at a single lane
138         lanes.push_back(lane);
139     }
140 
141     initAuxiliaries(lanes);
142     checkPositioning(endPosInvalid, length);
143     addDetectorToLanes(lanes);
144 }
145 
146 
147 MSE2Collector::MSE2Collector(const std::string& id,
148                              DetectorUsage usage, std::vector<MSLane*> lanes, double startPos, double endPos,
149                              SUMOTime haltingTimeThreshold, double haltingSpeedThreshold, double jamDistThreshold,
150                              const std::string& vTypes) :
151     MSMoveReminder(id, lanes[lanes.size() - 1], false), // assure that lanes.size() > 0 at caller side!!!
152     MSDetectorFileOutput(id, vTypes),
153     myUsage(usage),
154     myFirstLane(lanes[0]),
155     myLastLane(lanes[lanes.size() - 1]),
156     myStartPos(startPos),
157     myEndPos(endPos),
158     myJamHaltingSpeedThreshold(haltingSpeedThreshold),
159     myJamHaltingTimeThreshold(haltingTimeThreshold),
160     myJamDistanceThreshold(jamDistThreshold),
161     myNumberOfEnteredVehicles(0),
162     myNumberOfSeenVehicles(0),
163     myNumberOfLeftVehicles(0) {
164     reset();
165 
166     for (std::vector<MSLane*>::const_iterator i = lanes.begin(); i != lanes.end(); ++i) {
167         assert((*i) != 0);
168     }
169 
170 #ifdef DEBUG_E2_CONSTRUCTOR
171     if DEBUG_COND {
172     std::cout << "\n" << "Creating MSE2Collector " << id
173               << " with endLane = " << myLastLane->getID()
174                   << " endPos = " << endPos
175                   << " startLane = " << myFirstLane->getID()
176                   << " startPos = " << startPos
177                   << " haltingTimeThreshold = " << haltingTimeThreshold
178                   << " haltingSpeedThreshold = " << haltingSpeedThreshold
179                   << " jamDistThreshold = " << jamDistThreshold
180                   << std::endl;
181     }
182 #endif
183 
184     myStartPos = myStartPos < 0 ? lanes[0]->getLength() + myStartPos : myStartPos;
185     myEndPos = myEndPos < 0 ? lanes[lanes.size() - 1]->getLength() + myEndPos : myEndPos;
186 
187     if (myStartPos < POSITION_EPS) {
188         myStartPos = 0;
189     }
190     if (myEndPos > lanes[lanes.size() - 1]->getLength() - POSITION_EPS) {
191         myEndPos = lanes[lanes.size() - 1]->getLength();
192     }
193 
194 
195     initAuxiliaries(lanes);
196     checkPositioning();
197     addDetectorToLanes(lanes);
198 }
199 
200 
201 void
202 MSE2Collector::checkPositioning(bool posGiven, double desiredLength) {
203     // check if detector was truncated
204     if (desiredLength > 0 && myDetectorLength < desiredLength - NUMERICAL_EPS) {
205         std::stringstream ss;
206         ss << "Cannot build detector of length " << desiredLength
207            << " because no further continuation lane was found for lane '" << (posGiven ? myLastLane->getID() : myFirstLane->getID())
208            << "'! Truncated detector at length " << myDetectorLength << ".";
209         WRITE_WARNING(ss.str());
210     }
211 
212     if (myDetectorLength < POSITION_EPS && (myStartPos > 0. || myEndPos < myLastLane->getLength())) {
213         // assure minimal detector length
214         double prolong = POSITION_EPS - myDetectorLength;
215         double startPos = MAX2(0., myStartPos - prolong); // new startPos
216         prolong -= myStartPos - startPos;
217         myStartPos = startPos;
218         if (prolong > 0.) {
219             myEndPos = MIN2(myEndPos + prolong, myLastLane->getLength());
220         }
221         WRITE_WARNING("Adjusted detector positioning to meet requirement length >= " + toString(POSITION_EPS)
222                       + ". New position is [" + toString(myStartPos) + "," + toString(myEndPos) + "]");
223     }
224 
225     // do some regularization snapping...
226     myStartPos = snap(myStartPos, 0., POSITION_EPS);
227     myStartPos = snap(myStartPos, myFirstLane->getLength() - POSITION_EPS, POSITION_EPS);
228     myStartPos = snap(myStartPos, 0., POSITION_EPS);
229     myEndPos = snap(myEndPos, myFirstLane->getLength(), POSITION_EPS);
230     myEndPos = snap(myEndPos, POSITION_EPS, POSITION_EPS);
231     myEndPos = snap(myEndPos, myFirstLane->getLength(), POSITION_EPS);
232     recalculateDetectorLength();
233 
234 #ifdef DEBUG_E2_CONSTRUCTOR
235     if DEBUG_COND {
236     std::stringstream ss;
237     //    ss << std::setprecision(32) << myEndPos << " : " << POSITION_EPS;
238     //    std::cout << ss.str() << std::endl;
239     std::cout << "myStartPos = " << myStartPos << std::endl;
240     std::cout << "myEndPos = " << myEndPos << std::endl;
241     std::cout << "myLastLane->getLength() = " << myLastLane->getLength() << std::endl;
242     }
243 #endif
244 
245 
246     assert((myStartPos >= POSITION_EPS  || myStartPos == 0) && myStartPos < myFirstLane->getLength());
247     assert(myEndPos <= myLastLane->getLength() - POSITION_EPS || myEndPos == myLastLane->getLength());
248     assert(myFirstLane != myLastLane || myEndPos - myStartPos > 0);
249 }
250 
251 
252 double
253 MSE2Collector::snap(double value, double snapPoint, double snapDist) {
254     if (fabs(value - snapPoint) < snapDist) {
255         return snapPoint;
256     } else {
257         return value;
258     }
259 }
260 
261 
262 void
263 MSE2Collector::recalculateDetectorLength() {
264     std::vector<std::string>::const_iterator i;
265     std::vector<MSLane*> lanes;
266     // get real lanes
267     for (i = myLanes.begin(); i != myLanes.end(); ++i) {
268         MSLane* lane = MSLane::dictionary(*i);
269         lanes.push_back(lane);
270     }
271 
272     // sum up their lengths
273     std::vector<MSLane*>::const_iterator j;
274     MSLane* previous = nullptr;
275     myDetectorLength = 0;
276     for (j = lanes.begin(); j != lanes.end(); ++j) {
277         // lane length
278         myDetectorLength += (*j)->getLength();
279         if (previous != nullptr && !MSGlobals::gUsingInternalLanes) {
280             // eventually link length
281             myDetectorLength += previous->getLinkTo(*j)->getLength();
282         }
283         previous = *j;
284     }
285     // substract uncovered area on first and last lane
286     myDetectorLength -= myStartPos;
287     myDetectorLength -= myLastLane->getLength() - myEndPos;
288 
289 #ifdef DEBUG_E2_CONSTRUCTOR
290     if DEBUG_COND {
291     std::cout << "Total detector length after recalculation = " << myDetectorLength << std::endl;
292 }
293 #endif
294 }
295 
296 
297 MSE2Collector::~MSE2Collector() {
298     // clear move notifications
299     for (std::vector<MoveNotificationInfo*>::iterator j = myMoveNotifications.begin(); j != myMoveNotifications.end(); ++j) {
300         delete *j;
301     }
302     myMoveNotifications.clear();
303 
304     // clear vehicle infos
305     for (VehicleInfoMap::iterator j = myVehicleInfos.begin(); j != myVehicleInfos.end(); ++j) {
306         delete j->second;
307     }
308     myVehicleInfos.clear();
309 }
310 
311 
312 std::vector<MSLane*>
313 MSE2Collector::selectLanes(MSLane* lane, double length, std::string dir) {
314     // direction of detector extension
315     assert(dir == "fw" || dir == "bw");
316     bool fw = dir == "fw";
317     double linkLength = 0; // linkLength (used if no internal lanes are present)
318     bool substractedLinkLength = false; // whether linkLength was substracted during the last iteration.
319 
320 #ifdef DEBUG_E2_CONSTRUCTOR
321     if DEBUG_COND {
322     std::cout << "\n" << "selectLanes()" << (fw ? "(forward)" : "(backward)") << std::endl;
323     }
324 #endif
325     std::vector<MSLane*> lanes;
326     // Selected lanes are stacked into vector 'lanes'. If dir == "bw" lanes will be reversed after this is done.
327     // The length is reduced while adding lanes to the detector
328     // First we adjust the starting value for length (in the first iteration, the whole length of the first considered lane is substracted,
329     // while it might only be partially covered by the detector)
330     if (fw) {
331         assert(myStartPos != std::numeric_limits<double>::max());
332         length += myStartPos;
333     } else {
334         assert(myEndPos != std::numeric_limits<double>::max());
335         length += lane->getLength() - myEndPos;
336     }
337     length = MAX2(POSITION_EPS, length); // this assures to add at least one lane to lanes
338     while (length >= POSITION_EPS && lane != nullptr) {
339         // Break loop for length <= NUMERICAL_EPS to avoid placement of very small
340         // detector piece on the end or beginning of one lane due to numerical rounding errors.
341         lanes.push_back(lane);
342 #ifdef DEBUG_E2_CONSTRUCTOR
343         if DEBUG_COND {
344         std::cout << "Added lane " << lane->getID()
345                       << " (length: " << lane->getLength() << ")" << std::endl;
346         }
347 #endif
348 
349         length -= lane->getLength();
350 
351         // proceed to upstream predecessor
352         if (fw) {
353             lane = lane->getCanonicalSuccessorLane();
354         } else {
355             lane = lane->getCanonicalPredecessorLane();
356         }
357 
358 
359         substractedLinkLength = false;
360         if (lane != nullptr && !MSGlobals::gUsingInternalLanes && length > POSITION_EPS) {
361             // In case wher no internal lanes are used,
362             // take into account the link length for the detector range
363             linkLength = 0;
364             if (fw) {
365                 linkLength = lanes.back()->getLinkTo(lane)->getLength();
366             } else {
367                 linkLength = lane->getLinkTo(lanes.back())->getLength();
368             }
369             length -= linkLength;
370             substractedLinkLength = true;
371         }
372 
373 
374 #ifdef DEBUG_E2_CONSTRUCTOR
375         if DEBUG_COND {
376         if (lane != 0) {
377                 std::cout << (fw ? "Successor lane: " : "Predecessor lane: ") << "'" << lane->getID() << "'";
378             }
379             std::cout << std::endl;
380         }
381 #endif
382     }
383 
384     if (substractedLinkLength) {
385         // if the link's length was substracted during the last step,
386         // the start/endPos would lie on a non-existing internal lane,
387         // therefore revert and truncate detector part on the non-existing internal lane.
388         length += linkLength;
389     }
390 
391 
392     // 1) At this point a negative <length> means that not the whole last stored lane lanes[lanes.size()-1]
393     // should be added to the detector, but the detector should spare out a part with length = -<length>
394     // If this part is too small (of length < POSITION_EPS) we take the whole lane
395     // 2) The whole lane is also taken for the case that <length> is positive. This corresponds to on
396     // of three situations: i) <length> < POSITION_EPS (break condition -> don't take too small pieces on the next lane)
397     //                 ii&iii) <length> >= POS_EPSILON may arise either if no continuation lane was found (lane==0), or
398     //                         in case of not using internal lanes if the detector end/start falls on a link.
399     // In all cases we take the whole last lane.
400     if (fw) {
401         if (length > -POSITION_EPS) {
402             myEndPos = lanes[lanes.size() - 1]->getLength();
403         } else if (length < 0) {
404             myEndPos = lanes[lanes.size() - 1]->getLength() + length;
405         }
406     } else {
407         if (length > -POSITION_EPS) {
408             myStartPos = 0;
409         } else if (length < 0) {
410             myStartPos = -length;
411         }
412     }
413 
414     // reverse lanes if lane selection was backwards
415     if (!fw) {
416         std::reverse(lanes.begin(), lanes.end());
417     }
418 
419     return lanes;
420 }
421 
422 void
423 MSE2Collector::addDetectorToLanes(std::vector<MSLane*>& lanes) {
424 #ifdef DEBUG_E2_CONSTRUCTOR
425     if DEBUG_COND {
426     std::cout << "\n" << "Adding detector " << myID << " to lanes:" << std::endl;
427 }
428 #endif
429 for (std::vector<MSLane*>::iterator l = lanes.begin(); l != lanes.end(); ++l) {
430         (*l)->addMoveReminder(this);
431 #ifdef DEBUG_E2_CONSTRUCTOR
432         if DEBUG_COND {
433         std::cout << (*l)->getID() << std::endl;
434         }
435 #endif
436     }
437 }
438 
439 void
440 MSE2Collector::initAuxiliaries(std::vector<MSLane*>& lanes) {
441     // Checks integrity of myLanes, adds internal-lane information, inits myLength, myFirstLane, myLastLane, myOffsets, myEndPos/myStartPos
442     myFirstLane = lanes[0];
443     myLastLane = lanes[lanes.size() - 1];
444 
445 #ifdef DEBUG_E2_CONSTRUCTOR
446     if DEBUG_COND {
447     std::cout << "\n" << "Initializing auxiliaries:"
448               << "\nFirst lane: " << myFirstLane->getID() << " (startPos = " << myStartPos << ")"
449                   << "\nLast lane: " << myLastLane->getID() << " (endPos = " << myEndPos << ")"
450                   << std::endl;
451     }
452 #endif
453 
454     // Init myOffsets and myDetectorLength.
455     // The loop below runs through the given lanes assuming the possibility that only non-internal lanes are given
456     // or at least not all relevant internal lanes are considered. During this a new, complete list of lane ids is
457     // built into myLanes.
458     myLanes.clear();
459 
460     // myDetectorLength will be increased in the loop below, always giving
461     // the offset of the currently considered lane to the detector start
462     myDetectorLength = -myStartPos;
463     myOffsets.clear();
464 
465     // loop over detector lanes and accumulate offsets with respect to the first lane's begin
466     // (these will be corrected afterwards by substracting the start position.)
467     std::vector<MSLane*>::iterator il = lanes.begin();
468 
469     // start on an internal lane?
470     // (This may happen if specifying the detector by its upstream
471     //  length starting from a given end position)
472     const MSLane* internal = (*il)->isInternal() ? *il : nullptr;
473 
474 #ifdef DEBUG_E2_CONSTRUCTOR
475     if DEBUG_COND {
476     std::cout << "\n" << "Initializing offsets:" << std::endl;
477 }
478 #endif
479 
480 #ifdef _MSC_VER
481 #pragma warning(push)
482 #pragma warning(disable: 4127) // do not warn about constant conditional expression
483 #endif
484 while (true) {
485 #ifdef _MSC_VER
486 #pragma warning(pop)
487 #endif
488         // Consider the next internal lanes
489         while (internal != nullptr) {
490             myLanes.push_back(internal->getID());
491             myOffsets.push_back(myDetectorLength);
492 
493 #ifdef DEBUG_E2_CONSTRUCTOR
494             if DEBUG_COND {
495             std::cout << "Offset for lane " << internal->getID() << " = " << myDetectorLength
496                           << std::endl;
497             }
498 #endif
499 
500             myDetectorLength += internal->getLength();
501             if (internal->getID() == myLastLane->getID()) {
502                 break;
503             }
504 
505             // There should be a unique continuation for each internal lane
506             assert(internal->getLinkCont().size() == 1);
507 
508             internal = internal->getLinkCont()[0]->getViaLaneOrLane();
509             if (!internal->isInternal()) {
510                 // passed the junction
511                 internal = nullptr;
512                 break;
513             }
514         }
515 
516         // Consider the next non-internal lane
517         // This is the first lane in the first iteration, if it is non-internal
518         // However, it can equal myLanes.end() if the myLastLane is internal. In that case we break.
519 
520         // Move il to next non-internal
521         while (il != lanes.end() && (*il)->isInternal()) {
522             il++;
523         }
524         if (il == lanes.end()) {
525             break;
526         }
527 
528         // There is still a non-internal lane to consider
529         MSLane* lane = *il;
530         myLanes.push_back(lane->getID());
531 
532 #ifdef DEBUG_E2_CONSTRUCTOR
533         if DEBUG_COND {
534         std::cout << "Offset for lane " << lane->getID() << " = " << myDetectorLength
535                       << std::endl;
536         }
537 #endif
538 
539         // Offset to detector start for this lane
540         myOffsets.push_back(myDetectorLength);
541 
542         // Add the lanes length to the detector offset
543         myDetectorLength += lane->getLength();
544 
545         // Get the next lane if this lane isn't the last one
546         if (++il == lanes.end()) {
547             break;
548         }
549 
550         if ((*il)->isInternal()) {
551             // next lane in myLanes is internal
552             internal = *il;
553             continue;
554         }
555 
556         // find the connection to next
557         const MSLink* link = lane->getLinkTo(*il);
558         if (link == nullptr) {
559             throw InvalidArgument("Lanes '" + lane->getID() + "' and '" + (*il)->getID() + "' are not consecutive in defintion of e2Detector '" + getID() + "'");
560         }
561 
562         if (!MSGlobals::gUsingInternalLanes) {
563             myDetectorLength += link->getLength();
564         } else {
565             internal = link->getViaLane();
566         }
567     }
568 
569     // Substract distance not covered on the last considered lane
570     bool fw = myEndPos == std::numeric_limits<double>::max();
571     if (fw) {
572         myDetectorLength -= myStartPos;
573     } else {
574         myDetectorLength -= myLastLane->getLength() - myEndPos;
575     }
576 
577 #ifdef DEBUG_E2_CONSTRUCTOR
578     if DEBUG_COND {
579     std::cout << "Total detector length after initAuxiliaries() = " << myDetectorLength << std::endl;
580 }
581 #endif
582 
583 // make lanes a complete list including internal lanes
584 lanes = getLanes();
585 }
586 
587 
588 std::vector<MSLane*>
589 MSE2Collector::getLanes() {
590     std::vector<MSLane*> res;
591     for (std::vector<std::string>::const_iterator i = myLanes.begin(); i != myLanes.end(); ++i) {
592         res.push_back(MSLane::dictionary(*i));
593     }
594     return res;
595 }
596 
597 
598 bool
599 MSE2Collector::notifyMove(SUMOTrafficObject& tObject, double oldPos,
600                           double newPos, double newSpeed) {
601     if (!tObject.isVehicle()) {
602         return false;
603     }
604     SUMOVehicle& veh = static_cast<SUMOVehicle&>(tObject);
605     VehicleInfoMap::iterator vi = myVehicleInfos.find(veh.getID());
606     assert(vi != myVehicleInfos.end()); // all vehicles calling notifyMove() should have called notifyEnter() before
607 
608     const std::string& vehID = veh.getID();
609     VehicleInfo& vehInfo = *(vi->second);
610 
611     // position relative to the detector start
612     double relPos = vehInfo.entryOffset + newPos;
613 
614     // update current distance to the detector end
615     vehInfo.distToDetectorEnd = myDetectorLength - relPos;
616 
617 #ifdef DEBUG_E2_NOTIFY_MOVE
618     if DEBUG_COND {
619     std::cout << "\n" << SIMTIME
620               << " MSE2Collector::notifyMove() (detID = " << myID << "on lane '" << myLane->getID() << "')"
621                   << " called by vehicle '" << vehID << "'"
622                   << " at relative position " << relPos
623                   << ", distToDetectorEnd = " << vehInfo.distToDetectorEnd << std::endl;
624     }
625 #endif
626 
627     // Check whether vehicle has reached the detector begin
628     if (relPos <= 0) {
629         // detector not yet reached, request being informed further
630 #ifdef DEBUG_E2_NOTIFY_MOVE
631         if DEBUG_COND {
632         std::cout << "Vehicle has not yet reached the detector start position." << std::endl;
633     }
634 #endif
635     return true;
636 } else if (!vehInfo.hasEntered) {
637         vehInfo.hasEntered = true;
638         myNumberOfEnteredVehicles++;
639         myNumberOfSeenVehicles++;
640     }
641 
642 
643     // determine whether vehicle has moved beyond the detector's end
644     bool vehPassedDetectorEnd = - vehInfo.exitOffset <= newPos - veh.getVehicleType().getLength();
645 
646     // determine whether vehicle has been on the detector at all
647     bool vehicleEnteredLaneAfterDetector = vehPassedDetectorEnd && (-vehInfo.exitOffset <= oldPos - veh.getVehicleType().getLength());
648     // ... if not, dont create any notification at all
649     if (vehicleEnteredLaneAfterDetector) {
650 #ifdef DEBUG_E2_NOTIFY_MOVE
651         if DEBUG_COND {
652         std::cout << "Vehicle entered lane behind detector." << std::endl;
653     }
654 #endif
655 } else {
656     myMoveNotifications.push_back(makeMoveNotification(veh, oldPos, newPos, newSpeed, vehInfo));
657     }
658 
659 
660     if (vehPassedDetectorEnd) {
661 #ifdef DEBUG_E2_NOTIFY_MOVE
662         if DEBUG_COND {
663         std::cout << "Vehicle has left the detector longitudinally." << std::endl;
664     }
665 #endif
666     // Vehicle is beyond the detector, unsubscribe and register removal from myVehicleInfos
667     myLeftVehicles.insert(vehID);
668         return false;
669     } else {
670         // Receive further notifications
671         return true;
672     }
673 }
674 
675 bool
676 MSE2Collector::notifyLeave(SUMOTrafficObject& veh, double /* lastPos */, MSMoveReminder::Notification reason, const MSLane* enteredLane) {
677 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
678     if DEBUG_COND {
679     std::cout << "\n" << SIMTIME << " notifyLeave() (detID = " << myID << "on lane '" << myLane->getID() << "')"
680                   << "called by vehicle '" << veh.getID() << "'" << std::endl;
681     }
682 #endif
683 
684     if (reason == MSMoveReminder::NOTIFICATION_JUNCTION) {
685         // vehicle left lane via junction, unsubscription and registering in myLeftVehicles when
686         // moving beyond the detector end is controlled in notifyMove.
687 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
688         if DEBUG_COND {
689         std::cout << SIMTIME << " Left longitudinally (along junction) -> keep subscription [handle exit in notifyMove()]" << std::endl;
690     }
691 #endif
692 
693     if (std::find(myLanes.begin(), myLanes.end(), enteredLane->getID()) == myLanes.end()) {
694             // Entered lane is not part of the detector
695             VehicleInfoMap::iterator vi = myVehicleInfos.find(veh.getID());
696             // Determine exit offset, where vehicle left the detector
697             double exitOffset = vi->second->entryOffset - myOffsets[vi->second->currentOffsetIndex] - vi->second->currentLane->getLength();
698             vi->second->exitOffset = MAX2(vi->second->exitOffset, exitOffset);
699 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
700             if DEBUG_COND {
701             std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' leaves the detector. Exit offset = " << vi->second->exitOffset << std::endl;
702             }
703 #endif
704         }
705 
706         return true;
707     } else {
708         VehicleInfoMap::iterator vi = myVehicleInfos.find(veh.getID());
709         // erase vehicle, which leaves in a non-longitudinal way, immediately
710         if (vi->second->hasEntered) {
711             myNumberOfLeftVehicles++;
712         }
713         delete vi->second;
714         myVehicleInfos.erase(vi);
715 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
716         if DEBUG_COND {
717         std::cout << SIMTIME << " Left non-longitudinally (lanechange, teleport, parking, etc) -> discard subscription" << std::endl;
718     }
719 #endif
720     return false;
721 }
722 }
723 
724 
725 bool
726 MSE2Collector::notifyEnter(SUMOTrafficObject& tObject, MSMoveReminder::Notification reason, const MSLane* enteredLane) {
727     if (!tObject.isVehicle()) {
728         return false;
729     }
730     SUMOVehicle& veh = static_cast<SUMOVehicle&>(tObject);
731 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
732     if DEBUG_COND {
733     std::cout << std::endl << SIMTIME << " notifyEnter() (detID = " << myID << " on lane '" << myLane->getID() << "')"
734                   << " called by vehicle '" << veh.getID()
735                   << "' entering lane '" << (enteredLane != 0 ? enteredLane->getID() : "NULL") << "'" << std::endl;
736     }
737 #endif
738 
739     // notifyEnter() should only be called for lanes of the detector
740     assert(std::find(myLanes.begin(), myLanes.end(), enteredLane->getID()) != myLanes.end());
741 
742     assert(veh.getLane() == enteredLane);
743 
744     if (!vehicleApplies(veh)) {
745         // That's not my type...
746         return false;
747     }
748 
749     // determine whether the vehicle entered the lane behind the detector end
750     // e.g. due to lane change manoeuver
751     if (reason != NOTIFICATION_JUNCTION) {
752         const double vehBackPos = veh.getBackPositionOnLane(enteredLane);
753         bool vehEnteredBehindDetectorEnd = (enteredLane == myLastLane) && myEndPos <= vehBackPos;
754         if (vehEnteredBehindDetectorEnd) {
755             // this vehicle cannot influence detector readings, do not subscribe
756             // to move notifications
757 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
758             if DEBUG_COND {
759             std::cout << "Vehicle entered the lane behind the detector, ignoring it." << std::endl;
760             std::cout << "(myEndPos = " << this->myEndPos << ", veh.getBackPositionOnLane() = " << vehBackPos << ")" << std::endl;
761         }
762 #endif
763         return false;
764     }
765 }
766 
767 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
768 if DEBUG_COND {
769     if (!veh.isOnRoad()) {
770             // Vehicle is teleporting over the edge
771             std::cout << "Vehicle is off road (teleporting over edge)..." << std::endl;
772         }
773     }
774 #endif
775 
776     const std::string& vehID = veh.getID();
777     VehicleInfoMap::iterator vi = myVehicleInfos.find(vehID);
778     if (vi != myVehicleInfos.end()) {
779         // Register move current offset to the next lane
780         vi->second->currentOffsetIndex++;
781         vi->second->currentLane = enteredLane;
782 
783 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
784         if DEBUG_COND {
785         std::cout << SIMTIME << " Vehicle '" << veh.getID() << "' on lane '" << veh.getLane()->getID()
786                       << "' already known. No new VehicleInfo is created.\n"
787                       << "enteredLane = " << enteredLane->getID() << "\nmyLanes[vi->offset] = " << myLanes[vi->second->currentOffsetIndex]
788                       << std::endl;
789         }
790 #endif
791         assert(myLanes[vi->second->currentOffsetIndex] == enteredLane->getID());
792 
793         // but don't add a second subscription for another lane
794         return false;
795     }
796 
797 
798 
799 #ifdef DEBUG_E2_NOTIFY_ENTER_AND_LEAVE
800     if DEBUG_COND {
801     std::cout << SIMTIME << " Adding VehicleInfo for vehicle '" << veh.getID() << "'." << std::endl;
802     }
803 #endif
804 
805     // Add vehicle info
806     myVehicleInfos.insert(std::make_pair(vehID, makeVehicleInfo(veh, enteredLane)));
807     // Subscribe to vehicle's movement notifications
808     return true;
809 }
810 
811 MSE2Collector::VehicleInfo*
812 MSE2Collector::makeVehicleInfo(const SUMOVehicle& veh, const MSLane* enteredLane) const {
813     // The vehicle's distance to the detector end
814     int j = (int)(std::find(myLanes.begin(), myLanes.end(), enteredLane->getID()) - myLanes.begin());
815     assert(j >= 0 && j < (int)myLanes.size());
816     double entryOffset = myOffsets[j];
817     double distToDetectorEnd = myDetectorLength - (entryOffset + veh.getPositionOnLane());
818     bool onDetector = -entryOffset < veh.getPositionOnLane() && distToDetectorEnd > -veh.getVehicleType().getLength();
819 
820 #ifdef DEBUG_E2_MAKE_VEHINFO
821     if DEBUG_COND {
822     std::cout << SIMTIME << " Making VehicleInfo for vehicle '" << veh.getID() << "'."
823                   << "\ndistToDetectorEnd = " << distToDetectorEnd
824                   << "\nveh.getPositionOnLane() = " << veh.getPositionOnLane()
825                   << "\nentry lane offset (lane begin from detector begin) = " << entryOffset
826                   << std::endl;
827     }
828 #endif
829     return new VehicleInfo(veh.getID(), veh.getVehicleType().getID(), veh.getVehicleType().getLength(), veh.getVehicleType().getMinGap(), enteredLane, entryOffset, j,
830                            myOffsets[j] - myDetectorLength, distToDetectorEnd, onDetector);
831 }
832 
833 void
834 MSE2Collector::detectorUpdate(const SUMOTime /* step */) {
835 
836 #ifdef DEBUG_E2_DETECTOR_UPDATE
837     if DEBUG_COND {
838     std::cout << "\n" << SIMTIME << " detectorUpdate() for detector '" << myID << "'"
839               << "\nmyCurrentMeanSpeed = " << myCurrentMeanSpeed
840               << "\nmyCurrentMeanLength = " << myCurrentMeanLength
841               << "\nmyNumberOfEnteredVehicles = " << myNumberOfEnteredVehicles
842               << "\nmyNumberOfLeftVehicles = " << myNumberOfLeftVehicles
843               << "\nmyNumberOfSeenVehicles = " << myNumberOfSeenVehicles
844               << std::endl;
845 }
846 #endif
847 
848 // sort myMoveNotifications (required for jam processing) ascendingly according to vehicle's distance to the detector end
849 // (min = myMoveNotifications[0].distToDetectorEnd)
850 std::sort(myMoveNotifications.begin(), myMoveNotifications.end(), compareMoveNotification);
851 
852     // reset values concerning current time step (these are updated in integrateMoveNotification() and aggregateOutputValues())
853     myCurrentMeanSpeed = 0;
854     myCurrentMeanLength = 0;
855     myCurrentStartedHalts = 0;
856     myCurrentHaltingsNumber = 0;
857 
858     JamInfo* currentJam = nullptr;
859     std::vector<JamInfo*> jams;
860     std::map<std::string, SUMOTime> haltingVehicles;
861     std::map<std::string, SUMOTime> intervalHaltingVehicles;
862 
863     // go through the list of vehicles positioned on the detector
864     for (std::vector<MoveNotificationInfo*>::iterator i = myMoveNotifications.begin(); i != myMoveNotifications.end(); ++i) {
865         // The ID of the vehicle that has sent this notification in the last step
866         const std::string& vehID = (*i)->id;
867         VehicleInfoMap::iterator vi = myVehicleInfos.find(vehID);
868 
869         if (vi == myVehicleInfos.end()) {
870             // The vehicle has already left the detector by lanechange, teleport, etc. (not longitudinal)
871             integrateMoveNotification(nullptr, *i);
872         } else {
873             // Add move notification infos to detector values and VehicleInfo
874             integrateMoveNotification(vi->second, *i);
875         }
876         // construct jam structure
877         bool isInJam = checkJam(i, haltingVehicles, intervalHaltingVehicles);
878         buildJam(isInJam, i, currentJam, jams);
879     }
880 
881     // extract some aggregated values from the jam structure
882     processJams(jams, currentJam);
883 
884     // Aggregate and normalize values for the detector output
885     aggregateOutputValues();
886 
887     // save information about halting vehicles
888     myHaltingVehicleDurations = haltingVehicles;
889     myIntervalHaltingVehicleDurations = intervalHaltingVehicles;
890 
891 #ifdef DEBUG_E2_DETECTOR_UPDATE
892     if DEBUG_COND {
893     std::cout << "\n" << SIMTIME << " Current lanes for vehicles still on or approaching the detector:" << std::endl;
894 }
895 #endif
896 // update current and entered lanes for remaining vehicles
897 VehicleInfoMap::iterator iv;
898 for (iv = myVehicleInfos.begin(); iv != myVehicleInfos.end(); ++iv) {
899 #ifdef DEBUG_E2_DETECTOR_UPDATE
900         if DEBUG_COND {
901         std::cout << " Vehicle '" << iv->second->id << "'" << ": '"
902                   << iv->second->currentLane->getID() << "'"
903                       << std::endl;
904         }
905 #endif
906     }
907 
908 #ifdef DEBUG_E2_DETECTOR_UPDATE
909     if DEBUG_COND {
910     std::cout << SIMTIME << " Discarding vehicles that have left the detector:" << std::endl;
911 }
912 #endif
913 // Remove the vehicles that have left the detector
914 std::set<std::string>::const_iterator i;
915 for (i = myLeftVehicles.begin(); i != myLeftVehicles.end(); ++i) {
916         VehicleInfoMap::iterator j = myVehicleInfos.find(*i);
917         delete j->second;
918         myVehicleInfos.erase(*i);
919         myNumberOfLeftVehicles++;
920 #ifdef DEBUG_E2_DETECTOR_UPDATE
921         if DEBUG_COND {
922         std::cout << "Erased vehicle '" << *i << "'" << std::endl;
923     }
924 #endif
925 }
926 myLeftVehicles.clear();
927 
928     // reset move notifications
929     for (std::vector<MoveNotificationInfo*>::iterator j = myMoveNotifications.begin(); j != myMoveNotifications.end(); ++j) {
930         delete *j;
931     }
932     myMoveNotifications.clear();
933 }
934 
935 
936 void
937 MSE2Collector::aggregateOutputValues() {
938     myTimeSamples += 1;
939     // compute occupancy values (note myCurrentMeanLength is still not normalized here, but holds the sum of all vehicle lengths)
940     const double currentOccupancy = myCurrentMeanLength / myDetectorLength * (double) 100.;
941     myCurrentOccupancy = currentOccupancy;
942     myOccupancySum += currentOccupancy;
943     myMaxOccupancy = MAX2(myMaxOccupancy, currentOccupancy);
944     // compute jam values
945     myMeanMaxJamInVehicles += myCurrentMaxJamLengthInVehicles;
946     myMeanMaxJamInMeters += myCurrentMaxJamLengthInMeters;
947     myMaxJamInVehicles = MAX2(myMaxJamInVehicles, myCurrentMaxJamLengthInVehicles);
948     myMaxJamInMeters = MAX2(myMaxJamInMeters, myCurrentMaxJamLengthInMeters);
949     // compute information about vehicle numbers
950     const int numVehicles = (int)myMoveNotifications.size();
951     myMeanVehicleNumber += numVehicles;
952     myMaxVehicleNumber = MAX2(numVehicles, myMaxVehicleNumber);
953     // norm current values
954     myCurrentMeanSpeed = numVehicles != 0 ? myCurrentMeanSpeed / (double) numVehicles : -1;
955     myCurrentMeanLength = numVehicles != 0 ? myCurrentMeanLength / (double) numVehicles : -1;
956 }
957 
958 
959 
960 void
961 MSE2Collector::integrateMoveNotification(VehicleInfo* vi, const MoveNotificationInfo* mni) {
962 
963 #ifdef DEBUG_E2_DETECTOR_UPDATE
964     if DEBUG_COND {
965     std::cout << SIMTIME << " integrateMoveNotification() for vehicle '" << mni->id << "'"
966               << "\ntimeOnDetector = " << mni->timeOnDetector
967               << "\nlengthOnDetector = " << mni->lengthOnDetector
968               << "\ntimeLoss = " << mni->timeLoss
969               << "\nspeed = " << mni->speed
970               << std::endl;
971 }
972 #endif
973 
974 // Accumulate detector values
975 myVehicleSamples += mni->timeOnDetector;
976 myTotalTimeLoss += mni->timeLoss;
977 mySpeedSum += mni->speed * mni->timeOnDetector;
978 myCurrentMeanSpeed += mni->speed * mni->timeOnDetector;
979 myCurrentMeanLength += mni->lengthOnDetector;
980 
981 if (vi != nullptr) {
982         // Accumulate individual values for the vehicle.
983         // @note vi==0 occurs, if the vehicle info has been erased at
984         //       notifyLeave() in case of a non-longitudinal exit (lanechange, teleport, etc.)
985         vi->totalTimeOnDetector += mni->timeOnDetector;
986         vi->accumulatedTimeLoss += mni->timeLoss;
987         vi->lastAccel = mni->accel;
988         vi->lastSpeed = mni->speed;
989         vi->lastPos = myStartPos + vi->entryOffset + mni->newPos;
990         vi->onDetector = mni->onDetector;
991     }
992 }
993 
994 
995 
996 MSE2Collector::MoveNotificationInfo*
997 MSE2Collector::makeMoveNotification(const SUMOVehicle& veh, double oldPos, double newPos, double newSpeed, const VehicleInfo& vehInfo) const {
998 #ifdef DEBUG_E2_NOTIFY_MOVE
999     if DEBUG_COND {
1000     std::cout << SIMTIME << " makeMoveNotification() for vehicle '" << veh.getID() << "'"
1001                   << " oldPos = " << oldPos << " newPos = " << newPos << " newSpeed = " << newSpeed
1002                   << std::endl;
1003     }
1004 #endif
1005 
1006     // Timefraction in [0,TS] the vehicle has spend on the detector in the last step
1007     double timeOnDetector;
1008     // Note that at this point, vehInfo.currentLane points to the lane at the beginning of the last timestep,
1009     // and vehInfo.enteredLanes is a list of lanes entered in the last timestep
1010     double timeLoss;
1011     calculateTimeLossAndTimeOnDetector(veh, oldPos, newPos, vehInfo, timeOnDetector, timeLoss);
1012 
1013     // The length of the part of the vehicle on the detector at the end of the last time step
1014     // may be shorter than vehicle's length if its back reaches out
1015     double lengthOnDetector = MAX2(MIN2(vehInfo.length, newPos + vehInfo.entryOffset), 0.);
1016 
1017     // XXX: Fulfulling the specifications of the documentation (lengthOnDetector = time integral
1018     //      over length of the vehicle's part on the detector) would be much more cumbersome.
1019     double distToExit = -vehInfo.exitOffset - newPos;
1020     // Eventually decrease further to account for the front reaching out
1021     lengthOnDetector = MAX2(0., lengthOnDetector + MIN2(0., distToExit));
1022 
1023     // whether the vehicle is still on the detector at the end of the time step
1024     bool stillOnDetector = -distToExit < vehInfo.length;
1025 
1026 #ifdef DEBUG_E2_NOTIFY_MOVE
1027     if DEBUG_COND {
1028     std::cout << SIMTIME << " lengthOnDetector = " << lengthOnDetector
1029               << "\nvehInfo.exitOffset = " << vehInfo.exitOffset
1030               << " vehInfo.entryOffset = " << vehInfo.entryOffset
1031               << " distToExit = " << distToExit
1032               << std::endl;
1033 }
1034 #endif
1035 
1036 /* Store new infos */
1037 return new MoveNotificationInfo(veh.getID(), oldPos, newPos, newSpeed, veh.getAcceleration(),
1038                                 myDetectorLength - (vehInfo.entryOffset + newPos),
1039                                 timeOnDetector, lengthOnDetector, timeLoss, stillOnDetector);
1040 }
1041 
1042 void
1043 MSE2Collector::buildJam(bool isInJam, std::vector<MoveNotificationInfo*>::const_iterator mni, JamInfo*& currentJam, std::vector<JamInfo*>& jams) {
1044 #ifdef DEBUG_E2_JAMS
1045     if DEBUG_COND {
1046     std::cout << SIMTIME << " buildJam() for vehicle '" << (*mni)->id << "'" << std::endl;
1047     }
1048 #endif
1049     if (isInJam) {
1050         // The vehicle is in a jam;
1051         //  it may be a new one or already an existing one
1052         if (currentJam == nullptr) {
1053 #ifdef DEBUG_E2_JAMS
1054             if DEBUG_COND {
1055             std::cout << SIMTIME << " vehicle '" << (*mni)->id << "' forms the start of the first jam" << std::endl;
1056             }
1057 #endif
1058             // the vehicle is the first vehicle in a jam
1059             currentJam = new JamInfo();
1060             currentJam->firstStandingVehicle = mni;
1061         } else {
1062             // ok, we have a jam already. But - maybe it is too far away
1063             //  ... honestly, I can hardly find a reason for doing this,
1064             //  but jams were defined this way in an earlier version...
1065             MoveNotificationInfo* lastVeh = *currentJam->lastStandingVehicle;
1066             MoveNotificationInfo* currVeh = *mni;
1067             if (lastVeh->distToDetectorEnd - currVeh->distToDetectorEnd > myJamDistanceThreshold) {
1068 #ifdef DEBUG_E2_JAMS
1069                 if DEBUG_COND {
1070                 std::cout << SIMTIME << " vehicle '" << (*mni)->id << "' forms the start of a new jam" << std::endl;
1071                 }
1072 #endif
1073                 // yep, yep, yep - it's a new one...
1074                 //  close the frist, build a new
1075                 jams.push_back(currentJam);
1076                 currentJam = new JamInfo();
1077                 currentJam->firstStandingVehicle = mni;
1078             }
1079         }
1080         currentJam->lastStandingVehicle = mni;
1081     } else {
1082         // the vehicle is not part of a jam...
1083         //  maybe we have to close an already computed jam
1084         if (currentJam != nullptr) {
1085 #ifdef DEBUG_E2_JAMS
1086             if DEBUG_COND {
1087             std::cout << SIMTIME << " Closing current jam." << std::endl;
1088         }
1089 #endif
1090         jams.push_back(currentJam);
1091             currentJam = nullptr;
1092         }
1093     }
1094 }
1095 
1096 
1097 bool
1098 MSE2Collector::checkJam(std::vector<MoveNotificationInfo*>::const_iterator mni, std::map<std::string, SUMOTime>& haltingVehicles, std::map<std::string, SUMOTime>& intervalHaltingVehicles) {
1099 #ifdef DEBUG_E2_JAMS
1100     if DEBUG_COND {
1101     std::cout << SIMTIME << " CheckJam() for vehicle '" << (*mni)->id << "'" << std::endl;
1102     }
1103 #endif
1104     // jam-checking begins
1105     bool isInJam = false;
1106     // first, check whether the vehicle is slow enough to be counted as halting
1107     if ((*mni)->speed < myJamHaltingSpeedThreshold) {
1108         myCurrentHaltingsNumber++;
1109         // we have to track the time it was halting;
1110         // so let's look up whether it was halting before and compute the overall halting time
1111         bool wasHalting = myHaltingVehicleDurations.count((*mni)->id) > 0;
1112         if (wasHalting) {
1113             haltingVehicles[(*mni)->id] = myHaltingVehicleDurations[(*mni)->id] + DELTA_T;
1114             intervalHaltingVehicles[(*mni)->id] = myIntervalHaltingVehicleDurations[(*mni)->id] + DELTA_T;
1115         } else {
1116 #ifdef DEBUG_E2_JAMS
1117             if DEBUG_COND {
1118             std::cout << SIMTIME << " vehicle '" << (*mni)->id << "' starts halting." << std::endl;
1119             }
1120 #endif
1121             haltingVehicles[(*mni)->id] = DELTA_T;
1122             intervalHaltingVehicles[(*mni)->id] = DELTA_T;
1123             myCurrentStartedHalts++;
1124             myStartedHalts++;
1125         }
1126         // we now check whether the halting time is large enough
1127         if (haltingVehicles[(*mni)->id] > myJamHaltingTimeThreshold) {
1128             // yep --> the vehicle is a part of a jam
1129             isInJam = true;
1130         }
1131     } else {
1132         // is not standing anymore; keep duration information
1133         std::map<std::string, SUMOTime>::iterator v = myHaltingVehicleDurations.find((*mni)->id);
1134         if (v != myHaltingVehicleDurations.end()) {
1135             myPastStandingDurations.push_back(v->second);
1136             myHaltingVehicleDurations.erase(v);
1137         }
1138         v = myIntervalHaltingVehicleDurations.find((*mni)->id);
1139         if (v != myIntervalHaltingVehicleDurations.end()) {
1140             myPastIntervalStandingDurations.push_back((*v).second);
1141             myIntervalHaltingVehicleDurations.erase(v);
1142         }
1143     }
1144 #ifdef DEBUG_E2_JAMS
1145     if DEBUG_COND {
1146     std::cout << SIMTIME << " vehicle '" << (*mni)->id << "'" << (isInJam ? "is jammed." : "is not jammed.") << std::endl;
1147     }
1148 #endif
1149     return isInJam;
1150 }
1151 
1152 
1153 void
1154 MSE2Collector::processJams(std::vector<JamInfo*>& jams, JamInfo* currentJam) {
1155     // push last jam
1156     if (currentJam != nullptr) {
1157         jams.push_back(currentJam);
1158         currentJam = nullptr;
1159     }
1160 
1161 #ifdef DEBUG_E2_JAMS
1162     if DEBUG_COND {
1163     std::cout << "\n" << SIMTIME << " processJams()"
1164               << "\nNumber of jams: " << jams.size() << std::endl;
1165     }
1166 #endif
1167 
1168     // process jam information
1169     myCurrentMaxJamLengthInMeters = 0;
1170     myCurrentMaxJamLengthInVehicles = 0;
1171     myCurrentJamLengthInMeters = 0;
1172     myCurrentJamLengthInVehicles = 0;
1173     for (std::vector<JamInfo*>::const_iterator i = jams.begin(); i != jams.end(); ++i) {
1174         // compute current jam's values
1175         MoveNotificationInfo* lastVeh = *((*i)->lastStandingVehicle);
1176         MoveNotificationInfo* firstVeh = *((*i)->firstStandingVehicle);
1177         const double jamLengthInMeters = lastVeh->distToDetectorEnd
1178                                          - firstVeh->distToDetectorEnd
1179                                          + lastVeh->lengthOnDetector;
1180         const int jamLengthInVehicles = (int) distance((*i)->firstStandingVehicle, (*i)->lastStandingVehicle) + 1;
1181         // apply them to the statistics
1182         myCurrentMaxJamLengthInMeters = MAX2(myCurrentMaxJamLengthInMeters, jamLengthInMeters);
1183         myCurrentMaxJamLengthInVehicles = MAX2(myCurrentMaxJamLengthInVehicles, jamLengthInVehicles);
1184         myJamLengthInMetersSum += jamLengthInMeters;
1185         myJamLengthInVehiclesSum += jamLengthInVehicles;
1186         myCurrentJamLengthInMeters += jamLengthInMeters;
1187         myCurrentJamLengthInVehicles += jamLengthInVehicles;
1188 #ifdef DEBUG_E2_JAMS
1189         if DEBUG_COND {
1190         std::cout << SIMTIME << " processing jam nr." << ((int) distance((std::vector<JamInfo*>::const_iterator) jams.begin(), i) + 1)
1191                       << "\njamLengthInMeters = " << jamLengthInMeters
1192                       << " jamLengthInVehicles = " << jamLengthInVehicles
1193                       << std::endl;
1194         }
1195 #endif
1196     }
1197     myCurrentJamNo = (int) jams.size();
1198 
1199     // clean up jam structure
1200     for (std::vector<JamInfo*>::iterator i = jams.begin(); i != jams.end(); ++i) {
1201         delete *i;
1202     }
1203 }
1204 
1205 void
1206 MSE2Collector::calculateTimeLossAndTimeOnDetector(const SUMOVehicle& veh, double oldPos, double newPos, const VehicleInfo& vi, double& timeOnDetector, double& timeLoss) const {
1207     assert(veh.getID() == vi.id);
1208     assert(newPos + vi.entryOffset >= 0);
1209 
1210     if (oldPos == newPos) {
1211         // vehicle is stopped
1212         timeLoss = TS;
1213         timeOnDetector = TS;
1214         return;
1215     }
1216 
1217     // Eventual positional offset of the detector start from the lane's start
1218     double entryPos = MAX2(-vi.entryOffset, 0.);
1219     // Time of this vehicle entering the detector in the last time step
1220     double entryTime = 0;
1221     // Take into account the time before entering the detector, if there is.
1222     if (oldPos < entryPos) {
1223         // Vehicle entered the detector in the last step, either traversing the detector start or somewhere in the middle.
1224         entryTime = MSCFModel::passingTime(oldPos, entryPos, newPos, veh.getPreviousSpeed(), veh.getSpeed());
1225     }
1226     // speed at detector entry
1227     double entrySpeed = MSCFModel::speedAfterTime(entryTime, veh.getPreviousSpeed(), newPos - oldPos);
1228     // Calculate time spent on detector until reaching newPos or a detector exit
1229     double exitPos = MIN2(newPos, -vi.exitOffset + vi.length);
1230     assert(entryPos < exitPos);
1231 
1232     // calculate vehicle's time spent on the detector
1233     double exitTime;
1234     if (exitPos == newPos) {
1235         exitTime  = TS;
1236     } else {
1237         exitTime = MSCFModel::passingTime(oldPos, exitPos, newPos, veh.getPreviousSpeed(), veh.getSpeed());
1238     }
1239 
1240     // Vehicle's Speed when leaving the detector
1241     double exitSpeed = MSCFModel::speedAfterTime(exitTime, veh.getPreviousSpeed(), newPos - oldPos);
1242 
1243     // Maximal speed on vehicle's current lane (== lane before last time step)
1244     // Note: this disregards the possibility of different maximal speeds on different traversed lanes.
1245     //       (we accept this as discretization error)
1246     double vmax = MAX2(veh.getLane()->getVehicleMaxSpeed(&veh), NUMERICAL_EPS);
1247 
1248     // Time loss suffered on the detector
1249     timeOnDetector = exitTime - entryTime;
1250     timeLoss = MAX2(0., timeOnDetector * (vmax - (entrySpeed + exitSpeed) / 2) / vmax);
1251 
1252 #ifdef DEBUG_E2_TIME_ON_DETECTOR
1253     if DEBUG_COND {
1254     std::cout << SIMTIME << " calculateTimeLoss() for vehicle '" << veh.getID() << "'"
1255                   << " oldPos = " << oldPos << " newPos = " << newPos
1256                   << " entryPos = " << entryPos << " exitPos = " << exitPos
1257                   << " timeOnDetector = " << timeOnDetector
1258                   << " timeLoss = " << timeLoss
1259                   << std::endl;
1260     }
1261 #endif
1262 }
1263 
1264 
1265 void
1266 MSE2Collector::writeXMLDetectorProlog(OutputDevice& dev) const {
1267     dev.writeXMLHeader("detector", "det_e2_file.xsd");
1268 }
1269 
1270 void
1271 MSE2Collector::writeXMLOutput(OutputDevice& dev, SUMOTime startTime, SUMOTime stopTime) {
1272     dev << "   <interval begin=\"" << time2string(startTime) << "\" end=\"" << time2string(stopTime) << "\" " << "id=\"" << getID() << "\" ";
1273 
1274     const double meanSpeed = myVehicleSamples != 0 ? mySpeedSum / myVehicleSamples : -1;
1275     const double meanOccupancy = myTimeSamples != 0 ? myOccupancySum / (double) myTimeSamples : 0;
1276     const double meanJamLengthInMeters = myTimeSamples != 0 ? myMeanMaxJamInMeters / (double) myTimeSamples : 0;
1277     const double meanJamLengthInVehicles = myTimeSamples != 0 ? myMeanMaxJamInVehicles / (double) myTimeSamples : 0;
1278     const double meanVehicleNumber = myTimeSamples != 0 ? (double) myMeanVehicleNumber / (double) myTimeSamples : 0;
1279     const double meanTimeLoss = myNumberOfSeenVehicles != 0 ? myTotalTimeLoss / myNumberOfSeenVehicles : -1;
1280 
1281     SUMOTime haltingDurationSum = 0;
1282     SUMOTime maxHaltingDuration = 0;
1283     int haltingNo = 0;
1284     for (std::vector<SUMOTime>::iterator i = myPastStandingDurations.begin(); i != myPastStandingDurations.end(); ++i) {
1285         haltingDurationSum += (*i);
1286         maxHaltingDuration = MAX2(maxHaltingDuration, (*i));
1287         haltingNo++;
1288     }
1289     for (std::map<std::string, SUMOTime> ::iterator i = myHaltingVehicleDurations.begin(); i != myHaltingVehicleDurations.end(); ++i) {
1290         haltingDurationSum += (*i).second;
1291         maxHaltingDuration = MAX2(maxHaltingDuration, (*i).second);
1292         haltingNo++;
1293     }
1294     const SUMOTime meanHaltingDuration = haltingNo != 0 ? haltingDurationSum / haltingNo : 0;
1295 
1296     SUMOTime intervalHaltingDurationSum = 0;
1297     SUMOTime intervalMaxHaltingDuration = 0;
1298     int intervalHaltingNo = 0;
1299     for (std::vector<SUMOTime>::iterator i = myPastIntervalStandingDurations.begin(); i != myPastIntervalStandingDurations.end(); ++i) {
1300         intervalHaltingDurationSum += (*i);
1301         intervalMaxHaltingDuration = MAX2(intervalMaxHaltingDuration, (*i));
1302         intervalHaltingNo++;
1303     }
1304     for (std::map<std::string, SUMOTime> ::iterator i = myIntervalHaltingVehicleDurations.begin(); i != myIntervalHaltingVehicleDurations.end(); ++i) {
1305         intervalHaltingDurationSum += (*i).second;
1306         intervalMaxHaltingDuration = MAX2(intervalMaxHaltingDuration, (*i).second);
1307         intervalHaltingNo++;
1308     }
1309     const SUMOTime intervalMeanHaltingDuration = intervalHaltingNo != 0 ? intervalHaltingDurationSum / intervalHaltingNo : 0;
1310 
1311 #ifdef DEBUG_E2_XML_OUT
1312     if DEBUG_COND {
1313     std::stringstream ss;
1314     ss  << "sampledSeconds=\"" << myVehicleSamples << "\" "
1315         << "myTimeSamples=\"" << myTimeSamples << "\" "
1316         << "myOccupancySum=\"" << myOccupancySum << "\" "
1317         << "myMeanVehicleNumber=\"" << myMeanVehicleNumber << "\" "
1318         << "nVehEntered=\"" << myNumberOfEnteredVehicles << "\" "
1319         << "meanSpeed=\"" << meanSpeed << "\"";
1320     std::cout << ss.str() << std::endl;
1321     }
1322 #endif
1323 
1324 
1325     dev << "sampledSeconds=\"" << myVehicleSamples << "\" "
1326         << "nVehEntered=\"" << myNumberOfEnteredVehicles << "\" "
1327         << "nVehLeft=\"" << myNumberOfLeftVehicles << "\" "
1328         << "nVehSeen=\"" << myNumberOfSeenVehicles << "\" "
1329         << "meanSpeed=\"" << meanSpeed << "\" "
1330         << "meanTimeLoss=\"" << meanTimeLoss << "\" "
1331         << "meanOccupancy=\"" << meanOccupancy << "\" "
1332         << "maxOccupancy=\"" << myMaxOccupancy << "\" "
1333         << "meanMaxJamLengthInVehicles=\"" << meanJamLengthInVehicles << "\" "
1334         << "meanMaxJamLengthInMeters=\"" << meanJamLengthInMeters << "\" "
1335         << "maxJamLengthInVehicles=\"" << myMaxJamInVehicles << "\" "
1336         << "maxJamLengthInMeters=\"" << myMaxJamInMeters << "\" "
1337         << "jamLengthInVehiclesSum=\"" << myJamLengthInVehiclesSum << "\" "
1338         << "jamLengthInMetersSum=\"" << myJamLengthInMetersSum << "\" "
1339         << "meanHaltingDuration=\"" << STEPS2TIME(meanHaltingDuration) << "\" "
1340         << "maxHaltingDuration=\"" << STEPS2TIME(maxHaltingDuration) << "\" "
1341         << "haltingDurationSum=\"" << STEPS2TIME(haltingDurationSum) << "\" "
1342         << "meanIntervalHaltingDuration=\"" << STEPS2TIME(intervalMeanHaltingDuration) << "\" "
1343         << "maxIntervalHaltingDuration=\"" << STEPS2TIME(intervalMaxHaltingDuration) << "\" "
1344         << "intervalHaltingDurationSum=\"" << STEPS2TIME(intervalHaltingDurationSum) << "\" "
1345         << "startedHalts=\"" << myStartedHalts << "\" "
1346         << "meanVehicleNumber=\"" << meanVehicleNumber << "\" "
1347         << "maxVehicleNumber=\"" << myMaxVehicleNumber << "\" "
1348         << "/>\n";
1349     reset();
1350 
1351 }
1352 
1353 void
1354 MSE2Collector::reset() {
1355     myVehicleSamples = 0;
1356     myTotalTimeLoss = 0.;
1357     myNumberOfEnteredVehicles = 0;
1358     myNumberOfSeenVehicles -= myNumberOfLeftVehicles;
1359     myNumberOfLeftVehicles = 0;
1360     myMaxVehicleNumber = 0;
1361 
1362     mySpeedSum = 0;
1363     myStartedHalts = 0;
1364     myJamLengthInMetersSum = 0;
1365     myJamLengthInVehiclesSum = 0;
1366     myOccupancySum = 0;
1367     myMaxOccupancy = 0;
1368     myMeanMaxJamInVehicles = 0;
1369     myMeanMaxJamInMeters = 0;
1370     myMaxJamInVehicles = 0;
1371     myMaxJamInMeters = 0;
1372     myTimeSamples = 0;
1373     myMeanVehicleNumber = 0;
1374     for (std::map<std::string, SUMOTime>::iterator i = myIntervalHaltingVehicleDurations.begin(); i != myIntervalHaltingVehicleDurations.end(); ++i) {
1375         (*i).second = 0;
1376     }
1377     myPastStandingDurations.clear();
1378     myPastIntervalStandingDurations.clear();
1379 }
1380 
1381 
1382 int
1383 MSE2Collector::getCurrentVehicleNumber() const {
1384     int result = 0;
1385     for (VehicleInfoMap::const_iterator it = myVehicleInfos.begin(); it != myVehicleInfos.end(); it++) {
1386         if (it->second->onDetector) {
1387             result++;
1388         }
1389     }
1390     return result;
1391 }
1392 
1393 
1394 
1395 std::vector<std::string>
1396 MSE2Collector::getCurrentVehicleIDs() const {
1397     std::vector<std::string> ret;
1398     for (VehicleInfoMap::const_iterator i = myVehicleInfos.begin(); i != myVehicleInfos.end(); ++i) {
1399         if (i->second->onDetector) {
1400             ret.push_back(i->second->id);
1401         }
1402     }
1403     std::sort(ret.begin(), ret.end());
1404     return ret;
1405 }
1406 
1407 
1408 std::vector<MSE2Collector::VehicleInfo*>
1409 MSE2Collector::getCurrentVehicles() const {
1410     std::vector<VehicleInfo*> res;
1411     VehicleInfoMap::const_iterator i;
1412     for (i = myVehicleInfos.begin(); i != myVehicleInfos.end(); ++i) {
1413         if (i->second->onDetector) {
1414             res.push_back(i->second);
1415         }
1416     }
1417     return res;
1418 }
1419 
1420 
1421 
1422 int
1423 MSE2Collector::getEstimatedCurrentVehicleNumber(double speedThreshold) const {
1424 
1425 //    double distance = std::numeric_limits<double>::max();
1426     double thresholdSpeed = myLane->getSpeedLimit() / speedThreshold;
1427 
1428     int count = 0;
1429     for (VehicleInfoMap::const_iterator it = myVehicleInfos.begin();
1430             it != myVehicleInfos.end(); it++) {
1431         if (it->second->onDetector) {
1432 //            if (it->position < distance) {
1433 //                distance = it->position;
1434 //            }
1435 //            const double realDistance = myLane->getLength() - distance; // the closer vehicle get to the light the greater is the distance
1436             const double realDistance = it->second->distToDetectorEnd;
1437             if (it->second->lastSpeed <= thresholdSpeed || it->second->lastAccel > 0) { //TODO speed less half of the maximum speed for the lane NEED TUNING
1438                 count = (int)(realDistance / (it->second->length + it->second->minGap)) + 1;
1439             }
1440         }
1441     }
1442 
1443     return count;
1444 }
1445 
1446 double
1447 MSE2Collector::getEstimateQueueLength() const {
1448 
1449     if (myVehicleInfos.empty()) {
1450         return -1;
1451     }
1452 
1453     double distance = std::numeric_limits<double>::max();
1454     double realDistance = 0;
1455     bool flowing =  true;
1456     for (VehicleInfoMap::const_iterator it = myVehicleInfos.begin();
1457             it != myVehicleInfos.end(); it++) {
1458         if (it->second->onDetector) {
1459             distance = MIN2(it->second->lastPos, distance);
1460             //  double distanceTemp = myLane->getLength() - distance;
1461             if (it->second->lastSpeed <= 0.5) {
1462                 realDistance = distance - it->second->length + it->second->minGap;
1463                 flowing = false;
1464             }
1465 //            DBG(
1466 //                std::ostringstream str;
1467 //                str << time2string(MSNet::getInstance()->getCurrentTimeStep())
1468 //                << " MSE2Collector::getEstimateQueueLength::"
1469 //                << " lane " << myLane->getID()
1470 //                << " vehicle " << it->second.id
1471 //                << " positionOnLane " << it->second.position
1472 //                << " vel " << it->second.speed
1473 //                << " realDistance " << realDistance;
1474 //                WRITE_MESSAGE(str.str());
1475 //            )
1476         }
1477     }
1478     if (flowing) {
1479         return 0;
1480     } else {
1481         return myLane->getLength() - realDistance;
1482     }
1483 }
1484 
1485 /****************************************************************************/
1486 
1487