From 9b6a8cfae395c1f98f978bbf38d5e0d9f7d253a0 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 15:39:09 -0500
Subject: [PATCH 01/25] refactor costs with helpers

---
 .../CreateMergingScenarioGame.jl              | 258 +++++++++++-------
 example/PassingScenario/RunMergingScenario.jl |   4 +-
 2 files changed, 166 insertions(+), 96 deletions(-)

diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl
index b987dceb..592e6907 100644
--- a/example/PassingScenario/CreateMergingScenarioGame.jl
+++ b/example/PassingScenario/CreateMergingScenarioGame.jl
@@ -22,6 +22,81 @@ function combine_cost_funcs(funcs, weights)
     return g
 end
 
+function make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, player_idx)
+
+    base_idx = 4 * (player_idx - 1)
+    player_xidx = base_idx + 1
+    player_yidx = base_idx + 2
+
+    L₁ = cfg.region1_length_m
+    L₂ = cfg.region2_length_m
+    w = cfg.lane_width_m
+
+    merging_trajectory_position_cost(si, x, us, t) = begin
+        dist_along_lane = x[player_yidx]
+
+        if dist_along_lane ≤ L₁ # separate lanes
+            goal_pos = (p1_on_left) ? -w/2 : w/2
+        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+        #     goal_pos = -w/4 - lin_width_at_dist_proportion * w/4
+        else # in second or final region
+            goal_pos = 0.
+        end
+
+        return 1//2 * (x[player_xidx] - goal_pos)^2
+    end
+
+    return merging_trajectory_position_cost
+end
+
+function make_collision_cost(cfg)
+    avoid_collisions_cost_fn(si, x, us, t) = begin
+        # This log barrier avoids agents getting within some configured radius of one another.
+        dist_to_boundary = norm([1 1 0 0 -1 -1 0 0] * x, cfg.dist_norm_order) - cfg.collision_radius_m
+        # TODO(hamzah) - add term to drag trajectory back to valid zone
+        return (dist_to_boundary ≤ 0) ? large_number : -log(dist_to_boundary)
+    end
+
+    return avoid_collisions_cost_fn
+end
+
+function make_piecewise_lane_boundary_cost(cfg, p1_on_left, player_idx; large_number)
+    base_idx = 4 * (player_idx - 1)
+    player_xidx = base_idx + 1
+    player_yidx = base_idx + 2
+
+    L₁ = cfg.region1_length_m
+    L₂ = cfg.region2_length_m
+    w = cfg.lane_width_m
+
+    # This player is on the left if it's P1 on left or P2 where P1 not on left.
+    player_on_left = (p1_on_left && player_idx == 1) || !p1_on_left && player_idx == 2
+
+    stay_within_lanes(si, x, us, t) = begin
+        dist_along_lane = x[player_yidx]
+
+        if dist_along_lane ≤ L₁ # separate lanes
+            upper_bound = (player_on_left) ? 0. : w
+            lower_bound = (player_on_left) ? -w : 0.
+        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+            upper_bound = w/2 + lin_width_at_dist_proportion * w/2
+            lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
+        else # in final region
+            upper_bound = w/2
+            lower_bound = -w/2
+        end
+
+        # println(x[1], " ", lower_bound," ", upper_bound)
+        violates_bound = x[player_xidx] ≥ upper_bound || x[player_xidx] ≤ lower_bound
+        return violates_bound ? large_number : -log(upper_bound-x[player_xidx]) -log(x[player_xidx]-lower_bound)
+    end
+
+    return stay_within_lanes
+end
+
+
 function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p2, goal_p1, goal_p2; large_number=1e6, p1_on_left=true)
     @assert length(w_p1) == NUM_MERGING_SCENARIO_SUBCOSTS
     @assert length(w_p2) == NUM_MERGING_SCENARIO_SUBCOSTS
@@ -52,56 +127,49 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
     c2a = QuadraticCostWithOffset(q_cost2, goal_p2)
 
 
-    merging_trajectory_position_cost_p1(si, x, us, t) = begin
-        dist_along_lane = x[2]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            goal_pos = (p1_on_left) ? -w/2 : w/2
-        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-        #     goal_pos = -w/4 - lin_width_at_dist_proportion * w/4
-        else # in final region 
-            goal_pos = 0.
-        end
-
-        return 1//2 * (x[1] - goal_pos)^2
-    end
-
-    merging_trajectory_position_cost_p2(si, x, us, t) = begin
-        dist_along_lane = x[6]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            goal_pos = (p1_on_left) ? w/2 : -w/2
-            # goal_pos = w/2
-        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-        #     goal_pos = w/4 + lin_width_at_dist_proportion * w/4
-        else # in final region 
-            goal_pos = 0.
-        end
-
-        return 1//2 * (x[5] - goal_pos)^2
-    end
-
-    c1a_i = PlayerCost(merging_trajectory_position_cost_p1, si)
-    c2a_i = PlayerCost(merging_trajectory_position_cost_p2, si)
+    # merging_trajectory_position_cost_p1(si, x, us, t) = begin
+    #     dist_along_lane = x[2]
+    #     L₁ = cfg.region1_length_m
+    #     L₂ = cfg.region2_length_m
+    #     w = cfg.lane_width_m
+
+    #     if dist_along_lane ≤ L₁ # separate lanes
+    #         goal_pos = (p1_on_left) ? -w/2 : w/2
+    #     # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+    #     #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+    #     #     goal_pos = -w/4 - lin_width_at_dist_proportion * w/4
+    #     else # in second or final region 
+    #         goal_pos = 0.
+    #     end
+
+    #     return 1//2 * (x[1] - goal_pos)^2
+    # end
+
+    # merging_trajectory_position_cost_p2(si, x, us, t) = begin
+    #     dist_along_lane = x[6]
+    #     L₁ = cfg.region1_length_m
+    #     L₂ = cfg.region2_length_m
+    #     w = cfg.lane_width_m
+
+    #     if dist_along_lane ≤ L₁ # separate lanes
+    #         goal_pos = (p1_on_left) ? w/2 : -w/2
+    #         # goal_pos = w/2
+    #     # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+    #     #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+    #     #     goal_pos = w/4 + lin_width_at_dist_proportion * w/4
+    #     else # in second or final region 
+    #         goal_pos = 0.
+    #     end
+
+    #     return 1//2 * (x[5] - goal_pos)^2
+    # end
+
+    c1a_i = PlayerCost(make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, 1), si)
+    c2a_i = PlayerCost(make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, 2), si)
 
     # 2. avoid collisions
-    avoid_collisions_cost_fn(si, x, us, t) = begin
-        # This log barrier avoids agents getting within some configured radius of one another.
-        # TODO(hamzah) - this may accidentally be using 1-norm.
-        dist_to_boundary = norm([1 1 0 0 -1 -1 0 0] * x, cfg.dist_norm_order) - cfg.collision_radius_m
-        return (dist_to_boundary ≤ 0) ? large_number : -log(dist_to_boundary)
-    end
-
-    c1b = PlayerCost(avoid_collisions_cost_fn, si)
-    c2b = PlayerCost(avoid_collisions_cost_fn, si)
+    c1b = PlayerCost(make_collision_cost(cfg), si)
+    c2b = PlayerCost(make_collision_cost(cfg), si)
 
     # 3. enforce speed limit and turning limit
     c1c_i = AbsoluteLogBarrierCost(4, cfg.speed_limit_mps, false)
@@ -142,53 +210,53 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
 
     # 6. log barriers on the x dimension ensure that the vehicles don't exit the road
     # TODO(hamzah) - remove assumption of straight road
-    stay_within_lanes_p1(si, x, us, t) = begin
-        dist_along_lane = x[2]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            upper_bound = (p1_on_left) ? 0. : w
-            lower_bound = (p1_on_left) ? -w : 0.
-        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-            upper_bound = w/2 + lin_width_at_dist_proportion * w/2
-            lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
-        else # in final region 
-            upper_bound = w/2
-            lower_bound = -w/2
-        end
-
-        # println(x[1], " ", lower_bound," ", upper_bound)
-        violates_bound = x[1] ≥ upper_bound || x[1] ≤ lower_bound
-        return violates_bound ? large_number : -log(upper_bound-x[1]) -log(x[1]-lower_bound)
-    end
-
-    stay_within_lanes_p2(si, x, us, t) = begin
-        dist_along_lane = x[6]
-        L₁ = cfg.region1_length_m 
-        L₂ = cfg.region2_length_m 
-        w = cfg.lane_width_m
-
-        if dist_along_lane ≤ L₁ # separate lanes
-            upper_bound = (p1_on_left) ? w : 0.
-            lower_bound = (p1_on_left) ? 0. : -w
-        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-            upper_bound = w/2 + lin_width_at_dist_proportion * w/2
-            lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
-        else # in final region 
-            upper_bound = w/2
-            lower_bound = -w/2
-        end
-
-        violates_bound = x[5] ≥ upper_bound || x[5] ≤ lower_bound
-        return violates_bound ? large_number : -log(upper_bound-x[5]) -log(x[5]-lower_bound)
-    end
-
-    c1f = PlayerCost(stay_within_lanes_p1, si)
-    c2f = PlayerCost(stay_within_lanes_p2, si)
+    # stay_within_lanes_p1(si, x, us, t) = begin
+    #     dist_along_lane = x[2]
+    #     L₁ = cfg.region1_length_m
+    #     L₂ = cfg.region2_length_m
+    #     w = cfg.lane_width_m
+
+    #     if dist_along_lane ≤ L₁ # separate lanes
+    #         upper_bound = (p1_on_left) ? 0. : w
+    #         lower_bound = (p1_on_left) ? -w : 0.
+    #     elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+    #         lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+    #         upper_bound = w/2 + lin_width_at_dist_proportion * w/2
+    #         lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
+    #     else # in final region 
+    #         upper_bound = w/2
+    #         lower_bound = -w/2
+    #     end
+
+    #     # println(x[1], " ", lower_bound," ", upper_bound)
+    #     violates_bound = x[1] ≥ upper_bound || x[1] ≤ lower_bound
+    #     return violates_bound ? large_number : -log(upper_bound-x[1]) -log(x[1]-lower_bound)
+    # end
+
+    # stay_within_lanes_p2(si, x, us, t) = begin
+    #     dist_along_lane = x[6]
+    #     L₁ = cfg.region1_length_m
+    #     L₂ = cfg.region2_length_m
+    #     w = cfg.lane_width_m
+
+    #     if dist_along_lane ≤ L₁ # separate lanes
+    #         upper_bound = (p1_on_left) ? w : 0.
+    #         lower_bound = (p1_on_left) ? 0. : -w
+    #     elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+    #         lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+    #         upper_bound = w/2 + lin_width_at_dist_proportion * w/2
+    #         lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
+    #     else # in final region 
+    #         upper_bound = w/2
+    #         lower_bound = -w/2
+    #     end
+
+    #     violates_bound = x[5] ≥ upper_bound || x[5] ≤ lower_bound
+    #     return violates_bound ? large_number : -log(upper_bound-x[5]) -log(x[5]-lower_bound)
+    # end
+
+    c1f = PlayerCost(make_piecewise_lane_boundary_cost(cfg, p1_on_left, 1; large_number), si)
+    c2f = PlayerCost(make_piecewise_lane_boundary_cost(cfg, p1_on_left, 2; large_number), si)
 
     # player 1 stays ahead of player 2
     stay_ahead_cost(si, x, us, t) = begin
diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 42e0a584..a122b501 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -70,6 +70,8 @@ check_valid = get_validator(si, cfg)
 @assert check_valid(x_refs, us_refs, times[1:T]; p1_on_left)
 plot_silqgames_gt(dyn, cfg, times[1:T], x_refs, us_refs)
 
+plot(x_refs[4, :])
+
 # gt_threshold = 1e-2
 # gt_max_iters = 1000
 # gt_step_size = 1e-2
@@ -101,7 +103,7 @@ vel_unc = 1e-3
 P₁ = Diagonal([pos_unc, pos_unc, θ_inc, vel_unc, pos_unc, pos_unc, θ_inc, vel_unc])
 
 # Process noise uncertainty
-Q = 1e-2 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
+Q = 1e-1 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
 
 
 # CONFIG: 

From 3fb0c76d3568882a04f5ac24cf0cab1221b32001 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 17:54:31 -0500
Subject: [PATCH 02/25] Add intermediary goal in region 2, double goal weight
 for p1

---
 example/PassingScenario/CreateMergingScenarioGame.jl | 7 ++++---
 example/PassingScenario/RunMergingScenario.jl        | 2 +-
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl
index 592e6907..c769a390 100644
--- a/example/PassingScenario/CreateMergingScenarioGame.jl
+++ b/example/PassingScenario/CreateMergingScenarioGame.jl
@@ -37,9 +37,10 @@ function make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, player_idx)
 
         if dist_along_lane ≤ L₁ # separate lanes
             goal_pos = (p1_on_left) ? -w/2 : w/2
-        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-        #     goal_pos = -w/4 - lin_width_at_dist_proportion * w/4
+        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+            mult = (p1_on_left) ? -1 : 1
+            goal_pos = mult * (w/4 + lin_width_at_dist_proportion * w/4)
         else # in second or final region
             goal_pos = 0.
         end
diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index a122b501..31f41509 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -60,7 +60,7 @@ weights_p1 = ones(num_subcosts)
 weights_p2 = ones(num_subcosts)
 
 # Adjust goal tracking weights.
-weights_p1[2] = 1.
+weights_p1[2] = 2.
 weights_p2[2] = 1.
 costs = create_merging_scenario_costs(cfg, si, weights_p1, weights_p2, p1_goal, p2_goal; p1_on_left)
 

From 675a14ea831cd15e16a883813b369c2170edda78 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 18:13:05 -0500
Subject: [PATCH 03/25] Adjust meas LF R

---
 example/PassingScenario/RunMergingScenario.jl | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 31f41509..58099f8a 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -103,7 +103,7 @@ vel_unc = 1e-3
 P₁ = Diagonal([pos_unc, pos_unc, θ_inc, vel_unc, pos_unc, pos_unc, θ_inc, vel_unc])
 
 # Process noise uncertainty
-Q = 1e-1 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
+Q = 1e-2 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
 
 
 # CONFIG: 
@@ -112,6 +112,8 @@ Q = 1e-1 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
 rng = MersenneTwister(0)
 
 R = zeros(xdim(dyn), xdim(dyn)) + 1e-2 * I
+# lf_R = 1.1 * R
+lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1])
 zs = zeros(xdim(dyn), T)
 Ts = 20
 num_games = 1
@@ -161,7 +163,7 @@ x̂s, P̂s, probs, pf, sg_objs = leadership_filter(dyn, costs, t₀, times,
                            P₁,        # initial covariance at the beginning of simulation
                            us_refs,   # the control inputs that the actor takes
                            zs,        # the measurements
-                           1.1 * R,
+                           lf_R,
                            process_noise_distribution,
                            s_init_distrib,
                            discrete_state_transition;

From b7a64136e7c949e0f7d1b183e2deb22fdf962e28 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 18:45:32 -0500
Subject: [PATCH 04/25] Fix goals for GT utils

---
 example/PassingScenario/GroundTruthUtils.jl   | 76 +++++++++++++++++++
 example/PassingScenario/RunMergingScenario.jl |  4 +-
 2 files changed, 78 insertions(+), 2 deletions(-)

diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index 609b4289..20a68014 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -314,11 +314,15 @@ end
 # p1 on right and p2 on left
 function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
     v_init = 10.
+    v_goal = v_init
     lw_m = cfg.lane_width_m
 
     # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
     x₁ = [lw_m/2; 0.; pi/2; v_init; -lw_m/2; 15.; pi/2; v_init]
 
+    p1_goal = vcat([0.; 80; pi/2; v_goal], zeros(4))
+    p2_goal = vcat(zeros(4),               [0.; 95.; pi/2; v_goal])
+
     # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
     w1 = vcat(hcat(zeros(1, 40),
                   -0.2*ones(1, 8),
@@ -383,6 +387,78 @@ function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
     return ws, x₁, p1_goal, p2_goal
 end
 
+# # p1 on right and p2 on left
+# function get_merging_debug(cfg::MergingScenarioConfig)
+#     v_init = 10.
+#     lw_m = cfg.lane_width_m
+
+#     # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
+#     x₁ = [0.; 80.; pi/2; v_init; 0.; 70.; pi/2; v_init]
+
+#     # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
+#     w1 = vcat(hcat(zeros(1, 40),
+#                   -0.2*ones(1, 8),
+#                    zeros(1, 10),
+#                    0.2*ones(1, 8),
+#                    zeros(1, 9),
+#                    zeros(1, 16),
+#                    zeros(1, 10)
+#                 ),
+#               hcat( 4  *ones(1, 10),
+#                     7.7*ones(1, 15),
+#                        zeros(1, 49),
+#                        zeros(1, 27)
+#                     # -7.7*ones(1, 15),
+#                     # 7.7*ones(1, 15),
+#                     #     zeros(1, 46)
+#                        # zeros(1, 49),
+#                        # zeros(1, 27)
+#                    # -8.7*ones(1, 17),
+#                    # -5  *ones(1, 10)
+#                    )
+#               )
+
+#     # Agent 2 does a passing maneuver.
+#     w2 = vcat(hcat(zeros(1, 30),
+#                    0.2*ones(1, 8),
+#                    zeros(1, 10),
+#                   -0.2*ones(1, 8),
+#                    zeros(1, 9),
+#                    zeros(1, 16),
+#                    zeros(1, 20)
+#                 ),
+#               hcat( 4  *ones(1, 10),
+#                     7.7*ones(1, 15),
+#                     -7.7*ones(1, 15),
+#                     7.7*ones(1, 15),
+#                         zeros(1, 46)
+#                        # zeros(1, 49),
+#                        # zeros(1, 27)
+#                    # -8.7*ones(1, 17),
+#                    # -5  *ones(1, 10)
+#                    )
+#               )
+
+#     # vcat(hcat(
+#     #                zeros(1, 50),
+#     #                0.4*ones(1, 8),
+#     #               -0.4*ones(1, 8),
+#     #                zeros(1, 9),
+#     #                zeros(1, 16),
+#     #                zeros(1, 10)), 
+#     #           hcat( 5  *ones(1, 10),
+#     #                 8.7*ones(1, 15),
+#     #                    zeros(1, 49),
+#     #                    zeros(1, 27)
+#     #                # -8.7*ones(1, 17),
+#     #                # -5  *ones(1, 10)
+#     #                )
+#     #           )
+
+#     ws = [w2, w1]
+#     return ws, x₁, p1_goal, p2_goal
+# end
+
 
 
 # For saving the trajectory - in case we want to use it later.
diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 58099f8a..23850a93 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -42,8 +42,8 @@ lw_m = cfg.lane_width_m
 # x₁ = [-lw_m/2; 15.; pi/2; v_goal; lw_m/2; 0.; pi/2; v_goal]
 
 # Generate a ground truth trajectory on which to run the leadership filter for a merging trajectory.
-us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
-us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
+# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
+# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
 us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
 
 p1_on_left = (x₁[1] < 0 && x₁[5] > 0)

From a53152e126d2cec8280b3d7d0f7358f37138e54d Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 18:47:07 -0500
Subject: [PATCH 05/25] debug with following as in passing scenario 1

---
 example/PassingScenario/GroundTruthUtils.jl   | 146 +++++++++---------
 example/PassingScenario/RunMergingScenario.jl |   3 +-
 2 files changed, 77 insertions(+), 72 deletions(-)

diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index 20a68014..ed65b3e0 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -387,77 +387,81 @@ function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
     return ws, x₁, p1_goal, p2_goal
 end
 
-# # p1 on right and p2 on left
-# function get_merging_debug(cfg::MergingScenarioConfig)
-#     v_init = 10.
-#     lw_m = cfg.lane_width_m
-
-#     # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
-#     x₁ = [0.; 80.; pi/2; v_init; 0.; 70.; pi/2; v_init]
-
-#     # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
-#     w1 = vcat(hcat(zeros(1, 40),
-#                   -0.2*ones(1, 8),
-#                    zeros(1, 10),
-#                    0.2*ones(1, 8),
-#                    zeros(1, 9),
-#                    zeros(1, 16),
-#                    zeros(1, 10)
-#                 ),
-#               hcat( 4  *ones(1, 10),
-#                     7.7*ones(1, 15),
-#                        zeros(1, 49),
-#                        zeros(1, 27)
-#                     # -7.7*ones(1, 15),
-#                     # 7.7*ones(1, 15),
-#                     #     zeros(1, 46)
-#                        # zeros(1, 49),
-#                        # zeros(1, 27)
-#                    # -8.7*ones(1, 17),
-#                    # -5  *ones(1, 10)
-#                    )
-#               )
-
-#     # Agent 2 does a passing maneuver.
-#     w2 = vcat(hcat(zeros(1, 30),
-#                    0.2*ones(1, 8),
-#                    zeros(1, 10),
-#                   -0.2*ones(1, 8),
-#                    zeros(1, 9),
-#                    zeros(1, 16),
-#                    zeros(1, 20)
-#                 ),
-#               hcat( 4  *ones(1, 10),
-#                     7.7*ones(1, 15),
-#                     -7.7*ones(1, 15),
-#                     7.7*ones(1, 15),
-#                         zeros(1, 46)
-#                        # zeros(1, 49),
-#                        # zeros(1, 27)
-#                    # -8.7*ones(1, 17),
-#                    # -5  *ones(1, 10)
-#                    )
-#               )
-
-#     # vcat(hcat(
-#     #                zeros(1, 50),
-#     #                0.4*ones(1, 8),
-#     #               -0.4*ones(1, 8),
-#     #                zeros(1, 9),
-#     #                zeros(1, 16),
-#     #                zeros(1, 10)), 
-#     #           hcat( 5  *ones(1, 10),
-#     #                 8.7*ones(1, 15),
-#     #                    zeros(1, 49),
-#     #                    zeros(1, 27)
-#     #                # -8.7*ones(1, 17),
-#     #                # -5  *ones(1, 10)
-#     #                )
-#     #           )
-
-#     ws = [w2, w1]
-#     return ws, x₁, p1_goal, p2_goal
-# end
+# p1 on right and p2 on left
+function get_merging_debug(cfg::MergingScenarioConfig)
+    v_init = 10.
+    v_goal = 10.
+    lw_m = cfg.lane_width_m
+
+    # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
+    x₁ = [0.; 80.; pi/2; v_init; 0.; 70.; pi/2; v_init]
+
+    p1_goal = vcat([0.; 150; pi/2; v_goal], zeros(4))
+    p2_goal = vcat(zeros(4),               [0.; 140.; pi/2; v_goal])
+
+    # # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
+    # w1 = vcat(hcat(zeros(1, 40),
+    #               -0.2*ones(1, 8),
+    #                zeros(1, 10),
+    #                0.2*ones(1, 8),
+    #                zeros(1, 9),
+    #                zeros(1, 16),
+    #                zeros(1, 10)
+    #             ),
+    #           hcat( 4  *ones(1, 10),
+    #                 7.7*ones(1, 15),
+    #                    zeros(1, 49),
+    #                    zeros(1, 27)
+    #                 # -7.7*ones(1, 15),
+    #                 # 7.7*ones(1, 15),
+    #                 #     zeros(1, 46)
+    #                    # zeros(1, 49),
+    #                    # zeros(1, 27)
+    #                # -8.7*ones(1, 17),
+    #                # -5  *ones(1, 10)
+    #                )
+    #           )
+
+    # # Agent 2 does a passing maneuver.
+    # w2 = vcat(hcat(zeros(1, 30),
+    #                0.2*ones(1, 8),
+    #                zeros(1, 10),
+    #               -0.2*ones(1, 8),
+    #                zeros(1, 9),
+    #                zeros(1, 16),
+    #                zeros(1, 20)
+    #             ),
+    #           hcat( 4  *ones(1, 10),
+    #                 7.7*ones(1, 15),
+    #                 -7.7*ones(1, 15),
+    #                 7.7*ones(1, 15),
+    #                     zeros(1, 46)
+    #                    # zeros(1, 49),
+    #                    # zeros(1, 27)
+    #                # -8.7*ones(1, 17),
+    #                # -5  *ones(1, 10)
+    #                )
+    #           )
+
+    # vcat(hcat(
+    #                zeros(1, 50),
+    #                0.4*ones(1, 8),
+    #               -0.4*ones(1, 8),
+    #                zeros(1, 9),
+    #                zeros(1, 16),
+    #                zeros(1, 10)), 
+    #           hcat( 5  *ones(1, 10),
+    #                 8.7*ones(1, 15),
+    #                    zeros(1, 49),
+    #                    zeros(1, 27)
+    #                # -8.7*ones(1, 17),
+    #                # -5  *ones(1, 10)
+    #                )
+    #           )
+
+    ws = [zeros(2, 101), zeros(2, 101)]
+    return ws, x₁, p1_goal, p2_goal
+end
 
 
 
diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 23850a93..1b3ad01a 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -44,7 +44,8 @@ lw_m = cfg.lane_width_m
 # Generate a ground truth trajectory on which to run the leadership filter for a merging trajectory.
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
-us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
+# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
+us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg)
 
 p1_on_left = (x₁[1] < 0 && x₁[5] > 0)
 @assert xor(x₁[1] < 0 && x₁[5] > 0, x₁[1] > 0 && x₁[5] < 0)

From db22aa245186050259e6cfac8d12f3308de35831 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 19:58:30 -0500
Subject: [PATCH 06/25] debug with following as in passing scenario 1, bug 1

---
 example/PassingScenario/GroundTruthUtils.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index ed65b3e0..f3a09f79 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -394,7 +394,7 @@ function get_merging_debug(cfg::MergingScenarioConfig)
     lw_m = cfg.lane_width_m
 
     # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
-    x₁ = [0.; 80.; pi/2; v_init; 0.; 70.; pi/2; v_init]
+    x₁ = [-0.01; 80.; pi/2; v_init; 0.01.; 70.; pi/2; v_init]
 
     p1_goal = vcat([0.; 150; pi/2; v_goal], zeros(4))
     p2_goal = vcat(zeros(4),               [0.; 140.; pi/2; v_goal])

From d0755e247e575506477cd841520762667f000d3f Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 21:06:11 -0500
Subject: [PATCH 07/25] Flip leading agent in road scenario

---
 example/PassingScenario/GroundTruthUtils.jl | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index f3a09f79..06975675 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -394,10 +394,10 @@ function get_merging_debug(cfg::MergingScenarioConfig)
     lw_m = cfg.lane_width_m
 
     # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
-    x₁ = [-0.01; 80.; pi/2; v_init; 0.01.; 70.; pi/2; v_init]
+    x₁ = [-0.01; 70.; pi/2; v_init; 0.01.; 80.; pi/2; v_init]
 
-    p1_goal = vcat([0.; 150; pi/2; v_goal], zeros(4))
-    p2_goal = vcat(zeros(4),               [0.; 140.; pi/2; v_goal])
+    p1_goal = vcat([0.; 140; pi/2; v_goal], zeros(4))
+    p2_goal = vcat(zeros(4),               [0.; 150.; pi/2; v_goal])
 
     # # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
     # w1 = vcat(hcat(zeros(1, 40),

From 9817bd84c976da822af7f9b21f18585478cec4da Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 21:06:40 -0500
Subject: [PATCH 08/25] fix bug

---
 example/PassingScenario/GroundTruthUtils.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index f3a09f79..8f3027e4 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -394,7 +394,7 @@ function get_merging_debug(cfg::MergingScenarioConfig)
     lw_m = cfg.lane_width_m
 
     # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
-    x₁ = [-0.01; 80.; pi/2; v_init; 0.01.; 70.; pi/2; v_init]
+    x₁ = [-0.01; 80.; pi/2; v_init; 0.01; 70.; pi/2; v_init]
 
     p1_goal = vcat([0.; 150; pi/2; v_goal], zeros(4))
     p2_goal = vcat(zeros(4),               [0.; 140.; pi/2; v_goal])

From 7796263f1d6e5df1eddd014c33c037dd8f0f8745 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 21:53:16 -0500
Subject: [PATCH 09/25] switch back to A1 ahead and bigger y-goal difference

---
 example/PassingScenario/CreateMergingScenarioGame.jl | 4 ++--
 example/PassingScenario/GroundTruthUtils.jl          | 7 +++----
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl
index c769a390..6cece9b1 100644
--- a/example/PassingScenario/CreateMergingScenarioGame.jl
+++ b/example/PassingScenario/CreateMergingScenarioGame.jl
@@ -109,7 +109,7 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
     const_1 = 1.
     Q1 = zeros(8, 8)
     Q1[1, 1] = const_1 * 0.
-    Q1[2, 2] = const_1 * 0.5#2.
+    Q1[2, 2] = const_1 * 0.#0.5#2.
     Q1[3, 3] = const_1 * 1.
     Q1[4, 4] = const_1 * 1.
     q_cost1 = QuadraticCost(Q1)
@@ -119,7 +119,7 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
 
     Q2 = zeros(8, 8)
     Q2[5, 5] = const_1 * 0.
-    Q2[6, 6] = const_1 * 0.5
+    Q2[6, 6] = const_1 * 0#.5
     Q2[7, 7] = const_1 * 1.
     Q2[8, 8] = const_1 * 1.
     q_cost2 = QuadraticCost(Q2)
diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index 06975675..ae719ef8 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -393,11 +393,10 @@ function get_merging_debug(cfg::MergingScenarioConfig)
     v_goal = 10.
     lw_m = cfg.lane_width_m
 
-    # x₁ = [rlb_x/2; 10.; pi/2; v_init; rlb_x/1.5; 0.; pi/2; v_init]
-    x₁ = [-0.01; 70.; pi/2; v_init; 0.01.; 80.; pi/2; v_init]
+    x₁ = [-0.01; 50.; pi/2; v_init; 0.01; 40.; pi/2; v_init]
 
-    p1_goal = vcat([0.; 140; pi/2; v_goal], zeros(4))
-    p2_goal = vcat(zeros(4),               [0.; 150.; pi/2; v_goal])
+    p1_goal = vcat([0.; 150; pi/2; v_goal], zeros(4))
+    p2_goal = vcat(zeros(4),               [0.; 130.; pi/2; v_goal])
 
     # # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
     # w1 = vcat(hcat(zeros(1, 40),

From 6c0b7b6aa3ae043f155dd2727d320b648ac8b4e5 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 22:40:49 -0500
Subject: [PATCH 10/25] Refactor debug example

---
 example/PassingScenario/GroundTruthUtils.jl | 98 ++-------------------
 1 file changed, 7 insertions(+), 91 deletions(-)

diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index ae719ef8..f134ab42 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -302,8 +302,6 @@ function get_merging_trajectory_p1_same_start_101(cfg::MergingScenarioConfig)
                     7.7*ones(1, 15),
                        zeros(1, 49),
                        zeros(1, 27)
-                   # -8.7*ones(1, 17),
-                   # -5  *ones(1, 10)
                    )
               )
 
@@ -336,13 +334,6 @@ function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
                     7.7*ones(1, 15),
                        zeros(1, 49),
                        zeros(1, 27)
-                    # -7.7*ones(1, 15),
-                    # 7.7*ones(1, 15),
-                    #     zeros(1, 46)
-                       # zeros(1, 49),
-                       # zeros(1, 27)
-                   # -8.7*ones(1, 17),
-                   # -5  *ones(1, 10)
                    )
               )
 
@@ -360,103 +351,28 @@ function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
                     -7.7*ones(1, 15),
                     7.7*ones(1, 15),
                         zeros(1, 46)
-                       # zeros(1, 49),
-                       # zeros(1, 27)
-                   # -8.7*ones(1, 17),
-                   # -5  *ones(1, 10)
-                   )
-              )
-
-    # vcat(hcat(
-    #                zeros(1, 50),
-    #                0.4*ones(1, 8),
-    #               -0.4*ones(1, 8),
-    #                zeros(1, 9),
-    #                zeros(1, 16),
-    #                zeros(1, 10)), 
-    #           hcat( 5  *ones(1, 10),
-    #                 8.7*ones(1, 15),
-    #                    zeros(1, 49),
-    #                    zeros(1, 27)
-    #                # -8.7*ones(1, 17),
-    #                # -5  *ones(1, 10)
-    #                )
-    #           )
+                )
 
     ws = [w2, w1]
     return ws, x₁, p1_goal, p2_goal
 end
 
 # p1 on right and p2 on left
-function get_merging_debug(cfg::MergingScenarioConfig)
+function get_merging_debug(cfg::MergingScenarioConfig, player_in_front=2)
     v_init = 10.
     v_goal = 10.
     lw_m = cfg.lane_width_m
 
     x₁ = [-0.01; 50.; pi/2; v_init; 0.01; 40.; pi/2; v_init]
+    
 
     p1_goal = vcat([0.; 150; pi/2; v_goal], zeros(4))
     p2_goal = vcat(zeros(4),               [0.; 130.; pi/2; v_goal])
 
-    # # w1 = zeros(2, 101) # Agent 1 keeps going in straight line
-    # w1 = vcat(hcat(zeros(1, 40),
-    #               -0.2*ones(1, 8),
-    #                zeros(1, 10),
-    #                0.2*ones(1, 8),
-    #                zeros(1, 9),
-    #                zeros(1, 16),
-    #                zeros(1, 10)
-    #             ),
-    #           hcat( 4  *ones(1, 10),
-    #                 7.7*ones(1, 15),
-    #                    zeros(1, 49),
-    #                    zeros(1, 27)
-    #                 # -7.7*ones(1, 15),
-    #                 # 7.7*ones(1, 15),
-    #                 #     zeros(1, 46)
-    #                    # zeros(1, 49),
-    #                    # zeros(1, 27)
-    #                # -8.7*ones(1, 17),
-    #                # -5  *ones(1, 10)
-    #                )
-    #           )
-
-    # # Agent 2 does a passing maneuver.
-    # w2 = vcat(hcat(zeros(1, 30),
-    #                0.2*ones(1, 8),
-    #                zeros(1, 10),
-    #               -0.2*ones(1, 8),
-    #                zeros(1, 9),
-    #                zeros(1, 16),
-    #                zeros(1, 20)
-    #             ),
-    #           hcat( 4  *ones(1, 10),
-    #                 7.7*ones(1, 15),
-    #                 -7.7*ones(1, 15),
-    #                 7.7*ones(1, 15),
-    #                     zeros(1, 46)
-    #                    # zeros(1, 49),
-    #                    # zeros(1, 27)
-    #                # -8.7*ones(1, 17),
-    #                # -5  *ones(1, 10)
-    #                )
-    #           )
-
-    # vcat(hcat(
-    #                zeros(1, 50),
-    #                0.4*ones(1, 8),
-    #               -0.4*ones(1, 8),
-    #                zeros(1, 9),
-    #                zeros(1, 16),
-    #                zeros(1, 10)), 
-    #           hcat( 5  *ones(1, 10),
-    #                 8.7*ones(1, 15),
-    #                    zeros(1, 49),
-    #                    zeros(1, 27)
-    #                # -8.7*ones(1, 17),
-    #                # -5  *ones(1, 10)
-    #                )
-    #           )
+    if player_in_front == 2
+        x₁[2], x₁[6] = x₁[6], x₁[2]
+        p1_goal[2], p2_goal[6] = p2_goal[6], p1_goal[2]
+    end
 
     ws = [zeros(2, 101), zeros(2, 101)]
     return ws, x₁, p1_goal, p2_goal

From 0c3916367f21cd4be5d1e6bf2914cce8dc7e3e10 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 22:42:33 -0500
Subject: [PATCH 11/25] Refactor debug example, p2

---
 example/PassingScenario/GroundTruthUtils.jl | 1 +
 1 file changed, 1 insertion(+)

diff --git a/example/PassingScenario/GroundTruthUtils.jl b/example/PassingScenario/GroundTruthUtils.jl
index f134ab42..f694cd80 100644
--- a/example/PassingScenario/GroundTruthUtils.jl
+++ b/example/PassingScenario/GroundTruthUtils.jl
@@ -352,6 +352,7 @@ function get_merging_trajectory_p2_flipped_101(cfg::MergingScenarioConfig)
                     7.7*ones(1, 15),
                         zeros(1, 46)
                 )
+            )
 
     ws = [w2, w1]
     return ws, x₁, p1_goal, p2_goal

From e0ecdaf7df8bd93942d3fa736a723692849bdcd6 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 22:48:42 -0500
Subject: [PATCH 12/25] p2 in front from region two, with 0 xpos

---
 example/PassingScenario/CreateMergingScenarioGame.jl | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl
index 6cece9b1..e635933a 100644
--- a/example/PassingScenario/CreateMergingScenarioGame.jl
+++ b/example/PassingScenario/CreateMergingScenarioGame.jl
@@ -37,10 +37,10 @@ function make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, player_idx)
 
         if dist_along_lane ≤ L₁ # separate lanes
             goal_pos = (p1_on_left) ? -w/2 : w/2
-        elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-            lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-            mult = (p1_on_left) ? -1 : 1
-            goal_pos = mult * (w/4 + lin_width_at_dist_proportion * w/4)
+        # elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
+        #     lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
+        #     mult = (p1_on_left) ? -1 : 1
+        #     goal_pos = mult * (w/4 + lin_width_at_dist_proportion * w/4)
         else # in second or final region
             goal_pos = 0.
         end

From 23779fb4340a96076560c1ca4850814279ba5bbf Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 22:56:32 -0500
Subject: [PATCH 13/25] p1 in front from region two, with 0 xpos

---
 example/PassingScenario/RunMergingScenario.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 1b3ad01a..19743a4f 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -45,7 +45,7 @@ lw_m = cfg.lane_width_m
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
-us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg)
+us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 1)
 
 p1_on_left = (x₁[1] < 0 && x₁[5] > 0)
 @assert xor(x₁[1] < 0 && x₁[5] > 0, x₁[1] > 0 && x₁[5] < 0)

From 7c078176ae8b0a9132788bf36fce845d665948aa Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 23:05:35 -0500
Subject: [PATCH 14/25] remove prints, add iterations in nonlq game

---
 .../DELETE_leadfilt_non_LQ_parameters.jl               | 10 +++++-----
 .../SILQGamesExamples/RunMCSimulationsForNonLQGame.jl  |  4 ++--
 src/LeadershipFilter.jl                                |  2 +-
 src/TwoStateParticleFilter.jl                          |  2 +-
 4 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl b/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl
index e3c142a8..5c026d61 100644
--- a/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl
+++ b/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl
@@ -48,7 +48,7 @@ R = 0.02 * Matrix(I, xdim(dyn), xdim(dyn))
 zs = zeros(xdim(dyn), T)
 Ts = 30
 num_games = 1
-num_particles = 200
+num_particles = 100
 
 p_transition = 0.98
 p_init = 0.5
@@ -57,16 +57,16 @@ p_init = 0.5
 # max_iters = 50
 # step_size = 2e-2
 
-lf_threshold = 1e-3 #1e-3
-lf_max_iters = 50
-lf_step_size = 2e-2
+lf_threshold = 1e-2 #1e-3
+lf_max_iters = 100
+lf_step_size = 1e-2
 
 
 gt_silq_num_runs=1
 
 # config variables
 gt_silq_threshold=1e-3
-gt_silq_max_iters=2000
+gt_silq_max_iters=3000
 gt_silq_step_size=1e-2
 gt_silq_verbose=true
 
diff --git a/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl b/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl
index 0a201a00..dc48c855 100644
--- a/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl
+++ b/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl
@@ -21,9 +21,9 @@ include("SILQGamesMCUtils.jl")
 
 data_folder = "mc_data"
 
-num_sims = 20
+num_sims = 40
 # x₁ = [2.0, 1.0, -2.677945044588987, 0.0, -0.9192214382877095, 2.03838954750859, -1.1471487177940904, 0.0]
-angle_diff = 0.2
+angle_diff = 0.1
 # angle_diff = 0.
 
 topfolder_name = joinpath(data_folder, "nonlq_mc$(num_sims)_L$(leader_idx)_$(get_date_str())")
diff --git a/src/LeadershipFilter.jl b/src/LeadershipFilter.jl
index 4a4ee291..44a03591 100644
--- a/src/LeadershipFilter.jl
+++ b/src/LeadershipFilter.jl
@@ -138,7 +138,7 @@ function make_stackelberg_meas_model(tt::Int, sg_obj::SILQGamesObject, leader_id
         meas = extract_measurements_from_stack_trajectory(xs, tt-1, tt)
         # println(meas)
         if !is_converged
-            println("$(sg_obj.current_idx)/$(sg_obj.num_runs) @ $tt - $num_iters => $(convergence_metrics[1, num_iters]) > $(sg_obj.threshold)")
+#            println("$(sg_obj.current_idx)/$(sg_obj.num_runs) @ $tt - $num_iters => $(convergence_metrics[1, num_iters]) > $(sg_obj.threshold)")
         end
         return meas
     end
diff --git a/src/TwoStateParticleFilter.jl b/src/TwoStateParticleFilter.jl
index b9fff013..93dd08cb 100644
--- a/src/TwoStateParticleFilter.jl
+++ b/src/TwoStateParticleFilter.jl
@@ -192,7 +192,7 @@ function step_pf(pf::ParticleFilter, time_range, f_dynamics, h_measures, discret
 
         distrib = MvNormal(pf.z_models[:,i,k], R)
         p[i] = compute_measurement_lkhd(distrib, z)
-        println("meas. lkhd = $(p[i])")
+#        println("meas. lkhd = $(p[i])")
     end
     c_inv = weights_prev' * p
 

From 245ffa2e0b3a43d667e5fc811272f7b20dc3c971 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 23:06:07 -0500
Subject: [PATCH 15/25] remove commented code

---
 .../CreateMergingScenarioGame.jl              | 46 -------------------
 1 file changed, 46 deletions(-)

diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl
index e635933a..184f10e3 100644
--- a/example/PassingScenario/CreateMergingScenarioGame.jl
+++ b/example/PassingScenario/CreateMergingScenarioGame.jl
@@ -211,51 +211,6 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
 
     # 6. log barriers on the x dimension ensure that the vehicles don't exit the road
     # TODO(hamzah) - remove assumption of straight road
-    # stay_within_lanes_p1(si, x, us, t) = begin
-    #     dist_along_lane = x[2]
-    #     L₁ = cfg.region1_length_m
-    #     L₂ = cfg.region2_length_m
-    #     w = cfg.lane_width_m
-
-    #     if dist_along_lane ≤ L₁ # separate lanes
-    #         upper_bound = (p1_on_left) ? 0. : w
-    #         lower_bound = (p1_on_left) ? -w : 0.
-    #     elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-    #         lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-    #         upper_bound = w/2 + lin_width_at_dist_proportion * w/2
-    #         lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
-    #     else # in final region 
-    #         upper_bound = w/2
-    #         lower_bound = -w/2
-    #     end
-
-    #     # println(x[1], " ", lower_bound," ", upper_bound)
-    #     violates_bound = x[1] ≥ upper_bound || x[1] ≤ lower_bound
-    #     return violates_bound ? large_number : -log(upper_bound-x[1]) -log(x[1]-lower_bound)
-    # end
-
-    # stay_within_lanes_p2(si, x, us, t) = begin
-    #     dist_along_lane = x[6]
-    #     L₁ = cfg.region1_length_m
-    #     L₂ = cfg.region2_length_m
-    #     w = cfg.lane_width_m
-
-    #     if dist_along_lane ≤ L₁ # separate lanes
-    #         upper_bound = (p1_on_left) ? w : 0.
-    #         lower_bound = (p1_on_left) ? 0. : -w
-    #     elseif dist_along_lane ≤ L₁ + L₂ # linear progression to smaller lane
-    #         lin_width_at_dist_proportion = 1 - (dist_along_lane - L₁)/L₂
-    #         upper_bound = w/2 + lin_width_at_dist_proportion * w/2
-    #         lower_bound = -w/2 - lin_width_at_dist_proportion * w/2
-    #     else # in final region 
-    #         upper_bound = w/2
-    #         lower_bound = -w/2
-    #     end
-
-    #     violates_bound = x[5] ≥ upper_bound || x[5] ≤ lower_bound
-    #     return violates_bound ? large_number : -log(upper_bound-x[5]) -log(x[5]-lower_bound)
-    # end
-
     c1f = PlayerCost(make_piecewise_lane_boundary_cost(cfg, p1_on_left, 1; large_number), si)
     c2f = PlayerCost(make_piecewise_lane_boundary_cost(cfg, p1_on_left, 2; large_number), si)
 
@@ -268,7 +223,6 @@ function create_merging_scenario_costs(cfg::MergingScenarioConfig, si, w_p1, w_p
     end
     c1g = PlayerCost(stay_ahead_cost, si)
 
-
     costs_p1 = [c1a,
                 c1a_i,
                 c1b,

From 111c0dc04c6243d28f599050047ef61c846c1d38 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Mon, 11 Sep 2023 23:07:24 -0500
Subject: [PATCH 16/25] tweak non lq game

---
 .../DELETE_leadfilt_non_LQ_parameters.jl                 | 8 ++++----
 .../SILQGamesExamples/RunMCSimulationsForNonLQGame.jl    | 4 ++--
 src/LeadershipFilter.jl                                  | 6 +++---
 src/TwoStateParticleFilter.jl                            | 9 ++++++++-
 4 files changed, 17 insertions(+), 10 deletions(-)

diff --git a/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl b/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl
index e3c142a8..7f834ce8 100644
--- a/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl
+++ b/example/SILQGamesExamples/DELETE_leadfilt_non_LQ_parameters.jl
@@ -48,7 +48,7 @@ R = 0.02 * Matrix(I, xdim(dyn), xdim(dyn))
 zs = zeros(xdim(dyn), T)
 Ts = 30
 num_games = 1
-num_particles = 200
+num_particles = 100
 
 p_transition = 0.98
 p_init = 0.5
@@ -57,9 +57,9 @@ p_init = 0.5
 # max_iters = 50
 # step_size = 2e-2
 
-lf_threshold = 1e-3 #1e-3
-lf_max_iters = 50
-lf_step_size = 2e-2
+lf_threshold = 1e-2 #1e-3
+lf_max_iters = 200
+lf_step_size = 1e-2
 
 
 gt_silq_num_runs=1
diff --git a/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl b/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl
index 0a201a00..dc48c855 100644
--- a/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl
+++ b/example/SILQGamesExamples/RunMCSimulationsForNonLQGame.jl
@@ -21,9 +21,9 @@ include("SILQGamesMCUtils.jl")
 
 data_folder = "mc_data"
 
-num_sims = 20
+num_sims = 40
 # x₁ = [2.0, 1.0, -2.677945044588987, 0.0, -0.9192214382877095, 2.03838954750859, -1.1471487177940904, 0.0]
-angle_diff = 0.2
+angle_diff = 0.1
 # angle_diff = 0.
 
 topfolder_name = joinpath(data_folder, "nonlq_mc$(num_sims)_L$(leader_idx)_$(get_date_str())")
diff --git a/src/LeadershipFilter.jl b/src/LeadershipFilter.jl
index 4a4ee291..9987814d 100644
--- a/src/LeadershipFilter.jl
+++ b/src/LeadershipFilter.jl
@@ -137,9 +137,9 @@ function make_stackelberg_meas_model(tt::Int, sg_obj::SILQGamesObject, leader_id
         # Process the stackelberg trajectory to get the desired output and vectorize.
         meas = extract_measurements_from_stack_trajectory(xs, tt-1, tt)
         # println(meas)
-        if !is_converged
-            println("$(sg_obj.current_idx)/$(sg_obj.num_runs) @ $tt - $num_iters => $(convergence_metrics[1, num_iters]) > $(sg_obj.threshold)")
-        end
+        # if !is_converged
+        #     println("$(sg_obj.current_idx)/$(sg_obj.num_runs) @ $tt - $num_iters => $(convergence_metrics[1, num_iters]) > $(sg_obj.threshold)")
+        # end
         return meas
     end
     return h
diff --git a/src/TwoStateParticleFilter.jl b/src/TwoStateParticleFilter.jl
index b9fff013..1ad5da9d 100644
--- a/src/TwoStateParticleFilter.jl
+++ b/src/TwoStateParticleFilter.jl
@@ -186,13 +186,20 @@ function step_pf(pf::ParticleFilter, time_range, f_dynamics, h_measures, discret
         pf.particles[:,i,k] = f_dynamics[s_idx](time_range, 𝒳_prev[:,i], u_input_k, pf.rng)
         pf.z_models[:,i,k] = h_measures[s_idx](pf.particles[:,i,k])
 
+        rz = round.(z, sigdigits=6)
+        rzhat = round.(pf.z_models[:,i,k])
+        # println("L$(pf.s[1,i,k]) particle $i - x meas $(rz[1]) $(rz[5]) p1 ($(rzhat[1])) p2 ($(rzhat[5]))")
+        # println("L$(pf.s[1,i,k]) particle $i - y meas $(rz[2]) $(rz[6]) p1 ($(rzhat[2])) p2 ($(rzhat[6]))")
+        # println("L$(pf.s[1,i,k]) particle $i - θ meas $(rz[3]) $(rz[7]) p1 ($(rzhat[3])) p2 ($(rzhat[7]))")
+        # println("L$(pf.s[1,i,k]) particle $i - v meas $(rz[4]) $(rz[8]) p1 ($(rzhat[4])) p2 ($(rzhat[8]))")
+
         z_hat = pf.z_models[:,i,k]
         # println("||z - ẑ|| = $(norm(z - z_hat))")
         # println("true z: $z, z hat: $(z_hat)")
 
         distrib = MvNormal(pf.z_models[:,i,k], R)
         p[i] = compute_measurement_lkhd(distrib, z)
-        println("meas. lkhd = $(p[i])")
+        # println("meas. lkhd = $(p[i])")
     end
     c_inv = weights_prev' * p
 

From bf36ad0303f6b18943eea4def295726f343a858a Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 17:08:37 -0500
Subject: [PATCH 17/25] make costs equal again

---
 example/PassingScenario/RunMergingScenario.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 19743a4f..9743bf9e 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -61,7 +61,7 @@ weights_p1 = ones(num_subcosts)
 weights_p2 = ones(num_subcosts)
 
 # Adjust goal tracking weights.
-weights_p1[2] = 2.
+weights_p1[2] = 1.
 weights_p2[2] = 1.
 costs = create_merging_scenario_costs(cfg, si, weights_p1, weights_p2, p1_goal, p2_goal; p1_on_left)
 

From 4125f26ff7da7114226b392dad1c5e88d3040607 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 17:20:28 -0500
Subject: [PATCH 18/25] make costs equal again, but put p1 in back

---
 example/PassingScenario/RunMergingScenario.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 9743bf9e..7fd2c213 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -45,7 +45,7 @@ lw_m = cfg.lane_width_m
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
-us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 1)
+us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 2)
 
 p1_on_left = (x₁[1] < 0 && x₁[5] > 0)
 @assert xor(x₁[1] < 0 && x₁[5] > 0, x₁[1] > 0 && x₁[5] < 0)

From d4b3043de75e7bf066a7cf415e308138961031b7 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 17:32:47 -0500
Subject: [PATCH 19/25] make costs equal again, but put p2 in back

---
 example/PassingScenario/RunMergingScenario.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 7fd2c213..9743bf9e 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -45,7 +45,7 @@ lw_m = cfg.lane_width_m
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
-us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 2)
+us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 1)
 
 p1_on_left = (x₁[1] < 0 && x₁[5] > 0)
 @assert xor(x₁[1] < 0 && x₁[5] > 0, x₁[1] > 0 && x₁[5] < 0)

From 0f42c8888e6a250f5831d3f10978c03f0cad8759 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 19:07:09 -0500
Subject: [PATCH 20/25] revert to P2 ahead reverse.

---
 example/PassingScenario/RunMergingScenario.jl | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 9743bf9e..420d91e0 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -43,14 +43,14 @@ lw_m = cfg.lane_width_m
 
 # Generate a ground truth trajectory on which to run the leadership filter for a merging trajectory.
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
-# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
+us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
-us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 1)
+# us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 1)
 
 p1_on_left = (x₁[1] < 0 && x₁[5] > 0)
 @assert xor(x₁[1] < 0 && x₁[5] > 0, x₁[1] > 0 && x₁[5] < 0)
 
-println(p1_on_left)
+println("P1 on left: ", p1_on_left)
 
 # us_refs, x₁ = get_merging_trajectory_p1_same_start_101(cfg)
 # us_refs = [zeros(2, T) for ii in 1:2]
@@ -113,8 +113,8 @@ Q = 1e-2 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
 rng = MersenneTwister(0)
 
 R = zeros(xdim(dyn), xdim(dyn)) + 1e-2 * I
-# lf_R = 1.1 * R
-lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1])
+lf_R = 1.1 * R
+# lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1])
 zs = zeros(xdim(dyn), T)
 Ts = 20
 num_games = 1

From 290f864a90e0fb6266a62fd553b432b9a6c49ff8 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 19:11:00 -0500
Subject: [PATCH 21/25] revert to P2 ahead reverse. fix bug 1

---
 example/PassingScenario/CreateMergingScenarioGame.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl
index 184f10e3..cdcab43e 100644
--- a/example/PassingScenario/CreateMergingScenarioGame.jl
+++ b/example/PassingScenario/CreateMergingScenarioGame.jl
@@ -51,7 +51,7 @@ function make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, player_idx)
     return merging_trajectory_position_cost
 end
 
-function make_collision_cost(cfg)
+function make_collision_cost(cfg; large_number)
     avoid_collisions_cost_fn(si, x, us, t) = begin
         # This log barrier avoids agents getting within some configured radius of one another.
         dist_to_boundary = norm([1 1 0 0 -1 -1 0 0] * x, cfg.dist_norm_order) - cfg.collision_radius_m

From 56288e9c5ba654f52e44ae9881da097aa42a2705 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 19:12:40 -0500
Subject: [PATCH 22/25] revert to P2 ahead reverse. fix bug 2

---
 example/PassingScenario/CreateMergingScenarioGame.jl | 4 ++--
 example/PassingScenario/MergingScenarioConfig.jl     | 2 ++
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/example/PassingScenario/CreateMergingScenarioGame.jl b/example/PassingScenario/CreateMergingScenarioGame.jl
index cdcab43e..9e5c1245 100644
--- a/example/PassingScenario/CreateMergingScenarioGame.jl
+++ b/example/PassingScenario/CreateMergingScenarioGame.jl
@@ -51,7 +51,7 @@ function make_piecewise_horizontal_pos_goal_cost_fn(cfg, p1_on_left, player_idx)
     return merging_trajectory_position_cost
 end
 
-function make_collision_cost(cfg; large_number)
+function make_collision_cost(cfg; large_number=LARGE_NUMBER)
     avoid_collisions_cost_fn(si, x, us, t) = begin
         # This log barrier avoids agents getting within some configured radius of one another.
         dist_to_boundary = norm([1 1 0 0 -1 -1 0 0] * x, cfg.dist_norm_order) - cfg.collision_radius_m
@@ -62,7 +62,7 @@ function make_collision_cost(cfg; large_number)
     return avoid_collisions_cost_fn
 end
 
-function make_piecewise_lane_boundary_cost(cfg, p1_on_left, player_idx; large_number)
+function make_piecewise_lane_boundary_cost(cfg, p1_on_left, player_idx; large_number=LARGE_NUMBER)
     base_idx = 4 * (player_idx - 1)
     player_xidx = base_idx + 1
     player_yidx = base_idx + 2
diff --git a/example/PassingScenario/MergingScenarioConfig.jl b/example/PassingScenario/MergingScenarioConfig.jl
index a081218d..470f2f35 100644
--- a/example/PassingScenario/MergingScenarioConfig.jl
+++ b/example/PassingScenario/MergingScenarioConfig.jl
@@ -2,6 +2,8 @@ using Parameters
 
 # This file contains configuration variables related to the road and constraints.
 
+const LARGE_NUMBER = 1e6
+
 # The configuration defines a straight two-way lane that defines a right-hand coordinate system where y points in the
 # direction of the lane, and x points to the right side of the road.
 DEFAULT_LANE_WIDTH = 2.3

From e836576b760e658ec6ae8d126eab8671f0e8dd78 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 20:00:19 -0500
Subject: [PATCH 23/25] revert to P2 ahead reverse. fix bug 3

---
 example/PassingScenario/RunMergingScenario.jl | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index 420d91e0..f8a9c260 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -113,8 +113,8 @@ Q = 1e-2 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
 rng = MersenneTwister(0)
 
 R = zeros(xdim(dyn), xdim(dyn)) + 1e-2 * I
-lf_R = 1.1 * R
-# lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1])
+# lf_R = 1.1 * R
+lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1])
 zs = zeros(xdim(dyn), T)
 Ts = 20
 num_games = 1

From 77109f9d93c7f490b0376073337d7f130c461f88 Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 21:49:18 -0500
Subject: [PATCH 24/25] change to flipped scenario

---
 example/PassingScenario/RunMergingScenario.jl | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index f8a9c260..ce797c3c 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -43,8 +43,8 @@ lw_m = cfg.lane_width_m
 
 # Generate a ground truth trajectory on which to run the leadership filter for a merging trajectory.
 # us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p1_first_101(cfg)
-us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
-# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
+# us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_reverse_101(cfg)
+us_refs, x₁, p1_goal, p2_goal = get_merging_trajectory_p2_flipped_101(cfg)
 # us_refs, x₁, p1_goal, p2_goal = get_merging_debug(cfg, 1)
 
 p1_on_left = (x₁[1] < 0 && x₁[5] > 0)

From fa78f2a148c76aae19ea222853ceb4c1a5047c0f Mon Sep 17 00:00:00 2001
From: Hamzah Khan <2308491+khanh111@users.noreply.github.com>
Date: Tue, 12 Sep 2023 21:50:03 -0500
Subject: [PATCH 25/25] reduce lf R

---
 example/PassingScenario/RunMergingScenario.jl | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/example/PassingScenario/RunMergingScenario.jl b/example/PassingScenario/RunMergingScenario.jl
index ce797c3c..5cdd1717 100644
--- a/example/PassingScenario/RunMergingScenario.jl
+++ b/example/PassingScenario/RunMergingScenario.jl
@@ -113,8 +113,8 @@ Q = 1e-2 * Diagonal([1e-2, 1e-2, 1e-3, 1e-2, 1e-2, 1e-2, 1e-3, 1e-2])
 rng = MersenneTwister(0)
 
 R = zeros(xdim(dyn), xdim(dyn)) + 1e-2 * I
-# lf_R = 1.1 * R
-lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1])
+lf_R = 1.1 * R
+# lf_R = Diagonal([5e-3, 5e-3, 1e-3, 1e-1, 5e-3, 5e-3, 1e-3, 1e-1])
 zs = zeros(xdim(dyn), T)
 Ts = 20
 num_games = 1