1 package org.opentrafficsim.road.gtu.perception;
2
3 import java.util.Iterator;
4 import java.util.NoSuchElementException;
5 import java.util.function.Function;
6 import java.util.function.Supplier;
7
8 import org.djunits.value.vdouble.scalar.Length;
9 import org.djutils.exceptions.Try;
10 import org.opentrafficsim.base.DistancedObject;
11 import org.opentrafficsim.base.parameters.ParameterException;
12 import org.opentrafficsim.road.gtu.perception.object.PerceivedObject;
13 import org.opentrafficsim.road.network.object.LaneBasedObject;
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31 public abstract class AbstractPerceptionReiterable<O extends LaneBasedObject, P extends PerceivedObject, U>
32 implements PerceptionCollectable<P, U>
33 {
34
35
36 private SecondaryIteratorEntry first;
37
38
39 private SecondaryIteratorEntry last;
40
41
42 private Iterator<DistancedObject<U>> primaryIterator;
43
44
45 private final O perceivingObject;
46
47
48
49
50
51 protected AbstractPerceptionReiterable(final O perceivingObject)
52 {
53 this.perceivingObject = perceivingObject;
54 }
55
56
57
58
59
60 public O getObject()
61 {
62 return this.perceivingObject;
63 }
64
65
66
67
68
69 final Iterator<DistancedObject<U>> getPrimaryIterator()
70 {
71 if (this.primaryIterator == null)
72 {
73 this.primaryIterator = primaryIterator();
74 }
75 return this.primaryIterator;
76 }
77
78
79
80
81
82 protected abstract Iterator<DistancedObject<U>> primaryIterator();
83
84
85
86
87
88
89
90
91 protected abstract P perceive(U object, Length distance) throws ParameterException;
92
93 @Override
94 public final synchronized P first()
95 {
96 assureFirst();
97 if (this.first == null)
98 {
99 return null;
100 }
101 return this.first.getValue();
102 }
103
104
105
106
107 private synchronized void assureFirst()
108 {
109 if (this.first == null && getPrimaryIterator().hasNext())
110 {
111 addNext(getPrimaryIterator().next());
112 }
113 }
114
115
116
117
118
119 final void addNext(final DistancedObject<U> next)
120 {
121 SecondaryIteratorEntry entry = new SecondaryIteratorEntry(next);
122 if (AbstractPerceptionReiterable.this.last == null)
123 {
124 AbstractPerceptionReiterable.this.first = entry;
125 AbstractPerceptionReiterable.this.last = entry;
126 }
127 else
128 {
129 AbstractPerceptionReiterable.this.last.next = entry;
130 AbstractPerceptionReiterable.this.last = entry;
131 }
132 }
133
134 @Override
135 public final boolean isEmpty()
136 {
137 return first() == null;
138 }
139
140 @Override
141 public final Iterator<P> iterator()
142 {
143 return new PerceptionIterator();
144 }
145
146 @Override
147 public final <C, I> C collect(final Supplier<I> identity, final PerceptionAccumulator<? super U, I> accumulator,
148 final Function<I, C> finalizer)
149 {
150 Intermediate<I> intermediate = new Intermediate<>(identity.get());
151 assureFirst();
152 if (this.first != null)
153 {
154 SecondaryIteratorEntry lastReturned = null;
155 SecondaryIteratorEntry next = this.first;
156 next = assureNext(next, lastReturned);
157 while (next != null && !intermediate.isStop())
158 {
159 intermediate = accumulator.accumulate(intermediate, next.underlyingDistance.object(),
160 next.underlyingDistance.distance());
161 intermediate.step();
162 lastReturned = next;
163 next = lastReturned.next;
164 next = assureNext(next, lastReturned);
165 }
166 }
167 return finalizer.apply(intermediate.getObject());
168 }
169
170 @Override
171 public Iterator<U> underlying()
172 {
173 assureFirst();
174 SecondaryIteratorEntry firstInContext = this.first;
175 return new Iterator<U>()
176 {
177
178 private SecondaryIteratorEntry lastReturned = null;
179
180
181 private SecondaryIteratorEntry next = firstInContext;
182
183 @Override
184 public boolean hasNext()
185 {
186 this.next = assureNext(this.next, this.lastReturned);
187 return this.next != null;
188 }
189
190 @Override
191 public U next()
192 {
193
194
195
196
197
198
199
200
201
202 this.lastReturned = this.next;
203 this.next = this.lastReturned.next;
204 this.next = assureNext(this.next, this.lastReturned);
205 return this.lastReturned.underlyingDistance.object();
206 }
207 };
208 }
209
210 @Override
211 public Iterator<DistancedObject<U>> underlyingWithDistance()
212 {
213 assureFirst();
214 SecondaryIteratorEntry firstInContext = this.first;
215 return new Iterator<DistancedObject<U>>()
216 {
217
218 private SecondaryIteratorEntry lastReturned = null;
219
220
221 private SecondaryIteratorEntry next = firstInContext;
222
223 @Override
224 public boolean hasNext()
225 {
226 this.next = assureNext(this.next, this.lastReturned);
227 return this.next != null;
228 }
229
230 @Override
231 public DistancedObject<U> next()
232 {
233 this.lastReturned = this.next;
234 this.next = this.lastReturned.next;
235 this.next = assureNext(this.next, this.lastReturned);
236 return new DistancedObject<>(this.lastReturned.underlyingDistance.object(),
237 this.lastReturned.underlyingDistance.distance());
238 }
239 };
240 }
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255 public class PerceptionIterator implements Iterator<P>
256 {
257
258
259 private SecondaryIteratorEntry lastReturned;
260
261
262 private SecondaryIteratorEntry next;
263
264
265 PerceptionIterator()
266 {
267 this.next = AbstractPerceptionReiterable.this.first;
268 }
269
270 @Override
271 public boolean hasNext()
272 {
273 this.next = assureNext(this.next, this.lastReturned);
274 return this.next != null;
275 }
276
277 @Override
278 public P next()
279 {
280 this.next = assureNext(this.next, this.lastReturned);
281 if (this.next == null)
282 {
283 throw new NoSuchElementException();
284 }
285 this.lastReturned = this.next;
286 this.next = this.lastReturned.next;
287 return this.lastReturned.getValue();
288 }
289
290 }
291
292
293
294
295
296
297
298
299 synchronized SecondaryIteratorEntry assureNext(final SecondaryIteratorEntry next, final SecondaryIteratorEntry lastReturned)
300 {
301 if (next != null)
302 {
303 return next;
304 }
305 if (lastReturned != null)
306 {
307 if (lastReturned.next == null)
308 {
309 if (getPrimaryIterator().hasNext())
310 {
311 addNext(getPrimaryIterator().next());
312 }
313 }
314 return lastReturned.next;
315 }
316 if (getPrimaryIterator().hasNext())
317 {
318 addNext(getPrimaryIterator().next());
319 }
320 return AbstractPerceptionReiterable.this.first;
321 }
322
323
324
325
326
327
328
329
330
331
332
333
334 private class SecondaryIteratorEntry
335 {
336
337 private final DistancedObject<U> underlyingDistance;
338
339
340 private P value;
341
342
343 private SecondaryIteratorEntry next;
344
345
346
347
348
349 SecondaryIteratorEntry(final DistancedObject<U> underlyingDistance)
350 {
351 this.underlyingDistance = underlyingDistance;
352 }
353
354
355
356
357
358 P getValue()
359 {
360 if (this.value == null)
361 {
362 this.value = Try.assign(() -> perceive(this.underlyingDistance.object(), this.underlyingDistance.distance()),
363 "Parameter exception during perception of object.");
364 }
365 return this.value;
366 }
367 }
368
369 }