import os, sys, glob, math, time, random
from pathlib import Path
from typing import Optional, Tuple, List
from collections import deque
from fitness_function import fitness_score
import random
import json
import numpy as np
LANE_HALF = 2.0
LANE_MARGIN = 1.0
PED_LAT_RIGHT   = 3.0   # extend +1 m for sidewalk
PED_LAT_LEFT    = -3.0  # extend -1 m past left lane edge
PED_INLANE_EPS    = 0.5
PED_APPROACH_BAND = 3.0
LAT_V_MIN         = 0.20
CROSS_HORIZON_S   = 8.0
PED_DS_MAX        = 35                 # meters ahead for pedestrians snapshot
SAFE_DISTANCE_CONST = 35.0             # look-ahead for lead car
TL_WINDOW_AHEAD_M   = 35.0
TL_GATE_RADIUS_M    = 2.5
HIST_LEN            = 4
OPP_LANE_LEFT  = -6.0      # far edge of opposite lane
OPP_LANE_RIGHT = -2.0      # near edge of opposite lane (left edge of ego lane)

# how many vehicles to keep per group
MAX_LEAD_VEHICLES     = 2
MAX_OPPOSITE_VEHICLES = 2
# Ped lateral visibility window: full opposite lane (-6..-2)
# plus +1 m beyond our right lane edge (+2 -> +3)
PED_LAT_MIN = -6.0
PED_LAT_MAX = 3.0
from collections import defaultdict

import numbers
def clean_metrics(metrics: dict, decimals: int = 2) -> dict:
    out = {}
    for k, v in metrics.items():
        if isinstance(v, numbers.Number):
            out[k] = round(float(v), decimals)
        elif isinstance(v, (bool, np.bool_)):
            out[k] = bool(v)
        elif isinstance(v, (list, np.ndarray)):
            out[k] = [
                round(float(x), decimals) if isinstance(x, numbers.Number) else x
                for x in v
            ]
        else:
            out[k] = v
    return out
def merge_metrics(all_metrics):
    merged = defaultdict(list)
    for m in all_metrics:
        for k, v in m.items():
            merged[k].append(v)
    return dict(merged)

def load_path(path_file: str) -> np.ndarray:
    """Load 'x y' per line into np.ndarray (N,2), float32."""
    pts = []
    with open(path_file, "r") as f:
        for line in f:
            line = line.strip()
            if not line or line.startswith("#"):
                continue
            parts = line.replace(",", " ").split()
            if len(parts) >= 2:
                x, y = float(parts[0]), float(parts[1])
                pts.append([x, y])
    arr = np.array(pts, dtype=np.float32)
    if arr.shape[0] < 3:
        raise RuntimeError("Path too short; need >= 3 points.")
    return arr

def precompute_loop_segments(path: np.ndarray):
    """Precompute piecewise-linear loop geometry."""
    A  = path.copy()
    B  = np.roll(path, -1, axis=0)
    AB = B - A
    seg_len = np.linalg.norm(AB, axis=1)
    s_before = np.concatenate(([0.0], np.cumsum(seg_len)[:-1]))
    L = float(np.sum(seg_len))
    return A, B, AB, seg_len, s_before, L

def project_point_onto_loop(point_xy, A, AB, seg_len, s_before):
    AP = point_xy[None, :] - A
    denom = np.maximum(seg_len**2, 1e-9)
    t_all = np.clip(np.sum(AP * AB, axis=1) / denom, 0.0, 1.0)
    proj   = A + (t_all[:, None] * AB)
    d2     = np.sum((proj - point_xy[None, :])**2, axis=1)
    k = int(np.argmin(d2))
    t = float(t_all[k])
    s_now = float(s_before[k] + t * seg_len[k])

    if seg_len[k] > 1e-6:
        tangent = AB[k] / seg_len[k]
        normal  = np.array([-tangent[1], tangent[0]], dtype=np.float32)
        lateral = float(np.dot(point_xy - proj[k], normal))
    else:
        lateral = 0.0

    return s_now, lateral, k, t, proj[k]


def wrap_forward_progress(s_now, s0, L):
    d = s_now - s0
    return float(d % L)

def ped_cross_target_for(walker_loc: carla.Location, path_xy: np.ndarray, cross_offset: float = 8.0) -> Tuple[float, float]:
    """Geometric 'cross the road' target: push across the path normal."""
    p = np.array([walker_loc.x, walker_loc.y], dtype=np.float32)
    A  = path_xy
    B  = np.roll(path_xy, -1, axis=0)
    AB = B - A
    seg_len = np.linalg.norm(AB, axis=1)
    denom = np.maximum(seg_len**2, 1e-9)
    t_all = np.clip(np.sum((p[None,:]-A)*AB, axis=1) / denom, 0.0, 1.0)
    proj   = A + t_all[:,None]*AB
    d2     = np.sum((proj - p[None,:])**2, axis=1)
    k = int(np.argmin(d2))
    seg = AB[k]
    L   = np.linalg.norm(seg)
    if L < 1e-6:
        n = np.array([0.0, 1.0], dtype=np.float32)
    else:
        n = np.array([-seg[1]/L, seg[0]/L], dtype=np.float32)  # left normal
    side = np.sign(np.dot(p - proj[k], n))
    target = proj[k] - side * n * cross_offset
    return float(target[0]), float(target[1])

def get_tm_with_fallback(client, start_port=8000, tries=1000):
    """Traffic Manager bind fallback over a port range."""
    last_err = None
    for p in range(start_port, start_port + tries):
        try:
            tm = client.get_trafficmanager(p)
            return tm, p
        except RuntimeError as e:
            last_err = e
    raise RuntimeError(f"Traffic Manager bind failed on [{start_port}..{start_port+tries-1}]. Last: {last_err}")
def rot_world_to_ego(yaw_rad: float) -> np.ndarray:
    c, s = math.cos(yaw_rad), math.sin(yaw_rad)
    return np.array([[ c,  s],
                     [-s,  c]], dtype=np.float32)

def ttc_longitudinal(gap_long_m, rel_long_mps, cap=20.0):
    if rel_long_mps < -1e-3:   # require at least 1 mm/s closing
        t = gap_long_m / max(1e-3, -rel_long_mps)
        return float(min(t, cap))
    return None

def ttc_conflict(gap_long_m, rel_long_mps, cap=20.0):
    rel_speed = abs(rel_long_mps)
    if rel_speed < 1e-3:   # essentially not moving
        return None
    t = gap_long_m / rel_speed
    return float(min(t, cap))

def frenet_s_lat(point_xy, A, AB, seg_len, s_before):
    s, lat, *_ = project_point_onto_loop(point_xy, A, AB, seg_len, s_before)
    return s, lat

def in_lane_ahead(s_now, s_obj, lat_obj, L, max_ahead):
    ds = (s_obj - s_now) % L
    return (0.0 < ds <= max_ahead) and (abs(lat_obj) <= (LANE_HALF + LANE_MARGIN)), ds

def _time_to_target_lat(target_lat, gap_lat_m, rel_lat_mps, vmin=LAT_V_MIN):
    if abs(rel_lat_mps) < vmin:
        return None
    delta = target_lat - gap_lat_m
    if delta * rel_lat_mps <= 0:
        return None
    return delta / rel_lat_mps

def map_known_lights_to_actors(world, search_radius=50.0):
    tl_actors = list(world.get_actors().filter("traffic.traffic_light*"))
    mapping = {}
    for i, (lx, ly) in KNOWN_TL.items():
        best, best_d = None, 1e9
        for tl in tl_actors:
            loc = tl.get_transform().location
            d = math.hypot(loc.x - lx, loc.y - ly)
            if d < best_d:
                best_d = d
                best = tl
        mapping[i] = best.id if (best is not None and best_d <= search_radius) else None
    return mapping

def tl_state_str(world, actor_id):
    if actor_id is None:
        return "Unknown"
    tl = world.get_actor(actor_id)
    if tl is None:
        return "Unknown"
    st = str(tl.get_state())
    return "Red" if st == "Yellow" else st  # match inference: treat Yellow as Red
KNOWN_TL = {
    1: (321.68, 136.13),
    2: (331.99, 184.47),
    3: (102.72, 192.55),
    4: (94.99, 144.38),
}
# ------------------------ Environment class ------------------------
class CarlaTown01Env:
    """
    Town01 ENV (evolved, no obs/schema changes):
      - Keeps: fixed Town01 loop, Tesla Model 3 ego, TL gate logic (Yellow treated as Red), start/goal, termination rules.
      - Harder, interesting behaviors (solvable, incremental):
          1) Rolling-Slow Convoy: guarantee a 1–2 car convoy ahead that rolls ~35–45% below flow for ~12–18 s, then resumes.
          2) Oncoming Burst Platoons: short windows (10–14 s) of tighter oncoming via speed-up (no extra spawns), then clear gap.
          3) TL Queue Compression: 1–2 “gate-pausers” briefly hesitate (1–2 s) just before a stop line, then proceed on green.
          4) Accordion Leads: a small fraction of regular leads oscillate speed slightly (±5–8% over 6–10 s) to test stability.
          5) Stoppers: more reliable arming (ahead within ~40 m), longer but bounded pauses (9–13 s), then auto-resume.
      - Counts & steps: slightly higher vehicle/ped load by default, slightly lower max_steps baseline; density & timing still scale
        with Actual Score through existing _compute_diff() mapping. Jam-watchdog remains as safety valve. No lane change.
    NOTE: All helper functions / constants (load_path, precompute_loop_segments, project_point_onto_loop, frenet_s_lat,
          wrap_forward_progress, rot_world_to_ego, KNOWN_TL, map_known_lights_to_actors, tl_state_str, get_tm_with_fallback,
          ttc_longitudinal, ttc_conflict, ped_cross_target_for, _time_to_target_lat, fitness_score, and constants like
          HIST_LEN, TL_WINDOW_AHEAD_M, TL_GATE_RADIUS_M, LANE_HALF, LANE_MARGIN, OPP_LANE_LEFT, OPP_LANE_RIGHT,
          MAX_LEAD_VEHICLES, MAX_OPPOSITE_VEHICLES, SAFE_DISTANCE_CONST, PED_DS_MAX, PED_LAT_LEFT, PED_LAT_RIGHT,
          CROSS_HORIZON_S, etc.) are assumed to exist above and are treated as read-only.
    """

    # ---------------------- init ----------------------
    def __init__(self,
                 host="localhost",
                 port=2000,
                 tm_port=8000,
                 town="Town01",
                 path_file="recorded_path_cleaned.txt",
                 policy_path=None,
                 sync=True, fps=20,
                 target_kmh=25.0,
                 # ↑ Slightly higher defaults; still scaled by _compute_diff()
                 num_vehicles=100, num_peds=500,
                 global_plan_step=10,
                 start_tol_m=2.0, start_gate_frac=0.90,
                 save_plan_file="extracted_path.txt",
                 harder_cars=True, ignore_light_pct=10, lane_change_pct=0,
                 speed_diff_mean=12.0, min_follow_dist=3.0,
                 harder_peds=True, ped_cross_prob=0.7,
                 ped_cross_every_s=4.2, ped_cross_offset_m=6.0,
                 # ↓ Slightly lower baseline, but _compute_diff() can extend modestly
                 max_steps=5500,
                 jitter_ratio=0.15,
                 actual_score=None,
                 # ===== Existing strategic knobs (kept) =====
                 stopper_fraction=0.28,
                 stopper_min_steps=180,            # ~9 s @20 FPS
                 stopper_max_steps=260,            # ~13 s
                 fast_oncoming_fraction=0.40,
                 fast_oncoming_dv_pct=-25,         # faster than limit (negative %)
                 opp_view_ds_max=50.0,
                 # Jam watchdog (kept)
                 jam_check_every_s=1.0,
                 jam_speed_thresh=0.10,
                 jam_stuck_steps=80,
                 jam_boost_pct=-12,
                 jam_boost_hold_steps=40,
                 # ===== New behavior knobs (no obs changes) =====
                 # Rolling-slow convoy (guaranteed lead pressure)
                 convoy_enable=True,
                 convoy_size=2,
                 convoy_min_gap_m=20.0,
                 convoy_max_gap_m=60.0,
                 convoy_slow_pct=38,               # +% slower than limit (applied above baseline dv)
                 convoy_window_s=(12.0, 18.0),
                 # Oncoming burst platoons (temporal squeeze, then clear)
                 bursts_enable=True,
                 burst_first_delay_s=(3.0, 6.0),
                 burst_duration_s=(10.0, 14.0),
                 burst_cooldown_s=(10.0, 14.0),
                 burst_fast_dv_pct=-30,            # stronger speed-up during burst
                 # TL queue compression (brief hesitation before stop line)
                 gate_pause_enable=True,
                 gate_pauser_count=2,
                 gate_pause_steps=(20, 40),        # ~1–2 s hesitation
                 gate_pause_trigger_ds_m=12.0,     # along-loop distance to gate to trigger pause
                 # Accordion effect for some leads (gentle speed oscillation)
                 accordion_enable=True,
                 accordion_fraction=0.12,
                 accordion_amp_pct=6.0,
                 accordion_period_s=(6.0, 10.0)
                 ):
        import numpy as np

        # --- Config / parameters ---
        self.host = host
        self.port = int(port)
        self.tm_port_req = int(tm_port)
        self.town = town
        self.path_file = path_file
        self.sync = bool(sync); self.fps = int(fps)
        self.policy_path = policy_path

        self.target_kmh = float(target_kmh)
        self.num_vehicles = int(num_vehicles)
        self.num_peds = int(num_peds)
        self.global_plan_step = int(global_plan_step)
        self.max_steps = int(max_steps)

        self.start_tol_m = float(start_tol_m)
        self.start_gate_frac = float(start_gate_frac)
        self.save_plan_file = save_plan_file

        # Traffic controls
        self.harder_cars = bool(harder_cars)
        self.ignore_light_pct = int(ignore_light_pct)
        self.lane_change_pct = int(lane_change_pct)
        self.speed_diff_mean = float(speed_diff_mean)
        self.min_follow_dist = float(min_follow_dist)

        # Pedestrians
        self.harder_peds = bool(harder_peds)
        self.ped_cross_prob = float(ped_cross_prob)
        self.ped_cross_every_s = float(ped_cross_every_s)
        self.ped_cross_offset_m = float(ped_cross_offset_m)

        # Difficulty / evolution
        self.jitter_ratio = float(jitter_ratio)
        self.actual_score = None if actual_score is None else float(actual_score)
        self._eff = None  # computed per episode

        # Existing strategic knobs (kept)
        self.stopper_fraction = float(stopper_fraction)
        self.stopper_min_steps = int(stopper_min_steps)
        self.stopper_max_steps = int(stopper_max_steps)
        self.fast_oncoming_fraction = float(fast_oncoming_fraction)
        self.fast_oncoming_dv_pct = int(fast_oncoming_dv_pct)
        self.opp_view_ds_max = float(opp_view_ds_max)

        # Jam watchdog
        self.jam_check_every_s = float(jam_check_every_s)
        self.jam_speed_thresh = float(jam_speed_thresh)
        self.jam_stuck_steps = int(jam_stuck_steps)
        self.jam_boost_pct = int(jam_boost_pct)
        self.jam_boost_hold_steps = int(jam_boost_hold_steps)

        # New behaviors
        self.convoy_enable = bool(convoy_enable)
        self.convoy_size = int(convoy_size)
        self.convoy_min_gap_m = float(convoy_min_gap_m)
        self.convoy_max_gap_m = float(convoy_max_gap_m)
        self.convoy_slow_pct = int(convoy_slow_pct)
        self.convoy_window_s = tuple(float(x) for x in convoy_window_s)

        self.bursts_enable = bool(bursts_enable)
        self.burst_first_delay_s = tuple(float(x) for x in burst_first_delay_s)
        self.burst_duration_s = tuple(float(x) for x in burst_duration_s)
        self.burst_cooldown_s = tuple(float(x) for x in burst_cooldown_s)
        self.burst_fast_dv_pct = int(burst_fast_dv_pct)

        self.gate_pause_enable = bool(gate_pause_enable)
        self.gate_pauser_count = int(gate_pauser_count)
        self.gate_pause_steps = tuple(int(x) for x in gate_pause_steps)
        self.gate_pause_trigger_ds_m = float(gate_pause_trigger_ds_m)

        self.accordion_enable = bool(accordion_enable)
        self.accordion_fraction = float(accordion_fraction)
        self.accordion_amp_pct = float(accordion_amp_pct)
        self.accordion_period_s = tuple(float(x) for x in accordion_period_s)
        # === NEW harder-behavior knobs (constructor unchanged; tweak here as needed) ===
        self.bottleneck_enable = True
        self.bottleneck_len_m = 45.0
        self.bottleneck_width_m = 2.6
        self.bottleneck_start_delay_s = (6.0, 12.0)
        self.bottleneck_hold_s = (8.0, 12.0)
        self.brake_check_enable = True
        self.brake_check_fraction = 0.08
        self.brake_check_add_dv_pct = 80   # additional slow-down during check (positive = slower)
        self.brake_check_hold_s = (1.0, 2.0)
        self.brake_check_cooldown_s = (6.0, 10.0)
        self.crowded_peds = True
        self.crowd_cluster_min = 30
        self.crowd_cluster_max = 60
        self.crowd_stagger_max_s = 0.25
        self.cross_chain_enable = True
        self.cross_chain_len = (2, 3)           # number of quick successive triggers
        self.cross_chain_gap_s = (0.8, 1.4)     # spacing between them
        self.occluder_enable = True


        # --- Static path data ---
        self.path_xy = load_path(self.path_file)
        self.A, self.B, self.AB, self.seg_len, self.s_before, self.L = precompute_loop_segments(self.path_xy)
        self.first_pt = self.path_xy[0]

        # --- Runtime (set/reset per episode) ---
        self.client = None; self.world = None; self.bp_lib = None
        self.tm = None; self.tm_port_used = None

        self.ego = None; self.col_sensor = None
        self.vehicles = []; self.walkers = []; self.walker_ctrls = []; self.ped_pairs = []
        self.collision_flag = {"flag": False}; self.num_red_violations = 0
        self.positions = []; self.steerings = []; self.speeds_kmh = []; self.total_steps = 0; self.done_path = False

        # Pedestrian guards
        self.ped_cooldown_min_s   = 15.0
        self.ped_cooldown_max_s   = 30.0
        self.ped_lane_ttl_s       = 4.0
        self.ped_offlane_offset_m = 5.0
        self._ped_ready_at = {}
        self._ped_lane_enter_at = {}

        self.s0 = 0.0
        self.last_ped_retarget = 0.0
        self.spawn_points_xy = [(150.000, 133.437, 0.5), (0, 0, 0.5)]

        # OBS histories
        from collections import deque as _dq
        self.obs_speed_hist    = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_lateral_hist  = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_yaw_err_hist  = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_steer_hist    = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_throttle_hist = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_brake_hist    = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self._lat_ema = None; self._last_control = None

        # TLs
        self.tl_actor_map = {}; self.s_light = {}; self.gate_xy = {}; self._in_gate_prev = {}

        # Sync pacing
        self.pace_real_time = True; self._last_tick_time = None

        # Diagnostics
        self.collision_ids = []

        # Vehicle behavior bookkeeping
        self._veh_baseline_dv = {}     # veh.id -> baseline dv%
        self._veh_class = {}           # veh.id -> {"fast_oncoming":bool}
        self._veh_unstuck_until = {}   # veh.id -> time for spawn kick

        # Stoppers
        self._stoppers = {}            # veh.id -> {"remaining":int, "active":bool, "armed":bool}

        # Jam watchdog
        self._veh_stuck_count = {}     # veh.id -> consecutive steps below speed
        self._veh_jam_boost_until = {} # veh.id -> step index until boost holds

        # New runtime state
        self._convoy_members = set()   # vehicle ids in rolling convoy
        self._convoy_end_step = -1

        self._burst_active = False
        self._burst_end_step = -1
        self._burst_next_start_step = -1

        self._gate_pausers = {}        # vid -> remaining steps of brief pre-line hesitation

        self._accordion = {}           # vid -> {"amp":float, "period":int, "phase":float}
        # NEW runtime state
        self._bneck_active = False
        self._bneck_end_step = -1
        self._bneck_center_s = None
        self._bneck_blocker = None   # parked encroacher

        self._brake_checkers = {}    # vid -> {"next_step":int, "hold_until":int, "cool_until":int, "active":bool}
        self._chain_plan = {"remaining": 0, "next_step": -1}
        self._occluder = None


        # Zones (kept for compatibility)
        self._zones = []
        self._zone_active = {}

        if self.policy_path:
            self.load_policy(self.policy_path)

    # ---------------------- difficulty shaping ----------------------
    def set_actual_score(self, score: float):
        try:
            self.actual_score = float(score)
        except:
            self.actual_score = None

    def _compute_diff(self):
        """
        Map Actual Score to modest scaling (kept; no schema changes).
        """
        s = self.actual_score
        x = 0.0 if (s is None or not (0.0 <= s <= 1.0)) else max(-1.0, min(1.0, (s - 0.5) / 0.5))

        traffic_scale = 1.0 + 0.30 * max(0.0, x) - 0.10 * max(0.0, -x)
        ped_scale     = 1.0 + 0.50 * max(0.0, x) - 0.12 * max(0.0, -x)

        ignore_light = max(0, min(15, self.ignore_light_pct + int(round(5 * max(0.0, x)))))
        min_follow   = max(4.0, self.min_follow_dist - 1.0 * max(0.0, x))

        ped_prob     = min(0.9, self.ped_cross_prob + 0.20 * max(0.0, x))
        ped_every_s  = max(3.5, self.ped_cross_every_s * (1.0 / (1.0 + 0.30 * max(0.0, x))))

        step_mult    = 1.0 + min(0.20, 0.30*(traffic_scale-1.0) + 0.45*(ped_scale-1.0))

        return {
            "traffic_scale": traffic_scale,
            "ped_scale": ped_scale,
            "ignore_light_pct": int(ignore_light),
            "min_follow_dist": float(min_follow),
            "ped_cross_prob":  float(ped_prob),
            "ped_cross_every_s": float(ped_every_s),
            "step_mult": float(step_mult),
        }

    # ---------------------- policy loader ----------------------
    def load_policy(self, policy_file: str):
        code = Path(policy_file).read_text()
        ns = {}
        exec(code, ns, ns)
        if "policy" in ns and callable(ns["policy"]):
            self.policy_fn = ns["policy"]
        elif "Policy" in ns and callable(ns["Policy"]):
            inst = ns["Policy"]()
            if hasattr(inst, "compute_action") and callable(inst.compute_action):
                self.policy_fn = lambda obs: inst.compute_action(obs, None)
            else:
                raise RuntimeError("Policy class must define a 'compute_action(obs, path)'.")
        else:
            raise RuntimeError("Policy file must define either function 'policy(obs)' or class 'Policy'.")

    # ---------------------- reset/run/cleanup ----------------------
    def reset(self, seed=None, save_plan_on_ep0=False, ep_index=0, actual_score=0.7):
        import random, numpy as np, time, carla

        if seed is not None:
            random.seed(seed); np.random.seed(seed)

        if actual_score is not None:
            self.set_actual_score(actual_score)
        self._eff = self._compute_diff()

        # modestly extend max_steps when density rises (kept)
        self.max_steps = int(max(self.max_steps, round(self.max_steps * self._eff["step_mult"])))

        # cleanup previous episode
        self.cleanup(silent=True)

        # connect + map
        self.client = carla.Client(self.host, self.port); self.client.set_timeout(60.0)
        self.world = self.client.load_world(self.town)
        self.bp_lib = self.world.get_blueprint_library()
        self._set_sync(self.sync, self.fps)

        self._veh_baseline_dv.clear()
        self._veh_class.clear()
        self._veh_unstuck_until.clear()
        self._stoppers.clear()
        self._veh_stuck_count.clear()
        self._veh_jam_boost_until.clear()

        # New state clear
        self._convoy_members.clear()
        self._convoy_end_step = -1
        self._burst_active = False
        self._burst_end_step = -1
        self._burst_next_start_step = -1
        self._gate_pausers.clear()
        self._accordion.clear()

        # Traffic Manager
        self.tm, self.tm_port_used = get_tm_with_fallback(self.client, start_port=self.tm_port_req)
        self.tm.set_synchronous_mode(self.sync)

        if self.sync:
            self.world.tick()
        else:
            self.world.wait_for_tick()
        self._last_tick_time = time.perf_counter()

        # spawn ego (strict Tesla)
        self.ego = self._spawn_ego_from_list(index=0)
        if self.sync: self.world.tick()
        else: self.world.wait_for_tick()
        self._ped_last_positions = {}

        # collision sensor
        self.col_sensor = self._add_collision_sensor(self.ego, self._on_collision)

        # vehicles: scaled with jitter
        base_v = int(round(self.num_vehicles * self._eff["traffic_scale"]))
        veh_count = max(0, int(round(base_v * (1.0 + self._jitter()))))
        self.vehicles = self._spawn_traffic(veh_count)
        print(f"[VEH] requested={veh_count} spawned={len(self.vehicles)}")
        if self.harder_cars and self.vehicles:
            self._configure_harder_cars(self.vehicles)

        # pedestrians
        base_p = int(round(self.num_peds * self._eff["ped_scale"]))
        ped_count = max(0, int(round(base_p * (1.0 + self._jitter()))))
        walker_ids, ctrl_ids = self._spawn_pedestrians_batch(ped_count)
        self.walkers = [self.world.get_actor(wid) for wid in walker_ids if self.world.get_actor(wid)]
        self.walker_ctrls = [self.world.get_actor(cid) for cid in ctrl_ids if self.world.get_actor(cid)]
        self.ped_pairs = list(zip(self.walkers, self.walker_ctrls))
        print(f"[PED] requested={ped_count} spawned={len(self.walkers)} ctrls={len(self.walker_ctrls)}")
        if self.sync:
            for _ in range(2):
                self.world.tick()
        else:
            self.world.wait_for_tick()
        self._ped_ready_at = {}
        self._ped_lane_enter_at = {}

        # plan
        self.plan = self._build_global_plan(self.global_plan_step)
        if not self.plan:
            raise RuntimeError("Global plan is empty. Check your path/town.")
        if save_plan_on_ep0 and ep_index == 0:
            self._save_plan_to_txt(self.plan, self.save_plan_file)

        # start progress
        start_loc = self.ego.get_transform().location
        self.s0, *_ = project_point_onto_loop(np.array([start_loc.x, start_loc.y], dtype=np.float32),
                                              self.A, self.AB, self.seg_len, self.s_before)

        # logs & counters
        self.positions, self.steerings, self.speeds_kmh = [], [], []
        self.total_steps = 0; self.done_path = False
        self.collision_flag["flag"] = False; self.collision_ids = []
        self.num_red_violations = 0
        self.last_ped_retarget = time.time()

        # traffic lights map
        self.tl_actor_map = map_known_lights_to_actors(self.world, search_radius=25.0)
        self.s_light, self.gate_xy = {}, {}
        for idx, (lx, ly) in KNOWN_TL.items():
            pos = np.array([lx, ly], dtype=np.float32)
            s_i, _, _, _, proj_xy = project_point_onto_loop(pos, self.A, self.AB, self.seg_len, self.s_before)
            self.s_light[idx] = float(s_i); self.gate_xy[idx] = (float(proj_xy[0]), float(proj_xy[1]))
        self._in_gate_prev = {idx: False for idx in KNOWN_TL}

        # obs histories fresh
        from collections import deque as _dq
        self.obs_speed_hist    = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_lateral_hist  = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_yaw_err_hist  = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_steer_hist    = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_throttle_hist = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self.obs_brake_hist    = _dq([0.0]*HIST_LEN, maxlen=HIST_LEN)
        self._lat_ema = None; self._last_control = None

        # initialize behaviors
        self._init_vehicle_classes()
        self._assign_stoppers()           # improved reliability & durations
        self._assign_gate_pausers()       # brief pre-line hesitations near first TLs
        self._assign_rolling_convoy()     # guarantee early rolling lead pressure
        self._init_accordion_leads()      # gentle oscillations for a small fraction
        self._init_burst_schedule()       # temporal oncoming squeeze scheduler
        self._kickstart_traffic(duration_s=2.0)
        # --- NEW: schedule bottleneck encroacher + brake-checkers + ped chain + occluder
        try:
            # Bottleneck window
            if self.bottleneck_enable:
                import random as _r
                delay = _r.uniform(*self.bottleneck_start_delay_s)
                hold  = _r.uniform(*self.bottleneck_hold_s)
                self._bneck_end_step = self.total_steps + int(round(self.fps * (delay + hold)))
                # place center s roughly half-loop ahead
                self._bneck_center_s = (self.s0 + 0.5 * self.L + _r.uniform(-0.2, 0.2) * self.L) % self.L
                self._spawn_bottleneck_blocker(self._bneck_center_s)
                self._bneck_active = True
        except Exception as _e:
            self._bneck_active = False
            self._bneck_blocker = None

        # Choose brake-check actors (subset of in-lane vehicles)
        self._brake_checkers.clear()
        if self.brake_check_enable and self.vehicles:
            self._assign_brake_checkers(self.vehicles)

        # Plan a pedestrian chain (activated later in loop)
        self._chain_plan = {"remaining": 0, "next_step": -1}
        if self.cross_chain_enable:
            # 50% chance per episode
            import random as _r
            if _r.random() < 1:
                length = _r.randint(self.cross_chain_len[0], self.cross_chain_len[1])
                gap_s  = _r.uniform(self.cross_chain_gap_s[0], self.cross_chain_gap_s[1])
                self._chain_plan = {"remaining": int(length), "next_step": self.total_steps + int(round(self.fps * gap_s))}

        # Occluder near a TL gate (tall parked vehicle)
        self._spawn_occluder_near_gate()

        # Jam watchdog state
        self._last_jam_check_step = 0
        if self.sync:
            for _ in range(30):        # ~1.5s at 20 FPS
                self._tick()

    # ---------------------- observation (unchanged schema) ----------------------
    def get_obs(self, control=None) -> dict:
        import numpy as np, math, random as _r
        if self.ego is None:
            return {}

        tf = self.ego.get_transform()
        pos = np.array([tf.location.x, tf.location.y], dtype=np.float32)
        yaw_rad = math.radians(tf.rotation.yaw)
        vel = self.ego.get_velocity()
        ang = self.ego.get_angular_velocity()
        yaw_rate = math.radians(ang.z)
        v_w = np.array([vel.x, vel.y], dtype=np.float32)
        speed = float(np.linalg.norm(v_w))

        s_now, lateral, k_near, _, _proj_xy = project_point_onto_loop(pos, self.A, self.AB, self.seg_len, self.s_before)
        tangent = self.AB[k_near] / (np.linalg.norm(self.AB[k_near]) + 1e-9)
        path_yaw = math.atan2(tangent[1], tangent[0])
        yaw_error = ((yaw_rad - path_yaw + math.pi) % (2*math.pi)) - math.pi

        # EMA for lateral
        if self._lat_ema is None:
            self._lat_ema = lateral
        self._lat_ema = 0.2 * lateral + 0.8 * self._lat_ema
        lateral_out = self._lat_ema

        ctrl = control if control is not None else self._last_control
        steer    = float(getattr(ctrl, "steer", 0.0) if ctrl else 0.0)
        throttle = float(getattr(ctrl, "throttle", 0.0) if ctrl else 0.0)
        brake    = float(getattr(ctrl, "brake", 0.0) if ctrl else 0.0)

        self.obs_speed_hist.append(speed)
        self.obs_lateral_hist.append(lateral_out)
        self.obs_yaw_err_hist.append(yaw_error)
        self.obs_steer_hist.append(steer)
        self.obs_throttle_hist.append(throttle)
        self.obs_brake_hist.append(brake)

        R_we = rot_world_to_ego(yaw_rad)

        # ---- Traffic light snapshot (kept) ----
        tl_active_idx, tl_active_dist, tl_active_state = None, None, "Unknown"
        for idx, (lx, ly) in KNOWN_TL.items():
            gx, gy = self.gate_xy[idx]
            dist_gate = math.hypot(pos[0] - gx, pos[1] - gy)
            s_gate = self.s_light[idx]
            ds = (s_gate - s_now) % self.L
            if 0.0 <= ds <= TL_WINDOW_AHEAD_M:
                tl_active_idx = idx
                tl_active_dist = dist_gate
                tl_active_state = tl_state_str(self.world, self.tl_actor_map.get(idx))
                break
        tl_obs = {"exists": False, "state": None}
        if tl_active_idx is not None:
            tl_obs = {"exists": True,
                      "dist_m": None if tl_active_dist is None else round(float(tl_active_dist), 2),
                      "state": tl_active_state}

        # ---- Vehicles snapshot (lead & oncoming; horizons kept) ----
        ego_candidates, opp_candidates = [], []
        lead_cars, opposite_cars = [], []

        for v in self.world.get_actors().filter("vehicle*"):
            if v.id == self.ego.id:
                continue
            pl = v.get_transform().location
            pxy = np.array([pl.x, pl.y], dtype=np.float32)

            s_v, lat_v = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
            ds = (s_v - s_now) % self.L
            if ds <= 0.0:
                continue

            # Separate horizons: leads use SAFE_DISTANCE_CONST; opposite cars use self.opp_view_ds_max
            if abs(lat_v) <= (LANE_HALF + LANE_MARGIN):
                if ds > SAFE_DISTANCE_CONST:
                    continue
            else:
                if ds > self.opp_view_ds_max:
                    continue

            p_rel_e = R_we @ (pxy - pos)
            if p_rel_e[0] <= 0.0:
                continue

            vv = v.get_velocity()
            v2 = np.array([vv.x, vv.y], dtype=np.float32)
            v_rel_e = R_we @ (v2 - v_w)

            gap_long_m   = float(p_rel_e[0])
            gap_lat_m    = float(p_rel_e[1])
            rel_long_mps = float(v_rel_e[0])
            d_eucl = float(np.hypot(pxy[0]-pos[0], pxy[1]-pos[1]))

            if -LANE_HALF <= lat_v <= LANE_HALF:
                ttc_s = ttc_longitudinal(gap_long_m, rel_long_mps, cap=20.0)
                thw_s = (gap_long_m / max(0.5, speed)) if gap_long_m > 0 else None
                ego_candidates.append({
                    "gap_long_m": round(gap_long_m, 2),
                    "gap_lat_m":  round(gap_lat_m, 2),
                    "rel_long_mps": round(rel_long_mps, 2),
                    "ttc_s": None if ttc_s is None else round(ttc_s, 2),
                    "thw_s": None if thw_s is None else round(thw_s, 2),
                    "_sort_key": gap_long_m,
                })
            
            elif (p_rel_e[0] > 0.0                                   # still ahead (ego X)
                and abs(gap_lat_m) <= (LANE_HALF + 0.5)            # stays near our lane
                and ds <= SAFE_DISTANCE_CONST                      # within usual lead horizon
                and abs(ds - d_eucl) <= 1.):                        # arc ≈ euclid (tiny grace)
                ttc_s = ttc_longitudinal(gap_long_m, rel_long_mps, cap=20.0)
                thw_s = (gap_long_m / max(0.5, speed)) if gap_long_m > 0 else None
                ego_candidates.append({
                    "gap_long_m": round(gap_long_m, 2),
                    "gap_lat_m":  round(gap_lat_m, 2),
                    "rel_long_mps": round(rel_long_mps, 2),
                    "ttc_s": None if ttc_s is None else round(ttc_s, 2),
                    "thw_s": None if thw_s is None else round(thw_s, 2),
                    "_sort_key": gap_long_m,
                })

            elif OPP_LANE_LEFT <= lat_v < OPP_LANE_RIGHT:
                t_conflict_s = ttc_conflict(gap_long_m, rel_long_mps, cap=20.0)
                opp_candidates.append({
                    "gap_long_m": round(gap_long_m, 2),
                    "gap_lat_m":  round(gap_lat_m, 2),
                    "rel_long_mps": round(rel_long_mps, 2),
                    "t_conflict_s": None if t_conflict_s is None else round(t_conflict_s, 2),
                    "_sort_key": gap_long_m,
                })

        if ego_candidates:
            ego_candidates.sort(key=lambda d: d["_sort_key"])
            lead_cars = [{k: v for k, v in d.items() if k != "_sort_key"} for d in ego_candidates[:MAX_LEAD_VEHICLES]]
        if opp_candidates:
            opp_candidates.sort(key=lambda d: d["_sort_key"])
            opposite_cars = [{k: v for k, v in d.items() if k != "_sort_key"} for d in opp_candidates[:MAX_OPPOSITE_VEHICLES]]

        # --- Oncoming edge intrusions (kept as before) ---
        if opposite_cars:
            for oc in opposite_cars:
                try:
                    if oc.get("gap_long_m", 1e9) <= 35.0 and oc.get("gap_lat_m", 0.0) < 0.0:
                        r = _r.random()
                        if r >= 0.7:
                            oc["gap_lat_m"] = round(oc["gap_lat_m"] + (0.6 if r > 0.9 else 0.4), 2)
                except Exception:
                    continue

        # ---- Pedestrians snapshot (kept) ----
        ped_list = []
        dt = self.world.get_settings().fixed_delta_seconds or 0.05
        for p in self.world.get_actors().filter("walker.pedestrian*"):
            pl  = p.get_transform().location
            pxy = np.array([pl.x, pl.y], dtype=np.float32)

            s_p, _ = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
            ds = (s_p - s_now) % self.L
            if not (0.0 < ds <= min(PED_DS_MAX, 35.0)):
                continue

            pv   = p.get_velocity()
            pv2  = np.array([pv.x, pv.y], dtype=np.float32)
            if np.allclose(pv2, 0.0):
                if p.id in getattr(self, "_ped_last_positions", {}):
                    prev = self._ped_last_positions[p.id]
                    pv2 = (pxy - prev) / dt
            self._ped_last_positions[p.id] = pxy

            p_rel_e = R_we @ (pxy - pos)
            v_rel_e = R_we @ (pv2 - v_w)
            gap_long_m  = float(p_rel_e[0])
            gap_lat_m   = float(p_rel_e[1])
            rel_lat_mps = float(v_rel_e[1])

            if -LANE_HALF <= gap_lat_m <= LANE_HALF:
                ped_list.append({
                    "lane": "ego",
                    "state": "in_lane",
                    "gap_long_m": round(gap_long_m, 2),
                    "gap_lat_m":  round(gap_lat_m, 2),
                    "rel_lat_mps": round(rel_lat_mps, 2),
                    "t_enter_lane_s": 0.0,
                    "side": "right" if gap_lat_m > 0 else ("left" if gap_lat_m < 0 else "center"),
                })
                continue

            in_left_band  = (PED_LAT_LEFT  <= gap_lat_m < -LANE_HALF)
            in_right_band = (LANE_HALF < gap_lat_m <= PED_LAT_RIGHT)
            if in_left_band or in_right_band:
                moving_toward = (rel_lat_mps * gap_lat_m) < 0.0
                if not moving_toward:
                    continue
                target_edge = (-LANE_HALF) if in_left_band else (LANE_HALF)
                t_enter = _time_to_target_lat(target_edge, gap_lat_m, rel_lat_mps)
                if (t_enter is None) or not (0.0 < t_enter <= CROSS_HORIZON_S):
                    continue
                ped_list.append({
                    "lane": "approach",
                    "state": "approaching_lane",
                    "gap_long_m": round(gap_long_m, 2),
                    "gap_lat_m":  round(gap_lat_m, 2),
                    "rel_lat_mps": round(rel_lat_mps, 2),
                    "t_enter_lane_s": round(t_enter, 2),
                    "side": "right" if gap_lat_m > 0 else "left",
                })

        return {
            "speed_mps": round(speed, 2),
            "yaw_rate_rps": round(yaw_rate, 3),
            "speed_hist4":        [round(x, 2) for x in list(self.obs_speed_hist)],
            "lateral_hist4":      [round(x, 3) for x in list(self.obs_lateral_hist)],
            "yaw_error_hist4":    [round(x, 3) for x in list(self.obs_yaw_err_hist)],
            "steer_cmd_hist4":    [round(x, 3) for x in list(self.obs_steer_hist)],
            "throttle_cmd_hist4": [round(x, 3) for x in list(self.obs_throttle_hist)],
            "brake_cmd_hist4":    [round(x, 3) for x in list(self.obs_brake_hist)],
            "traffic_light": tl_obs,
            "lead_cars": lead_cars,
            "opposite_cars": opposite_cars,
            "pedestrians": ped_list,
        }

    # ---------------------- main loop ----------------------
    def run_episode(self):
        import time, math, numpy as np, carla
        spec = self.world.get_spectator()

        while True:
            self._tick()
            now = time.time()

            # update behaviors each tick
            self._update_burst_state()       # update burst schedule
            self._update_speed_profiles(now) # apply stoppers/convoy/burst/gate/accordion/jam
            self._update_bottleneck(now)
            self._apply_brake_checks(now)
            self._maybe_trigger_ped_chain(now)

            ego_tf = self.ego.get_transform()
            spec_tf = carla.Transform(
                ego_tf.location + carla.Location(z=30.0) + ego_tf.get_forward_vector() * -12.0,
                carla.Rotation(pitch=-50.0, yaw=ego_tf.rotation.yaw)
            )
            spec.set_transform(spec_tf)

            pos = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
            v = self.ego.get_velocity()
            speed = math.hypot(v.x, v.y)

            if self.policy_fn is None:
                raise RuntimeError("No policy loaded. Call env.load_policy(...) first.")

            obs = self.get_obs(self._last_control)
            steer, throttle, brake = self.policy_fn(obs)
            steer    = float(max(-1.0, min(1.0, steer)))
            throttle = float(max(0.0,  min(1.0, throttle)))
            brake    = float(max(0.0,  min(1.0, brake)))

            control = carla.VehicleControl(steer=steer, throttle=throttle, brake=brake)
            self.ego.apply_control(control)
            self._last_control = control

            # logs
            self.total_steps += 1
            self.positions.append((ego_tf.location.x, ego_tf.location.y))
            self.steerings.append(float(control.steer))
            speed_mps = math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z)
            self.speeds_kmh.append(3.6 * speed_mps)

            # red-light violations (gate crossing while red)
            for idx, (gx, gy) in self.gate_xy.items():
                dist_gate = math.hypot(pos[0] - gx, pos[1] - gy)
                now_in_gate = (dist_gate <= TL_GATE_RADIUS_M)
                if now_in_gate and not self._in_gate_prev[idx] and speed > 0.5:
                    st = tl_state_str(self.world, self.tl_actor_map.get(idx))
                    if st == "Red":
                        self.num_red_violations += 1
                self._in_gate_prev[idx] = now_in_gate

            # pedestrians: periodic retargets; sometimes cluster
            if self.harder_peds and (time.time() - self.last_ped_retarget) >= self._eff["ped_cross_every_s"]:
                self._trigger_pedestrian_crossings(cluster_chance=(0.85 if self.crowded_peds else 0.45), cluster_size_range=((self.crowd_cluster_min, self.crowd_cluster_max) if self.crowded_peds else (15,25)))
                self.last_ped_retarget = time.time()

            # termination checks
            if self.collision_flag["flag"]:
                end_reason = "collision"; break
            if self.total_steps >= self.max_steps:
                end_reason = "timeout"; break

            tf = self.ego.get_transform()
            pos = np.array([tf.location.x, tf.location.y], dtype=np.float32)
            s_now, *_ = project_point_onto_loop(pos, self.A, self.AB, self.seg_len, self.s_before)
            prog_m = wrap_forward_progress(s_now, self.s0, self.L)
            prog_frac = prog_m / max(self.L, 1e-6)
            dist_to_start = float(np.linalg.norm(pos - self.first_pt))
            if (dist_to_start <= self.start_tol_m) and (prog_frac >= self.start_gate_frac):
                end_reason = "loop_complete"; self.done_path = True; break

        pos_arr = np.array(self.positions, dtype=np.float32) if self.positions else np.zeros((0,2), dtype=np.float32)
        fit, metrics = fitness_score(
            self.path_xy, pos_arr, self.steerings, self.speeds_kmh,
            total_steps=self.total_steps,
            done_path=self.done_path,
            collision=self.collision_flag["flag"],
            num_red_violations=self.num_red_violations,
            min_moving_speed=2
        )
        metrics["end_reason"] = end_reason
        metrics["collided_with_ids"] = list(self.collision_ids)
        success = bool(self.done_path and not self.collision_flag["flag"])
        return success, float(fit), metrics

    # ---------------------- utils / plumbing ----------------------
    def _destroy_ids(self, ids):
        if not ids: return
        try:
            batch = [carla.command.DestroyActor(i) for i in set(ids)]
            self.client.apply_batch_sync(batch, True)
        except Exception:
            pass

    def cleanup(self, silent: bool = False):
        """Robust teardown: stop AI, destroy actors, revert to async, tick once, clear refs."""
        try:
            for v in list(getattr(self, "vehicles", [])):
                try:
                    v.set_autopilot(False, self.tm_port_used)
                except:
                    pass
            try:
                if getattr(self, "_bneck_blocker", None) is not None:
                    self._bneck_blocker.destroy()
            except:
                pass
            try:
                if getattr(self, "_occluder", None) is not None:
                    self._occluder.destroy()
            except:
                pass

            try:
                if getattr(self, "col_sensor", None) is not None:
                    try:
                        self.col_sensor.stop()
                    except:
                        pass
                    self._destroy_ids([self.col_sensor.id])
            except:
                pass

            try:
                self._destroy_ids([c.id for c in getattr(self, "walker_ctrls", [])])
            except:
                pass
            try:
                self._destroy_ids([w.id for w in getattr(self, "walkers", [])])
            except:
                pass
            try:
                self._destroy_ids([v.id for v in getattr(self, "vehicles", [])])
            except:
                pass
            try:
                if getattr(self, "ego", None) is not None:
                    self._destroy_ids([self.ego.id])
            except:
                pass

            try:
                if self.world is not None:
                    self._destroy_ids([a.id for a in self.world.get_actors().filter("controller.ai.walker")])
                    self._destroy_ids([a.id for a in self.world.get_actors().filter("walker.pedestrian*")])
                    self._destroy_ids([a.id for a in self.world.get_actors().filter("sensor.*")])
                    self._destroy_ids([a.id for a in self.world.get_actors().filter("vehicle.*")])
            except:
                pass

            try:
                if self.tm is not None:
                    self.tm.set_synchronous_mode(False)
            except:
                pass
            try:
                if self.world is not None:
                    settings = self.world.get_settings()
                    settings.synchronous_mode = False
                    settings.fixed_delta_seconds = None
                    self.world.apply_settings(settings)
                    try:
                        self.world.tick()
                    except:
                        pass
            except:
                pass

            if not silent:
                try:
                    cnt = len(self.world.get_actors()) if self.world else -1
                    print(f"[CLEAN] actors in world after cleanup: {cnt}")
                except:
                    pass

        finally:
            try:
                self.vehicles = []
                self.walkers = []
                self.walker_ctrls = []
                self.ped_pairs = []
            except:
                pass
            self.ego = None
            self.col_sensor = None
            self.tm = None
            self.world = None
            self.tl_actor_map = {}
            self._in_gate_prev = {}

    def _set_sync(self, sync: bool, fps: int):
        settings = self.world.get_settings()
        settings.synchronous_mode = bool(sync)
        settings.fixed_delta_seconds = (1.0 / fps) if sync else None
        self.world.apply_settings(settings)

    def _tick(self):
        import time
        frame = self.world.tick() if self.sync else self.world.wait_for_tick()
        if self.sync and self.pace_real_time and self._last_tick_time is not None:
            target_dt = 1.0 / max(1, self.fps)
            now = time.perf_counter()
            elapsed = now - self._last_tick_time
            sleep_s = target_dt - elapsed
            if sleep_s > 0: time.sleep(sleep_s)
            self._last_tick_time = time.perf_counter()
        return frame

    def _on_collision(self, evt):
        other = evt.other_actor
        self.collision_flag["flag"] = True
        try: self.collision_ids.append(str(other.type_id))
        except Exception: pass

    def _jitter(self):
        import random
        return random.uniform(-self.jitter_ratio, self.jitter_ratio)

    # ---------------------- spawning / config ----------------------
    def _spawn_ego_from_list(self, index: int = 0):
        """Spawn ego strictly as Tesla Model 3 at a predefined spawn point."""
        car_bp = self.bp_lib.find("vehicle.tesla.model3")
        x, y, z = self.spawn_points_xy[index]
        loc = carla.Location(x=x, y=y, z=z)
        spawn_tf = carla.Transform(loc, carla.Rotation(yaw=0.0))
        ego = self.world.try_spawn_actor(car_bp, spawn_tf)
        if not ego:
            raise RuntimeError("Failed to spawn ego (vehicle.tesla.model3) at the requested point.")
        ego.set_autopilot(False)  # ego is policy-controlled
        return ego

    def _add_collision_sensor(self, ego, cb):
        sensor_bp = self.bp_lib.find("sensor.other.collision")
        sensor = self.world.spawn_actor(sensor_bp, carla.Transform(), attach_to=ego)
        sensor.listen(cb)
        return sensor

    def _spawn_traffic(self, count: int):
        import random
        vehicles = []
        spawn_points = self.world.get_map().get_spawn_points()
        random.shuffle(spawn_points)
        for sp in spawn_points[:max(0, count)]:
            bp = random.choice(self.bp_lib.filter("vehicle.*"))
            # avoid oversized vehicles (buses, trucks) per rules
            tid = bp.id
            if any(x in tid for x in ["carlacola","firetruck","ambulance","t2","bus","truck","crossover","european_hgv","mitsubishi.fusorosa","cybertruck","semi","van","trailer"]):
                continue
            actor = self.world.try_spawn_actor(bp, sp)
            if actor:
                actor.set_autopilot(True, self.tm_port_used)
                vehicles.append(actor)
        return vehicles

    def _configure_harder_cars(self, vehicles):
        import random
        tm = self.tm
        tm.set_global_distance_to_leading_vehicle(self._eff["min_follow_dist"])
        tm.global_percentage_speed_difference(self.speed_diff_mean)

        for v in vehicles:
            base = int(self.speed_diff_mean + random.uniform(-5, 5))
            self._veh_baseline_dv[v.id] = base
            tm.vehicle_percentage_speed_difference(v, base)
            tm.auto_lane_change(v, bool(self.lane_change_pct > 0) and (random.random()*100.0 < self.lane_change_pct))
            tm.ignore_lights_percentage(v, self._eff["ignore_light_pct"])

    def _spawn_pedestrians_batch(self, n: int):
        # 1) Get once, modify, and keep the SAME list we will spawn from
        walker_bps = list(self.bp_lib.filter("walker.pedestrian.*"))
        for bp in walker_bps:
            if bp.has_attribute("is_invincible"):
                bp.set_attribute("is_invincible", "false")
            if bp.has_attribute("role_name"):
                bp.set_attribute("role_name", "pedestrian")
        ctrl_bp = self.bp_lib.find("controller.ai.walker")

        # 2) Pick spawn transforms on the navmesh
        tfs = []
        while len(tfs) < n:
            loc = self.world.get_random_location_from_navigation()
            if loc:
                tfs.append(carla.Transform(loc))

        # 3) Spawn walkers
        batch = [carla.command.SpawnActor(random.choice(walker_bps), tf) for tf in tfs]
        res = self.client.apply_batch_sync(batch, True)
        walker_ids = [r.actor_id for r in res if not r.error]

        # 4) IMPORTANT: enable physics on each walker so they collide with vehicles
        for wid in walker_ids:
            w = self.world.get_actor(wid)
            if not w:
                continue
            try:
                w.set_simulate_physics(True)     # <-- makes them solid / ragdoll on impact
                w.enable_gravity(True)           # safe no-op if already True
            except Exception:
                pass

        # 5) Spawn controllers and start them
        batch2 = [carla.command.SpawnActor(ctrl_bp, carla.Transform(), wid) for wid in walker_ids]
        res2 = self.client.apply_batch_sync(batch2, True)
        ctrl_ids = [r.actor_id for r in res2 if not r.error]

        # one tick to finalize spawns before starting controllers
        self._tick()

        speeds = [random.uniform(1.5, 2.2) for _ in ctrl_ids]
        for cid, vmax in zip(ctrl_ids, speeds):
            ctrl = self.world.get_actor(cid)
            if not ctrl:
                continue
            try:
                ctrl.start()
                dest = self.world.get_random_location_from_navigation()
                if dest:
                    ctrl.go_to_location(dest)
                ctrl.set_max_speed(float(vmax))
            except Exception:
                continue

        return walker_ids, ctrl_ids


    def _build_global_plan(self, step: int):
        plan = []
        pts = self.path_xy
        for i in range(0, len(pts), max(1, int(step))):
            x, y = float(pts[i,0]), float(pts[i,1])
            plan.append(carla.Transform(carla.Location(x=x, y=y, z=0.5)))
        return plan

    def _save_plan_to_txt(self, plan, out_path):
        try:
            with open(out_path, "w") as f:
                for tf in plan:
                    f.write(f"{tf.location.x:.3f} {tf.location.y:.3f}\n")
        except Exception:
            pass

    def _kickstart_traffic(self, duration_s=2.0):
        import time
        until = time.time() + float(duration_s)
        for v in self.vehicles:
            self._veh_unstuck_until[v.id] = until
            try:
                self.tm.vehicle_percentage_speed_difference(v, max(-10, self._veh_baseline_dv.get(v.id, 0) - 10))
            except:
                pass

    def _init_vehicle_classes(self):
        import random
        self._veh_class = {}
        for v in self.vehicles:
            is_fast = (random.random() < self.fast_oncoming_fraction)
            self._veh_class[v.id] = {"fast_oncoming": bool(is_fast)}

    def _assign_stoppers(self):
        """Pick stoppers only from vehicles IN EGO LANE and AHEAD, closest first, bias near TL gates."""
        import random, numpy as np

        ego_tf = self.ego.get_transform()
        ego_xy = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        s_ego, *_ = project_point_onto_loop(ego_xy, self.A, self.AB, self.seg_len, self.s_before)

        horizon_m = 120.0
        candidates = []  # (ds_bias, actor)

        for v in self.vehicles:
            if not v or not v.is_alive:
                continue
            loc = v.get_transform().location
            pxy = np.array([loc.x, loc.y], dtype=np.float32)
            s_v, lat_v = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
            ok, ds = in_lane_ahead(s_ego, s_v, lat_v, self.L, horizon_m)
            if ok:
                ds_bias = ds
                try:
                    for _idx, s_gate in self.s_light.items():
                        dsv = (float(s_gate) - float(s_v)) % self.L
                        if 0.0 < dsv <= 35.0:
                            ds_bias = ds - 10.0
                            break
                except Exception:
                    pass
                candidates.append((ds_bias, v))

        candidates.sort(key=lambda t: t[0])
        k = max(0, int(round(self.stopper_fraction * len(self.vehicles))))
        chosen = [v for _, v in candidates[:k]]

        self._stoppers = {}
        import random as _r
        for v in chosen:
            pause_steps = _r.randint(self.stopper_min_steps, self.stopper_max_steps)
            self._stoppers[v.id] = {"remaining": pause_steps, "active": False, "armed": True}

        print(f"[STOPPERS] assigned={len(self._stoppers)} candidates_in_lane_ahead={len(candidates)} horizon_m={horizon_m}")

    # ---------------------- NEW: rolling convoy / bursts / gate pause / accordion ----------------------
    def _assign_rolling_convoy(self):
        """Guarantee 1–2 vehicles ahead in ego lane roll notably slower for a limited window."""
        if not self.convoy_enable or not self.vehicles:
            return
        import numpy as np, random as _r

        ego_tf = self.ego.get_transform()
        ego_xy = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        s_ego, *_ = project_point_onto_loop(ego_xy, self.A, self.AB, self.seg_len, self.s_before)

        # Collect in-lane ahead candidates in [min_gap, max_gap]
        cand = []
        for v in self.vehicles:
            if not v or not v.is_alive: continue
            loc = v.get_transform().location
            pxy = np.array([loc.x, loc.y], dtype=np.float32)
            s_v, lat_v = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
            ok, ds = in_lane_ahead(s_ego, s_v, lat_v, self.L, self.convoy_max_gap_m)
            if ok and (self.convoy_min_gap_m <= ds <= self.convoy_max_gap_m):
                cand.append((ds, v))
        cand.sort(key=lambda t: t[0])
        if not cand:
            return
        size = max(1, min(self.convoy_size, len(cand)))
        chosen = [v for _, v in cand[:size]]

        self._convoy_members = set(v.id for v in chosen)
        import random as _r
        dur_s = _r.uniform(*self.convoy_window_s)
        self._convoy_end_step = self.total_steps + int(round(self.fps * dur_s))

    def _init_burst_schedule(self):
        """Initialize oncoming burst timings."""
        if not self.bursts_enable:
            self._burst_active = False
            self._burst_end_step = -1
            self._burst_next_start_step = -1
            return
        import random as _r
        first_delay = _r.uniform(*self.burst_first_delay_s)
        self._burst_next_start_step = self.total_steps + int(round(self.fps * first_delay))
        self._burst_active = False
        self._burst_end_step = -1

    def _update_burst_state(self):
        """Advance burst ON/OFF windows based on step counters."""
        if not self.bursts_enable:
            return
        import random as _r
        step = self.total_steps
        if self._burst_active:
            if step >= self._burst_end_step:
                # schedule cooldown
                cool = _r.uniform(*self.burst_cooldown_s)
                self._burst_next_start_step = step + int(round(self.fps * cool))
                self._burst_active = False
        else:
            if self._burst_next_start_step >= 0 and step >= self._burst_next_start_step:
                dur = _r.uniform(*self.burst_duration_s)
                self._burst_end_step = step + int(round(self.fps * dur))
                self._burst_active = True

    def _assign_gate_pausers(self):
        """Pick a couple of in-lane vehicles approaching known TL gates to hesitate briefly before the stop line."""
        if not self.gate_pause_enable or not self.vehicles:
            return
        import numpy as np, random as _r

        ego_tf = self.ego.get_transform()
        ego_xy = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        s_ego, *_ = project_point_onto_loop(ego_xy, self.A, self.AB, self.seg_len, self.s_before)

        # choose gates to consider (all)
        gate_s = list(self.s_light.values())
        cand = []  # (distance to nearest gate along s, veh)
        for v in self.vehicles:
            if not v or not v.is_alive: continue
            loc = v.get_transform().location
            pxy = np.array([loc.x, loc.y], dtype=np.float32)
            s_v, lat_v = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
            ok, ds = in_lane_ahead(s_ego, s_v, lat_v, self.L, 200.0)
            if not ok: continue
            # distance v->nearest gate ahead
            dmin = 1e9
            for sg in gate_s:
                dsv = (float(sg) - float(s_v)) % self.L
                if 0.0 < dsv < dmin:
                    dmin = dsv
            if dmin < 60.0:  # reasonably close to some gate
                cand.append((dmin, v))
        cand.sort(key=lambda t: t[0])
        take = min(self.gate_pauser_count, len(cand))
        self._gate_pausers = {}
        for i in range(take):
            vid = cand[i][1].id
            import random as _r
            steps = _r.randint(*self.gate_pause_steps)
            self._gate_pausers[vid] = steps  # remaining

    def _init_accordion_leads(self):
        """Assign gentle sinusoidal speed modulations to a small fraction of in-lane vehicles (not stoppers/convoy)."""
        if not self.accordion_enable or not self.vehicles:
            return
        import numpy as np, random as _r, math
        ego_tf = self.ego.get_transform()
        ego_xy = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        s_ego, *_ = project_point_onto_loop(ego_xy, self.A, self.AB, self.seg_len, self.s_before)

        pool = []
        for v in self.vehicles:
            if not v or not v.is_alive: continue
            if v.id in self._convoy_members: continue
            if v.id in self._stoppers: continue
            loc = v.get_transform().location
            pxy = np.array([loc.x, loc.y], dtype=np.float32)
            s_v, lat_v = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
            ok, ds = in_lane_ahead(s_ego, s_v, lat_v, self.L, 200.0)
            if ok:
                pool.append((ds, v))
        pool.sort(key=lambda t: t[0])
        import random as _r
        for _, v in pool:
            if _r.random() < self.accordion_fraction:
                period = int(round(self.fps * _r.uniform(*self.accordion_period_s)))
                phase  = _r.uniform(0.0, 2.0*math.pi)
                self._accordion[v.id] = {"amp": float(self.accordion_amp_pct),
                                         "period": max(10, period),
                                         "phase": float(phase)}

    # ---------------------- dynamic behaviors (per-tick) ----------------------
    def _update_speed_profiles(self, now_time):
        """
        Per-tick updates (layered, priority-aware):
          - Spawn kick restoration
          - Oncoming: fast & burst speed-ups (negative dv%)
          - Timed stoppers: single pause, then resume (armed within ~40 m)
          - Gate pausers: brief hesitation right before stop line
          - Rolling convoy: rolling slow window (not a stop)
          - Accordion: gentle baseline oscillation for some leads
          - Jam watchdog: unsticks long-stalled NPCs (negative dv%)
          - Otherwise: restore baseline dv%
        """
        import math, numpy as np

        dt_fixed = self.world.get_settings().fixed_delta_seconds or 0.05

        # Ego pose
        ego_tf = self.ego.get_transform()
        ego_pos = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        ego_vel = self.ego.get_velocity()
        ego_v = np.array([ego_vel.x, ego_vel.y], dtype=np.float32)
        yaw = math.radians(ego_tf.rotation.yaw)
        R_we = rot_world_to_ego(yaw)
        s_ego, *_ = project_point_onto_loop(ego_pos, self.A, self.AB, self.seg_len, self.s_before)

        step = self.total_steps

        for v in list(self.vehicles):
            if (v is None) or (not v.is_alive):
                continue

            vid = v.id

            # Restore baseline after spawn kick window
            kick_until = self._veh_unstuck_until.get(vid, 0.0)
            if now_time > kick_until:
                base = self._veh_baseline_dv.get(vid, 0)
                try:
                    self.tm.vehicle_percentage_speed_difference(v, base)
                except:
                    pass
                self._veh_unstuck_until[vid] = 0.0

            # Vehicle kinematics
            loc = v.get_transform().location
            pos = np.array([loc.x, loc.y], dtype=np.float32)
            s_v, lat_v = frenet_s_lat(pos, self.A, self.AB, self.seg_len, self.s_before)
            ds = (s_v - s_ego) % self.L
            vel = v.get_velocity()
            vv = np.array([vel.x, vel.y], dtype=np.float32)
            p_rel_e = R_we @ (pos - ego_pos)
            v_rel_e = R_we @ (vv - ego_v)

            base_dv = self._veh_baseline_dv.get(vid, 0)
            desired_dv = base_dv
            applied = False

            # 1) Oncoming speed-ups: fast flag & bursts (apply only in opposite corridor and within view window)
            if (OPP_LANE_LEFT <= lat_v < OPP_LANE_RIGHT) and (0.0 < ds <= self.opp_view_ds_max):
                if self._veh_class.get(vid, {}).get("fast_oncoming", False):
                    desired_dv = min(desired_dv, self.fast_oncoming_dv_pct)
                    applied = True
                if self._burst_active:
                    desired_dv = min(desired_dv, self.burst_fast_dv_pct)
                    applied = True

            # 2) Timed stoppers: engage once when in-lane and close ahead (wider arming ~40 m)
            st = self._stoppers.get(vid)
            if st and st.get("armed", False):
                if -LANE_HALF <= lat_v <= LANE_HALF and 0.0 < ds <= 40.0:
                    st["active"] = True
                    st["armed"] = False
            if st and st.get("active", False):
                if st["remaining"] > 0:
                    desired_dv = max(desired_dv, max(60, base_dv + 60))  # strong slow, but not infinite
                    st["remaining"] -= 1
                    applied = True
                else:
                    # resume baseline
                    st["active"] = False

            # 3) Gate pausers: brief hesitation before stop line (trigger within along-s distance to nearest gate)
            if self.gate_pause_enable and (vid in self._gate_pausers) and (self._gate_pausers[vid] > 0):
                # compute distance to nearest gate ahead
                dmin = 1e9
                for sg in self.s_light.values():
                    dsv = (float(sg) - float(s_v)) % self.L
                    if 0.0 < dsv < dmin:
                        dmin = dsv
                if dmin <= self.gate_pause_trigger_ds_m:
                    desired_dv = max(desired_dv, max(45, base_dv + 45))  # brief hesitation
                    self._gate_pausers[vid] -= 1
                    applied = True

            # 4) Rolling convoy: rolling slow (not stop), windowed by steps
            if self.convoy_enable and (vid in self._convoy_members) and (step <= self._convoy_end_step):
                # apply only if in ego lane corridor (lead behavior)
                if -LANE_HALF <= lat_v <= LANE_HALF:
                    desired_dv = max(desired_dv, base_dv + int(self.convoy_slow_pct))
                    desired_dv = min(desired_dv, 58)  # cap to avoid pathological TM behavior
                    applied = True

            # 5) Accordion: gentle oscillation around baseline (skip if any higher-priority slow/stop already applied)
            if self.accordion_enable and (vid in self._accordion) and not (st and st.get("active", False)) and not (vid in self._convoy_members and step <= self._convoy_end_step):
                acc = self._accordion[vid]
                import math as _m
                osc = acc["amp"] * _m.sin((2.0*_m.pi/acc["period"]) * (step + acc["phase"]))
                desired_dv = int(round(desired_dv + osc))
                desired_dv = max(min(desired_dv, 30), -10)  # keep modest
                applied = True or applied

            # 6) Jam watchdog: if stalled too long, apply temporary boost (negative dv) unless stopper/gate active
            speed_v = float(np.linalg.norm(vv))
            if speed_v < self.jam_speed_thresh:
                self._veh_stuck_count[vid] = self._veh_stuck_count.get(vid, 0) + 1
            else:
                self._veh_stuck_count[vid] = 0

            if self._veh_stuck_count.get(vid, 0) >= self.jam_stuck_steps:
                boost_until_step = self.total_steps + self.jam_boost_hold_steps
                self._veh_jam_boost_until[vid] = boost_until_step
                self._veh_stuck_count[vid] = 0

            if self._veh_jam_boost_until.get(vid, -1) >= self.total_steps:
                # only boost if not explicitly slowed by stopper/gate
                if not (st and st.get("active", False)) and not (vid in self._gate_pausers and self._gate_pausers[vid] > 0):
                    desired_dv = min(desired_dv, self.jam_boost_pct)
                    applied = True

            # 7) Apply desired dv% (single write)
            try:
                self.tm.vehicle_percentage_speed_difference(v, int(desired_dv))
            except:
                pass

    # ---------------------- pedestrian crossing trigger (kept) ----------------------

    # ---------- NEW HELPERS: geometry ----------
    def _xy_yaw_at_s(self, s_val):
        """Interpolate world (x,y) and yaw (rad) at arc-length s along the loop."""
        import numpy as np, math
        s = float(s_val) % max(self.L, 1e-6)
        # find segment k
        idx = int(np.searchsorted(self.s_before, s, side='right') - 1)
        idx = max(0, min(idx, len(self.A)-1))
        s0 = self.s_before[idx]
        seg = self.AB[idx]
        Ls = float(np.linalg.norm(seg))
        t = 0.0 if Ls < 1e-6 else (s - s0) / Ls
        t = max(0.0, min(1.0, t))
        xy = self.A[idx] + t * seg
        yaw = math.atan2(seg[1], seg[0]) if Ls >= 1e-6 else 0.0
        return float(xy[0]), float(xy[1]), float(yaw)

    # ---------- NEW: bottleneck encroacher ----------
    def _spawn_bottleneck_blocker(self, center_s):
        import carla, math
        if self.world is None or self.bp_lib is None:
            return
        try:
            x, y, yaw = self._xy_yaw_at_s(center_s)
            # lateral shift into ego lane (~0.5 m)
            nx, ny = -math.sin(yaw), math.cos(yaw)  # left normal
            # park slightly to left of centerline (into ego lane)
            lat = self.bottleneck_width_m * 0.5 - 0.5
            px, py = x - nx * lat, y - ny * lat
            loc = carla.Location(x=px, y=py, z=0.5)
            rot = carla.Rotation(yaw=math.degrees(yaw))
            tf  = carla.Transform(loc, rot)
            # choose a compact sedan
            cand = [bp for bp in self.bp_lib.filter("vehicle.*") if "tesla.model3" in bp.id or "audi.tt" in bp.id or "mini.cooper" in bp.id]
            bp = cand[0] if cand else self.bp_lib.filter("vehicle.tesla.model3")[0]
            actor = self.world.try_spawn_actor(bp, tf)
            if actor:
                actor.set_autopilot(False)
                actor.set_light_state(carla.VehicleLightState(carla.VehicleLightState.Position))
                actor.apply_control(carla.VehicleControl(hand_brake=True, throttle=0.0, brake=1.0))
                self._bneck_blocker = actor
                self._bneck_active = True
        except Exception:
            self._bneck_blocker = None
            self._bneck_active = False

    def _update_bottleneck(self, now_time):
        # destroy encroacher after window
        if not self._bneck_active:
            return
        if self.total_steps >= self._bneck_end_step:
            try:
                if self._bneck_blocker is not None:
                    self._bneck_blocker.destroy()
            except Exception:
                pass
            self._bneck_blocker = None
            self._bneck_active = False

    # ---------- NEW: brake-checkers ----------
    def _assign_brake_checkers(self, vehicles):
        import random, numpy as np
        # pick a small subset in ego lane and ahead
        ego_tf = self.ego.get_transform()
        ego_xy = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        s_ego, *_ = project_point_onto_loop(ego_xy, self.A, self.AB, self.seg_len, self.s_before)
        picks = []
        for v in vehicles:
            if not v or not v.is_alive: continue
            loc = v.get_transform().location
            pxy = np.array([loc.x, loc.y], dtype=np.float32)
            s_v, lat_v = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
            ok, ds = in_lane_ahead(s_ego, s_v, lat_v, self.L, 80.0)
            if ok and ds >= 12.0:  # at least 12 m ahead
                picks.append(v)
        random.shuffle(picks)
        k = max(0, int(round(self.brake_check_fraction * len(picks))))
        for v in picks[:k]:
            self._brake_checkers[v.id] = {"next_step": self.total_steps + int(round(self.fps * random.uniform(2.0, 6.0))),
                                          "hold_until": -1,
                                          "cool_until": -1,
                                          "active": False}

    def _apply_brake_checks(self, now_time):
        # override dv% AFTER baseline updates
        import random, numpy as np
        step = self.total_steps
        ego_tf = self.ego.get_transform()
        ego_xy = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        s_ego, *_ = project_point_onto_loop(ego_xy, self.A, self.AB, self.seg_len, self.s_before)
        for v in list(self.vehicles):
            st = self._brake_checkers.get(v.id)
            if st is None: 
                continue
            # cooldown check
            if st["cool_until"] > step and not st["active"]:
                continue
            # start if time and in useful range
            if (not st["active"]) and step >= st["next_step"]:
                # ensure it's still ahead and within 15..45 m
                loc = v.get_transform().location
                pxy = np.array([loc.x, loc.y], dtype=np.float32)
                s_v, lat_v = frenet_s_lat(pxy, self.A, self.AB, self.seg_len, self.s_before)
                ok, ds = in_lane_ahead(s_ego, s_v, lat_v, self.L, 60.0)
                if ok and 15.0 <= ds <= 45.0:
                    hold = random.uniform(*self.brake_check_hold_s)
                    st["hold_until"] = step + int(round(self.fps * hold))
                    st["active"] = True
            # apply or finish
            if st["active"]:
                if step <= st["hold_until"]:
                    base = self._veh_baseline_dv.get(v.id, 0)
                    dv = int(base + self.brake_check_add_dv_pct)
                    try:
                        self.tm.vehicle_percentage_speed_difference(v, dv)
                    except:
                        pass
                else:
                    st["active"] = False
                    cool = random.uniform(*self.brake_check_cooldown_s)
                    st["cool_until"] = step + int(round(self.fps * cool))
                    st["next_step"] = st["cool_until"] + int(round(self.fps * random.uniform(2.0, 5.0)))

    # ---------- NEW: pedestrian chain scheduler ----------
    def _maybe_trigger_ped_chain(self, now_time):
        if self._chain_plan.get("remaining", 0) <= 0:
            return
        if self.total_steps >= self._chain_plan.get("next_step", -1):
            # small burst triggers few walkers with small stagger
            try:
                self._trigger_pedestrian_crossings(
                    cluster_chance=1.0,
                    cluster_size_range=(2, 4)
                )
            except Exception:
                pass
            # schedule next or finish
            import random as _r
            self._chain_plan["remaining"] -= 1
            if self._chain_plan["remaining"] > 0:
                gap = _r.uniform(self.cross_chain_gap_s[0], self.cross_chain_gap_s[1])
                self._chain_plan["next_step"] = self.total_steps + int(round(self.fps * gap))
            else:
                self._chain_plan["next_step"] = -1

    # ---------- NEW: occluder near gate ----------
    def _spawn_occluder_near_gate(self):
        if not self.occluder_enable or not self.gate_xy:
            return
        try:
            import carla, random as _r, math
            # pick a random gate
            idx = list(self.gate_xy.keys())[0]
            gx, gy = self.gate_xy[idx]
            # place a tall parked vehicle near the curb ~5 m before gate
            gate_xy = (gx, gy)
            # get local path orientation at gate s
            s_gate = self.s_light.get(idx, None)
            if s_gate is None:
                return
            x, y, yaw = self._xy_yaw_at_s(s_gate - 5.0)
            nx, ny = -math.sin(yaw), math.cos(yaw)
            px, py = x + nx * (LANE_HALF + 0.8), y + ny * (LANE_HALF + 0.8)
            loc = carla.Location(x=float(px), y=float(py), z=0.5)
            rot = carla.Rotation(yaw=math.degrees(yaw))
            tf  = carla.Transform(loc, rot)
            # choose a van/pickup if available
            cands = [bp for bp in self.bp_lib.filter("vehicle.*") if ("van" in bp.id or "pickup" in bp.id)]
            bp = cands[0] if cands else self.bp_lib.filter("vehicle.audi.tt")[0]
            actor = self.world.try_spawn_actor(bp, tf)
            if actor:
                actor.set_autopilot(False)
                actor.apply_control(carla.VehicleControl(hand_brake=True, throttle=0.0, brake=1.0))
                self._occluder = actor
        except Exception:
            self._occluder = None


    def _trigger_pedestrian_crossings(self, cluster_chance=0.45, cluster_size_range=(15, 25)):
        import random, time, math, numpy as np, carla
        nowt = time.time()
        ego_tf = self.ego.get_transform()
        pos = np.array([ego_tf.location.x, ego_tf.location.y], dtype=np.float32)
        yaw_rad = math.radians(ego_tf.rotation.yaw)
        R_we = rot_world_to_ego(yaw_rad)
        v = self.ego.get_velocity()
        ego_speed = math.hypot(v.x, v.y)

        do_cluster = (random.random() < cluster_chance)

        ready_walkers = []
        for walker, ctrl in self.ped_pairs:
            try:
                if not walker or not walker.is_alive:  continue
                if not ctrl   or not ctrl.is_alive:    continue

                wloc = walker.get_location()
                pxy  = np.array([wloc.x, wloc.y], dtype=np.float32)
                s_w, lat_w, k, t, proj_xy = project_point_onto_loop(
                    pxy, self.A, self.AB, self.seg_len, self.s_before
                )

                # TTL push-off if lingering in lane
                if abs(lat_w) <= LANE_HALF:
                    enter_at = self._ped_lane_enter_at.get(walker.id)
                    if enter_at is None:
                        self._ped_lane_enter_at[walker.id] = nowt
                    else:
                        if (nowt - enter_at) >= self.ped_lane_ttl_s:
                            seg = self.AB[k]; Ls = float(np.linalg.norm(seg))
                            if Ls > 1e-6:
                                n = np.array([-seg[1]/Ls, seg[0]/Ls], dtype=np.float32)
                                sign_dir = 1.0 if lat_w >= 0.0 else -1.0
                                target = proj_xy + sign_dir * n * self.ped_offlane_offset_m
                                ctrl.go_to_location(carla.Location(x=float(target[0]), y=float(target[1]), z=wloc.z))
                                ctrl.set_max_speed(random.uniform(1.2, 2.0))
                                cd = random.uniform(self.ped_cooldown_min_s, self.ped_cooldown_max_s)
                                self._ped_ready_at[walker.id] = nowt + cd
                                self._ped_lane_enter_at[walker.id] = None
                            continue
                else:
                    if walker.id in self._ped_lane_enter_at:
                        self._ped_lane_enter_at[walker.id] = None

                # side-hit guard (don’t pull into nearly-stopped ego)
                p_rel_e = R_we @ (pxy - pos)
                if ego_speed <= 1 and abs(p_rel_e[1]) <= 1.5:
                    continue

                ready_at = self._ped_ready_at.get(walker.id, 0.0)
                if nowt >= ready_at:
                    ready_walkers.append((walker, ctrl, wloc))
            except Exception:
                continue

        random.shuffle(ready_walkers)
        if not ready_walkers:
            return

        wanted = 1
        if do_cluster and len(ready_walkers) >= 2 and (random.random() < min(0.9, self._eff["ped_cross_prob"])):
            lo, hi = cluster_size_range
            wanted = max(2, min(hi, len(ready_walkers)))

        selected = ready_walkers[:wanted]
        for (walker, ctrl, wloc) in selected:
            try:
                p = min(0.9, self._eff["ped_cross_prob"])
                if random.random() < p:
                    tx, ty = ped_cross_target_for(wloc, self.path_xy, self.ped_cross_offset_m)
                    ctrl.go_to_location(carla.Location(x=tx, y=ty, z=wloc.z))
                    ctrl.set_max_speed(random.uniform(1.2, 2.0))
                    cd = random.uniform(self.ped_cooldown_min_s, self.ped_cooldown_max_s)
                    self._ped_ready_at[walker.id] = time.time() + cd
            except Exception:
                continue

    # ---------------------- Task description (required) ----------------------
    def task_description(self) -> str:
        """
        High-level summary of current difficulty. No exact percentages.
        """
        return (
            "Town01 single-carriageway loop with two opposing lanes. "
            "Ego (Tesla Model 3) must complete one loop without collisions, obeying traffic lights (Yellow treated as Red). "
            "Traffic density and pedestrian activity are moderate-to-high. Small groups of pedestrians may cross. "
            "In-lane NPCs include occasional timed stoppers that pause once and then resume. "
            "A short rolling-slow convoy typically appears ahead early in the episode, requiring a well-timed overtake. "
            "Oncoming vehicles arrive in brief bursts (tighter temporal gaps) followed by clearer windows. "
            "Near some traffic lights, a vehicle may briefly hesitate just before the stop line, compressing queues. "
            "Some leads exhibit gentle speed oscillations (accordion effect). "
            "Observations are unchanged: ego kinematics history, nearest lead/opposite vehicles within fixed horizons, "
            "traffic-light snapshot, and pedestrians with approach timing when applicable."
        )
