File size: 21,654 Bytes
3a00e09
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
"""Next Drive: top-down urban intersection simulator, expert planner, rule-based driver, learned driver and safety check.

Simulation and training demo only. No physical vehicle, no claim about real-road safety.
The ego car follows a fixed route (straight, left or right turn) and only chooses its longitudinal
action every 0.1 s from ACTIONS. Everything is deterministic given (seed, kind, density) and is ported
line by line to web/core.js so the Space can generate new drives.
"""
import math

import numpy as np

from nlrt import Mulberry32, forward

DT, VMAX, V_LIMIT, T_MAX = 0.1, 14.0, 13.9, 45.0
ACTIONS = [-6.0, -3.0, -1.5, 0.0, 1.0, 2.0]
ACTION_NAMES = ["brake hard", "brake", "ease off", "hold speed", "speed up", "accelerate"]
LANE, S_STOP, HALF = 1.75, 60.0, 2.2
ROUTES = ["straight", "left", "right"]
KINDS = ["light", "crosswalk", "lead_brake", "occlusion", "mixed"]
TURN = {"right": (-5.75, 4.0), "left": (-5.25, 7.0)}
N_FEAT = 21


# ------------------------------------------------------------------ geometry
def path_len(route):
    if route == "straight":
        return 130.0
    y0, r = TURN[route]
    return (y0 + 70.0) + r * math.pi / 2 + 55.0


def pose(route, s):
    """Position and heading on the route at arc length s (clamped to the route)."""
    s = max(0.0, min(s, path_len(route)))
    if route == "straight":
        return LANE, -70.0 + s, math.pi / 2
    y0, r = TURN[route]
    L1 = y0 + 70.0
    if s < L1:
        return LANE, -70.0 + s, math.pi / 2
    arc = r * math.pi / 2
    if s < L1 + arc:
        th = (s - L1) / r
        if route == "right":
            return LANE + r - r * math.cos(th), y0 + r * math.sin(th), math.pi / 2 - th
        return LANE - r + r * math.cos(th), y0 + r * math.sin(th), math.pi / 2 + th
    d = s - L1 - arc
    if route == "right":
        return LANE + r + d, y0 + r, 0.0
    return LANE - r - d, y0 + r, math.pi


def circles(x, y, h):
    c, s = math.cos(h), math.sin(h)
    return [(x + d * c, y + d * s) for d in (-1.4, 0.0, 1.4)]


def hits_car(ego, other, margin=0.0):
    for ax, ay in ego:
        for bx, by in other:
            dx, dy, rr = ax - bx, ay - by, 2.0 + margin
            if dx * dx + dy * dy < rr * rr:
                return True
    return False


def hits_ped(ego, px, py, margin=0.0):
    for ax, ay in ego:
        dx, dy, rr = ax - px, ay - py, 1.35 + margin
        if dx * dx + dy * dy < rr * rr:
            return True
    return False


# ------------------------------------------------------------------ traffic lights
def light_ns(sc, t):
    G, Y, R = sc["G"], 3.0, sc["R"]
    u = (t + sc["phase"]) % (G + Y + R)
    if u < G:
        return 0, G - u
    if u < G + Y:
        return 1, G + Y - u
    return 2, G + Y + R - u


def light_ew(sc, t):
    G, Y, R = sc["G"], 3.0, sc["R"]
    u = (t + sc["phase"]) % (G + Y + R)
    if u < G + Y:
        return 2
    r = u - G - Y
    if r < 1.0:
        return 2
    if r < R - 4.0:
        return 0
    if r < R - 1.0:
        return 1
    return 2


# ------------------------------------------------------------------ scenarios
def make_scenario(seed, kind="mixed", density=1):
    rng = Mulberry32((seed * 7919 + KINDS.index(kind) * 101 + density * 13) % 4294967296)
    u = rng.random()
    route = "straight" if u < 0.5 else ("left" if u < 0.75 else "right")
    if kind in ("occlusion", "crosswalk") and rng.random() < 0.5:
        route = "straight"
    G, R = 7.0 + 6.0 * rng.random(), 8.0 + 5.0 * rng.random()
    sc = {"seed": seed, "kind": kind, "density": density, "route": route, "G": G, "R": R,
          "phase": rng.random() * (G + 3.0 + R), "cars": [], "peds": [], "parked": [], "lead": None}
    for k in range(density + 1):  # cross traffic, both directions
        for d in (1, -1):
            sc["cars"].append({"axis": "x", "dir": d, "lane": -LANE * d, "p0": -70.0 * d, "v0": 8.0 + 4.0 * rng.random(),
                               "t0": k * 7.0 + 5.0 * rng.random()})
    if route == "left" or kind == "mixed":  # oncoming traffic (southbound)
        for k in range(density):
            sc["cars"].append({"axis": "y", "dir": -1, "lane": -LANE, "p0": 70.0, "v0": 8.0 + 4.0 * rng.random(),
                               "t0": k * 6.0 + 4.0 * rng.random()})
    if kind in ("crosswalk", "mixed"):
        for k in range(density + 1):
            d = 1 if rng.random() < 0.5 else -1
            sc["peds"].append({"x0": -9.0 * d, "y0": -7.5 + 1.6 * (rng.random() - 0.5), "vx": d * (1.0 + 0.6 * rng.random()),
                               "vy": 0.0, "t0": 1.0 + 11.0 * rng.random(), "x_end": 9.0 * d})
    if kind in ("lead_brake", "mixed"):
        sc["lead"] = {"s0": 14.0 + 10.0 * rng.random(), "v0": 9.0 + 3.0 * rng.random(),
                      "tb": 2.5 + 5.0 * rng.random(), "db": 1.5 + 1.5 * rng.random()}
    if kind in ("occlusion", "mixed"):
        y = -46.0
        gaps = []
        while y < -16.0:
            L = 4.6
            sc["parked"].append([4.6, y + L / 2, 2.0, L])
            y += L + 0.4 + (2.6 if rng.random() < 0.45 else 0.0)
            gaps.append(y - 0.2)
        n_emerge = density
        for k in range(min(n_emerge, len(gaps))):
            gy = gaps[rng.randint(len(gaps))] - 1.0
            D = 12.0 + 10.0 * rng.random()
            sc["peds"].append({"x0": 5.8, "y0": gy, "vx": -(1.2 + 0.4 * rng.random()), "vy": 0.0,
                               "t0": max(0.0, (gy + 70.0 - HALF - D) / 10.0), "x_end": -6.0})
    return sc


# ------------------------------------------------------------------ world state
def init_world(sc):
    w = {"t": 0.0, "s": 0.0, "v": 10.0, "a": 0.0, "cars": [], "peds": [], "lead": None}
    for c in sc["cars"]:
        w["cars"].append({"p": c["p0"], "v": c["v0"], "active": False, "done": False, **{k: c[k] for k in ("axis", "dir", "lane", "v0", "t0")}})
    for p in sc["peds"]:
        w["peds"].append({"x": p["x0"], "y": p["y0"], "vx": 0.0, "vy": 0.0, "done": False, **{k: p[k] for k in ("t0", "x_end")}, "svx": p["vx"]})
    if sc["lead"]:
        L = sc["lead"]
        w["lead"] = {"s": L["s0"], "v": L["v0"], **L}
    return w


def car_xyh(c):
    if c["axis"] == "x":
        return c["p"], c["lane"], 0.0 if c["dir"] > 0 else math.pi
    return c["lane"], c["p"], math.pi / 2 if c["dir"] > 0 else -math.pi / 2


def step_agents(sc, w):
    t = w["t"]
    ew, (ns, _) = light_ew(sc, t), light_ns(sc, t)
    lanes = {}
    for c in w["cars"]:
        if not c["active"] and not c["done"] and t >= c["t0"]:
            c["active"] = True
        if c["active"]:
            lanes.setdefault((c["axis"], c["dir"]), []).append(c)
    for key, cs in lanes.items():
        cs.sort(key=lambda c: -c["p"] * c["dir"])  # front-most first
        for i, c in enumerate(cs):
            light = ew if c["axis"] == "x" else ns
            front = c["p"] * c["dir"] + HALF
            d_stop = -10.0 - front
            a = min(2.0, 1.5 * (c["v0"] - c["v"]))
            if light != 0 and d_stop > -1.0 and c["v"] * c["v"] / (2 * 3.0) < d_stop + 1.0:
                a = min(a, -c["v"] * c["v"] / (2 * max(d_stop - 0.5, 0.3)))
            if i > 0:
                gap = (cs[i - 1]["p"] - c["p"]) * c["dir"] - 4.4
                if gap < 6.0 + c["v"] * 1.2:
                    a = min(a, -3.0 if gap > 3.0 else -6.0)
            c["v"] = max(0.0, c["v"] + max(-6.0, a) * DT)
            c["p"] += c["dir"] * c["v"] * DT
            if c["p"] * c["dir"] > 75.0:
                c["active"], c["done"] = False, True
    for p in w["peds"]:
        if p["done"]:
            continue
        if t >= p["t0"]:
            p["vx"] = p["svx"]
            p["x"] += p["vx"] * DT
            if (p["x"] - p["x_end"]) * p["svx"] > 0:
                p["done"] = True
    L = w["lead"]
    if L is not None:
        if L["tb"] <= t < L["tb"] + L["db"]:
            a = -6.0
        else:
            a = min(2.0, 1.5 * (L["v0"] - L["v"]))
            d_stop = S_STOP - (L["s"] + HALF)
            if ns != 0 and d_stop > -1.0 and L["v"] * L["v"] / (2 * 3.0) < d_stop + 1.0:
                a = min(a, -L["v"] * L["v"] / (2 * max(d_stop - 0.5, 0.3)))
        L["v"] = max(0.0, L["v"] + max(-6.0, a) * DT)
        L["s"] += L["v"] * DT


# ------------------------------------------------------------------ perception
def seg_hits_rect(x0, y0, x1, y1, rect):
    cx, cy, wd, ht = rect
    xmin, xmax, ymin, ymax = cx - wd / 2, cx + wd / 2, cy - ht / 2, cy + ht / 2
    t0, t1 = 0.0, 1.0
    for p, q in ((-(x1 - x0), x0 - xmin), (x1 - x0, xmax - x0), (-(y1 - y0), y0 - ymin), (y1 - y0, ymax - y0)):
        if p == 0:
            if q < 0:
                return False
        else:
            r = q / p
            if p < 0:
                if r > t1:
                    return False
                if r > t0:
                    t0 = r
            else:
                if r < t0:
                    return False
                if r < t1:
                    t1 = r
    return True


def observe(sc, w):
    ex, ey, eh = pose(sc["route"], w["s"])
    eye = (ex + 1.0 * math.cos(eh), ey + 1.0 * math.sin(eh))
    agents = []
    for c in w["cars"]:
        if c["active"]:
            x, y, h = car_xyh(c)
            agents.append({"kind": "car", "x": x, "y": y, "h": h, "vx": c["v"] * math.cos(h), "vy": c["v"] * math.sin(h)})
    L = w["lead"]
    if L is not None and L["s"] < path_len(sc["route"]):
        x, y, h = pose(sc["route"], L["s"])
        agents.append({"kind": "lead", "x": x, "y": y, "h": h, "vx": L["v"] * math.cos(h), "vy": L["v"] * math.sin(h), "s": L["s"], "v": L["v"]})
    visible = []
    for p in w["peds"]:
        if p["done"]:
            continue
        dx, dy = p["x"] - eye[0], p["y"] - eye[1]
        vis = dx * dx + dy * dy < 3600.0 and not any(seg_hits_rect(eye[0], eye[1], p["x"], p["y"], r) for r in sc["parked"])
        visible.append(vis)
        if vis:
            agents.append({"kind": "ped", "x": p["x"], "y": p["y"], "h": 0.0, "vx": p["vx"], "vy": p["vy"]})
    phantoms = []
    s_front = w["s"] + HALF
    for r in sc["parked"]:
        gy = r[1] + r[3] / 2 + 0.3
        ahead = (gy + 70.0) - s_front
        if 0.0 < ahead < 35.0 and seg_hits_rect(eye[0], eye[1], 4.2, gy, r):
            phantoms.append({"x": 4.2, "y": gy, "vx": -1.4, "vy": 0.0, "ahead": ahead})
    ls, rem = light_ns(sc, w["t"])
    return {"t": w["t"], "s": w["s"], "v": w["v"], "a": w["a"], "route": sc["route"], "agents": agents,
            "phantoms": phantoms, "light": ls, "remaining": rem, "sc": sc, "visible": visible}


# ------------------------------------------------------------------ prediction helpers
def rollout_hits(obs, plan, horizon=3.0, dt=0.25, margin_car=0.4, margin_ped=0.6):
    """plan(k, tau) -> accel. Returns (first collision time or None, red-light crossing time or None, s_end, soft penalty).
    Soft penalty: speed above the limit, speed above what allows stopping before an occluded gap
    (where a hidden pedestrian could step out), and following closer than 2 m + 1.2 s to the lead car."""
    s, v = obs["s"], obs["v"]
    route = obs["route"]
    n = int(round(horizon / dt))
    excess = 0.0
    red_t = None
    lead = [ag for ag in obs["agents"] if ag["kind"] == "lead"]
    for k in range(1, n + 1):
        tau = k * dt
        a = plan(k, tau)
        v = min(VMAX, max(0.0, v + a * dt))
        s_prev = s
        s = s + v * dt
        if v > V_LIMIT:
            excess += v - V_LIMIT
        for ph in obs["phantoms"]:
            d = ph["y"] + 70.0 - (s + HALF)
            if d > -1.0:
                excess += max(0.0, v - (math.sqrt(2 * 4.0 * max(d - 2.0, 0.0)) + 3.0))
        for ag in lead:
            gap = ag["s"] + ag["v"] * tau - s - 4.4
            excess += 0.5 * max(0.0, 2.0 + 1.2 * v - gap)
        if red_t is None and s > s_prev:
            lt, _ = light_ns(obs["sc"], obs["t"] + tau)
            crossing = s_prev + HALF < S_STOP - 0.5 <= s + HALF
            creeping = S_STOP - 0.5 <= obs["s"] + HALF < S_STOP + 6.0 and obs["v"] < 3.0
            if lt == 2 and (crossing or creeping):
                red_t = tau
        ex, ey, eh = pose(route, s)
        ec = circles(ex, ey, eh)
        for ag in obs["agents"]:
            px, py = ag["x"] + ag["vx"] * tau, ag["y"] + ag["vy"] * tau
            if (px - ex) * (px - ex) + (py - ey) * (py - ey) > 64.0:
                continue
            if ag["kind"] == "ped":
                if hits_ped(ec, px, py, margin_ped):
                    return tau, red_t, s, excess
            elif hits_car(ec, circles(px, py, ag["h"]), margin_car):
                return tau, red_t, s, excess
    return None, red_t, s, excess


# ------------------------------------------------------------------ drivers
def expert(obs):
    """Search over two-phase action sequences (1 s then 2 s), constant-velocity predictions, phantom pedestrians."""
    best, best_cost = 0, float("inf")
    for i, a1 in enumerate(ACTIONS):
        for a2 in ACTIONS:
            plan = lambda k, tau, a1=a1, a2=a2: a1 if tau <= 1.0 + 1e-9 else a2
            hit, red, s_end, excess = rollout_hits(obs, plan)
            cost = 0.0
            if hit is not None:
                cost += 1e5 - 1e4 * hit
            if red is not None:
                cost += 2e4
            cost += -10.0 * (s_end - obs["s"])
            cost += 40.0 if a1 == -6.0 else (8.0 if a1 == -3.0 else 0.0)
            cost += 3.0 * abs(a1 - obs["a"])
            cost += 50.0 * excess
            if cost < best_cost:
                best, best_cost = i, cost
    return best


def rule_driver(obs):
    """IDM-style rules: lights, lead car, visible pedestrians near the route, yield before a left turn."""
    v, s = obs["v"], obs["s"]
    route = obs["route"]
    s_front = s + HALF
    obst = []
    for ag in obs["agents"]:
        if ag["kind"] == "lead":
            obst.append((ag["s"] - s - 4.4, ag["v"]))
    d_stop = S_STOP - s_front
    if d_stop > 0 and (obs["light"] == 2 or (obs["light"] == 1 and v * v / (2 * 3.0) < d_stop)):
        obst.append((d_stop - 0.5, 0.0))
    for ag in obs["agents"]:
        if ag["kind"] != "ped":
            continue
        for j in range(0, 51, 2):
            px, py, _ = pose(route, s + j)
            fx, fy = ag["x"] + ag["vx"] * 1.5, ag["y"] + ag["vy"] * 1.5
            d1 = (ag["x"] - px) * (ag["x"] - px) + (ag["y"] - py) * (ag["y"] - py)
            d2 = (fx - px) * (fx - px) + (fy - py) * (fy - py)
            if min(d1, d2) < 6.76:
                obst.append((j - HALF - 1.5, 0.0))
                break
    if route == "left":
        y0, _ = TURN["left"]
        d_turn = (y0 + 70.0) - s_front
        if d_turn > -1.0:
            for ag in obs["agents"]:
                if ag["kind"] == "car" and ag["vy"] < -0.5 and -4.0 < ag["y"] < 45.0 and ag["x"] < 0:
                    obst.append((d_turn - 0.5, 0.0))
                    break
    q = v / 12.0
    q4 = (q * q) * (q * q)
    a = 1.5 * (1.0 - q4)
    for gap, vo in obst:
        gap = max(gap, 0.1)
        sstar = 2.0 + v * 1.2 + v * (v - vo) / (2 * math.sqrt(1.5 * 3.0))
        a = min(a, 1.5 * (1.0 - q4 - (sstar / gap) * (sstar / gap)))
    a = max(-6.0, min(2.0, a))
    return min(range(len(ACTIONS)), key=lambda i: (abs(ACTIONS[i] - a), i))


def features(obs):
    v, s = obs["v"], obs["s"]
    d_stop = S_STOP - (s + HALF)
    f = [v / 14.0, obs["a"] / 6.0, min(max(d_stop, 0.0), 60.0) / 60.0, 1.0 if d_stop < 0 else 0.0,
         1.0 if obs["light"] == 0 else 0.0, 1.0 if obs["light"] == 1 else 0.0, 1.0 if obs["light"] == 2 else 0.0,
         min(obs["remaining"], 15.0) / 15.0]
    lead = [ag for ag in obs["agents"] if ag["kind"] == "lead"]
    if lead:
        f += [min(max(lead[0]["s"] - s - 4.4, 0.0), 50.0) / 50.0, max(-1.0, min(1.0, (lead[0]["v"] - v) / 14.0))]
    else:
        f += [1.0, 0.0]
    for acc in (0.0, -3.0, 1.0):
        hit, _, _, _ = rollout_hits(obs, lambda k, tau, acc=acc: acc, horizon=5.0)
        f.append((hit if hit is not None else 5.0) / 5.0)
    ex, ey, _ = pose(obs["route"], s)
    peds = [math.sqrt((ag["x"] - ex) * (ag["x"] - ex) + (ag["y"] - ey) * (ag["y"] - ey)) for ag in obs["agents"] if ag["kind"] == "ped"]
    f.append(min(min(peds), 40.0) / 40.0 if peds else 1.0)
    f.append(min(min(p["ahead"] for p in obs["phantoms"]), 40.0) / 40.0 if obs["phantoms"] else 1.0)
    busy = any(ag["kind"] == "car" and abs(ag["x"]) < 12.0 and abs(ag["y"]) < 12.0 for ag in obs["agents"])
    f.append(1.0 if busy else 0.0)
    f += [1.0 if obs["route"] == r else 0.0 for r in ROUTES]
    f.append(min(v * v / 12.0, 60.0) / 60.0)
    f.append(1.0 if obs["light"] != 0 and d_stop > 0 and v * v / 6.0 < d_stop + 1.0 else 0.0)
    return f


def learned(obs, layers):
    z = forward(layers, np.asarray([features(obs)]))[0]
    return int(np.argmax(z))


def shield(obs, i):
    """Safety check: keep action i only if braking hard after 0.5 s still avoids predicted collisions / red-light entry."""
    def unsafe(a):
        hit, red, _, _ = rollout_hits(obs, lambda k, tau: a if tau <= 0.5 + 1e-9 else -6.0, margin_car=0.2, margin_ped=0.4)
        if hit is not None:
            return True
        if red is not None:
            _, red_b, _, _ = rollout_hits(obs, lambda k, tau: -6.0, margin_car=0.2, margin_ped=0.4)
            return red_b is None
        return False
    if not unsafe(ACTIONS[i]):
        return i, False
    for j in range(i - 1, -1, -1):
        if not unsafe(ACTIONS[j]):
            return j, True
    return 0, True


# ------------------------------------------------------------------ episode
def run_episode(sc, driver, layers=None, record=True, timer=None, on_obs=None):
    w = init_world(sc)
    route = sc["route"]
    goal = path_len(route) - 5.0
    out = {"collision": False, "collision_with": None, "red_violations": 0, "reached": False, "hard_brakes": 0,
           "overrides": 0, "steps": 0, "time": 0.0, "decision_ms": [], "trace": [], "events": [], "actions": []}
    prev_a = 0.0
    for step in range(int(round(T_MAX / DT))):
        obs = observe(sc, w)
        if on_obs is not None:
            on_obs(obs)
        t0 = timer() if timer else 0.0
        override = False
        if driver == "expert":
            i = expert(obs)
        elif driver == "rule":
            i = rule_driver(obs)
        elif driver == "learned":
            i = learned(obs, layers)
        else:
            i, override = shield(obs, learned(obs, layers))
        if timer:
            out["decision_ms"].append(1000.0 * (timer() - t0))
        a = ACTIONS[i]
        out["actions"].append(i)
        if override:
            out["overrides"] += 1
        if a <= -6.0 and prev_a > -6.0:
            out["hard_brakes"] += 1
            out["events"].append({"t": round(w["t"], 2), "type": "hard_brake"})
        prev_a = a
        s_front_prev = w["s"] + HALF
        w["v"] = min(VMAX, max(0.0, w["v"] + a * DT))
        w["s"] += w["v"] * DT
        w["a"] = a
        step_agents(sc, w)
        w["t"] = (step + 1) * DT
        if s_front_prev < S_STOP <= w["s"] + HALF and light_ns(sc, w["t"])[0] == 2:
            out["red_violations"] += 1
            out["events"].append({"t": round(w["t"], 2), "type": "red_light"})
        ex, ey, eh = pose(route, w["s"])
        ec = circles(ex, ey, eh)
        hit = None
        for c in w["cars"]:
            if c["active"] and hits_car(ec, circles(*car_xyh(c))):
                hit = "car"
        if w["lead"] is not None and w["lead"]["s"] < path_len(route) and hits_car(ec, circles(*pose(route, w["lead"]["s"]))):
            hit = "lead car"
        for p in w["peds"]:
            if not p["done"] and hits_ped(ec, p["x"], p["y"]):
                hit = "pedestrian"
        if record:
            out["trace"].append([round(w["t"], 2), round(ex, 3), round(ey, 3), round(eh, 4), round(w["v"], 3), i, int(override),
                                 [[round(v_, 2) for v_ in car_xyh(c)] for c in w["cars"] if c["active"]] +
                                 ([[round(v_, 2) for v_ in pose(route, w["lead"]["s"])]] if w["lead"] is not None and w["lead"]["s"] < path_len(route) else []),
                                 [[round(p["x"], 2), round(p["y"], 2)] for p in w["peds"] if not p["done"]],
                                 obs["visible"], light_ns(sc, w["t"])[0], light_ew(sc, w["t"])])
        out["steps"] = step + 1
        out["time"] = w["t"]
        if hit:
            out["collision"], out["collision_with"] = True, hit
            out["events"].append({"t": round(w["t"], 2), "type": "collision", "with": hit})
            break
        if w["s"] >= goal:
            out["reached"] = True
            break
    return out


# ------------------------------------------------------------------ batch jobs (used with multiprocessing)
def summarize(r):
    return {k: r[k] for k in ("collision", "collision_with", "red_violations", "reached", "hard_brakes", "overrides", "steps", "time")}


def collect_job(job):
    """job = (seed, kind, density, driver, layers). Runs the driver and labels every visited state with the expert."""
    seed, kind, density, driver, layers = job
    X, y = [], []

    def label(obs):
        X.append(features(obs))
        y.append(expert(obs))

    r = run_episode(make_scenario(seed, kind, density), driver, layers, record=False, on_obs=label)
    return X, y, summarize(r)


def eval_job(job):
    seed, kind, density, driver, layers = job
    sc = make_scenario(seed, kind, density)
    r = run_episode(sc, driver, layers, record=False)
    return {"seed": seed, "kind": kind, "density": density, "driver": driver, "route": sc["route"], **summarize(r),
            "events": r["events"][-4:]}