From 46342054489543ed3e4d52debe776e6eb2bb1c93 Mon Sep 17 00:00:00 2001
From: Dustin Carlino <dabreegster@gmail.com>
Date: Wed, 29 Apr 2020 14:49:31 -0700
Subject: [PATCH] make the turn-conflict detector factor in blockages due to
 agents at the front of an at-capacity queue

---
 sim/src/mechanics/driving.rs      |  5 +----
 sim/src/mechanics/intersection.rs | 32 +++++++++++++++++++++++++++----
 2 files changed, 29 insertions(+), 8 deletions(-)

diff --git a/sim/src/mechanics/driving.rs b/sim/src/mechanics/driving.rs
index c9c6b347c8..e048baf180 100644
--- a/sim/src/mechanics/driving.rs
+++ b/sim/src/mechanics/driving.rs
@@ -340,10 +340,7 @@ impl DrivingSimState {
                         now,
                         map,
                         scheduler,
-                        Some((
-                            self.queues.get_mut(&Traversable::Lane(t.dst)).unwrap(),
-                            &car,
-                        )),
+                        Some((&car, &mut self.queues)),
                     ) {
                         // Don't schedule a retry here.
                         return false;
diff --git a/sim/src/mechanics/intersection.rs b/sim/src/mechanics/intersection.rs
index 1c841c90af..26227ecfa1 100644
--- a/sim/src/mechanics/intersection.rs
+++ b/sim/src/mechanics/intersection.rs
@@ -4,8 +4,8 @@ use crate::{AgentID, AlertLocation, CarID, Command, Event, Scheduler, Speed};
 use abstutil::{deserialize_btreemap, retain_btreeset, serialize_btreemap};
 use geom::{Duration, Time};
 use map_model::{
-    ControlStopSign, ControlTrafficSignal, IntersectionID, LaneID, Map, RoadID, TurnID,
-    TurnPriority, TurnType,
+    ControlStopSign, ControlTrafficSignal, IntersectionID, LaneID, Map, RoadID, Traversable,
+    TurnID, TurnPriority, TurnType,
 };
 use serde_derive::{Deserialize, Serialize};
 use std::collections::{BTreeMap, BTreeSet, HashSet};
@@ -217,7 +217,7 @@ impl IntersectionSimState {
         now: Time,
         map: &Map,
         scheduler: &mut Scheduler,
-        maybe_car_and_target_queue: Option<(&mut Queue, &Car)>,
+        maybe_car_and_queues: Option<(&Car, &mut BTreeMap<Traversable, Queue>)>,
     ) -> bool {
         let req = Request { agent, turn };
         self.state
@@ -241,7 +241,9 @@ impl IntersectionSimState {
         }
 
         // Don't block the box
-        if let Some((queue, car)) = maybe_car_and_target_queue {
+        if let Some((car, queues)) = maybe_car_and_queues {
+            assert_eq!(agent, AgentID::Car(car.vehicle.id));
+            let queue = queues.get_mut(&Traversable::Lane(turn.dst)).unwrap();
             // TODO Disable this policy for a particular intersection where a traffic signal is
             // surrounded by a tiny lane with almost no capacity. Generalization later, make
             // progress for now.
@@ -249,6 +251,28 @@ impl IntersectionSimState {
                 car,
                 !self.dont_block_the_box || map.get_i(turn.parent).orig_id.osm_node_id == 53211694,
             ) {
+                if self.break_turn_conflict_cycles {
+                    // TODO Should we run the detector here?
+                    if let Some(c) = queue.laggy_head {
+                        self.blocked_by.insert((car.vehicle.id, c));
+                    } else if let Some(c) = queue.cars.get(0) {
+                        self.blocked_by.insert((car.vehicle.id, *c));
+                    } else {
+                        // Nobody's in the target lane, but there's somebody already in the
+                        // intersection headed there, taking up all of the space.
+                        self.blocked_by.insert((
+                            car.vehicle.id,
+                            self.state[&turn.parent]
+                                .accepted
+                                .iter()
+                                .find(|r| r.turn.dst == turn.dst)
+                                .unwrap()
+                                .agent
+                                .as_car(),
+                        ));
+                    }
+                }
+
                 return false;
             }
         }