sim/mechanics/
driving.rs

1use std::collections::{BTreeMap, BTreeSet, HashMap, HashSet, VecDeque};
2
3use serde::{Deserialize, Serialize};
4
5use abstutil::{deserialize_hashmap, serialize_hashmap, FixedMap, IndexableKey};
6use geom::{Distance, Duration, PolyLine, Time};
7use map_model::{DrivingSide, IntersectionID, LaneID, Map, Path, PathStep, Position, Traversable};
8
9use crate::mechanics::car::{Car, CarState};
10use crate::mechanics::queue::{Queue, QueueEntry, Queued};
11use crate::sim::Ctx;
12use crate::{
13    ActionAtEnd, AgentID, AgentProperties, CarID, CarStatus, Command, CreateCar, DelayCause,
14    DistanceInterval, DrawCarInput, Event, IntersectionSimState, ParkedCar, ParkingSim,
15    ParkingSpot, PersonID, Problem, SimOptions, TimeInterval, TransitSimState, TripID, TripManager,
16    UnzoomedAgent, Vehicle, VehicleType, WalkingSimState, FOLLOWING_DISTANCE, MAX_CAR_LENGTH,
17};
18
19const TIME_TO_WAIT_AT_BUS_STOP: Duration = Duration::const_seconds(10.0);
20const TIME_TO_CHANGE_LANES: Duration = Duration::const_seconds(1.0);
21
22// TODO Do something else.
23pub const BLIND_RETRY_TO_CREEP_FORWARDS: Duration = Duration::const_seconds(0.1);
24pub const BLIND_RETRY_TO_REACH_END_DIST: Duration = Duration::const_seconds(5.0);
25
26/// Simulates vehicles!
27#[derive(Serialize, Deserialize, Clone)]
28pub(crate) struct DrivingSimState {
29    // This spends some space to save time. If a simulation contains 1 million cars over the course
30    // of a day, but only 100,000 are ever active simultaneously, we store 900,000 `None`s. But we
31    // gain much faster lookup, which has shown dramatic speedups in the scenarios being run so
32    // far.
33    cars: FixedMap<CarID, Car>,
34    // Note this uses a HashMap for faster lookup. Although the order of iterating over the HashMap
35    // is random, determinism in the simulation is preserved, because nothing iterates over
36    // everything.
37    #[serde(
38        serialize_with = "serialize_hashmap",
39        deserialize_with = "deserialize_hashmap"
40    )]
41    queues: HashMap<Traversable, Queue>,
42    events: Vec<Event>,
43
44    waiting_to_spawn: BTreeMap<CarID, (Position, Option<PersonID>)>,
45
46    recalc_lanechanging: bool,
47    handle_uber_turns: bool,
48
49    time_to_unpark_onstreet: Duration,
50    time_to_park_onstreet: Duration,
51    time_to_unpark_offstreet: Duration,
52    time_to_park_offstreet: Duration,
53}
54
55// Mutations
56impl DrivingSimState {
57    pub fn new(map: &Map, opts: &SimOptions) -> DrivingSimState {
58        let mut sim = DrivingSimState {
59            cars: FixedMap::new(),
60            queues: HashMap::new(),
61            events: Vec::new(),
62            recalc_lanechanging: !opts.dont_recalc_lanechanging,
63            handle_uber_turns: !opts.dont_handle_uber_turns,
64            waiting_to_spawn: BTreeMap::new(),
65
66            time_to_unpark_onstreet: Duration::seconds(10.0),
67            time_to_park_onstreet: Duration::seconds(15.0),
68            time_to_unpark_offstreet: Duration::seconds(5.0),
69            time_to_park_offstreet: Duration::seconds(5.0),
70        };
71        if opts.infinite_parking {
72            sim.time_to_unpark_offstreet = Duration::seconds(0.1);
73            sim.time_to_park_offstreet = Duration::seconds(0.1);
74        }
75
76        for l in map.all_lanes() {
77            if l.lane_type.is_for_moving_vehicles() {
78                let q = Queue::new(Traversable::Lane(l.id), map);
79                sim.queues.insert(q.id, q);
80            }
81        }
82        for t in map.all_turns() {
83            if !t.between_sidewalks() {
84                let q = Queue::new(Traversable::Turn(t.id), map);
85                sim.queues.insert(q.id, q);
86            }
87        }
88
89        sim
90    }
91
92    /// None if it worked, otherwise returns the CreateCar unmodified for possible retry.
93    pub fn start_car_on_lane(
94        &mut self,
95        now: Time,
96        mut params: CreateCar,
97        ctx: &mut Ctx,
98    ) -> Option<CreateCar> {
99        let first_lane = params.router.head().as_lane();
100        let mut start_dist = params.router.get_path().get_req().start.dist_along();
101        if let Some(ref p) = params.maybe_parked_car {
102            // If we're exiting a driveway, make the front of the vehicle wind up in the correct
103            // spot after the driveway. We could attempt to do this when we create the PathRequest,
104            // but it's complicated to adjust the position correctly, and this is the only place
105            // that needs to know.
106            if !matches!(p.spot, ParkingSpot::Onstreet(_, _)) {
107                start_dist += params.vehicle.length;
108                // TODO Should we also adjust the request?
109                if start_dist > ctx.map.get_l(first_lane).length() {
110                    error!(
111                        "At {}, {} needs to exit a driveway at {} on {}, but they overflow",
112                        now, params.vehicle.id, start_dist, first_lane
113                    );
114                    // Ideally we'd snap driveways to a point such that all equivalent positions
115                    // don't overflow, but until then, just have an unrealistic exit!
116                    start_dist -= params.vehicle.length;
117                }
118            }
119        }
120
121        // First handle any of the intermediate queues, failing fast. Record the position of the
122        // blockage's front and the index in that queue.
123        let mut blocked_starts: Vec<(Position, usize)> = Vec::new();
124        for lane in params.router.get_path().get_blocked_starts() {
125            // This buffer makes sure other vehicles can enter the queue behind a blockage very
126            // close to the start of the lane and not spillover.
127            let pos = match params
128                .router
129                .get_path()
130                .get_req()
131                .start
132                .equiv_pos(lane, ctx.map)
133                .buffer_dist(MAX_CAR_LENGTH + FOLLOWING_DISTANCE, ctx.map)
134            {
135                Some(pos) => pos,
136                None => {
137                    // TODO Loss of some simulation realism. We could also ban this upfront in
138                    // leave_from_driveway by requiring a minimum lane length on all intermediate
139                    // lanes...
140                    // Also this is super spammy
141                    if false {
142                        warn!("Not inserting a static blockage on {} at {} for {} to spawn, because the lane is too short", lane, now, params.vehicle.id);
143                    }
144                    continue;
145                }
146            };
147            if !self.queues.contains_key(&Traversable::Lane(lane)) {
148                // TODO This is probably a center turn lane, or maybe some kind of exotic center
149                // parking. Just skip over it, until we properly model them.
150                continue;
151            }
152            if let Some(idx) = self.queues[&Traversable::Lane(lane)].can_block_from_driveway(
153                // This is before adjusting for the length of the vehicle exiting the driveway
154                &pos,
155                params.vehicle.length,
156                now,
157                &self.cars,
158                &self.queues,
159            ) {
160                blocked_starts.push((pos, idx));
161            } else {
162                return Some(params);
163            }
164            // TODO What's the purpose of nobody_headed_towards again? Do we need to enforce it for
165            // intermediate lanes too?
166        }
167
168        if !ctx
169            .intersections
170            .nobody_headed_towards(first_lane, ctx.map.get_l(first_lane).src_i)
171        {
172            return Some(params);
173        }
174        if let Some(idx) = self.queues[&Traversable::Lane(first_lane)].get_idx_to_insert_car(
175            start_dist,
176            params.vehicle.length,
177            now,
178            &self.cars,
179            &self.queues,
180        ) {
181            let mut car = Car {
182                vehicle: params.vehicle,
183                router: params.router,
184                // Temporary
185                state: CarState::Queued {
186                    blocked_since: now,
187                    want_to_change_lanes: None,
188                },
189                last_steps: VecDeque::new(),
190                started_at: now,
191                total_blocked_time: Duration::ZERO,
192                trip_and_person: params.trip_and_person,
193                wants_to_overtake: BTreeSet::new(),
194            };
195            let mut start_crossing = false;
196            if let Some(p) = params.maybe_parked_car {
197                let delay = match p.spot {
198                    ParkingSpot::Onstreet(_, _) => self.time_to_unpark_onstreet,
199                    ParkingSpot::Offstreet(_, _) | ParkingSpot::Lot(_, _) => {
200                        // Even in infinite parking mode, we want to block intermediate lanes for a
201                        // few seconds.
202                        //
203                        // TODO Actually, revisit ~instantaneous unparking in infinite mode. Were
204                        // we making gridlock progress somewhere because of this?
205                        if blocked_starts.is_empty() {
206                            self.time_to_unpark_offstreet
207                        } else {
208                            Duration::seconds(5.0)
209                        }
210                    }
211                };
212                let mut lanes = Vec::new();
213                for (pos, idx) in blocked_starts {
214                    self.queues
215                        .get_mut(&Traversable::Lane(pos.lane()))
216                        .unwrap()
217                        .add_static_blockage(
218                            car.vehicle.id,
219                            pos.dist_along(),
220                            pos.dist_along() - car.vehicle.length,
221                            idx,
222                        );
223                    lanes.push(pos.lane());
224                }
225
226                car.state = CarState::Unparking {
227                    front: start_dist,
228                    spot: p.spot,
229                    time_int: TimeInterval::new(now, now + delay),
230                    blocked_starts: lanes,
231                };
232            } else {
233                // Have to do this early
234                if car.router.last_step() {
235                    match car.router.maybe_handle_end(
236                        start_dist,
237                        &car.vehicle,
238                        ctx.parking,
239                        ctx.map,
240                        car.trip_and_person,
241                        &mut self.events,
242                    ) {
243                        None | Some(ActionAtEnd::GotoLaneEnd) => {}
244                        x => {
245                            panic!(
246                                "Car with one-step route {:?} had unexpected result from \
247                                 maybe_handle_end: {:?}",
248                                car.router, x
249                            );
250                        }
251                    }
252                    // We might've decided to go park somewhere farther, so get_end_dist no longer
253                    // makes sense.
254                    if car.router.last_step() && start_dist > car.router.get_end_dist() {
255                        println!(
256                            "WARNING: {} wants to spawn at {}, which is past their end of {} on a \
257                             one-step path {}",
258                            car.vehicle.id,
259                            start_dist,
260                            car.router.get_end_dist(),
261                            first_lane
262                        );
263                        params.router = car.router;
264                        params.vehicle = car.vehicle;
265                        return Some(params);
266                    }
267                }
268
269                car.state = car.crossing_state(start_dist, now, ctx.map);
270                start_crossing = true;
271            }
272            ctx.scheduler
273                .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
274            self.queues
275                .get_mut(&Traversable::Lane(first_lane))
276                .unwrap()
277                .insert_car_at_idx(idx, &car);
278            self.waiting_to_spawn.remove(&car.vehicle.id);
279
280            if start_crossing {
281                // Don't call this earlier where we set crossing_state, because we're not in the
282                // queue yet
283                self.new_crossing_state(ctx, &car);
284            }
285
286            self.cars.insert(car.vehicle.id, car);
287
288            return None;
289        }
290        Some(params)
291    }
292
293    /// If start_car_on_lane fails and a retry is scheduled, this is an idempotent way to mark the
294    /// vehicle as active, but waiting to spawn.
295    pub fn vehicle_waiting_to_spawn(&mut self, id: CarID, pos: Position, person: Option<PersonID>) {
296        self.waiting_to_spawn.insert(id, (pos, person));
297    }
298
299    /// State transitions for this car:
300    ///
301    /// Crossing -> Queued or WaitingToAdvance
302    /// Unparking -> Crossing
303    /// IdlingAtStop -> Crossing
304    /// Queued -> last step handling (Parking or done)
305    /// WaitingToAdvance -> try to advance to the next step of the path
306    /// Parking -> done
307    ///
308    /// State transitions for other cars:
309    ///
310    /// Crossing -> Crossing (recalculate dist/time)
311    /// Queued -> Crossing
312    ///
313    /// Why is it safe to process cars in any order, rather than making sure to follow the order
314    /// of queues? Because of the invariant that distances should never suddenly jump when a car
315    /// has entered/exiting a queue.
316    /// This car might have reached the router's end distance, but maybe not -- might
317    /// actually be stuck behind other cars. We have to calculate the distances right now to
318    /// be sure.
319    pub fn update_car(
320        &mut self,
321        id: CarID,
322        now: Time,
323        ctx: &mut Ctx,
324        trips: &mut TripManager,
325        transit: &mut TransitSimState,
326        walking: &mut WalkingSimState,
327    ) {
328        let mut need_distances = {
329            let car = &self.cars[&id];
330            match car.state {
331                CarState::Queued { .. } => car.router.last_step(),
332                CarState::Parking(_, _, _) => true,
333                CarState::IdlingAtStop(_, _) => true,
334                _ => false,
335            }
336        };
337
338        if !need_distances {
339            // We need to mutate two different cars in one case. To avoid fighting the borrow
340            // checker, temporarily move one of them out of the map.
341            let mut car = self.cars.remove(&id).unwrap();
342            // Responsibility of update_car to manage scheduling stuff!
343            need_distances = self.update_car_without_distances(&mut car, now, ctx, transit);
344            self.cars.insert(id, car);
345        }
346        // Note we might set need_distances to true, so both of these conditionals might run.
347        if need_distances {
348            // Do this before removing the car!
349            let dists = self.queues[&self.cars[&id].router.head()].get_car_positions(
350                now,
351                &self.cars,
352                &self.queues,
353            );
354            let idx = dists
355                .iter()
356                .position(|entry| entry.member == Queued::Vehicle(id))
357                .unwrap();
358
359            // We need to mutate two different cars in some cases. To avoid fighting the borrow
360            // checker, temporarily move one of them out of the map.
361            let mut car = self.cars.remove(&id).unwrap();
362            // Responsibility of update_car_with_distances to manage scheduling stuff!
363            if self
364                .update_car_with_distances(&mut car, &dists, idx, now, ctx, trips, transit, walking)
365            {
366                self.cars.insert(id, car);
367            } else {
368                self.delete_car_internal(&mut car, dists, idx, now, ctx);
369            }
370        }
371    }
372
373    // If this returns true, we need to immediately run update_car_with_distances. If we don't,
374    // then the car will briefly be Queued and might immediately become something else, which
375    // affects how leaders update followers.
376    fn update_car_without_distances(
377        &mut self,
378        car: &mut Car,
379        now: Time,
380        ctx: &mut Ctx,
381        transit: &mut TransitSimState,
382    ) -> bool {
383        match car.state {
384            CarState::Crossing { .. } => {
385                car.state = CarState::Queued {
386                    blocked_since: now,
387                    want_to_change_lanes: None,
388                };
389                if car.router.last_step() {
390                    // Immediately run update_car_with_distances.
391                    return true;
392                }
393                let queue = &self.queues[&car.router.head()];
394                if queue.is_car_at_front(car.vehicle.id) {
395                    // Want to re-run, but no urgency about it happening immediately.
396                    car.state = CarState::WaitingToAdvance { blocked_since: now };
397                    if self.recalc_lanechanging {
398                        car.router.opportunistically_lanechange(
399                            &self.queues,
400                            ctx.map,
401                            self.handle_uber_turns,
402                        );
403                    }
404                    ctx.scheduler.push(now, Command::UpdateCar(car.vehicle.id));
405                } else if let Some(slow_leader) = self.wants_to_overtake(car) {
406                    // TODO This entire check kicks in a little late; we only enter Queued after
407                    // spending the freeflow time possibly moving very slowly.
408                    let first_conflict = car.wants_to_overtake.insert(slow_leader);
409
410                    // Record when a vehicle wants to pass a bike
411                    if first_conflict
412                        && slow_leader.vehicle_type == VehicleType::Bike
413                        && car.vehicle.vehicle_type != VehicleType::Bike
414                    {
415                        self.events.push(Event::ProblemEncountered(
416                            self.cars[&slow_leader].trip_and_person.unwrap().0,
417                            Problem::OvertakeDesired(queue.id),
418                        ));
419                    }
420
421                    if let Some(target_lane) = self.pick_overtaking_lane(car, ctx.map) {
422                        // We need the current position of the car to see if lane-changing is
423                        // actually feasible right now, so record our intention and trigger
424                        // update_car_with_distances.
425                        car.state = CarState::Queued {
426                            blocked_since: now,
427                            want_to_change_lanes: Some(target_lane),
428                        };
429                        return true;
430                    }
431                }
432            }
433            CarState::Unparking {
434                front,
435                ref blocked_starts,
436                ..
437            } => {
438                for lane in blocked_starts {
439                    // Calculate the exact positions along this blocked queue (which is ***NOT***
440                    // the same queue that the unparking car is in!). Use that to update the
441                    // follower.
442                    //
443                    // It SHOULD be fine that the current car isn't currently in self.cars; the
444                    // static blockage doesn't need it. But calculating positions on one queue may
445                    // recurse to the queue where the current car is. So temporarily make the car
446                    // visible in this query.
447                    self.cars.insert(car.vehicle.id, car.clone());
448
449                    let dists = self.queues[&Traversable::Lane(*lane)].get_car_positions(
450                        now,
451                        &self.cars,
452                        &self.queues,
453                    );
454
455                    // Undo the above hack.
456                    self.cars.remove(&car.vehicle.id);
457
458                    let idx = dists.iter().position(|entry| matches!(entry.member, Queued::StaticBlockage { cause, ..} if cause == car.vehicle.id)).unwrap();
459                    self.update_follower(idx, &dists, now, ctx);
460
461                    self.queues
462                        .get_mut(&Traversable::Lane(*lane))
463                        .unwrap()
464                        .clear_static_blockage(car.vehicle.id, idx);
465                }
466
467                if car.router.last_step() {
468                    // Actually, we need to do this first. Ignore the answer -- if we're doing
469                    // something weird like vanishing or re-parking immediately (quite unlikely),
470                    // the next loop will pick that up. Just trigger the side effect of choosing an
471                    // end_dist.
472                    car.router.maybe_handle_end(
473                        front,
474                        &car.vehicle,
475                        ctx.parking,
476                        ctx.map,
477                        car.trip_and_person,
478                        &mut self.events,
479                    );
480                }
481                car.state = car.crossing_state(front, now, ctx.map);
482                ctx.scheduler
483                    .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
484                self.new_crossing_state(ctx, car);
485            }
486            CarState::WaitingToAdvance { blocked_since } => {
487                // 'car' is the leader.
488                let from = car.router.head();
489                let goto = car.router.next();
490                assert!(from != goto);
491
492                if let Traversable::Turn(t) = goto {
493                    if !ctx.intersections.maybe_start_turn(
494                        AgentID::Car(car.vehicle.id),
495                        t,
496                        PathStep::Turn(t).max_speed_along(
497                            car.vehicle.max_speed,
498                            car.vehicle.vehicle_type.to_constraints(),
499                            ctx.map,
500                        ),
501                        now,
502                        ctx.map,
503                        ctx.scheduler,
504                        Some((car, &self.cars, &mut self.queues)),
505                    ) {
506                        // Don't schedule a retry here.
507                        return false;
508                    }
509                    if let Some((trip, _)) = car.trip_and_person {
510                        self.events.push(Event::IntersectionDelayMeasured(
511                            trip,
512                            t,
513                            AgentID::Car(car.vehicle.id),
514                            now - blocked_since,
515                        ));
516                    }
517                }
518
519                {
520                    let queue = self.queues.get_mut(&from).unwrap();
521                    assert_eq!(queue.move_first_car_to_laggy_head(), car.vehicle.id);
522                }
523
524                // We do NOT need to update the follower. If they were Queued, they'll remain that
525                // way, until laggy_head is None.
526
527                let last_step = car.router.advance(
528                    &car.vehicle,
529                    ctx.parking,
530                    ctx.map,
531                    car.trip_and_person,
532                    &mut self.events,
533                );
534                car.total_blocked_time += now - blocked_since;
535                car.state = car.crossing_state(Distance::ZERO, now, ctx.map);
536                ctx.scheduler
537                    .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
538                self.events.push(Event::AgentEntersTraversable(
539                    AgentID::Car(car.vehicle.id),
540                    car.trip_and_person.map(|(t, _)| t),
541                    goto,
542                    if car.vehicle.vehicle_type.is_transit() {
543                        Some(transit.get_passengers(car.vehicle.id).len())
544                    } else {
545                        None
546                    },
547                ));
548
549                // Don't mark turn_finished until our back is out of the turn.
550                car.last_steps.push_front(last_step);
551
552                // Optimistically assume we'll be out of the way ASAP.
553                // This is update, not push, because we might've scheduled a blind retry too late,
554                // and the car actually crosses an entire new traversable in the meantime.
555                ctx.scheduler.update(
556                    car.crossing_state_with_end_dist(
557                        DistanceInterval::new_driving(
558                            Distance::ZERO,
559                            car.vehicle.length + FOLLOWING_DISTANCE,
560                        ),
561                        now,
562                        ctx.map,
563                    )
564                    .get_end_time(),
565                    Command::UpdateLaggyHead(car.vehicle.id),
566                );
567
568                self.queues
569                    .get_mut(&goto)
570                    .unwrap()
571                    .push_car_onto_end(car.vehicle.id);
572            }
573            CarState::ChangingLanes {
574                from,
575                new_time,
576                new_dist,
577                ..
578            } => {
579                // The car is already in the target queue. Just set them in the crossing state; we
580                // already calculated the intervals for it.
581                car.state = CarState::Crossing {
582                    time_int: new_time,
583                    dist_int: new_dist,
584                    steep_uphill: false,
585                };
586                ctx.scheduler
587                    .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
588
589                // And remove the blockage from the old queue. Similar to the note in this function
590                // for Unparking, we calculate distances in that OTHER queue.
591                let dists = self.queues[&Traversable::Lane(from)].get_car_positions(
592                    now,
593                    &self.cars,
594                    &self.queues,
595                );
596                let idx = dists.iter().position(|entry| matches!(entry.member, Queued::DynamicBlockage { cause, ..} if cause == car.vehicle.id)).unwrap();
597                self.update_follower(idx, &dists, now, ctx);
598
599                self.queues
600                    .get_mut(&Traversable::Lane(from))
601                    .unwrap()
602                    .clear_dynamic_blockage(car.vehicle.id, idx);
603            }
604            CarState::Queued { .. } => unreachable!(),
605            CarState::Parking(_, _, _) => unreachable!(),
606            CarState::IdlingAtStop(_, _) => unreachable!(),
607        }
608        false
609    }
610
611    // Returns true if the car survives.
612    fn update_car_with_distances(
613        &mut self,
614        car: &mut Car,
615        dists: &[QueueEntry],
616        idx: usize,
617        now: Time,
618        ctx: &mut Ctx,
619        trips: &mut TripManager,
620        transit: &mut TransitSimState,
621        walking: &mut WalkingSimState,
622    ) -> bool {
623        let our_dist = dists[idx].front;
624
625        match car.state {
626            CarState::Crossing { .. }
627            | CarState::Unparking { .. }
628            | CarState::WaitingToAdvance { .. }
629            | CarState::ChangingLanes { .. } => unreachable!(),
630            CarState::Queued {
631                blocked_since,
632                want_to_change_lanes,
633            } => {
634                // Two totally different reasons we'll wind up here: we want to lane-change, and
635                // we're on our last step.
636                if let Some(target_lane) = want_to_change_lanes {
637                    self.try_start_lc(car, our_dist, idx, target_lane, now, ctx);
638                    return true;
639                }
640
641                match car.router.maybe_handle_end(
642                    our_dist,
643                    &car.vehicle,
644                    ctx.parking,
645                    ctx.map,
646                    car.trip_and_person,
647                    &mut self.events,
648                ) {
649                    Some(ActionAtEnd::VanishAtBorder(i)) => {
650                        car.total_blocked_time += now - blocked_since;
651                        // Don't do this for buses
652                        if car.trip_and_person.is_some() {
653                            trips.car_or_bike_reached_border(
654                                now,
655                                car.vehicle.id,
656                                i,
657                                car.total_blocked_time,
658                                car.router.get_path().total_length(),
659                                ctx,
660                            );
661                        }
662                        false
663                    }
664                    Some(ActionAtEnd::GiveUpOnParking) => {
665                        car.total_blocked_time += now - blocked_since;
666                        trips.cancel_trip(
667                            now,
668                            car.trip_and_person.unwrap().0,
669                            "no available parking anywhere".to_string(),
670                            // If we couldn't find parking normally, doesn't make sense to warp the
671                            // car to the destination. There's no parking!
672                            None,
673                            ctx,
674                        );
675                        false
676                    }
677                    Some(ActionAtEnd::StartParking(spot)) => {
678                        car.total_blocked_time += now - blocked_since;
679                        let delay = match spot {
680                            ParkingSpot::Onstreet(_, _) => self.time_to_park_onstreet,
681                            ParkingSpot::Offstreet(_, _) | ParkingSpot::Lot(_, _) => {
682                                self.time_to_park_offstreet
683                            }
684                        };
685                        car.state =
686                            CarState::Parking(our_dist, spot, TimeInterval::new(now, now + delay));
687                        // If we don't do this, then we might have another car creep up behind, see
688                        // the spot free, and start parking too. This can happen with multiple
689                        // lanes and certain vehicle lengths.
690                        ctx.parking.reserve_spot(spot, car.vehicle.id);
691                        ctx.scheduler
692                            .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
693                        true
694                    }
695                    Some(ActionAtEnd::GotoLaneEnd) => {
696                        car.total_blocked_time += now - blocked_since;
697                        car.state = car.crossing_state(our_dist, now, ctx.map);
698                        ctx.scheduler
699                            .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
700                        self.new_crossing_state(ctx, car);
701                        true
702                    }
703                    Some(ActionAtEnd::StopBiking(bike_rack)) => {
704                        car.total_blocked_time += now - blocked_since;
705                        trips.bike_reached_end(
706                            now,
707                            car.vehicle.id,
708                            bike_rack,
709                            car.total_blocked_time,
710                            car.router.get_path().total_length(),
711                            ctx,
712                        );
713                        false
714                    }
715                    Some(ActionAtEnd::BusAtStop) => {
716                        car.total_blocked_time += now - blocked_since;
717                        if transit.bus_arrived_at_stop(now, car.vehicle.id, trips, walking, ctx) {
718                            car.state = CarState::IdlingAtStop(
719                                our_dist,
720                                TimeInterval::new(now, now + TIME_TO_WAIT_AT_BUS_STOP),
721                            );
722                            ctx.scheduler
723                                .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
724                            true
725                        } else {
726                            // Vanishing at a border
727                            false
728                        }
729                    }
730                    None => {
731                        ctx.scheduler.push(
732                            now + BLIND_RETRY_TO_REACH_END_DIST,
733                            Command::UpdateCar(car.vehicle.id),
734                        );
735
736                        // TODO For now, always use BLIND_RETRY_TO_REACH_END_DIST. Measured things
737                        // to be slower otherwise. :(
738                        /*
739                        // If this car wasn't blocked at all, when would it reach its goal?
740                        let ideal_end_time = match car.crossing_state(our_dist, now, map) {
741                            CarState::Crossing { time_int, .. } => time_int.end,
742                            _ => unreachable!(),
743                        };
744                        if ideal_end_time == now {
745                            // Haha, no such luck. We're super super close to the goal, but not
746                            // quite there yet.
747                            scheduler.push(now + BLIND_RETRY_TO_REACH_END_DIST, Command::UpdateCar(car.vehicle.id));
748                        } else {
749                            scheduler.push(ideal_end_time, Command::UpdateCar(car.vehicle.id));
750                        }
751                        // TODO For cars stuck on their last step, this will spam a fair bit. But
752                        // that should be pretty rare.
753                        */
754
755                        true
756                    }
757                }
758            }
759            CarState::Parking(_, spot, _) => {
760                ctx.parking.add_parked_car(ParkedCar {
761                    vehicle: car.vehicle.clone(),
762                    spot,
763                    parked_since: now,
764                });
765                trips.car_reached_parking_spot(
766                    now,
767                    car.vehicle.id,
768                    spot,
769                    car.total_blocked_time,
770                    car.router.get_path().total_length(),
771                    ctx,
772                );
773                false
774            }
775            CarState::IdlingAtStop(dist, _) => {
776                car.router = transit.bus_departed_from_stop(car.vehicle.id, ctx.map);
777                self.events
778                    .push(Event::PathAmended(car.router.get_path().clone()));
779                car.state = car.crossing_state(dist, now, ctx.map);
780                ctx.scheduler
781                    .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
782                self.new_crossing_state(ctx, car);
783
784                self.update_follower(idx, dists, now, ctx);
785
786                true
787            }
788        }
789    }
790
791    /// Abruptly remove a vehicle from the simulation. They may be in any arbitrary state, like in
792    /// the middle of a turn or parking.
793    pub fn delete_car(&mut self, c: CarID, now: Time, ctx: &mut Ctx) -> Vehicle {
794        self.waiting_to_spawn.remove(&c);
795
796        let dists = self.queues[&self.cars[&c].router.head()].get_car_positions(
797            now,
798            &self.cars,
799            &self.queues,
800        );
801        let idx = dists
802            .iter()
803            .position(|entry| entry.member == Queued::Vehicle(c))
804            .unwrap();
805        let mut car = self.cars.remove(&c).unwrap();
806
807        // Hacks to delete cars that're mid-turn
808        if let Traversable::Turn(t) = car.router.head() {
809            let queue = self.queues.get_mut(&car.router.head()).unwrap();
810            // delete_car_internal will call free_reserved_space, so this is necessary to balance
811            // that.
812            queue.reserved_length += car.vehicle.length + FOLLOWING_DISTANCE;
813            ctx.intersections.agent_deleted_mid_turn(AgentID::Car(c), t);
814
815            // Free any reserved space on the next step.
816            let queue = self.queues.get_mut(&car.router.next()).unwrap();
817            queue.free_reserved_space(&car);
818        }
819        if let Some(Traversable::Turn(t)) = car.router.maybe_next() {
820            ctx.intersections.cancel_request(AgentID::Car(c), t);
821        }
822
823        if car.router.last_step() {
824            ctx.parking.unreserve_spot(c);
825        }
826
827        self.delete_car_internal(&mut car, dists, idx, now, ctx);
828        // delete_car_internal cancels UpdateLaggyHead
829        ctx.scheduler.cancel(Command::UpdateCar(c));
830        car.vehicle
831    }
832
833    fn delete_car_internal(
834        &mut self,
835        car: &mut Car,
836        dists: Vec<QueueEntry>,
837        idx: usize,
838        now: Time,
839        ctx: &mut Ctx,
840    ) {
841        {
842            let queue = self.queues.get_mut(&car.router.head()).unwrap();
843            queue.remove_car_from_idx(car.vehicle.id, idx);
844            // trim_last_steps doesn't actually include the current queue!
845            queue.free_reserved_space(car);
846            let i = match queue.id {
847                Traversable::Lane(l) => ctx.map.get_l(l).src_i,
848                Traversable::Turn(t) => t.parent,
849            };
850            if ctx.handling_live_edits.is_none() {
851                ctx.intersections
852                    .space_freed(now, i, ctx.scheduler, ctx.map);
853            }
854        }
855
856        ctx.intersections.vehicle_gone(car.vehicle.id);
857
858        // We might be vanishing while partly clipping into other stuff.
859        self.trim_last_steps(car, now, car.last_steps.len(), ctx);
860
861        // We might've scheduled one of those using BLIND_RETRY_TO_CREEP_FORWARDS.
862        ctx.scheduler
863            .cancel(Command::UpdateLaggyHead(car.vehicle.id));
864
865        self.update_follower(idx, &dists, now, ctx);
866    }
867
868    /// After a leader (maybe an active vehicle, maybe a static blockage) gets out of the way,
869    /// update the follower so that they don't suddenly jump forwards.
870    fn update_follower(
871        &mut self,
872        idx_leader: usize,
873        dists: &[QueueEntry],
874        now: Time,
875        ctx: &mut Ctx,
876    ) {
877        if idx_leader == dists.len() - 1 {
878            return;
879        }
880        let idx_follower = idx_leader + 1;
881
882        if let Queued::Vehicle(follower_id) = dists[idx_follower].member {
883            let follower_dist = dists[idx_follower].front;
884
885            // If we're going to delete the follower soon, don't bother waking them up.
886            if let Some(ref deleting_agents) = ctx.handling_live_edits {
887                if deleting_agents.contains(&AgentID::Car(follower_id)) {
888                    return;
889                }
890            }
891
892            let follower = self.cars.get_mut(&follower_id).unwrap();
893            // TODO If the leader vanished at a border node, this still jumps a bit -- the lead
894            // car's back is still sticking out. Need to still be bound by them, even though they
895            // don't exist! If the leader just parked, then we're fine.
896            match follower.state {
897                CarState::Queued { blocked_since, .. } => {
898                    // TODO If they're on their last step, they might be ending early and not right
899                    // behind us? !follower.router.last_step()
900
901                    // Prevent them from jumping forwards.
902                    follower.total_blocked_time += now - blocked_since;
903                    follower.state = follower.crossing_state(follower_dist, now, ctx.map);
904                    ctx.scheduler.update(
905                        follower.state.get_end_time(),
906                        Command::UpdateCar(follower_id),
907                    );
908                    let follower = &self.cars[&follower_id];
909                    self.new_crossing_state(ctx, follower);
910                }
911                CarState::Crossing { .. } => {
912                    // If the follower was still Crossing, they might not've been blocked by the
913                    // leader yet. But recalculating their Crossing state isn't necessarily a no-op
914                    // -- this could prevent them from suddenly warping past a blockage.
915                    follower.state = follower.crossing_state(follower_dist, now, ctx.map);
916                    ctx.scheduler.update(
917                        follower.state.get_end_time(),
918                        Command::UpdateCar(follower_id),
919                    );
920                    // This'll possibly update the ETA
921                    let follower = &self.cars[&follower_id];
922                    self.new_crossing_state(ctx, follower);
923                }
924                CarState::ChangingLanes {
925                    from, to, lc_time, ..
926                } => {
927                    // This is a fun case -- something stopped blocking somebody that was in the
928                    // process of lane-changing! Similar to the Crossing case above, we just have
929                    // to update the distance/time intervals, but otherwise leave them in the
930                    // middle of their lane-changing. It's guaranteed that lc_time will continue to
931                    // finish before the new time interval, because there's no possible way
932                    // recalculating this crossing state here will speed things up from the
933                    // original estimate.
934                    let (new_time, new_dist) = match follower.crossing_state_with_end_dist(
935                        DistanceInterval::new_driving(follower_dist, ctx.map.get_l(to).length()),
936                        now,
937                        ctx.map,
938                    ) {
939                        CarState::Crossing {
940                            time_int, dist_int, ..
941                        } => (time_int, dist_int),
942                        _ => unreachable!(),
943                    };
944                    assert!(new_time.end >= lc_time.end);
945                    follower.state = CarState::ChangingLanes {
946                        from,
947                        to,
948                        new_time,
949                        new_dist,
950                        lc_time,
951                    };
952                }
953                // They weren't blocked
954                CarState::Unparking { .. }
955                | CarState::Parking(_, _, _)
956                | CarState::IdlingAtStop(_, _) => {}
957                CarState::WaitingToAdvance { .. } => unreachable!(),
958            }
959        }
960    }
961
962    pub fn update_laggy_head(&mut self, id: CarID, now: Time, ctx: &mut Ctx) {
963        let currently_on = self.cars[&id].router.head();
964        // This car must be the tail.
965        let dist_along_last = {
966            let (last_id, dist) = self.queues[&currently_on]
967                .get_last_car_position(now, &self.cars, &self.queues)
968                .unwrap();
969            if id != last_id {
970                panic!(
971                    "At {} on {:?}, laggy head {} isn't the last on the lane; it's {}",
972                    now, currently_on, id, last_id
973                );
974            }
975            dist
976        };
977
978        // Trim off as many of the oldest last_steps as we've made distance.
979        let mut dist_left_to_cleanup = self.cars[&id].vehicle.length + FOLLOWING_DISTANCE;
980        dist_left_to_cleanup -= dist_along_last;
981        let mut num_to_trim = None;
982        for (idx, step) in self.cars[&id].last_steps.iter().enumerate() {
983            if dist_left_to_cleanup <= Distance::ZERO {
984                num_to_trim = Some(self.cars[&id].last_steps.len() - idx);
985                break;
986            }
987            dist_left_to_cleanup -= step.get_polyline(ctx.map).length();
988        }
989
990        if let Some(n) = num_to_trim {
991            let mut car = self.cars.remove(&id).unwrap();
992            self.trim_last_steps(&mut car, now, n, ctx);
993            self.cars.insert(id, car);
994        }
995
996        if !self.cars[&id].last_steps.is_empty() {
997            // Might have to retry again later.
998            let retry_at = self.cars[&id]
999                .crossing_state_with_end_dist(
1000                    // Update again when we've completely cleared all last_steps. We could be more
1001                    // precise and do it sooner when we clear the last step, but a little delay is
1002                    // fine for correctness.
1003                    DistanceInterval::new_driving(
1004                        dist_along_last,
1005                        self.cars[&id].vehicle.length + FOLLOWING_DISTANCE,
1006                    ),
1007                    now,
1008                    ctx.map,
1009                )
1010                .get_end_time();
1011            // Sometimes due to rounding, retry_at will be exactly time, but we really need to
1012            // wait a bit longer.
1013            // TODO Smarter retry based on states and stuckness?
1014            if retry_at > now {
1015                ctx.scheduler.push(retry_at, Command::UpdateLaggyHead(id));
1016            } else {
1017                // If we look up car positions before this retry happens, weird things can
1018                // happen -- the laggy head could be well clear of the old queue by then. Make
1019                // sure to handle that there. Consequences of this retry being long? A follower
1020                // will wait a bit before advancing.
1021                ctx.scheduler.push(
1022                    now + BLIND_RETRY_TO_CREEP_FORWARDS,
1023                    Command::UpdateLaggyHead(id),
1024                );
1025            }
1026        }
1027    }
1028
1029    // Caller has to figure out how many steps to trim!
1030    fn trim_last_steps(&mut self, car: &mut Car, now: Time, n: usize, ctx: &mut Ctx) {
1031        for i in 0..n {
1032            let on = car.last_steps.pop_back().unwrap();
1033            let old_queue = self.queues.get_mut(&on).unwrap();
1034            assert_eq!(old_queue.laggy_head, Some(car.vehicle.id));
1035            old_queue.laggy_head = None;
1036            match on {
1037                Traversable::Turn(t) => {
1038                    ctx.intersections.turn_finished(
1039                        now,
1040                        AgentID::Car(car.vehicle.id),
1041                        t,
1042                        ctx.scheduler,
1043                        ctx.map,
1044                        ctx.handling_live_edits.is_some(),
1045                    );
1046                }
1047                Traversable::Lane(l) => {
1048                    old_queue.free_reserved_space(car);
1049                    if ctx.handling_live_edits.is_none() {
1050                        ctx.intersections.space_freed(
1051                            now,
1052                            ctx.map.get_l(l).src_i,
1053                            ctx.scheduler,
1054                            ctx.map,
1055                        );
1056                    }
1057                }
1058            }
1059
1060            if i == 0 {
1061                // Wake up the follower
1062                if let Some(follower_id) = old_queue.get_active_cars().get(0) {
1063                    // TODO Stop using get_active_cars for this! Be paranoid.
1064                    if old_queue.is_car_at_front(*follower_id) {
1065                        let follower = self.cars.get_mut(follower_id).unwrap();
1066
1067                        match follower.state {
1068                            CarState::Queued { blocked_since, .. } => {
1069                                // If they're on their last step, they might be ending early and not
1070                                // right behind us.
1071                                if !follower.router.last_step() {
1072                                    // The follower has been smoothly following while the laggy head
1073                                    // gets out of the way. So immediately promote them to
1074                                    // WaitingToAdvance.
1075                                    follower.state = CarState::WaitingToAdvance { blocked_since };
1076                                    if self.recalc_lanechanging && ctx.handling_live_edits.is_none()
1077                                    {
1078                                        follower.router.opportunistically_lanechange(
1079                                            &self.queues,
1080                                            ctx.map,
1081                                            self.handle_uber_turns,
1082                                        );
1083                                    }
1084                                    ctx.scheduler
1085                                        .push(now, Command::UpdateCar(follower.vehicle.id));
1086                                }
1087                            }
1088                            CarState::WaitingToAdvance { .. } => unreachable!(),
1089                            // They weren't blocked. Note that there's no way the Crossing state could
1090                            // jump forwards here; the leader vanished from the end of the traversable.
1091                            CarState::Crossing { .. }
1092                            | CarState::ChangingLanes { .. }
1093                            | CarState::Unparking { .. }
1094                            | CarState::Parking(_, _, _)
1095                            | CarState::IdlingAtStop(_, _) => {}
1096                        }
1097                    }
1098                }
1099            } else {
1100                // Only the last step we cleared could possibly have cars. Any intermediates, this
1101                // car was previously completely blocking them.
1102                assert!(old_queue.get_active_cars().is_empty());
1103            }
1104        }
1105    }
1106
1107    /// If the car wants to over-take somebody, what adjacent lane should they use?
1108    /// - The lane must be in the same direction as the current; no support for crossing the road's
1109    ///   yellow line yet.
1110    /// - Prefer passing on the left (for DrivingSide::Right)
1111    /// For now, just pick one candidate lane, even if both might be usable.
1112    fn pick_overtaking_lane(&self, car: &Car, map: &Map) -> Option<LaneID> {
1113        // Don't overtake in the middle of a turn!
1114        let current_lane = map.get_l(car.router.head().maybe_lane()?);
1115        let road = map.get_parent(current_lane.id);
1116        let idx = current_lane.id.offset;
1117
1118        let mut candidates = Vec::new();
1119        if idx != 0 {
1120            candidates.push(road.lanes[idx - 1].id);
1121        }
1122        if idx != road.lanes.len() - 1 {
1123            candidates.push(road.lanes[idx + 1].id);
1124        }
1125        if map.get_config().driving_side == DrivingSide::Left {
1126            candidates.reverse();
1127        }
1128
1129        for l in candidates {
1130            let target_lane = map.get_l(l);
1131            // Must be the same direction -- no crossing into oncoming traffic yet
1132            if current_lane.dir != target_lane.dir {
1133                continue;
1134            }
1135            // The lane types can differ, as long as the vehicle can use the target. Imagine
1136            // overtaking a slower cyclist in a bike lane using the rest of the road.
1137            if !car
1138                .vehicle
1139                .vehicle_type
1140                .to_constraints()
1141                .can_use(target_lane, map)
1142            {
1143                continue;
1144            }
1145            // Is this other lane compatible with the path? We won't make any attempts to return to the
1146            // original lane after changing.
1147            if !car
1148                .router
1149                .can_lanechange(current_lane.id, target_lane.id, map)
1150            {
1151                continue;
1152            }
1153            return Some(target_lane.id);
1154        }
1155
1156        None
1157    }
1158
1159    fn try_start_lc(
1160        &mut self,
1161        car: &mut Car,
1162        front_current_queue: Distance,
1163        idx_in_current_queue: usize,
1164        target_lane: LaneID,
1165        now: Time,
1166        ctx: &mut Ctx,
1167    ) {
1168        // If we are a laggy head somewhere else (our back is still sticking into another lane or
1169        // turn), don't start lane-changing!
1170        if !car.last_steps.is_empty() {
1171            return;
1172        }
1173        // If the lanes are very different lengths and we're too close to the end at the target,
1174        // not going to work.
1175        if front_current_queue >= ctx.map.get_l(target_lane).length() {
1176            return;
1177        }
1178        let current_lane = car.router.head().as_lane();
1179        let front_target_queue = Position::new(current_lane, front_current_queue)
1180            .equiv_pos(target_lane, ctx.map)
1181            .dist_along();
1182
1183        // Calculate the crossing state in the target queue. Pass in the DistanceInterval
1184        // explicitly, because we haven't modified the route yet.
1185        let (new_time, new_dist) = match car.crossing_state_with_end_dist(
1186            DistanceInterval::new_driving(front_target_queue, ctx.map.get_l(target_lane).length()),
1187            now,
1188            ctx.map,
1189        ) {
1190            CarState::Crossing {
1191                time_int, dist_int, ..
1192            } => (time_int, dist_int),
1193            _ => unreachable!(),
1194        };
1195
1196        // Do we have enough time to finish the lane-change, assuming that we go as fast as
1197        // possible in the target?
1198        let lc_time = TimeInterval::new(now, now + TIME_TO_CHANGE_LANES);
1199        if lc_time.end >= new_time.end {
1200            return;
1201        }
1202
1203        // Is there room for us to sliiiide on over into that lane's DMs?
1204        if let Some(idx_in_target_queue) = self.queues[&Traversable::Lane(target_lane)]
1205            .get_idx_to_insert_car(
1206                front_target_queue,
1207                car.vehicle.length,
1208                now,
1209                &self.cars,
1210                &self.queues,
1211            )
1212        {
1213            // TODO Can downgrade this to an alert or debug once active work has settled down
1214            if false {
1215                info!(
1216                    "{} is starting to change lanes from {} to {}",
1217                    car.vehicle.id,
1218                    car.router.head(),
1219                    target_lane
1220                );
1221            }
1222
1223            // Exit the old queue (leaving a dynamic blockage in place)
1224            self.queues
1225                .get_mut(&car.router.head())
1226                .unwrap()
1227                .replace_car_with_dynamic_blockage(car, idx_in_current_queue);
1228
1229            // Change the path
1230            car.router.confirm_lanechange(target_lane, ctx.map);
1231
1232            // Insert into the new queue
1233            self.queues
1234                .get_mut(&car.router.head())
1235                .unwrap()
1236                .insert_car_at_idx(idx_in_target_queue, car);
1237
1238            // Put into the new state
1239            car.state = CarState::ChangingLanes {
1240                from: current_lane,
1241                to: target_lane,
1242                new_time,
1243                new_dist,
1244                lc_time,
1245            };
1246            ctx.scheduler
1247                .push(car.state.get_end_time(), Command::UpdateCar(car.vehicle.id));
1248        }
1249    }
1250
1251    pub fn collect_events(&mut self) -> Vec<Event> {
1252        std::mem::take(&mut self.events)
1253    }
1254
1255    pub fn handle_live_edits(&mut self, map: &Map) {
1256        // Calculate all queues that should exist now.
1257        let mut new_queues = HashSet::new();
1258        for l in map.all_lanes() {
1259            if l.lane_type.is_for_moving_vehicles() {
1260                new_queues.insert(Traversable::Lane(l.id));
1261            }
1262        }
1263        for t in map.all_turns() {
1264            if !t.between_sidewalks() {
1265                new_queues.insert(Traversable::Turn(t.id));
1266            }
1267        }
1268
1269        // Delete any old queues.
1270        self.queues.retain(|k, v| {
1271            if new_queues.remove(k) {
1272                // No changes
1273                true
1274            } else {
1275                // Make sure it's empty!
1276                if v.laggy_head.is_some() || !v.get_active_cars().is_empty() {
1277                    panic!(
1278                        "After live map edits, deleted queue {} still has vehicles! {:?}, {:?}",
1279                        k,
1280                        v.laggy_head,
1281                        v.get_active_cars()
1282                    );
1283                }
1284                false
1285            }
1286        });
1287
1288        // Create any new queues
1289        for key in new_queues {
1290            self.queues.insert(key, Queue::new(key, map));
1291        }
1292    }
1293
1294    fn new_crossing_state(&self, ctx: &mut Ctx, car: &Car) {
1295        if self.queues[&car.router.head()].is_car_at_front(car.vehicle.id) {
1296            if let Some(Traversable::Turn(turn)) = car.router.maybe_next() {
1297                ctx.intersections.approaching_leader(
1298                    AgentID::Car(car.vehicle.id),
1299                    turn,
1300                    car.state.get_end_time(),
1301                );
1302            }
1303        }
1304    }
1305}
1306
1307// Queries
1308impl DrivingSimState {
1309    /// Note the ordering of results is non-deterministic!
1310    pub fn get_unzoomed_agents(&self, now: Time, map: &Map) -> Vec<UnzoomedAgent> {
1311        let mut result = Vec::new();
1312
1313        for queue in self.queues.values() {
1314            if queue.get_active_cars().is_empty() {
1315                continue;
1316            }
1317
1318            for entry in queue.get_car_positions(now, &self.cars, &self.queues) {
1319                if let Queued::Vehicle(c) = entry.member {
1320                    let car = &self.cars[&c];
1321                    result.push(UnzoomedAgent {
1322                        id: AgentID::Car(car.vehicle.id),
1323                        pos: match queue.id.get_polyline(map).dist_along(entry.front) {
1324                            Ok((pt, _)) => pt,
1325                            Err(err) => panic!(
1326                                "At {}, invalid dist_along of {} for queue {}: {}",
1327                                now, entry.front, queue.id, err
1328                            ),
1329                        },
1330                        person: car.trip_and_person.map(|(_, p)| p),
1331                        parking: car.is_parking(),
1332                    });
1333                }
1334            }
1335        }
1336
1337        for (id, (pos, person)) in &self.waiting_to_spawn {
1338            result.push(UnzoomedAgent {
1339                id: AgentID::Car(*id),
1340                pos: pos.pt(map),
1341                person: *person,
1342                parking: false,
1343            });
1344        }
1345
1346        result
1347    }
1348
1349    pub fn does_car_exist(&self, id: CarID) -> bool {
1350        // Because of the shortcut IndexableKey takes with ignoring the VehicleType part of the ID,
1351        // we have to double-check that it matches!
1352        match self.cars.get(&id) {
1353            Some(car) => car.vehicle.id == id,
1354            None => false,
1355        }
1356    }
1357
1358    /// Note the ordering of results is non-deterministic!
1359    pub fn get_all_draw_cars(
1360        &self,
1361        now: Time,
1362        map: &Map,
1363        transit: &TransitSimState,
1364    ) -> Vec<DrawCarInput> {
1365        let mut result = Vec::new();
1366        for queue in self.queues.values() {
1367            result.extend(
1368                queue
1369                    .get_car_positions(now, &self.cars, &self.queues)
1370                    .into_iter()
1371                    .filter_map(|entry| {
1372                        if let Queued::Vehicle(id) = entry.member {
1373                            Some(self.cars[&id].get_draw_car(entry.front, now, map, transit))
1374                        } else {
1375                            None
1376                        }
1377                    }),
1378            );
1379        }
1380        result
1381    }
1382
1383    /// This is about as expensive as get_draw_cars_on.
1384    pub fn get_single_draw_car(
1385        &self,
1386        id: CarID,
1387        now: Time,
1388        map: &Map,
1389        transit: &TransitSimState,
1390    ) -> Option<DrawCarInput> {
1391        let car = self.cars.get(&id)?;
1392        self.get_draw_cars_on(now, car.router.head(), map, transit)
1393            .into_iter()
1394            .find(|d| d.id == id)
1395    }
1396
1397    pub fn get_draw_cars_on(
1398        &self,
1399        now: Time,
1400        on: Traversable,
1401        map: &Map,
1402        transit: &TransitSimState,
1403    ) -> Vec<DrawCarInput> {
1404        match self.queues.get(&on) {
1405            Some(q) => q
1406                .get_car_positions(now, &self.cars, &self.queues)
1407                .into_iter()
1408                .filter_map(|entry| match entry.member {
1409                    Queued::Vehicle(id) => {
1410                        Some(self.cars[&id].get_draw_car(entry.front, now, map, transit))
1411                    }
1412                    // Manually enable to debug exiting driveways and lane-changing
1413                    Queued::StaticBlockage { cause, front, back } => {
1414                        if false {
1415                            Some(DrawCarInput {
1416                                id: cause,
1417                                waiting_for_turn: None,
1418                                status: CarStatus::Parked,
1419                                intent: None,
1420                                on,
1421                                partly_on: Vec::new(),
1422                                label: Some("block".to_string()),
1423                                person: None,
1424                                body: on.get_polyline(map).exact_slice(back, front),
1425                            })
1426                        } else {
1427                            None
1428                        }
1429                    }
1430                    Queued::DynamicBlockage { cause, vehicle_len } => {
1431                        if false {
1432                            Some(DrawCarInput {
1433                                id: cause,
1434                                waiting_for_turn: None,
1435                                status: CarStatus::Parked,
1436                                intent: None,
1437                                on,
1438                                partly_on: Vec::new(),
1439                                label: Some("block".to_string()),
1440                                person: None,
1441                                body: on
1442                                    .get_polyline(map)
1443                                    .exact_slice(entry.front - vehicle_len, entry.front),
1444                            })
1445                        } else {
1446                            None
1447                        }
1448                    }
1449                })
1450                .collect(),
1451            None => Vec::new(),
1452        }
1453    }
1454
1455    pub fn debug_car_json(&self, id: CarID) -> String {
1456        if let Some(ref car) = self.cars.get(&id) {
1457            abstutil::to_json(car)
1458        } else {
1459            format!("{} is parked somewhere", id)
1460        }
1461    }
1462
1463    pub fn debug_car_ui(&self, id: CarID) -> String {
1464        if let Some(car) = self.cars.get(&id) {
1465            format!("{:?}", car.state)
1466        } else {
1467            format!("{} isn't in DrivingSimState", id)
1468        }
1469    }
1470
1471    pub fn debug_lane(&self, id: LaneID) {
1472        if let Some(ref queue) = self.queues.get(&Traversable::Lane(id)) {
1473            println!("{}", abstutil::to_json(queue));
1474        }
1475    }
1476
1477    pub fn agent_properties(&self, id: CarID, now: Time) -> AgentProperties {
1478        if let Some(car) = self.cars.get(&id) {
1479            let path = car.router.get_path();
1480            let time_spent_waiting = car.state.time_spent_waiting(now);
1481
1482            // In all cases, we can figure out exactly where we are along the current queue, then
1483            // assume we've travelled from the start of that, unless it's the very first step.
1484            let front = self.get_car_front(now, car);
1485            let current_state_dist =
1486                if car.router.head() == Traversable::Lane(path.get_req().start.lane()) {
1487                    front - path.get_req().start.dist_along()
1488                } else {
1489                    front
1490                };
1491
1492            AgentProperties {
1493                total_time: now - car.started_at,
1494                waiting_here: time_spent_waiting,
1495                total_waiting: car.total_blocked_time + time_spent_waiting,
1496                dist_crossed: path.crossed_so_far() + current_state_dist,
1497                total_dist: path.total_length(),
1498            }
1499        } else {
1500            for car in self.waiting_to_spawn.keys() {
1501                if id == *car {
1502                    // If the vehicle is waiting to spawn, we don't have any stats on them yet.  We
1503                    // could track when they originally tried to spawn and use for a few of these
1504                    // fields, but we should also make sure that delay gets recorded later.
1505                    return AgentProperties {
1506                        total_time: Duration::ZERO,
1507                        waiting_here: Duration::ZERO,
1508                        total_waiting: Duration::ZERO,
1509                        dist_crossed: Distance::ZERO,
1510                        total_dist: Distance::ZERO,
1511                    };
1512                }
1513            }
1514            panic!(
1515                "Can't get agent_properties of {} at {}; they don't exist",
1516                id, now
1517            );
1518        }
1519    }
1520
1521    pub fn get_path(&self, id: CarID) -> Option<&Path> {
1522        let car = self.cars.get(&id)?;
1523        Some(car.router.get_path())
1524    }
1525    pub fn get_all_driving_paths(&self) -> Vec<&Path> {
1526        self.cars
1527            .values()
1528            .map(|car| car.router.get_path())
1529            .collect()
1530    }
1531
1532    pub fn trace_route(&self, now: Time, id: CarID, map: &Map) -> Option<PolyLine> {
1533        let car = self.cars.get(&id)?;
1534        let front = self.get_car_front(now, car);
1535        car.router.get_path().trace_from_start(map, front)
1536    }
1537
1538    pub fn percent_along_route(&self, id: CarID) -> f64 {
1539        self.cars[&id].router.get_path().percent_dist_crossed()
1540    }
1541
1542    pub fn get_owner_of_car(&self, id: CarID) -> Option<PersonID> {
1543        let car = self.cars.get(&id)?;
1544        car.vehicle.owner
1545    }
1546
1547    pub fn target_lane_penalty(&self, l: LaneID) -> (usize, usize) {
1548        self.queues[&Traversable::Lane(l)].target_lane_penalty()
1549    }
1550
1551    pub fn find_trips_to_edited_parking(
1552        &self,
1553        spots: BTreeSet<ParkingSpot>,
1554    ) -> Vec<(AgentID, TripID)> {
1555        let mut affected = Vec::new();
1556        for car in self.cars.values() {
1557            if let Some(spot) = car.router.get_parking_spot_goal() {
1558                if !spots.contains(spot) {
1559                    // Buses don't park
1560                    affected.push((AgentID::Car(car.vehicle.id), car.trip_and_person.unwrap().0));
1561                }
1562            }
1563        }
1564        affected
1565    }
1566
1567    /// Finds vehicles that're laggy heads on affected parts of the map.
1568    pub fn find_vehicles_affected_by_live_edits(
1569        &self,
1570        closed_intersections: &HashSet<IntersectionID>,
1571        edited_lanes: &BTreeSet<LaneID>,
1572    ) -> Vec<(AgentID, TripID)> {
1573        let mut affected = Vec::new();
1574        for car in self.cars.values() {
1575            if car.last_steps.iter().any(|step| match step {
1576                Traversable::Lane(l) => edited_lanes.contains(l),
1577                Traversable::Turn(t) => {
1578                    closed_intersections.contains(&t.parent)
1579                        || edited_lanes.contains(&t.src)
1580                        || edited_lanes.contains(&t.dst)
1581                }
1582            }) {
1583                // TODO Buses aren't handled yet! Mostly not a big deal, because they're pretty
1584                // much never created anyway.
1585                if let Some((trip, _)) = car.trip_and_person {
1586                    affected.push((AgentID::Car(car.vehicle.id), trip));
1587                }
1588            }
1589        }
1590        affected
1591    }
1592
1593    pub fn all_waiting_people(&self, now: Time, delays: &mut BTreeMap<PersonID, Duration>) {
1594        for c in self.cars.values() {
1595            if let Some((_, person)) = c.trip_and_person {
1596                let delay = c.state.time_spent_waiting(now);
1597                if delay > Duration::ZERO {
1598                    delays.insert(person, delay);
1599                }
1600            }
1601        }
1602    }
1603
1604    pub fn debug_queue_lengths(&self, l: LaneID) -> Option<(Distance, Distance)> {
1605        let queue = self.queues.get(&Traversable::Lane(l))?;
1606        Some((queue.reserved_length, queue.geom_len))
1607    }
1608
1609    pub fn get_blocked_by_graph(
1610        &self,
1611        now: Time,
1612        map: &Map,
1613        intersections: &IntersectionSimState,
1614    ) -> BTreeMap<AgentID, (Duration, DelayCause)> {
1615        let mut graph = BTreeMap::new();
1616
1617        // Just look for every case where somebody is behind someone else, whether or not they're
1618        // blocked by them and waiting.
1619        for queue in self.queues.values() {
1620            if let Some(head) = queue.laggy_head {
1621                if let Some(next) = queue.get_active_cars().get(0) {
1622                    graph.insert(
1623                        AgentID::Car(*next),
1624                        (
1625                            self.cars[&head].state.time_spent_waiting(now),
1626                            DelayCause::Agent(AgentID::Car(head)),
1627                        ),
1628                    );
1629                }
1630            }
1631            // This doesn't need to account for blockages. Somebody unparking won't start doing it
1632            // until they're guaranteed to be able to finish it.
1633            let cars = queue.get_active_cars();
1634            for (head, tail) in cars.iter().zip(cars.iter().skip(1)) {
1635                graph.insert(
1636                    AgentID::Car(*tail),
1637                    (
1638                        self.cars[tail].state.time_spent_waiting(now),
1639                        DelayCause::Agent(AgentID::Car(*head)),
1640                    ),
1641                );
1642            }
1643        }
1644
1645        intersections.populate_blocked_by(now, &mut graph, map, &self.cars, &self.queues);
1646        graph
1647    }
1648
1649    fn get_car_front(&self, now: Time, car: &Car) -> Distance {
1650        self.queues[&car.router.head()]
1651            .get_car_positions(now, &self.cars, &self.queues)
1652            .into_iter()
1653            .find(|entry| entry.member == Queued::Vehicle(car.vehicle.id))
1654            .unwrap()
1655            .front
1656    }
1657
1658    /// Does the given car want to over-take the vehicle in front of it?
1659    fn wants_to_overtake(&self, car: &Car) -> Option<CarID> {
1660        let queue = &self.queues[&car.router.head()];
1661        let leader = &self.cars[&queue.get_leader(car.vehicle.id)?];
1662
1663        // Are we faster than them?
1664        // TODO This shouldn't be a blocking check; we also want to pass parking cars and buses
1665        // waiting at stops.
1666        let their_speed = leader.vehicle.max_speed?;
1667        if car
1668            .vehicle
1669            .max_speed
1670            .map(|s| s <= their_speed)
1671            .unwrap_or(false)
1672        {
1673            return None;
1674        }
1675
1676        // Are they moving slowly or also stuck behind someone?
1677        match leader.state {
1678            // TODO Maybe we want to pass someone queued, if they're too slow to pass their own
1679            // leader?
1680            CarState::WaitingToAdvance { .. } | CarState::Queued { .. } => {
1681                return None;
1682            }
1683            // In all other cases, we may want to pass them.
1684            _ => {}
1685        }
1686
1687        // TODO Check if there's room in front of them to pass. Handle passing two bikes?
1688        // TODO Check remaining distance before next intersection
1689        // TODO Check relative speed difference, and time to pass them
1690        // TODO Eventually, check if there's room to do the maneuever (lane-changing to
1691        // the left only, or popping into an oncoming lane!
1692
1693        Some(leader.vehicle.id)
1694    }
1695}
1696
1697// This implementation relies on the fact that car IDs are unique just by their number. Vehicle
1698// type is also in there, but during lookup, it'll be ignored!
1699impl IndexableKey for CarID {
1700    fn index(&self) -> usize {
1701        self.id
1702    }
1703}