1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.util.Iterator;
4 import java.util.LinkedHashMap;
5 import java.util.LinkedHashSet;
6 import java.util.LinkedList;
7 import java.util.Map;
8 import java.util.Queue;
9 import java.util.Set;
10 import java.util.SortedMap;
11 import java.util.TreeMap;
12
13 import org.djunits.value.vdouble.scalar.Length;
14 import org.djutils.exceptions.Try;
15 import org.opentrafficsim.core.gtu.GtuException;
16 import org.opentrafficsim.core.gtu.RelativePosition;
17 import org.opentrafficsim.core.network.Link;
18 import org.opentrafficsim.core.network.route.Route;
19 import org.opentrafficsim.road.gtu.lane.LaneBasedGtu;
20 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35 public abstract class AbstractPerceptionIterable<H extends Headway, U, C> extends AbstractPerceptionReiterable<H, U>
36 {
37
38
39 private final LaneRecordInterface<?> root;
40
41
42 private final Length initialPosition;
43
44
45 private final boolean downstream;
46
47
48 private final double maxDistance;
49
50
51 private final RelativePosition relativePosition;
52
53
54 private final Route route;
55
56
57
58
59
60
61
62
63
64
65
66 public AbstractPerceptionIterable(final LaneBasedGtu perceivingGtu, final LaneRecordInterface<?> root,
67 final Length initialPosition, final boolean downstream, final Length maxDistance,
68 final RelativePosition relativePosition, final Route route)
69 {
70 super(perceivingGtu);
71 this.root = root;
72 this.initialPosition = initialPosition;
73 this.downstream = downstream;
74 this.maxDistance = maxDistance.si;
75 this.relativePosition = relativePosition;
76 this.route = route;
77 }
78
79
80
81
82
83 public boolean isDownstream()
84 {
85 return this.downstream;
86 }
87
88
89 @Override
90 public Iterator<PrimaryIteratorEntry> primaryIterator()
91 {
92 return new PrimaryIterator();
93 }
94
95
96
97
98
99
100
101
102
103
104
105
106 protected abstract Entry getNext(LaneRecordInterface<?> record, Length position, C counter) throws GtuException;
107
108
109
110
111
112
113
114
115
116 protected abstract Length getDistance(U object, LaneRecordInterface<?> record, Length position);
117
118
119
120
121
122
123 protected Length getDx()
124 {
125 return this.relativePosition.dx();
126 }
127
128
129
130
131
132
133
134
135
136
137
138
139
140 private class PrimaryIterator implements Iterator<PrimaryIteratorEntry>
141 {
142
143
144 private SortedMap<PrimaryIteratorEntry, LaneRecordInterface<?>> map;
145
146
147 private Map<LaneRecordInterface<?>, Length> positions = new LinkedHashMap<>();
148
149
150 private Set<U> returnedItems = new LinkedHashSet<>();
151
152
153 private Map<LaneRecordInterface<?>, Queue<PrimaryIteratorEntry>> queues = new LinkedHashMap<>();
154
155
156 private Map<LaneRecordInterface<?>, C> counters = new LinkedHashMap<>();
157
158
159 private LaneRecordInterface<?> postponedRecord = null;
160
161
162 private Length postponedPosition = null;
163
164
165 PrimaryIterator()
166 {
167
168 }
169
170
171 @Override
172 public boolean hasNext()
173 {
174
175 startProcess();
176
177
178 return !this.map.isEmpty();
179 }
180
181
182 @Override
183 public PrimaryIteratorEntry next()
184 {
185
186 startProcess();
187
188
189 PrimaryIteratorEntry nextEntry = this.map.firstKey();
190 U next = nextEntry.getObject();
191 LaneRecordInterface<?> record = this.map.get(nextEntry);
192 this.map.remove(nextEntry);
193
194
195 Queue<PrimaryIteratorEntry> queue = this.queues.get(next);
196 if (queue != null)
197 {
198 PrimaryIteratorEntry nextNext = queue.poll();
199 this.map.put(nextNext, record);
200 if (queue.isEmpty())
201 {
202 this.queues.remove(record);
203 }
204 preventDuplicateEntries(nextEntry.getObject());
205 return nextNext;
206 }
207
208
209 this.postponedRecord = record;
210 this.postponedPosition = this.positions.get(record);
211 preventDuplicateEntries(nextEntry.getObject());
212 return nextEntry;
213 }
214
215
216
217
218
219 private void preventDuplicateEntries(final U object)
220 {
221 this.returnedItems.add(object);
222 Iterator<PrimaryIteratorEntry> it = this.map.keySet().iterator();
223 while (it.hasNext())
224 {
225 PrimaryIteratorEntry entry = it.next();
226 if (entry.getObject().equals(object))
227 {
228 it.remove();
229 }
230 }
231 }
232
233
234
235
236 @SuppressWarnings("synthetic-access")
237 private void startProcess()
238 {
239 if (this.postponedRecord != null)
240 {
241
242 prepareNext(this.postponedRecord, this.postponedPosition);
243 this.postponedRecord = null;
244 this.postponedPosition = null;
245 }
246 else if (this.map == null)
247 {
248
249 this.map = new TreeMap<>();
250 prepareNext(AbstractPerceptionIterable.this.root, AbstractPerceptionIterable.this.initialPosition);
251 }
252 }
253
254
255
256
257
258
259 @SuppressWarnings("synthetic-access")
260 private void prepareNext(final LaneRecordInterface<?> record, final Length position)
261 {
262 Entry next = Try.assign(() -> AbstractPerceptionIterable.this.getNext(record, position, this.counters.get(record)),
263 "Exception while deriving next object.");
264 if (next == null)
265 {
266 this.counters.remove(record);
267 double distance = AbstractPerceptionIterable.this.downstream
268 ? record.getStartDistance().si + record.getLength().si : -record.getStartDistance().si;
269
270 if (distance < AbstractPerceptionIterable.this.maxDistance)
271 {
272 if (AbstractPerceptionIterable.this.downstream)
273 {
274 for (LaneRecordInterface<?> nextRecord : record.getNext())
275 {
276 if (isOnRoute(nextRecord))
277 {
278 prepareNext(nextRecord, Length.instantiateSI(-1e-9));
279 }
280 }
281 }
282 else
283 {
284 for (LaneRecordInterface<?> nextRecord : record.getPrev())
285 {
286 if (isOnRoute(nextRecord))
287 {
288 prepareNext(nextRecord, nextRecord.getLength());
289 }
290 }
291 }
292 }
293 }
294 else
295 {
296 this.counters.put(record, next.counter);
297 if (next.isSet())
298 {
299 Iterator<U> it = next.set.iterator();
300 U nextNext = it.next();
301 if (!this.returnedItems.contains(nextNext))
302 {
303 Length distance = getDistance(nextNext, record, next.position);
304 if (distance == null
305 || distance.si <= AbstractPerceptionIterable.this.maxDistance)
306 {
307
308 this.map.put(new PrimaryIteratorEntry(nextNext, distance), record);
309 this.positions.put(record, next.position);
310 if (next.set.size() > 1)
311 {
312
313 Queue<PrimaryIteratorEntry> queue = new LinkedList<>();
314 while (it.hasNext())
315 {
316 nextNext = it.next();
317 queue.add(new PrimaryIteratorEntry(nextNext, getDistance(nextNext, record, next.position)));
318 }
319 this.queues.put(record, queue);
320 }
321 }
322 }
323 }
324 else if (!this.returnedItems.contains(next.object))
325 {
326 Length distance = getDistance(next.object, record, next.position);
327 if (distance == null
328 || distance.si <= AbstractPerceptionIterable.this.maxDistance)
329 {
330
331 this.map.put(new PrimaryIteratorEntry(next.object, distance), record);
332 this.positions.put(record, next.position);
333 }
334 }
335 }
336 }
337
338 }
339
340
341
342
343
344
345 final boolean isOnRoute(final LaneRecordInterface<?> record)
346 {
347 if (this.route == null)
348 {
349 return true;
350 }
351 Link link = record.getLane().getLink();
352 int from;
353 int to;
354 from = this.route.indexOf(link.getStartNode());
355 to = this.route.indexOf(link.getEndNode());
356 return from != -1 && to != -1 && to - from == 1;
357 }
358
359
360
361
362
363
364
365
366
367
368
369
370
371 protected class Entry
372 {
373
374
375 private final Set<U> set;
376
377
378 private final U object;
379
380
381 private final C counter;
382
383
384 private final Length position;
385
386
387
388
389
390
391
392 public Entry(final U object, final C counter, final Length position)
393 {
394 this.set = null;
395 this.object = object;
396 this.counter = counter;
397 this.position = position;
398 }
399
400
401
402
403
404
405
406 public Entry(final Set<U> set, final C counter, final Length position)
407 {
408 this.set = set;
409 this.object = null;
410 this.counter = counter;
411 this.position = position;
412 }
413
414
415
416
417
418 final boolean isSet()
419 {
420 return this.set != null;
421 }
422
423
424
425
426
427 public U getObject()
428 {
429 return this.object;
430 }
431
432
433
434
435
436 public Set<U> getSet()
437 {
438 return this.set;
439 }
440
441 }
442
443 }