1 package org.opentrafficsim.road.network.speed;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.List;
6 import java.util.SortedSet;
7 import java.util.TreeSet;
8
9 import org.djunits.unit.LengthUnit;
10 import org.djunits.value.vdouble.scalar.Acceleration;
11 import org.djunits.value.vdouble.scalar.Duration;
12 import org.djunits.value.vdouble.scalar.Length;
13 import org.djunits.value.vdouble.scalar.Speed;
14
15 import nl.tudelft.simulation.language.Throw;
16
17
18
19
20
21
22
23
24
25
26
27
28 public class SpeedLimitProspect implements Serializable
29 {
30
31
32 private static final long serialVersionUID = 20160501L;
33
34
35 private final SortedSet<SpeedLimitEntry<?>> prospect = new TreeSet<>();
36
37
38
39
40
41
42
43
44
45
46
47 public final <T> void addSpeedInfo(final Length distance, final SpeedLimitType<T> speedLimitType, final T speedInfo)
48 {
49 Throw.whenNull(distance, "Distance may not be null.");
50 Throw.whenNull(speedLimitType, "Speed limit type may not be null.");
51 Throw.whenNull(speedInfo, "Speed info may not be null.");
52 checkAndAdd(new SpeedLimitEntry<>(distance, speedLimitType, speedInfo));
53 }
54
55
56
57
58
59
60
61
62
63
64 @SuppressWarnings({ "unchecked", "rawtypes" })
65 public final void removeSpeedInfo(final Length distance, final SpeedLimitType<?> speedLimitType)
66 {
67 Throw.whenNull(distance, "Distance may not be null.");
68 Throw.when(distance.si < 0, IllegalArgumentException.class,
69 "Removing speed info in the past is not allowed. " + "Only add still active speed info.");
70 Throw.whenNull(speedLimitType, "Speed limit type may not be null.");
71 Throw.when(speedLimitType.equals(SpeedLimitTypes.MAX_VEHICLE_SPEED), IllegalArgumentException.class,
72 "May not remove the maximum vehicle speed.");
73
74 checkAndAdd(new SpeedLimitEntry(distance, speedLimitType, null));
75 }
76
77
78
79
80
81
82 private void checkAndAdd(final SpeedLimitEntry<?> speedLimitEntry)
83 {
84 for (SpeedLimitEntry<?> s : this.prospect)
85 {
86 if (s.getSpeedLimitType().equals(speedLimitEntry.getSpeedLimitType()))
87 {
88
89
90
91
92 Throw.when(s.getDistance().equals(speedLimitEntry.getDistance()), IllegalStateException.class,
93 "Info " + "of speed limit type '%s' is set twice at the same location (%s). This is undefined. "
94 + "Either remove speed info, or overwrite with new speed info.",
95 s.getSpeedLimitType(), s.getDistance());
96 }
97 }
98 this.prospect.add(speedLimitEntry);
99 }
100
101
102
103
104
105
106 public final List<Length> getDistances()
107 {
108 List<Length> list = new ArrayList<>();
109 for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
110 {
111 list.add(speedLimitEntry.getDistance());
112 }
113 return list;
114 }
115
116
117
118
119
120
121
122 public final List<Length> getDistances(final SpeedLimitType<?> speedLimitType)
123 {
124 return getDistancesInRange(speedLimitType, null, null);
125 }
126
127
128
129
130
131
132
133 public final List<Length> getUpstreamDistances(final SpeedLimitType<?> speedLimitType)
134 {
135 return getDistancesInRange(speedLimitType, null, Length.ZERO);
136 }
137
138
139
140
141
142
143
144 public final List<Length> getDownstreamDistances(final SpeedLimitType<?> speedLimitType)
145 {
146 return getDistancesInRange(speedLimitType, Length.ZERO, null);
147 }
148
149
150
151
152
153
154
155
156
157 private List<Length> getDistancesInRange(final SpeedLimitType<?> speedLimitType, final Length min, final Length max)
158 {
159 List<Length> list = new ArrayList<>();
160 for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
161 {
162 if (speedLimitEntry.getSpeedLimitType().equals(speedLimitType)
163 && (min == null || speedLimitEntry.getDistance().gt(min))
164 && (max == null || speedLimitEntry.getDistance().le(max)))
165 {
166 list.add(speedLimitEntry.getDistance());
167 }
168 }
169 return list;
170 }
171
172
173
174
175
176
177
178
179 public final boolean speedInfoChanged(final Length distance, final SpeedLimitType<?> speedLimitType)
180 {
181 Throw.whenNull(distance, "Distance may not be null.");
182 for (SpeedLimitEntry<?> sle : this.prospect)
183 {
184 if (sle.getDistance().eq(distance) && sle.getSpeedLimitType().equals(speedLimitType))
185 {
186 return true;
187 }
188 }
189 return false;
190 }
191
192
193
194
195
196
197
198
199
200
201 public final <T> T getSpeedInfoChange(final Length distance, final SpeedLimitType<T> speedLimitType)
202 {
203 for (SpeedLimitEntry<?> sle : this.prospect)
204 {
205 if (sle.getDistance().eq(distance) && sle.getSpeedLimitType().equals(speedLimitType))
206 {
207 @SuppressWarnings("unchecked")
208 T info = (T) sle.getSpeedInfo();
209 return info;
210 }
211 }
212 throw new IllegalArgumentException("Speed info of speed limit type '" + speedLimitType.getId()
213 + "' is requested at a distance '" + distance + "' where the info is not changed.");
214 }
215
216
217
218
219
220
221
222 public final SpeedLimitInfo getSpeedLimitInfo(final Length distance)
223 {
224 Throw.whenNull(distance, "Distance may not be null.");
225 SpeedLimitInfo speedLimitInfo = new SpeedLimitInfo();
226 for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
227 {
228
229 if (speedLimitEntry.getDistance().compareTo(distance) > 0)
230 {
231
232 return speedLimitInfo;
233 }
234
235 if (speedLimitEntry.getSpeedInfo() == null)
236 {
237 speedLimitInfo.removeSpeedInfo(speedLimitEntry.getSpeedLimitType());
238 }
239 else
240 {
241
242
243 setAsType(speedLimitInfo, speedLimitEntry);
244 }
245 }
246 return speedLimitInfo;
247 }
248
249
250
251
252
253
254
255
256
257 public final SpeedLimitInfo getSpeedLimitInfo(final Speed speed, final Acceleration acceleration, final Duration time)
258 {
259 Throw.whenNull(speed, "Speed may not be null.");
260 Throw.whenNull(acceleration, "Acceleration may not be null.");
261 Throw.whenNull(time, "Time may not be null.");
262 return getSpeedLimitInfo(new Length(speed.si * time.si + .5 * acceleration.si * time.si * time.si, LengthUnit.SI));
263 }
264
265
266
267
268
269
270
271
272
273
274 @SuppressWarnings("unchecked")
275 private <T> void setAsType(final SpeedLimitInfo speedLimitInfo, final SpeedLimitEntry<?> speedLimitEntry)
276 {
277 SpeedLimitType<T> speedLimitType = (SpeedLimitType<T>) speedLimitEntry.getSpeedLimitType();
278 T speedInfoOfType = (T) speedLimitEntry.getSpeedInfo();
279 speedLimitInfo.addSpeedInfo(speedLimitType, speedInfoOfType);
280 }
281
282
283
284
285
286
287
288
289
290 public final <T> SpeedLimitInfo buildSpeedLimitInfo(final Length distance, final SpeedLimitType<T> speedLimitType)
291 {
292 SpeedLimitInfo out = new SpeedLimitInfo();
293 out.addSpeedInfo(speedLimitType, getSpeedInfoChange(distance, speedLimitType));
294 for (SpeedLimitEntry<?> speedLimitEntry : this.prospect)
295 {
296 if (speedLimitEntry.getDistance().gt(distance))
297 {
298 break;
299 }
300 if (speedLimitEntry.getSpeedLimitType().equals(SpeedLimitTypes.MAX_VEHICLE_SPEED))
301 {
302 out.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED,
303 SpeedLimitTypes.MAX_VEHICLE_SPEED.getInfoClass().cast(speedLimitEntry.getSpeedInfo()));
304 }
305 }
306 return out;
307 }
308
309
310 @Override
311 public final String toString()
312 {
313 StringBuilder stringBuilder = new StringBuilder("SpeedLimitProspect [");
314 String sep = "";
315 for (SpeedLimitEntry<?> sle : this.prospect)
316 {
317 stringBuilder.append(sep).append(sle.getDistance()).append(": ");
318 if (sle.getSpeedInfo() == null)
319 {
320 stringBuilder.append(sle.getSpeedLimitType().getId()).append("=END");
321 }
322 else
323 {
324 stringBuilder.append(sle.getSpeedLimitType().getId()).append("=");
325 stringBuilder.append(sle.getSpeedInfo());
326 }
327 sep = ", ";
328 }
329 stringBuilder.append("]");
330 return stringBuilder.toString();
331 }
332
333
334
335
336
337
338
339
340
341
342
343
344 private static class SpeedLimitEntry<T> implements Comparable<SpeedLimitEntry<?>>, Serializable
345 {
346
347
348 private static final long serialVersionUID = 20160501L;
349
350
351 private final Length distance;
352
353
354 private final SpeedLimitType<T> speedLimitType;
355
356
357 private final T speedInfo;
358
359
360
361
362
363
364
365 SpeedLimitEntry(final Length distance, final SpeedLimitType<T> speedLimitType, final T speedInfo)
366 {
367 this.distance = distance;
368 this.speedLimitType = speedLimitType;
369 this.speedInfo = speedInfo;
370 }
371
372
373
374
375
376 public final Length getDistance()
377 {
378 return this.distance;
379 }
380
381
382
383
384
385 public final SpeedLimitType<T> getSpeedLimitType()
386 {
387 return this.speedLimitType;
388 }
389
390
391
392
393
394 public final T getSpeedInfo()
395 {
396 return this.speedInfo;
397 }
398
399
400 @Override
401 public final int hashCode()
402 {
403 final int prime = 31;
404 int result = 1;
405 result = prime * result + this.distance.hashCode();
406 result = prime * result + this.speedInfo.hashCode();
407 result = prime * result + this.speedLimitType.hashCode();
408 return result;
409 }
410
411
412 @Override
413 public final boolean equals(final Object obj)
414 {
415 if (this == obj)
416 {
417 return true;
418 }
419 if (obj == null)
420 {
421 return false;
422 }
423 if (getClass() != obj.getClass())
424 {
425 return false;
426 }
427 SpeedLimitEntry<?> other = (SpeedLimitEntry<?>) obj;
428 if (!this.distance.equals(other.distance))
429 {
430 return false;
431 }
432 if (!this.speedLimitType.equals(other.speedLimitType))
433 {
434 return false;
435 }
436 if (this.speedInfo == null)
437 {
438 if (other.speedInfo != null)
439 {
440 return false;
441 }
442 }
443 else if (!this.speedInfo.equals(other.speedInfo))
444 {
445 return false;
446 }
447 return true;
448 }
449
450
451 @Override
452 public final int compareTo(final SpeedLimitEntry<?> speedLimitEntry)
453 {
454 if (this.equals(speedLimitEntry))
455 {
456 return 0;
457 }
458
459 int comp = this.distance.compareTo(speedLimitEntry.distance);
460 if (comp != 0)
461 {
462 return comp;
463 }
464
465 comp = this.speedLimitType.getId().compareTo(speedLimitEntry.speedLimitType.getId());
466 if (comp != 0)
467 {
468 return comp;
469 }
470
471
472 if (this.speedInfo == null)
473 {
474 if (speedLimitEntry.speedInfo == null)
475 {
476 return 0;
477 }
478 return -1;
479 }
480 else if (speedLimitEntry.speedInfo == null)
481 {
482 return 1;
483 }
484 return this.speedInfo.hashCode() < speedLimitEntry.speedInfo.hashCode() ? -1 : 1;
485 }
486
487
488 @Override
489 public final String toString()
490 {
491 return "SpeedLimitEntry [distance=" + this.distance + ", speedLimitType=" + this.speedLimitType + ", speedInfo="
492 + this.speedInfo + "]";
493 }
494
495 }
496
497 }