"""依頼235：止まった車いす（x 0.00・向き 0°）を、その場で向きを変えて x +0.42・向き 0° へ寄せる道があるかの計算（真上の2次元・長方形で近似）。
決まり（依頼235 の答え 1）：向き ±30° まで・速さ 0.3 m/秒・向きの変わる速さ 25°/秒・後ろの車輪は車軸の向きにだけ進む（その場で回るのは車軸の中心で）。
前へ：足のせの前の端とハコブくんの前の端のあいだ 0.6 m 以上。後ろへ：1.0 m まで（車軸の中心の z）。
寸法は 233 と同じ：車いす 車軸の中心から 横 ±0.326・前 0.790（足のせの前の端）・後ろ 0.280。大城さん 根は車軸の後ろ 0.444・幅 ±0.2381・奥行 ±0.157。
手すりの内の面 ±0.84。ハコブくん x −0.4643・幅 ±0.330・前の端は z 0（止まった所。足のせの前の端は G=2.30 m 先）。
z は前（ハコブくんの方）を − に取る。道は「その場で回る」「まっすぐ前／後ろ」をつないだもの（hybrid A*）。費用＝時間（速さの上限で）＋切りかえ 1 回 0.8 秒"""
import math, heapq, sys, json
RAIL = 0.84
CW, CF, CB = 0.326, 0.790, 0.280
OB, OW, OD = 0.444, 0.2381, 0.157
G = float(sys.argv[1]) if len(sys.argv) > 1 else 2.30
MARGIN = float(sys.argv[2]) if len(sys.argv) > 2 else 0.05     # 手すりとの余り
BACK_MAX = float(sys.argv[3]) if len(sys.argv) > 3 else 1.0
FWD_MAX = G - 0.6                                                 # 足のせが前へ出てよい長さ（z）
TH_MAX = 30.0
# 寺島さん（−x を向いて立つ・前 0.150・後ろ 0.140・幅 ±0.2381）。TZ は大城さんの根からの前後（+ は後ろ）。TX は根の x
TX = float(sys.argv[5]) if len(sys.argv) > 5 else 9.0
TZ = float(sys.argv[6]) if len(sys.argv) > 6 else 0.0
PCLR = float(sys.argv[7]) if len(sys.argv) > 7 else 0.05
TF, TB, TW = 0.1499, 0.1399, 0.2381
def tera_box(): return (TX - TF, TX + TB, OB + TZ - TW, OB + TZ + TW)
def poly_box_gap(pts, box):
    # 凸の四角（pts）と軸に沿った箱のすき間（近似：頂点と辺の点の距離・中に入れば −1）
    x0, x1, z0, z1 = box; best = 9.0
    for i in range(4):
        a, b = pts[i], pts[(i + 1) % 4]
        for k in range(9):
            px = a[0] + (b[0] - a[0]) * k / 8; pz = a[1] + (b[1] - a[1]) * k / 8
            dx = max(x0 - px, 0, px - x1); dz = max(z0 - pz, 0, pz - z1)
            best = min(best, -1.0 if dx == 0 and dz == 0 else math.hypot(dx, dz))
    # 箱の角が四角の中
    for (cx, cz) in ((x0, z0), (x0, z1), (x1, z0), (x1, z1)):
        inside = True
        for i in range(4):
            a, b = pts[i], pts[(i + 1) % 4]
            if (b[0] - a[0]) * (cz - a[1]) - (b[1] - a[1]) * (cx - a[0]) > 0: inside = False
        if inside: return -1.0
    return best
def corners(ax, az, th):
    s, c = math.sin(math.radians(th)), math.cos(math.radians(th))
    # 前＝−z、向き + は +x へ。点 (lx 横, lu 前) → 世界
    def w(lx, lu): return (ax + lx * c + lu * s, az - lu * c + lx * s)
    ch = [w(-CW, CF), w(CW, CF), w(CW, -CB), w(-CW, -CB)]
    ob = [w(-OW, -OB + OD), w(OW, -OB + OD), w(OW, -OB - OD), w(-OW, -OB - OD)]
    return ch, ob
def ok(ax, az, th):
    ch, ob = corners(ax, az, th)
    xs = [p[0] for p in ch + ob]
    if max(xs) > RAIL - MARGIN or min(xs) < -RAIL + MARGIN: return False
    fz = min(p[1] for p in ch)            # 足のせの前の端（いちばん前の点）
    if 0.0 - fz > FWD_MAX + 1e-9: return False    # z0 = 足のせの始めの前の端を 0 とする
    if az > BACK_MAX + 1e-9: return False
    if TX < 5:
        bx = tera_box()
        if min(poly_box_gap(ch, bx), poly_box_gap(ob, bx)) < PCLR: return False
    return True
def rail_gap(ax, az, th):
    ch, ob = corners(ax, az, th); xs = [p[0] for p in ch + ob]
    return min(RAIL - max(xs), RAIL + min(xs))
DTH, DS = 1.0, 0.01
V, W = 0.3, 25.0
import os; SW = float(os.environ.get("SW", "0.8"))
def key(ax, az, th): return (round(ax / 0.005), round(az / 0.01), round(th))
start = (0.0, 0.0, 0.0)
goalx = 0.42
pq = [(0.0, 0.0, 0.0, 0.0, 0.0, "", None)]
seen = {}
parent = {}
best = None
n = 0
while pq:
    cost, ax, az, th, _, last, par = heapq.heappop(pq)
    k = key(ax, az, th)
    if k in seen: continue
    seen[k] = (ax, az, th, last, par)
    n += 1
    if abs(ax - goalx) < 0.0026 and abs(th) < 0.5:
        best = k; break
    for mv in ("r+", "r-", "f", "b"):
        if mv == "r+": nax, naz, nth, dc = ax, az, th + DTH, DTH / W
        elif mv == "r-": nax, naz, nth, dc = ax, az, th - DTH, DTH / W
        else:
            sg = 1 if mv == "f" else -1
            nax = ax + sg * DS * math.sin(math.radians(th)); naz = az - sg * DS * math.cos(math.radians(th)); nth = th; dc = DS / V
        if abs(nth) > TH_MAX + 1e-9 or not ok(nax, naz, nth): continue
        # 回る途中も確かめる（1° ごとなので端だけで足りる）
        kind = "r" if mv[0] == "r" else mv
        c2 = cost + dc + (SW if (last and kind != last) else 0.0)
        nk = key(nax, naz, nth)
        if nk in seen: continue
        heapq.heappush(pq, (c2, nax, naz, nth, 0.0, kind, k))
if best is None:
    print(f"G {G} MARGIN {MARGIN} BACK {BACK_MAX}: 道なし（調べた {n}）"); sys.exit(1)
path = []
k = best
while k is not None:
    ax, az, th, last, par = seen[k]; path.append((ax, az, th, last)); k = par
path.reverse()
# 区切り
segs = []
for p in path[1:]:
    if not segs or segs[-1][0] != p[3]: segs.append([p[3], p])
    else: segs[-1].append(p)
fwd = -min(p[1] for p in path); back = max(p[1] for p in path)
fz_min = min(min(q[1] for q in corners(*p[:3])[0]) for p in path)
rg = min(rail_gap(*p[:3]) for p in path)
thmax = max(abs(p[2]) for p in path)
T = 0; prev = None
for s in segs:
    kind = s[0]; a = s[1]; b = s[-1]
    T += (len(s) - 1) * (DTH / W if kind == "r" else DS / V)
desc = []
p0 = path[0]
for s in segs:
    b = s[-1]
    desc.append(f"{ {'r':'回る','f':'前へ','b':'後ろへ'}[s[0]] }→(x {b[0]:+.3f}, z {b[1]:+.3f}, 向き {b[2]:+.0f}°)")
res = {"G": G, "margin": MARGIN, "back_max": BACK_MAX, "turns": len(segs), "time_s": round(T + 0.8 * (len(segs) - 1), 2), "fwd_axle_m": round(fwd, 3), "back_axle_m": round(back, 3),
       "footrest_to_robot_min_m": round(G + fz_min, 3), "rail_gap_min_m": round(rg, 3), "heading_max_deg": thmax, "segs": desc,
       "path": [[round(p[0], 4), round(p[1], 4), p[2], p[3]] for p in path]}
print(json.dumps({k: v for k, v in res.items() if k != "path"}, ensure_ascii=False))
if len(sys.argv) > 4: json.dump(res, open(sys.argv[4], "w"), ensure_ascii=False)
