1 use crate::mechanics::Queue;
2 use crate::{
3     CarID, Event, ParkingSimState, ParkingSpot, PersonID, SidewalkSpot, TripID, TripPhaseType,
4     Vehicle, VehicleType,
5 };
6 use geom::Distance;
7 use map_model::{
8     BuildingID, IntersectionID, LaneID, Map, Path, PathConstraints, PathRequest, PathStep,
9     Position, Traversable, TurnID,
10 };
11 use serde::{Deserialize, Serialize};
12 use std::collections::BTreeMap;
13 
14 #[derive(Serialize, Deserialize, Clone, Debug, PartialEq)]
15 pub struct Router {
16     // Front is always the current step
17     path: Path,
18     goal: Goal,
19     owner: CarID,
20 }
21 
22 #[derive(Debug)]
23 pub enum ActionAtEnd {
24     VanishAtBorder(IntersectionID),
25     StartParking(ParkingSpot),
26     GotoLaneEnd,
27     StopBiking(SidewalkSpot),
28     BusAtStop,
29     GiveUpOnParking,
30 }
31 
32 #[derive(Serialize, Deserialize, Clone, Debug, PartialEq)]
33 enum Goal {
34     // Spot and cached distance along the last driving lane
35     // TODO Right now, the building is ignored when choosing the best spot.
36     ParkNearBuilding {
37         target: BuildingID,
38         spot: Option<(ParkingSpot, Distance)>,
39         // No parking available at all!
40         stuck_end_dist: Option<Distance>,
41         started_looking: bool,
42     },
43     EndAtBorder {
44         end_dist: Distance,
45         i: IntersectionID,
46     },
47     BikeThenStop {
48         goal: SidewalkSpot,
49     },
50     FollowBusRoute {
51         end_dist: Distance,
52     },
53 }
54 
55 impl Router {
end_at_border( owner: CarID, path: Path, end_dist: Distance, i: IntersectionID, ) -> Router56     pub fn end_at_border(
57         owner: CarID,
58         path: Path,
59         end_dist: Distance,
60         i: IntersectionID,
61     ) -> Router {
62         Router {
63             path,
64             goal: Goal::EndAtBorder { end_dist, i },
65             owner,
66         }
67     }
vanish_bus(owner: CarID, l: LaneID, map: &Map) -> Router68     pub fn vanish_bus(owner: CarID, l: LaneID, map: &Map) -> Router {
69         let lane = map.get_l(l);
70         Router {
71             path: Path::one_step(l, map),
72             goal: Goal::EndAtBorder {
73                 end_dist: lane.length(),
74                 i: lane.dst_i,
75             },
76             owner,
77         }
78     }
79 
park_near(owner: CarID, path: Path, bldg: BuildingID) -> Router80     pub fn park_near(owner: CarID, path: Path, bldg: BuildingID) -> Router {
81         Router {
82             path,
83             goal: Goal::ParkNearBuilding {
84                 target: bldg,
85                 spot: None,
86                 stuck_end_dist: None,
87                 started_looking: false,
88             },
89             owner,
90         }
91     }
92 
bike_then_stop(owner: CarID, path: Path, goal: SidewalkSpot) -> Router93     pub fn bike_then_stop(owner: CarID, path: Path, goal: SidewalkSpot) -> Router {
94         Router {
95             goal: Goal::BikeThenStop { goal },
96             path,
97             owner,
98         }
99     }
100 
follow_bus_route(owner: CarID, path: Path, end_dist: Distance) -> Router101     pub fn follow_bus_route(owner: CarID, path: Path, end_dist: Distance) -> Router {
102         Router {
103             path,
104             goal: Goal::FollowBusRoute { end_dist },
105             owner,
106         }
107     }
108 
head(&self) -> Traversable109     pub fn head(&self) -> Traversable {
110         self.path.current_step().as_traversable()
111     }
112 
next(&self) -> Traversable113     pub fn next(&self) -> Traversable {
114         self.path.next_step().as_traversable()
115     }
116 
maybe_next(&self) -> Option<Traversable>117     pub fn maybe_next(&self) -> Option<Traversable> {
118         if self.last_step() {
119             None
120         } else {
121             Some(self.path.next_step().as_traversable())
122         }
123     }
124 
last_step(&self) -> bool125     pub fn last_step(&self) -> bool {
126         self.path.is_last_step()
127     }
128 
get_end_dist(&self) -> Distance129     pub fn get_end_dist(&self) -> Distance {
130         // Shouldn't ask earlier!
131         assert!(self.last_step());
132         match self.goal {
133             Goal::EndAtBorder { end_dist, .. } => end_dist,
134             Goal::ParkNearBuilding {
135                 spot,
136                 stuck_end_dist,
137                 ..
138             } => stuck_end_dist.unwrap_or_else(|| spot.unwrap().1),
139             Goal::BikeThenStop { ref goal } => goal.sidewalk_pos.dist_along(),
140             Goal::FollowBusRoute { end_dist } => end_dist,
141         }
142     }
143 
get_path(&self) -> &Path144     pub fn get_path(&self) -> &Path {
145         &self.path
146     }
147 
148     // Returns the step just finished
advance( &mut self, vehicle: &Vehicle, parking: &ParkingSimState, map: &Map, trip_and_person: Option<(TripID, PersonID)>, events: &mut Vec<Event>, ) -> Traversable149     pub fn advance(
150         &mut self,
151         vehicle: &Vehicle,
152         parking: &ParkingSimState,
153         map: &Map,
154         trip_and_person: Option<(TripID, PersonID)>,
155         events: &mut Vec<Event>,
156     ) -> Traversable {
157         let prev = self.path.shift(map).as_traversable();
158         if self.last_step() {
159             // Do this to trigger the side-effect of looking for parking.
160             self.maybe_handle_end(
161                 Distance::ZERO,
162                 vehicle,
163                 parking,
164                 map,
165                 trip_and_person,
166                 events,
167             );
168         }
169 
170         // Sanity check laws haven't been broken
171         if let Traversable::Lane(l) = self.head() {
172             let lane = map.get_l(l);
173             if !vehicle.vehicle_type.to_constraints().can_use(lane, map) {
174                 panic!(
175                     "{} just wound up on {}, a {:?} (check the OSM tags)",
176                     vehicle.id, l, lane.lane_type
177                 );
178             }
179         }
180 
181         prev
182     }
183 
184     // Called when the car is Queued at the last step, or when they initially advance to the last
185     // step.
maybe_handle_end( &mut self, front: Distance, vehicle: &Vehicle, parking: &ParkingSimState, map: &Map, trip_and_person: Option<(TripID, PersonID)>, events: &mut Vec<Event>, ) -> Option<ActionAtEnd>186     pub fn maybe_handle_end(
187         &mut self,
188         front: Distance,
189         vehicle: &Vehicle,
190         parking: &ParkingSimState,
191         map: &Map,
192         // TODO Not so nice to plumb all of this here
193         trip_and_person: Option<(TripID, PersonID)>,
194         events: &mut Vec<Event>,
195     ) -> Option<ActionAtEnd> {
196         match self.goal {
197             Goal::EndAtBorder { end_dist, i } => {
198                 if end_dist == front {
199                     Some(ActionAtEnd::VanishAtBorder(i))
200                 } else {
201                     None
202                 }
203             }
204             Goal::ParkNearBuilding {
205                 ref mut spot,
206                 ref mut stuck_end_dist,
207                 target,
208                 ref mut started_looking,
209             } => {
210                 if let Some(d) = stuck_end_dist {
211                     if *d == front {
212                         return Some(ActionAtEnd::GiveUpOnParking);
213                     } else {
214                         return None;
215                     }
216                 }
217 
218                 let need_new_spot = match spot {
219                     Some((s, _)) => !parking.is_free(*s),
220                     None => true,
221                 };
222                 if need_new_spot {
223                     *started_looking = true;
224                     let current_lane = self.path.current_step().as_lane();
225                     let candidates = parking.get_all_free_spots(
226                         Position::new(current_lane, front),
227                         vehicle,
228                         target,
229                         map,
230                     );
231                     let best =
232                         if let Some((driving_pos, _)) = map.get_b(target).driving_connection(map) {
233                             if driving_pos.lane() == current_lane {
234                                 let target_dist = driving_pos.dist_along();
235                                 // Closest to the building
236                                 candidates
237                                     .into_iter()
238                                     .min_by_key(|(_, pos)| (pos.dist_along() - target_dist).abs())
239                             } else {
240                                 // Closest to the road endpoint, I guess
241                                 candidates
242                                     .into_iter()
243                                     .min_by_key(|(_, pos)| pos.dist_along())
244                             }
245                         } else {
246                             // Closest to the road endpoint, I guess
247                             candidates
248                                 .into_iter()
249                                 .min_by_key(|(_, pos)| pos.dist_along())
250                         };
251                     if let Some((new_spot, new_pos)) = best {
252                         if let Some((t, p)) = trip_and_person {
253                             events.push(Event::TripPhaseStarting(
254                                 t,
255                                 p,
256                                 Some(PathRequest {
257                                     start: Position::new(current_lane, front),
258                                     end: new_pos,
259                                     constraints: PathConstraints::Car,
260                                 }),
261                                 TripPhaseType::Parking,
262                             ));
263                         }
264                         assert_eq!(new_pos.lane(), current_lane);
265                         assert!(new_pos.dist_along() >= front);
266                         *spot = Some((new_spot, new_pos.dist_along()));
267                     } else {
268                         if let Some((new_path_steps, new_spot, new_pos)) =
269                             parking.path_to_free_parking_spot(current_lane, vehicle, target, map)
270                         {
271                             assert!(!new_path_steps.is_empty());
272                             for step in new_path_steps {
273                                 self.path.add(step, map);
274                             }
275                             *spot = Some((new_spot, new_pos.dist_along()));
276                             events.push(Event::PathAmended(self.path.clone()));
277                             // TODO This path might not be the same as the one found here...
278                             if let Some((t, p)) = trip_and_person {
279                                 events.push(Event::TripPhaseStarting(
280                                     t,
281                                     p,
282                                     Some(PathRequest {
283                                         start: Position::new(current_lane, front),
284                                         end: new_pos,
285                                         constraints: PathConstraints::Car,
286                                     }),
287                                     TripPhaseType::Parking,
288                                 ));
289                             }
290                         } else {
291                             println!(
292                                 "WARNING: {} can't find parking on {} or anywhere reachable from \
293                                  it. Possibly we're just totally out of parking space!",
294                                 vehicle.id, current_lane
295                             );
296                             *stuck_end_dist = Some(map.get_l(current_lane).length());
297                         }
298                         return Some(ActionAtEnd::GotoLaneEnd);
299                     }
300                 }
301 
302                 if spot.unwrap().1 == front {
303                     Some(ActionAtEnd::StartParking(spot.unwrap().0))
304                 } else {
305                     None
306                 }
307             }
308             Goal::BikeThenStop { ref goal } => {
309                 if goal.sidewalk_pos.dist_along() == front {
310                     Some(ActionAtEnd::StopBiking(goal.clone()))
311                 } else {
312                     None
313                 }
314             }
315             Goal::FollowBusRoute { end_dist } => {
316                 if end_dist == front {
317                     Some(ActionAtEnd::BusAtStop)
318                 } else {
319                     None
320                 }
321             }
322         }
323     }
324 
opportunistically_lanechange( &mut self, queues: &BTreeMap<Traversable, Queue>, map: &Map, handle_uber_turns: bool, )325     pub fn opportunistically_lanechange(
326         &mut self,
327         queues: &BTreeMap<Traversable, Queue>,
328         map: &Map,
329         handle_uber_turns: bool,
330     ) {
331         if handle_uber_turns
332             && (self.path.approaching_uber_turn() || self.path.currently_inside_ut().is_some())
333         {
334             return;
335         }
336         let (current_turn, next_lane) = {
337             let steps = self.path.get_steps();
338             if steps.len() < 5 {
339                 return;
340             }
341             match (steps[1], steps[4]) {
342                 (PathStep::Turn(t), PathStep::Lane(l)) => (t, l),
343                 _ => {
344                     return;
345                 }
346             }
347         };
348 
349         let orig_target_lane = current_turn.dst;
350         let parent = map.get_parent(orig_target_lane);
351         let next_parent = map.get_l(next_lane).src_i;
352 
353         // Look for other candidates, and assign a cost to each.
354         let constraints = self.owner.1.to_constraints();
355         let dir = parent.dir(orig_target_lane);
356         let (_, turn1, best_lane, turn2) = parent
357             .lanes_ltr()
358             .into_iter()
359             .filter(|(l, d, _)| dir == *d && constraints.can_use(map.get_l(*l), map))
360             .filter_map(|(l, _, _)| {
361                 let t1 = TurnID {
362                     parent: current_turn.parent,
363                     src: current_turn.src,
364                     dst: l,
365                 };
366                 if let Some(turn1) = map.maybe_get_t(t1) {
367                     // Make sure we can go from this lane to next_lane.
368                     if let Some(turn2) = map.maybe_get_t(TurnID {
369                         parent: next_parent,
370                         src: l,
371                         dst: next_lane,
372                     }) {
373                         Some((turn1, l, turn2))
374                     } else {
375                         None
376                     }
377                 } else {
378                     None
379                 }
380             })
381             .map(|(turn1, l, turn2)| {
382                 let (lt, lc, mut rightmost) = turn1.penalty(map);
383                 let (vehicles, mut bike) = queues[&Traversable::Lane(l)].target_lane_penalty();
384 
385                 // The magic happens here. We have different penalties:
386                 //
387                 // 1) Are we headed towards a general purpose lane instead of a dedicated bike/bus
388                 //    lane?
389                 // 2) Are there any bikes in the target lane? This ONLY matters if we're a car. If
390                 //    we're another bike, the speed difference won't matter.
391                 // 3) IF we're a bike, are we headed to something other than the rightmost lane?
392                 // 4) Are there lots of vehicles stacked up in one lane?
393                 // 5) Are we changing lanes?
394                 //
395                 // A linear combination of these penalties is hard to reason about. Instead, we
396                 // make our choice based on each penalty in order, breaking ties by moving onto the
397                 // next thing.
398                 if self.owner.1 == VehicleType::Bike {
399                     bike = 0;
400                 } else {
401                     rightmost = 0;
402                 }
403                 let cost = (lt, bike, rightmost, vehicles, lc);
404 
405                 (cost, turn1, l, turn2)
406             })
407             .min_by_key(|(cost, _, _, _)| *cost)
408             .unwrap();
409         // TODO Only switch if the target queue is some amount better; don't oscillate
410         // unnecessarily.
411         if best_lane == orig_target_lane {
412             return;
413         }
414 
415         self.path.modify_step(1, PathStep::Turn(turn1.id), map);
416         self.path.modify_step(2, PathStep::Lane(best_lane), map);
417         self.path.modify_step(3, PathStep::Turn(turn2.id), map);
418     }
419 
replace_path_for_serialization(&mut self, path: Path) -> Path420     pub fn replace_path_for_serialization(&mut self, path: Path) -> Path {
421         std::mem::replace(&mut self.path, path)
422     }
423 
is_parking(&self) -> bool424     pub fn is_parking(&self) -> bool {
425         match self.goal {
426             Goal::ParkNearBuilding {
427                 started_looking, ..
428             } => started_looking,
429             _ => false,
430         }
431     }
432 }
433