Line data    Source code

       1             : #include "Dynamic/Env/Event/EventUpdateVehicle.hpp"
       2             : 
       3             : #include <spdlog/spdlog.h>
       4             : 
       5             : #include "Dynamic/Env/Env.hpp"
       6             : #include "Dynamic/Env/Event/EventDespawnVehicle.hpp"
       7             : #include "Dynamic/Env/Event/EventMoveVehicle.hpp"
       8             : #include "Dynamic/Env/Event/EventPopQueue.hpp"
       9             : #include "Dynamic/Env/Lane.hpp"
      10             : 
      11             : using namespace std;
      12             : using namespace Dynamic;
      13             : using namespace Dynamic::Env;
      14             : 
      15             : const Length EPSILON = 1e-3;
      16             : 
      17           0 : EventUpdateVehicle::EventUpdateVehicle(Time t_, Vehicle &vehicle_):
      18           0 :     EventMoveVehicle(t_, vehicle_), vehicle(vehicle_) {}
      19             : 
      20           0 : void enqueue(Dynamic::Env::Env &env, Dynamic::Env::Vehicle &vehicle, shared_ptr<Dynamic::Env::Action> action) {
      21           0 :     vehicle.position.offset = vehicle.position.lane.queuePosition();
      22             : 
      23           0 :     vehicle.position.lane.stopped.emplace(vehicle, action);
      24             : 
      25           0 :     vehicle.lastUpdateTime = env.getTime();
      26           0 :     vehicle.lastMoveTime   = env.getTime();
      27             : 
      28           0 :     vehicle.speed = 0;
      29             : 
      30           0 :     vehicle.state = Dynamic::Env::Vehicle::State::STOPPED;
      31             : 
      32             :     // if(vehicle.position.lane.stopped.size() >= vehicle.position.lane.queueCapacity() + 10) {
      33             :     //     spdlog::warn(
      34             :     //         "[t={}] Queue size exceeded in lane {}, capacity is {}, size is {}",
      35             :     //         env.getTime(),
      36             :     //         vehicle.position.lane.idAsString(),
      37             :     //         vehicle.position.lane.queueCapacity(),
      38             :     //         vehicle.position.lane.stopped.size()
      39             :     //     );
      40             :     // }
      41             : 
      42           0 :     if(
      43           0 :         env.getDespawnTime() < numeric_limits<Time>::infinity()
      44           0 :         && vehicle.position.lane.stopped.size() == 1
      45             :     ) {
      46             :         // No need to update lastUpdateTime or lastMoveTime, as it is already done above
      47           0 :         env.pushEvent(make_shared<EventDespawnVehicle>(
      48           0 :             env.getTime() + env.getDespawnTime(),
      49           0 :             vehicle.id
      50             :         ));
      51             :     }
      52           0 : }
      53             : 
      54           0 : void EventUpdateVehicle::process(Env &env) {
      55           0 :     EventMoveVehicle::process(env);
      56             : 
      57             :     // assert(vehicle.position.offset > vehicle.position.lane.queuePosition() - EPSILON);
      58             : 
      59             :     // Not at end of edge
      60           0 :     if(vehicle.position.offset < vehicle.position.lane.queuePosition() - EPSILON) {
      61           0 :         Time Dt      = (vehicle.position.lane.queuePosition() - vehicle.position.offset) / vehicle.speed;
      62           0 :         Dt           = max(Dt, 0.0);
      63           0 :         Time tFuture = env.getTime() + Dt;
      64           0 :         env.pushEvent(make_shared<EventUpdateVehicle>(tFuture, vehicle));
      65           0 :         return;
      66             :     }
      67             : 
      68             :     // Is at end of edge; enqueue or move to next edge
      69             : 
      70           0 :     auto action = vehicle.pickConnection(env);
      71             : 
      72           0 :     if(action->connection == Connection::LEAVE) {
      73           0 :         if(vehicle.toTAZ.sinks.count(vehicle.position.lane.edge) <= 0) {  // Leaving network at wrong place
      74           0 :             spdlog::warn(
      75             :                 "Vehicle {} is leaving network at wrong place",
      76           0 :                 env.getTime(),
      77           0 :                 vehicle.id
      78             :             );
      79           0 :             action->reward(-numeric_limits<Action::Reward>::infinity());
      80             :         }
      81             : 
      82           0 :         vehicle.state = Vehicle::State::LEFT;
      83             : 
      84           0 :         assert(vehicle.position.lane.moving.erase(vehicle.id) == 1);
      85             : 
      86           0 :         return;
      87           0 :     } else if(action->connection == Connection::STOP) {
      88             :         return;
      89             :     }
      90             : 
      91           0 :     Lane &fromLane = action->connection.fromLane;
      92           0 :     Lane &toLane   = action->connection.toLane;
      93             : 
      94           0 :     assert(vehicle.position.lane == fromLane);
      95           0 :     assert(toLane.edge == action->lane.edge);
      96             : 
      97           0 :     assert(fromLane.moving.erase(vehicle.id) == 1);
      98             : 
      99             :     // Try to leave current edge
     100           0 :     if(
     101           0 :         !vehicle.position.lane.stopped.empty()
     102           0 :         || action->connection.isRed()
     103           0 :         || action->connection.toLane.isFull()
     104             :     ) {
     105             :         // Enqueue
     106           0 :         enqueue(env, vehicle, action);
     107             : 
     108           0 :         return;
     109             :     }
     110             : 
     111           0 :     Time yieldUntil = action->connection.mustYieldUntil();
     112           0 :     if(yieldUntil > env.getTime()) {
     113             :         // Must yield to higher-priority flow; enqueue and wait
     114           0 :         enqueue(env, vehicle, action);
     115             : 
     116           0 :         env.pushEvent(make_shared<EventPopQueue>(
     117             :             yieldUntil,
     118           0 :             vehicle.position.lane
     119             :         ));
     120             : 
     121           0 :         return;
     122             :     }
     123             : 
     124             :     // Move to next edge
     125           0 :     vehicle.moveToAnotherEdge(env, action);
     126             : }