1   package org.opentrafficsim.core.gtu;
2   
3   import java.util.LinkedHashSet;
4   import java.util.Set;
5   
6   import org.djunits.unit.DurationUnit;
7   import org.djunits.unit.TimeUnit;
8   import org.djunits.value.vdouble.scalar.Acceleration;
9   import org.djunits.value.vdouble.scalar.Duration;
10  import org.djunits.value.vdouble.scalar.Length;
11  import org.djunits.value.vdouble.scalar.Speed;
12  import org.djunits.value.vdouble.scalar.Time;
13  import org.djutils.exceptions.Throw;
14  import org.djutils.exceptions.Try;
15  import org.opentrafficsim.base.parameters.ParameterException;
16  import org.opentrafficsim.base.parameters.Parameters;
17  import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
18  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
19  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
20  import org.opentrafficsim.core.gtu.plan.strategical.StrategicalPlanner;
21  import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
22  import org.opentrafficsim.core.idgenerator.IdGenerator;
23  import org.opentrafficsim.core.network.NetworkException;
24  import org.opentrafficsim.core.perception.Historical;
25  import org.opentrafficsim.core.perception.HistoricalValue;
26  import org.opentrafficsim.core.perception.HistoryManager;
27  import org.opentrafficsim.core.perception.PerceivableContext;
28  
29  import nl.tudelft.simulation.dsol.SimRuntimeException;
30  import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEvent;
31  import nl.tudelft.simulation.dsol.logger.SimLogger;
32  import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
33  import nl.tudelft.simulation.event.EventProducer;
34  import nl.tudelft.simulation.language.d3.DirectedPoint;
35  
36  
37  
38  
39  
40  
41  
42  
43  
44  
45  
46  
47  public abstract class AbstractGTU extends EventProducer implements GTU
48  {
49      
50      private static final long serialVersionUID = 20140822L;
51  
52      
53      private final String id;
54  
55      
56      private final int uniqueNumber;
57  
58      
59      private static int staticUNIQUENUMBER = 0;
60  
61      
62      private final GTUType gtuType;
63  
64      
65      private final OTSSimulatorInterface simulator;
66  
67      
68      private Parameters parameters;
69  
70      
71      private Acceleration maximumAcceleration;
72  
73      
74      private Acceleration maximumDeceleration;
75  
76      
77  
78  
79  
80  
81      private Historical<Length> odometer;
82  
83      
84      private final Historical<StrategicalPlanner> strategicalPlanner;
85  
86      
87      private final Historical<TacticalPlanner<?, ?>> tacticalPlanner;
88  
89      
90      protected final Historical<OperationalPlan> operationalPlan;
91  
92      
93      private SimEvent<SimTimeDoubleUnit> nextMoveEvent;
94  
95      
96      private PerceivableContext perceivableContext;
97  
98      
99      private boolean destroyed = false;
100 
101     
102     
103     public static boolean ALIGNED = true;
104 
105     
106     
107     public static int ALIGN_COUNT = 0;
108 
109     
110     private double cachedSpeedTime = Double.NaN;
111 
112     
113     private Speed cachedSpeed = null;
114 
115     
116     private double cachedAccelerationTime = Double.NaN;
117 
118     
119     private Acceleration cachedAcceleration = null;
120 
121     
122     private GTU parent = null;
123 
124     
125     private Set<GTU> children = new LinkedHashSet<>();
126 
127     
128     private GTUErrorHandler errorHandler = GTUErrorHandler.THROW;
129 
130     
131 
132 
133 
134 
135 
136 
137     @SuppressWarnings("checkstyle:parameternumber")
138     public AbstractGTU(final String id, final GTUType gtuType, final OTSSimulatorInterface simulator,
139             final PerceivableContext perceivableContext) throws GTUException
140     {
141         Throw.when(id == null, GTUException.class, "id is null");
142         Throw.when(gtuType == null, GTUException.class, "gtuType is null");
143         Throw.when(perceivableContext == null, GTUException.class, "perceivableContext is null for GTU with id %s", id);
144         Throw.when(perceivableContext.containsGtuId(id), GTUException.class,
145                 "GTU with id %s already registered in perceivableContext %s", id, perceivableContext.getId());
146         Throw.when(simulator == null, GTUException.class, "simulator is null for GTU with id %s", id);
147 
148         HistoryManager historyManager = simulator.getReplication().getHistoryManager(simulator);
149         this.id = id;
150         this.uniqueNumber = ++staticUNIQUENUMBER;
151         this.gtuType = gtuType;
152         this.simulator = simulator;
153         this.odometer = new HistoricalValue<>(historyManager, Length.ZERO);
154         this.perceivableContext = perceivableContext;
155         this.perceivableContext.addGTU(this);
156         this.strategicalPlanner = new HistoricalValue<>(historyManager);
157         this.tacticalPlanner = new HistoricalValue<>(historyManager, null);
158         this.operationalPlan = new HistoricalValue<>(historyManager, null);
159     }
160 
161     
162 
163 
164 
165 
166 
167 
168     @SuppressWarnings("checkstyle:parameternumber")
169     public AbstractGTU(final IdGenerator idGenerator, final GTUType gtuType, final OTSSimulatorInterface simulator,
170             final PerceivableContext perceivableContext) throws GTUException
171     {
172         this(generateId(idGenerator), gtuType, simulator, perceivableContext);
173     }
174 
175     
176 
177 
178 
179 
180 
181 
182 
183 
184 
185     @SuppressWarnings({ "checkstyle:hiddenfield", "hiding", "checkstyle:designforextension" })
186     public void init(final StrategicalPlanner strategicalPlanner, final DirectedPoint initialLocation, final Speed initialSpeed)
187             throws SimRuntimeException, GTUException
188     {
189         Throw.when(strategicalPlanner == null, GTUException.class, "strategicalPlanner is null for GTU with id %s", this.id);
190         Throw.whenNull(initialLocation, "Initial location of GTU cannot be null");
191         Throw.when(Double.isNaN(initialLocation.x) || Double.isNaN(initialLocation.y) || Double.isNaN(initialLocation.z),
192                 GTUException.class, "initialLocation %s invalid for GTU with id %s", initialLocation, this.id);
193         Throw.when(initialSpeed == null, GTUException.class, "initialSpeed is null for GTU with id %s", this.id);
194         Throw.when(!getId().equals(strategicalPlanner.getGtu().getId()), GTUException.class,
195                 "GTU %s is initialized with a strategical planner for GTU %s", getId(), strategicalPlanner.getGtu().getId());
196 
197         this.strategicalPlanner.set(strategicalPlanner);
198         this.tacticalPlanner.set(strategicalPlanner.getTacticalPlanner());
199         Time now = this.simulator.getSimulatorTime();
200 
201         fireTimedEvent(GTU.INIT_EVENT, new Object[] { getId(), initialLocation, getLength(), getWidth() }, now);
202 
203         try
204         {
205             move(initialLocation);
206         }
207         catch (OperationalPlanException | NetworkException | ParameterException exception)
208         {
209             throw new GTUException("Failed to create OperationalPlan for GTU " + this.id, exception);
210         }
211     }
212 
213     
214 
215 
216 
217 
218 
219     private static String generateId(final IdGenerator idGenerator) throws GTUException
220     {
221         Throw.when(idGenerator == null, GTUException.class, "AbstractGTU.<init>: idGenerator is null");
222         return idGenerator.nextId();
223     }
224 
225     
226 
227 
228     @Override
229     @SuppressWarnings("checkstyle:designforextension")
230     public void destroy()
231     {
232         fireTimedEvent(GTU.DESTROY_EVENT, new Object[] { getId(), getLocation(), getOdometer() },
233                 this.simulator.getSimulatorTime());
234 
235         
236         if (this.nextMoveEvent != null)
237         {
238             this.simulator.cancelEvent(this.nextMoveEvent);
239             this.nextMoveEvent = null;
240         }
241 
242         this.perceivableContext.removeGTU(this);
243         this.destroyed = true;
244     }
245 
246     
247 
248 
249 
250 
251 
252 
253 
254 
255 
256 
257 
258 
259 
260 
261     @SuppressWarnings("checkstyle:designforextension")
262     protected boolean move(final DirectedPoint fromLocation)
263             throws SimRuntimeException, OperationalPlanException, GTUException, NetworkException, ParameterException
264     {
265         try
266         {
267             Time now = this.simulator.getSimulatorTime();
268 
269             
270             
271             Length currentOdometer;
272             if (this.operationalPlan.get() != null)
273             {
274                 currentOdometer = this.odometer.get().plus(this.operationalPlan.get().getTraveledDistance(now));
275             }
276             else
277             {
278                 currentOdometer = this.odometer.get();
279             }
280 
281             
282             
283             TacticalPlanner<?, ?> tactPlanner = this.tacticalPlanner.get();
284             if (tactPlanner == null)
285             {
286                 
287                 tactPlanner = this.strategicalPlanner.get().getTacticalPlanner();
288                 this.tacticalPlanner.set(tactPlanner);
289             }
290             tactPlanner.getPerception().perceive();
291             OperationalPlan newOperationalPlan = tactPlanner.generateOperationalPlan(now, fromLocation);
292             this.operationalPlan.set(newOperationalPlan);
293             this.cachedSpeedTime = Double.NaN;
294             this.cachedAccelerationTime = Double.NaN;
295             this.odometer.set(currentOdometer);
296 
297             
298             if (ALIGNED && newOperationalPlan.getTotalDuration().si == 0.5)
299             {
300                 
301                 
302                 double tNext = Math.floor(2.0 * now.si + 1.0) / 2.0;
303                 DirectedPoint p = (tNext - now.si < 0.5) ? newOperationalPlan.getEndLocation()
304                         : newOperationalPlan.getLocation(new Duration(tNext - now.si, DurationUnit.SI));
305                 this.nextMoveEvent = new SimEvent<>(new SimTimeDoubleUnit(new Time(tNext, TimeUnit.DEFAULT)), this, this, "move",
306                         new Object[] { p });
307                 ALIGN_COUNT++;
308             }
309             else
310             {
311                 
312                 
313                 this.nextMoveEvent = new SimEvent<>(new SimTimeDoubleUnit(now.plus(newOperationalPlan.getTotalDuration())),
314                         this, this, "move", new Object[] { newOperationalPlan.getEndLocation() });
315             }
316             this.simulator.scheduleEvent(this.nextMoveEvent);
317 
318             fireTimedEvent(GTU.MOVE_EVENT, new Object[] { getId(), fromLocation, getSpeed(), getAcceleration(), getOdometer() },
319                     this.simulator.getSimulatorTime());
320 
321             return false;
322         }
323         catch (Exception ex)
324         {
325             try
326             {
327                 this.errorHandler.handle(this, ex);
328             }
329             catch (Exception exception)
330             {
331                 throw new GTUException(exception);
332             }
333             return true;
334         }
335     }
336 
337     
338 
339 
340 
341 
342 
343 
344 
345 
346 
347     @SuppressWarnings("checkstyle:designforextension")
348     protected void interruptMove()
349             throws SimRuntimeException, OperationalPlanException, GTUException, NetworkException, ParameterException
350     {
351         this.simulator.cancelEvent(this.nextMoveEvent);
352         move(this.operationalPlan.get().getLocation(this.simulator.getSimulatorTime()));
353     }
354 
355     
356     @Override
357     public final String getId()
358     {
359         return this.id;
360     }
361 
362     
363     @SuppressWarnings("checkstyle:designforextension")
364     @Override
365     public GTUType getGTUType()
366     {
367         return this.gtuType;
368     }
369 
370     
371     @Override
372     public final RelativePosition getReference()
373     {
374         return RelativePosition.REFERENCE_POSITION;
375     }
376 
377     
378     @Override
379     public final OTSSimulatorInterface getSimulator()
380     {
381         return this.simulator;
382     }
383 
384     
385     @Override
386     public final Parameters getParameters()
387     {
388         return this.parameters;
389     }
390 
391     
392     @Override
393     public final void setParameters(final Parameters parameters)
394     {
395         this.parameters = parameters;
396     }
397 
398     
399     @Override
400     public StrategicalPlanner getStrategicalPlanner()
401     {
402         return this.strategicalPlanner.get();
403     }
404 
405     
406     @Override
407     public StrategicalPlanner getStrategicalPlanner(final Time time)
408     {
409         return this.strategicalPlanner.get(time);
410     }
411 
412     
413     @Override
414     public final OperationalPlan getOperationalPlan()
415     {
416         return this.operationalPlan.get();
417     }
418 
419     
420     @Override
421     public final OperationalPlan getOperationalPlan(final Time time)
422     {
423         return this.operationalPlan.get(time);
424     }
425 
426     
427     @Override
428     public final Length getOdometer()
429     {
430         return getOdometer(this.simulator.getSimulatorTime());
431     }
432 
433     
434     @Override
435     public final Length getOdometer(final Time time)
436     {
437         if (getOperationalPlan(time) == null)
438         {
439             return this.odometer.get(time);
440         }
441         try
442         {
443             return this.odometer.get(time).plus(getOperationalPlan(time).getTraveledDistance(time));
444         }
445         catch (OperationalPlanException ope)
446         {
447             return this.odometer.get(time);
448         }
449     }
450 
451     
452     @Override
453     public final Speed getSpeed()
454     {
455         return getSpeed(this.simulator.getSimulatorTime());
456     }
457 
458     
459     @Override
460     public final Speed getSpeed(final Time time)
461     {
462         if (this.cachedSpeedTime != time.si)
463         {
464             this.cachedSpeedTime = time.si;
465             OperationalPlan plan = getOperationalPlan(time);
466             if (plan == null)
467             {
468                 this.cachedSpeed = Speed.ZERO;
469             }
470             else if (time.si < plan.getStartTime().si)
471             {
472                 this.cachedSpeed = plan.getStartSpeed();
473             }
474             else if (time.si > plan.getEndTime().si)
475             {
476                 if (time.si - plan.getEndTime().si < 1e-6)
477                 {
478                     this.cachedSpeed = Try.assign(() -> plan.getSpeed(plan.getEndTime()),
479                             "getSpeed() could not derive a valid speed for the current operationalPlan");
480                 }
481                 else
482                 {
483                     throw new IllegalStateException("Requesting speed value beyond plan.");
484                 }
485             }
486             else
487             {
488                 this.cachedSpeed = Try.assign(() -> plan.getSpeed(time),
489                         "getSpeed() could not derive a valid speed for the current operationalPlan");
490             }
491         }
492         return this.cachedSpeed;
493     }
494 
495     
496     @Override
497     public final Acceleration getAcceleration()
498     {
499         return getAcceleration(this.simulator.getSimulatorTime());
500     }
501 
502     
503     @Override
504     public final Acceleration getAcceleration(final Time time)
505     {
506         if (this.cachedAccelerationTime != time.si)
507         {
508             this.cachedAccelerationTime = time.si;
509             OperationalPlan plan = getOperationalPlan(time);
510             if (plan == null)
511             {
512                 this.cachedAcceleration = Acceleration.ZERO;
513             }
514             else if (time.si < plan.getStartTime().si)
515             {
516                 this.cachedAcceleration =
517                         Try.assign(() -> plan.getAcceleration(plan.getStartTime()), "Exception obtaining acceleration.");
518             }
519             else if (time.si > plan.getEndTime().si)
520             {
521                 if (time.si - plan.getEndTime().si < 1e-6)
522                 {
523                     this.cachedAcceleration = Try.assign(() -> plan.getAcceleration(plan.getEndTime()),
524                             "getAcceleration() could not derive a valid acceleration for the current operationalPlan");
525                 }
526                 else
527                 {
528                     throw new IllegalStateException("Requesting acceleration value beyond plan.");
529                 }
530             }
531             else
532             {
533                 this.cachedAcceleration = Try.assign(() -> plan.getAcceleration(time),
534                         "getAcceleration() could not derive a valid acceleration for the current operationalPlan");
535             }
536         }
537         return this.cachedAcceleration;
538     }
539 
540     
541 
542 
543     @Override
544     public final Acceleration getMaximumAcceleration()
545     {
546         return this.maximumAcceleration;
547     }
548 
549     
550 
551 
552     public final void setMaximumAcceleration(final Acceleration maximumAcceleration)
553     {
554         if (maximumAcceleration.le(Acceleration.ZERO))
555         {
556             throw new RuntimeException("Maximum acceleration of GTU " + this.id + " set to value <= 0");
557         }
558         this.maximumAcceleration = maximumAcceleration;
559     }
560 
561     
562 
563 
564     @Override
565     public final Acceleration getMaximumDeceleration()
566     {
567         return this.maximumDeceleration;
568     }
569 
570     
571 
572 
573     public final void setMaximumDeceleration(final Acceleration maximumDeceleration)
574     {
575         if (maximumDeceleration.ge(Acceleration.ZERO))
576         {
577             throw new RuntimeException("Maximum deceleration of GTU " + this.id + " set to value >= 0");
578         }
579         this.maximumDeceleration = maximumDeceleration;
580     }
581 
582     
583     private Time cacheLocationTime = new Time(Double.NaN, TimeUnit.DEFAULT);
584 
585     
586     private DirectedPoint cacheLocation = null;
587 
588     
589     @Override
590     @SuppressWarnings("checkstyle:designforextension")
591     public DirectedPoint getLocation()
592     {
593         if (this.operationalPlan.get() == null)
594         {
595             SimLogger.always()
596                     .error("No operational plan for GTU " + this.id + " at t=" + this.getSimulator().getSimulatorTime());
597             return new DirectedPoint(0, 0, 0);
598         }
599         try
600         {
601             
602             if (this.cacheLocationTime.si != this.simulator.getSimulatorTime().si)
603             {
604                 this.cacheLocationTime = this.simulator.getSimulatorTime();
605                 this.cacheLocation = this.operationalPlan.get().getLocation(this.cacheLocationTime);
606             }
607             return this.cacheLocation;
608         }
609         catch (OperationalPlanException exception)
610         {
611             return new DirectedPoint(0, 0, 0);
612         }
613     }
614 
615     
616     @Override
617     public final boolean isDestroyed()
618     {
619         return this.destroyed;
620     }
621 
622     
623     @Override
624     public PerceivableContext getPerceivableContext()
625     {
626         return this.perceivableContext;
627     }
628 
629     
630     @Override
631     public void addGtu(final GTU gtu) throws GTUException
632     {
633         this.children.add(gtu);
634         gtu.setParent(this);
635     }
636 
637     
638     @Override
639     public void removeGtu(final GTU gtu)
640     {
641         this.children.remove(gtu);
642         try
643         {
644             gtu.setParent(null);
645         }
646         catch (GTUException exception)
647         {
648             
649         }
650     }
651 
652     
653     @Override
654     public void setParent(final GTU gtu) throws GTUException
655     {
656         Throw.when(gtu != null && this.parent != null, GTUException.class, "GTU %s already has a parent.", this);
657         this.parent = gtu;
658     }
659 
660     
661     @Override
662     public GTU getParent()
663     {
664         return this.parent;
665     }
666 
667     
668     @Override
669     public Set<GTU> getChildren()
670     {
671         return new LinkedHashSet<>(this.children); 
672     }
673 
674     
675 
676 
677     protected GTUErrorHandler getErrorHandler()
678     {
679         return this.errorHandler;
680     }
681 
682     
683     @Override
684     public void setErrorHandler(final GTUErrorHandler errorHandler)
685     {
686         this.errorHandler = errorHandler;
687     }
688 
689     
690 
691 
692 
693     public final SimEvent<SimTimeDoubleUnit> getNextMoveEvent()
694     {
695         return this.nextMoveEvent;
696     }
697 
698     
699     @Override
700     @SuppressWarnings("designforextension")
701     public int hashCode()
702     {
703         final int prime = 31;
704         int result = 1;
705         result = prime * result + ((this.id == null) ? 0 : this.id.hashCode());
706         result = prime * result + this.uniqueNumber;
707         return result;
708     }
709 
710     
711     @Override
712     @SuppressWarnings({ "designforextension", "needbraces" })
713     public boolean equals(final Object obj)
714     {
715         if (this == obj)
716             return true;
717         if (obj == null)
718             return false;
719         if (getClass() != obj.getClass())
720             return false;
721         AbstractGTU./../../org/opentrafficsim/core/gtu/AbstractGTU.html#AbstractGTU">AbstractGTU other = (AbstractGTU) obj;
722         if (this.id == null)
723         {
724             if (other.id != null)
725                 return false;
726         }
727         else if (!this.id.equals(other.id))
728             return false;
729         if (this.uniqueNumber != other.uniqueNumber)
730             return false;
731         return true;
732     }
733 
734 }