"""依頼233：寄せて通る形があるかの計算（真上の2次元・長方形で近似）。
車いす（根から x ±0.326・前後 −0.430〜+0.640。229 の Unity で測った範囲）、大城さん（根は車いすの根の後ろ 0.594・体 幅 ±0.238・奥行 ±0.157）、
ハコブくん（根から x ±0.330・前後 −0.380〜+0.382、寄った位置 x −0.4643 で止まる。229）。
車いすは 229 の寄せ方の決まり（前へ進みながら・向き 8° まで・横の速さ 0.12 m/秒まで・寄せ終わりで向き 0°）で、廊下の真ん中 x 0.00 から寄せる。
向きは道に沿って 0 → 最大 → 0（変わる所の長さ a）。最大 8°・a を変えて、寄せ終わりまでに前へ進む長さと、ハコブくんとのすき間の最小を出す"""
import math, sys
import numpy as np
RX = -0.4643; RHX = 0.330; RF, RB = 0.382, 0.380
CX0, CX1, CF, CB = -0.326, 0.326, 0.640, 0.430
def rect_pts(cx, cz, yaw, x0, x1, f, b, n=12):
    # 前＝−z。yaw は +x へ向く角度
    s, c = math.sin(yaw), math.cos(yaw)
    pts = []
    for i in range(n + 1):
        for (lx, lu) in ((x0 + (x1 - x0) * i / n, f), (x0 + (x1 - x0) * i / n, -b), (x0, -b + (f + b) * i / n), (x1, -b + (f + b) * i / n)):
            pts.append((cx + lx * c + lu * s, cz - lu * c + lx * s))
    return pts
def dist_robot(pts, rz):
    best = 9.0
    for (x, z) in pts:
        dx = max(RX - RHX - x, 0, x - (RX + RHX)); dz = max(rz - RB - z, 0, z - (rz + RF))
        inside = dx == 0 and dz == 0
        d = -1.0 if inside else math.hypot(dx, dz)
        best = min(best, d)
    return best
def run(DX, ang_deg, a, G, ds=0.002):
    rz = 0.0; zfront = rz + RF + G           # 足のせの前の端
    root_z0 = zfront + CF
    tmax = math.tan(math.radians(ang_deg))
    # 向き θ(s)：0→ang（長さ a）・平ら（長さ p）・ang→0（長さ a）。横の動き＝∫tanθ ds = tmax の近似で解く
    # p を二分法で
    def lat(p):
        x = 0.0; s = 0.0; L = 2 * a + p
        while s < L:
            th = math.radians(ang_deg) * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a))
            x += math.tan(th) * ds; s += ds
        return x
    lo, hi = 0.0, 20.0
    if lat(0.0) > DX:   # 平らが無くても寄りすぎ → 角度を下げる代わりに a を縮めずにそのまま（報告用に記す）
        hi = 0.0
    for _ in range(40):
        mid = 0.5 * (lo + hi)
        if lat(mid) < DX: lo = mid
        else: hi = mid
    p = hi; L = 2 * a + p
    x = 0.0; s = 0.0; gmin = 9.0; gmin_s = 0; x_at = []
    while s <= L + 1e-9:
        th = math.radians(ang_deg) * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a))
        # 車軸は根の後ろ 0.15。根の位置（x, z）
        ax_z = root_z0 + 0.15 - s; ax_x = x
        rx_ = ax_x + 0.15 * math.sin(th); rz_ = ax_z - 0.15 * math.cos(th)
        pts = rect_pts(rx_, rz_, th, CX0, CX1, CF, CB)
        # 大城さん
        ox = rx_ - 0.594 * math.sin(th); oz = rz_ + 0.594 * math.cos(th)
        pts += rect_pts(ox, oz, th, -0.238, 0.238, 0.157, 0.157, 6)
        g = dist_robot(pts, rz)
        if g < gmin: gmin, gmin_s = g, s
        x += math.tan(th) * ds; s += ds
    return L, gmin, gmin_s
out = []
P = lambda t: (print(t), out.append(t))
P("寄せる横の長さ ΔX と、向きの最大 8° で前へ進む長さの下限（向きが一瞬で変わるとしても）= ΔX / tan 8°")
for DX, why in ((0.42, "229 と同じ x +0.42 へ"), (0.346, "寄った位置のハコブくんと 0.15 m あくのに要る最小（229：+0.42 で 0.224 m）"), (0.4805, "ふだんの位置 x −0.33 のハコブくんと 0.15 m あくのに要る最小（+0.42 で 0.0895 m）")):
    P(f"  ΔX {DX:.4f}（{why}）：{DX / math.tan(math.radians(8)):.3f} m")
P("止まった時の足のせの前の端とハコブくんの前の端のあいだ G（229 の決まりで止まると 2.30 m。できるだけ守る 2.0〜3.0 m）")
P("向きの変わる所の長さ a・前へ進む長さ L・ハコブくん（x −0.4643 で止まったまま）と車いす/大城さんのすき間の最小（負は重なる）")
for G in (2.0, 2.3, 3.0):
    for DX in (0.42, 0.346):
        for a in (0.3, 0.6, 1.0):
            L, g, gs = run(DX, 8.0, a, G)
            P(f"  G {G:.1f}・ΔX {DX:.3f}・a {a:.1f}：前へ {L:.3f} m 進む（足のせの前の端がハコブくんの前の端を {L - G:+.3f} m 越える）・すき間の最小 {g:.3f} m（前へ {gs:.2f} m の所）")
open(sys.argv[1] if len(sys.argv) > 1 else "/tmp/c233/geo/feas233.txt", "w").write("\n".join(out) + "\n")

# ---------------------------------------------------------------- 依頼233 の置き方（寺島さんは +x 側）と、寄せる所のくわしい値
P("")
P("■ 止まった所の置き方（車いす x 0.00・向き 0°・大城さん x 0.00、寺島さんは大城さんの +x 側で −x を向く。寸法は 231 の terashima231.txt）")
RAIL = 0.84; NEED = 0.053 + 0.639 + 0.15
O_HW, O_HD = 0.2381, 0.157
T_F, T_B, T_HW = 0.1499, 0.1399, 0.2381
for g in (0.10, 0.15):
    xT = O_HW + g + T_F
    P(f"  寺島さんと大城さんのすき間 {g:.2f} m に置くと：寺島さんの根 x {xT:+.3f}・背中 x {xT + T_B:+.3f}（手すりと {RAIL - xT - T_B:.3f} m）")
P(f"  車いすの所：−x 側 {(-0.326) + RAIL:.3f} m・+x 側 {RAIL - 0.326:.3f} m／大城さん・寺島さんの所：−x 側（大城さんと手すり）{RAIL - O_HW:.3f} m・+x 側（寺島さんの背中と手すり）0.162〜0.212 m")
P(f"  ハコブくんが手すりの側を通るのに要る幅 {NEED:.3f} m（231 と同じ）→ どこも狭く、通れない")
P(f"  寺島さんが後ろから追いついて横に並ぶ時（前を向いて歩く・幅 {2*T_HW:.3f} m）：大城さんの +x の端 {O_HW:.3f} と手すり {RAIL:.2f} の間 {RAIL - O_HW:.3f} m → 真ん中を歩いて両側 {(RAIL - O_HW - 2*T_HW) / 2:.3f} m ずつ")
P(f"  寺島さんの横（大城さんと同じ z）で、大城さんが寺島さんの顔を見るのに要る顔の向き：約 {math.degrees(math.atan2(O_HW + 0.15 + T_F, -0.09)) :.0f}°（228・229 の大城さんの顔の向きの最大 34°。大城さんの顔は Head の骨だけで回す作り）")

P("")
P("■ その場で寄せる形（依頼の形：車いすの前に空いている所の中で寄せ、寺島さんが下がって大城さんの寄る所を空ける）")
for G in (2.3, 3.0):
    for a in (0.0, 0.6):
        D = G - 0.15
        lat = math.tan(math.radians(8)) * max(0.0, D - a)
        P(f"  足のせとハコブくんのあいだ {G:.1f} m・前へ {D:.2f} m まで（ハコブくんの前の端と 0.15 m）・向きの変わる所 {a:.1f} m：寄せられる横の長さ {lat:.3f} m"
          f"（x +0.42 まで {0.42 - lat:.3f} m 足りない・寄った位置のハコブくんと 0.15 m あく x +0.346 まで {max(0.0, 0.346 - lat):.3f} m 足りない）")

P("")
P("■ 前へ進んでハコブくんの横まで行く形（229 の寄せ方の決まりで、当たらずに寄せられる形）で、寺島さんが動かない時の大城さんとのすき間")
def tera_gap(DX, a, G, xT, zrel):
    rz = 0.0; zfront = rz + RF + G; root_z0 = zfront + CF; oz0 = root_z0 + 0.594
    tz = oz0 + zrel
    tpts = rect_pts(xT, tz, -math.pi / 2, -T_HW, T_HW, T_F, T_B, 8)
    ds = 0.002; tmax = math.radians(8.0)
    def lat(p):
        x = 0.0; s = 0.0; L = 2 * a + p
        while s < L:
            th = tmax * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a)); x += math.tan(th) * ds; s += ds
        return x
    lo, hi = 0.0, 20.0
    for _ in range(40):
        mid = 0.5 * (lo + hi)
        if lat(mid) < DX: lo = mid
        else: hi = mid
    p = hi; L = 2 * a + p; x = 0.0; s = 0.0; best = 9.0
    while s <= L:
        th = tmax * (min(1, s / a) if s < a else 1 if s < a + p else max(0, (L - s) / a))
        ax_z = root_z0 + 0.15 - s; rx_ = x + 0.15 * math.sin(th); rz_ = ax_z - 0.15 * math.cos(th)
        ox = rx_ - 0.594 * math.sin(th); oz = rz_ + 0.594 * math.cos(th)
        opts = rect_pts(ox, oz, th, -O_HW, O_HW, O_HD, O_HD, 6) + rect_pts(rx_, rz_, th, CX0, CX1, CF, CB)
        for (px_, pz_) in opts:
            # 寺島さんの長方形（−x を向く）の中か・距離
            lx = -(pz_ - tz); lu = -(px_ - xT)   # 寺島さんの枠：横＝z・前＝−x
            dx = max(-T_HW - lx, 0, lx - T_HW); du = max(-T_B - lu, 0, lu - T_F)
            best = min(best, -1.0 if dx == 0 and du == 0 else math.hypot(dx, du))
        x += math.tan(th) * ds; s += ds
    return best
for zrel in (0.0, -0.2):
    g = tera_gap(0.42, 0.6, 2.3, O_HW + 0.15 + T_F, zrel)
    P(f"  寺島さん x {O_HW + 0.15 + T_F:+.3f}・大城さんより前へ {abs(zrel):.1f} m：下がらなくても大城さん・車いすとのすき間の最小 {g:.3f} m（大城さんは前へ離れていく）→ 下がる距離 0 m で当たらない")
open(sys.argv[1] if len(sys.argv) > 1 else "/tmp/c233/geo/feas233.txt", "w").write("\n".join(out) + "\n")
