use std::collections::{BTreeSet, HashMap, VecDeque};
use serde::{Deserialize, Serialize};
use abstutil::FixedMap;
use geom::{Distance, Time};
use map_model::{Map, Position, Traversable};
use crate::mechanics::car::{Car, CarState};
use crate::{CarID, VehicleType, FOLLOWING_DISTANCE};
#[derive(Serialize, Deserialize, Clone, Debug)]
pub(crate) struct Queue {
pub id: Traversable,
members: VecDeque<Queued>,
pub laggy_head: Option<CarID>,
pub geom_len: Distance,
pub reserved_length: Distance,
}
#[derive(Serialize, Deserialize, Clone, Debug, PartialEq)]
pub enum Queued {
Vehicle(CarID),
StaticBlockage {
cause: CarID,
front: Distance,
back: Distance,
},
DynamicBlockage {
cause: CarID,
vehicle_len: Distance,
},
}
#[derive(Clone, Debug)]
pub struct QueueEntry {
pub member: Queued,
pub front: Distance,
pub back: Distance,
}
impl Queue {
pub fn new(id: Traversable, map: &Map) -> Queue {
Queue {
id,
members: VecDeque::new(),
laggy_head: None,
geom_len: id.get_polyline(map).length(),
reserved_length: Distance::ZERO,
}
}
pub fn get_last_car_position(
&self,
now: Time,
cars: &FixedMap<CarID, Car>,
queues: &HashMap<Traversable, Queue>,
) -> Option<(CarID, Distance)> {
self.inner_get_last_car_position(now, cars, queues, &mut BTreeSet::new(), None)
}
pub fn get_car_positions(
&self,
now: Time,
cars: &FixedMap<CarID, Car>,
queues: &HashMap<Traversable, Queue>,
) -> Vec<QueueEntry> {
let mut all_cars = vec![];
self.inner_get_last_car_position(
now,
cars,
queues,
&mut BTreeSet::new(),
Some(&mut all_cars),
);
all_cars
}
fn inner_get_last_car_position(
&self,
now: Time,
cars: &FixedMap<CarID, Car>,
queues: &HashMap<Traversable, Queue>,
recursed_queues: &mut BTreeSet<Traversable>,
mut intermediate_results: Option<&mut Vec<QueueEntry>>,
) -> Option<(CarID, Distance)> {
if self.members.is_empty() {
return None;
}
let mut previous: Option<QueueEntry> = None;
for queued in self.members.iter().cloned() {
let bound = match previous {
Some(entry) => entry.back - FOLLOWING_DISTANCE,
None => match self.laggy_head {
Some(id) => {
let leader = &cars[&id];
let recurse_to = leader.router.head();
if recursed_queues.contains(&recurse_to) {
self.geom_len
} else {
recursed_queues.insert(recurse_to);
let (head, head_dist) = queues[&leader.router.head()]
.inner_get_last_car_position(
now,
cars,
queues,
recursed_queues,
None,
)
.unwrap();
assert_eq!(head, id);
let mut dist_away_from_this_queue = head_dist;
for on in &leader.last_steps {
if *on == self.id {
break;
}
dist_away_from_this_queue += queues[on].geom_len;
}
if dist_away_from_this_queue
< leader.vehicle.length + FOLLOWING_DISTANCE
{
self.geom_len
- (cars[&id].vehicle.length - dist_away_from_this_queue)
- FOLLOWING_DISTANCE
} else {
self.geom_len
}
}
}
None => self.geom_len,
},
};
if bound < Distance::ZERO {
if let Some(intermediate_results) = intermediate_results {
dump_cars(intermediate_results, cars, self.id, now);
}
panic!(
"Queue has spillover on {} at {} -- can't draw {:?}, bound is {}. Laggy head is \
{:?}. This is usually a geometry bug; check for duplicate roads going \
between the same intersections.",
self.id, now, queued, bound, self.laggy_head
);
}
let entry = match queued {
Queued::Vehicle(id) => {
let car = &cars[&id];
let front = match car.state {
CarState::Queued { .. } => {
if car.router.last_step() {
car.router.get_end_dist().min(bound)
} else {
bound
}
}
CarState::WaitingToAdvance { .. } => {
if bound != self.geom_len {
if let Some(intermediate_results) = intermediate_results {
dump_cars(intermediate_results, cars, self.id, now);
}
panic!("{} is waiting to advance on {}, but the current bound is {}, not geom_len {}. How can anything be in front of them?", id, self.id, bound, self.geom_len);
}
self.geom_len
}
CarState::Crossing(ref time_int, ref dist_int) => {
dist_int.lerp(time_int.percent_clamp_end(now)).min(bound)
}
CarState::ChangingLanes {
ref new_time,
ref new_dist,
..
} => {
new_dist.lerp(new_time.percent_clamp_end(now)).min(bound)
}
CarState::Unparking { front, .. } => front,
CarState::Parking(front, _, _) => front,
CarState::IdlingAtStop(front, _) => front,
};
QueueEntry {
member: queued,
front,
back: front - car.vehicle.length,
}
}
Queued::StaticBlockage { front, back, .. } => QueueEntry {
member: queued,
front,
back,
},
Queued::DynamicBlockage { vehicle_len, .. } => QueueEntry {
member: queued,
front: bound,
back: bound - vehicle_len,
},
};
if let Some(ref mut intermediate_results) = intermediate_results {
intermediate_results.push(entry.clone());
}
previous = Some(entry);
}
if false {
if let Some(intermediate_results) = intermediate_results {
validate_positions(intermediate_results, cars, now, self.id)
}
}
let previous = previous?;
match previous.member {
Queued::Vehicle(car) => Some((car, previous.front)),
Queued::StaticBlockage { .. } => None,
Queued::DynamicBlockage { .. } => None,
}
}
pub fn get_idx_to_insert_car(
&self,
start_dist: Distance,
vehicle_len: Distance,
now: Time,
cars: &FixedMap<CarID, Car>,
queues: &HashMap<Traversable, Queue>,
) -> Option<usize> {
if self.laggy_head.is_none() && self.members.is_empty() {
return Some(0);
}
let dists = self.get_car_positions(now, cars, queues);
let idx = match dists.iter().position(|entry| start_dist >= entry.front) {
Some(i) => i,
None => dists.len(),
};
if idx == 0 {
if let Some(c) = self.laggy_head {
if self.geom_len - cars[&c].vehicle.length - FOLLOWING_DISTANCE < start_dist {
return None;
}
}
}
if idx != 0 && dists[idx - 1].back - FOLLOWING_DISTANCE < start_dist {
return None;
}
if idx != dists.len() && start_dist - vehicle_len - FOLLOWING_DISTANCE < dists[idx].front {
return None;
}
Some(idx)
}
pub fn insert_car_at_idx(&mut self, idx: usize, car: &Car) {
self.members.insert(idx, Queued::Vehicle(car.vehicle.id));
self.reserved_length += car.vehicle.length + FOLLOWING_DISTANCE;
}
pub fn push_car_onto_end(&mut self, car: CarID) {
self.members.push_back(Queued::Vehicle(car));
}
pub fn move_first_car_to_laggy_head(&mut self) -> CarID {
assert!(self.laggy_head.is_none());
let car = match self.members.pop_front() {
Some(Queued::Vehicle(c)) => c,
x => {
panic!(
"First member of {} is {:?}, not an active vehicle",
self.id, x
);
}
};
self.laggy_head = Some(car);
car
}
pub fn try_to_reserve_entry(&mut self, car: &Car, force_entry: bool) -> bool {
if self.room_for_car(car) || force_entry {
self.reserved_length += car.vehicle.length + FOLLOWING_DISTANCE;
return true;
}
false
}
pub fn is_overflowing(&self) -> bool {
self.reserved_length >= self.geom_len
}
pub fn room_for_car(&self, car: &Car) -> bool {
self.reserved_length == Distance::ZERO
|| self.reserved_length + car.vehicle.length + FOLLOWING_DISTANCE < self.geom_len
}
pub fn free_reserved_space(&mut self, car: &Car) {
self.reserved_length -= car.vehicle.length + FOLLOWING_DISTANCE;
assert!(
self.reserved_length >= Distance::ZERO,
"invalid reserved length: {:?}, car: {:?}",
self.reserved_length,
car
);
}
pub fn target_lane_penalty(&self) -> (usize, usize) {
let mut num_vehicles = self.members.len();
if self.laggy_head.is_some() {
num_vehicles += 1;
}
let bike_cost = if self
.members
.iter()
.any(|x| matches!(x, Queued::Vehicle(c) if c.vehicle_type == VehicleType::Bike))
|| self
.laggy_head
.map(|c| c.vehicle_type == VehicleType::Bike)
.unwrap_or(false)
{
1
} else {
0
};
(num_vehicles, bike_cost)
}
pub fn get_leader(&self, id: CarID) -> Option<CarID> {
let mut leader = None;
for queued in &self.members {
match queued {
Queued::Vehicle(car) => {
if *car == id {
return leader;
}
leader = Some(*car);
}
Queued::StaticBlockage { .. } | Queued::DynamicBlockage { .. } => {
leader = None;
}
}
}
None
}
pub fn add_static_blockage(
&mut self,
cause: CarID,
front: Distance,
back: Distance,
idx: usize,
) {
assert!(front > back);
assert!(back >= FOLLOWING_DISTANCE);
let vehicle_len = front - back;
self.members
.insert(idx, Queued::StaticBlockage { cause, front, back });
self.reserved_length += vehicle_len + FOLLOWING_DISTANCE;
}
pub fn clear_static_blockage(&mut self, caused_by: CarID, idx: usize) {
let blockage = self.members.remove(idx).unwrap();
match blockage {
Queued::StaticBlockage { front, back, cause } => {
assert_eq!(caused_by, cause);
let vehicle_len = front - back;
self.reserved_length -= vehicle_len + FOLLOWING_DISTANCE;
}
_ => unreachable!(),
}
}
pub fn replace_car_with_dynamic_blockage(&mut self, car: &Car, idx: usize) {
self.remove_car_from_idx(car.vehicle.id, idx);
self.members.insert(
idx,
Queued::DynamicBlockage {
cause: car.vehicle.id,
vehicle_len: car.vehicle.length,
},
);
}
pub fn clear_dynamic_blockage(&mut self, caused_by: CarID, idx: usize) {
let blockage = self.members.remove(idx).unwrap();
match blockage {
Queued::DynamicBlockage { cause, vehicle_len } => {
assert_eq!(caused_by, cause);
self.reserved_length -= vehicle_len + FOLLOWING_DISTANCE;
}
_ => unreachable!(),
}
}
pub fn can_block_from_driveway(
&self,
pos: &Position,
vehicle_len: Distance,
now: Time,
cars: &FixedMap<CarID, Car>,
queues: &HashMap<Traversable, Queue>,
) -> Option<usize> {
self.get_idx_to_insert_car(pos.dist_along(), vehicle_len, now, cars, queues)
}
pub fn get_active_cars(&self) -> Vec<CarID> {
self.members
.iter()
.filter_map(|x| match x {
Queued::Vehicle(c) => Some(*c),
Queued::StaticBlockage { .. } => None,
Queued::DynamicBlockage { .. } => None,
})
.collect()
}
pub fn remove_car_from_idx(&mut self, car: CarID, idx: usize) {
assert_eq!(self.members.remove(idx), Some(Queued::Vehicle(car)));
}
pub fn is_car_at_front(&self, car: CarID) -> bool {
self.laggy_head.is_none() && self.members.get(0) == Some(&Queued::Vehicle(car))
}
}
fn validate_positions(
dists: &[QueueEntry],
cars: &FixedMap<CarID, Car>,
now: Time,
id: Traversable,
) {
for pair in dists.windows(2) {
if pair[0].back - FOLLOWING_DISTANCE < pair[1].front {
dump_cars(dists, cars, id, now);
panic!(
"get_car_positions wound up with bad positioning: {} then {}\n{:?}",
pair[0].front, pair[1].front, dists
);
}
}
}
fn dump_cars(dists: &[QueueEntry], cars: &FixedMap<CarID, Car>, id: Traversable, now: Time) {
println!("\nOn {} at {}...", id, now);
for entry in dists {
println!("- {:?} @ {}..{}", entry.member, entry.front, entry.back);
match entry.member {
Queued::Vehicle(id) => match cars[&id].state {
CarState::Crossing(ref time_int, ref dist_int) => {
println!(
" Going {} .. {} during {} .. {}",
dist_int.start, dist_int.end, time_int.start, time_int.end
);
}
CarState::ChangingLanes {
ref new_time,
ref new_dist,
..
} => {
println!(
" Going {} .. {} during {} .. {}, also in the middle of lane-changing",
new_dist.start, new_dist.end, new_time.start, new_time.end
);
}
CarState::Queued { .. } => {
println!(" Queued currently");
}
CarState::WaitingToAdvance { .. } => {
println!(" WaitingToAdvance currently");
}
CarState::Unparking { ref time_int, .. } => {
println!(" Unparking during {} .. {}", time_int.start, time_int.end);
}
CarState::Parking(_, _, ref time_int) => {
println!(" Parking during {} .. {}", time_int.start, time_int.end);
}
CarState::IdlingAtStop(_, ref time_int) => {
println!(" Idling during {} .. {}", time_int.start, time_int.end);
}
},
Queued::StaticBlockage { cause, .. } => {
println!(" Static blockage by {}", cause);
}
Queued::DynamicBlockage { cause, vehicle_len } => {
println!(" Dynamic blockage of length {} by {}", vehicle_len, cause);
}
}
}
println!();
}