"""Design fit (pure Python): place the hanging arm for one door and score it.
Parameters: s (m per ref1 px), u (where on the door's inner face, 0 top .. 1 bottom), px (door-local x of the attach
point, AFT sense; mirrored for FORE), cle (extra carriage clearance at the end wall), split (how the carriage's shortfall
is shared: 'prop' = carriage runs at T/6.14 of the door's speed; a number k in 0..1 = carriage follows the door 1:1 and
stops at its ends, k of the shortfall taken at the closed end, 1-k at the open end).
"""
import json, math, os, sys
HERE = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, HERE)
import design_kin as K

ctx = json.load(open(os.path.join(HERE, "design_context.json")))
ctx2 = json.load(open(os.path.join(HERE, "design_context2.json")))
INNER = sorted({(f[0], f[1], f[2], f[3]) for f in ctx2["door_inner_faces_x0"]})
INNER = [f for f in INNER if f[3] > 0.3]
TRACK = {n: [tuple(r[1:4]) for r in ctx["door_track"][n]] for n in ctx["door_track"]}
DECK_Z = -3.755
RAIL = dict(y0=-6.0238, y1=-5.5662, zb=-4.01, zt=-3.6745, x_in=-2.5453, x_wall=-9.0348, fx_in=8.6453, fx_wall=15.1348)
MIRROR_X = 3.05          # the FORE side is the AFT side mirrored in x = 3.05
lin = [p for s in ctx["sections"]["Moonpool | hull lining"]["-5.0"] for p in s]
LIN = sorted([tuple(p) for p in lin if p[1] > -9.0 and p[0] < -5.0], key=lambda p: -p[1])

# rest-pose geometry of the links for the coarse clearance model, ref1 pixels in the arm frame (forward, up), radius px
# turret: points on the J1 axis plane; lower arm: J2 -> knee -> J3; upper arm: J3 -> WC with the truss knob
CAPS_PX = {
    "turret": [((-150, 120), (230, 120), 175), ((-150, 280), (200, 280), 160)],
    "drum": [((-155, 235), (-155, 235), 150)],
}


def seg_dist2d(p, a, b):
    ax, ay = a; bx, by = b; px, py = p
    dx, dy = bx - ax, by - ay
    L2 = dx * dx + dy * dy
    t = 0 if L2 == 0 else max(0, min(1, ((px - ax) * dx + (py - ay) * dy) / L2))
    return math.hypot(px - ax - t * dx, py - ay - t * dy)


def door_face_dist(yz):
    best = 1e9; sgn = 1
    for i in range(len(INNER) - 1):
        a, b = INNER[i][:2], INNER[i + 1][:2]
        d = seg_dist2d(yz, a, b)
        if d < best:
            best = d
            n = (INNER[i][2], INNER[i][3])
            sgn = 1 if (yz[0] - a[0]) * n[0] + (yz[1] - a[1]) * n[1] > 0 else -1
    return best * sgn


def lining_dist(yz):
    return min(seg_dist2d(yz, LIN[i], LIN[i + 1]) for i in range(len(LIN) - 1))


def face_point(u):
    f = u * (len(INNER) - 1)
    i = min(int(f), len(INNER) - 2)
    t = f - i
    a, b = INNER[i], INNER[i + 1]
    y = a[0] + t * (b[0] - a[0]); z = a[1] + t * (b[1] - a[1])
    ny = a[2] + t * (b[2] - a[2]); nz = a[3] + t * (b[3] - a[3])
    n = math.hypot(ny, nz)
    return (y, z), (ny / n, nz / n)


def tool_len(s, version):
    """WC -> door face along the approach axis"""
    return (60 + 75 + 115) * s + 0.08          # both versions: B's neck is long enough to share A's wrist centre


def carriage(s):
    Lc = 390 * s
    Wc = max(390 * s, 0.458 + 2 * 0.12)
    Hc = max(185 * s, 0.255 + 0.13)
    return Lc, Wc, Hc


def setup(s, u, px, cle, split, door, version, roll=None, branch=None):
    side = +1 if door.endswith("Aft") else -1
    Lc, Wc, Hc = carriage(s)
    zb = -3.775 - Hc
    yb = (RAIL["y0"] + RAIL["y1"]) / 2
    tr = TRACK[door]
    xs = [p[0] for p in tr]
    x_cl = tr[0][0]
    x_op = min(xs) if side > 0 else max(xs)
    # carriage travel (AFT sense, then mirror)
    hi = RAIL["x_in"] - 0.04 - Lc / 2
    lo = RAIL["x_wall"] + 0.04 + Lc / 2 + cle
    if side < 0:
        hi, lo = 2 * MIRROR_X - hi, 2 * MIRROR_X - lo
    (fy, fz), (ny, nz) = face_point(u)
    d_t = tool_len(s, version)
    xt = (0.0, -ny, -nz)
    return dict(side=side, Lc=Lc, Wc=Wc, Hc=Hc, zb=zb, yb=yb, tr=tr, x_cl=x_cl, x_op=x_op, c_hi=hi, c_lo=lo, T=abs(hi - lo),
                face=(fy, fz), n=(ny, nz), d_t=d_t, xt=xt, pxl=px * side, split=split, s=s, version=version)


def carriage_x(S, dx):
    """carriage position for door x"""
    x_cl, x_op, hi, lo = S["x_cl"], S["x_op"], S["c_hi"], S["c_lo"]
    if S["split"] == "prop":
        return lo + (dx - x_op) * (hi - lo) / (x_cl - x_op)
    k = S["split"]
    excess = abs(x_cl - x_op) - S["T"]
    sd = 1 if x_cl > x_op else -1
    # carriage = door + offset, clamped; offset puts (1-k)*excess at the open end
    off = lo - x_op - sd * (1 - k) * excess
    c = dx + off
    return min(max(c, min(lo, hi)), max(lo, hi))


def tool_axes(S, roll):
    xt = S["xt"]
    a = (1.0, 0.0, 0.0)
    b = K.cross(xt, a)
    yt = tuple(math.cos(roll) * a[i] + math.sin(roll) * b[i] for i in range(3))
    zt = K.cross(xt, yt)
    return xt, yt, zt


def run(S, roll=0.0, branch=+1, frames=None):
    arm = K.Arm(S["s"])
    xt, yt, zt = tool_axes(S, roll)
    tool_c = (K.w2c(xt), K.w2c(yt), K.w2c(zt))
    out = []
    for f, (dx, dy, dz) in enumerate(S["tr"], start=1):
        if frames and f not in frames:
            continue
        c = carriage_x(S, dx)
        P = (dx + S["pxl"], dy + S["face"][0], dz + S["face"][1])
        WC = tuple(P[i] - S["d_t"] * xt[i] for i in range(3))
        wc_c = K.w2c((WC[0] - c, WC[1] - S["yb"], WC[2] - S["zb"]))
        q = K.solve(arm, wc_c, tool_c, branch5=branch)
        out.append(dict(f=f, c=c, P=P, WC=WC, q=q, door=(dx, dy, dz)))
    return out


def world_pts(S, rec):
    """coarse capsules of the posed arm in world: list of (name, a, b, r)"""
    s = S["s"]
    arm = K.Arm(s)
    q = rec["q"]
    base = (rec["c"], S["yb"], S["zb"])
    c1, s1 = math.cos(q["q1"]), math.sin(q["q1"])
    W = lambda pl: tuple(base[i] + K.c2w((pl[0] * c1, pl[0] * s1, pl[1]))[i] for i in range(3))
    Wv = lambda v: tuple(base[i] + K.c2w(v)[i] for i in range(3))
    caps = []
    for nm, lst in CAPS_PX.items():
        for (a, b, r) in lst:
            caps.append((nm, W((a[0] * s, a[1] * s)), W((b[0] * s, b[1] * s)), r * s))
    phi_l, phi_f = q["phi_l"], q["phi_f"]
    t, kk, L = arm.rod(phi_l, phi_f)
    j3, wc = arm.planar_fk(phi_l, phi_f)
    caps.append(("lower arm", W(arm.j2), W(t), 125 * s))
    caps.append(("lower arm", W(t), W(j3), 115 * s))
    ef = (math.cos(phi_f), math.sin(phi_f))
    efp = (-ef[1], ef[0])
    # upper arm housing: J3 up to the big cap and along the forearm axis
    cap_big = K.add(j3, K.add(K.mul(ef, 290 * s), K.mul(efp, 150 * s)))
    caps.append(("upper arm", W(j3), W(cap_big), 105 * s))
    fa0 = K.add(j3, K.add(K.mul(ef, 300 * s), K.mul(efp, 107 * s)))
    caps.append(("forearm", W(fa0), W(K.add(wc, K.mul(ef, -60 * s))), 95 * s))
    caps.append(("truss", W(j3), W(kk), 45 * s))
    caps.append(("rod", W(t), W(kk), 18 * s))
    F = K.fk(arm, q)
    WCw = Wv(F["WC"])
    xt = K.c2w(K.col(F["R6"], 0))
    d_t = S["d_t"]
    tip = tuple(WCw[i] + xt[i] * (d_t - 0.12) for i in range(3))
    caps.append(("wrist+tool", WCw, tip, (70 if S["version"] == "A" else 60) * s))
    return caps


def clearance(S, rec):
    """coarse clearances (m) of the capsules to the deck, the rail, the end wall, the lining, the door; min and where"""
    best = (9.0, "")
    dx, dy, dz = rec["door"]
    for nm, a, b, r in world_pts(S, rec):
        for k in range(9):
            t = k / 8.0
            p = tuple(a[i] + t * (b[i] - a[i]) for i in range(3))
            v = DECK_Z - p[2] - r
            if v < best[0]:
                best = (v, nm + " deck")
            # rail (box) under the deck: only when the capsule is near the rail in y
            ry = max(RAIL["y0"] - p[1], 0, p[1] - RAIL["y1"])
            rz = max(RAIL["zb"] - p[2], 0)
            in_x = (RAIL["x_wall"] - 0.1 < p[0] < RAIL["x_in"]) if S["side"] > 0 else (RAIL["fx_in"] < p[0] < RAIL["fx_wall"] + 0.1)
            if in_x and nm not in ("turret",):
                v = math.hypot(ry, rz) - r
                if v < best[0]:
                    best = (v, nm + " rail")
            wall = RAIL["x_wall"] if S["side"] > 0 else RAIL["fx_wall"]
            v = (p[0] - wall) * S["side"] - r
            if v < best[0]:
                best = (v, nm + " wall")
            in_pocket = p[0] < -3.1 if S["side"] > 0 else p[0] > 9.2
            if in_pocket:
                v = lining_dist((p[1], p[2])) - r
                if v < best[0]:
                    best = (v, nm + " lining")
            if nm != "wrist+tool":
                v = door_face_dist((p[1] - dy, p[2] - dz)) - r
                if v < best[0]:
                    best = (v, nm + " door")
    return best


def assess(S, roll=0.0, branch=+1):
    recs = run(S, roll, branch)
    arm = K.Arm(S["s"])
    m = dict(fail=None, q={k: [9, -9] for k in ("q1", "q2", "q3", "q4", "q5", "q6")}, elb=[9, -9], minq5=9, rod=[9, -9], cl=(9, ""), recs=recs,
             wc_axis=9, c=[99, -99])
    prev = None
    for r in recs:
        q = r["q"]
        if q is None:
            m["fail"] = "unreachable f%d" % r["f"]
            return m
        if prev:
            for k in ("q1", "q4", "q6"):
                if abs(q[k] - prev[k]) > 0.35:
                    m["fail"] = "jump %s f%d" % (k, r["f"])
                    return m
        prev = q
        for k in m["q"]:
            m["q"][k] = [min(m["q"][k][0], q[k]), max(m["q"][k][1], q[k])]
        m["elb"] = [min(m["elb"][0], q["elbow_inner"]), max(m["elb"][1], q["elbow_inner"])]
        m["minq5"] = min(m["minq5"], abs(q["q5"]))
        m["wc_axis"] = min(m["wc_axis"], q["rho"])
        t, kk, L = arm.rod(q["phi_l"], q["phi_f"])
        m["rod"] = [min(m["rod"][0], L), max(m["rod"][1], L)]
        m["c"] = [min(m["c"][0], r["c"]), max(m["c"][1], r["c"])]
    return m


def full(S, roll=0.0, branch=+1, step=3):
    m = assess(S, roll, branch)
    if m["fail"]:
        return m
    for r in m["recs"][::step]:
        c = clearance(S, r)
        if c[0] < m["cl"][0]:
            m["cl"] = (c[0], c[1] + " f%d" % r["f"])
    return m