WIP start a specific parked car, not the last one in the lane. broken,

because multiple cars jump to the beginning of the driving lane. need to
make them start in the right position (and then go there slowly)

editor/src/ui.rs

@@ -512,6 +512,10 @@ impl gui::GUI for UI {
                     self.sim_ctrl.sim.toggle_debug(id);
                     return gui::EventLoopMode::InputOnly;
                 }
+                if input.key_pressed(Key::A, "start this parked car") {
+                    self.sim_ctrl.sim.start_parked_car(&self.map, id);
+                    return gui::EventLoopMode::InputOnly;
+                }
             }
             SelectionState::SelectedLane(id, _) => {
                 if input.key_pressed(Key::F, "start floodfilling from this lane") {
@@ -519,9 +523,11 @@ impl gui::GUI for UI {
                     return gui::EventLoopMode::InputOnly;
                 }
 
-                if input.key_pressed(Key::A, "start something on this lane") {
-                    self.sim_ctrl.sim.start_agent(&self.map, id);
-                    return gui::EventLoopMode::InputOnly;
+                if self.map.get_l(id).lane_type == map_model::LaneType::Sidewalk {
+                    if input.key_pressed(Key::A, "spawn a pedestrian here") {
+                        self.sim_ctrl.sim.spawn_pedestrian(&self.map, id);
+                        return gui::EventLoopMode::InputOnly;
+                    }
                 }
             }
             SelectionState::SelectedIntersection(id) => {

map_model/src/map.rs

@@ -308,9 +308,9 @@ impl Map {
             .collect()
     }
 
-    pub fn get_lane_and_parent(&self, id: LaneID) -> (&Lane, &Road) {
+    pub fn get_parent(&self, id: LaneID) -> &Road {
         let l = self.get_l(id);
-        (l, self.get_r(l.parent))
+        self.get_r(l.parent)
     }
 
     // TODO can we return a borrow?

sim/src/driving.rs

@@ -5,7 +5,7 @@ use draw_car::DrawCar;
 use intersections::{AgentInfo, IntersectionSimState, Request};
 use kinematics;
 use kinematics::Vehicle;
-use map_model::{LaneID, LaneType, Map, TurnID};
+use map_model::{LaneID, Map, TurnID};
 use models::{choose_turn, FOLLOWING_DISTANCE};
 use multimap::MultiMap;
 use ordered_float::NotNaN;
@@ -296,6 +296,14 @@ impl SimQueue {
             .find(|id| sim.cars[id].dist_along > dist)
             .map(|id| *id)
     }
+
+    fn insert_at(&mut self, car: CarID, dist_along: Distance, sim: &DrivingSimState) {
+        if let Some(idx) = self.cars_queue.iter().position(|id| sim.cars[id].dist_along < dist_along) {
+            self.cars_queue.insert(idx, car);
+        } else {
+            self.cars_queue.push(car);
+        }
+    }
 }
 
 // This manages only actively driving cars
@@ -488,45 +496,37 @@ impl DrivingSimState {
         Ok(())
     }
 
-    // TODO cars basically start in the intersection, with their front bumper right at the
-    // beginning of the lane. later, we want cars starting at arbitrary points in the middle of the
-    // lane (from a building), so just ignore this problem for now.
     // True if we spawned one
     pub fn start_car_on_lane(
         &mut self,
         _time: Tick,
         car: CarID,
+        dist_along: Distance,
         mut path: VecDeque<LaneID>,
-        _map: &Map,
+        map: &Map,
     ) -> bool {
         let start = path.pop_front().unwrap();
+        // If not, we have a parking lane much longer than a driving lane...
+        assert!(dist_along <= map.get_l(start).length());
+
+        // TODO verify it's safe to appear here at dist_along and not cause a crash
 
         self.cars.insert(
             car,
             Car {
                 id: car,
                 path,
-                dist_along: 0.0 * si::M,
+                dist_along: dist_along,
                 speed: 0.0 * si::MPS,
                 on: On::Lane(start),
                 waiting_for: None,
                 debug: false,
             },
         );
-        self.lanes[start.0].cars_queue.push(car);
+        self.lanes[start.0].insert_at(car, dist_along, self);
         true
     }
 
-    pub fn get_empty_lanes(&self, map: &Map) -> Vec<LaneID> {
-        let mut lanes: Vec<LaneID> = Vec::new();
-        for (idx, queue) in self.lanes.iter().enumerate() {
-            if map.get_l(LaneID(idx)).lane_type == LaneType::Driving && queue.is_empty() {
-                lanes.push(queue.id.as_lane());
-            }
-        }
-        lanes
-    }
-
     pub fn get_draw_car(&self, id: CarID, _time: Tick, map: &Map) -> Option<DrawCar> {
         let c = self.cars.get(&id)?;
         let (pos, angle) = c.on.dist_along(c.dist_along, map);

sim/src/lib.rs

@@ -155,8 +155,8 @@ impl On {
 
     fn speed_limit(&self, map: &Map) -> Speed {
         match self {
-            &On::Lane(id) => map.get_lane_and_parent(id).1.get_speed_limit(),
-            &On::Turn(id) => map.get_lane_and_parent(id.dst).1.get_speed_limit(),
+            &On::Lane(id) => map.get_parent(id).get_speed_limit(),
+            &On::Turn(id) => map.get_parent(id.dst).get_speed_limit(),
         }
     }
 }

sim/src/parametric_driving.rs

@@ -10,7 +10,7 @@ use draw_car::DrawCar;
 use geom::{Angle, Pt2D};
 use intersections::{AgentInfo, IntersectionSimState, Request};
 use kinematics::Vehicle;
-use map_model::{LaneID, LaneType, Map, TurnID};
+use map_model::{LaneID, Map, TurnID};
 use models::{choose_turn, Action, FOLLOWING_DISTANCE};
 use multimap::MultiMap;
 use std::collections::{BTreeMap, HashSet, VecDeque};
@@ -459,6 +459,7 @@ impl DrivingSimState {
         &mut self,
         time: Tick,
         car: CarID,
+        _dist_along: Distance,
         mut path: VecDeque<LaneID>,
         map: &Map,
     ) -> bool {
@@ -485,16 +486,6 @@ impl DrivingSimState {
         true
     }
 
-    pub fn get_empty_lanes(&self, map: &Map) -> Vec<LaneID> {
-        let mut lanes: Vec<LaneID> = Vec::new();
-        for (idx, queue) in self.lanes.iter().enumerate() {
-            if map.get_l(LaneID(idx)).lane_type == LaneType::Driving && queue.is_empty() {
-                lanes.push(queue.id.as_lane());
-            }
-        }
-        lanes
-    }
-
     pub fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option<DrawCar> {
         let all = match self.cars.get(&id)?.on {
             On::Lane(l) => self.get_draw_cars_on_lane(l, time, map),

sim/src/parking.rs

@@ -5,7 +5,7 @@ use map_model;
 use map_model::{Lane, LaneID, LaneType, Map};
 use rand::Rng;
 use std::iter;
-use CarID;
+use {Distance, CarID};
 
 #[derive(Serialize, Deserialize, PartialEq, Eq)]
 pub struct ParkingSimState {
@@ -28,7 +28,7 @@ impl ParkingSimState {
     pub fn edit_remove_lane(&mut self, id: LaneID) {
         assert!(self.lanes[id.0].is_empty());
         self.lanes[id.0] = ParkingLane {
-            l: id,
+            id: id,
             spots: Vec::new(),
         };
     }
@@ -72,12 +72,8 @@ impl ParkingSimState {
         );
     }
 
-    pub fn get_last_parked_car(&self, id: LaneID) -> Option<CarID> {
-        self.lanes[id.0].get_last_parked_car()
-    }
-
-    pub fn remove_last_parked_car(&mut self, id: LaneID, car: CarID) {
-        self.lanes[id.0].remove_last_parked_car(car);
+    pub fn remove_parked_car(&mut self, id: LaneID, car: CarID) {
+        self.lanes[id.0].remove_parked_car(car);
         self.total_count -= 1;
     }
 
@@ -94,11 +90,41 @@ impl ParkingSimState {
         }
         None
     }
+
+    pub fn lane_of_car(&self, id: CarID) -> Option<LaneID> {
+        // TODO this is so horrendously slow :D
+        for l in &self.lanes {
+            if l.spots.contains(&Some(id)) {
+                return Some(l.id);
+            }
+        }
+        None
+    }
+
+    // Of the front of the car
+    pub fn get_dist_along_lane(&self, c: CarID, l: LaneID) -> Distance {
+        let idx = self.lanes[l.0].spots.iter().position(|x| *x == Some(c)).unwrap();
+        // TODO some overlap
+        let spot_start = map_model::PARKING_SPOT_LENGTH * (1.0 + idx as f64);
+        spot_start - (map_model::PARKING_SPOT_LENGTH - draw_car::CAR_LENGTH) / 2.0
+    }
+
+    pub fn get_all_cars(&self) -> Vec<(CarID, LaneID)> {
+        let mut result = Vec::new();
+        for l in &self.lanes {
+            for maybe_car in &l.spots {
+                if let Some(car) = maybe_car {
+                    result.push((*car, l.id));
+                }
+            }
+        }
+        result
+    }
 }
 
 #[derive(Serialize, Deserialize, PartialEq, Eq)]
 struct ParkingLane {
-    l: LaneID,
+    id: LaneID,
     spots: Vec<Option<CarID>>,
 }
 
@@ -106,35 +132,24 @@ impl ParkingLane {
     fn new(l: &Lane) -> ParkingLane {
         if l.lane_type != LaneType::Parking {
             return ParkingLane {
-                l: l.id,
+                id: l.id,
                 spots: Vec::new(),
             };
         }
 
         ParkingLane {
-            l: l.id,
+            id: l.id,
             spots: iter::repeat(None).take(l.number_parking_spots()).collect(),
         }
     }
 
-    fn get_last_parked_car(&self) -> Option<CarID> {
-        self.spots
-            .iter()
-            .rfind(|&&x| x.is_some())
-            .map(|l| l.unwrap())
-    }
-
-    fn remove_last_parked_car(&mut self, car: CarID) {
-        let idx = self.spots
-            .iter()
-            .rposition(|&x| x.is_some())
-            .expect("No parked cars at all now");
-        assert_eq!(self.spots[idx], Some(car));
+    fn remove_parked_car(&mut self, car: CarID) {
+        let idx = self.spots.iter().position(|x| *x == Some(car)).unwrap();
         self.spots[idx] = None;
     }
 
     fn get_draw_cars(&self, map: &Map) -> Vec<DrawCar> {
-        let l = map.get_l(self.l);
+        let l = map.get_l(self.id);
         // TODO this is slow to do constantly! can we precompute for each spot or something like
         // that?
         self.spots
@@ -152,7 +167,7 @@ impl ParkingLane {
             .collect()
     }
 
-    pub fn is_empty(&self) -> bool {
-        !self.get_last_parked_car().is_some()
+    fn is_empty(&self) -> bool {
+        !self.spots.iter().find(|&&x| x.is_some()).is_some()
     }
 }

sim/src/sim.rs

@@ -15,7 +15,7 @@ use std::collections::{HashMap, HashSet, VecDeque};
 use std::f64;
 use std::time::{Duration, Instant};
 use walking::WalkingSimState;
-use {CarID, CarState, InvariantViolated, PedestrianID, Tick, TIMESTEP};
+use {CarID, CarState, InvariantViolated, PedestrianID, Tick, TIMESTEP, Distance};
 
 #[derive(Serialize, Deserialize, Derivative, PartialEq, Eq)]
 enum DrivingModel {
@@ -90,10 +90,10 @@ impl DrivingModel {
         &mut self,
         time: Tick,
         car: CarID,
+        dist_along: Distance,
         path: VecDeque<LaneID>,
         map: &Map
     ) -> bool);
-    delegate!(fn get_empty_lanes(&self, map: &Map) -> Vec<LaneID>);
     delegate!(fn get_draw_car(&self, id: CarID, time: Tick, map: &Map) -> Option<DrawCar>);
     delegate!(fn get_draw_cars_on_lane(&self, lane: LaneID, time: Tick, map: &Map) -> Vec<DrawCar>);
     delegate!(fn get_draw_cars_on_turn(&self, turn: TurnID, time: Tick, map: &Map) -> Vec<DrawCar>);
@@ -179,35 +179,52 @@ impl Sim {
     pub fn start_many_parked_cars(&mut self, map: &Map, num_cars: usize) {
         use rayon::prelude::*;
 
-        let mut driving_lanes = self.driving_state.get_empty_lanes(map);
-        // Don't ruin determinism for silly reasons. :)
-        if !driving_lanes.is_empty() {
-            self.rng.shuffle(&mut driving_lanes);
+        let mut cars_and_starts: Vec<(CarID, LaneID)> = self.parking_state
+            .get_all_cars()
+            .into_iter()
+            .filter_map(|(car, parking_lane)| {
+                map.get_parent(parking_lane)
+                    .find_driving_lane(parking_lane)
+                    .and_then(|driving_lane| Some((car, driving_lane)))
+            })
+            .collect();
+        if cars_and_starts.is_empty() {
+            return;
         }
+        self.rng.shuffle(&mut cars_and_starts);
 
-        let mut requested_paths: Vec<(LaneID, LaneID)> = Vec::new();
-        for i in 0..num_cars.min(driving_lanes.len()) {
-            let start = driving_lanes[i];
+        let driving_lanes: Vec<LaneID> = map.all_lanes()
+            .iter()
+            .filter_map(|l| {
+                if l.lane_type == LaneType::Driving {
+                    Some(l.id)
+                } else {
+                    None
+                }
+            })
+            .collect();
+        let mut requested_paths: Vec<(CarID, LaneID, LaneID)> = Vec::new();
+        for i in 0..num_cars.min(cars_and_starts.len()) {
+            let (car, start) = cars_and_starts[i];
             let goal = choose_different(&mut self.rng, &driving_lanes, start);
-            requested_paths.push((start, goal));
+            requested_paths.push((car, start, goal));
         }
 
         println!("Calculating {} paths for cars", requested_paths.len());
         let timer = Instant::now();
-        let paths: Vec<Option<Vec<LaneID>>> = requested_paths
+        let paths: Vec<(CarID, Option<Vec<LaneID>>)> = requested_paths
             .par_iter()
-            .map(|(start, goal)| map_model::pathfind(map, *start, *goal))
+            .map(|(car, start, goal)| (*car, map_model::pathfind(map, *start, *goal)))
             .collect();
 
         let mut actual = 0;
-        for path in paths.into_iter() {
+        for (car, path) in paths.into_iter() {
             if let Some(steps) = path {
-                if self.start_parked_car(map, steps) {
+                if self.start_parked_car_with_path(car, map, steps) {
                     actual += 1;
                 }
             } else {
-                // zip with request to have start/goal?
-                //println!("Failed to pathfind for a pedestrian");
+                println!("Failed to pathfind for {}", car);
             };
         }
 
@@ -219,62 +236,49 @@ impl Sim {
         println!("Started {} parked cars of requested {}", actual, num_cars);
     }
 
-    fn start_parked_car(&mut self, map: &Map, steps: Vec<LaneID>) -> bool {
+    fn start_parked_car_with_path(&mut self, car: CarID, map: &Map, steps: Vec<LaneID>) -> bool {
         let driving_lane = steps[0];
-        if let Some(parking_lane) = map.get_lane_and_parent(driving_lane)
-            .1
+        let parking_lane = map.get_parent(driving_lane)
             .find_parking_lane(driving_lane)
+            .unwrap();
+        let dist_along = self.parking_state.get_dist_along_lane(car, parking_lane);
+
+        if self.driving_state
+            .start_car_on_lane(self.time, car, dist_along, VecDeque::from(steps), map)
         {
-            if let Some(car) = self.parking_state.get_last_parked_car(parking_lane) {
-                if self.driving_state
-                    .start_car_on_lane(self.time, car, VecDeque::from(steps), map)
-                {
-                    self.parking_state.remove_last_parked_car(parking_lane, car);
-                    return true;
-                }
-            } else {
-                println!("No parked cars on {}", parking_lane);
-            }
+            self.parking_state.remove_parked_car(parking_lane, car);
+            true
         } else {
-            println!("{} has no parking lane", driving_lane);
+            false
         }
-        false
     }
 
-    // TODO make the UI do some of this
-    pub fn start_agent(&mut self, map: &Map, id: LaneID) -> bool {
-        // TODO maybe a way to grab both?
-        let (lane, road) = map.get_lane_and_parent(id);
-        let driving_lane = match lane.lane_type {
-            LaneType::Sidewalk => {
-                if let Some(path) = pick_goal_and_find_path(&mut self.rng, map, id) {
-                    println!("Spawned a pedestrian at {}", id);
-                    self.walking_state
-                        .seed_pedestrian(map, VecDeque::from(path));
-                    return true;
-                } else {
-                    return false;
-                }
-            }
-            LaneType::Driving => id,
-            LaneType::Parking => {
-                if let Some(driving) = road.find_driving_lane(id) {
-                    driving
-                } else {
-                    println!("{} has no driving lane", id);
-                    return false;
-                }
-            }
-            LaneType::Biking => {
-                println!("TODO implement bikes");
-                return false;
-            }
-        };
+    pub fn start_parked_car(&mut self, map: &Map, car: CarID) -> bool {
+        let parking_lane = self.parking_state
+            .lane_of_car(car)
+            .expect("Car isn't parked");
+        let road = map.get_parent(parking_lane);
+        let driving_lane = road.find_driving_lane(parking_lane)
+            .expect("Parking lane has no driving lane");
 
         if let Some(path) = pick_goal_and_find_path(&mut self.rng, map, driving_lane) {
-            return self.start_parked_car(map, path);
+            self.start_parked_car_with_path(car, map, path)
+        } else {
+            false
+        }
+    }
+
+    pub fn spawn_pedestrian(&mut self, map: &Map, sidewalk: LaneID) -> bool {
+        assert!(map.get_l(sidewalk).lane_type == LaneType::Sidewalk);
+
+        if let Some(path) = pick_goal_and_find_path(&mut self.rng, map, sidewalk) {
+            self.walking_state
+                .seed_pedestrian(map, VecDeque::from(path));
+            println!("Spawned a pedestrian at {}", sidewalk);
+            true
+        } else {
+            false
         }
-        false
     }
 
     pub fn seed_pedestrians(&mut self, map: &Map, num: usize) {