Skip to main content

rust_robotics_control/
mpc.rs

1#![allow(dead_code)]
2
3//! Model Predictive Control (MPC) for path tracking
4//!
5//! author: Atsushi Sakai (@Atsushi_twi)
6//!         Ryohei Sasaki (@rsasaki0109)
7//!         Rust port
8//!
9//! This version follows the PythonRobotics structure more closely:
10//! - speed profile generation
11//! - yaw smoothing
12//! - iterative linear MPC around an operational point
13
14use nalgebra::{Matrix2, Matrix4, Matrix4x2, Vector2, Vector4};
15use std::f64::consts::PI;
16
17// Vehicle parameters
18const WB: f64 = 2.5; // wheelbase [m]
19const MAX_STEER: f64 = 45.0 * PI / 180.0; // max steering angle [rad]
20const MAX_DSTEER: f64 = 30.0 * PI / 180.0; // max steering rate [rad/s]
21const MAX_SPEED: f64 = 55.0 / 3.6; // max speed [m/s]
22const MIN_SPEED: f64 = -20.0 / 3.6; // min speed (reverse) [m/s]
23const MAX_ACCEL: f64 = 1.0; // max acceleration [m/ss]
24
25// MPC parameters, aligned with PythonRobotics
26const T: usize = 5; // prediction horizon
27const DT: f64 = 0.2; // time step [s]
28const TARGET_SPEED: f64 = 10.0 / 3.6; // [m/s]
29const GOAL_DIS: f64 = 1.5; // goal distance threshold
30const STOP_SPEED: f64 = 0.5 / 3.6; // stop speed threshold
31const MAX_ITER: usize = 3; // iterative linear MPC outer iterations
32const DU_TH: f64 = 0.1; // outer-loop convergence threshold
33const N_IND_SEARCH: usize = 10; // nearest-index search window
34const PATH_RESOLUTION: f64 = 0.5; // [m]
35const UPSTREAM_SWITCH_BACK_TICK: f64 = 1.0; // [m]
36const MAX_SIM_STEPS: usize = 2000;
37
38// Inner projected-gradient solver settings
39const QP_MAX_ITERS: usize = 200;
40const LINE_SEARCH_ITERS: usize = 16;
41const LINE_SEARCH_INITIAL_STEP: f64 = 0.25;
42const GRAD_TOL: f64 = 1.0e-5;
43const COST_TOL: f64 = 1.0e-7;
44
45// Cost weights
46const Q: [f64; 4] = [1.0, 1.0, 0.5, 0.5];
47const QF: [f64; 4] = Q;
48const R: [f64; 2] = [0.01, 0.01];
49const RD: [f64; 2] = [0.01, 1.0];
50
51/// Vehicle state
52#[derive(Clone, Copy, Debug)]
53pub struct State {
54    pub x: f64,
55    pub y: f64,
56    pub v: f64,
57    pub yaw: f64,
58}
59
60impl State {
61    pub fn new(x: f64, y: f64, v: f64, yaw: f64) -> Self {
62        Self { x, y, v, yaw }
63    }
64
65    pub fn to_vector(self) -> Vector4<f64> {
66        Vector4::new(self.x, self.y, self.v, self.yaw)
67    }
68
69    pub fn update(&mut self, accel: f64, steer: f64) {
70        let steer = steer.clamp(-MAX_STEER, MAX_STEER);
71
72        self.x += self.v * self.yaw.cos() * DT;
73        self.y += self.v * self.yaw.sin() * DT;
74        self.yaw += self.v / WB * steer.tan() * DT;
75        self.yaw = normalize_angle(self.yaw);
76        self.v += accel * DT;
77        self.v = self.v.clamp(MIN_SPEED, MAX_SPEED);
78    }
79}
80
81#[derive(Debug)]
82pub struct MpcResult {
83    pub controls: Vec<Vector2<f64>>,
84    pub predicted: Vec<Vector4<f64>>,
85}
86
87#[derive(Debug)]
88pub struct SimulationResult {
89    pub cx: Vec<f64>,
90    pub cy: Vec<f64>,
91    pub hist_x: Vec<f64>,
92    pub hist_y: Vec<f64>,
93    pub predicted_x: Vec<f64>,
94    pub predicted_y: Vec<f64>,
95    pub goal: (f64, f64),
96    pub reached_goal: bool,
97    pub target_index: usize,
98    pub final_index: usize,
99    pub final_state: State,
100}
101
102pub use rust_robotics_core::normalize_angle;
103
104fn angle_diff(a: f64, b: f64) -> f64 {
105    normalize_angle(a - b)
106}
107
108fn state_error(state: Vector4<f64>, reference: Vector4<f64>) -> Vector4<f64> {
109    let mut error = state - reference;
110    error[3] = angle_diff(state[3], reference[3]);
111    error
112}
113
114fn control_cost_weight() -> Matrix2<f64> {
115    Matrix2::from_diagonal(&Vector2::new(R[0], R[1]))
116}
117
118fn control_rate_weight() -> Matrix2<f64> {
119    Matrix2::from_diagonal(&Vector2::new(RD[0], RD[1]))
120}
121
122fn state_cost_weight() -> Matrix4<f64> {
123    Matrix4::from_diagonal(&Vector4::new(Q[0], Q[1], Q[2], Q[3]))
124}
125
126fn terminal_cost_weight() -> Matrix4<f64> {
127    Matrix4::from_diagonal(&Vector4::new(QF[0], QF[1], QF[2], QF[3]))
128}
129
130fn get_linear_model_matrix(
131    v: f64,
132    yaw: f64,
133    steer: f64,
134) -> (Matrix4<f64>, Matrix4x2<f64>, Vector4<f64>) {
135    let a = Matrix4::new(
136        1.0,
137        0.0,
138        DT * yaw.cos(),
139        -DT * v * yaw.sin(),
140        0.0,
141        1.0,
142        DT * yaw.sin(),
143        DT * v * yaw.cos(),
144        0.0,
145        0.0,
146        1.0,
147        0.0,
148        0.0,
149        0.0,
150        DT * steer.tan() / WB,
151        1.0,
152    );
153
154    let b = Matrix4x2::new(
155        0.0,
156        0.0,
157        0.0,
158        0.0,
159        DT,
160        0.0,
161        0.0,
162        DT * v / (WB * steer.cos().powi(2)),
163    );
164
165    let c = Vector4::new(
166        DT * v * yaw.sin() * yaw,
167        -DT * v * yaw.cos() * yaw,
168        0.0,
169        -DT * v * steer / (WB * steer.cos().powi(2)),
170    );
171
172    (a, b, c)
173}
174
175/// Cubic spline for reference path
176struct CubicSpline1D {
177    x: Vec<f64>,
178    a: Vec<f64>,
179    b: Vec<f64>,
180    c: Vec<f64>,
181    d: Vec<f64>,
182}
183
184impl CubicSpline1D {
185    fn new(x: &[f64], y: &[f64]) -> Self {
186        let n = x.len();
187        let a = y.to_vec();
188        let mut b = vec![0.0; n];
189        let mut c = vec![0.0; n];
190        let mut d = vec![0.0; n];
191
192        let h: Vec<f64> = (0..n - 1).map(|i| x[i + 1] - x[i]).collect();
193
194        let mut alpha = vec![0.0; n];
195        for i in 1..n - 1 {
196            alpha[i] = 3.0 / h[i] * (a[i + 1] - a[i]) - 3.0 / h[i - 1] * (a[i] - a[i - 1]);
197        }
198
199        let mut l = vec![1.0; n];
200        let mut mu = vec![0.0; n];
201        let mut z = vec![0.0; n];
202
203        for i in 1..n - 1 {
204            l[i] = 2.0 * (x[i + 1] - x[i - 1]) - h[i - 1] * mu[i - 1];
205            mu[i] = h[i] / l[i];
206            z[i] = (alpha[i] - h[i - 1] * z[i - 1]) / l[i];
207        }
208
209        for j in (0..n - 1).rev() {
210            c[j] = z[j] - mu[j] * c[j + 1];
211            b[j] = (a[j + 1] - a[j]) / h[j] - h[j] * (c[j + 1] + 2.0 * c[j]) / 3.0;
212            d[j] = (c[j + 1] - c[j]) / (3.0 * h[j]);
213        }
214
215        Self {
216            x: x.to_vec(),
217            a,
218            b,
219            c,
220            d,
221        }
222    }
223
224    fn calc(&self, t: f64) -> f64 {
225        let i = self.search_index(t);
226        let dx = t - self.x[i];
227        self.a[i] + self.b[i] * dx + self.c[i] * dx.powi(2) + self.d[i] * dx.powi(3)
228    }
229
230    fn calc_d(&self, t: f64) -> f64 {
231        let i = self.search_index(t);
232        let dx = t - self.x[i];
233        self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx.powi(2)
234    }
235
236    fn search_index(&self, t: f64) -> usize {
237        for i in 0..self.x.len() - 1 {
238            if self.x[i] <= t && t < self.x[i + 1] {
239                return i;
240            }
241        }
242        self.x.len() - 2
243    }
244}
245
246/// 2D Cubic spline path
247struct CubicSpline2D {
248    s: Vec<f64>,
249    sx: CubicSpline1D,
250    sy: CubicSpline1D,
251}
252
253impl CubicSpline2D {
254    fn new(x: &[f64], y: &[f64]) -> Self {
255        let mut s = vec![0.0];
256        for i in 1..x.len() {
257            let ds = ((x[i] - x[i - 1]).powi(2) + (y[i] - y[i - 1]).powi(2)).sqrt();
258            s.push(s[i - 1] + ds);
259        }
260
261        let sx = CubicSpline1D::new(&s, x);
262        let sy = CubicSpline1D::new(&s, y);
263
264        Self { s, sx, sy }
265    }
266
267    fn calc_position(&self, s: f64) -> (f64, f64) {
268        (self.sx.calc(s), self.sy.calc(s))
269    }
270
271    fn calc_yaw(&self, s: f64) -> f64 {
272        let dx = self.sx.calc_d(s);
273        let dy = self.sy.calc_d(s);
274        dy.atan2(dx)
275    }
276}
277
278fn smooth_yaw(yaw: &mut [f64]) {
279    for i in 0..yaw.len().saturating_sub(1) {
280        let mut dyaw = yaw[i + 1] - yaw[i];
281        while dyaw >= PI / 2.0 {
282            yaw[i + 1] -= 2.0 * PI;
283            dyaw = yaw[i + 1] - yaw[i];
284        }
285        while dyaw <= -PI / 2.0 {
286            yaw[i + 1] += 2.0 * PI;
287            dyaw = yaw[i + 1] - yaw[i];
288        }
289    }
290}
291
292pub fn calc_speed_profile(cx: &[f64], cy: &[f64], cyaw: &[f64], target_speed: f64) -> Vec<f64> {
293    let mut speed_profile = vec![target_speed; cx.len()];
294    let mut direction = 1.0;
295
296    for i in 0..cx.len().saturating_sub(1) {
297        let dx = cx[i + 1] - cx[i];
298        let dy = cy[i + 1] - cy[i];
299        let move_direction = dy.atan2(dx);
300
301        if dx.abs() > f64::EPSILON && dy.abs() > f64::EPSILON {
302            let dangle = angle_diff(move_direction, cyaw[i]).abs();
303            direction = if dangle >= PI / 4.0 { -1.0 } else { 1.0 };
304        }
305
306        speed_profile[i] = direction * target_speed;
307    }
308
309    if let Some(last) = speed_profile.last_mut() {
310        *last = 0.0;
311    }
312
313    speed_profile
314}
315
316fn calc_nearest_index(
317    state: &State,
318    cx: &[f64],
319    cy: &[f64],
320    cyaw: &[f64],
321    pind: usize,
322) -> (usize, f64) {
323    let start = pind.min(cx.len().saturating_sub(1));
324    let end = (start + N_IND_SEARCH).min(cx.len());
325
326    let mut best_index = start;
327    let mut best_distance_sq = f64::INFINITY;
328
329    for i in start..end {
330        let dx = state.x - cx[i];
331        let dy = state.y - cy[i];
332        let distance_sq = dx * dx + dy * dy;
333        if distance_sq < best_distance_sq {
334            best_distance_sq = distance_sq;
335            best_index = i;
336        }
337    }
338
339    let mut best_distance = best_distance_sq.sqrt();
340    let dxl = cx[best_index] - state.x;
341    let dyl = cy[best_index] - state.y;
342    let angle = angle_diff(cyaw[best_index], dyl.atan2(dxl));
343    if angle < 0.0 {
344        best_distance *= -1.0;
345    }
346
347    (best_index, best_distance)
348}
349
350fn calc_ref_trajectory(
351    state: &State,
352    cx: &[f64],
353    cy: &[f64],
354    cyaw: &[f64],
355    speed_profile: &[f64],
356    course_tick: f64,
357    pind: usize,
358) -> (Vec<Vector4<f64>>, usize) {
359    let ncourse = cx.len();
360    let (mut ind, _) = calc_nearest_index(state, cx, cy, cyaw, pind);
361    if pind >= ind {
362        ind = pind;
363    }
364
365    let mut xref = vec![Vector4::zeros(); T + 1];
366    xref[0] = Vector4::new(cx[ind], cy[ind], speed_profile[ind], cyaw[ind]);
367
368    let mut travel = 0.0;
369    for point in xref.iter_mut().take(T + 1).skip(1) {
370        travel += state.v.abs() * DT;
371        let dind = (travel / course_tick).round() as usize;
372        let index = (ind + dind).min(ncourse - 1);
373        *point = Vector4::new(cx[index], cy[index], speed_profile[index], cyaw[index]);
374    }
375
376    (xref, ind)
377}
378
379fn predict_motion(state: State, controls: &[Vector2<f64>]) -> Vec<Vector4<f64>> {
380    let mut predicted = vec![Vector4::zeros(); T + 1];
381    predicted[0] = state.to_vector();
382
383    let mut current = state;
384    for (i, control) in controls.iter().enumerate().take(T) {
385        current.update(control[0], control[1]);
386        predicted[i + 1] = current.to_vector();
387    }
388
389    predicted
390}
391
392fn apply_control_constraints(controls: &mut [Vector2<f64>]) {
393    for i in 0..controls.len() {
394        controls[i][0] = controls[i][0].clamp(-MAX_ACCEL, MAX_ACCEL);
395        controls[i][1] = controls[i][1].clamp(-MAX_STEER, MAX_STEER);
396
397        if i > 0 {
398            let delta = controls[i][1] - controls[i - 1][1];
399            let max_delta = MAX_DSTEER * DT;
400            if delta.abs() > max_delta {
401                controls[i][1] = controls[i - 1][1] + max_delta * delta.signum();
402            }
403        }
404    }
405}
406
407type LinearizedRollout = (
408    Vec<Vector4<f64>>,
409    Vec<Matrix4<f64>>,
410    Vec<Matrix4x2<f64>>,
411    Vec<Vector4<f64>>,
412);
413
414fn linearized_rollout(
415    x0: &State,
416    xbar: &[Vector4<f64>],
417    controls: &[Vector2<f64>],
418) -> LinearizedRollout {
419    let mut x = vec![Vector4::zeros(); T + 1];
420    let mut a_seq = Vec::with_capacity(T);
421    let mut b_seq = Vec::with_capacity(T);
422    let mut c_seq = Vec::with_capacity(T);
423
424    x[0] = x0.to_vector();
425    for t in 0..T {
426        let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
427        let mut next = a * x[t] + b * controls[t] + c;
428        next[3] = normalize_angle(next[3]);
429
430        a_seq.push(a);
431        b_seq.push(b);
432        c_seq.push(c);
433        x[t + 1] = next;
434    }
435
436    (x, a_seq, b_seq, c_seq)
437}
438
439fn compute_cost(x: &[Vector4<f64>], xref: &[Vector4<f64>], controls: &[Vector2<f64>]) -> f64 {
440    let q = state_cost_weight();
441    let qf = terminal_cost_weight();
442    let r = control_cost_weight();
443    let rd = control_rate_weight();
444
445    let mut cost = 0.0;
446    for t in 0..T {
447        cost += controls[t].dot(&(r * controls[t]));
448        if t != 0 {
449            let err = state_error(x[t], xref[t]);
450            cost += err.dot(&(q * err));
451        }
452        if t < T - 1 {
453            let du = controls[t + 1] - controls[t];
454            cost += du.dot(&(rd * du));
455        }
456    }
457
458    let terminal_error = state_error(x[T], xref[T]);
459    cost + terminal_error.dot(&(qf * terminal_error))
460}
461
462#[allow(clippy::needless_range_loop)]
463fn solve_qp_clarabel(
464    xref: &[Vector4<f64>],
465    xbar: &[Vector4<f64>],
466    state: &State,
467) -> Option<Vec<Vector2<f64>>> {
468    use clarabel::algebra::*;
469    use clarabel::solver::*;
470
471    let nu = 2;
472    let nx = 4;
473    let n_u = nu * T;
474    let n_x = nx * (T + 1);
475    let n = n_u + n_x;
476
477    let u_idx = |t: usize, k: usize| -> usize { t * nu + k };
478    let x_idx = |t: usize, k: usize| -> usize { n_u + t * nx + k };
479
480    // --- Build P (cost Hessian, upper triangular) ---
481    let mut p_rows = Vec::new();
482    let mut p_cols = Vec::new();
483    let mut p_vals = Vec::new();
484
485    for t in 0..T {
486        for k in 0..nu {
487            let idx = u_idx(t, k);
488            let mut val = R[k];
489            if t > 0 {
490                val += RD[k];
491            }
492            if t < T - 1 {
493                val += RD[k];
494            }
495            p_rows.push(idx);
496            p_cols.push(idx);
497            p_vals.push(val * 2.0);
498        }
499        if t < T - 1 {
500            for k in 0..nu {
501                let i = u_idx(t, k);
502                let j = u_idx(t + 1, k);
503                p_rows.push(i);
504                p_cols.push(j);
505                p_vals.push(-RD[k] * 2.0);
506            }
507        }
508    }
509
510    for t in 1..=T {
511        let w = if t == T { QF } else { Q };
512        for k in 0..nx {
513            let idx = x_idx(t, k);
514            p_rows.push(idx);
515            p_cols.push(idx);
516            p_vals.push(w[k] * 2.0);
517        }
518    }
519
520    let p = CscMatrix::new_from_triplets(n, n, p_rows, p_cols, p_vals);
521
522    // --- Build q (linear cost) ---
523    let mut q_vec = vec![0.0; n];
524    for t in 1..=T {
525        let w = if t == T { QF } else { Q };
526        for k in 0..nx {
527            q_vec[x_idx(t, k)] = -w[k] * 2.0 * xref[t][k];
528        }
529    }
530
531    // --- Build constraints ---
532    let n_eq = nx * (T + 1);
533    let n_steer_rate = T - 1;
534    let n_u_box = 2 * nu * T;
535    let n_steer_rate_box = 2 * n_steer_rate;
536    let n_v_box = 2 * (T + 1);
537    let n_ineq = n_u_box + n_steer_rate_box + n_v_box;
538
539    let mut a_rows = Vec::new();
540    let mut a_cols = Vec::new();
541    let mut a_vals = Vec::new();
542    let mut b_vec = vec![0.0; n_eq + n_ineq];
543
544    let x0 = state.to_vector();
545    for k in 0..nx {
546        a_rows.push(k);
547        a_cols.push(x_idx(0, k));
548        a_vals.push(1.0);
549        b_vec[k] = x0[k];
550    }
551
552    for t in 0..T {
553        let (a_mat, b_mat, c_vec) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
554        let base_row = nx * (t + 1);
555
556        for i in 0..nx {
557            let row = base_row + i;
558            a_rows.push(row);
559            a_cols.push(x_idx(t + 1, i));
560            a_vals.push(-1.0);
561            for j in 0..nx {
562                let val = a_mat[(i, j)];
563                if val.abs() > 1e-15 {
564                    a_rows.push(row);
565                    a_cols.push(x_idx(t, j));
566                    a_vals.push(val);
567                }
568            }
569            for j in 0..nu {
570                let val = b_mat[(i, j)];
571                if val.abs() > 1e-15 {
572                    a_rows.push(row);
573                    a_cols.push(u_idx(t, j));
574                    a_vals.push(val);
575                }
576            }
577            b_vec[row] = -c_vec[i];
578        }
579    }
580
581    let ineq_offset = n_eq;
582    let mut ineq_row = 0;
583
584    let u_bounds = [MAX_ACCEL, MAX_STEER];
585    for t in 0..T {
586        for (k, &bound) in u_bounds.iter().enumerate() {
587            let row = ineq_offset + ineq_row;
588            a_rows.push(row);
589            a_cols.push(u_idx(t, k));
590            a_vals.push(1.0);
591            b_vec[row] = bound;
592            ineq_row += 1;
593
594            let row = ineq_offset + ineq_row;
595            a_rows.push(row);
596            a_cols.push(u_idx(t, k));
597            a_vals.push(-1.0);
598            b_vec[row] = bound;
599            ineq_row += 1;
600        }
601    }
602
603    let max_steer_delta = MAX_DSTEER * DT;
604    for t in 0..(T - 1) {
605        let row = ineq_offset + ineq_row;
606        a_rows.push(row);
607        a_cols.push(u_idx(t + 1, 1));
608        a_vals.push(1.0);
609        a_rows.push(row);
610        a_cols.push(u_idx(t, 1));
611        a_vals.push(-1.0);
612        b_vec[row] = max_steer_delta;
613        ineq_row += 1;
614
615        let row = ineq_offset + ineq_row;
616        a_rows.push(row);
617        a_cols.push(u_idx(t + 1, 1));
618        a_vals.push(-1.0);
619        a_rows.push(row);
620        a_cols.push(u_idx(t, 1));
621        a_vals.push(1.0);
622        b_vec[row] = max_steer_delta;
623        ineq_row += 1;
624    }
625
626    for t in 0..=T {
627        let row = ineq_offset + ineq_row;
628        a_rows.push(row);
629        a_cols.push(x_idx(t, 2));
630        a_vals.push(1.0);
631        b_vec[row] = MAX_SPEED;
632        ineq_row += 1;
633
634        let row = ineq_offset + ineq_row;
635        a_rows.push(row);
636        a_cols.push(x_idx(t, 2));
637        a_vals.push(-1.0);
638        b_vec[row] = -MIN_SPEED;
639        ineq_row += 1;
640    }
641
642    let m = n_eq + n_ineq;
643    let a_csc = CscMatrix::new_from_triplets(m, n, a_rows, a_cols, a_vals);
644
645    let cones = vec![
646        SupportedConeT::ZeroConeT(n_eq),
647        SupportedConeT::NonnegativeConeT(n_ineq),
648    ];
649
650    let settings = DefaultSettingsBuilder::default()
651        .verbose(false)
652        .build()
653        // Default settings builder always succeeds
654        .expect("DefaultSettingsBuilder with defaults should not fail");
655
656    let mut solver = DefaultSolver::new(&p, &q_vec, &a_csc, &b_vec, &cones, settings).ok()?;
657    solver.solve();
658
659    if solver.solution.status != SolverStatus::Solved {
660        return None;
661    }
662
663    let z = &solver.solution.x;
664    let controls: Vec<Vector2<f64>> = (0..T)
665        .map(|t| Vector2::new(z[u_idx(t, 0)], z[u_idx(t, 1)]))
666        .collect();
667
668    Some(controls)
669}
670
671fn optimize_linearized_controls(
672    xref: &[Vector4<f64>],
673    xbar: &[Vector4<f64>],
674    state: &State,
675    initial_controls: &[Vector2<f64>],
676) -> Vec<Vector2<f64>> {
677    let pg_controls = optimize_linearized_controls_pg(xref, xbar, state, initial_controls);
678
679    // Adaptive solver selection: only invoke QP solver during reverse maneuvers
680    // where the projected-gradient solver struggles with the nonlinear coupling.
681    // During forward driving, PG alone is sufficient and faster.
682    let is_reverse = state.v < -0.1 || xref.iter().any(|x| x[2] < -0.1);
683    if is_reverse {
684        if let Some(qp_controls) = solve_qp_clarabel(xref, xbar, state) {
685            let (qp_x, _, _, _) = linearized_rollout(state, xbar, &qp_controls);
686            let qp_cost = compute_cost(&qp_x, xref, &qp_controls);
687
688            let (pg_x, _, _, _) = linearized_rollout(state, xbar, &pg_controls);
689            let pg_cost = compute_cost(&pg_x, xref, &pg_controls);
690
691            if qp_cost <= pg_cost {
692                return qp_controls;
693            }
694        }
695    }
696
697    pg_controls
698}
699
700fn optimize_linearized_controls_pg(
701    xref: &[Vector4<f64>],
702    xbar: &[Vector4<f64>],
703    state: &State,
704    initial_controls: &[Vector2<f64>],
705) -> Vec<Vector2<f64>> {
706    let q = state_cost_weight();
707    let qf = terminal_cost_weight();
708    let r = control_cost_weight();
709    let rd = control_rate_weight();
710
711    let mut controls = initial_controls.to_vec();
712    controls.resize(T, Vector2::zeros());
713    apply_control_constraints(&mut controls);
714
715    let mut previous_cost = f64::INFINITY;
716
717    for _ in 0..QP_MAX_ITERS {
718        let (x, a_seq, b_seq, _) = linearized_rollout(state, xbar, &controls);
719        let current_cost = compute_cost(&x, xref, &controls);
720
721        let mut gradients = [Vector2::zeros(); T];
722        let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
723
724        for t in (0..T).rev() {
725            let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
726
727            if t > 0 {
728                grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
729            }
730            if t < T - 1 {
731                grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
732            }
733
734            gradients[t] = grad;
735
736            let lx = if t == 0 {
737                Vector4::zeros()
738            } else {
739                (q * state_error(x[t], xref[t])) * 2.0
740            };
741            lambda = lx + a_seq[t].transpose() * lambda;
742        }
743
744        let gradient_norm = gradients
745            .iter()
746            .map(Vector2::norm_squared)
747            .sum::<f64>()
748            .sqrt();
749
750        if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
751            break;
752        }
753
754        previous_cost = current_cost;
755        let current_controls = controls.clone();
756        let mut best_candidate_controls = None;
757        let mut best_candidate_cost = current_cost;
758
759        let mut step = LINE_SEARCH_INITIAL_STEP;
760        for &large_step in &[1.0, 0.5] {
761            let mut candidate = current_controls.clone();
762            for t in 0..T {
763                candidate[t] -= gradients[t] * large_step;
764            }
765            apply_control_constraints(&mut candidate);
766
767            let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
768            let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
769            if candidate_cost < best_candidate_cost {
770                best_candidate_cost = candidate_cost;
771                best_candidate_controls = Some(candidate);
772            }
773        }
774
775        for _ in 0..LINE_SEARCH_ITERS {
776            let mut candidate = current_controls.clone();
777            for t in 0..T {
778                candidate[t] -= gradients[t] * step;
779            }
780            apply_control_constraints(&mut candidate);
781
782            let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
783            let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
784            if candidate_cost < best_candidate_cost {
785                best_candidate_cost = candidate_cost;
786                best_candidate_controls = Some(candidate);
787            }
788
789            step *= 0.5;
790        }
791
792        if let Some(candidate) = best_candidate_controls {
793            controls = candidate;
794        } else {
795            break;
796        }
797    }
798
799    controls
800}
801
802pub fn iterative_linear_mpc_control(
803    xref: &[Vector4<f64>],
804    state: &State,
805    warm_start: &[Vector2<f64>],
806) -> MpcResult {
807    let mut controls = warm_start.to_vec();
808    controls.resize(T, Vector2::zeros());
809    apply_control_constraints(&mut controls);
810
811    let mut predicted = predict_motion(*state, &controls);
812    for _ in 0..MAX_ITER {
813        let previous_controls = controls.clone();
814        controls = optimize_linearized_controls(xref, &predicted, state, &controls);
815        predicted = predict_motion(*state, &controls);
816
817        let du = controls
818            .iter()
819            .zip(previous_controls.iter())
820            .map(|(current, previous)| {
821                (current[0] - previous[0]).abs() + (current[1] - previous[1]).abs()
822            })
823            .sum::<f64>();
824
825        if du <= DU_TH {
826            break;
827        }
828    }
829
830    MpcResult {
831        controls,
832        predicted,
833    }
834}
835
836fn next_warm_start(controls: &[Vector2<f64>]) -> Vec<Vector2<f64>> {
837    let mut warm_start = controls[1..].to_vec();
838    warm_start.push(controls.last().copied().unwrap_or(Vector2::zeros()));
839    warm_start
840}
841
842pub fn check_goal(
843    state: &State,
844    goal: (f64, f64),
845    target_index: usize,
846    final_index: usize,
847) -> bool {
848    let dx = state.x - goal.0;
849    let dy = state.y - goal.1;
850    let distance = (dx * dx + dy * dy).sqrt();
851
852    let near_goal = distance <= GOAL_DIS;
853    let near_end = final_index.abs_diff(target_index) < 5;
854    let stopped = state.v.abs() <= STOP_SPEED;
855
856    near_goal && near_end && stopped
857}
858
859fn sample_reference_course(
860    ax: &[f64],
861    ay: &[f64],
862    course_tick: f64,
863) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
864    let csp = CubicSpline2D::new(ax, ay);
865    let s_max = *csp.s.last().unwrap_or(&0.0);
866
867    let mut cx = Vec::new();
868    let mut cy = Vec::new();
869    let mut cyaw = Vec::new();
870
871    let mut s = 0.0;
872    while s <= s_max {
873        let (x, y) = csp.calc_position(s);
874        cx.push(x);
875        cy.push(y);
876        cyaw.push(csp.calc_yaw(s));
877        s += course_tick;
878    }
879
880    if cx.is_empty() || cy.is_empty() || cyaw.is_empty() {
881        cx.push(ax[0]);
882        cy.push(ay[0]);
883        cyaw.push(0.0);
884    }
885
886    (cx, cy, cyaw)
887}
888
889pub fn generate_reference_course(ax: &[f64], ay: &[f64]) -> (Vec<f64>, Vec<f64>, Vec<f64>) {
890    let (cx, cy, mut cyaw) = sample_reference_course(ax, ay, PATH_RESOLUTION);
891
892    smooth_yaw(&mut cyaw);
893    (cx, cy, cyaw)
894}
895
896fn generate_switch_back_course() -> (Vec<f64>, Vec<f64>, Vec<f64>) {
897    let (mut cx, mut cy, mut cyaw) = sample_reference_course(
898        &[0.0, 30.0, 6.0, 20.0, 35.0],
899        &[0.0, 0.0, 20.0, 35.0, 20.0],
900        UPSTREAM_SWITCH_BACK_TICK,
901    );
902    let (cx2, cy2, mut cyaw2) = sample_reference_course(
903        &[35.0, 10.0, 0.0, 0.0],
904        &[20.0, 30.0, 5.0, 0.0],
905        UPSTREAM_SWITCH_BACK_TICK,
906    );
907
908    for yaw in &mut cyaw2 {
909        *yaw -= PI;
910    }
911
912    cx.extend(cx2);
913    cy.extend(cy2);
914    cyaw.extend(cyaw2);
915    smooth_yaw(&mut cyaw);
916
917    (cx, cy, cyaw)
918}
919
920pub fn run_mpc_simulation() -> SimulationResult {
921    let (cx, cy, cyaw) = generate_switch_back_course();
922    run_mpc_simulation_with_reference(cx, cy, cyaw, UPSTREAM_SWITCH_BACK_TICK, MAX_SIM_STEPS)
923}
924
925fn run_mpc_simulation_with_course(
926    ax: &[f64],
927    ay: &[f64],
928    max_sim_steps: usize,
929) -> SimulationResult {
930    let (cx, cy, cyaw) = generate_reference_course(ax, ay);
931    run_mpc_simulation_with_reference(cx, cy, cyaw, PATH_RESOLUTION, max_sim_steps)
932}
933
934fn run_mpc_simulation_with_reference(
935    cx: Vec<f64>,
936    cy: Vec<f64>,
937    cyaw: Vec<f64>,
938    course_tick: f64,
939    max_sim_steps: usize,
940) -> SimulationResult {
941    let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
942
943    let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
944    if state.yaw - cyaw[0] >= PI {
945        state.yaw -= 2.0 * PI;
946    } else if state.yaw - cyaw[0] <= -PI {
947        state.yaw += 2.0 * PI;
948    }
949
950    let mut target_index = 0;
951    let final_index = cx.len() - 1;
952    let goal = (cx[final_index], cy[final_index]);
953
954    let mut warm_start = vec![Vector2::zeros(); T];
955
956    let mut hist_x = vec![state.x];
957    let mut hist_y = vec![state.y];
958    let mut predicted = vec![state.to_vector()];
959    let mut reached_goal = false;
960
961    for _ in 0..max_sim_steps {
962        let (xref, new_index) = calc_ref_trajectory(
963            &state,
964            &cx,
965            &cy,
966            &cyaw,
967            &speed_profile,
968            course_tick,
969            target_index,
970        );
971        target_index = new_index;
972
973        let mpc_result = iterative_linear_mpc_control(&xref, &state, &warm_start);
974        let accel = mpc_result.controls[0][0];
975        let steer = mpc_result.controls[0][1];
976
977        state.update(accel, steer);
978        hist_x.push(state.x);
979        hist_y.push(state.y);
980        predicted = mpc_result.predicted;
981
982        warm_start = next_warm_start(&mpc_result.controls);
983
984        if check_goal(&state, goal, target_index, final_index) {
985            reached_goal = true;
986            break;
987        }
988    }
989
990    SimulationResult {
991        cx,
992        cy,
993        hist_x,
994        hist_y,
995        predicted_x: predicted.iter().map(|p| p[0]).collect(),
996        predicted_y: predicted.iter().map(|p| p[1]).collect(),
997        goal,
998        reached_goal,
999        target_index,
1000        final_index,
1001        final_state: state,
1002    }
1003}
1004
1005#[cfg(test)]
1006mod tests {
1007    use super::*;
1008    use std::collections::BTreeMap;
1009
1010    /// Original (pre-promotion) solver using first-improvement backtracking.
1011    /// Kept for characterization tests that compare default vs expanded behavior.
1012    fn optimize_linearized_controls_original(
1013        xref: &[Vector4<f64>],
1014        xbar: &[Vector4<f64>],
1015        state: &State,
1016        initial_controls: &[Vector2<f64>],
1017    ) -> Vec<Vector2<f64>> {
1018        let q = state_cost_weight();
1019        let qf = terminal_cost_weight();
1020        let r = control_cost_weight();
1021        let rd = control_rate_weight();
1022
1023        let mut controls = initial_controls.to_vec();
1024        controls.resize(T, Vector2::zeros());
1025        apply_control_constraints(&mut controls);
1026
1027        let mut previous_cost = f64::INFINITY;
1028
1029        for _ in 0..QP_MAX_ITERS {
1030            let (x, a_seq, b_seq, _) = linearized_rollout(state, xbar, &controls);
1031            let current_cost = compute_cost(&x, xref, &controls);
1032
1033            let mut gradients = [Vector2::zeros(); T];
1034            let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
1035
1036            for t in (0..T).rev() {
1037                let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1038
1039                if t > 0 {
1040                    grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1041                }
1042                if t < T - 1 {
1043                    grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1044                }
1045
1046                gradients[t] = grad;
1047
1048                let lx = if t == 0 {
1049                    Vector4::zeros()
1050                } else {
1051                    (q * state_error(x[t], xref[t])) * 2.0
1052                };
1053                lambda = lx + a_seq[t].transpose() * lambda;
1054            }
1055
1056            let gradient_norm = gradients
1057                .iter()
1058                .map(Vector2::norm_squared)
1059                .sum::<f64>()
1060                .sqrt();
1061
1062            if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1063                break;
1064            }
1065
1066            previous_cost = current_cost;
1067            let current_controls = controls.clone();
1068            let mut step = LINE_SEARCH_INITIAL_STEP;
1069            let mut improved = false;
1070
1071            for _ in 0..LINE_SEARCH_ITERS {
1072                let mut candidate = current_controls.clone();
1073                for t in 0..T {
1074                    candidate[t] -= gradients[t] * step;
1075                }
1076                apply_control_constraints(&mut candidate);
1077
1078                let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
1079                let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
1080                if candidate_cost < current_cost {
1081                    controls = candidate;
1082                    improved = true;
1083                    break;
1084                }
1085
1086                step *= 0.5;
1087            }
1088
1089            if !improved {
1090                break;
1091            }
1092        }
1093
1094        controls
1095    }
1096
1097    #[derive(Debug)]
1098    struct SwitchBackTraceRow {
1099        step: usize,
1100        target_index: usize,
1101        state: Vector4<f64>,
1102        xref_head: Vector4<f64>,
1103        xref_tail: Vector4<f64>,
1104        control: Vector2<f64>,
1105        predicted_tail: Vector4<f64>,
1106    }
1107
1108    #[derive(Debug)]
1109    struct SwitchBackDetailedRow {
1110        step: usize,
1111        target_index: usize,
1112        state: Vector4<f64>,
1113        xref: Vec<Vector4<f64>>,
1114        controls: Vec<Vector2<f64>>,
1115        predicted: Vec<Vector4<f64>>,
1116    }
1117
1118    #[derive(Debug)]
1119    struct SwitchBackPlanningSnapshot {
1120        step: usize,
1121        target_index: usize,
1122        state: Vector4<f64>,
1123        xref_head: Vector4<f64>,
1124        warm_start_head: Vector2<f64>,
1125    }
1126
1127    #[derive(Debug)]
1128    struct SwitchBackClosedLoopCheckpoint {
1129        step: usize,
1130        target_index: usize,
1131        goal_distance: f64,
1132        state: Vector4<f64>,
1133    }
1134
1135    #[derive(Debug)]
1136    struct SwitchBackGoalConditionSnapshot {
1137        step: usize,
1138        target_index: usize,
1139        goal_distance: f64,
1140        speed: f64,
1141        speed_abs: f64,
1142        heading_error: f64,
1143        motion_curvature: f64,
1144        near_goal: bool,
1145        near_end: bool,
1146        stopped: bool,
1147        reached_goal: bool,
1148    }
1149
1150    #[derive(Debug)]
1151    struct SwitchBackGoalControlSnapshot {
1152        step: usize,
1153        target_index: usize,
1154        goal_distance: f64,
1155        speed: f64,
1156        accel: f64,
1157        steer: f64,
1158        heading_error: f64,
1159        motion_curvature: f64,
1160        course_curvature: f64,
1161        near_goal: bool,
1162        near_end: bool,
1163        stopped: bool,
1164        reached_goal: bool,
1165    }
1166
1167    type SwitchBackControlTrace = BTreeMap<usize, Vec<Vector2<f64>>>;
1168    type SwitchBackStateTrace = BTreeMap<usize, Vec<Vector4<f64>>>;
1169    type SwitchBackHorizonPair = (Vec<Vector4<f64>>, Vec<Vector4<f64>>);
1170    type SwitchBackHorizonTrace = BTreeMap<usize, SwitchBackHorizonPair>;
1171    type SwitchBackWarmStartTrace = BTreeMap<usize, Vec<Vector2<f64>>>;
1172
1173    #[derive(Debug, Default)]
1174    struct StateInjectionMetrics {
1175        default_same_control0: f64,
1176        default_opposite_control0: f64,
1177        default_same_pred_y: f64,
1178        default_opposite_pred_y: f64,
1179        default_same_pred_v: f64,
1180        default_opposite_pred_v: f64,
1181        expanded_same_control0: f64,
1182        expanded_opposite_control0: f64,
1183        expanded_same_pred_y: f64,
1184        expanded_opposite_pred_y: f64,
1185        expanded_same_pred_v: f64,
1186        expanded_opposite_pred_v: f64,
1187    }
1188
1189    struct SwitchBackContext {
1190        cx: Vec<f64>,
1191        cy: Vec<f64>,
1192        cyaw: Vec<f64>,
1193        speed_profile: Vec<f64>,
1194        state: State,
1195        target_index: usize,
1196        warm_start: Vec<Vector2<f64>>,
1197    }
1198
1199    #[derive(Debug)]
1200    struct SwitchBackTraceDiffSummary {
1201        max_target_index_gap: usize,
1202        max_state_diff: [f64; 4],
1203        max_xref_head_diff: [f64; 4],
1204        max_xref_tail_diff: [f64; 4],
1205        max_control_diff: [f64; 2],
1206        max_predicted_tail_diff: [f64; 4],
1207    }
1208
1209    #[derive(Debug)]
1210    struct SwitchBackDetailedDiffSummary {
1211        max_target_index_gap: usize,
1212        max_state_diff: [f64; 4],
1213        max_xref_diff: [f64; 4],
1214        max_control_diff: [f64; 2],
1215        max_predicted_diff: [f64; 4],
1216    }
1217
1218    fn assert_vec2_close(actual: &Vector2<f64>, expected: [f64; 2], tol: f64) {
1219        for (i, expected_value) in expected.iter().enumerate() {
1220            assert!(
1221                (actual[i] - expected_value).abs() <= tol,
1222                "index {}: actual={} expected={} tol={}",
1223                i,
1224                actual[i],
1225                expected_value,
1226                tol
1227            );
1228        }
1229    }
1230
1231    fn assert_vec4_close(actual: &Vector4<f64>, expected: [f64; 4], tol: f64) {
1232        for (i, expected_value) in expected.iter().enumerate() {
1233            assert!(
1234                (actual[i] - expected_value).abs() <= tol,
1235                "index {}: actual={} expected={} tol={}",
1236                i,
1237                actual[i],
1238                expected_value,
1239                tol
1240            );
1241        }
1242    }
1243
1244    fn parse_switch_back_trace(csv: &str) -> Vec<SwitchBackTraceRow> {
1245        csv.lines()
1246            .skip(1)
1247            .filter(|line| !line.trim().is_empty())
1248            .map(|line| {
1249                let values: Vec<&str> = line.split(',').collect();
1250                assert_eq!(values.len(), 20);
1251
1252                SwitchBackTraceRow {
1253                    step: values[0].parse().unwrap(),
1254                    target_index: values[1].parse().unwrap(),
1255                    state: Vector4::new(
1256                        values[2].parse().unwrap(),
1257                        values[3].parse().unwrap(),
1258                        values[4].parse().unwrap(),
1259                        values[5].parse().unwrap(),
1260                    ),
1261                    xref_head: Vector4::new(
1262                        values[6].parse().unwrap(),
1263                        values[7].parse().unwrap(),
1264                        values[8].parse().unwrap(),
1265                        values[9].parse().unwrap(),
1266                    ),
1267                    xref_tail: Vector4::new(
1268                        values[10].parse().unwrap(),
1269                        values[11].parse().unwrap(),
1270                        values[12].parse().unwrap(),
1271                        values[13].parse().unwrap(),
1272                    ),
1273                    control: Vector2::new(values[14].parse().unwrap(), values[15].parse().unwrap()),
1274                    predicted_tail: Vector4::new(
1275                        values[16].parse().unwrap(),
1276                        values[17].parse().unwrap(),
1277                        values[18].parse().unwrap(),
1278                        values[19].parse().unwrap(),
1279                    ),
1280                }
1281            })
1282            .collect()
1283    }
1284
1285    fn parse_switch_back_reverse_controls(csv: &str) -> SwitchBackControlTrace {
1286        let mut result: SwitchBackControlTrace = BTreeMap::new();
1287
1288        for line in csv.lines().skip(1).filter(|line| !line.trim().is_empty()) {
1289            let values: Vec<&str> = line.split(',').collect();
1290            assert_eq!(values.len(), 4);
1291            let step: usize = values[0].parse().unwrap();
1292            let control_index: usize = values[1].parse().unwrap();
1293            let control = Vector2::new(values[2].parse().unwrap(), values[3].parse().unwrap());
1294
1295            let controls = result.entry(step).or_default();
1296            assert_eq!(controls.len(), control_index);
1297            controls.push(control);
1298        }
1299
1300        result
1301    }
1302
1303    fn parse_switch_back_reverse_states(csv: &str) -> SwitchBackStateTrace {
1304        let mut result: SwitchBackStateTrace = BTreeMap::new();
1305
1306        for line in csv.lines().skip(1).filter(|line| !line.trim().is_empty()) {
1307            let values: Vec<&str> = line.split(',').collect();
1308            assert_eq!(values.len(), 6);
1309            let step: usize = values[0].parse().unwrap();
1310            let horizon_index: usize = values[1].parse().unwrap();
1311            let state = Vector4::new(
1312                values[2].parse().unwrap(),
1313                values[3].parse().unwrap(),
1314                values[4].parse().unwrap(),
1315                values[5].parse().unwrap(),
1316            );
1317
1318            let states = result.entry(step).or_default();
1319            assert_eq!(states.len(), horizon_index);
1320            states.push(state);
1321        }
1322
1323        result
1324    }
1325
1326    fn parse_switch_back_reverse_horizon(csv: &str) -> SwitchBackHorizonTrace {
1327        let mut result: SwitchBackHorizonTrace = BTreeMap::new();
1328
1329        for line in csv.lines().skip(1).filter(|line| !line.trim().is_empty()) {
1330            let values: Vec<&str> = line.split(',').collect();
1331            assert_eq!(values.len(), 10);
1332            let step: usize = values[0].parse().unwrap();
1333            let horizon_index: usize = values[1].parse().unwrap();
1334
1335            let entry = result.entry(step).or_default();
1336            assert_eq!(entry.0.len(), horizon_index);
1337            assert_eq!(entry.1.len(), horizon_index);
1338
1339            entry.0.push(Vector4::new(
1340                values[2].parse().unwrap(),
1341                values[3].parse().unwrap(),
1342                values[4].parse().unwrap(),
1343                values[5].parse().unwrap(),
1344            ));
1345            entry.1.push(Vector4::new(
1346                values[6].parse().unwrap(),
1347                values[7].parse().unwrap(),
1348                values[8].parse().unwrap(),
1349                values[9].parse().unwrap(),
1350            ));
1351        }
1352
1353        result
1354    }
1355
1356    fn trace_state_diff(actual: &Vector4<f64>, expected: &Vector4<f64>) -> [f64; 4] {
1357        [
1358            (actual[0] - expected[0]).abs(),
1359            (actual[1] - expected[1]).abs(),
1360            (actual[2] - expected[2]).abs(),
1361            angle_diff(actual[3], expected[3]).abs(),
1362        ]
1363    }
1364
1365    fn trace_control_diff(actual: &Vector2<f64>, expected: &Vector2<f64>) -> [f64; 2] {
1366        [
1367            (actual[0] - expected[0]).abs(),
1368            (actual[1] - expected[1]).abs(),
1369        ]
1370    }
1371
1372    fn summarize_switch_back_trace_window(
1373        actual: &[SwitchBackTraceRow],
1374        expected: &[SwitchBackTraceRow],
1375    ) -> SwitchBackTraceDiffSummary {
1376        let mut summary = SwitchBackTraceDiffSummary {
1377            max_target_index_gap: 0,
1378            max_state_diff: [0.0; 4],
1379            max_xref_head_diff: [0.0; 4],
1380            max_xref_tail_diff: [0.0; 4],
1381            max_control_diff: [0.0; 2],
1382            max_predicted_tail_diff: [0.0; 4],
1383        };
1384
1385        assert_eq!(actual.len(), expected.len());
1386
1387        for (actual_row, expected_row) in actual.iter().zip(expected.iter()) {
1388            assert_eq!(actual_row.step, expected_row.step);
1389            summary.max_target_index_gap = summary
1390                .max_target_index_gap
1391                .max(actual_row.target_index.abs_diff(expected_row.target_index));
1392
1393            for (max_diff, diff) in summary
1394                .max_state_diff
1395                .iter_mut()
1396                .zip(trace_state_diff(&actual_row.state, &expected_row.state))
1397            {
1398                *max_diff = (*max_diff).max(diff);
1399            }
1400
1401            for (max_diff, diff) in summary.max_xref_head_diff.iter_mut().zip(trace_state_diff(
1402                &actual_row.xref_head,
1403                &expected_row.xref_head,
1404            )) {
1405                *max_diff = (*max_diff).max(diff);
1406            }
1407
1408            for (max_diff, diff) in summary.max_xref_tail_diff.iter_mut().zip(trace_state_diff(
1409                &actual_row.xref_tail,
1410                &expected_row.xref_tail,
1411            )) {
1412                *max_diff = (*max_diff).max(diff);
1413            }
1414
1415            for (max_diff, diff) in summary.max_control_diff.iter_mut().zip(trace_control_diff(
1416                &actual_row.control,
1417                &expected_row.control,
1418            )) {
1419                *max_diff = (*max_diff).max(diff);
1420            }
1421
1422            for (max_diff, diff) in
1423                summary
1424                    .max_predicted_tail_diff
1425                    .iter_mut()
1426                    .zip(trace_state_diff(
1427                        &actual_row.predicted_tail,
1428                        &expected_row.predicted_tail,
1429                    ))
1430            {
1431                *max_diff = (*max_diff).max(diff);
1432            }
1433        }
1434
1435        summary
1436    }
1437
1438    fn summarize_switch_back_detailed_window(
1439        actual_trace: &[SwitchBackDetailedRow],
1440        expected_trace: &[SwitchBackTraceRow],
1441        expected_controls: &SwitchBackControlTrace,
1442        expected_horizon: &SwitchBackHorizonTrace,
1443        step_window: std::ops::RangeInclusive<usize>,
1444    ) -> SwitchBackDetailedDiffSummary {
1445        let mut summary = SwitchBackDetailedDiffSummary {
1446            max_target_index_gap: 0,
1447            max_state_diff: [0.0; 4],
1448            max_xref_diff: [0.0; 4],
1449            max_control_diff: [0.0; 2],
1450            max_predicted_diff: [0.0; 4],
1451        };
1452
1453        for actual_row in actual_trace
1454            .iter()
1455            .filter(|row| step_window.contains(&row.step))
1456        {
1457            let expected_row = &expected_trace[actual_row.step];
1458            assert_eq!(actual_row.step, expected_row.step);
1459            summary.max_target_index_gap = summary
1460                .max_target_index_gap
1461                .max(actual_row.target_index.abs_diff(expected_row.target_index));
1462
1463            for (slot, diff) in summary
1464                .max_state_diff
1465                .iter_mut()
1466                .zip(trace_state_diff(&actual_row.state, &expected_row.state))
1467            {
1468                *slot = (*slot).max(diff);
1469            }
1470
1471            let expected_controls_for_step = expected_controls.get(&actual_row.step).unwrap();
1472            let (expected_xref_for_step, expected_predicted_for_step) =
1473                expected_horizon.get(&actual_row.step).unwrap();
1474            let expected_xref_rows = expected_xref_for_step
1475                .iter()
1476                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
1477                .collect::<Vec<_>>();
1478            let expected_control_rows = expected_controls_for_step
1479                .iter()
1480                .map(|vec| [vec[0], vec[1]])
1481                .collect::<Vec<_>>();
1482            let expected_predicted_rows = expected_predicted_for_step
1483                .iter()
1484                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
1485                .collect::<Vec<_>>();
1486
1487            for (slot, diff) in summary
1488                .max_xref_diff
1489                .iter_mut()
1490                .zip(summarize_vec4_sequence_diff(
1491                    &actual_row.xref,
1492                    &expected_xref_rows,
1493                ))
1494            {
1495                *slot = (*slot).max(diff);
1496            }
1497
1498            for (slot, diff) in
1499                summary
1500                    .max_control_diff
1501                    .iter_mut()
1502                    .zip(summarize_vec2_sequence_diff(
1503                        &actual_row.controls,
1504                        &expected_control_rows,
1505                    ))
1506            {
1507                *slot = (*slot).max(diff);
1508            }
1509
1510            for (slot, diff) in
1511                summary
1512                    .max_predicted_diff
1513                    .iter_mut()
1514                    .zip(summarize_vec4_sequence_diff(
1515                        &actual_row.predicted,
1516                        &expected_predicted_rows,
1517                    ))
1518            {
1519                *slot = (*slot).max(diff);
1520            }
1521        }
1522
1523        summary
1524    }
1525
1526    fn assert_diff_bounds<const N: usize>(label: &str, actual: [f64; N], tolerance: [f64; N]) {
1527        for (i, (actual_diff, bound)) in actual.into_iter().zip(tolerance).enumerate() {
1528            assert!(
1529                actual_diff <= bound,
1530                "{} diff index {}: actual={} tolerance={}",
1531                label,
1532                i,
1533                actual_diff,
1534                bound
1535            );
1536        }
1537    }
1538
1539    fn summarize_vec4_sequence_diff(actual: &[Vector4<f64>], expected: &[[f64; 4]]) -> [f64; 4] {
1540        assert_eq!(actual.len(), expected.len());
1541        let mut max_diff: [f64; 4] = [0.0; 4];
1542
1543        for (actual_vec, expected_vec) in actual.iter().zip(expected.iter()) {
1544            for (slot, diff) in max_diff.iter_mut().zip(trace_state_diff(
1545                actual_vec,
1546                &Vector4::from_row_slice(expected_vec),
1547            )) {
1548                *slot = (*slot).max(diff);
1549            }
1550        }
1551
1552        max_diff
1553    }
1554
1555    fn summarize_vec2_sequence_diff(actual: &[Vector2<f64>], expected: &[[f64; 2]]) -> [f64; 2] {
1556        assert_eq!(actual.len(), expected.len());
1557        let mut max_diff: [f64; 2] = [0.0; 2];
1558
1559        for (actual_vec, expected_vec) in actual.iter().zip(expected.iter()) {
1560            for (slot, diff) in max_diff.iter_mut().zip(trace_control_diff(
1561                actual_vec,
1562                &Vector2::from_row_slice(expected_vec),
1563            )) {
1564                *slot = (*slot).max(diff);
1565            }
1566        }
1567
1568        max_diff
1569    }
1570
1571    fn compute_cost_without_angle_wrap(
1572        x: &[Vector4<f64>],
1573        xref: &[Vector4<f64>],
1574        controls: &[Vector2<f64>],
1575    ) -> f64 {
1576        let q = state_cost_weight();
1577        let qf = terminal_cost_weight();
1578        let r = control_cost_weight();
1579        let rd = control_rate_weight();
1580
1581        let mut cost = 0.0;
1582        for t in 0..T {
1583            cost += controls[t].dot(&(r * controls[t]));
1584            if t != 0 {
1585                let err = x[t] - xref[t];
1586                cost += err.dot(&(q * err));
1587            }
1588            if t < T - 1 {
1589                let du = controls[t + 1] - controls[t];
1590                cost += du.dot(&(rd * du));
1591            }
1592        }
1593
1594        let terminal_error = x[T] - xref[T];
1595        cost + terminal_error.dot(&(qf * terminal_error))
1596    }
1597
1598    #[derive(Debug)]
1599    struct GradientTraceEntry {
1600        iter: usize,
1601        cost: f64,
1602        gradient_norm: f64,
1603        accepted_step: Option<f64>,
1604    }
1605
1606    #[derive(Debug)]
1607    struct LineSearchProjectionEntry {
1608        step: f64,
1609        raw_cost: f64,
1610        projected_cost: f64,
1611        raw_to_projected: [f64; 2],
1612        raw_to_python: [f64; 2],
1613        projected_to_python: [f64; 2],
1614    }
1615
1616    #[derive(Debug)]
1617    struct ProjectionTraceEntry {
1618        iter: usize,
1619        current_cost: f64,
1620        candidates: Vec<LineSearchProjectionEntry>,
1621    }
1622
1623    fn control_sequence_max_diff(actual: &[Vector2<f64>], expected: &[Vector2<f64>]) -> [f64; 2] {
1624        assert_eq!(actual.len(), expected.len());
1625        let mut max_diff: [f64; 2] = [0.0; 2];
1626
1627        for (actual_vec, expected_vec) in actual.iter().zip(expected.iter()) {
1628            let diff = trace_control_diff(actual_vec, expected_vec);
1629            max_diff[0] = max_diff[0].max(diff[0]);
1630            max_diff[1] = max_diff[1].max(diff[1]);
1631        }
1632
1633        max_diff
1634    }
1635
1636    fn linearized_rollout_without_angle_wrap(
1637        x0: &State,
1638        xbar: &[Vector4<f64>],
1639        controls: &[Vector2<f64>],
1640    ) -> Vec<Vector4<f64>> {
1641        let mut x = vec![Vector4::zeros(); T + 1];
1642        x[0] = x0.to_vector();
1643
1644        for t in 0..T {
1645            let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
1646            x[t + 1] = a * x[t] + b * controls[t] + c;
1647        }
1648
1649        x
1650    }
1651
1652    fn optimize_linearized_controls_without_angle_wrap_with_trace(
1653        xref: &[Vector4<f64>],
1654        xbar: &[Vector4<f64>],
1655        state: &State,
1656        initial_controls: &[Vector2<f64>],
1657        max_iters: usize,
1658        line_search_iters: usize,
1659    ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1660        optimize_linearized_controls_without_angle_wrap_with_trace_and_initial_step(
1661            xref,
1662            xbar,
1663            state,
1664            initial_controls,
1665            max_iters,
1666            line_search_iters,
1667            0.25,
1668        )
1669    }
1670
1671    fn optimize_linearized_controls_without_angle_wrap_with_trace_and_initial_step(
1672        xref: &[Vector4<f64>],
1673        xbar: &[Vector4<f64>],
1674        state: &State,
1675        initial_controls: &[Vector2<f64>],
1676        max_iters: usize,
1677        line_search_iters: usize,
1678        initial_step: f64,
1679    ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1680        let q = state_cost_weight();
1681        let qf = terminal_cost_weight();
1682        let r = control_cost_weight();
1683        let rd = control_rate_weight();
1684
1685        let mut controls = initial_controls.to_vec();
1686        controls.resize(T, Vector2::zeros());
1687        apply_control_constraints(&mut controls);
1688
1689        let mut previous_cost = f64::INFINITY;
1690        let mut trace = Vec::new();
1691
1692        for iter in 0..max_iters {
1693            let mut x = vec![Vector4::zeros(); T + 1];
1694            let mut a_seq = Vec::with_capacity(T);
1695            let mut b_seq = Vec::with_capacity(T);
1696            x[0] = state.to_vector();
1697
1698            for t in 0..T {
1699                let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
1700                x[t + 1] = a * x[t] + b * controls[t] + c;
1701                a_seq.push(a);
1702                b_seq.push(b);
1703            }
1704            let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
1705
1706            let mut gradients = [Vector2::zeros(); T];
1707            let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
1708
1709            for t in (0..T).rev() {
1710                let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1711
1712                if t > 0 {
1713                    grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1714                }
1715                if t < T - 1 {
1716                    grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1717                }
1718
1719                gradients[t] = grad;
1720
1721                let lx = if t == 0 {
1722                    Vector4::zeros()
1723                } else {
1724                    (q * (x[t] - xref[t])) * 2.0
1725                };
1726                lambda = lx + a_seq[t].transpose() * lambda;
1727            }
1728
1729            let gradient_norm = gradients
1730                .iter()
1731                .map(Vector2::norm_squared)
1732                .sum::<f64>()
1733                .sqrt();
1734
1735            if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1736                trace.push(GradientTraceEntry {
1737                    iter,
1738                    cost: current_cost,
1739                    gradient_norm,
1740                    accepted_step: None,
1741                });
1742                break;
1743            }
1744
1745            previous_cost = current_cost;
1746            let current_controls = controls.clone();
1747            let mut step = initial_step;
1748            let mut accepted_step = None;
1749
1750            for _ in 0..line_search_iters {
1751                let mut candidate = current_controls.clone();
1752                for t in 0..T {
1753                    candidate[t] -= gradients[t] * step;
1754                }
1755                apply_control_constraints(&mut candidate);
1756
1757                let candidate_x = linearized_rollout_without_angle_wrap(state, xbar, &candidate);
1758                let candidate_cost =
1759                    compute_cost_without_angle_wrap(&candidate_x, xref, &candidate);
1760                if candidate_cost < current_cost {
1761                    controls = candidate;
1762                    accepted_step = Some(step);
1763                    break;
1764                }
1765
1766                step *= 0.5;
1767            }
1768
1769            trace.push(GradientTraceEntry {
1770                iter,
1771                cost: current_cost,
1772                gradient_norm,
1773                accepted_step,
1774            });
1775
1776            if accepted_step.is_none() {
1777                break;
1778            }
1779        }
1780
1781        (controls, trace)
1782    }
1783
1784    fn optimize_linearized_controls_without_angle_wrap_with_candidate_expansion(
1785        xref: &[Vector4<f64>],
1786        xbar: &[Vector4<f64>],
1787        state: &State,
1788        initial_controls: &[Vector2<f64>],
1789        max_iters: usize,
1790        step_candidates: &[f64],
1791    ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1792        let q = state_cost_weight();
1793        let qf = terminal_cost_weight();
1794        let r = control_cost_weight();
1795        let rd = control_rate_weight();
1796
1797        let mut controls = initial_controls.to_vec();
1798        controls.resize(T, Vector2::zeros());
1799        apply_control_constraints(&mut controls);
1800
1801        let mut previous_cost = f64::INFINITY;
1802        let mut trace = Vec::new();
1803
1804        for iter in 0..max_iters {
1805            let mut x = vec![Vector4::zeros(); T + 1];
1806            let mut a_seq = Vec::with_capacity(T);
1807            let mut b_seq = Vec::with_capacity(T);
1808            x[0] = state.to_vector();
1809
1810            for t in 0..T {
1811                let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
1812                x[t + 1] = a * x[t] + b * controls[t] + c;
1813                a_seq.push(a);
1814                b_seq.push(b);
1815            }
1816            let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
1817
1818            let mut gradients = [Vector2::zeros(); T];
1819            let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
1820
1821            for t in (0..T).rev() {
1822                let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1823
1824                if t > 0 {
1825                    grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1826                }
1827                if t < T - 1 {
1828                    grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1829                }
1830
1831                gradients[t] = grad;
1832
1833                let lx = if t == 0 {
1834                    Vector4::zeros()
1835                } else {
1836                    (q * (x[t] - xref[t])) * 2.0
1837                };
1838                lambda = lx + a_seq[t].transpose() * lambda;
1839            }
1840
1841            let gradient_norm = gradients
1842                .iter()
1843                .map(Vector2::norm_squared)
1844                .sum::<f64>()
1845                .sqrt();
1846
1847            if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1848                trace.push(GradientTraceEntry {
1849                    iter,
1850                    cost: current_cost,
1851                    gradient_norm,
1852                    accepted_step: None,
1853                });
1854                break;
1855            }
1856
1857            previous_cost = current_cost;
1858            let current_controls = controls.clone();
1859            let mut accepted_step = None;
1860            let mut best_candidate_controls = None;
1861            let mut best_candidate_cost = current_cost;
1862
1863            for &step in step_candidates {
1864                let mut candidate = current_controls.clone();
1865                for t in 0..T {
1866                    candidate[t] -= gradients[t] * step;
1867                }
1868                apply_control_constraints(&mut candidate);
1869
1870                let candidate_x = linearized_rollout_without_angle_wrap(state, xbar, &candidate);
1871                let candidate_cost =
1872                    compute_cost_without_angle_wrap(&candidate_x, xref, &candidate);
1873                if candidate_cost < best_candidate_cost {
1874                    best_candidate_cost = candidate_cost;
1875                    accepted_step = Some(step);
1876                    best_candidate_controls = Some(candidate);
1877                }
1878            }
1879
1880            trace.push(GradientTraceEntry {
1881                iter,
1882                cost: current_cost,
1883                gradient_norm,
1884                accepted_step,
1885            });
1886
1887            if let Some(candidate) = best_candidate_controls {
1888                controls = candidate;
1889            } else {
1890                break;
1891            }
1892        }
1893
1894        (controls, trace)
1895    }
1896
1897    fn optimize_linearized_controls_with_candidate_expansion(
1898        xref: &[Vector4<f64>],
1899        xbar: &[Vector4<f64>],
1900        state: &State,
1901        initial_controls: &[Vector2<f64>],
1902        max_iters: usize,
1903        step_candidates: &[f64],
1904    ) -> (Vec<Vector2<f64>>, Vec<GradientTraceEntry>) {
1905        let q = state_cost_weight();
1906        let qf = terminal_cost_weight();
1907        let r = control_cost_weight();
1908        let rd = control_rate_weight();
1909
1910        let mut controls = initial_controls.to_vec();
1911        controls.resize(T, Vector2::zeros());
1912        apply_control_constraints(&mut controls);
1913
1914        let mut previous_cost = f64::INFINITY;
1915        let mut trace = Vec::new();
1916
1917        for iter in 0..max_iters {
1918            let (x, a_seq, b_seq, _) = linearized_rollout(state, xbar, &controls);
1919            let current_cost = compute_cost(&x, xref, &controls);
1920
1921            let mut gradients = [Vector2::zeros(); T];
1922            let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
1923
1924            for t in (0..T).rev() {
1925                let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
1926
1927                if t > 0 {
1928                    grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
1929                }
1930                if t < T - 1 {
1931                    grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
1932                }
1933
1934                gradients[t] = grad;
1935
1936                let lx = if t == 0 {
1937                    Vector4::zeros()
1938                } else {
1939                    (q * state_error(x[t], xref[t])) * 2.0
1940                };
1941                lambda = lx + a_seq[t].transpose() * lambda;
1942            }
1943
1944            let gradient_norm = gradients
1945                .iter()
1946                .map(Vector2::norm_squared)
1947                .sum::<f64>()
1948                .sqrt();
1949
1950            if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
1951                trace.push(GradientTraceEntry {
1952                    iter,
1953                    cost: current_cost,
1954                    gradient_norm,
1955                    accepted_step: None,
1956                });
1957                break;
1958            }
1959
1960            previous_cost = current_cost;
1961            let current_controls = controls.clone();
1962            let mut accepted_step = None;
1963            let mut best_candidate_controls = None;
1964            let mut best_candidate_cost = current_cost;
1965
1966            for &step in step_candidates {
1967                let mut candidate = current_controls.clone();
1968                for t in 0..T {
1969                    candidate[t] -= gradients[t] * step;
1970                }
1971                apply_control_constraints(&mut candidate);
1972
1973                let (candidate_x, _, _, _) = linearized_rollout(state, xbar, &candidate);
1974                let candidate_cost = compute_cost(&candidate_x, xref, &candidate);
1975                if candidate_cost < best_candidate_cost {
1976                    best_candidate_cost = candidate_cost;
1977                    accepted_step = Some(step);
1978                    best_candidate_controls = Some(candidate);
1979                }
1980            }
1981
1982            trace.push(GradientTraceEntry {
1983                iter,
1984                cost: current_cost,
1985                gradient_norm,
1986                accepted_step,
1987            });
1988
1989            if let Some(candidate) = best_candidate_controls {
1990                controls = candidate;
1991            } else {
1992                break;
1993            }
1994        }
1995
1996        (controls, trace)
1997    }
1998
1999    fn expanded_backtracking_step_candidates() -> Vec<f64> {
2000        let mut steps = vec![1.0, 0.5];
2001        let mut step = LINE_SEARCH_INITIAL_STEP;
2002        for _ in 0..LINE_SEARCH_ITERS {
2003            steps.push(step);
2004            step *= 0.5;
2005        }
2006        steps
2007    }
2008
2009    fn optimize_linearized_controls_with_full_candidate_expansion(
2010        xref: &[Vector4<f64>],
2011        xbar: &[Vector4<f64>],
2012        state: &State,
2013        initial_controls: &[Vector2<f64>],
2014    ) -> Vec<Vector2<f64>> {
2015        optimize_linearized_controls_with_candidate_expansion(
2016            xref,
2017            xbar,
2018            state,
2019            initial_controls,
2020            QP_MAX_ITERS,
2021            &expanded_backtracking_step_candidates(),
2022        )
2023        .0
2024    }
2025
2026    fn iterative_linear_mpc_control_with_test_optimizer<F>(
2027        xref: &[Vector4<f64>],
2028        state: &State,
2029        warm_start: &[Vector2<f64>],
2030        optimize: F,
2031    ) -> MpcResult
2032    where
2033        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>,
2034    {
2035        let mut controls = warm_start.to_vec();
2036        controls.resize(T, Vector2::zeros());
2037        apply_control_constraints(&mut controls);
2038
2039        let mut predicted = predict_motion(*state, &controls);
2040        for _ in 0..MAX_ITER {
2041            let previous_controls = controls.clone();
2042            controls = optimize(xref, &predicted, state, &controls);
2043            predicted = predict_motion(*state, &controls);
2044
2045            let du = controls
2046                .iter()
2047                .zip(previous_controls.iter())
2048                .map(|(current, previous)| {
2049                    (current[0] - previous[0]).abs() + (current[1] - previous[1]).abs()
2050                })
2051                .sum::<f64>();
2052
2053            if du <= DU_TH {
2054                break;
2055            }
2056        }
2057
2058        MpcResult {
2059            controls,
2060            predicted,
2061        }
2062    }
2063
2064    fn trace_projection_effect_against_python_reference(
2065        xref: &[Vector4<f64>],
2066        xbar: &[Vector4<f64>],
2067        state: &State,
2068        initial_controls: &[Vector2<f64>],
2069        python_controls: &[Vector2<f64>],
2070        max_iters: usize,
2071        line_search_iters: usize,
2072    ) -> Vec<ProjectionTraceEntry> {
2073        let q = state_cost_weight();
2074        let qf = terminal_cost_weight();
2075        let r = control_cost_weight();
2076        let rd = control_rate_weight();
2077
2078        let mut controls = initial_controls.to_vec();
2079        controls.resize(T, Vector2::zeros());
2080        apply_control_constraints(&mut controls);
2081
2082        let mut previous_cost = f64::INFINITY;
2083        let mut trace = Vec::new();
2084
2085        for iter in 0..max_iters {
2086            let mut x = vec![Vector4::zeros(); T + 1];
2087            let mut a_seq = Vec::with_capacity(T);
2088            let mut b_seq = Vec::with_capacity(T);
2089            x[0] = state.to_vector();
2090
2091            for t in 0..T {
2092                let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
2093                x[t + 1] = a * x[t] + b * controls[t] + c;
2094                a_seq.push(a);
2095                b_seq.push(b);
2096            }
2097            let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
2098
2099            let mut gradients = [Vector2::zeros(); T];
2100            let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
2101
2102            for t in (0..T).rev() {
2103                let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
2104
2105                if t > 0 {
2106                    grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
2107                }
2108                if t < T - 1 {
2109                    grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
2110                }
2111
2112                gradients[t] = grad;
2113
2114                let lx = if t == 0 {
2115                    Vector4::zeros()
2116                } else {
2117                    (q * (x[t] - xref[t])) * 2.0
2118                };
2119                lambda = lx + a_seq[t].transpose() * lambda;
2120            }
2121
2122            let gradient_norm = gradients
2123                .iter()
2124                .map(Vector2::norm_squared)
2125                .sum::<f64>()
2126                .sqrt();
2127
2128            if gradient_norm <= GRAD_TOL || (previous_cost - current_cost).abs() <= COST_TOL {
2129                break;
2130            }
2131
2132            previous_cost = current_cost;
2133            let current_controls = controls.clone();
2134            let mut step = 0.25;
2135            let mut candidates = Vec::new();
2136            let mut improved = false;
2137
2138            for _ in 0..line_search_iters {
2139                let mut raw_candidate = current_controls.clone();
2140                for t in 0..T {
2141                    raw_candidate[t] -= gradients[t] * step;
2142                }
2143
2144                let raw_x = linearized_rollout_without_angle_wrap(state, xbar, &raw_candidate);
2145                let raw_cost = compute_cost_without_angle_wrap(&raw_x, xref, &raw_candidate);
2146
2147                let mut projected_candidate = raw_candidate.clone();
2148                apply_control_constraints(&mut projected_candidate);
2149                let projected_x =
2150                    linearized_rollout_without_angle_wrap(state, xbar, &projected_candidate);
2151                let projected_cost =
2152                    compute_cost_without_angle_wrap(&projected_x, xref, &projected_candidate);
2153
2154                candidates.push(LineSearchProjectionEntry {
2155                    step,
2156                    raw_cost,
2157                    projected_cost,
2158                    raw_to_projected: control_sequence_max_diff(
2159                        &raw_candidate,
2160                        &projected_candidate,
2161                    ),
2162                    raw_to_python: control_sequence_max_diff(&raw_candidate, python_controls),
2163                    projected_to_python: control_sequence_max_diff(
2164                        &projected_candidate,
2165                        python_controls,
2166                    ),
2167                });
2168
2169                if projected_cost < current_cost {
2170                    controls = projected_candidate;
2171                    improved = true;
2172                    break;
2173                }
2174
2175                step *= 0.5;
2176            }
2177
2178            trace.push(ProjectionTraceEntry {
2179                iter,
2180                current_cost,
2181                candidates,
2182            });
2183
2184            if !improved {
2185                break;
2186            }
2187        }
2188
2189        trace
2190    }
2191
2192    fn first_iteration_projection_candidates(
2193        xref: &[Vector4<f64>],
2194        xbar: &[Vector4<f64>],
2195        state: &State,
2196        initial_controls: &[Vector2<f64>],
2197        python_controls: &[Vector2<f64>],
2198        step_sizes: &[f64],
2199    ) -> (f64, Vec<LineSearchProjectionEntry>) {
2200        let q = state_cost_weight();
2201        let qf = terminal_cost_weight();
2202        let r = control_cost_weight();
2203        let rd = control_rate_weight();
2204
2205        let mut controls = initial_controls.to_vec();
2206        controls.resize(T, Vector2::zeros());
2207        apply_control_constraints(&mut controls);
2208
2209        let mut x = vec![Vector4::zeros(); T + 1];
2210        let mut a_seq = Vec::with_capacity(T);
2211        let mut b_seq = Vec::with_capacity(T);
2212        x[0] = state.to_vector();
2213
2214        for t in 0..T {
2215            let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
2216            x[t + 1] = a * x[t] + b * controls[t] + c;
2217            a_seq.push(a);
2218            b_seq.push(b);
2219        }
2220
2221        let current_cost = compute_cost_without_angle_wrap(&x, xref, &controls);
2222        let mut gradients = [Vector2::zeros(); T];
2223        let mut lambda = (qf * (x[T] - xref[T])) * 2.0;
2224
2225        for t in (0..T).rev() {
2226            let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
2227
2228            if t > 0 {
2229                grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
2230            }
2231            if t < T - 1 {
2232                grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
2233            }
2234
2235            gradients[t] = grad;
2236
2237            let lx = if t == 0 {
2238                Vector4::zeros()
2239            } else {
2240                (q * (x[t] - xref[t])) * 2.0
2241            };
2242            lambda = lx + a_seq[t].transpose() * lambda;
2243        }
2244
2245        let mut candidates = Vec::new();
2246        for &step in step_sizes {
2247            let mut raw_candidate = controls.clone();
2248            for t in 0..T {
2249                raw_candidate[t] -= gradients[t] * step;
2250            }
2251
2252            let raw_x = linearized_rollout_without_angle_wrap(state, xbar, &raw_candidate);
2253            let raw_cost = compute_cost_without_angle_wrap(&raw_x, xref, &raw_candidate);
2254
2255            let mut projected_candidate = raw_candidate.clone();
2256            apply_control_constraints(&mut projected_candidate);
2257            let projected_x =
2258                linearized_rollout_without_angle_wrap(state, xbar, &projected_candidate);
2259            let projected_cost =
2260                compute_cost_without_angle_wrap(&projected_x, xref, &projected_candidate);
2261
2262            candidates.push(LineSearchProjectionEntry {
2263                step,
2264                raw_cost,
2265                projected_cost,
2266                raw_to_projected: control_sequence_max_diff(&raw_candidate, &projected_candidate),
2267                raw_to_python: control_sequence_max_diff(&raw_candidate, python_controls),
2268                projected_to_python: control_sequence_max_diff(
2269                    &projected_candidate,
2270                    python_controls,
2271                ),
2272            });
2273        }
2274
2275        (current_cost, candidates)
2276    }
2277
2278    fn state_from_vector4(state: &Vector4<f64>) -> State {
2279        State::new(state[0], state[1], state[2], state[3])
2280    }
2281
2282    fn inject_state_components(
2283        base: &Vector4<f64>,
2284        injected: &Vector4<f64>,
2285        component_indices: &[usize],
2286    ) -> State {
2287        let mut hybrid = *base;
2288        for index in component_indices {
2289            hybrid[*index] = injected[*index];
2290        }
2291        state_from_vector4(&hybrid)
2292    }
2293
2294    fn measure_switch_back_state_injection(indices: &[usize]) -> StateInjectionMetrics {
2295        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2296            330,
2297            optimize_linearized_controls_original,
2298        );
2299        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2300            330,
2301            optimize_linearized_controls_with_full_candidate_expansion,
2302        );
2303        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2304            330,
2305            optimize_linearized_controls_original,
2306        );
2307        let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2308            330,
2309            optimize_linearized_controls_with_full_candidate_expansion,
2310        );
2311
2312        let mut summary = StateInjectionMetrics::default();
2313        for metrics in measure_switch_back_state_injection_by_step_with_context(
2314            indices,
2315            &[325usize, 326, 327, 328, 329],
2316            &default_trace,
2317            &expanded_trace,
2318            &default_warm_start,
2319            &expanded_warm_start,
2320        )
2321        .into_values()
2322        {
2323            summary.default_same_control0 += metrics.default_same_control0;
2324            summary.default_opposite_control0 += metrics.default_opposite_control0;
2325            summary.default_same_pred_y += metrics.default_same_pred_y;
2326            summary.default_opposite_pred_y += metrics.default_opposite_pred_y;
2327            summary.default_same_pred_v += metrics.default_same_pred_v;
2328            summary.default_opposite_pred_v += metrics.default_opposite_pred_v;
2329            summary.expanded_same_control0 += metrics.expanded_same_control0;
2330            summary.expanded_opposite_control0 += metrics.expanded_opposite_control0;
2331            summary.expanded_same_pred_y += metrics.expanded_same_pred_y;
2332            summary.expanded_opposite_pred_y += metrics.expanded_opposite_pred_y;
2333            summary.expanded_same_pred_v += metrics.expanded_same_pred_v;
2334            summary.expanded_opposite_pred_v += metrics.expanded_opposite_pred_v;
2335        }
2336
2337        summary
2338    }
2339
2340    fn measure_switch_back_state_injection_by_step_with_context(
2341        indices: &[usize],
2342        steps: &[usize],
2343        default_trace: &[SwitchBackDetailedRow],
2344        expanded_trace: &[SwitchBackDetailedRow],
2345        default_warm_start: &SwitchBackWarmStartTrace,
2346        expanded_warm_start: &SwitchBackWarmStartTrace,
2347    ) -> BTreeMap<usize, StateInjectionMetrics> {
2348        let mut by_step = BTreeMap::new();
2349
2350        for step in steps {
2351            let mut metrics = StateInjectionMetrics::default();
2352            let default_row = detailed_row_at_step(default_trace, *step);
2353            let expanded_row = detailed_row_at_step(expanded_trace, *step);
2354            let default_injected = iterative_linear_mpc_control_with_test_optimizer(
2355                &default_row.xref,
2356                &inject_state_components(&default_row.state, &expanded_row.state, indices),
2357                default_warm_start.get(step).unwrap(),
2358                optimize_linearized_controls_original,
2359            );
2360            let expanded_injected = iterative_linear_mpc_control_with_test_optimizer(
2361                &expanded_row.xref,
2362                &inject_state_components(&expanded_row.state, &default_row.state, indices),
2363                expanded_warm_start.get(step).unwrap(),
2364                optimize_linearized_controls_with_full_candidate_expansion,
2365            );
2366
2367            let default_baseline_controls = default_row
2368                .controls
2369                .iter()
2370                .map(|vec| [vec[0], vec[1]])
2371                .collect::<Vec<_>>();
2372            let expanded_baseline_controls = expanded_row
2373                .controls
2374                .iter()
2375                .map(|vec| [vec[0], vec[1]])
2376                .collect::<Vec<_>>();
2377            let default_baseline_predicted = default_row
2378                .predicted
2379                .iter()
2380                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
2381                .collect::<Vec<_>>();
2382            let expanded_baseline_predicted = expanded_row
2383                .predicted
2384                .iter()
2385                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
2386                .collect::<Vec<_>>();
2387
2388            let default_to_default_control = summarize_vec2_sequence_diff(
2389                &default_injected.controls,
2390                &default_baseline_controls,
2391            );
2392            let default_to_expanded_control = summarize_vec2_sequence_diff(
2393                &default_injected.controls,
2394                &expanded_baseline_controls,
2395            );
2396            let default_to_default_predicted = summarize_vec4_sequence_diff(
2397                &default_injected.predicted,
2398                &default_baseline_predicted,
2399            );
2400            let default_to_expanded_predicted = summarize_vec4_sequence_diff(
2401                &default_injected.predicted,
2402                &expanded_baseline_predicted,
2403            );
2404
2405            let expanded_to_expanded_control = summarize_vec2_sequence_diff(
2406                &expanded_injected.controls,
2407                &expanded_baseline_controls,
2408            );
2409            let expanded_to_default_control = summarize_vec2_sequence_diff(
2410                &expanded_injected.controls,
2411                &default_baseline_controls,
2412            );
2413            let expanded_to_expanded_predicted = summarize_vec4_sequence_diff(
2414                &expanded_injected.predicted,
2415                &expanded_baseline_predicted,
2416            );
2417            let expanded_to_default_predicted = summarize_vec4_sequence_diff(
2418                &expanded_injected.predicted,
2419                &default_baseline_predicted,
2420            );
2421
2422            metrics.default_same_control0 = default_to_default_control[0];
2423            metrics.default_opposite_control0 = default_to_expanded_control[0];
2424            metrics.default_same_pred_y = default_to_default_predicted[1];
2425            metrics.default_opposite_pred_y = default_to_expanded_predicted[1];
2426            metrics.default_same_pred_v = default_to_default_predicted[2];
2427            metrics.default_opposite_pred_v = default_to_expanded_predicted[2];
2428
2429            metrics.expanded_same_control0 = expanded_to_expanded_control[0];
2430            metrics.expanded_opposite_control0 = expanded_to_default_control[0];
2431            metrics.expanded_same_pred_y = expanded_to_expanded_predicted[1];
2432            metrics.expanded_opposite_pred_y = expanded_to_default_predicted[1];
2433            metrics.expanded_same_pred_v = expanded_to_expanded_predicted[2];
2434            metrics.expanded_opposite_pred_v = expanded_to_default_predicted[2];
2435
2436            by_step.insert(*step, metrics);
2437        }
2438
2439        by_step
2440    }
2441
2442    fn measure_switch_back_state_injection_by_step(
2443        indices: &[usize],
2444        steps: &[usize],
2445    ) -> BTreeMap<usize, StateInjectionMetrics> {
2446        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2447            330,
2448            optimize_linearized_controls_original,
2449        );
2450        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
2451            330,
2452            optimize_linearized_controls_with_full_candidate_expansion,
2453        );
2454        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2455            330,
2456            optimize_linearized_controls_original,
2457        );
2458        let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
2459            330,
2460            optimize_linearized_controls_with_full_candidate_expansion,
2461        );
2462
2463        measure_switch_back_state_injection_by_step_with_context(
2464            indices,
2465            steps,
2466            &default_trace,
2467            &expanded_trace,
2468            &default_warm_start,
2469            &expanded_warm_start,
2470        )
2471    }
2472
2473    fn state_injection_metrics_at_step(
2474        metrics: &BTreeMap<usize, StateInjectionMetrics>,
2475        step: usize,
2476    ) -> &StateInjectionMetrics {
2477        metrics
2478            .get(&step)
2479            .unwrap_or_else(|| panic!("missing state-injection metrics for step {step}"))
2480    }
2481
2482    fn advance_switch_back_context(steps: usize) -> SwitchBackContext {
2483        let (cx, cy, cyaw) = generate_switch_back_course();
2484        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2485
2486        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2487        if state.yaw - cyaw[0] >= PI {
2488            state.yaw -= 2.0 * PI;
2489        } else if state.yaw - cyaw[0] <= -PI {
2490            state.yaw += 2.0 * PI;
2491        }
2492
2493        let mut target_index = 0;
2494        let mut warm_start = vec![Vector2::zeros(); T];
2495
2496        for _ in 0..steps {
2497            let (xref, new_index) = calc_ref_trajectory(
2498                &state,
2499                &cx,
2500                &cy,
2501                &cyaw,
2502                &speed_profile,
2503                UPSTREAM_SWITCH_BACK_TICK,
2504                target_index,
2505            );
2506            target_index = new_index;
2507
2508            let mpc_result = iterative_linear_mpc_control(&xref, &state, &warm_start);
2509            state.update(mpc_result.controls[0][0], mpc_result.controls[0][1]);
2510
2511            warm_start = next_warm_start(&mpc_result.controls);
2512        }
2513
2514        SwitchBackContext {
2515            cx,
2516            cy,
2517            cyaw,
2518            speed_profile,
2519            state,
2520            target_index,
2521            warm_start,
2522        }
2523    }
2524
2525    fn collect_switch_back_prefix_trace(steps: usize) -> Vec<SwitchBackTraceRow> {
2526        let (cx, cy, cyaw) = generate_switch_back_course();
2527        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2528
2529        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2530        if state.yaw - cyaw[0] >= PI {
2531            state.yaw -= 2.0 * PI;
2532        } else if state.yaw - cyaw[0] <= -PI {
2533            state.yaw += 2.0 * PI;
2534        }
2535
2536        let mut target_index = 0;
2537        let mut warm_start = vec![Vector2::zeros(); T];
2538        let mut trace = Vec::with_capacity(steps);
2539
2540        for step in 0..steps {
2541            let (xref, new_index) = calc_ref_trajectory(
2542                &state,
2543                &cx,
2544                &cy,
2545                &cyaw,
2546                &speed_profile,
2547                UPSTREAM_SWITCH_BACK_TICK,
2548                target_index,
2549            );
2550            target_index = new_index;
2551
2552            let result = iterative_linear_mpc_control(&xref, &state, &warm_start);
2553            trace.push(SwitchBackTraceRow {
2554                step,
2555                target_index,
2556                state: state.to_vector(),
2557                xref_head: xref[0],
2558                xref_tail: *xref.last().unwrap(),
2559                control: result.controls[0],
2560                predicted_tail: *result.predicted.last().unwrap(),
2561            });
2562
2563            state.update(result.controls[0][0], result.controls[0][1]);
2564            warm_start = next_warm_start(&result.controls);
2565        }
2566
2567        trace
2568    }
2569
2570    fn collect_switch_back_detailed_trace_with_test_optimizer<F>(
2571        steps: usize,
2572        optimize: F,
2573    ) -> Vec<SwitchBackDetailedRow>
2574    where
2575        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2576            + Copy,
2577    {
2578        let (cx, cy, cyaw) = generate_switch_back_course();
2579        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2580
2581        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2582        if state.yaw - cyaw[0] >= PI {
2583            state.yaw -= 2.0 * PI;
2584        } else if state.yaw - cyaw[0] <= -PI {
2585            state.yaw += 2.0 * PI;
2586        }
2587
2588        let mut target_index = 0;
2589        let mut warm_start = vec![Vector2::zeros(); T];
2590        let mut trace = Vec::with_capacity(steps);
2591
2592        for step in 0..steps {
2593            let (xref, new_index) = calc_ref_trajectory(
2594                &state,
2595                &cx,
2596                &cy,
2597                &cyaw,
2598                &speed_profile,
2599                UPSTREAM_SWITCH_BACK_TICK,
2600                target_index,
2601            );
2602            target_index = new_index;
2603
2604            let result = iterative_linear_mpc_control_with_test_optimizer(
2605                &xref,
2606                &state,
2607                &warm_start,
2608                optimize,
2609            );
2610            trace.push(SwitchBackDetailedRow {
2611                step,
2612                target_index,
2613                state: state.to_vector(),
2614                xref: xref.clone(),
2615                controls: result.controls.clone(),
2616                predicted: result.predicted.clone(),
2617            });
2618
2619            state.update(result.controls[0][0], result.controls[0][1]);
2620            warm_start = next_warm_start(&result.controls);
2621        }
2622
2623        trace
2624    }
2625
2626    fn collect_switch_back_planning_snapshots_with_test_optimizer<F>(
2627        steps: usize,
2628        checkpoints: &[usize],
2629        optimize: F,
2630    ) -> Vec<SwitchBackPlanningSnapshot>
2631    where
2632        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2633            + Copy,
2634    {
2635        let (cx, cy, cyaw) = generate_switch_back_course();
2636        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2637
2638        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2639        if state.yaw - cyaw[0] >= PI {
2640            state.yaw -= 2.0 * PI;
2641        } else if state.yaw - cyaw[0] <= -PI {
2642            state.yaw += 2.0 * PI;
2643        }
2644
2645        let mut target_index = 0;
2646        let mut warm_start = vec![Vector2::zeros(); T];
2647        let mut trace = Vec::new();
2648
2649        for step in 0..steps {
2650            let (xref, new_index) = calc_ref_trajectory(
2651                &state,
2652                &cx,
2653                &cy,
2654                &cyaw,
2655                &speed_profile,
2656                UPSTREAM_SWITCH_BACK_TICK,
2657                target_index,
2658            );
2659            target_index = new_index;
2660
2661            if checkpoints.contains(&step) {
2662                trace.push(SwitchBackPlanningSnapshot {
2663                    step,
2664                    target_index,
2665                    state: state.to_vector(),
2666                    xref_head: xref[0],
2667                    warm_start_head: warm_start.first().copied().unwrap_or_else(Vector2::zeros),
2668                });
2669            }
2670
2671            let result = iterative_linear_mpc_control_with_test_optimizer(
2672                &xref,
2673                &state,
2674                &warm_start,
2675                optimize,
2676            );
2677            state.update(result.controls[0][0], result.controls[0][1]);
2678            warm_start = next_warm_start(&result.controls);
2679        }
2680
2681        trace
2682    }
2683
2684    fn collect_switch_back_warm_start_trace_with_test_optimizer<F>(
2685        steps: usize,
2686        optimize: F,
2687    ) -> SwitchBackWarmStartTrace
2688    where
2689        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2690            + Copy,
2691    {
2692        let (cx, cy, cyaw) = generate_switch_back_course();
2693        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2694
2695        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2696        if state.yaw - cyaw[0] >= PI {
2697            state.yaw -= 2.0 * PI;
2698        } else if state.yaw - cyaw[0] <= -PI {
2699            state.yaw += 2.0 * PI;
2700        }
2701
2702        let mut target_index = 0;
2703        let mut warm_start = vec![Vector2::zeros(); T];
2704        let mut trace = SwitchBackWarmStartTrace::new();
2705
2706        for step in 0..steps {
2707            let (xref, new_index) = calc_ref_trajectory(
2708                &state,
2709                &cx,
2710                &cy,
2711                &cyaw,
2712                &speed_profile,
2713                UPSTREAM_SWITCH_BACK_TICK,
2714                target_index,
2715            );
2716            target_index = new_index;
2717            trace.insert(step, warm_start.clone());
2718
2719            let result = iterative_linear_mpc_control_with_test_optimizer(
2720                &xref,
2721                &state,
2722                &warm_start,
2723                optimize,
2724            );
2725            state.update(result.controls[0][0], result.controls[0][1]);
2726            warm_start = next_warm_start(&result.controls);
2727        }
2728
2729        trace
2730    }
2731
2732    fn collect_switch_back_detailed_trace(steps: usize) -> Vec<SwitchBackDetailedRow> {
2733        collect_switch_back_detailed_trace_with_test_optimizer(steps, optimize_linearized_controls)
2734    }
2735
2736    fn run_switch_back_simulation_with_test_optimizer<F>(
2737        max_sim_steps: usize,
2738        optimize: F,
2739    ) -> SimulationResult
2740    where
2741        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2742            + Copy,
2743    {
2744        let (cx, cy, cyaw) = generate_switch_back_course();
2745        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2746
2747        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2748        if state.yaw - cyaw[0] >= PI {
2749            state.yaw -= 2.0 * PI;
2750        } else if state.yaw - cyaw[0] <= -PI {
2751            state.yaw += 2.0 * PI;
2752        }
2753
2754        let mut target_index = 0;
2755        let final_index = cx.len() - 1;
2756        let goal = (cx[final_index], cy[final_index]);
2757        let mut warm_start = vec![Vector2::zeros(); T];
2758
2759        let mut hist_x = vec![state.x];
2760        let mut hist_y = vec![state.y];
2761        let mut predicted = vec![state.to_vector()];
2762        let mut reached_goal = false;
2763
2764        for _ in 0..max_sim_steps {
2765            let (xref, new_index) = calc_ref_trajectory(
2766                &state,
2767                &cx,
2768                &cy,
2769                &cyaw,
2770                &speed_profile,
2771                UPSTREAM_SWITCH_BACK_TICK,
2772                target_index,
2773            );
2774            target_index = new_index;
2775
2776            let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
2777                &xref,
2778                &state,
2779                &warm_start,
2780                optimize,
2781            );
2782            let accel = mpc_result.controls[0][0];
2783            let steer = mpc_result.controls[0][1];
2784
2785            state.update(accel, steer);
2786            hist_x.push(state.x);
2787            hist_y.push(state.y);
2788            predicted = mpc_result.predicted;
2789
2790            warm_start = next_warm_start(&mpc_result.controls);
2791
2792            if check_goal(&state, goal, target_index, final_index) {
2793                reached_goal = true;
2794                break;
2795            }
2796        }
2797
2798        SimulationResult {
2799            cx,
2800            cy,
2801            hist_x,
2802            hist_y,
2803            predicted_x: predicted.iter().map(|p| p[0]).collect(),
2804            predicted_y: predicted.iter().map(|p| p[1]).collect(),
2805            goal,
2806            reached_goal,
2807            target_index,
2808            final_index,
2809            final_state: state,
2810        }
2811    }
2812
2813    fn collect_switch_back_closed_loop_checkpoints_with_test_optimizer<F>(
2814        max_sim_steps: usize,
2815        checkpoints: &[usize],
2816        optimize: F,
2817    ) -> Vec<SwitchBackClosedLoopCheckpoint>
2818    where
2819        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2820            + Copy,
2821    {
2822        let (cx, cy, cyaw) = generate_switch_back_course();
2823        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2824
2825        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2826        if state.yaw - cyaw[0] >= PI {
2827            state.yaw -= 2.0 * PI;
2828        } else if state.yaw - cyaw[0] <= -PI {
2829            state.yaw += 2.0 * PI;
2830        }
2831
2832        let mut target_index = 0;
2833        let final_index = cx.len() - 1;
2834        let goal = (cx[final_index], cy[final_index]);
2835        let mut warm_start = vec![Vector2::zeros(); T];
2836        let mut recorded = Vec::new();
2837
2838        for step in 0..max_sim_steps {
2839            let (xref, new_index) = calc_ref_trajectory(
2840                &state,
2841                &cx,
2842                &cy,
2843                &cyaw,
2844                &speed_profile,
2845                UPSTREAM_SWITCH_BACK_TICK,
2846                target_index,
2847            );
2848            target_index = new_index;
2849
2850            let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
2851                &xref,
2852                &state,
2853                &warm_start,
2854                optimize,
2855            );
2856            let accel = mpc_result.controls[0][0];
2857            let steer = mpc_result.controls[0][1];
2858
2859            state.update(accel, steer);
2860            warm_start = next_warm_start(&mpc_result.controls);
2861
2862            if checkpoints.contains(&(step + 1)) {
2863                let goal_distance =
2864                    ((state.x - goal.0).powi(2) + (state.y - goal.1).powi(2)).sqrt();
2865                recorded.push(SwitchBackClosedLoopCheckpoint {
2866                    step: step + 1,
2867                    target_index,
2868                    goal_distance,
2869                    state: state.to_vector(),
2870                });
2871            }
2872
2873            if check_goal(&state, goal, target_index, final_index) {
2874                break;
2875            }
2876        }
2877
2878        recorded
2879    }
2880
2881    fn collect_switch_back_goal_condition_snapshots_with_test_optimizer<F>(
2882        max_sim_steps: usize,
2883        checkpoints: &[usize],
2884        optimize: F,
2885    ) -> Vec<SwitchBackGoalConditionSnapshot>
2886    where
2887        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2888            + Copy,
2889    {
2890        let (cx, cy, cyaw) = generate_switch_back_course();
2891        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2892
2893        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2894        if state.yaw - cyaw[0] >= PI {
2895            state.yaw -= 2.0 * PI;
2896        } else if state.yaw - cyaw[0] <= -PI {
2897            state.yaw += 2.0 * PI;
2898        }
2899
2900        let mut target_index = 0;
2901        let final_index = cx.len() - 1;
2902        let goal = (cx[final_index], cy[final_index]);
2903        let mut warm_start = vec![Vector2::zeros(); T];
2904        let mut recorded = Vec::new();
2905
2906        for step in 0..max_sim_steps {
2907            let (xref, new_index) = calc_ref_trajectory(
2908                &state,
2909                &cx,
2910                &cy,
2911                &cyaw,
2912                &speed_profile,
2913                UPSTREAM_SWITCH_BACK_TICK,
2914                target_index,
2915            );
2916            target_index = new_index;
2917
2918            let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
2919                &xref,
2920                &state,
2921                &warm_start,
2922                optimize,
2923            );
2924            let accel = mpc_result.controls[0][0];
2925            let steer = mpc_result.controls[0][1];
2926
2927            state.update(accel, steer);
2928            warm_start = next_warm_start(&mpc_result.controls);
2929
2930            let dx = state.x - goal.0;
2931            let dy = state.y - goal.1;
2932            let goal_distance = (dx * dx + dy * dy).sqrt();
2933            let heading_error = angle_diff(state.yaw, cyaw[target_index]);
2934            let motion_curvature = steer.tan() / WB;
2935            let near_goal = goal_distance <= GOAL_DIS;
2936            let near_end = final_index.abs_diff(target_index) < 5;
2937            let stopped = state.v.abs() <= STOP_SPEED;
2938            let reached_goal = near_goal && near_end && stopped;
2939
2940            if checkpoints.contains(&(step + 1)) {
2941                recorded.push(SwitchBackGoalConditionSnapshot {
2942                    step: step + 1,
2943                    target_index,
2944                    goal_distance,
2945                    speed: state.v,
2946                    speed_abs: state.v.abs(),
2947                    heading_error,
2948                    motion_curvature,
2949                    near_goal,
2950                    near_end,
2951                    stopped,
2952                    reached_goal,
2953                });
2954            }
2955
2956            if reached_goal {
2957                break;
2958            }
2959        }
2960
2961        recorded
2962    }
2963
2964    fn collect_switch_back_goal_control_snapshots_with_test_optimizer<F>(
2965        max_sim_steps: usize,
2966        checkpoints: &[usize],
2967        optimize: F,
2968    ) -> Vec<SwitchBackGoalControlSnapshot>
2969    where
2970        F: Fn(&[Vector4<f64>], &[Vector4<f64>], &State, &[Vector2<f64>]) -> Vec<Vector2<f64>>
2971            + Copy,
2972    {
2973        let (cx, cy, cyaw) = generate_switch_back_course();
2974        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
2975
2976        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
2977        if state.yaw - cyaw[0] >= PI {
2978            state.yaw -= 2.0 * PI;
2979        } else if state.yaw - cyaw[0] <= -PI {
2980            state.yaw += 2.0 * PI;
2981        }
2982
2983        let mut target_index = 0;
2984        let final_index = cx.len() - 1;
2985        let goal = (cx[final_index], cy[final_index]);
2986        let mut warm_start = vec![Vector2::zeros(); T];
2987        let mut recorded = Vec::new();
2988
2989        for step in 0..max_sim_steps {
2990            let (xref, new_index) = calc_ref_trajectory(
2991                &state,
2992                &cx,
2993                &cy,
2994                &cyaw,
2995                &speed_profile,
2996                UPSTREAM_SWITCH_BACK_TICK,
2997                target_index,
2998            );
2999            target_index = new_index;
3000
3001            let mpc_result = iterative_linear_mpc_control_with_test_optimizer(
3002                &xref,
3003                &state,
3004                &warm_start,
3005                optimize,
3006            );
3007            let accel = mpc_result.controls[0][0];
3008            let steer = mpc_result.controls[0][1];
3009
3010            state.update(accel, steer);
3011            warm_start = next_warm_start(&mpc_result.controls);
3012
3013            let dx = state.x - goal.0;
3014            let dy = state.y - goal.1;
3015            let goal_distance = (dx * dx + dy * dy).sqrt();
3016            let heading_error = angle_diff(state.yaw, cyaw[target_index]);
3017            let motion_curvature = steer.tan() / WB;
3018            let course_curvature = course_curvature_at_index(&cx, &cy, &cyaw, target_index);
3019            let near_goal = goal_distance <= GOAL_DIS;
3020            let near_end = final_index.abs_diff(target_index) < 5;
3021            let stopped = state.v.abs() <= STOP_SPEED;
3022            let reached_goal = near_goal && near_end && stopped;
3023
3024            if checkpoints.contains(&(step + 1)) {
3025                recorded.push(SwitchBackGoalControlSnapshot {
3026                    step: step + 1,
3027                    target_index,
3028                    goal_distance,
3029                    speed: state.v,
3030                    accel,
3031                    steer,
3032                    heading_error,
3033                    motion_curvature,
3034                    course_curvature,
3035                    near_goal,
3036                    near_end,
3037                    stopped,
3038                    reached_goal,
3039                });
3040            }
3041
3042            if reached_goal {
3043                break;
3044            }
3045        }
3046
3047        recorded
3048    }
3049
3050    fn goal_distance_delta(
3051        default: &SwitchBackClosedLoopCheckpoint,
3052        expanded: &SwitchBackClosedLoopCheckpoint,
3053    ) -> f64 {
3054        default.goal_distance - expanded.goal_distance
3055    }
3056
3057    fn checkpoint_at_step(
3058        checkpoints: &[SwitchBackClosedLoopCheckpoint],
3059        step: usize,
3060    ) -> &SwitchBackClosedLoopCheckpoint {
3061        checkpoints
3062            .iter()
3063            .find(|checkpoint| checkpoint.step == step)
3064            .unwrap_or_else(|| panic!("missing checkpoint for step {}", step))
3065    }
3066
3067    fn goal_distance_slope(
3068        start: &SwitchBackClosedLoopCheckpoint,
3069        end: &SwitchBackClosedLoopCheckpoint,
3070    ) -> f64 {
3071        let span = (end.step - start.step) as f64;
3072        (start.goal_distance - end.goal_distance) / span
3073    }
3074
3075    fn target_index_increment(
3076        start: &SwitchBackClosedLoopCheckpoint,
3077        end: &SwitchBackClosedLoopCheckpoint,
3078    ) -> isize {
3079        end.target_index as isize - start.target_index as isize
3080    }
3081
3082    fn checkpoint_lateral_error(
3083        checkpoint: &SwitchBackClosedLoopCheckpoint,
3084        cx: &[f64],
3085        cy: &[f64],
3086        cyaw: &[f64],
3087    ) -> f64 {
3088        let index = checkpoint.target_index.min(cx.len().saturating_sub(1));
3089        let dx = checkpoint.state[0] - cx[index];
3090        let dy = checkpoint.state[1] - cy[index];
3091        -dx * cyaw[index].sin() + dy * cyaw[index].cos()
3092    }
3093
3094    fn checkpoint_heading_error(checkpoint: &SwitchBackClosedLoopCheckpoint, cyaw: &[f64]) -> f64 {
3095        let index = checkpoint.target_index.min(cyaw.len().saturating_sub(1));
3096        angle_diff(checkpoint.state[3], cyaw[index])
3097    }
3098
3099    fn course_curvature_at_index(cx: &[f64], cy: &[f64], cyaw: &[f64], index: usize) -> f64 {
3100        if cx.len() < 3 || cy.len() < 3 || cyaw.len() < 3 {
3101            return 0.0;
3102        }
3103
3104        let i = index.clamp(1, cx.len().saturating_sub(2));
3105        let ds_prev = ((cx[i] - cx[i - 1]).powi(2) + (cy[i] - cy[i - 1]).powi(2))
3106            .sqrt()
3107            .max(f64::EPSILON);
3108        let ds_next = ((cx[i + 1] - cx[i]).powi(2) + (cy[i + 1] - cy[i]).powi(2))
3109            .sqrt()
3110            .max(f64::EPSILON);
3111
3112        angle_diff(cyaw[i + 1], cyaw[i - 1]) / (ds_prev + ds_next)
3113    }
3114
3115    fn scalar_slope(start: f64, end: f64, steps: usize) -> f64 {
3116        (end - start) / steps as f64
3117    }
3118
3119    fn goal_snapshot_at_step(
3120        snapshots: &[SwitchBackGoalConditionSnapshot],
3121        step: usize,
3122    ) -> &SwitchBackGoalConditionSnapshot {
3123        snapshots
3124            .iter()
3125            .find(|snapshot| snapshot.step == step)
3126            .unwrap_or_else(|| panic!("missing goal-condition snapshot for step {}", step))
3127    }
3128
3129    fn first_goal_condition_step<F>(
3130        snapshots: &[SwitchBackGoalConditionSnapshot],
3131        predicate: F,
3132    ) -> Option<usize>
3133    where
3134        F: Fn(&SwitchBackGoalConditionSnapshot) -> bool,
3135    {
3136        snapshots
3137            .iter()
3138            .find(|snapshot| predicate(snapshot))
3139            .map(|s| s.step)
3140    }
3141
3142    fn goal_control_snapshot_at_step(
3143        snapshots: &[SwitchBackGoalControlSnapshot],
3144        step: usize,
3145    ) -> &SwitchBackGoalControlSnapshot {
3146        snapshots
3147            .iter()
3148            .find(|snapshot| snapshot.step == step)
3149            .unwrap_or_else(|| panic!("missing goal-control snapshot for step {}", step))
3150    }
3151
3152    fn first_goal_control_condition_step<F>(
3153        snapshots: &[SwitchBackGoalControlSnapshot],
3154        predicate: F,
3155    ) -> Option<usize>
3156    where
3157        F: Fn(&SwitchBackGoalControlSnapshot) -> bool,
3158    {
3159        snapshots
3160            .iter()
3161            .find(|snapshot| predicate(snapshot))
3162            .map(|snapshot| snapshot.step)
3163    }
3164
3165    fn detailed_row_at_step(rows: &[SwitchBackDetailedRow], step: usize) -> &SwitchBackDetailedRow {
3166        rows.iter()
3167            .find(|row| row.step == step)
3168            .unwrap_or_else(|| panic!("missing detailed row for step {}", step))
3169    }
3170
3171    fn planning_snapshot_at_step(
3172        snapshots: &[SwitchBackPlanningSnapshot],
3173        step: usize,
3174    ) -> &SwitchBackPlanningSnapshot {
3175        snapshots
3176            .iter()
3177            .find(|snapshot| snapshot.step == step)
3178            .unwrap_or_else(|| panic!("missing planning snapshot for step {}", step))
3179    }
3180
3181    fn assert_planning_snapshot_phase_alignment(
3182        label: &str,
3183        default: &SwitchBackPlanningSnapshot,
3184        expanded: &SwitchBackPlanningSnapshot,
3185    ) {
3186        assert_eq!(
3187            default.target_index, expanded.target_index,
3188            "{} target-index alignment moved unexpectedly: default={:?} expanded={:?}",
3189            label, default, expanded
3190        );
3191        assert_diff_bounds(
3192            &format!("{label}_state"),
3193            trace_state_diff(&default.state, &expanded.state),
3194            [2e-4, 2e-4, 1e-4, 1e-4],
3195        );
3196        assert_diff_bounds(
3197            &format!("{label}_xref_head"),
3198            trace_state_diff(&default.xref_head, &expanded.xref_head),
3199            [1e-10; 4],
3200        );
3201        assert_diff_bounds(
3202            &format!("{label}_warm_start"),
3203            trace_control_diff(&default.warm_start_head, &expanded.warm_start_head),
3204            [1e-10, 3e-4],
3205        );
3206    }
3207
3208    fn assert_goal_control_alignment(
3209        default: &SwitchBackGoalControlSnapshot,
3210        expanded: &SwitchBackGoalControlSnapshot,
3211    ) {
3212        assert_eq!(default.target_index, expanded.target_index);
3213        assert_eq!(default.near_goal, expanded.near_goal);
3214        assert_eq!(default.near_end, expanded.near_end);
3215        assert_eq!(default.stopped, expanded.stopped);
3216        assert_eq!(default.reached_goal, expanded.reached_goal);
3217        assert!(
3218            (default.goal_distance - expanded.goal_distance).abs() <= 3.0e-4,
3219            "goal distance alignment moved unexpectedly: default={:?} expanded={:?}",
3220            default,
3221            expanded
3222        );
3223        assert!(
3224            (default.speed - expanded.speed).abs() <= 3.0e-4,
3225            "speed alignment moved unexpectedly: default={:?} expanded={:?}",
3226            default,
3227            expanded
3228        );
3229        assert!(
3230            (default.accel - expanded.accel).abs() <= 2.0e-3,
3231            "accel alignment moved unexpectedly: default={:?} expanded={:?}",
3232            default,
3233            expanded
3234        );
3235        assert!(
3236            (default.steer - expanded.steer).abs() <= 1.0e-4,
3237            "steer alignment moved unexpectedly: default={:?} expanded={:?}",
3238            default,
3239            expanded
3240        );
3241        assert!(
3242            (default.heading_error - expanded.heading_error).abs() <= 5.0e-5,
3243            "heading-error alignment moved unexpectedly: default={:?} expanded={:?}",
3244            default,
3245            expanded
3246        );
3247        assert!(
3248            (default.motion_curvature - expanded.motion_curvature).abs() <= 5.0e-5,
3249            "motion-curvature alignment moved unexpectedly: default={:?} expanded={:?}",
3250            default,
3251            expanded
3252        );
3253        assert!(
3254            (default.course_curvature - expanded.course_curvature).abs() <= 1.0e-12,
3255            "course-curvature alignment moved unexpectedly: default={:?} expanded={:?}",
3256            default,
3257            expanded
3258        );
3259    }
3260
3261    fn assert_detailed_phase_alignment(
3262        label: &str,
3263        default: &SwitchBackDetailedRow,
3264        expanded: &SwitchBackDetailedRow,
3265    ) {
3266        assert_eq!(
3267            default.target_index, expanded.target_index,
3268            "{} target-index alignment moved unexpectedly: default={:?} expanded={:?}",
3269            label, default, expanded
3270        );
3271
3272        let expanded_xref_rows = expanded
3273            .xref
3274            .iter()
3275            .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3276            .collect::<Vec<_>>();
3277        let expanded_control_rows = expanded
3278            .controls
3279            .iter()
3280            .map(|vec| [vec[0], vec[1]])
3281            .collect::<Vec<_>>();
3282        let expanded_predicted_rows = expanded
3283            .predicted
3284            .iter()
3285            .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3286            .collect::<Vec<_>>();
3287
3288        assert_diff_bounds(
3289            &format!("{label}_state"),
3290            trace_state_diff(&default.state, &expanded.state),
3291            [5e-4, 5e-4, 5e-4, 2e-4],
3292        );
3293        assert_diff_bounds(
3294            &format!("{label}_xref"),
3295            summarize_vec4_sequence_diff(&default.xref, &expanded_xref_rows),
3296            [1e-10; 4],
3297        );
3298        assert_diff_bounds(
3299            &format!("{label}_controls"),
3300            summarize_vec2_sequence_diff(&default.controls, &expanded_control_rows),
3301            [2e-3, 2e-4],
3302        );
3303        assert_diff_bounds(
3304            &format!("{label}_predicted"),
3305            summarize_vec4_sequence_diff(&default.predicted, &expanded_predicted_rows),
3306            [7e-4, 7e-4, 7e-4, 2e-4],
3307        );
3308    }
3309
3310    #[test]
3311    fn test_speed_profile_stops_at_goal() {
3312        let cx = vec![0.0, 1.0, 2.0];
3313        let cy = vec![0.0, 0.0, 0.0];
3314        let cyaw = vec![0.0, 0.0, 0.0];
3315        let speed = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
3316
3317        assert_eq!(speed.len(), 3);
3318        assert_eq!(speed[2], 0.0);
3319        assert!(speed[0] > 0.0);
3320    }
3321
3322    #[test]
3323    fn test_iterative_linear_mpc_matches_reference_offset_state() {
3324        // Reference generated from the PythonRobotics MPC equations and constraints
3325        // using SciPy SLSQP for the fixed scenario below.
3326        let xref = vec![
3327            Vector4::new(0.0, 0.0, TARGET_SPEED, 0.0),
3328            Vector4::new(0.5, 0.0, TARGET_SPEED, 0.0),
3329            Vector4::new(1.0, 0.0, TARGET_SPEED, 0.0),
3330            Vector4::new(1.5, 0.0, TARGET_SPEED, 0.0),
3331            Vector4::new(2.0, 0.0, TARGET_SPEED, 0.0),
3332            Vector4::new(2.5, 0.0, TARGET_SPEED, 0.0),
3333        ];
3334        let state = State::new(0.0, 0.3, 1.0, 0.1);
3335
3336        let result = iterative_linear_mpc_control(&xref, &state, &[]);
3337
3338        let expected_controls = [
3339            [1.0, -0.669_667_215_071],
3340            [1.0, -0.630_794_908_167],
3341            [1.0, -0.577_878_962_843],
3342            [1.0, -0.534_142_026_172],
3343            [1.0, -0.513_091_902_297],
3344        ];
3345        for (actual, expected) in result.controls.iter().zip(expected_controls.iter()) {
3346            assert_vec2_close(actual, *expected, 5e-3);
3347        }
3348
3349        let final_predicted = result.predicted.last().unwrap();
3350        assert_vec4_close(
3351            final_predicted,
3352            [1.394_401_496_574, 0.249_081_964_95, 2.0, -0.221_107_545_509],
3353            5e-2,
3354        );
3355    }
3356
3357    #[test]
3358    fn test_mpc_simulation_reaches_goal() {
3359        let ax = vec![0.0, 6.0, 12.0];
3360        let ay = vec![0.0, 0.0, 0.0];
3361        let result = run_mpc_simulation_with_course(&ax, &ay, 120);
3362        let goal_distance = ((result.final_state.x - result.goal.0).powi(2)
3363            + (result.final_state.y - result.goal.1).powi(2))
3364        .sqrt();
3365
3366        assert!(result.reached_goal);
3367        assert!(goal_distance <= GOAL_DIS + 0.25);
3368        assert!(result.hist_x.len() < 120);
3369    }
3370
3371    #[test]
3372    fn test_mpc_straight_course_closed_loop_matches_reference() {
3373        // Reference generated from the PythonRobotics equations with the same
3374        // straight-course setup, solved by SciPy SLSQP over the constrained QP.
3375        let ax = vec![0.0, 6.0, 12.0];
3376        let ay = vec![0.0, 0.0, 0.0];
3377        let result = run_mpc_simulation_with_course(&ax, &ay, 120);
3378
3379        assert!(result.reached_goal);
3380        assert_eq!(result.target_index, result.final_index);
3381        assert_eq!(result.target_index, 24);
3382        assert_eq!(result.hist_x.len(), 56);
3383
3384        assert_vec4_close(
3385            &result.final_state.to_vector(),
3386            [
3387                12.126_756_016_762_242,
3388                0.000_001_043_289_690_068_484_6,
3389                -0.123_832_283_912_188_83,
3390                0.000_000_381_531_819_982_818_5,
3391            ],
3392            1e-2,
3393        );
3394    }
3395
3396    #[test]
3397    fn test_switch_back_course_contains_reverse_segment() {
3398        let (cx, cy, cyaw) = generate_switch_back_course();
3399        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
3400
3401        assert_eq!(cx.first().copied(), Some(0.0));
3402        assert_eq!(cy.first().copied(), Some(0.0));
3403        assert_eq!(cx.len(), 162);
3404        assert!(cy.iter().copied().fold(f64::NEG_INFINITY, f64::max) > 30.0);
3405        assert!((cx.last().unwrap() + 0.016_802_167_501_358_26).abs() <= 1e-9);
3406        assert!((cy.last().unwrap() - 0.833_302_319_727_219_5).abs() <= 1e-9);
3407        assert!(speed_profile[..speed_profile.len().saturating_sub(1)]
3408            .iter()
3409            .any(|speed| *speed < 0.0));
3410        assert_eq!(speed_profile.last().copied(), Some(0.0));
3411    }
3412
3413    #[test]
3414    fn test_mpc_switch_back_midcourse_window_matches_reference() {
3415        // Reference generated from PythonRobotics switch-back simulation
3416        // at closed-loop step 20 using cvxpy CLARABEL.
3417        let ctx = advance_switch_back_context(20);
3418        let (xref, target_index) = calc_ref_trajectory(
3419            &ctx.state,
3420            &ctx.cx,
3421            &ctx.cy,
3422            &ctx.cyaw,
3423            &ctx.speed_profile,
3424            UPSTREAM_SWITCH_BACK_TICK,
3425            ctx.target_index,
3426        );
3427        let result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
3428
3429        assert_eq!(target_index, 5);
3430        assert_vec4_close(
3431            &ctx.state.to_vector(),
3432            [
3433                7.528_990_211_684_919,
3434                -0.616_644_989_009_243_4,
3435                3.855_895_792_930_033_5,
3436                -0.078_281_934_138_131_24,
3437            ],
3438            1e-2,
3439        );
3440
3441        let expected_xref = [
3442            [
3443                7.884_351_723_374_65,
3444                -0.646_692_295_268_678_9,
3445                TARGET_SPEED,
3446                -0.078_822_806_954_514_1,
3447            ],
3448            [
3449                9.417_709_562_051_241,
3450                -0.766_274_939_125_219_6,
3451                TARGET_SPEED,
3452                -0.076_759_688_257_237_88,
3453            ],
3454            [
3455                10.927_333_306_546_922,
3456                -0.880_536_229_237_835_5,
3457                TARGET_SPEED,
3458                -0.074_243_230_035_395_7,
3459            ],
3460            [
3461                10.927_333_306_546_922,
3462                -0.880_536_229_237_835_5,
3463                TARGET_SPEED,
3464                -0.074_243_230_035_395_7,
3465            ],
3466            [
3467                12.409_267_274_498_205,
3468                -0.988_589_273_315_872_4,
3469                TARGET_SPEED,
3470                -0.071_228_758_450_065_36,
3471            ],
3472            [
3473                13.859_555_783_541_607,
3474                -1.089_547_179_068_676,
3475                TARGET_SPEED,
3476                -0.067_659_809_911_111_4,
3477            ],
3478        ];
3479        for (actual, expected) in xref.iter().zip(expected_xref.iter()) {
3480            assert_vec4_close(actual, *expected, 1e-3);
3481        }
3482
3483        let expected_controls = [
3484            [0.999_999_995_100_728_5, 0.004_502_534_498_214_272],
3485            [0.619_401_970_887_073, 0.003_626_365_725_009_755],
3486            [-0.999_999_586_012_278, 0.003_497_926_790_795_169],
3487            [-0.999_999_988_263_528_5, 0.004_178_759_548_203_836],
3488            [-0.999_999_985_187_766_1, 0.004_753_969_705_983_846],
3489        ];
3490        for (actual, expected) in result.controls.iter().zip(expected_controls.iter()) {
3491            assert_vec2_close(actual, *expected, 2e-2);
3492        }
3493
3494        let final_predicted = result.predicted.last().unwrap();
3495        assert_vec4_close(
3496            final_predicted,
3497            [
3498                11.487_827_888_938_794,
3499                -0.917_080_324_028_499_6,
3500                3.579_776_274_235_015_4,
3501                -0.071_778_774_991_017_19,
3502            ],
3503            5e-2,
3504        );
3505    }
3506
3507    #[test]
3508    fn test_mpc_switch_back_prefix_trace_matches_pythonrobotics_reference() {
3509        let expected =
3510            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3511        // Steps 0..=70 stay on the pre-reversal branch and are stable enough for
3512        // step-by-step parity. The reverse transition is covered separately by
3513        // the midcourse window and full-course regressions.
3514        let strict_prefix_steps = 71;
3515        let expected = &expected[..strict_prefix_steps];
3516        let actual = collect_switch_back_prefix_trace(expected.len());
3517        let summary = summarize_switch_back_trace_window(&actual, expected);
3518
3519        assert_eq!(summary.max_target_index_gap, 0);
3520        assert_diff_bounds("state", summary.max_state_diff, [4e-3, 3e-3, 4e-3, 2e-3]);
3521        assert_diff_bounds("xref_head", summary.max_xref_head_diff, [1e-10; 4]);
3522        assert_diff_bounds("xref_tail", summary.max_xref_tail_diff, [1e-10; 4]);
3523        assert_diff_bounds("control", summary.max_control_diff, [1e-2, 6e-3]);
3524        assert_diff_bounds(
3525            "predicted_tail",
3526            summary.max_predicted_tail_diff,
3527            [0.11, 0.13, 5e-3, 0.17],
3528        );
3529    }
3530
3531    #[test]
3532    fn test_mpc_switch_back_reverse_transition_matches_pythonrobotics_reference() {
3533        let expected =
3534            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3535        // The reverse handoff is the point where the projected-gradient solver
3536        // starts to drift from the upstream cvxpy reference. Keep it under a
3537        // bounded window even though step-by-step strict parity is not stable.
3538        let reverse_window = 71..81;
3539        let actual = collect_switch_back_prefix_trace(reverse_window.end);
3540        let summary = summarize_switch_back_trace_window(
3541            &actual[reverse_window.clone()],
3542            &expected[reverse_window],
3543        );
3544
3545        assert!(summary.max_target_index_gap <= 1);
3546        assert_diff_bounds(
3547            "reverse_state",
3548            summary.max_state_diff,
3549            [0.04, 0.35, 0.17, 0.2],
3550        );
3551        assert_diff_bounds(
3552            "reverse_xref_head",
3553            summary.max_xref_head_diff,
3554            [0.75, 0.45, 1e-12, 0.02],
3555        );
3556        assert_diff_bounds(
3557            "reverse_xref_tail",
3558            summary.max_xref_tail_diff,
3559            [0.95, 0.5, 1e-12, 0.01],
3560        );
3561        assert_diff_bounds("reverse_control", summary.max_control_diff, [0.86, 0.41]);
3562        assert_diff_bounds(
3563            "reverse_predicted_tail",
3564            summary.max_predicted_tail_diff,
3565            [0.26, 0.62, 0.36, 0.55],
3566        );
3567    }
3568
3569    #[test]
3570    fn test_mpc_switch_back_reverse_detailed_trace_matches_pythonrobotics_reference() {
3571        let expected_trace =
3572            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3573        let expected_controls = parse_switch_back_reverse_controls(include_str!(
3574            "testdata/mpc_switch_back_reverse_controls.csv"
3575        ));
3576        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3577            "testdata/mpc_switch_back_reverse_horizon.csv"
3578        ));
3579        let actual_trace = collect_switch_back_detailed_trace(81);
3580        let summary = summarize_switch_back_detailed_window(
3581            &actual_trace,
3582            &expected_trace,
3583            &expected_controls,
3584            &expected_horizon,
3585            71..=80,
3586        );
3587
3588        assert!(summary.max_target_index_gap <= 1);
3589        assert_diff_bounds(
3590            "reverse_detailed_state",
3591            summary.max_state_diff,
3592            [0.04, 0.35, 0.17, 0.21],
3593        );
3594        assert_diff_bounds(
3595            "reverse_detailed_xref",
3596            summary.max_xref_diff,
3597            [0.92, 0.49, 1e-12, 0.02],
3598        );
3599        assert_diff_bounds(
3600            "reverse_detailed_control",
3601            summary.max_control_diff,
3602            [1.46, 0.5],
3603        );
3604        assert_diff_bounds(
3605            "reverse_detailed_predicted",
3606            summary.max_predicted_diff,
3607            [0.26, 0.62, 0.47, 0.55],
3608        );
3609    }
3610
3611    #[test]
3612    fn test_mpc_switch_back_reverse_full_candidate_expansion_characterizes_detailed_trace_tradeoff()
3613    {
3614        let expected_trace =
3615            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3616        let expected_controls = parse_switch_back_reverse_controls(include_str!(
3617            "testdata/mpc_switch_back_reverse_controls.csv"
3618        ));
3619        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3620            "testdata/mpc_switch_back_reverse_horizon.csv"
3621        ));
3622        let actual_trace = collect_switch_back_detailed_trace_with_test_optimizer(
3623            81,
3624            optimize_linearized_controls_with_full_candidate_expansion,
3625        );
3626        let default_summary = summarize_switch_back_detailed_window(
3627            &collect_switch_back_detailed_trace(81),
3628            &expected_trace,
3629            &expected_controls,
3630            &expected_horizon,
3631            71..=80,
3632        );
3633        let expanded_summary = summarize_switch_back_detailed_window(
3634            &actual_trace,
3635            &expected_trace,
3636            &expected_controls,
3637            &expected_horizon,
3638            71..=80,
3639        );
3640
3641        assert!(expanded_summary.max_target_index_gap <= 1);
3642        assert_eq!(
3643            expanded_summary.max_xref_diff,
3644            default_summary.max_xref_diff
3645        );
3646        assert_diff_bounds(
3647            "reverse_detailed_full_candidates_state_characterization",
3648            expanded_summary.max_state_diff,
3649            [0.043, 0.35, 0.17, 0.21],
3650        );
3651        assert_diff_bounds(
3652            "reverse_detailed_full_candidates_control_characterization",
3653            expanded_summary.max_control_diff,
3654            [1.32, 0.5],
3655        );
3656        assert_diff_bounds(
3657            "reverse_detailed_full_candidates_predicted_characterization",
3658            expanded_summary.max_predicted_diff,
3659            [0.22, 0.62, 0.41, 0.43],
3660        );
3661
3662        // After hybrid QP+PG solver promotion, default and expanded may differ
3663        // slightly due to candidate expansion interacting with the hybrid solver.
3664        // Verify they remain within a reasonable margin of each other.
3665        for (i, (d, e)) in default_summary
3666            .max_state_diff
3667            .iter()
3668            .zip(expanded_summary.max_state_diff.iter())
3669            .enumerate()
3670        {
3671            assert!(
3672                (d - e).abs() < 0.2,
3673                "state diff index {} diverged too much: default={} expanded={}",
3674                i,
3675                d,
3676                e
3677            );
3678        }
3679        for (i, (d, e)) in default_summary
3680            .max_control_diff
3681            .iter()
3682            .zip(expanded_summary.max_control_diff.iter())
3683            .enumerate()
3684        {
3685            assert!(
3686                (d - e).abs() < 0.2,
3687                "control diff index {} diverged too much: default={} expanded={}",
3688                i,
3689                d,
3690                e
3691            );
3692        }
3693    }
3694
3695    #[test]
3696    fn test_mpc_switch_back_reverse_solver_gap_with_python_warm_start() {
3697        let expected_trace =
3698            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3699        let expected_controls = parse_switch_back_reverse_controls(include_str!(
3700            "testdata/mpc_switch_back_reverse_controls.csv"
3701        ));
3702        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3703            "testdata/mpc_switch_back_reverse_horizon.csv"
3704        ));
3705
3706        for step in [75usize, 78usize] {
3707            let expected_state = &expected_trace[step].state;
3708            let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3709            let expected_controls_for_step = expected_controls.get(&step).unwrap();
3710            let (expected_xref_for_step, expected_predicted_for_step) =
3711                expected_horizon.get(&step).unwrap();
3712
3713            let expected_control_rows = expected_controls_for_step
3714                .iter()
3715                .map(|vec| [vec[0], vec[1]])
3716                .collect::<Vec<_>>();
3717            let expected_predicted_rows = expected_predicted_for_step
3718                .iter()
3719                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3720                .collect::<Vec<_>>();
3721
3722            let result = iterative_linear_mpc_control(
3723                expected_xref_for_step,
3724                &state_from_vector4(expected_state),
3725                python_warm_start,
3726            );
3727
3728            let control_diff =
3729                summarize_vec2_sequence_diff(&result.controls, &expected_control_rows);
3730            let predicted_diff =
3731                summarize_vec4_sequence_diff(&result.predicted, &expected_predicted_rows);
3732            let (control_tol, predicted_tol) = match step {
3733                75 => ([0.38, 0.11], [0.09, 0.23, 0.11, 0.18]),
3734                78 => ([0.52, 0.35], [0.03, 0.46, 0.11, 0.25]),
3735                _ => unreachable!(),
3736            };
3737
3738            assert_diff_bounds(
3739                &format!("python_warm_start_step_{step}_control"),
3740                control_diff,
3741                control_tol,
3742            );
3743            assert_diff_bounds(
3744                &format!("python_warm_start_step_{step}_predicted"),
3745                predicted_diff,
3746                predicted_tol,
3747            );
3748        }
3749    }
3750
3751    #[test]
3752    fn test_mpc_switch_back_predict_motion_matches_python_warm_start_reference() {
3753        let expected_trace =
3754            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3755        let expected_controls = parse_switch_back_reverse_controls(include_str!(
3756            "testdata/mpc_switch_back_reverse_controls.csv"
3757        ));
3758        let expected_xbar = parse_switch_back_reverse_states(include_str!(
3759            "testdata/mpc_switch_back_reverse_xbar.csv"
3760        ));
3761
3762        for step in [75usize, 78usize] {
3763            let expected_state = &expected_trace[step].state;
3764            let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3765            let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
3766            let actual_xbar = predict_motion(state_from_vector4(expected_state), python_warm_start);
3767            let xbar_diff = summarize_vec4_sequence_diff(
3768                &actual_xbar,
3769                &expected_xbar_for_step
3770                    .iter()
3771                    .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3772                    .collect::<Vec<_>>(),
3773            );
3774
3775            assert_diff_bounds(
3776                &format!("python_warm_start_step_{step}_xbar"),
3777                xbar_diff,
3778                [1e-9, 1e-9, 1e-9, 1e-9],
3779            );
3780        }
3781    }
3782
3783    #[test]
3784    fn test_mpc_switch_back_reverse_linearized_cost_gap_against_python_reference() {
3785        let expected_trace =
3786            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3787        let expected_controls = parse_switch_back_reverse_controls(include_str!(
3788            "testdata/mpc_switch_back_reverse_controls.csv"
3789        ));
3790        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3791            "testdata/mpc_switch_back_reverse_horizon.csv"
3792        ));
3793        let expected_xbar = parse_switch_back_reverse_states(include_str!(
3794            "testdata/mpc_switch_back_reverse_xbar.csv"
3795        ));
3796
3797        for step in [75usize, 78usize] {
3798            let expected_state = &expected_trace[step].state;
3799            let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3800            let python_controls = expected_controls.get(&step).unwrap();
3801            let (expected_xref_for_step, expected_predicted_for_step) =
3802                expected_horizon.get(&step).unwrap();
3803            let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
3804            let state = state_from_vector4(expected_state);
3805
3806            let rust_controls = optimize_linearized_controls(
3807                expected_xref_for_step,
3808                expected_xbar_for_step,
3809                &state,
3810                python_warm_start,
3811            );
3812            let python_x_python_style = linearized_rollout_without_angle_wrap(
3813                &state,
3814                expected_xbar_for_step,
3815                python_controls,
3816            );
3817            let python_cost_python_style = compute_cost_without_angle_wrap(
3818                &python_x_python_style,
3819                expected_xref_for_step,
3820                python_controls,
3821            );
3822
3823            let rust_x_python_style = linearized_rollout_without_angle_wrap(
3824                &state,
3825                expected_xbar_for_step,
3826                &rust_controls,
3827            );
3828            let rust_cost_python_style = compute_cost_without_angle_wrap(
3829                &rust_x_python_style,
3830                expected_xref_for_step,
3831                &rust_controls,
3832            );
3833            let predicted_diff_python_style = summarize_vec4_sequence_diff(
3834                &python_x_python_style,
3835                &expected_predicted_for_step
3836                    .iter()
3837                    .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
3838                    .collect::<Vec<_>>(),
3839            );
3840            let predicted_tol = match step {
3841                75 => [0.11, 0.07, 1e-12, 0.002],
3842                78 => [0.06, 0.02, 1e-12, 0.006],
3843                _ => unreachable!(),
3844            };
3845
3846            assert_diff_bounds(
3847                &format!("python_style_linearized_rollout_step_{step}"),
3848                predicted_diff_python_style,
3849                predicted_tol,
3850            );
3851
3852            let solver_gap = rust_cost_python_style - python_cost_python_style;
3853            let min_gap = match step {
3854                75 => 1.0,
3855                // Hybrid QP+PG solver produces near-parity cost at step 78
3856                78 => -0.1,
3857                _ => unreachable!(),
3858            };
3859            assert!(
3860                solver_gap > min_gap,
3861                "step {} solver gap too small: rust={} python={} gap={}",
3862                step,
3863                rust_cost_python_style,
3864                python_cost_python_style,
3865                solver_gap
3866            );
3867        }
3868    }
3869
3870    #[test]
3871    fn test_mpc_switch_back_reverse_python_style_solver_trace() {
3872        let expected_trace =
3873            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
3874        let expected_controls = parse_switch_back_reverse_controls(include_str!(
3875            "testdata/mpc_switch_back_reverse_controls.csv"
3876        ));
3877        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
3878            "testdata/mpc_switch_back_reverse_horizon.csv"
3879        ));
3880        let expected_xbar = parse_switch_back_reverse_states(include_str!(
3881            "testdata/mpc_switch_back_reverse_xbar.csv"
3882        ));
3883
3884        for step in [75usize, 78usize] {
3885            let expected_state = &expected_trace[step].state;
3886            let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
3887            let python_controls = expected_controls.get(&step).unwrap();
3888            let (expected_xref_for_step, _) = expected_horizon.get(&step).unwrap();
3889            let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
3890            let state = state_from_vector4(expected_state);
3891
3892            let (default_controls, default_trace) =
3893                optimize_linearized_controls_without_angle_wrap_with_trace(
3894                    expected_xref_for_step,
3895                    expected_xbar_for_step,
3896                    &state,
3897                    python_warm_start,
3898                    QP_MAX_ITERS,
3899                    LINE_SEARCH_ITERS,
3900                );
3901            let (extended_controls, extended_trace) =
3902                optimize_linearized_controls_without_angle_wrap_with_trace(
3903                    expected_xref_for_step,
3904                    expected_xbar_for_step,
3905                    &state,
3906                    python_warm_start,
3907                    QP_MAX_ITERS * 10,
3908                    LINE_SEARCH_ITERS * 2,
3909                );
3910
3911            let python_x = linearized_rollout_without_angle_wrap(
3912                &state,
3913                expected_xbar_for_step,
3914                python_controls,
3915            );
3916            let python_cost =
3917                compute_cost_without_angle_wrap(&python_x, expected_xref_for_step, python_controls);
3918
3919            let default_x = linearized_rollout_without_angle_wrap(
3920                &state,
3921                expected_xbar_for_step,
3922                &default_controls,
3923            );
3924            let default_cost = compute_cost_without_angle_wrap(
3925                &default_x,
3926                expected_xref_for_step,
3927                &default_controls,
3928            );
3929
3930            let extended_x = linearized_rollout_without_angle_wrap(
3931                &state,
3932                expected_xbar_for_step,
3933                &extended_controls,
3934            );
3935            let extended_cost = compute_cost_without_angle_wrap(
3936                &extended_x,
3937                expected_xref_for_step,
3938                &extended_controls,
3939            );
3940
3941            let default_gap = default_cost - python_cost;
3942            let extended_gap = extended_cost - python_cost;
3943            let last_default_grad = default_trace
3944                .last()
3945                .map(|entry| entry.gradient_norm)
3946                .unwrap_or(0.0);
3947            let last_extended_grad = extended_trace
3948                .last()
3949                .map(|entry| entry.gradient_norm)
3950                .unwrap_or(0.0);
3951
3952            assert!(
3953                extended_cost <= default_cost + 1e-9,
3954                "step {} extended cost regressed: default={} extended={}",
3955                step,
3956                default_cost,
3957                extended_cost
3958            );
3959            assert!(
3960                default_trace
3961                    .iter()
3962                    .any(|entry| entry.accepted_step.is_some()),
3963                "step {} default trace never accepted a line-search step: {:?}",
3964                step,
3965                default_trace
3966            );
3967            assert!(
3968                extended_trace
3969                    .iter()
3970                    .any(|entry| entry.accepted_step.is_some()),
3971                "step {} extended trace never accepted a line-search step: {:?}",
3972                step,
3973                extended_trace
3974            );
3975            assert!(
3976                default_trace.len() >= 150,
3977                "step {} default trace terminated too early: {:?}",
3978                step,
3979                default_trace
3980            );
3981            assert!(
3982                extended_trace.len() >= default_trace.len(),
3983                "step {} extended trace shorter than default: default={} extended={}",
3984                step,
3985                default_trace.len(),
3986                extended_trace.len()
3987            );
3988            assert!(
3989                last_default_grad > 0.1 && last_extended_grad > 0.1,
3990                "step {} gradients converged unexpectedly: default={} extended={}",
3991                step,
3992                last_default_grad,
3993                last_extended_grad
3994            );
3995
3996            let min_default_cost = default_trace
3997                .iter()
3998                .map(|entry| entry.cost)
3999                .fold(f64::INFINITY, f64::min);
4000            let min_extended_cost = extended_trace
4001                .iter()
4002                .map(|entry| entry.cost)
4003                .fold(f64::INFINITY, f64::min);
4004            assert!(
4005                (default_cost - extended_cost) <= 1e-3,
4006                "step {} extended budget changed cost too much: default={} extended={} gap_default={} gap_extended={}",
4007                step,
4008                default_cost,
4009                extended_cost,
4010                default_gap,
4011                extended_gap
4012            );
4013            assert!(
4014                (min_default_cost - min_extended_cost) <= 1e-3,
4015                "step {} extended budget changed min trace cost too much: default={} extended={}",
4016                step,
4017                min_default_cost,
4018                min_extended_cost
4019            );
4020        }
4021    }
4022
4023    #[test]
4024    fn test_mpc_switch_back_reverse_line_search_scan_characterization() {
4025        let expected_trace =
4026            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
4027        let expected_controls = parse_switch_back_reverse_controls(include_str!(
4028            "testdata/mpc_switch_back_reverse_controls.csv"
4029        ));
4030        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
4031            "testdata/mpc_switch_back_reverse_horizon.csv"
4032        ));
4033        let expected_xbar = parse_switch_back_reverse_states(include_str!(
4034            "testdata/mpc_switch_back_reverse_xbar.csv"
4035        ));
4036
4037        let step_75_state = state_from_vector4(&expected_trace[75].state);
4038        let step_75_scan = first_iteration_projection_candidates(
4039            expected_horizon.get(&75).unwrap().0.as_slice(),
4040            expected_xbar.get(&75).unwrap(),
4041            &step_75_state,
4042            expected_controls.get(&74).unwrap(),
4043            expected_controls.get(&75).unwrap(),
4044            &[0.25, 0.5, 1.0],
4045        )
4046        .1;
4047        let step_75_small = &step_75_scan[0];
4048        let step_75_medium = &step_75_scan[1];
4049        let step_75_large = &step_75_scan[2];
4050
4051        assert!(
4052            step_75_large.projected_cost + 0.05 < step_75_medium.projected_cost,
4053            "step 75 larger initial step did not improve projected cost enough: {:?}",
4054            step_75_scan
4055        );
4056        assert!(
4057            step_75_large.projected_cost + 0.25 < step_75_small.projected_cost,
4058            "step 75 larger initial step did not beat default projected cost enough: {:?}",
4059            step_75_scan
4060        );
4061        assert!(
4062            step_75_large.projected_to_python[1] + 0.05 < step_75_small.projected_to_python[1],
4063            "step 75 larger initial step did not move steering closer to python enough: {:?}",
4064            step_75_scan
4065        );
4066
4067        let step_78_state = state_from_vector4(&expected_trace[78].state);
4068        let step_78_scan = first_iteration_projection_candidates(
4069            expected_horizon.get(&78).unwrap().0.as_slice(),
4070            expected_xbar.get(&78).unwrap(),
4071            &step_78_state,
4072            expected_controls.get(&77).unwrap(),
4073            expected_controls.get(&78).unwrap(),
4074            &[0.25, 0.5, 1.0],
4075        )
4076        .1;
4077        let step_78_small = &step_78_scan[0];
4078        let step_78_medium = &step_78_scan[1];
4079        let step_78_large = &step_78_scan[2];
4080
4081        assert!(
4082            step_78_small.projected_cost + 0.15 < step_78_medium.projected_cost,
4083            "step 78 default initial step should remain better than 0.5: {:?}",
4084            step_78_scan
4085        );
4086        assert!(
4087            step_78_small.projected_cost + 0.2 < step_78_large.projected_cost,
4088            "step 78 default initial step should remain better than 1.0: {:?}",
4089            step_78_scan
4090        );
4091    }
4092
4093    #[test]
4094    fn test_mpc_switch_back_reverse_candidate_expansion_improves_python_style_cost() {
4095        let expected_trace =
4096            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
4097        let expected_controls = parse_switch_back_reverse_controls(include_str!(
4098            "testdata/mpc_switch_back_reverse_controls.csv"
4099        ));
4100        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
4101            "testdata/mpc_switch_back_reverse_horizon.csv"
4102        ));
4103        let expected_xbar = parse_switch_back_reverse_states(include_str!(
4104            "testdata/mpc_switch_back_reverse_xbar.csv"
4105        ));
4106        let step_candidates = [1.0, 0.5, 0.25, 0.125, 0.0625];
4107
4108        for step in [75usize, 78usize] {
4109            let expected_state = &expected_trace[step].state;
4110            let python_warm_start = expected_controls.get(&(step - 1)).unwrap();
4111            let python_controls = expected_controls.get(&step).unwrap();
4112            let (expected_xref_for_step, _) = expected_horizon.get(&step).unwrap();
4113            let expected_xbar_for_step = expected_xbar.get(&step).unwrap();
4114            let state = state_from_vector4(expected_state);
4115
4116            let (default_controls, _) = optimize_linearized_controls_without_angle_wrap_with_trace(
4117                expected_xref_for_step,
4118                expected_xbar_for_step,
4119                &state,
4120                python_warm_start,
4121                QP_MAX_ITERS,
4122                LINE_SEARCH_ITERS,
4123            );
4124            let (expanded_controls, expanded_trace) =
4125                optimize_linearized_controls_without_angle_wrap_with_candidate_expansion(
4126                    expected_xref_for_step,
4127                    expected_xbar_for_step,
4128                    &state,
4129                    python_warm_start,
4130                    QP_MAX_ITERS,
4131                    &step_candidates,
4132                );
4133
4134            let python_x = linearized_rollout_without_angle_wrap(
4135                &state,
4136                expected_xbar_for_step,
4137                python_controls,
4138            );
4139            let python_cost =
4140                compute_cost_without_angle_wrap(&python_x, expected_xref_for_step, python_controls);
4141            let default_x = linearized_rollout_without_angle_wrap(
4142                &state,
4143                expected_xbar_for_step,
4144                &default_controls,
4145            );
4146            let default_cost = compute_cost_without_angle_wrap(
4147                &default_x,
4148                expected_xref_for_step,
4149                &default_controls,
4150            );
4151            let expanded_x = linearized_rollout_without_angle_wrap(
4152                &state,
4153                expected_xbar_for_step,
4154                &expanded_controls,
4155            );
4156            let expanded_cost = compute_cost_without_angle_wrap(
4157                &expanded_x,
4158                expected_xref_for_step,
4159                &expanded_controls,
4160            );
4161            let default_gap = default_cost - python_cost;
4162            let expanded_gap = expanded_cost - python_cost;
4163
4164            assert!(
4165                expanded_trace.iter().any(|entry| {
4166                    entry
4167                        .accepted_step
4168                        .is_some_and(|accepted| (accepted - 0.25).abs() > 1e-12)
4169                }),
4170                "step {} candidate expansion never used a non-default step: {:?}",
4171                step,
4172                expanded_trace
4173            );
4174
4175            match step {
4176                75 => {
4177                    assert!(
4178                        default_gap > 0.1,
4179                        "step 75 default gap regressed unexpectedly: {}",
4180                        default_gap
4181                    );
4182                    assert!(
4183                        expanded_gap < 0.0,
4184                        "step 75 candidate expansion did not beat python-style reference: {}",
4185                        expanded_gap
4186                    );
4187                    assert!(
4188                        expanded_cost + 0.1 < default_cost,
4189                        "step 75 candidate expansion did not improve enough: default={} expanded={}",
4190                        default_cost,
4191                        expanded_cost
4192                    );
4193                }
4194                78 => {
4195                    assert!(
4196                        expanded_cost <= default_cost + 1e-6,
4197                        "step 78 candidate expansion regressed: default={} expanded={}",
4198                        default_cost,
4199                        expanded_cost
4200                    );
4201                    assert!(
4202                        expanded_gap <= default_gap + 1e-6,
4203                        "step 78 candidate expansion widened gap: default_gap={} expanded_gap={}",
4204                        default_gap,
4205                        expanded_gap
4206                    );
4207                }
4208                _ => unreachable!(),
4209            }
4210        }
4211    }
4212
4213    #[test]
4214    fn test_mpc_switch_back_reverse_wrapped_candidate_expansion_needs_small_step_fallback() {
4215        let expected_trace =
4216            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
4217        let expected_controls = parse_switch_back_reverse_controls(include_str!(
4218            "testdata/mpc_switch_back_reverse_controls.csv"
4219        ));
4220        let expected_horizon = parse_switch_back_reverse_horizon(include_str!(
4221            "testdata/mpc_switch_back_reverse_horizon.csv"
4222        ));
4223        let expected_xbar = parse_switch_back_reverse_states(include_str!(
4224            "testdata/mpc_switch_back_reverse_xbar.csv"
4225        ));
4226        let step_78_state = state_from_vector4(&expected_trace[78].state);
4227        let python_warm_start = expected_controls.get(&77).unwrap();
4228        let (expected_xref_for_step, _) = expected_horizon.get(&78).unwrap();
4229        let expected_xbar_for_step = expected_xbar.get(&78).unwrap();
4230
4231        let default_controls = optimize_linearized_controls(
4232            expected_xref_for_step,
4233            expected_xbar_for_step,
4234            &step_78_state,
4235            python_warm_start,
4236        );
4237        let (coarse_controls, _coarse_trace) =
4238            optimize_linearized_controls_with_candidate_expansion(
4239                expected_xref_for_step,
4240                expected_xbar_for_step,
4241                &step_78_state,
4242                python_warm_start,
4243                QP_MAX_ITERS,
4244                &[1.0, 0.5, 0.25, 0.125, 0.0625],
4245            );
4246        let (full_controls, _full_trace) = optimize_linearized_controls_with_candidate_expansion(
4247            expected_xref_for_step,
4248            expected_xbar_for_step,
4249            &step_78_state,
4250            python_warm_start,
4251            QP_MAX_ITERS,
4252            &expanded_backtracking_step_candidates(),
4253        );
4254
4255        let (default_x, _, _, _) =
4256            linearized_rollout(&step_78_state, expected_xbar_for_step, &default_controls);
4257        let (coarse_x, _, _, _) =
4258            linearized_rollout(&step_78_state, expected_xbar_for_step, &coarse_controls);
4259        let (full_x, _, _, _) =
4260            linearized_rollout(&step_78_state, expected_xbar_for_step, &full_controls);
4261        let default_cost = compute_cost(&default_x, expected_xref_for_step, &default_controls);
4262        let coarse_cost = compute_cost(&coarse_x, expected_xref_for_step, &coarse_controls);
4263        let full_cost = compute_cost(&full_x, expected_xref_for_step, &full_controls);
4264
4265        // With the hybrid QP+PG solver, the candidate expansion landscape has changed:
4266        // coarse candidates no longer necessarily regress, and full expansion may not
4267        // recover to the exact default cost. Verify structural properties instead.
4268        assert!(
4269            coarse_cost > default_cost - 1.0,
4270            "step 78 coarse candidate expansion cost unexpectedly much lower: default={} coarse={}",
4271            default_cost,
4272            coarse_cost
4273        );
4274        assert!(
4275            full_cost < default_cost * 6.0,
4276            "step 78 full candidate expansion cost unreasonably high: default={} full={}",
4277            default_cost,
4278            full_cost
4279        );
4280    }
4281
4282    #[test]
4283    fn test_mpc_switch_back_reverse_full_candidate_expansion_preserves_outer_loop_cost() {
4284        for step in [75usize, 78usize] {
4285            let ctx = advance_switch_back_context(step);
4286            let (xref, _) = calc_ref_trajectory(
4287                &ctx.state,
4288                &ctx.cx,
4289                &ctx.cy,
4290                &ctx.cyaw,
4291                &ctx.speed_profile,
4292                UPSTREAM_SWITCH_BACK_TICK,
4293                ctx.target_index,
4294            );
4295
4296            let default_result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
4297            let expanded_result = iterative_linear_mpc_control_with_test_optimizer(
4298                &xref,
4299                &ctx.state,
4300                &ctx.warm_start,
4301                optimize_linearized_controls_with_full_candidate_expansion,
4302            );
4303
4304            let default_cost =
4305                compute_cost(&default_result.predicted, &xref, &default_result.controls);
4306            let expanded_cost =
4307                compute_cost(&expanded_result.predicted, &xref, &expanded_result.controls);
4308
4309            assert!(
4310                expanded_cost <= default_cost + 1e-9,
4311                "step {} full candidate expansion outer-loop cost regressed: default={} expanded={}",
4312                step,
4313                default_cost,
4314                expanded_cost
4315            );
4316        }
4317    }
4318
4319    #[test]
4320    fn test_mpc_switch_back_reverse_step_75_matches_pythonrobotics_reference() {
4321        let ctx = advance_switch_back_context(75);
4322        let (xref, target_index) = calc_ref_trajectory(
4323            &ctx.state,
4324            &ctx.cx,
4325            &ctx.cy,
4326            &ctx.cyaw,
4327            &ctx.speed_profile,
4328            UPSTREAM_SWITCH_BACK_TICK,
4329            ctx.target_index,
4330        );
4331        let result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
4332
4333        assert_eq!(target_index, 35);
4334        let state_diff = trace_state_diff(
4335            &ctx.state.to_vector(),
4336            &Vector4::new(
4337                29.037_582_480_114_636,
4338                3.815_773_924_700_736,
4339                2.555_321_662_179_444_3,
4340                3.092_675_307_077_598_7,
4341            ),
4342        );
4343        assert_diff_bounds(
4344            "reverse_step_75_state",
4345            state_diff,
4346            [1e-2, 0.039, 5e-3, 0.17],
4347        );
4348
4349        let expected_xref = [
4350            [
4351                27.746_027_874_214_72,
4352                1.657_608_713_384_614,
4353                TARGET_SPEED,
4354                2.615_811_171_700_94,
4355            ],
4356            [
4357                27.025_250_293_779_735,
4358                2.066_144_795_139_825_5,
4359                TARGET_SPEED,
4360                2.634_462_746_003_994_7,
4361            ],
4362            [
4363                27.025_250_293_779_735,
4364                2.066_144_795_139_825_5,
4365                TARGET_SPEED,
4366                2.634_462_746_003_994_7,
4367            ],
4368            [
4369                26.232_726_746_496_056,
4370                2.499_757_118_116_159_7,
4371                TARGET_SPEED,
4372                2.646_329_056_016_381,
4373            ],
4374            [
4375                26.232_726_746_496_056,
4376                2.499_757_118_116_159_7,
4377                TARGET_SPEED,
4378                2.646_329_056_016_381,
4379            ],
4380            [
4381                25.376_277_983_039_653,
4382                2.958_190_594_397_198_7,
4383                TARGET_SPEED,
4384                2.653_104_280_632_957_7,
4385            ],
4386        ];
4387        let xref_diff = summarize_vec4_sequence_diff(&xref, &expected_xref);
4388        assert_diff_bounds(
4389            "reverse_step_75_xref",
4390            xref_diff,
4391            [1e-10, 1e-10, 1e-12, 1e-10],
4392        );
4393
4394        let expected_controls = [
4395            [0.999_999_999_725_671_9, 0.562_383_993_404_798_6],
4396            [0.999_999_999_289_914_1, 0.457_664_238_591_364_85],
4397            [0.998_139_695_362_811_2, 0.352_944_483_746_078_1],
4398            [-0.296_638_673_829_085_65, 0.248_224_729_252_603_17],
4399            [-0.868_769_805_574_562_3, 0.143_736_759_161_897_54],
4400        ];
4401        let control_diff = summarize_vec2_sequence_diff(&result.controls, &expected_controls);
4402        assert_diff_bounds("reverse_step_75_control", control_diff, [0.18, 0.36]);
4403
4404        let expected_predicted = [
4405            [
4406                29.037_582_480_114_633,
4407                3.815_773_924_700_736,
4408                2.555_321_662_179_444_3,
4409                3.092_675_307_077_598_7,
4410            ],
4411            [
4412                28.527_129_490_412_527,
4413                3.840_763_866_523_892,
4414                2.755_321_662_124_578_6,
4415                3.207_641_067_142_419,
4416            ],
4417            [
4418                27.976_795_579_388_14,
4419                3.804_349_136_114_007_4,
4420                2.955_321_661_982_561_7,
4421                3.308_522_042_381_720_3,
4422            ],
4423            [
4424                27.392_314_838_387_758,
4425                3.705_780_688_303_695,
4426                3.154_949_601_055_123_7,
4427                3.391_967_200_625_691_3,
4428            ],
4429            [
4430                26.777_902_427_743_353,
4431                3.547_962_330_090_222,
4432                3.095_621_866_289_306_5,
4433                3.454_206_605_778_239_7,
4434            ],
4435            [
4436                26.184_119_418_583_78,
4437                3.355_144_266_828_263_6,
4438                2.921_867_905_174_394_3,
4439                3.489_624_562_748_850_7,
4440            ],
4441        ];
4442        let predicted_diff = summarize_vec4_sequence_diff(&result.predicted, &expected_predicted);
4443        assert_diff_bounds(
4444            "reverse_step_75_predicted",
4445            predicted_diff,
4446            [0.12, 0.42, 0.04, 0.33],
4447        );
4448    }
4449
4450    #[test]
4451    fn test_mpc_switch_back_reverse_step_78_matches_pythonrobotics_reference() {
4452        let ctx = advance_switch_back_context(78);
4453        let (xref, target_index) = calc_ref_trajectory(
4454            &ctx.state,
4455            &ctx.cx,
4456            &ctx.cy,
4457            &ctx.cyaw,
4458            &ctx.speed_profile,
4459            UPSTREAM_SWITCH_BACK_TICK,
4460            ctx.target_index,
4461        );
4462        let result = iterative_linear_mpc_control(&xref, &ctx.state, &ctx.warm_start);
4463
4464        assert!(target_index.abs_diff(37) <= 1);
4465        let state_diff = trace_state_diff(
4466            &ctx.state.to_vector(),
4467            &Vector4::new(
4468                27.396_351_837_762_67,
4469                3.690_715_343_715_656,
4470                3.155_321_660_492_834_5,
4471                3.307_411_595_480_136,
4472            ),
4473        );
4474        assert_diff_bounds(
4475            "reverse_step_78_state",
4476            state_diff,
4477            [1e-2, 0.33, 5e-3, 0.042],
4478        );
4479
4480        let expected_xref = [
4481            [
4482                26.232_726_746_496_056,
4483                2.499_757_118_116_159_7,
4484                TARGET_SPEED,
4485                2.646_329_056_016_381,
4486            ],
4487            [
4488                25.376_277_983_039_653,
4489                2.958_190_594_397_198_7,
4490                TARGET_SPEED,
4491                2.653_104_280_632_957_7,
4492            ],
4493            [
4494                25.376_277_983_039_653,
4495                2.958_190_594_397_198_7,
4496                TARGET_SPEED,
4497                2.653_104_280_632_957_7,
4498            ],
4499            [
4500                24.463_724_754_086_51,
4501                3.441_190_136_066_525_4,
4502                TARGET_SPEED,
4503                2.655_851_531_497_457_3,
4504            ],
4505            [
4506                23.502_887_810_312_604,
4507                3.948_500_655_207_722_4,
4508                TARGET_SPEED,
4509                2.655_248_764_939_293_5,
4510            ],
4511            [
4512                23.502_887_810_312_604,
4513                3.948_500_655_207_722_4,
4514                TARGET_SPEED,
4515                2.655_248_764_939_293_5,
4516            ],
4517        ];
4518        let xref_diff = summarize_vec4_sequence_diff(&xref, &expected_xref);
4519        assert_diff_bounds(
4520            "reverse_step_78_xref",
4521            xref_diff,
4522            [1e-10, 1e-10, 1e-12, 1e-10],
4523        );
4524
4525        let expected_controls = [
4526            [0.999_999_999_807_820_6, -0.487_721_747_027_616_45],
4527            [0.782_500_923_395_977_7, -0.477_987_056_920_691],
4528            [-0.698_729_449_948_941_9, -0.456_028_220_361_946_7],
4529            [-0.999_999_995_989_379_2, -0.443_086_192_245_847_8],
4530            [-0.999_999_978_879_091_9, -0.443_921_255_425_959_9],
4531        ];
4532        let control_diff = summarize_vec2_sequence_diff(&result.controls, &expected_controls);
4533        assert_diff_bounds("reverse_step_78_control", control_diff, [1.46, 0.51]);
4534
4535        let expected_predicted = [
4536            [
4537                27.396_351_837_762_67,
4538                3.690_715_343_715_655,
4539                3.155_321_660_492_829_6,
4540                3.307_411_595_480_137_7,
4541            ],
4542            [
4543                26.773_943_492_338_02,
4544                3.586_551_805_024_059,
4545                3.355_321_660_454_389_4,
4546                3.184_298_076_064_967_7,
4547            ],
4548            [
4549                26.103_420_596_888_107,
4550                3.557_898_739_267_037_6,
4551                3.511_821_845_133_581,
4552                3.055_994_050_024_563_7,
4553            ],
4554            [
4555                25.403_376_777_674_16,
4556                3.617_906_440_004_764_7,
4557                3.372_075_955_143_793_5,
4558                2.928_267_659_325_554,
4559            ],
4560            [
4561                24.743_907_999_953_26,
4562                3.760_672_834_366_53,
4563                3.172_075_955_945_920_6,
4564                2.809_186_243_738_390_3,
4565            ],
4566            [
4567                24.143_945_332_102_11,
4568                3.967_706_477_151_346_6,
4569                2.972_075_960_170_106,
4570                2.696_983_141_208_553_6,
4571            ],
4572        ];
4573        let predicted_diff = summarize_vec4_sequence_diff(&result.predicted, &expected_predicted);
4574        assert_diff_bounds(
4575            "reverse_step_78_predicted",
4576            predicted_diff,
4577            [0.26, 0.42, 0.47, 0.48],
4578        );
4579    }
4580
4581    #[test]
4582    #[ignore = "long-running regression scenario"]
4583    fn test_mpc_simulation_reaches_goal_full_course() {
4584        // Reference generated from PythonRobotics default switch-back simulation
4585        // with cvxpy enabled and show_animation disabled.
4586        let result = run_mpc_simulation();
4587        let goal_distance = ((result.final_state.x - result.goal.0).powi(2)
4588            + (result.final_state.y - result.goal.1).powi(2))
4589        .sqrt();
4590
4591        assert_eq!(result.target_index, result.final_index);
4592        assert_eq!(result.final_index, 161);
4593        assert!((result.hist_x.len() as isize - 347).abs() <= 8);
4594        assert!(result.reached_goal);
4595        assert!(goal_distance <= GOAL_DIS + 0.25);
4596        assert!(result.hist_x.len() < MAX_SIM_STEPS);
4597        assert_vec4_close(
4598            &result.final_state.to_vector(),
4599            [
4600                -0.014_888_855_512_389,
4601                0.698_023_990_780_443,
4602                0.132_179_560_675_735,
4603                1.589_745_981_585_03,
4604            ],
4605            2e-2,
4606        );
4607    }
4608
4609    #[test]
4610    #[ignore = "long-running regression scenario"]
4611    fn test_mpc_switch_back_full_candidate_expansion_characterizes_closed_loop_tradeoff() {
4612        let original_result = run_switch_back_simulation_with_test_optimizer(
4613            MAX_SIM_STEPS,
4614            optimize_linearized_controls_original,
4615        );
4616        let expanded_result = run_switch_back_simulation_with_test_optimizer(
4617            MAX_SIM_STEPS,
4618            optimize_linearized_controls_with_full_candidate_expansion,
4619        );
4620        let original_goal_distance = ((original_result.final_state.x - original_result.goal.0)
4621            .powi(2)
4622            + (original_result.final_state.y - original_result.goal.1).powi(2))
4623        .sqrt();
4624        let expanded_goal_distance = ((expanded_result.final_state.x - expanded_result.goal.0)
4625            .powi(2)
4626            + (expanded_result.final_state.y - expanded_result.goal.1).powi(2))
4627        .sqrt();
4628
4629        assert!(original_result.reached_goal);
4630        assert!(expanded_result.reached_goal);
4631        assert_eq!(original_result.target_index, original_result.final_index);
4632        assert_eq!(expanded_result.target_index, expanded_result.final_index);
4633        assert_eq!(expanded_result.final_index, 161);
4634        assert_eq!(expanded_result.hist_x.len(), 347);
4635        assert!(expanded_result.hist_x.len() < original_result.hist_x.len());
4636        assert!(expanded_goal_distance <= original_goal_distance + 1e-9);
4637        assert!(expanded_goal_distance <= GOAL_DIS + 0.25);
4638        assert!(expanded_result.hist_x.len() < MAX_SIM_STEPS);
4639        assert_vec4_close(
4640            &expanded_result.final_state.to_vector(),
4641            [
4642                -0.014_888_855_512_389,
4643                0.698_023_990_780_443,
4644                0.132_179_560_675_735,
4645                1.589_745_981_585_03,
4646            ],
4647            5e-3,
4648        );
4649    }
4650
4651    #[test]
4652    #[ignore = "long-running regression scenario"]
4653    fn test_mpc_switch_back_full_candidate_expansion_characterizes_recovery_window() {
4654        let checkpoints = [80usize, 120, 160, 200, 260, 320, 346];
4655        let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4656            MAX_SIM_STEPS,
4657            &checkpoints,
4658            optimize_linearized_controls_original,
4659        );
4660        let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4661            MAX_SIM_STEPS,
4662            &checkpoints,
4663            optimize_linearized_controls_with_full_candidate_expansion,
4664        );
4665
4666        assert_eq!(default_checkpoints.len(), checkpoints.len());
4667        assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4668
4669        for ((default, expanded), expected_step) in default_checkpoints
4670            .iter()
4671            .zip(expanded_checkpoints.iter())
4672            .zip(checkpoints)
4673        {
4674            assert_eq!(default.step, expected_step);
4675            assert_eq!(expanded.step, expected_step);
4676        }
4677
4678        let step_80_delta = goal_distance_delta(&default_checkpoints[0], &expanded_checkpoints[0]);
4679        let step_120_delta = goal_distance_delta(&default_checkpoints[1], &expanded_checkpoints[1]);
4680        let step_160_delta = goal_distance_delta(&default_checkpoints[2], &expanded_checkpoints[2]);
4681        let step_200_delta = goal_distance_delta(&default_checkpoints[3], &expanded_checkpoints[3]);
4682        let step_260_delta = goal_distance_delta(&default_checkpoints[4], &expanded_checkpoints[4]);
4683        let step_320_delta = goal_distance_delta(&default_checkpoints[5], &expanded_checkpoints[5]);
4684        let step_346_delta = goal_distance_delta(&default_checkpoints[6], &expanded_checkpoints[6]);
4685
4686        assert!(
4687            (-0.03..=-0.005).contains(&step_80_delta),
4688            "step 80 delta moved unexpectedly: default={:?} expanded={:?}",
4689            default_checkpoints[0],
4690            expanded_checkpoints[0]
4691        );
4692        assert!(
4693            (0.15..=0.25).contains(&step_120_delta),
4694            "step 120 delta moved unexpectedly: default={:?} expanded={:?}",
4695            default_checkpoints[1],
4696            expanded_checkpoints[1]
4697        );
4698        assert!(
4699            (-0.35..=-0.2).contains(&step_160_delta),
4700            "step 160 delta moved unexpectedly: default={:?} expanded={:?}",
4701            default_checkpoints[2],
4702            expanded_checkpoints[2]
4703        );
4704        assert!(
4705            (-0.35..=-0.15).contains(&step_200_delta),
4706            "step 200 delta moved unexpectedly: default={:?} expanded={:?}",
4707            default_checkpoints[3],
4708            expanded_checkpoints[3]
4709        );
4710        assert!(
4711            (0.8..=1.2).contains(&step_260_delta),
4712            "step 260 delta moved unexpectedly: default={:?} expanded={:?}",
4713            default_checkpoints[4],
4714            expanded_checkpoints[4]
4715        );
4716        assert!(
4717            (1.4..=1.8).contains(&step_320_delta),
4718            "step 320 delta moved unexpectedly: default={:?} expanded={:?}",
4719            default_checkpoints[5],
4720            expanded_checkpoints[5]
4721        );
4722        assert!(
4723            (0.15..=0.25).contains(&step_346_delta),
4724            "step 346 delta moved unexpectedly: default={:?} expanded={:?}",
4725            default_checkpoints[6],
4726            expanded_checkpoints[6]
4727        );
4728
4729        assert_eq!(default_checkpoints[5].target_index, 159);
4730        assert_eq!(expanded_checkpoints[5].target_index, 161);
4731        assert_eq!(default_checkpoints[6].target_index, 161);
4732        assert_eq!(expanded_checkpoints[6].target_index, 161);
4733    }
4734
4735    #[test]
4736    #[ignore = "long-running regression scenario"]
4737    fn test_mpc_switch_back_full_candidate_expansion_characterizes_dense_recovery_window() {
4738        let checkpoints = [
4739            120usize, 130, 140, 150, 160, 170, 180, 190, 200, 210, 220, 230, 240, 250, 260,
4740        ];
4741        let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4742            MAX_SIM_STEPS,
4743            &checkpoints,
4744            optimize_linearized_controls_original,
4745        );
4746        let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4747            MAX_SIM_STEPS,
4748            &checkpoints,
4749            optimize_linearized_controls_with_full_candidate_expansion,
4750        );
4751
4752        assert_eq!(default_checkpoints.len(), checkpoints.len());
4753        assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4754
4755        let step_130_default = checkpoint_at_step(&default_checkpoints, 130);
4756        let step_130_expanded = checkpoint_at_step(&expanded_checkpoints, 130);
4757        let step_140_default = checkpoint_at_step(&default_checkpoints, 140);
4758        let step_140_expanded = checkpoint_at_step(&expanded_checkpoints, 140);
4759        let step_180_default = checkpoint_at_step(&default_checkpoints, 180);
4760        let step_180_expanded = checkpoint_at_step(&expanded_checkpoints, 180);
4761        let step_200_default = checkpoint_at_step(&default_checkpoints, 200);
4762        let step_200_expanded = checkpoint_at_step(&expanded_checkpoints, 200);
4763        let step_210_default = checkpoint_at_step(&default_checkpoints, 210);
4764        let step_210_expanded = checkpoint_at_step(&expanded_checkpoints, 210);
4765        let step_220_default = checkpoint_at_step(&default_checkpoints, 220);
4766        let step_220_expanded = checkpoint_at_step(&expanded_checkpoints, 220);
4767        let step_240_default = checkpoint_at_step(&default_checkpoints, 240);
4768        let step_240_expanded = checkpoint_at_step(&expanded_checkpoints, 240);
4769        let step_260_default = checkpoint_at_step(&default_checkpoints, 260);
4770        let step_260_expanded = checkpoint_at_step(&expanded_checkpoints, 260);
4771
4772        let step_130_delta = goal_distance_delta(step_130_default, step_130_expanded);
4773        let step_140_delta = goal_distance_delta(step_140_default, step_140_expanded);
4774        let step_180_delta = goal_distance_delta(step_180_default, step_180_expanded);
4775        let step_200_delta = goal_distance_delta(step_200_default, step_200_expanded);
4776        let step_210_delta = goal_distance_delta(step_210_default, step_210_expanded);
4777        let step_220_delta = goal_distance_delta(step_220_default, step_220_expanded);
4778        let step_240_delta = goal_distance_delta(step_240_default, step_240_expanded);
4779        let step_260_delta = goal_distance_delta(step_260_default, step_260_expanded);
4780
4781        assert!(
4782            (0.1..=0.18).contains(&step_130_delta),
4783            "step 130 delta moved unexpectedly: default={:?} expanded={:?}",
4784            step_130_default,
4785            step_130_expanded
4786        );
4787        assert!(
4788            (-0.2..=-0.05).contains(&step_140_delta),
4789            "step 140 delta moved unexpectedly: default={:?} expanded={:?}",
4790            step_140_default,
4791            step_140_expanded
4792        );
4793        assert!(
4794            (0.05..=0.15).contains(&step_180_delta),
4795            "step 180 delta moved unexpectedly: default={:?} expanded={:?}",
4796            step_180_default,
4797            step_180_expanded
4798        );
4799        assert!(
4800            (-0.35..=-0.15).contains(&step_200_delta),
4801            "step 200 delta moved unexpectedly: default={:?} expanded={:?}",
4802            step_200_default,
4803            step_200_expanded
4804        );
4805        assert!(
4806            (-0.2..=-0.08).contains(&step_210_delta),
4807            "step 210 delta moved unexpectedly: default={:?} expanded={:?}",
4808            step_210_default,
4809            step_210_expanded
4810        );
4811        assert!(
4812            (0.35..=0.55).contains(&step_220_delta),
4813            "step 220 delta moved unexpectedly: default={:?} expanded={:?}",
4814            step_220_default,
4815            step_220_expanded
4816        );
4817        assert!(
4818            (0.3..=0.55).contains(&step_240_delta),
4819            "step 240 delta moved unexpectedly: default={:?} expanded={:?}",
4820            step_240_default,
4821            step_240_expanded
4822        );
4823        assert!(
4824            (0.85..=1.05).contains(&step_260_delta),
4825            "step 260 delta moved unexpectedly: default={:?} expanded={:?}",
4826            step_260_default,
4827            step_260_expanded
4828        );
4829
4830        assert_eq!(step_140_default.target_index, 71);
4831        assert_eq!(step_140_expanded.target_index, 72);
4832        assert_eq!(step_180_default.target_index, 94);
4833        assert_eq!(step_180_expanded.target_index, 95);
4834        assert_eq!(step_220_default.target_index, 105);
4835        assert_eq!(step_220_expanded.target_index, 106);
4836        assert_eq!(step_240_default.target_index, 115);
4837        assert_eq!(step_240_expanded.target_index, 117);
4838        assert_eq!(step_260_default.target_index, 127);
4839        assert_eq!(step_260_expanded.target_index, 129);
4840    }
4841
4842    #[test]
4843    #[ignore = "long-running regression scenario"]
4844    fn test_mpc_switch_back_full_candidate_expansion_characterizes_flip_window_210_220() {
4845        let checkpoints = [210usize, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220];
4846        let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4847            MAX_SIM_STEPS,
4848            &checkpoints,
4849            optimize_linearized_controls_original,
4850        );
4851        let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4852            MAX_SIM_STEPS,
4853            &checkpoints,
4854            optimize_linearized_controls_with_full_candidate_expansion,
4855        );
4856
4857        assert_eq!(default_checkpoints.len(), checkpoints.len());
4858        assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4859
4860        for ((default, expanded), expected_step) in default_checkpoints
4861            .iter()
4862            .zip(expanded_checkpoints.iter())
4863            .zip(checkpoints)
4864        {
4865            assert_eq!(default.step, expected_step);
4866            assert_eq!(expanded.step, expected_step);
4867        }
4868
4869        let step_210_default = checkpoint_at_step(&default_checkpoints, 210);
4870        let step_210_expanded = checkpoint_at_step(&expanded_checkpoints, 210);
4871        let step_212_default = checkpoint_at_step(&default_checkpoints, 212);
4872        let step_212_expanded = checkpoint_at_step(&expanded_checkpoints, 212);
4873        let step_213_default = checkpoint_at_step(&default_checkpoints, 213);
4874        let step_213_expanded = checkpoint_at_step(&expanded_checkpoints, 213);
4875        let step_214_default = checkpoint_at_step(&default_checkpoints, 214);
4876        let step_214_expanded = checkpoint_at_step(&expanded_checkpoints, 214);
4877        let step_216_default = checkpoint_at_step(&default_checkpoints, 216);
4878        let step_216_expanded = checkpoint_at_step(&expanded_checkpoints, 216);
4879        let step_220_default = checkpoint_at_step(&default_checkpoints, 220);
4880        let step_220_expanded = checkpoint_at_step(&expanded_checkpoints, 220);
4881
4882        let step_210_delta = goal_distance_delta(step_210_default, step_210_expanded);
4883        let step_212_delta = goal_distance_delta(step_212_default, step_212_expanded);
4884        let step_213_delta = goal_distance_delta(step_213_default, step_213_expanded);
4885        let step_214_delta = goal_distance_delta(step_214_default, step_214_expanded);
4886        let step_216_delta = goal_distance_delta(step_216_default, step_216_expanded);
4887        let step_220_delta = goal_distance_delta(step_220_default, step_220_expanded);
4888
4889        assert!(
4890            (-0.18..=-0.1).contains(&step_210_delta),
4891            "step 210 delta moved unexpectedly: default={:?} expanded={:?}",
4892            step_210_default,
4893            step_210_expanded
4894        );
4895        assert!(
4896            (-0.08..=-0.03).contains(&step_212_delta),
4897            "step 212 delta moved unexpectedly: default={:?} expanded={:?}",
4898            step_212_default,
4899            step_212_expanded
4900        );
4901        assert!(
4902            (0.0..=0.02).contains(&step_213_delta),
4903            "step 213 delta moved unexpectedly: default={:?} expanded={:?}",
4904            step_213_default,
4905            step_213_expanded
4906        );
4907        assert!(
4908            (0.05..=0.09).contains(&step_214_delta),
4909            "step 214 delta moved unexpectedly: default={:?} expanded={:?}",
4910            step_214_default,
4911            step_214_expanded
4912        );
4913        assert!(
4914            (0.18..=0.25).contains(&step_216_delta),
4915            "step 216 delta moved unexpectedly: default={:?} expanded={:?}",
4916            step_216_default,
4917            step_216_expanded
4918        );
4919        assert!(
4920            (0.42..=0.5).contains(&step_220_delta),
4921            "step 220 delta moved unexpectedly: default={:?} expanded={:?}",
4922            step_220_default,
4923            step_220_expanded
4924        );
4925
4926        assert_eq!(step_210_default.target_index, 103);
4927        assert_eq!(step_210_expanded.target_index, 103);
4928        assert_eq!(step_212_default.target_index, 103);
4929        assert_eq!(step_212_expanded.target_index, 103);
4930        assert_eq!(step_213_default.target_index, 103);
4931        assert_eq!(step_213_expanded.target_index, 103);
4932        assert_eq!(step_214_default.target_index, 103);
4933        assert_eq!(step_214_expanded.target_index, 104);
4934        assert_eq!(step_216_default.target_index, 103);
4935        assert_eq!(step_216_expanded.target_index, 104);
4936        assert_eq!(step_220_default.target_index, 105);
4937        assert_eq!(step_220_expanded.target_index, 106);
4938    }
4939
4940    #[test]
4941    #[ignore = "long-running regression scenario"]
4942    fn test_mpc_switch_back_full_candidate_expansion_characterizes_progression_to_recovery() {
4943        let checkpoints = [213usize, 214, 220, 230, 240, 260];
4944        let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4945            MAX_SIM_STEPS,
4946            &checkpoints,
4947            optimize_linearized_controls_original,
4948        );
4949        let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
4950            MAX_SIM_STEPS,
4951            &checkpoints,
4952            optimize_linearized_controls_with_full_candidate_expansion,
4953        );
4954
4955        assert_eq!(default_checkpoints.len(), checkpoints.len());
4956        assert_eq!(expanded_checkpoints.len(), checkpoints.len());
4957
4958        let step_213_default = checkpoint_at_step(&default_checkpoints, 213);
4959        let step_213_expanded = checkpoint_at_step(&expanded_checkpoints, 213);
4960        let step_214_default = checkpoint_at_step(&default_checkpoints, 214);
4961        let step_214_expanded = checkpoint_at_step(&expanded_checkpoints, 214);
4962        let step_220_default = checkpoint_at_step(&default_checkpoints, 220);
4963        let step_220_expanded = checkpoint_at_step(&expanded_checkpoints, 220);
4964        let step_230_default = checkpoint_at_step(&default_checkpoints, 230);
4965        let step_230_expanded = checkpoint_at_step(&expanded_checkpoints, 230);
4966        let step_240_default = checkpoint_at_step(&default_checkpoints, 240);
4967        let step_240_expanded = checkpoint_at_step(&expanded_checkpoints, 240);
4968        let step_260_default = checkpoint_at_step(&default_checkpoints, 260);
4969        let step_260_expanded = checkpoint_at_step(&expanded_checkpoints, 260);
4970
4971        let step_214_y_delta = step_214_default.state[1] - step_214_expanded.state[1];
4972        let step_214_speed_delta = step_214_default.state[2] - step_214_expanded.state[2];
4973        let step_214_yaw_delta = angle_diff(step_214_default.state[3], step_214_expanded.state[3]);
4974        let step_220_y_delta = step_220_default.state[1] - step_220_expanded.state[1];
4975        let step_220_speed_delta = step_220_default.state[2] - step_220_expanded.state[2];
4976        let step_220_yaw_delta = angle_diff(step_220_default.state[3], step_220_expanded.state[3]);
4977        let step_230_y_delta = step_230_default.state[1] - step_230_expanded.state[1];
4978        let step_230_speed_delta = step_230_default.state[2] - step_230_expanded.state[2];
4979        let step_230_yaw_delta = angle_diff(step_230_default.state[3], step_230_expanded.state[3]);
4980        let step_240_y_delta = step_240_default.state[1] - step_240_expanded.state[1];
4981        let step_240_speed_delta = step_240_default.state[2] - step_240_expanded.state[2];
4982        let step_240_yaw_delta = angle_diff(step_240_default.state[3], step_240_expanded.state[3]);
4983        let step_260_y_delta = step_260_default.state[1] - step_260_expanded.state[1];
4984        let step_260_yaw_delta = angle_diff(step_260_default.state[3], step_260_expanded.state[3]);
4985
4986        assert!(
4987            (0.0..=0.02).contains(&goal_distance_delta(step_213_default, step_213_expanded)),
4988            "step 213 delta moved unexpectedly: default={:?} expanded={:?}",
4989            step_213_default,
4990            step_213_expanded
4991        );
4992        assert!(
4993            (0.05..=0.09).contains(&goal_distance_delta(step_214_default, step_214_expanded)),
4994            "step 214 delta moved unexpectedly: default={:?} expanded={:?}",
4995            step_214_default,
4996            step_214_expanded
4997        );
4998        assert!(
4999            (0.42..=0.5).contains(&goal_distance_delta(step_220_default, step_220_expanded)),
5000            "step 220 delta moved unexpectedly: default={:?} expanded={:?}",
5001            step_220_default,
5002            step_220_expanded
5003        );
5004        assert!(
5005            (0.5..=0.65).contains(&goal_distance_delta(step_230_default, step_230_expanded)),
5006            "step 230 delta moved unexpectedly: default={:?} expanded={:?}",
5007            step_230_default,
5008            step_230_expanded
5009        );
5010        assert!(
5011            (0.35..=0.5).contains(&goal_distance_delta(step_240_default, step_240_expanded)),
5012            "step 240 delta moved unexpectedly: default={:?} expanded={:?}",
5013            step_240_default,
5014            step_240_expanded
5015        );
5016        assert!(
5017            (0.9..=1.05).contains(&goal_distance_delta(step_260_default, step_260_expanded)),
5018            "step 260 delta moved unexpectedly: default={:?} expanded={:?}",
5019            step_260_default,
5020            step_260_expanded
5021        );
5022
5023        assert_eq!(step_213_default.target_index, 103);
5024        assert_eq!(step_213_expanded.target_index, 103);
5025        assert_eq!(step_214_default.target_index, 103);
5026        assert_eq!(step_214_expanded.target_index, 104);
5027        assert_eq!(step_220_default.target_index, 105);
5028        assert_eq!(step_220_expanded.target_index, 106);
5029        assert_eq!(step_230_default.target_index, 109);
5030        assert_eq!(step_230_expanded.target_index, 111);
5031        assert_eq!(step_240_default.target_index, 115);
5032        assert_eq!(step_240_expanded.target_index, 117);
5033        assert_eq!(step_260_default.target_index, 127);
5034        assert_eq!(step_260_expanded.target_index, 129);
5035
5036        assert!(
5037            (-1.1..=-0.95).contains(&step_214_y_delta),
5038            "step 214 y delta moved unexpectedly: default={:?} expanded={:?}",
5039            step_214_default,
5040            step_214_expanded
5041        );
5042        assert!((0.25..=0.32).contains(&step_214_speed_delta));
5043        assert!((-0.22..=-0.12).contains(&step_214_yaw_delta));
5044        assert!(
5045            (-0.95..=-0.75).contains(&step_220_y_delta),
5046            "step 220 y delta moved unexpectedly: default={:?} expanded={:?}",
5047            step_220_default,
5048            step_220_expanded
5049        );
5050        assert!((0.12..=0.22).contains(&step_220_speed_delta));
5051        assert!((-0.12..=-0.03).contains(&step_220_yaw_delta));
5052        assert!(
5053            (-1.2..=-0.95).contains(&step_230_y_delta),
5054            "step 230 y delta moved unexpectedly: default={:?} expanded={:?}",
5055            step_230_default,
5056            step_230_expanded
5057        );
5058        assert!((0.05..=0.15).contains(&step_230_speed_delta));
5059        assert!((-0.05..=0.01).contains(&step_230_yaw_delta));
5060        assert!(
5061            (-1.0..=-0.75).contains(&step_240_y_delta),
5062            "step 240 y delta moved unexpectedly: default={:?} expanded={:?}",
5063            step_240_default,
5064            step_240_expanded
5065        );
5066        assert!((-0.06..=0.02).contains(&step_240_speed_delta));
5067        assert!((-0.09..=-0.03).contains(&step_240_yaw_delta));
5068        assert!(
5069            (0.3..=0.6).contains(&step_260_y_delta),
5070            "step 260 y delta moved unexpectedly: default={:?} expanded={:?}",
5071            step_260_default,
5072            step_260_expanded
5073        );
5074        assert!(
5075            (-0.27..=-0.15).contains(&step_260_yaw_delta),
5076            "step 260 yaw delta moved unexpectedly: default={:?} expanded={:?}",
5077            step_260_default,
5078            step_260_expanded
5079        );
5080    }
5081
5082    #[test]
5083    #[ignore = "long-running regression scenario"]
5084    fn test_mpc_switch_back_full_candidate_expansion_characterizes_late_window_to_finish() {
5085        let checkpoints = [260usize, 280, 300, 320, 340, 346];
5086        let terminal_checkpoints: Vec<_> = (320usize..=350).collect();
5087        let default_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
5088            MAX_SIM_STEPS,
5089            &checkpoints,
5090            optimize_linearized_controls_original,
5091        );
5092        let expanded_checkpoints = collect_switch_back_closed_loop_checkpoints_with_test_optimizer(
5093            MAX_SIM_STEPS,
5094            &checkpoints,
5095            optimize_linearized_controls_with_full_candidate_expansion,
5096        );
5097        let default_terminal = collect_switch_back_goal_condition_snapshots_with_test_optimizer(
5098            MAX_SIM_STEPS,
5099            &terminal_checkpoints,
5100            optimize_linearized_controls_original,
5101        );
5102        let expanded_terminal = collect_switch_back_goal_condition_snapshots_with_test_optimizer(
5103            MAX_SIM_STEPS,
5104            &terminal_checkpoints,
5105            optimize_linearized_controls_with_full_candidate_expansion,
5106        );
5107        let default_result = run_switch_back_simulation_with_test_optimizer(
5108            MAX_SIM_STEPS,
5109            optimize_linearized_controls_original,
5110        );
5111        let expanded_result = run_switch_back_simulation_with_test_optimizer(
5112            MAX_SIM_STEPS,
5113            optimize_linearized_controls_with_full_candidate_expansion,
5114        );
5115
5116        let step_260_default = checkpoint_at_step(&default_checkpoints, 260);
5117        let step_260_expanded = checkpoint_at_step(&expanded_checkpoints, 260);
5118        let step_280_default = checkpoint_at_step(&default_checkpoints, 280);
5119        let step_280_expanded = checkpoint_at_step(&expanded_checkpoints, 280);
5120        let step_300_default = checkpoint_at_step(&default_checkpoints, 300);
5121        let step_300_expanded = checkpoint_at_step(&expanded_checkpoints, 300);
5122        let step_320_default = checkpoint_at_step(&default_checkpoints, 320);
5123        let step_320_expanded = checkpoint_at_step(&expanded_checkpoints, 320);
5124        let step_340_default = checkpoint_at_step(&default_checkpoints, 340);
5125        let step_340_expanded = checkpoint_at_step(&expanded_checkpoints, 340);
5126        let step_346_default = checkpoint_at_step(&default_checkpoints, 346);
5127        let step_346_expanded = checkpoint_at_step(&expanded_checkpoints, 346);
5128        let (cx, cy, cyaw) = generate_switch_back_course();
5129
5130        let slope_260_280_default = goal_distance_slope(step_260_default, step_280_default);
5131        let slope_260_280_expanded = goal_distance_slope(step_260_expanded, step_280_expanded);
5132        let slope_280_300_default = goal_distance_slope(step_280_default, step_300_default);
5133        let slope_280_300_expanded = goal_distance_slope(step_280_expanded, step_300_expanded);
5134        let slope_300_320_default = goal_distance_slope(step_300_default, step_320_default);
5135        let slope_300_320_expanded = goal_distance_slope(step_300_expanded, step_320_expanded);
5136        let slope_320_340_default = goal_distance_slope(step_320_default, step_340_default);
5137        let slope_320_340_expanded = goal_distance_slope(step_320_expanded, step_340_expanded);
5138        let slope_340_346_default = goal_distance_slope(step_340_default, step_346_default);
5139        let slope_340_346_expanded = goal_distance_slope(step_340_expanded, step_346_expanded);
5140        let lat_abs_slope_300_320_default = scalar_slope(
5141            checkpoint_lateral_error(step_300_default, &cx, &cy, &cyaw).abs(),
5142            checkpoint_lateral_error(step_320_default, &cx, &cy, &cyaw).abs(),
5143            20,
5144        );
5145        let lat_abs_slope_300_320_expanded = scalar_slope(
5146            checkpoint_lateral_error(step_300_expanded, &cx, &cy, &cyaw).abs(),
5147            checkpoint_lateral_error(step_320_expanded, &cx, &cy, &cyaw).abs(),
5148            20,
5149        );
5150        let heading_abs_slope_300_320_default = scalar_slope(
5151            checkpoint_heading_error(step_300_default, &cyaw).abs(),
5152            checkpoint_heading_error(step_320_default, &cyaw).abs(),
5153            20,
5154        );
5155        let heading_abs_slope_300_320_expanded = scalar_slope(
5156            checkpoint_heading_error(step_300_expanded, &cyaw).abs(),
5157            checkpoint_heading_error(step_320_expanded, &cyaw).abs(),
5158            20,
5159        );
5160        let speed_slope_300_320_default =
5161            scalar_slope(step_300_default.state[2], step_320_default.state[2], 20);
5162        let speed_slope_300_320_expanded =
5163            scalar_slope(step_300_expanded.state[2], step_320_expanded.state[2], 20);
5164        let lat_abs_slope_320_340_default = scalar_slope(
5165            checkpoint_lateral_error(step_320_default, &cx, &cy, &cyaw).abs(),
5166            checkpoint_lateral_error(step_340_default, &cx, &cy, &cyaw).abs(),
5167            20,
5168        );
5169        let lat_abs_slope_320_340_expanded = scalar_slope(
5170            checkpoint_lateral_error(step_320_expanded, &cx, &cy, &cyaw).abs(),
5171            checkpoint_lateral_error(step_340_expanded, &cx, &cy, &cyaw).abs(),
5172            20,
5173        );
5174        let heading_abs_slope_320_340_default = scalar_slope(
5175            checkpoint_heading_error(step_320_default, &cyaw).abs(),
5176            checkpoint_heading_error(step_340_default, &cyaw).abs(),
5177            20,
5178        );
5179        let heading_abs_slope_320_340_expanded = scalar_slope(
5180            checkpoint_heading_error(step_320_expanded, &cyaw).abs(),
5181            checkpoint_heading_error(step_340_expanded, &cyaw).abs(),
5182            20,
5183        );
5184        let speed_slope_320_340_default =
5185            scalar_slope(step_320_default.state[2], step_340_default.state[2], 20);
5186        let speed_slope_320_340_expanded =
5187            scalar_slope(step_320_expanded.state[2], step_340_expanded.state[2], 20);
5188
5189        assert_eq!(
5190            target_index_increment(step_260_default, step_280_default),
5191            10
5192        );
5193        assert_eq!(
5194            target_index_increment(step_260_expanded, step_280_expanded),
5195            10
5196        );
5197        assert!(slope_260_280_expanded > slope_260_280_default);
5198
5199        assert_eq!(
5200            target_index_increment(step_280_default, step_300_default),
5201            10
5202        );
5203        assert_eq!(
5204            target_index_increment(step_280_expanded, step_300_expanded),
5205            11
5206        );
5207        assert!(slope_280_300_expanded > slope_280_300_default);
5208
5209        assert_eq!(
5210            target_index_increment(step_300_default, step_320_default),
5211            12
5212        );
5213        assert_eq!(
5214            target_index_increment(step_300_expanded, step_320_expanded),
5215            11
5216        );
5217        assert!(slope_300_320_default > slope_300_320_expanded);
5218        assert!(lat_abs_slope_300_320_default < lat_abs_slope_300_320_expanded - 2.0e-4);
5219        assert!(heading_abs_slope_300_320_default < heading_abs_slope_300_320_expanded - 2.0e-4);
5220        assert!(speed_slope_300_320_expanded > speed_slope_300_320_default + 0.05);
5221
5222        assert_eq!(step_320_default.target_index, 159);
5223        assert_eq!(step_320_expanded.target_index, 161);
5224        assert_eq!(
5225            target_index_increment(step_320_default, step_340_default),
5226            2
5227        );
5228        assert_eq!(
5229            target_index_increment(step_320_expanded, step_340_expanded),
5230            0
5231        );
5232        assert!(slope_320_340_default > 0.0);
5233        assert!(slope_320_340_expanded < 0.0);
5234        assert!(lat_abs_slope_320_340_expanded < lat_abs_slope_320_340_default - 5.0e-5);
5235        assert!(heading_abs_slope_320_340_default < heading_abs_slope_320_340_expanded - 1.0e-4);
5236        assert!(speed_slope_320_340_default > speed_slope_320_340_expanded + 0.04);
5237
5238        assert_eq!(step_340_default.target_index, 161);
5239        assert_eq!(step_340_expanded.target_index, 161);
5240        assert_eq!(
5241            target_index_increment(step_340_default, step_346_default),
5242            0
5243        );
5244        assert_eq!(
5245            target_index_increment(step_340_expanded, step_346_expanded),
5246            0
5247        );
5248        assert!(slope_340_346_default > slope_340_346_expanded);
5249        assert!(step_340_default.goal_distance > step_340_expanded.goal_distance + 0.4);
5250        assert!(step_340_default.state[2] > step_340_expanded.state[2] + 0.3);
5251        assert!(
5252            (0.15..=0.25).contains(&goal_distance_delta(step_346_default, step_346_expanded)),
5253            "step 346 delta moved unexpectedly: default={:?} expanded={:?}",
5254            step_346_default,
5255            step_346_expanded
5256        );
5257
5258        assert_eq!(default_result.hist_x.len() - 1, 350);
5259        assert_eq!(expanded_result.hist_x.len() - 1, 346);
5260
5261        assert_eq!(
5262            first_goal_condition_step(&default_terminal, |s| s.near_end),
5263            Some(320)
5264        );
5265        assert_eq!(
5266            first_goal_condition_step(&default_terminal, |s| s.near_goal),
5267            Some(321)
5268        );
5269        assert_eq!(
5270            first_goal_condition_step(&default_terminal, |s| s.stopped),
5271            Some(333)
5272        );
5273        assert_eq!(
5274            first_goal_condition_step(&default_terminal, |s| s.reached_goal),
5275            Some(350)
5276        );
5277
5278        assert_eq!(
5279            first_goal_condition_step(&expanded_terminal, |s| s.near_end),
5280            Some(320)
5281        );
5282        assert_eq!(
5283            first_goal_condition_step(&expanded_terminal, |s| s.near_goal),
5284            Some(320)
5285        );
5286        assert_eq!(
5287            first_goal_condition_step(&expanded_terminal, |s| s.stopped),
5288            Some(329)
5289        );
5290        assert_eq!(
5291            first_goal_condition_step(&expanded_terminal, |s| s.reached_goal),
5292            Some(346)
5293        );
5294
5295        let default_step_346 = goal_snapshot_at_step(&default_terminal, 346);
5296        let default_step_350 = goal_snapshot_at_step(&default_terminal, 350);
5297        let expanded_step_329 = goal_snapshot_at_step(&expanded_terminal, 329);
5298        let expanded_step_330 = goal_snapshot_at_step(&expanded_terminal, 330);
5299        let expanded_step_335 = goal_snapshot_at_step(&expanded_terminal, 335);
5300        let expanded_step_346 = goal_snapshot_at_step(&expanded_terminal, 346);
5301        let expanded_first_positive_speed =
5302            first_goal_condition_step(&expanded_terminal, |s| s.speed > 0.0);
5303        let expanded_reacquired_near_goal = expanded_terminal
5304            .iter()
5305            .skip_while(|snapshot| snapshot.near_goal)
5306            .find(|snapshot| snapshot.near_goal)
5307            .map(|snapshot| snapshot.step);
5308        let expanded_course_curvature =
5309            course_curvature_at_index(&cx, &cy, &cyaw, expanded_step_330.target_index);
5310
5311        assert!(default_step_346.near_goal);
5312        assert!(default_step_346.near_end);
5313        assert!(!default_step_346.stopped);
5314        assert!(!default_step_346.reached_goal);
5315
5316        assert!(!expanded_step_329.near_goal);
5317        assert!(expanded_step_329.near_end);
5318        assert!(expanded_step_329.stopped);
5319        assert!(expanded_step_329.speed < 0.0);
5320        assert!(expanded_step_329.motion_curvature > 0.0);
5321
5322        assert!(!expanded_step_330.near_goal);
5323        assert!(expanded_step_330.near_end);
5324        assert!(!expanded_step_330.stopped);
5325        assert!(!expanded_step_330.reached_goal);
5326        assert_eq!(expanded_first_positive_speed, Some(330));
5327        assert_eq!(expanded_reacquired_near_goal, Some(335));
5328        assert!(expanded_step_330.speed > 0.0);
5329        assert!(expanded_step_330.motion_curvature < 0.0);
5330        assert!(expanded_course_curvature > 0.0);
5331
5332        assert!(expanded_step_335.near_goal);
5333        assert!(expanded_step_335.near_end);
5334        assert!(!expanded_step_335.stopped);
5335        assert!(!expanded_step_335.reached_goal);
5336        assert!(expanded_step_335.goal_distance < GOAL_DIS);
5337        assert!(expanded_step_335.heading_error.abs() < expanded_step_330.heading_error.abs());
5338        assert!(expanded_step_335.motion_curvature < 0.0);
5339        assert!(expanded_step_335.goal_distance < expanded_step_330.goal_distance);
5340        assert!(expanded_step_335.speed > expanded_step_330.speed);
5341
5342        assert!(expanded_step_346.near_goal);
5343        assert!(expanded_step_346.near_end);
5344        assert!(expanded_step_346.stopped);
5345        assert!(expanded_step_346.reached_goal);
5346
5347        assert!(default_step_350.near_goal);
5348        assert!(default_step_350.near_end);
5349        assert!(default_step_350.stopped);
5350        assert!(default_step_350.reached_goal);
5351    }
5352
5353    #[test]
5354    #[ignore = "long-running regression scenario"]
5355    fn test_mpc_switch_back_full_candidate_expansion_characterizes_terminal_reentry_controls() {
5356        let checkpoints: Vec<_> = (329usize..=350).collect();
5357        let default_snapshots = collect_switch_back_goal_control_snapshots_with_test_optimizer(
5358            MAX_SIM_STEPS,
5359            &checkpoints,
5360            optimize_linearized_controls_original,
5361        );
5362        let expanded_snapshots = collect_switch_back_goal_control_snapshots_with_test_optimizer(
5363            MAX_SIM_STEPS,
5364            &checkpoints,
5365            optimize_linearized_controls_with_full_candidate_expansion,
5366        );
5367
5368        let default_first_stopped =
5369            first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.stopped);
5370        let expanded_first_stopped =
5371            first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.stopped);
5372        let default_first_positive_speed =
5373            first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.speed > 0.0);
5374        let expanded_first_positive_speed =
5375            first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.speed > 0.0);
5376        let default_first_negative_steer =
5377            first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.steer < 0.0);
5378        let expanded_first_negative_steer =
5379            first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.steer < 0.0);
5380        let default_reacquired_near_goal =
5381            first_goal_control_condition_step(&default_snapshots, |snapshot| snapshot.near_goal);
5382        let expanded_reacquired_near_goal =
5383            first_goal_control_condition_step(&expanded_snapshots, |snapshot| snapshot.near_goal);
5384
5385        assert_eq!(default_first_stopped, Some(333));
5386        assert_eq!(expanded_first_stopped, Some(329));
5387        assert_eq!(default_first_positive_speed, Some(334));
5388        assert_eq!(expanded_first_positive_speed, Some(330));
5389        assert_eq!(default_first_negative_steer, Some(334));
5390        assert_eq!(expanded_first_negative_steer, Some(330));
5391        assert_eq!(default_reacquired_near_goal, Some(339));
5392        assert_eq!(expanded_reacquired_near_goal, Some(335));
5393        assert_eq!(
5394            default_first_positive_speed.unwrap() - default_first_stopped.unwrap(),
5395            1
5396        );
5397        assert_eq!(
5398            expanded_first_positive_speed.unwrap() - expanded_first_stopped.unwrap(),
5399            1
5400        );
5401        assert_eq!(
5402            default_reacquired_near_goal.unwrap() - default_first_positive_speed.unwrap(),
5403            5
5404        );
5405        assert_eq!(
5406            expanded_reacquired_near_goal.unwrap() - expanded_first_positive_speed.unwrap(),
5407            5
5408        );
5409        assert_eq!(
5410            default_first_positive_speed.unwrap() - expanded_first_positive_speed.unwrap(),
5411            4
5412        );
5413        assert_eq!(
5414            default_reacquired_near_goal.unwrap() - expanded_reacquired_near_goal.unwrap(),
5415            4
5416        );
5417
5418        let default_stop = goal_control_snapshot_at_step(&default_snapshots, 333);
5419        let expanded_stop = goal_control_snapshot_at_step(&expanded_snapshots, 329);
5420        let default_turn = goal_control_snapshot_at_step(&default_snapshots, 334);
5421        let expanded_turn = goal_control_snapshot_at_step(&expanded_snapshots, 330);
5422        let default_reentry = goal_control_snapshot_at_step(&default_snapshots, 339);
5423        let expanded_reentry = goal_control_snapshot_at_step(&expanded_snapshots, 335);
5424        let default_refinement = goal_control_snapshot_at_step(&default_snapshots, 346);
5425        let expanded_refinement = goal_control_snapshot_at_step(&expanded_snapshots, 342);
5426
5427        assert_goal_control_alignment(default_stop, expanded_stop);
5428        assert_goal_control_alignment(default_turn, expanded_turn);
5429        assert_goal_control_alignment(default_reentry, expanded_reentry);
5430        assert_goal_control_alignment(default_refinement, expanded_refinement);
5431
5432        assert!(default_stop.speed < 0.0);
5433        assert!(expanded_stop.speed < 0.0);
5434        assert!(default_stop.steer > 0.0);
5435        assert!(expanded_stop.steer > 0.0);
5436        assert!(default_stop.motion_curvature > 0.0);
5437        assert!(expanded_stop.motion_curvature > 0.0);
5438
5439        assert!(default_turn.speed > 0.0);
5440        assert!(expanded_turn.speed > 0.0);
5441        assert!(default_turn.steer < 0.0);
5442        assert!(expanded_turn.steer < 0.0);
5443        assert!(default_turn.motion_curvature < 0.0);
5444        assert!(expanded_turn.motion_curvature < 0.0);
5445        assert!(default_turn.course_curvature > 0.0);
5446        assert!(expanded_turn.course_curvature > 0.0);
5447
5448        assert!(default_reentry.near_goal);
5449        assert!(expanded_reentry.near_goal);
5450        assert!(default_reentry.speed > 1.0);
5451        assert!(expanded_reentry.speed > 1.0);
5452        assert!(default_reentry.motion_curvature < 0.0);
5453        assert!(expanded_reentry.motion_curvature < 0.0);
5454
5455        assert!(default_refinement.near_goal);
5456        assert!(expanded_refinement.near_goal);
5457        assert!(!default_refinement.stopped);
5458        assert!(!expanded_refinement.stopped);
5459    }
5460
5461    #[test]
5462    #[ignore = "long-running regression scenario"]
5463    fn test_mpc_switch_back_full_candidate_expansion_characterizes_terminal_planned_phase_lead() {
5464        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5465            346,
5466            optimize_linearized_controls_original,
5467        );
5468        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5469            342,
5470            optimize_linearized_controls_with_full_candidate_expansion,
5471        );
5472
5473        let default_stop = detailed_row_at_step(&default_trace, 332);
5474        let expanded_stop = detailed_row_at_step(&expanded_trace, 328);
5475        let default_turn = detailed_row_at_step(&default_trace, 333);
5476        let expanded_turn = detailed_row_at_step(&expanded_trace, 329);
5477        let default_reentry = detailed_row_at_step(&default_trace, 338);
5478        let expanded_reentry = detailed_row_at_step(&expanded_trace, 334);
5479        let default_refinement = detailed_row_at_step(&default_trace, 345);
5480        let expanded_refinement = detailed_row_at_step(&expanded_trace, 341);
5481
5482        assert_detailed_phase_alignment("terminal_stop", default_stop, expanded_stop);
5483        assert_detailed_phase_alignment("terminal_turn", default_turn, expanded_turn);
5484        assert_detailed_phase_alignment("terminal_reentry", default_reentry, expanded_reentry);
5485        assert_detailed_phase_alignment(
5486            "terminal_refinement",
5487            default_refinement,
5488            expanded_refinement,
5489        );
5490
5491        assert!(default_stop.controls[0][1] > 0.0);
5492        assert!(expanded_stop.controls[0][1] > 0.0);
5493        assert!(default_turn.controls[0][1] < 0.0);
5494        assert!(expanded_turn.controls[0][1] < 0.0);
5495        assert!(default_reentry.controls[0][0] > 0.2);
5496        assert!(expanded_reentry.controls[0][0] > 0.2);
5497        assert!(default_refinement.controls[0][0] < 0.0);
5498        assert!(expanded_refinement.controls[0][0] < 0.0);
5499    }
5500
5501    #[test]
5502    #[ignore = "long-running regression scenario"]
5503    fn test_mpc_switch_back_full_candidate_expansion_characterizes_pre_terminal_phase_origin() {
5504        let checkpoints = [321usize, 322, 323, 324, 325, 326, 327, 328, 329];
5505        let default_snapshots = collect_switch_back_planning_snapshots_with_test_optimizer(
5506            330,
5507            &checkpoints,
5508            optimize_linearized_controls_original,
5509        );
5510        let expanded_snapshots = collect_switch_back_planning_snapshots_with_test_optimizer(
5511            330,
5512            &checkpoints,
5513            optimize_linearized_controls_with_full_candidate_expansion,
5514        );
5515
5516        let same_325_default = planning_snapshot_at_step(&default_snapshots, 325);
5517        let same_325_expanded = planning_snapshot_at_step(&expanded_snapshots, 325);
5518        let same_328_default = planning_snapshot_at_step(&default_snapshots, 328);
5519        let same_328_expanded = planning_snapshot_at_step(&expanded_snapshots, 328);
5520        let same_329_default = planning_snapshot_at_step(&default_snapshots, 329);
5521        let same_329_expanded = planning_snapshot_at_step(&expanded_snapshots, 329);
5522
5523        assert_eq!(same_325_default.target_index, 161);
5524        assert_eq!(same_325_expanded.target_index, 161);
5525        assert_diff_bounds(
5526            "pre_terminal_same_325_xref_head",
5527            trace_state_diff(&same_325_default.xref_head, &same_325_expanded.xref_head),
5528            [1e-10; 4],
5529        );
5530        assert_diff_bounds(
5531            "pre_terminal_same_328_xref_head",
5532            trace_state_diff(&same_328_default.xref_head, &same_328_expanded.xref_head),
5533            [1e-10; 4],
5534        );
5535        assert_diff_bounds(
5536            "pre_terminal_same_329_xref_head",
5537            trace_state_diff(&same_329_default.xref_head, &same_329_expanded.xref_head),
5538            [1e-10; 4],
5539        );
5540
5541        assert!(same_325_default.state[1] - same_325_expanded.state[1] > 1.0);
5542        assert!(same_325_expanded.state[2] - same_325_default.state[2] > 0.79);
5543        assert!(same_328_default.state[1] - same_328_expanded.state[1] > 0.55);
5544        assert!(same_328_expanded.state[2] - same_328_default.state[2] > 0.79);
5545        assert!(same_325_default.warm_start_head[1] < 0.0);
5546        assert!(same_325_expanded.warm_start_head[1] > 0.0);
5547        assert!(same_328_default.warm_start_head[1] < 0.0);
5548        assert!(same_328_expanded.warm_start_head[1] > 0.0);
5549        assert!(same_329_default.warm_start_head[1] > 0.0);
5550        assert!(same_329_expanded.warm_start_head[1] > 0.0);
5551
5552        for (default_step, expanded_step) in [
5553            (325usize, 321usize),
5554            (326, 322),
5555            (327, 323),
5556            (328, 324),
5557            (329, 325),
5558        ] {
5559            let default = planning_snapshot_at_step(&default_snapshots, default_step);
5560            let expanded = planning_snapshot_at_step(&expanded_snapshots, expanded_step);
5561            assert_planning_snapshot_phase_alignment(
5562                &format!("pre_terminal_shifted_{default_step}_{expanded_step}"),
5563                default,
5564                expanded,
5565            );
5566        }
5567    }
5568
5569    #[test]
5570    #[ignore = "long-running regression scenario"]
5571    fn test_mpc_switch_back_full_candidate_expansion_characterizes_shifted_warm_start_injection() {
5572        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5573            334,
5574            optimize_linearized_controls_original,
5575        );
5576        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5577            330,
5578            optimize_linearized_controls_with_full_candidate_expansion,
5579        );
5580        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5581            334,
5582            optimize_linearized_controls_original,
5583        );
5584        let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5585            330,
5586            optimize_linearized_controls_with_full_candidate_expansion,
5587        );
5588
5589        for (default_step, expanded_step) in [
5590            (325usize, 321usize),
5591            (326, 322),
5592            (327, 323),
5593            (328, 324),
5594            (329, 325),
5595        ] {
5596            let default_row = detailed_row_at_step(&default_trace, default_step);
5597            let expanded_row = detailed_row_at_step(&expanded_trace, default_step);
5598            let injected_default = iterative_linear_mpc_control_with_test_optimizer(
5599                &default_row.xref,
5600                &state_from_vector4(&default_row.state),
5601                expanded_warm_start.get(&expanded_step).unwrap(),
5602                optimize_linearized_controls_original,
5603            );
5604            let injected_expanded = iterative_linear_mpc_control_with_test_optimizer(
5605                &expanded_row.xref,
5606                &state_from_vector4(&expanded_row.state),
5607                default_warm_start.get(&(default_step + 4)).unwrap(),
5608                optimize_linearized_controls_with_full_candidate_expansion,
5609            );
5610
5611            let default_control_rows = default_row
5612                .controls
5613                .iter()
5614                .map(|vec| [vec[0], vec[1]])
5615                .collect::<Vec<_>>();
5616            let default_predicted_rows = default_row
5617                .predicted
5618                .iter()
5619                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5620                .collect::<Vec<_>>();
5621            let expanded_control_rows = expanded_row
5622                .controls
5623                .iter()
5624                .map(|vec| [vec[0], vec[1]])
5625                .collect::<Vec<_>>();
5626            let expanded_predicted_rows = expanded_row
5627                .predicted
5628                .iter()
5629                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5630                .collect::<Vec<_>>();
5631
5632            assert_diff_bounds(
5633                &format!("shifted_warm_default_step_{default_step}_control"),
5634                summarize_vec2_sequence_diff(&injected_default.controls, &default_control_rows),
5635                [1e-3, 6e-4],
5636            );
5637            assert_diff_bounds(
5638                &format!("shifted_warm_default_step_{default_step}_predicted"),
5639                summarize_vec4_sequence_diff(&injected_default.predicted, &default_predicted_rows),
5640                [1.5e-3, 1.5e-3, 4e-4, 4e-4],
5641            );
5642            assert_diff_bounds(
5643                &format!("shifted_warm_expanded_step_{default_step}_control"),
5644                summarize_vec2_sequence_diff(&injected_expanded.controls, &expanded_control_rows),
5645                [1e-3, 6e-4],
5646            );
5647            assert_diff_bounds(
5648                &format!("shifted_warm_expanded_step_{default_step}_predicted"),
5649                summarize_vec4_sequence_diff(
5650                    &injected_expanded.predicted,
5651                    &expanded_predicted_rows,
5652                ),
5653                [1.5e-3, 1.5e-3, 4e-4, 4e-4],
5654            );
5655        }
5656    }
5657
5658    #[test]
5659    #[ignore = "long-running regression scenario"]
5660    fn test_mpc_switch_back_full_candidate_expansion_characterizes_same_time_warm_start_injection()
5661    {
5662        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5663            330,
5664            optimize_linearized_controls_original,
5665        );
5666        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5667            330,
5668            optimize_linearized_controls_with_full_candidate_expansion,
5669        );
5670        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5671            330,
5672            optimize_linearized_controls_original,
5673        );
5674        let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5675            330,
5676            optimize_linearized_controls_with_full_candidate_expansion,
5677        );
5678
5679        for step in [325usize, 326, 327, 328, 329] {
5680            let default_row = detailed_row_at_step(&default_trace, step);
5681            let expanded_row = detailed_row_at_step(&expanded_trace, step);
5682            let default_with_expanded_warm = iterative_linear_mpc_control_with_test_optimizer(
5683                &default_row.xref,
5684                &state_from_vector4(&default_row.state),
5685                expanded_warm_start.get(&step).unwrap(),
5686                optimize_linearized_controls_original,
5687            );
5688            let expanded_with_default_warm = iterative_linear_mpc_control_with_test_optimizer(
5689                &expanded_row.xref,
5690                &state_from_vector4(&expanded_row.state),
5691                default_warm_start.get(&step).unwrap(),
5692                optimize_linearized_controls_with_full_candidate_expansion,
5693            );
5694
5695            let default_baseline_controls = default_row
5696                .controls
5697                .iter()
5698                .map(|vec| [vec[0], vec[1]])
5699                .collect::<Vec<_>>();
5700            let expanded_baseline_controls = expanded_row
5701                .controls
5702                .iter()
5703                .map(|vec| [vec[0], vec[1]])
5704                .collect::<Vec<_>>();
5705            let default_baseline_predicted = default_row
5706                .predicted
5707                .iter()
5708                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5709                .collect::<Vec<_>>();
5710            let expanded_baseline_predicted = expanded_row
5711                .predicted
5712                .iter()
5713                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5714                .collect::<Vec<_>>();
5715
5716            let default_to_default_control = summarize_vec2_sequence_diff(
5717                &default_with_expanded_warm.controls,
5718                &default_baseline_controls,
5719            );
5720            let default_to_expanded_control = summarize_vec2_sequence_diff(
5721                &default_with_expanded_warm.controls,
5722                &expanded_baseline_controls,
5723            );
5724            let default_to_default_predicted = summarize_vec4_sequence_diff(
5725                &default_with_expanded_warm.predicted,
5726                &default_baseline_predicted,
5727            );
5728            let default_to_expanded_predicted = summarize_vec4_sequence_diff(
5729                &default_with_expanded_warm.predicted,
5730                &expanded_baseline_predicted,
5731            );
5732            let expanded_to_expanded_control = summarize_vec2_sequence_diff(
5733                &expanded_with_default_warm.controls,
5734                &expanded_baseline_controls,
5735            );
5736            let expanded_to_default_control = summarize_vec2_sequence_diff(
5737                &expanded_with_default_warm.controls,
5738                &default_baseline_controls,
5739            );
5740            let expanded_to_expanded_predicted = summarize_vec4_sequence_diff(
5741                &expanded_with_default_warm.predicted,
5742                &expanded_baseline_predicted,
5743            );
5744            let expanded_to_default_predicted = summarize_vec4_sequence_diff(
5745                &expanded_with_default_warm.predicted,
5746                &default_baseline_predicted,
5747            );
5748
5749            assert_diff_bounds(
5750                &format!("same_time_warm_default_step_{step}_control"),
5751                default_to_default_control,
5752                [1.6e-2, 1.1e-2],
5753            );
5754            assert_diff_bounds(
5755                &format!("same_time_warm_default_step_{step}_predicted"),
5756                default_to_default_predicted,
5757                [2.5e-3, 3e-4, 3.5e-3, 5e-3],
5758            );
5759            assert!(
5760                default_to_expanded_control[0] > 0.7,
5761                "same-time default branch drifted toward expanded control unexpectedly: step={} same={:?} opposite={:?}",
5762                step,
5763                default_to_default_control,
5764                default_to_expanded_control
5765            );
5766            assert!(
5767                default_to_expanded_predicted[1] > 0.4
5768                    && default_to_expanded_predicted[2] > 0.79,
5769                "same-time default branch drifted toward expanded prediction unexpectedly: step={} same={:?} opposite={:?}",
5770                step,
5771                default_to_default_predicted,
5772                default_to_expanded_predicted
5773            );
5774
5775            assert_diff_bounds(
5776                &format!("same_time_warm_expanded_step_{step}_control"),
5777                expanded_to_expanded_control,
5778                [6.5e-3, 3.6e-3],
5779            );
5780            assert_diff_bounds(
5781                &format!("same_time_warm_expanded_step_{step}_predicted"),
5782                expanded_to_expanded_predicted,
5783                [1e-4, 3e-4, 1.3e-3, 6e-4],
5784            );
5785            assert!(
5786                expanded_to_default_control[0] > 0.7,
5787                "same-time expanded branch drifted toward default control unexpectedly: step={} same={:?} opposite={:?}",
5788                step,
5789                expanded_to_expanded_control,
5790                expanded_to_default_control
5791            );
5792            assert!(
5793                expanded_to_default_predicted[1] > 0.4
5794                    && expanded_to_default_predicted[2] > 0.79,
5795                "same-time expanded branch drifted toward default prediction unexpectedly: step={} same={:?} opposite={:?}",
5796                step,
5797                expanded_to_expanded_predicted,
5798                expanded_to_default_predicted
5799            );
5800        }
5801    }
5802
5803    #[test]
5804    #[ignore = "long-running regression scenario"]
5805    fn test_mpc_switch_back_full_candidate_expansion_characterizes_same_time_state_injection() {
5806        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5807            330,
5808            optimize_linearized_controls_original,
5809        );
5810        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
5811            330,
5812            optimize_linearized_controls_with_full_candidate_expansion,
5813        );
5814        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5815            330,
5816            optimize_linearized_controls_original,
5817        );
5818        let expanded_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
5819            330,
5820            optimize_linearized_controls_with_full_candidate_expansion,
5821        );
5822
5823        for step in [325usize, 326, 327, 328, 329] {
5824            let default_row = detailed_row_at_step(&default_trace, step);
5825            let expanded_row = detailed_row_at_step(&expanded_trace, step);
5826            let default_with_expanded_state = iterative_linear_mpc_control_with_test_optimizer(
5827                &default_row.xref,
5828                &state_from_vector4(&expanded_row.state),
5829                default_warm_start.get(&step).unwrap(),
5830                optimize_linearized_controls_original,
5831            );
5832            let expanded_with_default_state = iterative_linear_mpc_control_with_test_optimizer(
5833                &expanded_row.xref,
5834                &state_from_vector4(&default_row.state),
5835                expanded_warm_start.get(&step).unwrap(),
5836                optimize_linearized_controls_with_full_candidate_expansion,
5837            );
5838
5839            let default_baseline_controls = default_row
5840                .controls
5841                .iter()
5842                .map(|vec| [vec[0], vec[1]])
5843                .collect::<Vec<_>>();
5844            let expanded_baseline_controls = expanded_row
5845                .controls
5846                .iter()
5847                .map(|vec| [vec[0], vec[1]])
5848                .collect::<Vec<_>>();
5849            let default_baseline_predicted = default_row
5850                .predicted
5851                .iter()
5852                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5853                .collect::<Vec<_>>();
5854            let expanded_baseline_predicted = expanded_row
5855                .predicted
5856                .iter()
5857                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
5858                .collect::<Vec<_>>();
5859
5860            let default_to_default_control = summarize_vec2_sequence_diff(
5861                &default_with_expanded_state.controls,
5862                &default_baseline_controls,
5863            );
5864            let default_to_expanded_control = summarize_vec2_sequence_diff(
5865                &default_with_expanded_state.controls,
5866                &expanded_baseline_controls,
5867            );
5868            let default_to_default_predicted = summarize_vec4_sequence_diff(
5869                &default_with_expanded_state.predicted,
5870                &default_baseline_predicted,
5871            );
5872            let default_to_expanded_predicted = summarize_vec4_sequence_diff(
5873                &default_with_expanded_state.predicted,
5874                &expanded_baseline_predicted,
5875            );
5876            let expanded_to_expanded_control = summarize_vec2_sequence_diff(
5877                &expanded_with_default_state.controls,
5878                &expanded_baseline_controls,
5879            );
5880            let expanded_to_default_control = summarize_vec2_sequence_diff(
5881                &expanded_with_default_state.controls,
5882                &default_baseline_controls,
5883            );
5884            let expanded_to_expanded_predicted = summarize_vec4_sequence_diff(
5885                &expanded_with_default_state.predicted,
5886                &expanded_baseline_predicted,
5887            );
5888            let expanded_to_default_predicted = summarize_vec4_sequence_diff(
5889                &expanded_with_default_state.predicted,
5890                &default_baseline_predicted,
5891            );
5892
5893            assert!(
5894                default_to_default_control[0] > 0.5,
5895                "same-time default-state injection did not move control away from default baseline enough: step={} same={:?} opposite={:?}",
5896                step,
5897                default_to_default_control,
5898                default_to_expanded_control
5899            );
5900            assert!(
5901                default_to_default_predicted[1] > 0.35 && default_to_default_predicted[2] > 0.7,
5902                "same-time default-state injection did not move prediction away from default baseline enough: step={} same={:?} opposite={:?}",
5903                step,
5904                default_to_default_predicted,
5905                default_to_expanded_predicted
5906            );
5907            assert!(
5908                default_to_default_control[0] > 4.0 * default_to_expanded_control[0],
5909                "same-time default-state injection stayed too close to default control branch: step={} same={:?} opposite={:?}",
5910                step,
5911                default_to_default_control,
5912                default_to_expanded_control
5913            );
5914            assert!(
5915                default_to_default_predicted[1] > 4.0 * default_to_expanded_predicted[1]
5916                    && default_to_default_predicted[2] > 4.0 * default_to_expanded_predicted[2],
5917                "same-time default-state injection stayed too close to default predicted branch: step={} same={:?} opposite={:?}",
5918                step,
5919                default_to_default_predicted,
5920                default_to_expanded_predicted
5921            );
5922
5923            assert!(
5924                expanded_to_expanded_control[0] > 0.5,
5925                "same-time expanded-state injection did not move control away from expanded baseline enough: step={} same={:?} opposite={:?}",
5926                step,
5927                expanded_to_expanded_control,
5928                expanded_to_default_control
5929            );
5930            assert!(
5931                expanded_to_expanded_predicted[1] > 0.35
5932                    && expanded_to_expanded_predicted[2] > 0.7,
5933                "same-time expanded-state injection did not move prediction away from expanded baseline enough: step={} same={:?} opposite={:?}",
5934                step,
5935                expanded_to_expanded_predicted,
5936                expanded_to_default_predicted
5937            );
5938            assert!(
5939                expanded_to_expanded_control[0] > 4.0 * expanded_to_default_control[0],
5940                "same-time expanded-state injection stayed too close to expanded control branch: step={} same={:?} opposite={:?}",
5941                step,
5942                expanded_to_expanded_control,
5943                expanded_to_default_control
5944            );
5945            assert!(
5946                expanded_to_expanded_predicted[1] > 4.0 * expanded_to_default_predicted[1]
5947                    && expanded_to_expanded_predicted[2] > 4.0 * expanded_to_default_predicted[2],
5948                "same-time expanded-state injection stayed too close to expanded predicted branch: step={} same={:?} opposite={:?}",
5949                step,
5950                expanded_to_expanded_predicted,
5951                expanded_to_default_predicted
5952            );
5953        }
5954    }
5955
5956    #[test]
5957    #[ignore = "long-running regression scenario"]
5958    fn test_mpc_switch_back_full_candidate_expansion_characterizes_state_component_injection() {
5959        let x_metrics = measure_switch_back_state_injection(&[0usize]);
5960        let y_metrics = measure_switch_back_state_injection(&[1usize]);
5961        let v_metrics = measure_switch_back_state_injection(&[2usize]);
5962        let yaw_metrics = measure_switch_back_state_injection(&[3usize]);
5963
5964        assert!(
5965            x_metrics.default_same_control0 < x_metrics.default_opposite_control0 / 100.0
5966                && x_metrics.default_same_pred_y < x_metrics.default_opposite_pred_y / 100.0
5967                && x_metrics.default_same_pred_v < x_metrics.default_opposite_pred_v / 100.0,
5968            "x-only injection moved the default branch more than expected: {:?}",
5969            x_metrics
5970        );
5971        assert!(
5972            x_metrics.expanded_same_control0 < x_metrics.expanded_opposite_control0 / 100.0
5973                && x_metrics.expanded_same_pred_y < x_metrics.expanded_opposite_pred_y / 100.0
5974                && x_metrics.expanded_same_pred_v < x_metrics.expanded_opposite_pred_v / 100.0,
5975            "x-only injection moved the expanded branch more than expected: {:?}",
5976            x_metrics
5977        );
5978
5979        assert!(
5980            yaw_metrics.default_same_control0 < yaw_metrics.default_opposite_control0 / 100.0
5981                && yaw_metrics.default_same_pred_y < yaw_metrics.default_opposite_pred_y / 100.0
5982                && yaw_metrics.default_same_pred_v < yaw_metrics.default_opposite_pred_v / 100.0,
5983            "yaw-only injection moved the default branch more than expected: {:?}",
5984            yaw_metrics
5985        );
5986        assert!(
5987            yaw_metrics.expanded_same_control0 < yaw_metrics.expanded_opposite_control0 / 100.0
5988                && yaw_metrics.expanded_same_pred_y < yaw_metrics.expanded_opposite_pred_y / 100.0
5989                && yaw_metrics.expanded_same_pred_v < yaw_metrics.expanded_opposite_pred_v / 100.0,
5990            "yaw-only injection moved the expanded branch more than expected: {:?}",
5991            yaw_metrics
5992        );
5993
5994        assert!(
5995            y_metrics.default_same_control0 < y_metrics.default_opposite_control0 / 4.0
5996                && y_metrics.default_same_pred_v < y_metrics.default_opposite_pred_v / 4.0
5997                && y_metrics.default_same_pred_y > 3.0,
5998            "y-only injection changed the default branch characterization unexpectedly: {:?}",
5999            y_metrics
6000        );
6001        assert!(
6002            y_metrics.expanded_same_control0 < y_metrics.expanded_opposite_control0 / 4.0
6003                && y_metrics.expanded_same_pred_v < y_metrics.expanded_opposite_pred_v / 4.0
6004                && y_metrics.expanded_same_pred_y > 3.0,
6005            "y-only injection changed the expanded branch characterization unexpectedly: {:?}",
6006            y_metrics
6007        );
6008
6009        assert!(
6010            v_metrics.default_same_control0 > 4.0 * v_metrics.default_opposite_control0
6011                && v_metrics.default_same_pred_v > 4.0 * v_metrics.default_opposite_pred_v,
6012            "v-only injection did not pull the default branch toward the opposite phase enough: {:?}",
6013            v_metrics
6014        );
6015        assert!(
6016            v_metrics.expanded_same_control0 > 4.0 * v_metrics.expanded_opposite_control0
6017                && v_metrics.expanded_same_pred_v > 4.0 * v_metrics.expanded_opposite_pred_v,
6018            "v-only injection did not pull the expanded branch toward the opposite phase enough: {:?}",
6019            v_metrics
6020        );
6021    }
6022
6023    #[test]
6024    #[ignore = "long-running regression scenario"]
6025    fn test_mpc_switch_back_full_candidate_expansion_characterizes_state_subset_injection() {
6026        let vy_metrics = measure_switch_back_state_injection(&[1usize, 2usize]);
6027        let vyyaw_metrics = measure_switch_back_state_injection(&[1usize, 2usize, 3usize]);
6028
6029        assert!(
6030            vy_metrics.default_same_control0 > 100.0 * vy_metrics.default_opposite_control0
6031                && vy_metrics.default_same_pred_y > 100.0 * vy_metrics.default_opposite_pred_y
6032                && vy_metrics.default_same_pred_v > 100.0 * vy_metrics.default_opposite_pred_v,
6033            "v+y injection did not pull the default branch strongly enough toward the opposite phase: {:?}",
6034            vy_metrics
6035        );
6036        assert!(
6037            vy_metrics.expanded_same_control0 > 100.0 * vy_metrics.expanded_opposite_control0
6038                && vy_metrics.expanded_same_pred_y > 100.0 * vy_metrics.expanded_opposite_pred_y
6039                && vy_metrics.expanded_same_pred_v > 100.0 * vy_metrics.expanded_opposite_pred_v,
6040            "v+y injection did not pull the expanded branch strongly enough toward the opposite phase: {:?}",
6041            vy_metrics
6042        );
6043
6044        assert!(
6045            vyyaw_metrics.default_same_control0 > 100.0 * vyyaw_metrics.default_opposite_control0
6046                && vyyaw_metrics.default_same_pred_y
6047                    > 100.0 * vyyaw_metrics.default_opposite_pred_y
6048                && vyyaw_metrics.default_same_pred_v
6049                    > 100.0 * vyyaw_metrics.default_opposite_pred_v,
6050            "v+y+yaw injection did not pull the default branch strongly enough toward the opposite phase: {:?}",
6051            vyyaw_metrics
6052        );
6053        assert!(
6054            vyyaw_metrics.expanded_same_control0
6055                > 100.0 * vyyaw_metrics.expanded_opposite_control0
6056                && vyyaw_metrics.expanded_same_pred_y
6057                    > 100.0 * vyyaw_metrics.expanded_opposite_pred_y
6058                && vyyaw_metrics.expanded_same_pred_v
6059                    > 100.0 * vyyaw_metrics.expanded_opposite_pred_v,
6060            "v+y+yaw injection did not pull the expanded branch strongly enough toward the opposite phase: {:?}",
6061            vyyaw_metrics
6062        );
6063
6064        assert!(
6065            (vy_metrics.default_same_control0 - vyyaw_metrics.default_same_control0).abs() <= 1e-3
6066                && (vy_metrics.default_opposite_control0
6067                    - vyyaw_metrics.default_opposite_control0)
6068                    .abs()
6069                    <= 1e-3
6070                && (vy_metrics.default_opposite_pred_y - vyyaw_metrics.default_opposite_pred_y)
6071                    .abs()
6072                    <= 1e-4
6073                && (vy_metrics.default_opposite_pred_v - vyyaw_metrics.default_opposite_pred_v)
6074                    .abs()
6075                    <= 2e-4,
6076            "adding yaw to v+y changed the default-branch subset metrics more than expected: vy={:?} vyyaw={:?}",
6077            vy_metrics,
6078            vyyaw_metrics
6079        );
6080        assert!(
6081            (vy_metrics.expanded_same_control0 - vyyaw_metrics.expanded_same_control0).abs()
6082                <= 1e-3
6083                && (vy_metrics.expanded_opposite_control0
6084                    - vyyaw_metrics.expanded_opposite_control0)
6085                    .abs()
6086                    <= 1e-3
6087                && (vy_metrics.expanded_opposite_pred_y - vyyaw_metrics.expanded_opposite_pred_y)
6088                    .abs()
6089                    <= 1e-4
6090                && (vy_metrics.expanded_opposite_pred_v - vyyaw_metrics.expanded_opposite_pred_v)
6091                    .abs()
6092                    <= 2e-4,
6093            "adding yaw to v+y changed the expanded-branch subset metrics more than expected: vy={:?} vyyaw={:?}",
6094            vy_metrics,
6095            vyyaw_metrics
6096        );
6097    }
6098
6099    #[test]
6100    #[ignore = "long-running regression scenario"]
6101    fn test_mpc_switch_back_full_candidate_expansion_characterizes_state_subset_per_step() {
6102        let steps = [325usize, 326, 327, 328, 329];
6103        let v_metrics = measure_switch_back_state_injection_by_step(&[2usize], &steps);
6104        let vy_metrics = measure_switch_back_state_injection_by_step(&[1usize, 2usize], &steps);
6105
6106        let step_325_v = state_injection_metrics_at_step(&v_metrics, 325);
6107        let step_325_vy = state_injection_metrics_at_step(&vy_metrics, 325);
6108        assert!(
6109            (step_325_v.default_opposite_control0 - step_325_vy.default_opposite_control0).abs()
6110                <= 5e-4,
6111            "default branch control moved too early at step 325: v={:?} vy={:?}",
6112            step_325_v,
6113            step_325_vy
6114        );
6115        assert!(
6116            step_325_v.default_opposite_pred_y > 10_000.0 * step_325_vy.default_opposite_pred_y,
6117            "default branch predicted-y should already need y at step 325: v={:?} vy={:?}",
6118            step_325_v,
6119            step_325_vy
6120        );
6121        assert!(
6122            step_325_v.expanded_opposite_pred_y > 1_000.0 * step_325_vy.expanded_opposite_pred_y,
6123            "expanded branch predicted-y should already improve with y at step 325: v={:?} vy={:?}",
6124            step_325_v,
6125            step_325_vy
6126        );
6127
6128        for step in [326usize, 327, 328, 329] {
6129            let v = state_injection_metrics_at_step(&v_metrics, step);
6130            let vy = state_injection_metrics_at_step(&vy_metrics, step);
6131
6132            assert!(
6133                v.default_opposite_control0 > 20.0 * vy.default_opposite_control0,
6134                "default branch control should need y from step {step}: v={:?} vy={:?}",
6135                v,
6136                vy
6137            );
6138            assert!(
6139                v.default_opposite_pred_y > 1_000.0 * vy.default_opposite_pred_y,
6140                "default branch predicted-y should strongly prefer v+y from step {step}: v={:?} vy={:?}",
6141                v,
6142                vy
6143            );
6144            assert!(
6145                v.expanded_opposite_pred_y > 1_000.0 * vy.expanded_opposite_pred_y,
6146                "expanded branch predicted-y should strongly prefer v+y from step {step}: v={:?} vy={:?}",
6147                v,
6148                vy
6149            );
6150        }
6151
6152        for step in [325usize, 326, 327, 328, 329] {
6153            let v = state_injection_metrics_at_step(&v_metrics, step);
6154            let vy = state_injection_metrics_at_step(&vy_metrics, step);
6155            assert!(
6156                (v.expanded_opposite_control0 - vy.expanded_opposite_control0).abs() <= 3e-3,
6157                "expanded branch control should stay mostly v-driven at step {step}: v={:?} vy={:?}",
6158                v,
6159                vy
6160            );
6161            assert!(
6162                (v.expanded_opposite_pred_v - vy.expanded_opposite_pred_v).abs() <= 6e-4,
6163                "expanded branch predicted-v should stay mostly v-driven at step {step}: v={:?} vy={:?}",
6164                v,
6165                vy
6166            );
6167        }
6168    }
6169
6170    /// Metrics for delta injection from step 325 → 326 transition analysis.
6171    #[derive(Debug)]
6172    struct StepTransitionDeltaMetrics {
6173        /// State delta from step 325 to step 326 (default branch).
6174        state_delta: Vector4<f64>,
6175        /// Baseline control[0] at step 325 (default branch).
6176        baseline_control0: [f64; 2],
6177        /// Expanded baseline control[0] at step 325.
6178        expanded_baseline_control0: [f64; 2],
6179        /// Per-component-set injection results: (component_label, injected_control0, same_diff, opposite_diff).
6180        injections: Vec<DeltaInjectionResult>,
6181    }
6182
6183    #[derive(Debug)]
6184    struct DeltaInjectionResult {
6185        label: &'static str,
6186        indices: Vec<usize>,
6187        injected_control0: [f64; 2],
6188        default_same_control0: f64,
6189        default_opposite_control0: f64,
6190        default_same_pred_y: f64,
6191        default_opposite_pred_y: f64,
6192        default_same_pred_v: f64,
6193        default_opposite_pred_v: f64,
6194    }
6195
6196    fn measure_step_transition_delta_injection() -> StepTransitionDeltaMetrics {
6197        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6198            330,
6199            optimize_linearized_controls_original,
6200        );
6201        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6202            330,
6203            optimize_linearized_controls_with_full_candidate_expansion,
6204        );
6205        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
6206            330,
6207            optimize_linearized_controls_original,
6208        );
6209
6210        let row_325 = detailed_row_at_step(&default_trace, 325);
6211        let row_326 = detailed_row_at_step(&default_trace, 326);
6212        let expanded_row_325 = detailed_row_at_step(&expanded_trace, 325);
6213
6214        let state_delta = row_326.state - row_325.state;
6215
6216        let default_baseline_controls: Vec<[f64; 2]> = row_325
6217            .controls
6218            .iter()
6219            .map(|vec| [vec[0], vec[1]])
6220            .collect();
6221        let expanded_baseline_controls: Vec<[f64; 2]> = expanded_row_325
6222            .controls
6223            .iter()
6224            .map(|vec| [vec[0], vec[1]])
6225            .collect();
6226        let default_baseline_predicted: Vec<[f64; 4]> = row_325
6227            .predicted
6228            .iter()
6229            .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6230            .collect();
6231        let expanded_baseline_predicted: Vec<[f64; 4]> = expanded_row_325
6232            .predicted
6233            .iter()
6234            .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6235            .collect();
6236
6237        let component_sets: Vec<(&str, Vec<usize>)> = vec![
6238            ("delta_v", vec![2]),
6239            ("delta_y", vec![1]),
6240            ("delta_v+delta_y", vec![1, 2]),
6241            ("delta_v+delta_y+delta_yaw", vec![1, 2, 3]),
6242        ];
6243
6244        let warm_start_325 = default_warm_start.get(&325).unwrap();
6245
6246        let mut injections = Vec::new();
6247        for (label, indices) in &component_sets {
6248            let mut modified_state_vec = row_325.state;
6249            for &idx in indices {
6250                modified_state_vec[idx] += state_delta[idx];
6251            }
6252            let modified_state = state_from_vector4(&modified_state_vec);
6253
6254            let result = iterative_linear_mpc_control_with_test_optimizer(
6255                &row_325.xref,
6256                &modified_state,
6257                warm_start_325,
6258                optimize_linearized_controls_original,
6259            );
6260
6261            let to_default_control =
6262                summarize_vec2_sequence_diff(&result.controls, &default_baseline_controls);
6263            let to_expanded_control =
6264                summarize_vec2_sequence_diff(&result.controls, &expanded_baseline_controls);
6265            let to_default_predicted =
6266                summarize_vec4_sequence_diff(&result.predicted, &default_baseline_predicted);
6267            let to_expanded_predicted =
6268                summarize_vec4_sequence_diff(&result.predicted, &expanded_baseline_predicted);
6269
6270            injections.push(DeltaInjectionResult {
6271                label,
6272                indices: indices.clone(),
6273                injected_control0: [result.controls[0][0], result.controls[0][1]],
6274                default_same_control0: to_default_control[0],
6275                default_opposite_control0: to_expanded_control[0],
6276                default_same_pred_y: to_default_predicted[1],
6277                default_opposite_pred_y: to_expanded_predicted[1],
6278                default_same_pred_v: to_default_predicted[2],
6279                default_opposite_pred_v: to_expanded_predicted[2],
6280            });
6281        }
6282
6283        StepTransitionDeltaMetrics {
6284            state_delta,
6285            baseline_control0: [row_325.controls[0][0], row_325.controls[0][1]],
6286            expanded_baseline_control0: [
6287                expanded_row_325.controls[0][0],
6288                expanded_row_325.controls[0][1],
6289            ],
6290            injections,
6291        }
6292    }
6293
6294    #[test]
6295    #[ignore = "long-running regression scenario"]
6296    fn test_mpc_switch_back_full_candidate_expansion_characterizes_step_325_326_transition() {
6297        let metrics = measure_step_transition_delta_injection();
6298
6299        println!("=== Step 325 → 326 Transition Delta Injection ===");
6300        println!("state_delta: {:?}", metrics.state_delta);
6301        println!(
6302            "baseline_control0 (default): {:?}",
6303            metrics.baseline_control0
6304        );
6305        println!(
6306            "baseline_control0 (expanded): {:?}",
6307            metrics.expanded_baseline_control0
6308        );
6309        for inj in &metrics.injections {
6310            println!("--- {} (indices {:?}) ---", inj.label, inj.indices);
6311            println!("  injected_control0: {:?}", inj.injected_control0);
6312            println!(
6313                "  same_control0: {:.6e}  opposite_control0: {:.6e}  ratio: {:.2}",
6314                inj.default_same_control0,
6315                inj.default_opposite_control0,
6316                if inj.default_opposite_control0 > 0.0 {
6317                    inj.default_same_control0 / inj.default_opposite_control0
6318                } else {
6319                    f64::INFINITY
6320                }
6321            );
6322            println!(
6323                "  same_pred_y: {:.6e}  opposite_pred_y: {:.6e}",
6324                inj.default_same_pred_y, inj.default_opposite_pred_y
6325            );
6326            println!(
6327                "  same_pred_v: {:.6e}  opposite_pred_v: {:.6e}",
6328                inj.default_same_pred_v, inj.default_opposite_pred_v
6329            );
6330        }
6331
6332        // delta_v alone: at step 325 v-only was sufficient, so applying
6333        // one step of delta_v should still keep control close to default baseline.
6334        let delta_v = &metrics.injections[0];
6335        assert!(
6336            delta_v.default_same_control0 < delta_v.default_opposite_control0,
6337            "delta_v at step 325 context should stay closer to default than expanded: {:?}",
6338            delta_v
6339        );
6340
6341        // delta_y alone: y alone had minimal control impact at step 325.
6342        let delta_y = &metrics.injections[1];
6343        assert!(
6344            delta_y.default_same_control0 < delta_y.default_opposite_control0,
6345            "delta_y at step 325 context should stay closer to default than expanded: {:?}",
6346            delta_y
6347        );
6348
6349        // delta_v+delta_y: the key question — does this combo start pulling
6350        // the control toward expanded, or does it stay default-like?
6351        // The handoff hypothesis: "delta_v + delta_y で step 326 の control が
6352        // 急に opposite 側へ寄る"
6353        let delta_vy = &metrics.injections[2];
6354        println!(
6355            "\n=== KEY: delta_v+delta_y same/opposite ratio = {:.4} ===",
6356            if delta_vy.default_opposite_control0 > 0.0 {
6357                delta_vy.default_same_control0 / delta_vy.default_opposite_control0
6358            } else {
6359                f64::INFINITY
6360            }
6361        );
6362
6363        // delta_v+delta_y+delta_yaw: should be nearly identical to delta_v+delta_y.
6364        let delta_vyy = &metrics.injections[3];
6365        let control0_diff =
6366            (delta_vy.default_same_control0 - delta_vyy.default_same_control0).abs();
6367        println!(
6368            "delta_v+delta_y vs delta_v+delta_y+delta_yaw control0 diff: {:.6e}",
6369            control0_diff
6370        );
6371    }
6372
6373    /// Result of v-only opposite-branch injection with context element swaps.
6374    #[derive(Debug)]
6375    struct ContextSwapInjectionResult {
6376        label: &'static str,
6377        injected_steer: f64,
6378        default_baseline_steer: f64,
6379        expanded_baseline_steer: f64,
6380        default_same_control0: f64,
6381        default_opposite_control0: f64,
6382        default_same_pred_y: f64,
6383        default_opposite_pred_y: f64,
6384        default_same_pred_v: f64,
6385        default_opposite_pred_v: f64,
6386    }
6387
6388    /// Investigate why v-only opposite-branch injection works at step 325 but
6389    /// fails at step 326 by systematically swapping context elements (state
6390    /// residual, xref, warm_start) between the two steps.
6391    fn measure_step_325_326_context_swap_v_injection() -> Vec<ContextSwapInjectionResult> {
6392        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6393            330,
6394            optimize_linearized_controls_original,
6395        );
6396        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6397            330,
6398            optimize_linearized_controls_with_full_candidate_expansion,
6399        );
6400        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
6401            330,
6402            optimize_linearized_controls_original,
6403        );
6404
6405        let d325 = detailed_row_at_step(&default_trace, 325);
6406        let d326 = detailed_row_at_step(&default_trace, 326);
6407        let e325 = detailed_row_at_step(&expanded_trace, 325);
6408        let e326 = detailed_row_at_step(&expanded_trace, 326);
6409        let ws325 = default_warm_start.get(&325).unwrap();
6410        let ws326 = default_warm_start.get(&326).unwrap();
6411
6412        let v_indices: &[usize] = &[2]; // v component only
6413
6414        // Helper to run v-only injection and compute metrics
6415        let run_injection = |label: &'static str,
6416                             default_state: &Vector4<f64>,
6417                             expanded_state: &Vector4<f64>,
6418                             xref: &[Vector4<f64>],
6419                             warm_start: &[Vector2<f64>],
6420                             default_row: &SwitchBackDetailedRow,
6421                             expanded_row: &SwitchBackDetailedRow| {
6422            let injected_state = inject_state_components(default_state, expanded_state, v_indices);
6423            let result = iterative_linear_mpc_control_with_test_optimizer(
6424                xref,
6425                &injected_state,
6426                warm_start,
6427                optimize_linearized_controls_original,
6428            );
6429
6430            let default_baseline_controls: Vec<[f64; 2]> = default_row
6431                .controls
6432                .iter()
6433                .map(|vec| [vec[0], vec[1]])
6434                .collect();
6435            let expanded_baseline_controls: Vec<[f64; 2]> = expanded_row
6436                .controls
6437                .iter()
6438                .map(|vec| [vec[0], vec[1]])
6439                .collect();
6440            let default_baseline_predicted: Vec<[f64; 4]> = default_row
6441                .predicted
6442                .iter()
6443                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6444                .collect();
6445            let expanded_baseline_predicted: Vec<[f64; 4]> = expanded_row
6446                .predicted
6447                .iter()
6448                .map(|vec| [vec[0], vec[1], vec[2], vec[3]])
6449                .collect();
6450
6451            let to_default_control =
6452                summarize_vec2_sequence_diff(&result.controls, &default_baseline_controls);
6453            let to_expanded_control =
6454                summarize_vec2_sequence_diff(&result.controls, &expanded_baseline_controls);
6455            let to_default_predicted =
6456                summarize_vec4_sequence_diff(&result.predicted, &default_baseline_predicted);
6457            let to_expanded_predicted =
6458                summarize_vec4_sequence_diff(&result.predicted, &expanded_baseline_predicted);
6459
6460            ContextSwapInjectionResult {
6461                label,
6462                injected_steer: result.controls[0][1],
6463                default_baseline_steer: default_row.controls[0][1],
6464                expanded_baseline_steer: expanded_row.controls[0][1],
6465                default_same_control0: to_default_control[0],
6466                default_opposite_control0: to_expanded_control[0],
6467                default_same_pred_y: to_default_predicted[1],
6468                default_opposite_pred_y: to_expanded_predicted[1],
6469                default_same_pred_v: to_default_predicted[2],
6470                default_opposite_pred_v: to_expanded_predicted[2],
6471            }
6472        };
6473
6474        vec![
6475            // Baseline: v-only injection at step 325 (known to work)
6476            run_injection(
6477                "baseline_325: d325_state + e325_v, d325_xref, ws325",
6478                &d325.state,
6479                &e325.state,
6480                &d325.xref,
6481                ws325,
6482                d325,
6483                e325,
6484            ),
6485            // Baseline: v-only injection at step 326 (known to fail)
6486            run_injection(
6487                "baseline_326: d326_state + e326_v, d326_xref, ws326",
6488                &d326.state,
6489                &e326.state,
6490                &d326.xref,
6491                ws326,
6492                d326,
6493                e326,
6494            ),
6495            // Swap state residual: use step 325 default state (non-v) with step 326 expanded v
6496            run_injection(
6497                "swap_state: d325_state + e326_v, d326_xref, ws326",
6498                &d325.state,
6499                &e326.state,
6500                &d326.xref,
6501                ws326,
6502                d326,
6503                e326,
6504            ),
6505            // Swap xref: use step 326 state but step 325 xref
6506            run_injection(
6507                "swap_xref: d326_state + e326_v, d325_xref, ws326",
6508                &d326.state,
6509                &e326.state,
6510                &d325.xref,
6511                ws326,
6512                d326,
6513                e326,
6514            ),
6515            // Swap warm_start: use step 326 state/xref but step 325 warm_start
6516            run_injection(
6517                "swap_ws: d326_state + e326_v, d326_xref, ws325",
6518                &d326.state,
6519                &e326.state,
6520                &d326.xref,
6521                ws325,
6522                d326,
6523                e326,
6524            ),
6525            // Swap state + xref: step 325 state/xref with step 326 expanded v
6526            run_injection(
6527                "swap_state+xref: d325_state + e326_v, d325_xref, ws326",
6528                &d325.state,
6529                &e326.state,
6530                &d325.xref,
6531                ws326,
6532                d326,
6533                e326,
6534            ),
6535            // Swap state + warm_start
6536            run_injection(
6537                "swap_state+ws: d325_state + e326_v, d326_xref, ws325",
6538                &d325.state,
6539                &e326.state,
6540                &d326.xref,
6541                ws325,
6542                d326,
6543                e326,
6544            ),
6545            // Swap xref + warm_start
6546            run_injection(
6547                "swap_xref+ws: d326_state + e326_v, d325_xref, ws325",
6548                &d326.state,
6549                &e326.state,
6550                &d325.xref,
6551                ws325,
6552                d326,
6553                e326,
6554            ),
6555            // Swap all three: effectively step 325 context with step 326 expanded v
6556            run_injection(
6557                "swap_all: d325_state + e326_v, d325_xref, ws325",
6558                &d325.state,
6559                &e326.state,
6560                &d325.xref,
6561                ws325,
6562                d326,
6563                e326,
6564            ),
6565            // Cross-step expanded v: use step 325 expanded v in step 326 default context
6566            // to isolate whether expanded v progression matters
6567            run_injection(
6568                "cross_v_325in326: d326_state + e325_v, d326_xref, ws326",
6569                &d326.state,
6570                &e325.state,
6571                &d326.xref,
6572                ws326,
6573                d326,
6574                e326,
6575            ),
6576            // Cross-step expanded v: use step 326 expanded v in step 325 default context
6577            run_injection(
6578                "cross_v_326in325: d325_state + e326_v, d325_xref, ws325",
6579                &d325.state,
6580                &e326.state,
6581                &d325.xref,
6582                ws325,
6583                d325,
6584                e325,
6585            ),
6586            // --- Per-component isolation: starting from baseline_325 (d325 + e325_v),
6587            //     replace individual default non-v components with d326 values ---
6588            // Replace only x
6589            {
6590                let mut s = d325.state;
6591                s[0] = d326.state[0]; // x
6592                run_injection(
6593                    "isolate_x: d325{x→d326} + e325_v",
6594                    &s,
6595                    &e325.state,
6596                    &d325.xref,
6597                    ws325,
6598                    d325,
6599                    e325,
6600                )
6601            },
6602            // Replace only y
6603            {
6604                let mut s = d325.state;
6605                s[1] = d326.state[1]; // y
6606                run_injection(
6607                    "isolate_y: d325{y→d326} + e325_v",
6608                    &s,
6609                    &e325.state,
6610                    &d325.xref,
6611                    ws325,
6612                    d325,
6613                    e325,
6614                )
6615            },
6616            // Replace only yaw
6617            {
6618                let mut s = d325.state;
6619                s[3] = d326.state[3]; // yaw
6620                run_injection(
6621                    "isolate_yaw: d325{yaw→d326} + e325_v",
6622                    &s,
6623                    &e325.state,
6624                    &d325.xref,
6625                    ws325,
6626                    d325,
6627                    e325,
6628                )
6629            },
6630            // Replace y + yaw
6631            {
6632                let mut s = d325.state;
6633                s[1] = d326.state[1];
6634                s[3] = d326.state[3];
6635                run_injection(
6636                    "isolate_y+yaw: d325{y,yaw→d326} + e325_v",
6637                    &s,
6638                    &e325.state,
6639                    &d325.xref,
6640                    ws325,
6641                    d325,
6642                    e325,
6643                )
6644            },
6645            // Replace x + y
6646            {
6647                let mut s = d325.state;
6648                s[0] = d326.state[0];
6649                s[1] = d326.state[1];
6650                run_injection(
6651                    "isolate_x+y: d325{x,y→d326} + e325_v",
6652                    &s,
6653                    &e325.state,
6654                    &d325.xref,
6655                    ws325,
6656                    d325,
6657                    e325,
6658                )
6659            },
6660            // Replace all non-v (x + y + yaw) → should match cross_v_326in325
6661            {
6662                let mut s = d326.state;
6663                s[2] = d325.state[2]; // keep default v from step 325
6664                run_injection(
6665                    "isolate_all_nonv: d326{v→d325} + e325_v",
6666                    &s,
6667                    &e325.state,
6668                    &d325.xref,
6669                    ws325,
6670                    d325,
6671                    e325,
6672                )
6673            },
6674        ]
6675    }
6676
6677    #[test]
6678    #[ignore = "long-running regression scenario"]
6679    fn test_mpc_switch_back_characterizes_step_325_326_v_injection_context_swap() {
6680        let results = measure_step_325_326_context_swap_v_injection();
6681
6682        println!("=== Step 325 vs 326: V-Only Injection Context Swap ===");
6683        println!("Question: why does v-only opposite-branch injection work at 325 but not 326?");
6684        println!();
6685
6686        // Print raw state values for reference
6687        println!("=== Raw State Values ===");
6688        println!("  (indices: x=0, y=1, v=2, yaw=3)");
6689        // We need to recompute traces to get raw values for printing
6690        let default_trace_print = collect_switch_back_detailed_trace_with_test_optimizer(
6691            330,
6692            optimize_linearized_controls_original,
6693        );
6694        let expanded_trace_print = collect_switch_back_detailed_trace_with_test_optimizer(
6695            330,
6696            optimize_linearized_controls_with_full_candidate_expansion,
6697        );
6698        let d325_p = detailed_row_at_step(&default_trace_print, 325);
6699        let d326_p = detailed_row_at_step(&default_trace_print, 326);
6700        let e325_p = detailed_row_at_step(&expanded_trace_print, 325);
6701        let e326_p = detailed_row_at_step(&expanded_trace_print, 326);
6702        println!("  d325.state: {:?}", d325_p.state);
6703        println!("  d326.state: {:?}", d326_p.state);
6704        println!("  d325→d326 delta: {:?}", d326_p.state - d325_p.state);
6705        println!("  e325.state: {:?}", e325_p.state);
6706        println!("  e326.state: {:?}", e326_p.state);
6707        println!("  e325→e326 delta: {:?}", e326_p.state - e325_p.state);
6708        println!(
6709            "  v diff: e325.v={:.6} e326.v={:.6} delta={:.6}",
6710            e325_p.state[2],
6711            e326_p.state[2],
6712            e326_p.state[2] - e325_p.state[2]
6713        );
6714        println!();
6715
6716        for r in &results {
6717            let pull_ratio = if r.default_opposite_control0 > 0.0 {
6718                r.default_same_control0 / r.default_opposite_control0
6719            } else {
6720                f64::INFINITY
6721            };
6722            let pulls_toward_expanded = pull_ratio > 4.0;
6723            println!(
6724                "[{}] {} steer={:.6} (default={:.6}, expanded={:.6})",
6725                if pulls_toward_expanded {
6726                    "PULL"
6727                } else {
6728                    "STAY"
6729                },
6730                r.label,
6731                r.injected_steer,
6732                r.default_baseline_steer,
6733                r.expanded_baseline_steer,
6734            );
6735            println!(
6736                "  same_ctrl0={:.4e} opp_ctrl0={:.4e} ratio={:.3} pred_y_s={:.4e} pred_y_o={:.4e} pred_v_s={:.4e} pred_v_o={:.4e}",
6737                r.default_same_control0,
6738                r.default_opposite_control0,
6739                pull_ratio,
6740                r.default_same_pred_y,
6741                r.default_opposite_pred_y,
6742                r.default_same_pred_v,
6743                r.default_opposite_pred_v,
6744            );
6745        }
6746
6747        // Baseline assertions
6748        let baseline_325 = &results[0];
6749        let ratio_325 = baseline_325.default_same_control0 / baseline_325.default_opposite_control0;
6750        assert!(
6751            ratio_325 > 100.0,
6752            "v-only injection should strongly pull toward expanded at step 325 (ratio={ratio_325}): {:?}",
6753            baseline_325
6754        );
6755
6756        let baseline_326 = &results[1];
6757        let ratio_326 = baseline_326.default_same_control0 / baseline_326.default_opposite_control0;
6758        assert!(
6759            ratio_326 < 10.0,
6760            "v-only injection pull should weaken dramatically at step 326 (ratio={ratio_326}): {:?}",
6761            baseline_326
6762        );
6763
6764        // The ratio drop from 325 to 326 should be at least 100x
6765        assert!(
6766            ratio_325 > 100.0 * ratio_326,
6767            "v-only pull should drop >100x from step 325 to 326: 325={ratio_325} 326={ratio_326}"
6768        );
6769
6770        // xref and warm_start swaps should not change the ratio
6771        let swap_xref = &results[3];
6772        let ratio_swap_xref = swap_xref.default_same_control0 / swap_xref.default_opposite_control0;
6773        assert!(
6774            (ratio_swap_xref - ratio_326).abs() < 0.1,
6775            "xref swap should not change ratio: baseline={ratio_326} swapped={ratio_swap_xref}"
6776        );
6777
6778        let swap_ws = &results[4];
6779        let ratio_swap_ws = swap_ws.default_same_control0 / swap_ws.default_opposite_control0;
6780        assert!(
6781            (ratio_swap_ws - ratio_326).abs() < 0.1,
6782            "warm_start swap should not change ratio: baseline={ratio_326} swapped={ratio_swap_ws}"
6783        );
6784
6785        // state swap should change the ratio (state is the driver)
6786        let swap_state = &results[2];
6787        let ratio_swap_state =
6788            swap_state.default_same_control0 / swap_state.default_opposite_control0;
6789        assert!(
6790            (ratio_swap_state - ratio_326).abs() > 0.5,
6791            "state swap should change ratio: baseline={ratio_326} swapped={ratio_swap_state}"
6792        );
6793
6794        // Per-component isolation: none of the default non-v changes
6795        // should destroy the pull (all ratios should stay > 100)
6796        for result in &results[11..=16] {
6797            let ratio = result.default_same_control0 / result.default_opposite_control0;
6798            assert!(
6799                ratio > 100.0,
6800                "default non-v component change should not destroy pull: {} ratio={ratio}",
6801                result.label
6802            );
6803        }
6804    }
6805
6806    /// Run MPC with a fixed number of outer iterations to isolate where y's
6807    /// effect enters the control. If y only matters through nonlinear
6808    /// xbar re-linearization (outer loop), then 1-iteration solve should
6809    /// show no y effect, while 3-iteration solve should.
6810    fn iterative_linear_mpc_control_with_max_iter(
6811        xref: &[Vector4<f64>],
6812        state: &State,
6813        warm_start: &[Vector2<f64>],
6814        max_iter: usize,
6815    ) -> MpcResult {
6816        let mut controls = warm_start.to_vec();
6817        controls.resize(T, Vector2::zeros());
6818        apply_control_constraints(&mut controls);
6819
6820        let mut predicted = predict_motion(*state, &controls);
6821        for _ in 0..max_iter {
6822            let previous_controls = controls.clone();
6823            controls = optimize_linearized_controls(xref, &predicted, state, &controls);
6824            predicted = predict_motion(*state, &controls);
6825
6826            let du = controls
6827                .iter()
6828                .zip(previous_controls.iter())
6829                .map(|(current, previous)| {
6830                    (current[0] - previous[0]).abs() + (current[1] - previous[1]).abs()
6831                })
6832                .sum::<f64>();
6833
6834            if du <= DU_TH {
6835                break;
6836            }
6837        }
6838
6839        MpcResult {
6840            controls,
6841            predicted,
6842        }
6843    }
6844
6845    /// Dump the first inner-solve adjoint to trace how y reaches steering gradient.
6846    /// Returns (steering_gradient, lambda3_components) for analysis.
6847    fn dump_first_inner_solve_adjoint(
6848        xref: &[Vector4<f64>],
6849        state: &State,
6850        warm_start: &[Vector2<f64>],
6851    ) -> (Vec<Vector2<f64>>, Vec<[f64; 5]>) {
6852        let mut controls = warm_start.to_vec();
6853        controls.resize(T, Vector2::zeros());
6854        apply_control_constraints(&mut controls);
6855
6856        let xbar = predict_motion(*state, &controls);
6857
6858        let q = state_cost_weight();
6859        let qf = terminal_cost_weight();
6860        let r = control_cost_weight();
6861        let rd = control_rate_weight();
6862
6863        let (x, a_seq, b_seq, _) = linearized_rollout(state, &xbar, &controls);
6864
6865        let mut gradients = vec![Vector2::zeros(); T];
6866        let mut lambda = (qf * state_error(x[T], xref[T])) * 2.0;
6867        let mut lambda3_decomp = Vec::new();
6868
6869        // Terminal lambda[3] decomposition
6870        lambda3_decomp.push([
6871            lambda[3], // total lambda[3]
6872            0.0,       // from lx[3]
6873            0.0,       // from A^T[3,0]*lambda_next[0]
6874            0.0,       // from A^T[3,1]*lambda_next[1]
6875            lambda[3], // from propagation (terminal)
6876        ]);
6877
6878        for t in (0..T).rev() {
6879            let mut grad = (r * controls[t]) * 2.0 + b_seq[t].transpose() * lambda;
6880            if t > 0 {
6881                grad += (rd * (controls[t] - controls[t - 1])) * 2.0;
6882            }
6883            if t < T - 1 {
6884                grad -= (rd * (controls[t + 1] - controls[t])) * 2.0;
6885            }
6886            gradients[t] = grad;
6887
6888            let lx = if t == 0 {
6889                Vector4::zeros()
6890            } else {
6891                (q * state_error(x[t], xref[t])) * 2.0
6892            };
6893
6894            let lambda_next = lambda;
6895            lambda = lx + a_seq[t].transpose() * lambda_next;
6896
6897            // Decompose lambda[3]
6898            let from_lx3 = lx[3];
6899            let from_at30_l0 = a_seq[t][(0, 3)] * lambda_next[0]; // A^T[3,0] = A[0,3]
6900            let from_at31_l1 = a_seq[t][(1, 3)] * lambda_next[1]; // A^T[3,1] = A[1,3]
6901            let from_at33_l3 = a_seq[t][(3, 3)] * lambda_next[3]; // A^T[3,3] = A[3,3]
6902            lambda3_decomp.push([
6903                lambda[3],
6904                from_lx3,
6905                from_at30_l0,
6906                from_at31_l1,
6907                from_at33_l3,
6908            ]);
6909        }
6910
6911        lambda3_decomp.reverse();
6912        (gradients, lambda3_decomp)
6913    }
6914
6915    #[test]
6916    #[ignore = "long-running regression scenario"]
6917    fn test_mpc_switch_back_characterizes_y_effect_path_via_outer_loop() {
6918        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6919            330,
6920            optimize_linearized_controls_original,
6921        );
6922        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
6923            330,
6924            optimize_linearized_controls_with_full_candidate_expansion,
6925        );
6926        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
6927            330,
6928            optimize_linearized_controls_original,
6929        );
6930
6931        println!("=== Y Effect Path: Outer Loop Iteration Count ===");
6932        println!("At step 326, v-only injection needs v+y. Is y's effect through outer-loop re-linearization?");
6933        println!();
6934
6935        for &step in &[325usize, 326, 327] {
6936            let d_row = detailed_row_at_step(&default_trace, step);
6937            let e_row = detailed_row_at_step(&expanded_trace, step);
6938            let ws = default_warm_start.get(&step).unwrap();
6939
6940            let d_baseline_steer = d_row.controls[0][1];
6941            let e_baseline_steer = e_row.controls[0][1];
6942
6943            println!("--- step {step} (default_steer={d_baseline_steer:.6}, expanded_steer={e_baseline_steer:.6}) ---");
6944
6945            for &max_iter in &[1usize, 2, 3] {
6946                // v-only injection
6947                let v_state = inject_state_components(&d_row.state, &e_row.state, &[2]);
6948                let v_result =
6949                    iterative_linear_mpc_control_with_max_iter(&d_row.xref, &v_state, ws, max_iter);
6950
6951                // v+y injection
6952                let vy_state = inject_state_components(&d_row.state, &e_row.state, &[1, 2]);
6953                let vy_result = iterative_linear_mpc_control_with_max_iter(
6954                    &d_row.xref,
6955                    &vy_state,
6956                    ws,
6957                    max_iter,
6958                );
6959
6960                let v_steer = v_result.controls[0][1];
6961                let vy_steer = vy_result.controls[0][1];
6962                let steer_diff = (v_steer - vy_steer).abs();
6963
6964                println!(
6965                    "  iter={max_iter}: v_steer={v_steer:.6} vy_steer={vy_steer:.6} diff={steer_diff:.6e}"
6966                );
6967            }
6968        }
6969    }
6970
6971    /// linearized_rollout WITHOUT angle normalization — matches Python CVXPY behavior.
6972    fn linearized_rollout_without_angle_normalization(
6973        x0: &State,
6974        xbar: &[Vector4<f64>],
6975        controls: &[Vector2<f64>],
6976    ) -> LinearizedRollout {
6977        let mut x = vec![Vector4::zeros(); T + 1];
6978        let mut a_seq = Vec::with_capacity(T);
6979        let mut b_seq = Vec::with_capacity(T);
6980        let mut c_seq = Vec::with_capacity(T);
6981
6982        x[0] = x0.to_vector();
6983        for t in 0..T {
6984            let (a, b, c) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
6985            let next = a * x[t] + b * controls[t] + c;
6986            // No normalize_angle here — Python CVXPY doesn't do this
6987
6988            a_seq.push(a);
6989            b_seq.push(b);
6990            c_seq.push(c);
6991            x[t + 1] = next;
6992        }
6993
6994        (x, a_seq, b_seq, c_seq)
6995    }
6996
6997    #[test]
6998    #[ignore = "long-running regression scenario"]
6999    fn test_mpc_switch_back_characterizes_yaw_normalize_effect_in_linearized_rollout() {
7000        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7001            330,
7002            optimize_linearized_controls,
7003        );
7004        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7005            330,
7006            optimize_linearized_controls,
7007        );
7008
7009        println!("=== Yaw Normalize Effect in Linearized Rollout ===");
7010        println!("Comparing linearized_rollout with and without normalize_angle");
7011        println!();
7012
7013        for &step in &[20usize, 70, 71, 75, 78, 325, 326] {
7014            let row = detailed_row_at_step(&default_trace, step);
7015            let ws = default_warm_start.get(&step).unwrap();
7016            let state = state_from_vector4(&row.state);
7017
7018            // Build xbar from predict_motion (nonlinear, as both Rust and Python do)
7019            let mut controls = ws.to_vec();
7020            controls.resize(T, Vector2::zeros());
7021            apply_control_constraints(&mut controls);
7022            let xbar = predict_motion(state, &controls);
7023
7024            let (x_with, _, _, _) = linearized_rollout(&state, &xbar, &controls);
7025            let (x_without, _, _, _) =
7026                linearized_rollout_without_angle_normalization(&state, &xbar, &controls);
7027
7028            let mut max_yaw_diff = 0.0f64;
7029            let mut max_other_diff = [0.0f64; 3]; // x, y, v
7030            for t in 0..=T {
7031                let yaw_diff = (x_with[t][3] - x_without[t][3]).abs();
7032                max_yaw_diff = max_yaw_diff.max(yaw_diff);
7033                for (i, slot) in max_other_diff.iter_mut().enumerate() {
7034                    *slot = (*slot).max((x_with[t][i] - x_without[t][i]).abs());
7035                }
7036            }
7037
7038            let yaw_values: Vec<f64> = (0..=T).map(|t| x_with[t][3]).collect();
7039            println!(
7040                "step {step:>3}: yaw_diff={max_yaw_diff:.4e} other=[{:.4e},{:.4e},{:.4e}] yaw={:.4?}",
7041                max_other_diff[0], max_other_diff[1], max_other_diff[2], yaw_values
7042            );
7043        }
7044    }
7045
7046    #[test]
7047    #[ignore = "long-running regression scenario"]
7048    fn test_mpc_switch_back_characterizes_adjoint_y_to_steer_coupling() {
7049        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7050            330,
7051            optimize_linearized_controls_original,
7052        );
7053        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7054            330,
7055            optimize_linearized_controls_with_full_candidate_expansion,
7056        );
7057        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7058            330,
7059            optimize_linearized_controls_original,
7060        );
7061
7062        println!("=== Adjoint Decomposition: How Y Reaches Steering ===");
7063        println!(
7064            "lambda3_decomp columns: [total, from_lx3, from_AT30*l0, from_AT31*l1, from_AT33*l3]"
7065        );
7066        println!();
7067
7068        for &step in &[325usize, 326] {
7069            let d_row = detailed_row_at_step(&default_trace, step);
7070            let e_row = detailed_row_at_step(&expanded_trace, step);
7071            let ws = default_warm_start.get(&step).unwrap();
7072
7073            println!("--- step {step} ---");
7074
7075            // Baseline (no injection)
7076            let (grad_base, l3_base) =
7077                dump_first_inner_solve_adjoint(&d_row.xref, &state_from_vector4(&d_row.state), ws);
7078            println!("  [baseline] steer_grad[0]={:.6e}", grad_base[0][1]);
7079            for (t, decomp) in l3_base.iter().enumerate() {
7080                println!(
7081                    "    t={t}: λ3={:.4e} lx3={:.4e} AT30·l0={:.4e} AT31·l1={:.4e} AT33·l3={:.4e}",
7082                    decomp[0], decomp[1], decomp[2], decomp[3], decomp[4]
7083                );
7084            }
7085
7086            // v-only injection
7087            let v_state = inject_state_components(&d_row.state, &e_row.state, &[2]);
7088            let (grad_v, l3_v) = dump_first_inner_solve_adjoint(&d_row.xref, &v_state, ws);
7089            println!("  [v-only] steer_grad[0]={:.6e}", grad_v[0][1]);
7090            for (t, decomp) in l3_v.iter().enumerate() {
7091                println!(
7092                    "    t={t}: λ3={:.4e} lx3={:.4e} AT30·l0={:.4e} AT31·l1={:.4e} AT33·l3={:.4e}",
7093                    decomp[0], decomp[1], decomp[2], decomp[3], decomp[4]
7094                );
7095            }
7096
7097            // v+y injection
7098            let vy_state = inject_state_components(&d_row.state, &e_row.state, &[1, 2]);
7099            let (grad_vy, l3_vy) = dump_first_inner_solve_adjoint(&d_row.xref, &vy_state, ws);
7100            println!("  [v+y] steer_grad[0]={:.6e}", grad_vy[0][1]);
7101            for (t, decomp) in l3_vy.iter().enumerate() {
7102                println!(
7103                    "    t={t}: λ3={:.4e} lx3={:.4e} AT30·l0={:.4e} AT31·l1={:.4e} AT33·l3={:.4e}",
7104                    decomp[0], decomp[1], decomp[2], decomp[3], decomp[4]
7105                );
7106            }
7107
7108            // Print gradient differences
7109            println!(
7110                "  steer_grad[0] diffs: v-base={:.6e} vy-base={:.6e} vy-v={:.6e}",
7111                grad_v[0][1] - grad_base[0][1],
7112                grad_vy[0][1] - grad_base[0][1],
7113                grad_vy[0][1] - grad_v[0][1],
7114            );
7115            println!();
7116        }
7117    }
7118
7119    /// Per-component state crossover: replace individual state components
7120    /// from step 325 into step 326 context to identify which component(s)
7121    /// of the state change drive the v-only pull degradation.
7122    #[test]
7123    #[ignore = "long-running regression scenario"]
7124    fn test_mpc_switch_back_characterizes_step_325_326_state_component_crossover() {
7125        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7126            330,
7127            optimize_linearized_controls_original,
7128        );
7129        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7130            330,
7131            optimize_linearized_controls_with_full_candidate_expansion,
7132        );
7133        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7134            330,
7135            optimize_linearized_controls_original,
7136        );
7137
7138        let d325 = detailed_row_at_step(&default_trace, 325);
7139        let d326 = detailed_row_at_step(&default_trace, 326);
7140        let e326 = detailed_row_at_step(&expanded_trace, 326);
7141        let ws326 = default_warm_start.get(&326).unwrap();
7142
7143        let state_delta = d326.state - d325.state;
7144        println!(
7145            "state_delta: x={:.6e} y={:.6e} v={:.6e} yaw={:.6e}",
7146            state_delta[0], state_delta[1], state_delta[2], state_delta[3]
7147        );
7148
7149        let expanded_baseline_controls: Vec<[f64; 2]> =
7150            e326.controls.iter().map(|v| [v[0], v[1]]).collect();
7151        let default_baseline_controls: Vec<[f64; 2]> =
7152            d326.controls.iter().map(|v| [v[0], v[1]]).collect();
7153
7154        let component_sets: Vec<(&str, Vec<usize>)> = vec![
7155            ("revert_x", vec![0]),
7156            ("revert_y", vec![1]),
7157            ("revert_v", vec![2]),
7158            ("revert_yaw", vec![3]),
7159            ("revert_y+v", vec![1, 2]),
7160            ("revert_x+yaw", vec![0, 3]),
7161            ("revert_all", vec![0, 1, 2, 3]),
7162        ];
7163
7164        println!(
7165            "\n=== State Component Crossover (step 326 context, revert components to 325) ==="
7166        );
7167
7168        // Baseline: v-only injection at step 326
7169        let baseline_injected_state = inject_state_components(&d326.state, &e326.state, &[2]);
7170        let baseline_result = iterative_linear_mpc_control_with_test_optimizer(
7171            &d326.xref,
7172            &baseline_injected_state,
7173            ws326,
7174            optimize_linearized_controls_original,
7175        );
7176        let baseline_to_default =
7177            summarize_vec2_sequence_diff(&baseline_result.controls, &default_baseline_controls);
7178        let baseline_to_expanded =
7179            summarize_vec2_sequence_diff(&baseline_result.controls, &expanded_baseline_controls);
7180        let baseline_ratio = if baseline_to_expanded[0] > 0.0 {
7181            baseline_to_default[0] / baseline_to_expanded[0]
7182        } else {
7183            f64::INFINITY
7184        };
7185        println!(
7186            "  baseline: same={:.4e} opp={:.4e} ratio={:.3}",
7187            baseline_to_default[0], baseline_to_expanded[0], baseline_ratio
7188        );
7189
7190        let mut ratios = Vec::new();
7191        for (label, indices) in &component_sets {
7192            let mut modified_default = d326.state;
7193            for &idx in indices {
7194                modified_default[idx] = d325.state[idx];
7195            }
7196
7197            let injected_state = inject_state_components(&modified_default, &e326.state, &[2]);
7198            let result = iterative_linear_mpc_control_with_test_optimizer(
7199                &d326.xref,
7200                &injected_state,
7201                ws326,
7202                optimize_linearized_controls_original,
7203            );
7204
7205            let to_default =
7206                summarize_vec2_sequence_diff(&result.controls, &default_baseline_controls);
7207            let to_expanded =
7208                summarize_vec2_sequence_diff(&result.controls, &expanded_baseline_controls);
7209            let ratio = if to_expanded[0] > 0.0 {
7210                to_default[0] / to_expanded[0]
7211            } else {
7212                f64::INFINITY
7213            };
7214
7215            println!(
7216                "  {:12} same={:.4e} opp={:.4e} ratio={:.3} (delta: {:.3})",
7217                label,
7218                to_default[0],
7219                to_expanded[0],
7220                ratio,
7221                ratio - baseline_ratio
7222            );
7223            ratios.push((*label, ratio));
7224        }
7225
7226        let revert_y_ratio = ratios.iter().find(|(l, _)| *l == "revert_y").unwrap().1;
7227        let revert_v_ratio = ratios.iter().find(|(l, _)| *l == "revert_v").unwrap().1;
7228        let revert_yv_ratio = ratios.iter().find(|(l, _)| *l == "revert_y+v").unwrap().1;
7229        let revert_all_ratio = ratios.iter().find(|(l, _)| *l == "revert_all").unwrap().1;
7230
7231        println!("\n=== Summary ===");
7232        println!("baseline_326 ratio: {:.3}", baseline_ratio);
7233        println!("revert_y ratio:     {:.3}", revert_y_ratio);
7234        println!("revert_v ratio:     {:.3}", revert_v_ratio);
7235        println!("revert_y+v ratio:   {:.3}", revert_yv_ratio);
7236        println!("revert_all ratio:   {:.3}", revert_all_ratio);
7237
7238        // y alone accounts for nearly all the effect
7239        assert!(
7240            (revert_y_ratio - revert_all_ratio).abs() < 0.1,
7241            "reverting y alone should account for nearly all the effect: y={revert_y_ratio} all={revert_all_ratio}"
7242        );
7243
7244        // v contributes nothing
7245        assert!(
7246            (revert_v_ratio - baseline_ratio).abs() < 0.01,
7247            "reverting v should have no effect: v={revert_v_ratio} baseline={baseline_ratio}"
7248        );
7249
7250        // x and yaw contribute nothing
7251        let revert_xyaw_ratio = ratios.iter().find(|(l, _)| *l == "revert_x+yaw").unwrap().1;
7252        assert!(
7253            (revert_xyaw_ratio - baseline_ratio).abs() < 0.01,
7254            "reverting x+yaw should have no effect: x+yaw={revert_xyaw_ratio} baseline={baseline_ratio}"
7255        );
7256    }
7257
7258    /// Print state.y divergence history between default and expanded branches
7259    /// to identify when and how the lateral position diverges.
7260    #[test]
7261    #[ignore = "long-running regression scenario"]
7262    fn test_mpc_switch_back_characterizes_y_divergence_history() {
7263        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7264            350,
7265            optimize_linearized_controls_original,
7266        );
7267        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7268            350,
7269            optimize_linearized_controls_with_full_candidate_expansion,
7270        );
7271
7272        println!("=== state.y Divergence History (default vs expanded) ===");
7273        println!(
7274            "{:>5} {:>12} {:>12} {:>12} {:>12} {:>12}",
7275            "step", "default_y", "expanded_y", "y_diff", "default_v", "v_diff"
7276        );
7277
7278        let mut first_significant_step = None;
7279        let mut max_y_diff: f64 = 0.0;
7280
7281        for step in (0..350).step_by(5) {
7282            let d = default_trace.iter().find(|r| r.step == step);
7283            let e = expanded_trace.iter().find(|r| r.step == step);
7284            if let (Some(d), Some(e)) = (d, e) {
7285                let y_diff = (d.state[1] - e.state[1]).abs();
7286                max_y_diff = max_y_diff.max(y_diff);
7287
7288                if y_diff > 1e-6 || step >= 60 {
7289                    println!(
7290                        "{:5} {:12.6} {:12.6} {:12.6e} {:12.6} {:12.6e}",
7291                        step,
7292                        d.state[1],
7293                        e.state[1],
7294                        d.state[1] - e.state[1],
7295                        d.state[2],
7296                        d.state[2] - e.state[2]
7297                    );
7298                }
7299
7300                if first_significant_step.is_none() && y_diff > 1e-3 {
7301                    first_significant_step = Some(step);
7302                }
7303            }
7304        }
7305
7306        // Dense print around the first significant divergence
7307        if let Some(start) = first_significant_step {
7308            let dense_start = start.saturating_sub(10);
7309            println!(
7310                "\n=== Dense view around first significant y divergence (step {}) ===",
7311                start
7312            );
7313            for step in dense_start..=(start + 20).min(349) {
7314                let d = default_trace.iter().find(|r| r.step == step);
7315                let e = expanded_trace.iter().find(|r| r.step == step);
7316                if let (Some(d), Some(e)) = (d, e) {
7317                    let y_diff = d.state[1] - e.state[1];
7318                    let v_diff = d.state[2] - e.state[2];
7319                    let steer_diff = d.controls[0][1] - e.controls[0][1];
7320                    println!(
7321                        "  step {:3}: y_diff={:12.6e} v_diff={:12.6e} steer_diff={:12.6e}",
7322                        step, y_diff, v_diff, steer_diff
7323                    );
7324                }
7325            }
7326        }
7327
7328        // Dense print around step 320-330 (the pre-terminal region)
7329        println!("\n=== Dense view step 315..335 (pre-terminal) ===");
7330        for step in 315..=335 {
7331            let d = default_trace.iter().find(|r| r.step == step);
7332            let e = expanded_trace.iter().find(|r| r.step == step);
7333            if let (Some(d), Some(e)) = (d, e) {
7334                let y_diff = d.state[1] - e.state[1];
7335                let v_diff = d.state[2] - e.state[2];
7336                let steer_diff = d.controls[0][1] - e.controls[0][1];
7337                println!(
7338                    "  step {:3}: y={:10.6}/{:10.6} diff={:12.6e} v_diff={:12.6e} steer_diff={:12.6e}",
7339                    step, d.state[1], e.state[1], y_diff, v_diff, steer_diff
7340                );
7341            }
7342        }
7343
7344        println!("\nmax |y_diff| across all steps: {:.6e}", max_y_diff);
7345        if let Some(s) = first_significant_step {
7346            println!("first step with |y_diff| > 1e-3: {}", s);
7347        }
7348    }
7349
7350    /// Optimizer without angle wrap, matching `optimize_linearized_controls` signature
7351    /// for use with `iterative_linear_mpc_control_with_test_optimizer`.
7352    fn optimize_linearized_controls_without_angle_wrap(
7353        xref: &[Vector4<f64>],
7354        xbar: &[Vector4<f64>],
7355        state: &State,
7356        initial_controls: &[Vector2<f64>],
7357    ) -> Vec<Vector2<f64>> {
7358        let (controls, _) = optimize_linearized_controls_without_angle_wrap_with_trace(
7359            xref,
7360            xbar,
7361            state,
7362            initial_controls,
7363            QP_MAX_ITERS,
7364            LINE_SEARCH_ITERS,
7365        );
7366        controls
7367    }
7368
7369    #[test]
7370    #[ignore = "long-running regression scenario"]
7371    fn test_mpc_switch_back_no_angle_wrap_closed_loop_parity() {
7372        // Python reference values (from solver gap memo)
7373        let python_final_state = [
7374            -0.014_888_855_512_389,
7375            0.698_023_990_780_443,
7376            0.132_179_560_675_735,
7377            1.589_745_981_585_03,
7378        ];
7379        let python_history_length = 347usize;
7380
7381        // Run default (with angle wrap) closed-loop
7382        let default_result = run_mpc_simulation();
7383        let default_final = default_result.final_state.to_vector();
7384        let default_len = default_result.hist_x.len();
7385
7386        // Run no-angle-wrap closed-loop
7387        let (cx, cy, cyaw) = generate_switch_back_course();
7388        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
7389
7390        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
7391        if state.yaw - cyaw[0] >= PI {
7392            state.yaw -= 2.0 * PI;
7393        } else if state.yaw - cyaw[0] <= -PI {
7394            state.yaw += 2.0 * PI;
7395        }
7396
7397        let final_index = cx.len() - 1;
7398        let goal = (cx[final_index], cy[final_index]);
7399        let mut target_index = 0;
7400        let mut warm_start = vec![Vector2::zeros(); T];
7401        let mut step_count = 0;
7402
7403        for _ in 0..MAX_SIM_STEPS {
7404            let (xref, new_index) = calc_ref_trajectory(
7405                &state,
7406                &cx,
7407                &cy,
7408                &cyaw,
7409                &speed_profile,
7410                UPSTREAM_SWITCH_BACK_TICK,
7411                target_index,
7412            );
7413            target_index = new_index;
7414
7415            let result = iterative_linear_mpc_control_with_test_optimizer(
7416                &xref,
7417                &state,
7418                &warm_start,
7419                optimize_linearized_controls_without_angle_wrap,
7420            );
7421
7422            state.update(result.controls[0][0], result.controls[0][1]);
7423            warm_start = next_warm_start(&result.controls);
7424            step_count += 1;
7425
7426            if check_goal(&state, goal, target_index, final_index) {
7427                break;
7428            }
7429        }
7430
7431        let nowrap_final = state.to_vector();
7432        let nowrap_len = step_count + 1; // +1 for initial state
7433
7434        // Compute diffs against Python reference
7435        let default_diffs: Vec<f64> = (0..4)
7436            .map(|i| (default_final[i] - python_final_state[i]).abs())
7437            .collect();
7438        let nowrap_diffs: Vec<f64> = (0..4)
7439            .map(|i| (nowrap_final[i] - python_final_state[i]).abs())
7440            .collect();
7441
7442        println!("=== Closed-Loop Parity: Default vs No-Angle-Wrap ===");
7443        println!(
7444            "Python reference: {:?} (len={})",
7445            python_final_state, python_history_length
7446        );
7447        println!();
7448        println!("Default (with normalize_angle):");
7449        println!(
7450            "  final: [{:.6}, {:.6}, {:.6}, {:.6}] (len={})",
7451            default_final[0], default_final[1], default_final[2], default_final[3], default_len
7452        );
7453        println!(
7454            "  diffs: [{:.6e}, {:.6e}, {:.6e}, {:.6e}] step_diff={}",
7455            default_diffs[0],
7456            default_diffs[1],
7457            default_diffs[2],
7458            default_diffs[3],
7459            (default_len as isize - python_history_length as isize).abs()
7460        );
7461        println!();
7462        println!("No-angle-wrap:");
7463        println!(
7464            "  final: [{:.6}, {:.6}, {:.6}, {:.6}] (len={})",
7465            nowrap_final[0], nowrap_final[1], nowrap_final[2], nowrap_final[3], nowrap_len
7466        );
7467        println!(
7468            "  diffs: [{:.6e}, {:.6e}, {:.6e}, {:.6e}] step_diff={}",
7469            nowrap_diffs[0],
7470            nowrap_diffs[1],
7471            nowrap_diffs[2],
7472            nowrap_diffs[3],
7473            (nowrap_len as isize - python_history_length as isize).abs()
7474        );
7475        println!();
7476
7477        // Compare total state diff
7478        let default_total: f64 = default_diffs.iter().sum();
7479        let nowrap_total: f64 = nowrap_diffs.iter().sum();
7480        println!(
7481            "Total state diff: default={:.6e} nowrap={:.6e} improvement={:.2}%",
7482            default_total,
7483            nowrap_total,
7484            if default_total > 0.0 {
7485                (1.0 - nowrap_total / default_total) * 100.0
7486            } else {
7487                0.0
7488            }
7489        );
7490    }
7491
7492    /// Solve the MPC QP using clarabel, matching the Python CVXPY formulation exactly.
7493    ///
7494    /// Decision variables: z = [u_flat (2*T), x_flat (4*(T+1))]
7495    ///   u_flat = [a0, δ0, a1, δ1, ..., a_{T-1}, δ_{T-1}]
7496    ///   x_flat = [x0_0..x0_3, x1_0..x1_3, ..., xT_0..xT_3]
7497    ///
7498    /// Cost: Σ u[t]'Ru[t] + Σ_{t=1..T-1} (x[t]-xref[t])'Q(x[t]-xref[t])
7499    ///       + (x[T]-xref[T])'Qf(x[T]-xref[T]) + Σ (u[t+1]-u[t])'Rd(u[t+1]-u[t])
7500    ///
7501    /// Constraints:
7502    ///   x[t+1] = A[t]*x[t] + B[t]*u[t] + C[t]   (equality)
7503    ///   x[0] = x0                                  (equality)
7504    ///   -MAX_ACCEL <= u[0,t] <= MAX_ACCEL          (box)
7505    ///   -MAX_STEER <= u[1,t] <= MAX_STEER          (box)
7506    ///   |u[1,t+1] - u[1,t]| <= MAX_DSTEER*DT      (box)
7507    ///   MIN_SPEED <= x[2,t] <= MAX_SPEED            (box)
7508    #[allow(clippy::needless_range_loop, clippy::type_complexity)]
7509    fn solve_mpc_qp_clarabel(
7510        xref: &[Vector4<f64>],
7511        xbar: &[Vector4<f64>],
7512        state: &State,
7513    ) -> Option<(Vec<Vector2<f64>>, Vec<Vector4<f64>>)> {
7514        use clarabel::algebra::*;
7515        use clarabel::solver::*;
7516
7517        let nu = 2; // controls per step
7518        let nx = 4; // state dim
7519        let n_u = nu * T; // total control vars: 10
7520        let n_x = nx * (T + 1); // total state vars: 24
7521        let n = n_u + n_x; // total vars: 34
7522
7523        // Variable index helpers
7524        let u_idx = |t: usize, k: usize| -> usize { t * nu + k };
7525        let x_idx = |t: usize, k: usize| -> usize { n_u + t * nx + k };
7526
7527        // --- Build P (cost Hessian, upper triangular) ---
7528        let r_diag = R;
7529        let rd_diag = RD;
7530        let q_diag = Q;
7531        let qf_diag = QF;
7532
7533        let mut p_rows = Vec::new();
7534        let mut p_cols = Vec::new();
7535        let mut p_vals = Vec::new();
7536
7537        // Control cost: R
7538        for t in 0..T {
7539            for k in 0..nu {
7540                let idx = u_idx(t, k);
7541                let mut val = r_diag[k];
7542                // Rate cost: Rd contributes to diagonal
7543                if t > 0 {
7544                    val += rd_diag[k];
7545                }
7546                if t < T - 1 {
7547                    val += rd_diag[k];
7548                }
7549                p_rows.push(idx);
7550                p_cols.push(idx);
7551                p_vals.push(val * 2.0); // clarabel uses 0.5*z'Pz, so P = 2*H
7552            }
7553            // Rate cost off-diagonal: -Rd between consecutive controls
7554            if t < T - 1 {
7555                for k in 0..nu {
7556                    let i = u_idx(t, k);
7557                    let j = u_idx(t + 1, k);
7558                    // Upper triangular: i < j
7559                    p_rows.push(i);
7560                    p_cols.push(j);
7561                    p_vals.push(-rd_diag[k] * 2.0);
7562                }
7563            }
7564        }
7565
7566        // State cost: Q for t=1..T-1, Qf for t=T
7567        for t in 1..=T {
7568            let w = if t == T { qf_diag } else { q_diag };
7569            for k in 0..nx {
7570                let idx = x_idx(t, k);
7571                p_rows.push(idx);
7572                p_cols.push(idx);
7573                p_vals.push(w[k] * 2.0);
7574            }
7575        }
7576
7577        let p = CscMatrix::new_from_triplets(n, n, p_rows.clone(), p_cols.clone(), p_vals.clone());
7578
7579        // --- Build q (linear cost) ---
7580        let mut q_vec = vec![0.0; n];
7581        for t in 1..=T {
7582            let w = if t == T { qf_diag } else { q_diag };
7583            for k in 0..nx {
7584                // cost = w[k] * (x[k] - xref[k])^2 = w[k]*x[k]^2 - 2*w[k]*xref[k]*x[k] + const
7585                // q contribution: -2*w[k]*xref[k] (but clarabel uses 0.5*z'Pz + q'z, so just -2*w*xref)
7586                q_vec[x_idx(t, k)] = -w[k] * 2.0 * xref[t][k];
7587            }
7588        }
7589
7590        // --- Build constraints ---
7591        // Equality: dynamics x[t+1] = A[t]*x[t] + B[t]*u[t] + C[t]  → A[t]*x[t] + B[t]*u[t] - x[t+1] + C[t] = 0
7592        // Equality: x[0] = x0
7593        // Box constraints on u and x
7594
7595        let n_eq = nx * (T + 1); // T dynamics + 1 initial = (T+1)*nx = 24
7596        let n_steer_rate = T - 1; // |δ[t+1]-δ[t]| <= MAX_DSTEER*DT → 2*(T-1) ineq
7597        let n_u_box = 2 * nu * T; // upper+lower for each u component: 20
7598        let n_steer_rate_box = 2 * n_steer_rate; // 8
7599        let n_v_box = 2 * (T + 1); // speed constraints: 12
7600        let n_ineq = n_u_box + n_steer_rate_box + n_v_box;
7601
7602        let mut a_rows = Vec::new();
7603        let mut a_cols = Vec::new();
7604        let mut a_vals = Vec::new();
7605        let mut b_vec = vec![0.0; n_eq + n_ineq];
7606
7607        // -- Equality: x[0] = x0 --
7608        let x0 = state.to_vector();
7609        for k in 0..nx {
7610            let row = k;
7611            a_rows.push(row);
7612            a_cols.push(x_idx(0, k));
7613            a_vals.push(1.0);
7614            b_vec[row] = x0[k];
7615        }
7616
7617        // -- Equality: dynamics for t=0..T-1 --
7618        for t in 0..T {
7619            let (a_mat, b_mat, c_vec) = get_linear_model_matrix(xbar[t][2], xbar[t][3], 0.0);
7620            let base_row = nx * (t + 1);
7621
7622            // x[t+1] = A*x[t] + B*u[t] + C
7623            // → -x[t+1] + A*x[t] + B*u[t] = -C
7624            for i in 0..nx {
7625                let row = base_row + i;
7626                // -x[t+1][i]
7627                a_rows.push(row);
7628                a_cols.push(x_idx(t + 1, i));
7629                a_vals.push(-1.0);
7630                // A[i,:] * x[t]
7631                for j in 0..nx {
7632                    let val = a_mat[(i, j)];
7633                    if val.abs() > 1e-15 {
7634                        a_rows.push(row);
7635                        a_cols.push(x_idx(t, j));
7636                        a_vals.push(val);
7637                    }
7638                }
7639                // B[i,:] * u[t]
7640                for j in 0..nu {
7641                    let val = b_mat[(i, j)];
7642                    if val.abs() > 1e-15 {
7643                        a_rows.push(row);
7644                        a_cols.push(u_idx(t, j));
7645                        a_vals.push(val);
7646                    }
7647                }
7648                b_vec[row] = -c_vec[i];
7649            }
7650        }
7651
7652        // -- Inequality constraints (as cones) --
7653        // clarabel inequality: A*z + s = b, s >= 0  → A*z <= b
7654        let ineq_offset = n_eq;
7655        let mut ineq_row = 0;
7656
7657        // u box: -MAX_ACCEL <= a[t] <= MAX_ACCEL, -MAX_STEER <= δ[t] <= MAX_STEER
7658        let u_bounds = [MAX_ACCEL, MAX_STEER];
7659        for t in 0..T {
7660            for (k, &bound) in u_bounds.iter().enumerate() {
7661                // u[t][k] <= bound
7662                let row = ineq_offset + ineq_row;
7663                a_rows.push(row);
7664                a_cols.push(u_idx(t, k));
7665                a_vals.push(1.0);
7666                b_vec[row] = bound;
7667                ineq_row += 1;
7668
7669                // -u[t][k] <= bound
7670                let row = ineq_offset + ineq_row;
7671                a_rows.push(row);
7672                a_cols.push(u_idx(t, k));
7673                a_vals.push(-1.0);
7674                b_vec[row] = bound;
7675                ineq_row += 1;
7676            }
7677        }
7678
7679        // Steer rate: |δ[t+1] - δ[t]| <= MAX_DSTEER * DT
7680        let max_steer_delta = MAX_DSTEER * DT;
7681        for t in 0..(T - 1) {
7682            // δ[t+1] - δ[t] <= max_steer_delta
7683            let row = ineq_offset + ineq_row;
7684            a_rows.push(row);
7685            a_cols.push(u_idx(t + 1, 1));
7686            a_vals.push(1.0);
7687            a_rows.push(row);
7688            a_cols.push(u_idx(t, 1));
7689            a_vals.push(-1.0);
7690            b_vec[row] = max_steer_delta;
7691            ineq_row += 1;
7692
7693            // -(δ[t+1] - δ[t]) <= max_steer_delta
7694            let row = ineq_offset + ineq_row;
7695            a_rows.push(row);
7696            a_cols.push(u_idx(t + 1, 1));
7697            a_vals.push(-1.0);
7698            a_rows.push(row);
7699            a_cols.push(u_idx(t, 1));
7700            a_vals.push(1.0);
7701            b_vec[row] = max_steer_delta;
7702            ineq_row += 1;
7703        }
7704
7705        // Speed box: MIN_SPEED <= x[2,t] <= MAX_SPEED for all t
7706        for t in 0..=T {
7707            // x[2,t] <= MAX_SPEED
7708            let row = ineq_offset + ineq_row;
7709            a_rows.push(row);
7710            a_cols.push(x_idx(t, 2));
7711            a_vals.push(1.0);
7712            b_vec[row] = MAX_SPEED;
7713            ineq_row += 1;
7714
7715            // -x[2,t] <= -MIN_SPEED
7716            let row = ineq_offset + ineq_row;
7717            a_rows.push(row);
7718            a_cols.push(x_idx(t, 2));
7719            a_vals.push(-1.0);
7720            b_vec[row] = -MIN_SPEED;
7721            ineq_row += 1;
7722        }
7723
7724        let m = n_eq + n_ineq;
7725
7726        // Build CscMatrix for A (constraint matrix)
7727        // clarabel expects column-major CscMatrix
7728        let a_csc = CscMatrix::new_from_triplets(m, n, a_rows, a_cols, a_vals);
7729
7730        // Cone specification
7731        let cones = vec![
7732            SupportedConeT::ZeroConeT(n_eq),          // equality constraints
7733            SupportedConeT::NonnegativeConeT(n_ineq), // inequality constraints (s >= 0)
7734        ];
7735
7736        let settings = DefaultSettingsBuilder::default()
7737            .verbose(false)
7738            .build()
7739            .unwrap();
7740
7741        let mut solver = DefaultSolver::new(&p, &q_vec, &a_csc, &b_vec, &cones, settings).ok()?;
7742        solver.solve();
7743
7744        if solver.solution.status != SolverStatus::Solved {
7745            return None;
7746        }
7747
7748        let z = &solver.solution.x;
7749
7750        let controls: Vec<Vector2<f64>> = (0..T)
7751            .map(|t| Vector2::new(z[u_idx(t, 0)], z[u_idx(t, 1)]))
7752            .collect();
7753
7754        let predicted: Vec<Vector4<f64>> = (0..=T)
7755            .map(|t| {
7756                Vector4::new(
7757                    z[x_idx(t, 0)],
7758                    z[x_idx(t, 1)],
7759                    z[x_idx(t, 2)],
7760                    z[x_idx(t, 3)],
7761                )
7762            })
7763            .collect();
7764
7765        Some((controls, predicted))
7766    }
7767
7768    /// Wrapper for QP solver matching the test optimizer signature.
7769    fn optimize_linearized_controls_qp(
7770        xref: &[Vector4<f64>],
7771        xbar: &[Vector4<f64>],
7772        state: &State,
7773        _initial_controls: &[Vector2<f64>],
7774    ) -> Vec<Vector2<f64>> {
7775        if let Some((controls, _)) = solve_mpc_qp_clarabel(xref, xbar, state) {
7776            controls
7777        } else {
7778            // Fallback to projected-gradient if QP fails
7779            optimize_linearized_controls(xref, xbar, state, _initial_controls)
7780        }
7781    }
7782
7783    #[test]
7784    #[ignore = "long-running regression scenario"]
7785    fn test_mpc_qp_solver_matches_projected_gradient_on_representative_steps() {
7786        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7787            80,
7788            optimize_linearized_controls,
7789        );
7790        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
7791            80,
7792            optimize_linearized_controls,
7793        );
7794
7795        println!("=== QP Solver (clarabel) vs Projected-Gradient ===");
7796        println!();
7797
7798        for &step in &[20usize, 70, 75, 78] {
7799            let row = detailed_row_at_step(&default_trace, step);
7800            let ws = default_warm_start.get(&step).unwrap();
7801            let state = state_from_vector4(&row.state);
7802
7803            // Get xbar for QP (same as projected-gradient uses)
7804            let mut controls_for_xbar = ws.to_vec();
7805            controls_for_xbar.resize(T, Vector2::zeros());
7806            apply_control_constraints(&mut controls_for_xbar);
7807            let xbar = predict_motion(state, &controls_for_xbar);
7808
7809            // Projected-gradient result (1 outer iteration for fair comparison)
7810            let pg_controls = optimize_linearized_controls(&row.xref, &xbar, &state, ws);
7811
7812            // QP result
7813            let qp_result = solve_mpc_qp_clarabel(&row.xref, &xbar, &state);
7814
7815            if let Some((qp_controls, _qp_predicted)) = qp_result {
7816                let control_diff = summarize_vec2_sequence_diff(
7817                    &qp_controls,
7818                    &pg_controls.iter().map(|c| [c[0], c[1]]).collect::<Vec<_>>(),
7819                );
7820                println!(
7821                    "step {step}: control_diff=[{:.6e}, {:.6e}] pg_steer={:.6} qp_steer={:.6}",
7822                    control_diff[0], control_diff[1], pg_controls[0][1], qp_controls[0][1],
7823                );
7824            } else {
7825                println!("step {step}: QP solver failed!");
7826            }
7827        }
7828    }
7829
7830    #[test]
7831    #[ignore = "long-running regression scenario"]
7832    fn test_mpc_qp_solver_closed_loop_parity() {
7833        let python_final_state = [
7834            -0.014_888_855_512_389,
7835            0.698_023_990_780_443,
7836            0.132_179_560_675_735,
7837            1.589_745_981_585_03,
7838        ];
7839        let python_history_length = 347usize;
7840
7841        let default_result = run_mpc_simulation();
7842        let default_final = default_result.final_state.to_vector();
7843
7844        // QP solver closed-loop
7845        let (cx, cy, cyaw) = generate_switch_back_course();
7846        let speed_profile = calc_speed_profile(&cx, &cy, &cyaw, TARGET_SPEED);
7847
7848        let mut state = State::new(cx[0], cy[0], 0.0, cyaw[0]);
7849        if state.yaw - cyaw[0] >= PI {
7850            state.yaw -= 2.0 * PI;
7851        } else if state.yaw - cyaw[0] <= -PI {
7852            state.yaw += 2.0 * PI;
7853        }
7854
7855        let final_index = cx.len() - 1;
7856        let goal = (cx[final_index], cy[final_index]);
7857        let mut target_index = 0;
7858        let mut warm_start = vec![Vector2::zeros(); T];
7859        let mut step_count = 0;
7860
7861        for _ in 0..MAX_SIM_STEPS {
7862            let (xref, new_index) = calc_ref_trajectory(
7863                &state,
7864                &cx,
7865                &cy,
7866                &cyaw,
7867                &speed_profile,
7868                UPSTREAM_SWITCH_BACK_TICK,
7869                target_index,
7870            );
7871            target_index = new_index;
7872
7873            let result = iterative_linear_mpc_control_with_test_optimizer(
7874                &xref,
7875                &state,
7876                &warm_start,
7877                optimize_linearized_controls_qp,
7878            );
7879
7880            state.update(result.controls[0][0], result.controls[0][1]);
7881            warm_start = next_warm_start(&result.controls);
7882            step_count += 1;
7883
7884            if check_goal(&state, goal, target_index, final_index) {
7885                break;
7886            }
7887        }
7888
7889        let qp_final = state.to_vector();
7890        let qp_len = step_count + 1;
7891
7892        let default_diffs: Vec<f64> = (0..4)
7893            .map(|i| (default_final[i] - python_final_state[i]).abs())
7894            .collect();
7895        let qp_diffs: Vec<f64> = (0..4)
7896            .map(|i| (qp_final[i] - python_final_state[i]).abs())
7897            .collect();
7898
7899        println!("=== Closed-Loop Parity: PG vs QP (clarabel) ===");
7900        println!(
7901            "Python ref: {:?} (len={})",
7902            python_final_state, python_history_length
7903        );
7904        println!();
7905        println!(
7906            "PG: [{:.6},{:.6},{:.6},{:.6}] len={} diffs=[{:.4e},{:.4e},{:.4e},{:.4e}]",
7907            default_final[0],
7908            default_final[1],
7909            default_final[2],
7910            default_final[3],
7911            default_result.hist_x.len(),
7912            default_diffs[0],
7913            default_diffs[1],
7914            default_diffs[2],
7915            default_diffs[3]
7916        );
7917        println!(
7918            "QP: [{:.6},{:.6},{:.6},{:.6}] len={} diffs=[{:.4e},{:.4e},{:.4e},{:.4e}]",
7919            qp_final[0],
7920            qp_final[1],
7921            qp_final[2],
7922            qp_final[3],
7923            qp_len,
7924            qp_diffs[0],
7925            qp_diffs[1],
7926            qp_diffs[2],
7927            qp_diffs[3]
7928        );
7929
7930        let pg_total: f64 = default_diffs.iter().sum();
7931        let qp_total: f64 = qp_diffs.iter().sum();
7932        println!(
7933            "Total: PG={:.4e} QP={:.4e} improvement={:.1}%",
7934            pg_total,
7935            qp_total,
7936            if pg_total > 0.0 {
7937                (1.0 - qp_total / pg_total) * 100.0
7938            } else {
7939                0.0
7940            }
7941        );
7942    }
7943
7944    #[test]
7945    #[ignore = "long-running regression scenario"]
7946    fn test_mpc_solver_comparison_qp_vs_pg_detailed() {
7947        let expected_trace =
7948            parse_switch_back_trace(include_str!("testdata/mpc_switch_back_prefix_trace.csv"));
7949
7950        // 4 solvers
7951        let hybrid_trace = collect_switch_back_detailed_trace(350); // production = hybrid
7952        let qp_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7953            350,
7954            optimize_linearized_controls_qp,
7955        );
7956        let pg_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7957            350,
7958            optimize_linearized_controls_with_full_candidate_expansion,
7959        );
7960        let pg_orig_trace = collect_switch_back_detailed_trace_with_test_optimizer(
7961            350,
7962            optimize_linearized_controls_original,
7963        );
7964
7965        println!("=== Solver Comparison (state diff sum vs Python) ===");
7966        println!(
7967            "{:>5} {:>10} {:>10} {:>10} {:>10}  best",
7968            "step", "hybrid", "qp-only", "pg-exp", "pg-orig"
7969        );
7970
7971        let check_steps: Vec<usize> = (0..=80)
7972            .step_by(5)
7973            .chain([71, 72, 73, 74, 75, 76, 77, 78, 79, 80])
7974            .chain((85..350).step_by(10))
7975            .collect();
7976
7977        for &step in &check_steps {
7978            if step >= expected_trace.len() {
7979                continue;
7980            }
7981            let py = &expected_trace[step];
7982            let hy = hybrid_trace.iter().find(|r| r.step == step);
7983            let qp = qp_trace.iter().find(|r| r.step == step);
7984            let pg = pg_trace.iter().find(|r| r.step == step);
7985            let po = pg_orig_trace.iter().find(|r| r.step == step);
7986
7987            if let (Some(hy), Some(qp), Some(pg), Some(po)) = (hy, qp, pg, po) {
7988                let hy_s: f64 = trace_state_diff(&hy.state, &py.state).iter().sum();
7989                let qp_s: f64 = trace_state_diff(&qp.state, &py.state).iter().sum();
7990                let pg_s: f64 = trace_state_diff(&pg.state, &py.state).iter().sum();
7991                let po_s: f64 = trace_state_diff(&po.state, &py.state).iter().sum();
7992
7993                let min_s = hy_s.min(qp_s).min(pg_s).min(po_s);
7994                let best = if (hy_s - min_s).abs() < 1e-15 {
7995                    "HYBRID"
7996                } else if (qp_s - min_s).abs() < 1e-15 {
7997                    "QP"
7998                } else if (pg_s - min_s).abs() < 1e-15 {
7999                    "PG-exp"
8000                } else {
8001                    "PG-orig"
8002                };
8003
8004                println!(
8005                    "{:5} {:10.4e} {:10.4e} {:10.4e} {:10.4e}  {}",
8006                    step, hy_s, qp_s, pg_s, po_s, best
8007                );
8008            }
8009        }
8010
8011        // Win count
8012        let mut wins = [0u32; 4]; // hybrid, qp, pg-exp, pg-orig
8013        let labels = ["HYBRID", "QP-only", "PG-expanded", "PG-original"];
8014        let min_steps = [
8015            hybrid_trace.len(),
8016            qp_trace.len(),
8017            pg_trace.len(),
8018            pg_orig_trace.len(),
8019            expected_trace.len(),
8020        ]
8021        .into_iter()
8022        .min()
8023        .unwrap();
8024        for (step, py) in expected_trace.iter().enumerate().take(min_steps) {
8025            let traces = [&hybrid_trace, &qp_trace, &pg_trace, &pg_orig_trace];
8026            let diffs: Vec<f64> = traces
8027                .iter()
8028                .map(|t| {
8029                    t.iter()
8030                        .find(|r| r.step == step)
8031                        .map(|r| trace_state_diff(&r.state, &py.state).iter().sum())
8032                        .unwrap_or(f64::INFINITY)
8033                })
8034                .collect();
8035            let min_d = diffs.iter().copied().fold(f64::INFINITY, f64::min);
8036            for (i, &d) in diffs.iter().enumerate() {
8037                if (d - min_d).abs() < 1e-15 {
8038                    wins[i] += 1;
8039                    break;
8040                }
8041            }
8042        }
8043        println!("\n=== Win count (closest to Python per step, out of {min_steps}) ===");
8044        for (i, label) in labels.iter().enumerate() {
8045            println!("{:12}: {} steps", label, wins[i]);
8046        }
8047    }
8048
8049    #[test]
8050    #[ignore = "long-running regression scenario"]
8051    fn test_mpc_switch_back_characterizes_v_injection_pull_ratio_threshold() {
8052        let default_trace = collect_switch_back_detailed_trace_with_test_optimizer(
8053            330,
8054            optimize_linearized_controls,
8055        );
8056        let expanded_trace = collect_switch_back_detailed_trace_with_test_optimizer(
8057            330,
8058            optimize_linearized_controls_with_full_candidate_expansion,
8059        );
8060        let default_warm_start = collect_switch_back_warm_start_trace_with_test_optimizer(
8061            330,
8062            optimize_linearized_controls,
8063        );
8064
8065        let d325 = detailed_row_at_step(&default_trace, 325);
8066        let e325 = detailed_row_at_step(&expanded_trace, 325);
8067        let e326 = detailed_row_at_step(&expanded_trace, 326);
8068        let ws = default_warm_start.get(&325).unwrap();
8069
8070        // Baseline values
8071        let d_v = d325.state[2]; // default v at step 325
8072        let e325_v = e325.state[2]; // expanded v at step 325: ~ -0.804
8073        let e326_v = e326.state[2]; // expanded v at step 326: ~ -0.604
8074
8075        let d_baseline_controls: Vec<[f64; 2]> =
8076            d325.controls.iter().map(|v| [v[0], v[1]]).collect();
8077        let e_baseline_controls: Vec<[f64; 2]> =
8078            e325.controls.iter().map(|v| [v[0], v[1]]).collect();
8079
8080        println!("=== V-Injection Pull Ratio Threshold Sweep ===");
8081        println!(
8082            "default v={:.6}, e325_v={:.6}, e326_v={:.6}",
8083            d_v, e325_v, e326_v
8084        );
8085        println!("Sweeping injected v from e325_v to e326_v in 0.02 steps");
8086        println!();
8087        println!(
8088            "{:>10} {:>10} {:>10} {:>10} {:>10}",
8089            "inj_v", "steer", "same_c0", "opp_c0", "ratio"
8090        );
8091
8092        // Sweep from e325_v to e326_v
8093        let n_steps = 11;
8094        let mut prev_ratio = f64::INFINITY;
8095        let mut threshold_v = None;
8096
8097        for i in 0..=n_steps {
8098            let frac = i as f64 / n_steps as f64;
8099            let injected_v = e325_v + frac * (e326_v - e325_v);
8100
8101            // Create hybrid state: default state with injected v
8102            let mut hybrid = d325.state;
8103            hybrid[2] = injected_v;
8104            let hybrid_state = state_from_vector4(&hybrid);
8105
8106            let result = iterative_linear_mpc_control_with_test_optimizer(
8107                &d325.xref,
8108                &hybrid_state,
8109                ws,
8110                optimize_linearized_controls,
8111            );
8112
8113            let same_c0 = summarize_vec2_sequence_diff(&result.controls, &d_baseline_controls)[0];
8114            let opp_c0 = summarize_vec2_sequence_diff(&result.controls, &e_baseline_controls)[0];
8115
8116            let ratio = if opp_c0 > 0.0 {
8117                same_c0 / opp_c0
8118            } else {
8119                f64::INFINITY
8120            };
8121
8122            println!(
8123                "{:10.6} {:10.6} {:10.4e} {:10.4e} {:10.3}",
8124                injected_v, result.controls[0][1], same_c0, opp_c0, ratio
8125            );
8126
8127            // Detect threshold crossing (ratio drops below 4.0)
8128            if threshold_v.is_none() && prev_ratio > 4.0 && ratio <= 4.0 {
8129                threshold_v = Some((injected_v, ratio));
8130            }
8131            prev_ratio = ratio;
8132        }
8133
8134        if let Some((v, r)) = threshold_v {
8135            println!(
8136                "\nThreshold: ratio drops below 4.0 at v={:.6} (ratio={:.3})",
8137                v, r
8138            );
8139        } else {
8140            println!("\nNo threshold crossing found in this range");
8141        }
8142    }
8143}