"""傍晚小雨十字路口：西左转让行东直行，南侧施工区阻挡，信号灯相位切换。

地图与路口来自 find_scene_location 实采目录：
Town04_Opt junction_id=148（cross，signal_control=true）。
四个 movement 的 entry/exit 原样取自同一候选的 movement_matches，s 保留完整精度。
世界坐标约定：+X 为东，+Y 为南，-Y 为北。
"""

import logging

DURATION_SECONDS = 60

SCENE_MAP_NAME = "Town04_Opt"
JUNCTION_ID = 148

# 西侧来车左转驶向北侧。
WEST_LEFT = {
    "map": "Town04_Opt",
    "junction_id": 148,
    "location_id": "Town04_Opt:junction:148:movement:8",
    "entry": {
        "road_id": 225,
        "section_id": 0,
        "lane_id": 1,
        "s": 19.5983286256915,
        "x": 191.846,
        "y": -307.678,
        "z": 0.034,
        "yaw": 0.143,
    },
    "exit": {
        "road_id": 225,
        "section_id": 0,
        "lane_id": 1,
        "s": 2.220446049250313e-14,
        "x": 205.548,
        "y": -320.344,
        "z": 0.02,
        "yaw": 271.314,
    },
}

# 东侧来车直行驶向西侧。
EAST_STRAIGHT = {
    "map": "Town04_Opt",
    "junction_id": 148,
    "location_id": "Town04_Opt:junction:148:movement:4",
    "entry": {
        "road_id": 179,
        "section_id": 0,
        "lane_id": 1,
        "s": 21.512363727825573,
        "x": 213.378,
        "y": -311.359,
        "z": 0.004,
        "yaw": -179.41,
    },
    "exit": {
        "road_id": 179,
        "section_id": 0,
        "lane_id": 1,
        "s": 2.220446049250313e-14,
        "x": 191.854,
        "y": -311.178,
        "z": 0.034,
        "yaw": -179.857,
    },
}

# 南侧来车直行驶向北侧（施工区所在车道）。
SOUTH_STRAIGHT = {
    "map": "Town04_Opt",
    "junction_id": 148,
    "location_id": "Town04_Opt:junction:148:movement:6",
    "entry": {
        "road_id": 209,
        "section_id": 0,
        "lane_id": 1,
        "s": 21.959999999999972,
        "x": 205.045,
        "y": -298.39,
        "z": 0.02,
        "yaw": 271.314,
    },
    "exit": {
        "road_id": 209,
        "section_id": 0,
        "lane_id": 1,
        "s": 2.220446049250313e-14,
        "x": 205.548,
        "y": -320.344,
        "z": 0.02,
        "yaw": 271.314,
    },
}

# 北侧来车直行驶向南侧。
NORTH_STRAIGHT = {
    "map": "Town04_Opt",
    "junction_id": 148,
    "location_id": "Town04_Opt:junction:148:movement:7",
    "entry": {
        "road_id": 210,
        "section_id": 0,
        "lane_id": -1,
        "s": 2.220446049250313e-14,
        "x": 202.049,
        "y": -320.424,
        "z": 0.02,
        "yaw": 91.314,
    },
    "exit": {
        "road_id": 210,
        "section_id": 0,
        "lane_id": -1,
        "s": 21.959999999999972,
        "x": 201.546,
        "y": -298.47,
        "z": 0.02,
        "yaw": 91.314,
    },
}

# 布局与行为参数。
ROUTE_BEFORE_M = 45.0
ROUTE_AFTER_M = 30.0
APPROACH_KMH = 28.0
CREEP_KMH = 7.0
YIELD_DECEL = 2.2
YIELD_TRIGGER_M = 20.0
STOP_BEFORE_CONFLICT_M = 7.0
CONFLICT_CLEAR_MARGIN_M = 4.0
WORK_ZONE_BEFORE_STOP_M = 10.0
SOUTH_STOP_GAP_M = 5.0
SOUTH_WAIT_TOLERANCE_M = 1.0
GRACE_SECONDS = 2.0
FAULT_SECONDS = 2.0
YELLOW_SECONDS = 3.0
ALL_RED_SECONDS = 2.0
EW_PASS_SAFETY_M = 6.0
MIN_SYNC_GAP_SECONDS = 1.0
MAX_SYNC_GAP_SECONDS = 15.0
OPPOSING_LANE_MIN_GAP_M = 2.0
VIEW_MARGIN_M = 4.0
VIEW_MIN_ACTOR_PIXELS = 26.0
INSPECT_INTERVAL = 1.0
SOUTH_MAX_DRIFT_M = 0.5
SOUTH_MAX_SPEED_MPS = 0.3
SUMMARY_LEAD_SECONDS = 1.0

# 四辆车使用同一车型、不同颜色，便于俯视区分。
VEHICLE_ID = "vehicle.audi.a2"
COLORS = {
    "west_left": "255,255,255",
    "east_straight": "0,0,0",
    "south_wait": "224,0,0",
    "north_straight": "134,134,134",
}
LABELS = {
    "west_left": "TURN_W",
    "east_straight": "GO_E",
    "south_wait": "WAIT_S",
    "north_straight": "GO_N",
}
ROLE_ORDER = ("west_left", "east_straight", "south_wait", "north_straight")

CONE_ID = "static.prop.constructioncone"
BARRIER_ID = "static.prop.streetbarrier"
# 施工区横向布置：(沿行驶方向偏移, 横向偏移, 是否路障)。
WORK_ZONE_LAYOUT = (
    (1.9, 0.0, True),
    (0.0, -0.95, False),
    (0.0, 0.0, False),
    (0.0, 0.95, False),
    (-1.9, -0.95, False),
    (-1.9, 0.95, False),
)


def _speed_mps(vehicle):
    velocity = vehicle.get_velocity()
    return math.sqrt(velocity.x * velocity.x + velocity.y * velocity.y + velocity.z * velocity.z)


def _make_vehicle_blueprint(ctx, role, color):
    blueprint = ctx.blueprints.find(VEHICLE_ID)
    if blueprint.has_attribute("role_name") and blueprint.get_attribute("role_name").is_modifiable:
        blueprint.set_attribute("role_name", role)
    if blueprint.has_attribute("color") and blueprint.get_attribute("color").is_modifiable:
        blueprint.set_attribute("color", color)
    return blueprint


def _make_prop_blueprint(ctx, prop_id, role):
    blueprint = ctx.blueprints.find(prop_id)
    if blueprint.has_attribute("role_name") and blueprint.get_attribute("role_name").is_modifiable:
        blueprint.set_attribute("role_name", role)
    return blueprint


def _apply_lights(ctx, lights, state, tag):
    ctx.set_traffic_lights(lights, state, freeze=True)
    logging.info("[lights] %s -> %s (%d lamps)", tag, state, len(lights))


def _build_route(ctx, record, role):
    route = ctx.route_from_location(
        record,
        before_m=ROUTE_BEFORE_M,
        after_m=ROUTE_AFTER_M,
        step_m=1.0,
        allow_shorter=True,
    )
    info = route["extension_info"]
    logging.info(
        "[route:%s] length=%.2fm entry_s=%.2f exit_s=%.2f before=%.2fm(%s) after=%.2fm(%s)",
        role,
        route["length_m"],
        route["entry_s"],
        route["exit_s"],
        info["before"]["actual_m"],
        info["before"]["reason"],
        info["after"]["actual_m"],
        info["after"]["reason"],
    )
    return route


def _pick_traffic_lights(ctx, route, role):
    result = ctx.traffic_lights_for_route(route, max_distance_m=60.0)
    if result is None:
        raise RuntimeError("traffic_lights_for_route returned None for %s" % role)
    status = result.get("status")
    if status is not None and status != "matched":
        raise RuntimeError(
            "no unambiguous traffic light stop line for %s: status=%s detail=%s"
            % (role, status, result.get("message") or result.get("reason"))
        )
    lights = result.get("traffic_lights") or []
    if not lights:
        raise RuntimeError("traffic light lookup for %s matched no lamp actor" % role)
    stop_s = result.get("stop_s")
    if stop_s is None:
        raise RuntimeError("traffic light lookup for %s returned no stop_s" % role)
    logging.info(
        "[lights:%s] stop_s=%.2f entry_s=%.2f lamps=%s",
        role,
        stop_s,
        route["entry_s"],
        [lamp.id for lamp in lights],
    )
    return lights, stop_s


def _spawn_moving_vehicle(ctx, route, role):
    blueprint = _make_vehicle_blueprint(ctx, role, COLORS[role])
    vehicle = ctx.spawn_vehicle(blueprint, route, role=role, candidate_indexes=(0, 2, 5, 10))
    if vehicle is None:
        raise RuntimeError("spawn_vehicle failed for %s on its approach route" % role)
    return vehicle


def _on_collision(event, role, state):
    other = event.other_actor
    other_id = other.id
    entry = {"role": role, "other_id": other_id, "other_type": other.type_id}
    state["collisions"].append(entry)
    if other_id in state["obstacle_ids"]:
        state["obstacle_hits"].append(entry)
        logging.error(
            "[collision] %s hit work-zone obstacle id=%d type=%s", role, other_id, other.type_id
        )
    else:
        logging.error("[collision] %s hit id=%d type=%s", role, other_id, other.type_id)


def _spawn_collision_sensor(ctx, vehicle, role, state):
    blueprint = ctx.blueprints.find("sensor.other.collision")
    if blueprint.has_attribute("role_name") and blueprint.get_attribute("role_name").is_modifiable:
        blueprint.set_attribute("role_name", "collision_" + role)
    sensor = ctx.spawn_actor(
        blueprint,
        carla.Transform(carla.Location(x=0.0, y=0.0, z=1.2)),
        attach_to=vehicle,
        attachment_type=carla.AttachmentType.Rigid,
    )
    if sensor is None:
        raise RuntimeError("collision sensor spawn failed for %s" % role)
    sensor.listen(lambda event, key=role: _on_collision(event, key, state))
    return sensor


def _verify_work_zone_lane(ctx, transform, role, index):
    waypoint = ctx.map.get_waypoint(
        transform.location,
        project_to_road=True,
        lane_type=carla.LaneType.Driving,
    )
    if waypoint is None:
        raise RuntimeError("work zone prop %d for %s is not on any Driving lane" % (index, role))
    logging.info(
        "[workzone:%s] prop %d at (%.2f, %.2f) -> road=%d section=%d lane=%d s=%.2f",
        role,
        index,
        transform.location.x,
        transform.location.y,
        waypoint.road_id,
        waypoint.section_id,
        waypoint.lane_id,
        waypoint.s,
    )
    return waypoint


def _build_work_zone(ctx, route, stop_s, role):
    zone_s = stop_s - WORK_ZONE_BEFORE_STOP_M
    if zone_s < 4.0:
        raise RuntimeError(
            "not enough approach lane upstream of the stop line for %s: stop_s=%.2f leaves zone_s=%.2f"
            % (role, stop_s, zone_s)
        )
    pose = ctx.route_pose(route, zone_s)
    forward = pose.get_forward_vector()
    right = pose.get_right_vector()
    cone_blueprint = _make_prop_blueprint(ctx, CONE_ID, "workzone_cone")
    barrier_blueprint = _make_prop_blueprint(ctx, BARRIER_ID, "workzone_barrier")
    props = []
    for index, item in enumerate(WORK_ZONE_LAYOUT):
        along = item[0]
        lateral = item[1]
        is_barrier = item[2]
        location = carla.Location(
            x=pose.location.x + forward.x * along + right.x * lateral,
            y=pose.location.y + forward.y * along + right.y * lateral,
            z=pose.location.z + 0.3,
        )
        transform = carla.Transform(
            location,
            carla.Rotation(pitch=0.0, yaw=pose.rotation.yaw, roll=0.0),
        )
        _verify_work_zone_lane(ctx, transform, role, index)
        blueprint = barrier_blueprint if is_barrier else cone_blueprint
        prop = ctx.spawn_actor(blueprint, transform)
        if prop is None:
            raise RuntimeError("failed to spawn work zone prop %d for %s" % (index, role))
        ctx.register_actor(prop)
        props.append(prop)
    logging.info(
        "[workzone:%s] %d props centered at route s=%.2f (stop line s=%.2f, %.1fm upstream)",
        role,
        len(props),
        zone_s,
        stop_s,
        WORK_ZONE_BEFORE_STOP_M,
    )
    return props, zone_s


def setup_scene(ctx):
    state = ctx.state
    state["phase"] = "ew_green"
    state["phase_t"] = 0.0
    state["ready"] = False
    state["last_inspect"] = -10.0
    state["grace_until"] = GRACE_SECONDS
    state["fault_since"] = None
    state["collisions"] = []
    state["obstacle_hits"] = []
    state["ew_passed"] = False
    state["ew_passed_at"] = None
    state["north_passed"] = False
    state["north_passed_at"] = None
    state["west_turn_done"] = False
    state["west_turn_done_at"] = None
    state["sync_eta_west"] = None
    state["sync_eta_east"] = None
    state["hold_role"] = None
    state["hold_delay"] = 0.0
    state["min_gap_to_zone"] = None
    state["summarized"] = False
    state["summary"] = {}

    ctx.set_weather(
        cloudiness=85.0,
        precipitation=35.0,
        precipitation_deposits=70.0,
        wind_intensity=15.0,
        sun_azimuth_angle=255.0,
        sun_altitude_angle=8.0,
        fog_density=6.0,
        fog_distance=35.0,
        fog_falloff=0.4,
        wetness=85.0,
    )
    logging.info("[weather] dusk, light rain, wet road, thin fog applied")

    west_route = _build_route(ctx, WEST_LEFT, "west_left")
    east_route = _build_route(ctx, EAST_STRAIGHT, "east_straight")
    south_route = _build_route(ctx, SOUTH_STRAIGHT, "south_wait")
    north_route = _build_route(ctx, NORTH_STRAIGHT, "north_straight")

    ew_lights, ew_stop_s = _pick_traffic_lights(ctx, east_route, "east_straight")
    ns_lights, ns_stop_s = _pick_traffic_lights(ctx, north_route, "north_straight")
    south_lights, south_stop_s = _pick_traffic_lights(ctx, south_route, "south_wait")
    ew_ids = [lamp.id for lamp in ew_lights]
    for lamp in ns_lights:
        if lamp.id in ew_ids:
            raise RuntimeError(
                "east-west and north-south lamp groups overlap on lamp id=%d; cannot build two phases"
                % lamp.id
            )
    logging.info(
        "[lights] south stop line s=%.2f shares the north-south phase (lamps=%s)",
        south_stop_s,
        [lamp.id for lamp in south_lights],
    )

    conflict = ctx.conflict_region(west_route, east_route, max_separation_m=2.5, max_height_m=0.75)
    if conflict is None:
        raise RuntimeError(
            "no geometric conflict region between the west left-turn route and the east "
            "through route; cannot stage the yield interaction"
        )
    conflict_s_west = conflict["a_s"]
    conflict_s_east = conflict["b_s"]
    if not (0.0 < conflict_s_west < west_route["length_m"]):
        raise RuntimeError("conflict reference s=%.2f outside the west route" % conflict_s_west)
    if not (0.0 < conflict_s_east < east_route["length_m"]):
        raise RuntimeError("conflict reference s=%.2f outside the east route" % conflict_s_east)
    logging.info(
        "[conflict] location=(%.2f, %.2f, %.2f) west_s=%.2f east_s=%.2f separation=%.2fm",
        conflict["location"].x,
        conflict["location"].y,
        conflict["location"].z,
        conflict_s_west,
        conflict_s_east,
        conflict["separation_m"],
    )

    zone_props, zone_s = _build_work_zone(ctx, south_route, south_stop_s, "south_wait")

    west_vehicle = _spawn_moving_vehicle(ctx, west_route, "west_left")
    east_vehicle = _spawn_moving_vehicle(ctx, east_route, "east_straight")
    north_vehicle = _spawn_moving_vehicle(ctx, north_route, "north_straight")

    south_wait_distance = (south_stop_s - zone_s) + SOUTH_STOP_GAP_M
    south_vehicle = ctx.spawn_waiting_vehicle(
        _make_vehicle_blueprint(ctx, "south_wait", COLORS["south_wait"]),
        south_route,
        role="south_wait",
        preferred_distance_m=south_wait_distance,
        clearance_m=0.5,
        reference_s=south_stop_s,
        max_search_m=30.0,
        max_attempts=32,
        distance_tolerance_m=SOUTH_WAIT_TOLERANCE_M,
    )
    if south_vehicle is None:
        raise RuntimeError(
            "spawn_waiting_vehicle found no legal spot %.2fm upstream of the south stop line "
            "(stop_s=%.2f, work zone s=%.2f)" % (south_wait_distance, south_stop_s, zone_s)
        )
    logging.info(
        "[south_wait] requested stop %.2fm before the stop line s=%.2f (work zone at s=%.2f)",
        south_wait_distance,
        south_stop_s,
        zone_s,
    )

    sensors = [
        _spawn_collision_sensor(ctx, west_vehicle, "west_left", state),
        _spawn_collision_sensor(ctx, east_vehicle, "east_straight", state),
        _spawn_collision_sensor(ctx, south_vehicle, "south_wait", state),
        _spawn_collision_sensor(ctx, north_vehicle, "north_straight", state),
    ]

    _apply_lights(ctx, ew_lights, carla.TrafficLightState.Green, "east-west")
    _apply_lights(ctx, ns_lights, carla.TrafficLightState.Red, "north-south")

    ctx.draw_route(west_route, label="TURN_W", color=(250, 250, 250), life_time=DURATION_SECONDS)
    ctx.draw_route(east_route, label="GO_E", color=(40, 40, 40), life_time=DURATION_SECONDS)
    ctx.draw_route(south_route, label="WAIT_S", color=(230, 40, 40), life_time=DURATION_SECONDS)
    ctx.draw_route(north_route, label="GO_N", color=(150, 150, 150), life_time=DURATION_SECONDS)

    state["routes"] = {
        "west_left": west_route,
        "east_straight": east_route,
        "south_wait": south_route,
        "north_straight": north_route,
    }
    state["vehicles"] = {
        "west_left": west_vehicle,
        "east_straight": east_vehicle,
        "south_wait": south_vehicle,
        "north_straight": north_vehicle,
    }
    state["sensors"] = sensors
    state["zone_props"] = zone_props
    state["obstacle_ids"] = set([prop.id for prop in zone_props])
    state["zone_s"] = zone_s
    state["south_stop_s"] = south_stop_s
    state["ew_lights"] = ew_lights
    state["ns_lights"] = ns_lights
    state["ew_stop_s"] = ew_stop_s
    state["ns_stop_s"] = ns_stop_s
    state["conflict_s_west"] = conflict_s_west
    state["conflict_s_east"] = conflict_s_east
    state["conflict_location"] = conflict["location"]
    state["index"] = {"west_left": 0, "east_straight": 0, "north_straight": 0}
    state["progress"] = {}
    state["spawn_locations"] = {}
    state["labels"] = [LABELS[role] for role in ROLE_ORDER]
    state["actors"] = [state["vehicles"][role] for role in ROLE_ORDER]
    logging.info("[setup] scene ready on %s junction %d", SCENE_MAP_NAME, JUNCTION_ID)


def _progress(state, role):
    cached = state["progress"].get(role)
    if cached is None:
        return {"index": 0, "s": 0.0, "remaining_m": 0.0, "speed_kmh": 0.0}
    return cached


def _update_progress(ctx, state, role):
    vehicle = state["vehicles"][role]
    route = state["routes"][role]
    progress = ctx.route_progress(vehicle, route, index=state["index"][role])
    state["index"][role] = progress["index"]
    state["progress"][role] = progress
    return progress


def _sync_departure(state, elapsed_seconds):
    """按到冲突区的实际路程换算出发延迟，使两车几乎同时接近交汇位置。"""
    west_progress = _progress(state, "west_left")
    east_progress = _progress(state, "east_straight")
    approach_mps = APPROACH_KMH / 3.6
    remaining_west = max(state["conflict_s_west"] - west_progress["s"], 0.0)
    remaining_east = max(state["conflict_s_east"] - east_progress["s"], 0.0)
    eta_west = remaining_west / approach_mps
    eta_east = remaining_east / approach_mps
    state["sync_eta_west"] = eta_west
    state["sync_eta_east"] = eta_east
    gap = abs(eta_west - eta_east)
    if eta_west <= eta_east:
        state["hold_role"] = "west_left"
        state["hold_delay"] = eta_east - eta_west
    else:
        state["hold_role"] = "east_straight"
        state["hold_delay"] = eta_west - eta_east
    if gap < MIN_SYNC_GAP_SECONDS:
        logging.info(
            "[sync] west remaining=%.2fm eta=%.2fs, east remaining=%.2fm eta=%.2fs; "
            "already simultaneous at %.1f km/h, no hold needed",
            remaining_west,
            eta_west,
            remaining_east,
            eta_east,
            APPROACH_KMH,
        )
        return True
    if gap > MAX_SYNC_GAP_SECONDS:
        raise RuntimeError(
            "cannot stage a near-simultaneous approach: ETA gap %.2fs exceeds %.1fs "
            "(west remaining %.2fm, east remaining %.2fm)"
            % (gap, MAX_SYNC_GAP_SECONDS, remaining_west, remaining_east)
        )
    state["phase"] = "hold"
    state["phase_t"] = elapsed_seconds
    logging.info(
        "[sync] west remaining=%.2fm eta=%.2fs, east remaining=%.2fm eta=%.2fs -> hold %s for %.2fs",
        remaining_west,
        eta_west,
        remaining_east,
        eta_east,
        state["hold_role"],
        state["hold_delay"],
    )
    return False


def _hold_control(vehicle):
    vehicle.apply_control(carla.VehicleControl(throttle=0.0, steer=0.0, brake=1.0))


def _drive(ctx, state, role, target_kmh):
    vehicle = state["vehicles"][role]
    route = state["routes"][role]
    state["index"][role] = ctx.drive_route(
        vehicle,
        route,
        state["index"][role],
        target_kmh,
        lookahead_m=6.0,
        deceleration=3.0,
    )


def _yield_target_kmh(state):
    progress = _progress(state, "west_left")
    remaining = state["conflict_s_west"] - STOP_BEFORE_CONFLICT_M - progress["s"]
    if remaining <= 0.5:
        return 0.0
    allowed = math.sqrt(2.0 * YIELD_DECEL * remaining)
    return max(0.0, min(CREEP_KMH, allowed * 3.6))


def _east_cleared_conflict(ctx, state):
    result = ctx.actor_passed_route_point(
        state["vehicles"]["east_straight"],
        state["routes"]["east_straight"],
        state["conflict_s_east"],
        margin=CONFLICT_CLEAR_MARGIN_M,
        max_height_m=1.0,
    )
    return bool(result[0]), result


def _ew_cleared_junction(ctx, state):
    west_route = state["routes"]["west_left"]
    east_route = state["routes"]["east_straight"]
    west_check_s = min(west_route["exit_s"] + EW_PASS_SAFETY_M, west_route["length_m"] - 0.2)
    east_check_s = min(east_route["exit_s"] + EW_PASS_SAFETY_M, east_route["length_m"] - 0.2)
    west_result = ctx.actor_passed_route_point(
        state["vehicles"]["west_left"],
        west_route,
        west_check_s,
        margin=0.0,
        max_height_m=1.0,
    )
    east_result = ctx.actor_passed_route_point(
        state["vehicles"]["east_straight"],
        east_route,
        east_check_s,
        margin=0.0,
        max_height_m=1.0,
    )
    return bool(west_result[0]) and bool(east_result[0]), west_result, east_result


def _check_zone_off_opposing_lane(state):
    """核验施工区没有侵入对向（北侧来车）驶出车道：与北向路线逐点保持横向净距。"""
    north_points = state["routes"]["north_straight"]["points"]
    worst_gap = None
    worst_prop = None
    for prop in state["zone_props"]:
        prop_location = prop.get_location()
        nearest = None
        for transform in north_points:
            gap = prop_location.distance(transform.location)
            if nearest is None or gap < nearest:
                nearest = gap
        if worst_gap is None or nearest < worst_gap:
            worst_gap = nearest
            worst_prop = prop.id
    state["zone_to_opposing_route_m"] = worst_gap
    logging.info(
        "[workzone] closest prop id=%d is %.2fm from the opposing north-bound route centreline",
        worst_prop,
        worst_gap,
    )
    if worst_gap < OPPOSING_LANE_MIN_GAP_M:
        raise RuntimeError(
            "work zone prop id=%d is only %.2fm from the opposing north-bound lane centreline; "
            "it would occupy the oncoming exit lane" % (worst_prop, worst_gap)
        )


def _check_zone_clearance(ctx, state):
    south_vehicle = state["vehicles"]["south_wait"]
    result = ctx.obstacle_clearance(south_vehicle, state["zone_props"])
    gap = result[0]
    overlaps = result[4]
    state["min_gap_to_zone"] = gap
    if overlaps:
        raise RuntimeError(
            "south waiting car overlaps work zone props %s at spawn (gap=%.2fm)" % (overlaps, gap)
        )
    if gap < 0.5:
        raise RuntimeError(
            "south waiting car is only %.2fm from the work zone; need a clear stopping gap" % gap
        )
    north_result = ctx.obstacle_clearance(state["vehicles"]["north_straight"], state["zone_props"])
    logging.info(
        "[workzone] south car gap=%.2fm (horizontal=%.2fm vertical=%.2fm); north car gap=%.2fm",
        gap,
        result[1],
        result[2],
        north_result[0],
    )


def _setup_view(ctx, state):
    overview = ctx.observe_overview(
        actors=state["actors"] + state["zone_props"],
        routes=[state["routes"][role] for role in ROLE_ORDER],
        locations=[state["conflict_location"]],
        margin_m=VIEW_MARGIN_M,
        min_height_m=20.0,
        max_height_m=120.0,
        yaw=-90.0,
        horizontal_fov_deg=90.0,
        viewport_width=1280,
        viewport_height=720,
        min_actor_pixels=VIEW_MIN_ACTOR_PIXELS,
    )
    logging.info(
        "[view] north-up overview center=(%.1f, %.1f, %.1f) height=%.1fm min_actor_pixels=%.1f",
        overview["center"]["x"],
        overview["center"]["y"],
        overview["center"]["z"],
        overview["height_m"],
        overview["actor_pixels"],
    )


def _inspect(ctx, state, elapsed_seconds):
    # expected_positions 省略：inspect_actors 只对 spawn_waiting_vehicle 创建的
    # 南侧等待车使用出生坐标核验漂移与速度，三辆行驶车不做等待判定。
    records = ctx.inspect_actors(
        state["actors"],
        labels=state["labels"],
        draw_labels=True,
        life_time=1.3,
        max_drift_m=SOUTH_MAX_DRIFT_M,
        max_speed_mps=SOUTH_MAX_SPEED_MPS,
    )
    fault = None
    for order, record in enumerate(records):
        label = state["labels"][order]
        if not record["alive"]:
            raise RuntimeError("actor %s (id=%s) disappeared during the run" % (label, record["id"]))
        issues = record["issues"] or []
        if issues and fault is None:
            fault = (label, issues)
    south_record = records[2]
    south_position = south_record["position"]
    spawn_location = state["spawn_locations"]["south_wait"]
    drift = math.sqrt(
        (south_position["x"] - spawn_location.x) ** 2
        + (south_position["y"] - spawn_location.y) ** 2
    )
    state["south_drift_m"] = drift
    logging.info(
        "[inspect] t=%.1fs W(%.1f,%.1f) %.1fkm/h | E(%.1f,%.1f) %.1fkm/h | "
        "S(%.1f,%.1f) %.2fm/s drift=%.2fm | N(%.1f,%.1f) %.1fkm/h",
        elapsed_seconds,
        records[0]["position"]["x"],
        records[0]["position"]["y"],
        records[0]["speed_mps"] * 3.6,
        records[1]["position"]["x"],
        records[1]["position"]["y"],
        records[1]["speed_mps"] * 3.6,
        south_position["x"],
        south_position["y"],
        south_record["speed_mps"],
        drift,
        records[3]["position"]["x"],
        records[3]["position"]["y"],
        records[3]["speed_mps"] * 3.6,
    )
    if fault is None:
        state["fault_since"] = None
        return
    label, issues = fault
    if elapsed_seconds < state["grace_until"]:
        logging.info(
            "[inspect] t=%.1fs landing transient on %s: %s", elapsed_seconds, label, issues
        )
        return
    if state["fault_since"] is None:
        state["fault_since"] = elapsed_seconds
        logging.warning("[inspect] t=%.1fs issues on %s: %s", elapsed_seconds, label, issues)
        return
    if elapsed_seconds - state["fault_since"] >= FAULT_SECONDS:
        raise RuntimeError(
            "persistent fault on %s for %.1fs: %s"
            % (label, elapsed_seconds - state["fault_since"], issues)
        )


def _log_summary(ctx, state, elapsed_seconds, reason):
    if state["summarized"]:
        return
    state["summarized"] = True
    south_vehicle = state["vehicles"]["south_wait"]
    summary = {
        "end_reason": reason,
        "elapsed_seconds": round(elapsed_seconds, 2),
        "phase": state["phase"],
        "west_left_turn_completed": state["west_turn_done"],
        "west_left_turn_at": state["west_turn_done_at"],
        "east_west_cleared_junction": state["ew_passed"],
        "east_west_cleared_at": state["ew_passed_at"],
        "north_passed_on_green": state["north_passed"],
        "north_passed_at": state["north_passed_at"],
        "south_wait_speed_mps": round(_speed_mps(south_vehicle), 3),
        "south_wait_drift_m": round(state.get("south_drift_m", 0.0), 3),
        "south_stop_line_s": round(state["south_stop_s"], 2),
        "work_zone_s": round(state["zone_s"], 2),
        "work_zone_props": len(state["zone_props"]),
        "min_gap_south_to_zone_m": state["min_gap_to_zone"],
        "zone_to_opposing_route_m": state.get("zone_to_opposing_route_m"),
        "conflict_s_west": round(state["conflict_s_west"], 2),
        "conflict_s_east": round(state["conflict_s_east"], 2),
        "sync_eta_west_s": state["sync_eta_west"],
        "sync_eta_east_s": state["sync_eta_east"],
        "collisions": list(state["collisions"]),
        "work_zone_hits": list(state["obstacle_hits"]),
    }
    state["summary"] = summary
    logging.info("[summary] %s", summary)


def step_scene(ctx, elapsed_seconds, frame_index):
    state = ctx.state
    if state["collisions"]:
        raise RuntimeError("collision detected during the run: %s" % state["collisions"])

    if not state["ready"]:
        for role in ROLE_ORDER:
            state["spawn_locations"][role] = state["vehicles"][role].get_location()
        state["south_drift_m"] = 0.0
        _check_zone_clearance(ctx, state)
        _check_zone_off_opposing_lane(state)
        _setup_view(ctx, state)
        state["ready"] = True
        state["grace_until"] = elapsed_seconds + GRACE_SECONDS
        logging.info("[init] first step at t=%.2fs; view and spawn references locked", elapsed_seconds)
        return False

    for role in ("west_left", "east_straight", "north_straight"):
        _update_progress(ctx, state, role)

    if elapsed_seconds - state["last_inspect"] >= INSPECT_INTERVAL:
        state["last_inspect"] = elapsed_seconds
        _inspect(ctx, state, elapsed_seconds)

    # 南侧等待车全程刹停；北侧车在南北绿灯前保持停车。
    _hold_control(state["vehicles"]["south_wait"])
    if state["phase"] != "ns_green":
        _hold_control(state["vehicles"]["north_straight"])

    phase = state["phase"]

    if phase == "ew_green":
        if _sync_departure(state, elapsed_seconds):
            state["phase"] = "approach"
            state["phase_t"] = elapsed_seconds
            logging.info(
                "[phase] t=%.1fs both east-west cars depart toward the conflict region at %.1f km/h",
                elapsed_seconds,
                APPROACH_KMH,
            )
            _drive(ctx, state, "west_left", APPROACH_KMH)
            _drive(ctx, state, "east_straight", APPROACH_KMH)
        else:
            hold_role = state["hold_role"]
            for role in ("west_left", "east_straight"):
                if role == hold_role:
                    _hold_control(state["vehicles"][role])
                else:
                    _drive(ctx, state, role, APPROACH_KMH)
        return False

    if phase == "hold":
        hold_role = state["hold_role"]
        released = elapsed_seconds - state["phase_t"] >= state["hold_delay"]
        if released:
            state["phase"] = "approach"
            state["phase_t"] = elapsed_seconds
            logging.info(
                "[phase] t=%.1fs %s released; both cars now approach the conflict region together",
                elapsed_seconds,
                hold_role,
            )
        for role in ("west_left", "east_straight"):
            if role == hold_role and not released:
                _hold_control(state["vehicles"][role])
            else:
                _drive(ctx, state, role, APPROACH_KMH)
        return False

    if phase == "approach":
        cleared, east_result = _east_cleared_conflict(ctx, state)
        west_progress = _progress(state, "west_left")
        distance_to_conflict = state["conflict_s_west"] - west_progress["s"]
        _drive(ctx, state, "east_straight", APPROACH_KMH)
        if cleared:
            state["phase"] = "west_go"
            state["phase_t"] = elapsed_seconds
            logging.info(
                "[phase] t=%.1fs east car cleared the conflict region (rear %.2fm past s=%.2f); "
                "west resumes the left turn",
                elapsed_seconds,
                east_result[1],
                state["conflict_s_east"],
            )
            _drive(ctx, state, "west_left", APPROACH_KMH)
            return False
        if distance_to_conflict <= YIELD_TRIGGER_M:
            state["phase"] = "yield"
            state["phase_t"] = elapsed_seconds
            east_progress = _progress(state, "east_straight")
            logging.info(
                "[phase] t=%.1fs west car %.2fm from the conflict region starts yielding "
                "(east %.2fm away at %.1f km/h)",
                elapsed_seconds,
                distance_to_conflict,
                state["conflict_s_east"] - east_progress["s"],
                east_progress["speed_kmh"],
            )
            target = _yield_target_kmh(state)
            if target <= 0.0:
                _hold_control(state["vehicles"]["west_left"])
            else:
                _drive(ctx, state, "west_left", target)
            return False
        _drive(ctx, state, "west_left", APPROACH_KMH)
        return False

    if phase == "yield":
        cleared, east_result = _east_cleared_conflict(ctx, state)
        _drive(ctx, state, "east_straight", APPROACH_KMH)
        if cleared:
            state["phase"] = "west_go"
            state["phase_t"] = elapsed_seconds
            logging.info(
                "[phase] t=%.1fs east car fully past the conflict region (rear %.2fm); "
                "west completes the left turn",
                elapsed_seconds,
                east_result[1],
            )
            _drive(ctx, state, "west_left", APPROACH_KMH)
        else:
            target = _yield_target_kmh(state)
            if target <= 0.0:
                _hold_control(state["vehicles"]["west_left"])
            else:
                _drive(ctx, state, "west_left", target)
        return False

    if phase == "west_go":
        _drive(ctx, state, "west_left", APPROACH_KMH)
        _drive(ctx, state, "east_straight", APPROACH_KMH)
        if not state["west_turn_done"]:
            west_route = state["routes"]["west_left"]
            west_result = ctx.actor_passed_route_point(
                state["vehicles"]["west_left"],
                west_route,
                west_route["exit_s"],
                margin=1.0,
                max_height_m=1.0,
            )
            if west_result[0]:
                state["west_turn_done"] = True
                state["west_turn_done_at"] = elapsed_seconds
                logging.info(
                    "[event] t=%.1fs west car completed the left turn "
                    "(whole body past the junction exit, rear %.2fm)",
                    elapsed_seconds,
                    west_result[1],
                )
        if not state["ew_passed"]:
            cleared, west_result, east_result = _ew_cleared_junction(ctx, state)
            if cleared:
                state["ew_passed"] = True
                state["ew_passed_at"] = elapsed_seconds
                logging.info(
                    "[event] t=%.1fs both east-west cars fully cleared the junction "
                    "(west rear %.2fm, east rear %.2fm)",
                    elapsed_seconds,
                    west_result[1],
                    east_result[1],
                )
                _apply_lights(ctx, state["ew_lights"], carla.TrafficLightState.Yellow, "east-west")
                state["phase"] = "ew_yellow"
                state["phase_t"] = elapsed_seconds
        return False

    if phase == "ew_yellow":
        _drive(ctx, state, "west_left", APPROACH_KMH)
        _drive(ctx, state, "east_straight", APPROACH_KMH)
        if elapsed_seconds - state["phase_t"] >= YELLOW_SECONDS:
            _apply_lights(ctx, state["ew_lights"], carla.TrafficLightState.Red, "east-west")
            _apply_lights(ctx, state["ns_lights"], carla.TrafficLightState.Red, "north-south")
            state["phase"] = "all_red"
            state["phase_t"] = elapsed_seconds
            logging.info("[phase] t=%.1fs all-red clearance interval", elapsed_seconds)
        return False

    if phase == "all_red":
        _drive(ctx, state, "west_left", APPROACH_KMH)
        _drive(ctx, state, "east_straight", APPROACH_KMH)
        if elapsed_seconds - state["phase_t"] >= ALL_RED_SECONDS:
            _apply_lights(ctx, state["ns_lights"], carla.TrafficLightState.Green, "north-south")
            state["phase"] = "ns_green"
            state["phase_t"] = elapsed_seconds
            logging.info("[phase] t=%.1fs north-south green", elapsed_seconds)
        return False

    if phase == "ns_green":
        _drive(ctx, state, "west_left", APPROACH_KMH)
        _drive(ctx, state, "east_straight", APPROACH_KMH)
        _drive(ctx, state, "north_straight", APPROACH_KMH)
        _hold_control(state["vehicles"]["south_wait"])
        if not state["north_passed"]:
            north_route = state["routes"]["north_straight"]
            check_s = min(north_route["exit_s"] + 1.0, north_route["length_m"] - 0.2)
            north_result = ctx.actor_passed_route_point(
                state["vehicles"]["north_straight"],
                north_route,
                check_s,
                margin=0.0,
                max_height_m=1.0,
            )
            if north_result[0]:
                state["north_passed"] = True
                state["north_passed_at"] = elapsed_seconds
                logging.info(
                    "[event] t=%.1fs north car passed through the junction on green "
                    "(rear %.2fm past s=%.2f)",
                    elapsed_seconds,
                    north_result[1],
                    check_s,
                )
        if elapsed_seconds >= DURATION_SECONDS - SUMMARY_LEAD_SECONDS:
            _log_summary(ctx, state, elapsed_seconds, "duration reached")
            return True
        return False

    raise RuntimeError("unknown scene phase %r at t=%.2fs" % (phase, elapsed_seconds))
