Line data    Source code

       1             : #include "Dynamic/Env/TrafficLight.hpp"
       2             : 
       3             : #include <cmath>
       4             : #include <iostream>
       5             : 
       6             : using namespace std;
       7             : using namespace Dynamic;
       8             : using namespace Dynamic::Env;
       9             : 
      10           0 : TrafficLight::Phase::Phase(TrafficLight &tl_, Time t_, Time duration_, vector<State> state_):
      11           0 :     tl(tl_), t(t_), duration(duration_), state(state_) {}
      12             : 
      13           0 : TrafficLight::Phase &TrafficLight::Phase::next() {
      14           0 :     auto it = tl.phases.upper_bound(t);
      15           0 :     return (it == tl.phases.end() ? tl.phases.begin() : it)->second;
      16             : }
      17             : 
      18           0 : const TrafficLight::Phase &TrafficLight::Phase::next() const {
      19           0 :     auto it = tl.phases.upper_bound(t);
      20           0 :     return (it == tl.phases.end() ? tl.phases.begin() : it)->second;
      21             : }
      22             : 
      23           0 : TrafficLight::Phase &TrafficLight::addPhase(Time time, Time duration, vector<Phase::State> state) {
      24           0 :     return phases.emplace(time, Phase{*this, time, duration, state}).first->second;
      25             : }
      26             : 
      27           0 : TrafficLight::TrafficLight(ID id_, Time offset_):
      28           0 :     id(id_), offset(offset_) {}
      29             : 
      30           0 : pair<const TrafficLight::Phase &, Time> TrafficLight::getPhase(Time t_) const {
      31           0 :     Time t = t_;
      32             : 
      33           0 :     t -= offset;
      34             : 
      35           0 :     Time d = duration();
      36             : 
      37           0 :     t = fmod(t, d);
      38           0 :     t = (t < 0 ? t + d : t);
      39             : 
      40           0 :     auto it = phases.upper_bound(t);
      41           0 :     --it;
      42             : 
      43           0 :     const Phase &phase = it->second;
      44             : 
      45           0 :     Time dt = t - phase.t;
      46             : 
      47           0 :     Time tStart_ = t_ - dt;
      48             : 
      49           0 :     return {phase, tStart_};
      50             : }
      51             : 
      52           0 : Time TrafficLight::duration() const {
      53           0 :     Time ret = 0.0;
      54           0 :     for(const auto &[_, phase]: phases)
      55           0 :         ret += phase.duration;
      56           0 :     return ret;
      57             : }