"""Plot candidate poses (pure Python -> SVG -> PNG with ImageMagick): y-z view (looking along x) and x-y plan.
python design_plot.py s u px cle version door out_prefix"""
import json, math, os, sys, subprocess
HERE = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, HERE)
import design_kin as K
import design_explore as E

MAGICK = r"E:\Tools\ImageMagick-7.1.2-Q16-HDRI\magick.exe"


def poses(s, u, px, cle, version, door, frames):
    r = E.evaluate(s, u, px, 0.0, 0.0, cle, door=door, version=version)
    arm = K.Arm(s)
    out = []
    side = +1 if door.endswith("Aft") else -1
    tr = E.TRACK[door]
    xs = [p[0] for p in tr]
    x_cl, x_op = tr[0][0], (min(xs) if side > 0 else max(xs))
    yb = (E.RAIL["y0"] + E.RAIL["y1"]) / 2
    for f in frames:
        q = r["q"][f - 1]
        dx, dy, dz = tr[f - 1]
        c = r["c_lo"] + (dx - x_op) * (r["c_hi"] - r["c_lo"]) / (x_cl - x_op)
        base = (c, yb, r["zb"])
        F = K.fk(arm, q)
        W = lambda v: tuple(base[i] + K.c2w(v)[i] for i in range(3))
        t, kk, L = arm.rod(q["phi_l"], q["phi_f"])
        c1, s1 = math.cos(q["q1"]), math.sin(q["q1"])
        P = lambda pl: W((pl[0] * c1, pl[0] * s1, pl[1]))
        # knee point
        el = (math.cos(q["phi_l"]), math.sin(q["phi_l"]))
        out.append(dict(f=f, q=q, door=(dx, dy, dz), base=base, J1=W((0, 0, 0)), J2=W(F["J2"]), J3=W(F["J3"]), WC=W(F["WC"]),
                        FL=W(F["flange"]), TR=P(t), KN=P(kk), tool=r["d_t"], xt=K.c2w(K.col(F["R6"], 0))))
    return r, out


def svg_yz(r, P, fn, s):
    sc = 90.0
    ox, oy = 1000, 60
    X = lambda y: ox + (y + 5.8) * sc
    Y = lambda z: oy + (-3.3 - z) * sc
    L = ['<svg xmlns="http://www.w3.org/2000/svg" width="1500" height="700" style="background:#fff">']
    # deck, rail
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="#ccc"/>' % (X(-9.5), Y(-3.355), 11 * sc, 0.4 * sc))
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="#6a6"/>' % (X(-6.024), Y(-3.675), 0.458 * sc, 0.335 * sc))
    # lining
    pts = " ".join("%f,%f" % (X(p[0]), Y(p[1])) for p in E.LIN)
    L.append('<polyline points="%s" fill="none" stroke="#888" stroke-width="2"/>' % pts)
    cols = ["#d33", "#e80", "#0a0", "#08c", "#a0a", "#555"]
    for i, p in enumerate(P):
        col = cols[i % len(cols)]
        dx, dy, dz = p["door"]
        door = " ".join("%f,%f" % (X(f[0] + dy), Y(f[1] + dz)) for f in E.INNER)
        L.append('<polyline points="%s" fill="none" stroke="%s" stroke-width="1.5" stroke-dasharray="4,3"/>' % (door, col))
        chain = [p["J1"], p["J2"], p["J3"], p["WC"], p["FL"]]
        L.append('<polyline points="%s" fill="none" stroke="%s" stroke-width="%f" stroke-opacity="0.35"/>' % (" ".join("%f,%f" % (X(c[1]), Y(c[2])) for c in chain[1:4]), col, 200 * s * sc))
        L.append('<polyline points="%s" fill="none" stroke="%s" stroke-width="3"/>' % (" ".join("%f,%f" % (X(c[1]), Y(c[2])) for c in chain), col))
        L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="%s" stroke-width="2" stroke-dasharray="2,2"/>' % (X(p["TR"][1]), Y(p["TR"][2]), X(p["KN"][1]), Y(p["KN"][2]), col))
        tip = tuple(p["WC"][k] + p["xt"][k] * p["tool"] for k in range(3))
        L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="%s" stroke-width="5"/>' % (X(p["WC"][1]), Y(p["WC"][2]), X(tip[1]), Y(tip[2]), col))
        L.append('<text x="%f" y="%f" font-size="16" fill="%s">f%d</text>' % (X(p["WC"][1]) + 5, Y(p["WC"][2]) - 5, col, p["f"]))
        cb = p["base"]
        L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="none" stroke="%s"/>' % (X(cb[1] - 227 * s), Y(-3.775), 454 * s * sc, (-3.775 - cb[2]) * sc, col))
    for z in range(-10, -2):
        L.append('<text x="5" y="%f" font-size="14">z %d</text>' % (Y(z), z))
    for y in range(-9, 2):
        L.append('<text x="%f" y="690" font-size="14">y %d</text>' % (X(y), y))
    L.append('</svg>')
    open(fn, "w").write("\n".join(L))


def svg_xy(r, P, fn, s, door):
    sc = 70.0
    side = +1 if door.endswith("Aft") else -1
    ox = 80 if side > 0 else -600
    X = lambda x: ox + (x + 9.5) * sc
    Y = lambda y: 40 + (y + 10) * sc       # +y down the page (inboard down)
    L = ['<svg xmlns="http://www.w3.org/2000/svg" width="1500" height="900" style="background:#fff">']
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="#6a6"/>' % (X(-9.1035), Y(-6.0238), 6.558 * sc, 0.458 * sc))
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="#6a6"/>' % (X(8.6453), Y(-6.0238), 6.558 * sc, 0.458 * sc))
    L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="#000" stroke-width="3"/>' % (X(-9.035), Y(-9), X(-9.035), Y(1)))
    L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="#000" stroke-width="3"/>' % (X(15.135), Y(-9), X(15.135), Y(1)))
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="none" stroke="#999" stroke-dasharray="6,4"/>' % (X(-3.15), Y(-6.97), 12.4 * sc, 6.61 * sc))
    cols = ["#d33", "#e80", "#0a0", "#08c", "#a0a", "#555"]
    for i, p in enumerate(P):
        col = cols[i % len(cols)]
        dx, dy, dz = p["door"]
        x0, x1 = (dx - 3.0, dx + 2.83) if side > 0 else (dx - 2.83, dx + 3.0)
        L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="none" stroke="%s" stroke-dasharray="3,3"/>' % (X(x0), Y(dy - 2.87), (x1 - x0) * sc, 5.42 * sc, col))
        chain = [p["J1"], p["J2"], p["J3"], p["WC"], p["FL"]]
        L.append('<polyline points="%s" fill="none" stroke="%s" stroke-width="3"/>' % (" ".join("%f,%f" % (X(c[0]), Y(c[1])) for c in chain), col))
        cb = p["base"]
        L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="none" stroke="%s"/>' % (X(cb[0] - 195 * s), Y(cb[1] - 227 * s), 390 * s * sc, 454 * s * sc, col))
        L.append('<text x="%f" y="%f" font-size="14" fill="%s">f%d</text>' % (X(p["WC"][0]) + 4, Y(p["WC"][1]) - 4, col, p["f"]))
    for x in range(-9, 16):
        L.append('<text x="%f" y="890" font-size="12">%d</text>' % (X(x), x))
    L.append('</svg>')
    open(fn, "w").write("\n".join(L))


if __name__ == "__main__":
    s, u, px, cle = map(float, sys.argv[1:5])
    version, door, pref = sys.argv[5], sys.argv[6], sys.argv[7]
    frames = (1, 6, 12, 30, 48)
    r, P = poses(s, u, px, cle, version, door, frames)
    for p in P:
        print("f%d q1 %.0f q2 %.0f q3 %.0f q4 %.0f q5 %.0f q6 %.0f elbow %.0f D %.2f rod %.2f" % (p["f"], *[math.degrees(p["q"][k]) for k in ("q1", "q2", "q3", "q4", "q5", "q6", "elbow_inner")], p["q"]["D"],
              math.dist(p["TR"], p["KN"])))
    svg_yz(r, P, pref + "_yz.svg", s)
    svg_xy(r, P, pref + "_xy.svg", s, door)
    for k in ("_yz", "_xy"):
        subprocess.run([MAGICK, pref + k + ".svg", pref + k + ".png"])