"""Plot poses of a design_fit candidate with the coarse capsules: y-z (along x) and x-y (plan) views."""
import math, sys, os, subprocess
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
import design_fit as F
import design_kin as K
MAGICK = r"E:\Tools\ImageMagick-7.1.2-Q16-HDRI\magick.exe"
s, u, px, cle = map(float, sys.argv[1:5]); split = sys.argv[5]; split = split if split == "prop" else float(split)
version, door, pref = sys.argv[6], sys.argv[7], sys.argv[8]
frames = [int(v) for v in sys.argv[9].split(",")] if len(sys.argv) > 9 else [1, 6, 12, 30, 48]
S = F.setup(s, u, px, cle, split, door, version)
br = +1
m = F.assess(S, 0.0, br)
if m["fail"]:
    br = -1; m = F.assess(S, 0.0, br)
print("fail", m["fail"], "branch", br)
recs = [r for r in m["recs"] if r["f"] in frames]
cols = ["#d33", "#e80", "#0a0", "#08c", "#a0a", "#555", "#0aa"]
def view(fn, proj, rng, title):
    (u0, u1), (v0, v1) = rng
    W, H = 1400, int(1400 * (v1 - v0) / (u1 - u0))
    sc = W / (u1 - u0)
    X = lambda a: (a - u0) * sc
    Y = lambda b: (v1 - b) * sc
    L = ['<svg xmlns="http://www.w3.org/2000/svg" width="%d" height="%d" style="background:#fff">' % (W, H), '<text x="10" y="20" font-size="16">%s</text>' % title]
    return L, X, Y
# y-z
L, X, Y = view(None, None, ((-10.0, -1.0), (-10.6, -3.2)), "y-z view (looking +x), %s door %s" % (version, door))
L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="#ccc"/>' % (X(-10), Y(-3.355), 9 * (X(1) - X(0)), (Y(-3.755) - Y(-3.355))))
L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="#6a6"/>' % (X(-6.024), Y(-3.675), X(-5.566) - X(-6.024), Y(-4.01) - Y(-3.675)))
L.append('<polyline points="%s" fill="none" stroke="#888" stroke-width="2"/>' % " ".join("%f,%f" % (X(p[0]), Y(p[1])) for p in F.LIN))
for i, r in enumerate(recs):
    col = cols[i % len(cols)]
    dx, dy, dz = r["door"]
    L.append('<polyline points="%s" fill="none" stroke="%s" stroke-width="1.5" stroke-dasharray="5,3"/>' % (" ".join("%f,%f" % (X(f[0] + dy), Y(f[1] + dz)) for f in F.INNER), col))
    for nm, a, b, rad in F.world_pts(S, r):
        L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="%s" stroke-opacity="0.28" stroke-width="%f" stroke-linecap="round"/>' % (X(a[1]), Y(a[2]), X(b[1]), Y(b[2]), col, 2 * rad * (X(1) - X(0))))
        L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="%s" stroke-width="1.5"/>' % (X(a[1]), Y(a[2]), X(b[1]), Y(b[2]), col))
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="none" stroke="%s" stroke-width="2"/>' % (X(S["yb"] - S["Wc"] / 2), Y(-3.775), S["Wc"] * (X(1) - X(0)), Y(S["zb"]) - Y(-3.775), col))
    L.append('<text x="%f" y="%f" font-size="15" fill="%s">f%d</text>' % (X(r["WC"][1]) - 30, Y(r["WC"][2]) + 18, col, r["f"]))
    L.append('<circle cx="%f" cy="%f" r="4" fill="%s"/>' % (X(r["P"][1]), Y(r["P"][2]), col))
L.append('</svg>')
open(pref + "_yz.svg", "w").write("\n".join(L))
# plan x-y (+y up the page)
side = S["side"]
xr = (-9.6, -0.4) if side > 0 else (6.5, 15.7)
L, X, Y = view(None, None, (xr, (-10.0, -2.0)), "plan (x right, +y up), %s door %s" % (version, door))
for xa, xb in ((-9.1035, -2.5453), (8.6453, 15.2035)):
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="#6a6" fill-opacity="0.5"/>' % (X(xa), Y(-5.5662), X(xb) - X(xa), Y(-6.0238) - Y(-5.5662)))
for xw in (-9.0348, 15.1348):
    L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="#000" stroke-width="4"/>' % (X(xw), Y(-10), X(xw), Y(-2)))
L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="#999" stroke-dasharray="6,4"/>' % (X(-3.15), Y(-10), X(-3.15), Y(-2)))
L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="#999" stroke-dasharray="6,4"/>' % (X(9.25), Y(-10), X(9.25), Y(-2)))
for i, r in enumerate(recs):
    col = cols[i % len(cols)]
    dx, dy, dz = r["door"]
    x0, x1 = (dx - 2.997, dx + 2.825) if side > 0 else (dx - 2.825, dx + 2.997)
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="none" stroke="%s" stroke-dasharray="4,3"/>' % (X(x0), Y(dy + 2.557), X(x1) - X(x0), Y(dy - 2.87) - Y(dy + 2.557), col))
    for nm, a, b, rad in F.world_pts(S, r):
        L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="%s" stroke-opacity="0.28" stroke-width="%f" stroke-linecap="round"/>' % (X(a[0]), Y(a[1]), X(b[0]), Y(b[1]), col, 2 * rad * (X(1) - X(0))))
        L.append('<line x1="%f" y1="%f" x2="%f" y2="%f" stroke="%s" stroke-width="1.5"/>' % (X(a[0]), Y(a[1]), X(b[0]), Y(b[1]), col))
    L.append('<rect x="%f" y="%f" width="%f" height="%f" fill="none" stroke="%s" stroke-width="2"/>' % (X(r["c"] - S["Lc"] / 2), Y(S["yb"] + S["Wc"] / 2), S["Lc"] * (X(1) - X(0)), S["Wc"] * (X(1) - X(0)), col))
    L.append('<text x="%f" y="%f" font-size="15" fill="%s">f%d</text>' % (X(r["WC"][0]) + 6, Y(r["WC"][1]) - 6, col, r["f"]))
L.append('</svg>')
open(pref + "_xy.svg", "w").write("\n".join(L))
for k in ("_yz", "_xy"):
    subprocess.run([MAGICK, pref + k + ".svg", pref + k + ".png"])
for r in recs:
    q = r["q"]
    print("f%d c %.3f q1 %.0f q2 %.0f q3 %.0f q4 %.0f q5 %.0f q6 %.0f elb %.0f WC %s" % (r["f"], r["c"], *[math.degrees(q[k]) for k in ("q1", "q2", "q3", "q4", "q5", "q6", "elbow_inner")], tuple(round(v, 2) for v in r["WC"])))