"""Run on models/M_Ship_v2_interior_season2.blend (read only, never saved). The Mako on the carousel and in the moon pool:
  * the carousel sweep with three stowed Tenders (2 degree steps), the same way vessel-interior/build/check_interior.py measures
    it for the Kazah (its margins table against "Moonpool | hull lining", roof by ceil_z), for the Kazah (to compare) and the Mako;
  * the Mako at BOARD on the launch arm (origin (3.015, -3.66, -2.16), turned 90 deg) against the deck well's lip: least
    distance in plan, which the user set at 1 m."""
import bpy, sys, os, math, json
import numpy as np
HERE = os.path.dirname(os.path.abspath(__file__))
VI = os.path.normpath(os.path.join(HERE, "..", "..", "vessel-interior", "build"))
sys.path.insert(0, VI)
import shipfit
fit = shipfit.Fit(["M_SHIP_Hull", "Moonpool | hull lining", "M_SHIP_Dorsal_Roof", "M_SHIP_Canopy"])
LINING = ("Moonpool | hull lining",)
OUTER = ("M_SHIP_Hull", "M_SHIP_Dorsal_Roof", "M_SHIP_Canopy")
X_AFT, X_FORE = -9.99, 15.51
XS = np.arange(-60.0, 60.01, 0.5); ZS = np.arange(-11.0, 11.01, 0.25)
def table(only):
    lo = np.full((len(XS), len(ZS)), np.nan); hi = np.full((len(XS), len(ZS)), np.nan)
    for i, x in enumerate(XS):
        if not (-14 < x < 20):
            continue
        for j, z in enumerate(ZS):
            a = fit.side_y(float(x), float(z), -1, only); b = fit.side_y(float(x), float(z), 1, only)
            if a is not None and b is not None and a < 0 < b:
                lo[i, j], hi[i, j] = a, b
    return lo, hi
LO_H, HI_H = table(OUTER)
LO_L, HI_L = table(LINING)
def margins(p):
    x, y, z = p[:, 0], p[:, 1], p[:, 2]
    fi = np.clip((x - XS[0]) / 0.5, 0, len(XS) - 1.001); fj = np.clip((z - ZS[0]) / 0.25, 0, len(ZS) - 1.001)
    i0, j0 = fi.astype(int), fj.astype(int); tx, tz = fi - i0, fj - j0
    in_bay = (x > X_AFT + 0.55) & (x < X_FORE - 0.55)
    def lerp(t):
        return ((t[i0, j0] * (1 - tx) + t[i0 + 1, j0] * tx) * (1 - tz) + (t[i0, j0 + 1] * (1 - tx) + t[i0 + 1, j0 + 1] * tx) * tz)
    lo = np.where(in_bay, lerp(LO_L), lerp(LO_H)); hi = np.where(in_bay, lerp(HI_L), lerp(HI_H))
    return np.minimum(y - lo, hi - y)
arm_root = next(o for o in bpy.data.objects if o.name.startswith("DockArm_Root"))
hub = np.array(arm_root.matrix_world.translation)
def kazah_pts():
    t = sorted([o for o in bpy.data.objects if o.name.startswith("_Tender")], key=lambda o: o.name)[0]
    out = []
    for o in t.instance_collection.objects:
        if o.type == "MESH":
            M = np.array(o.matrix_world); co = np.empty(len(o.data.vertices) * 3); o.data.vertices.foreach_get("co", co)
            out.append((co.reshape(-1, 3) @ M[:3, :3].T + M[:3, 3])[::3])
    return np.concatenate(out)
def sweep(P):
    res = {"side": 9.0, "roof": 9.0, "over_deck": 9.0, "aft_bulkhead": 9.0, "fore_bulkhead": 9.0}
    where = {}
    for ang in range(0, 360, 2):
        for k in range(3):
            th = math.radians(-90.0 - 120.0 * k + ang); rz = math.radians(90.0 - 120.0 * k + ang)
            c, s = math.cos(rz), math.sin(rz)
            p = P @ np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]).T + hub + np.array([6.0 * math.cos(th), 6.0 * math.sin(th), -1.0])
            m = margins(p); mm = float(np.nanmin(m))
            if mm < res["side"]:
                ii = int(np.nanargmin(m))
                res["side"] = mm; where["side"] = [ang, k + 1, [round(float(v), 2) for v in p[ii]], [round(float(v), 3) for v in P[ii]]]
            for q in p[np.argsort(p[:, 2])[-40:]][::8]:
                cz = fit.ceil_z(float(q[0]), float(q[1]), float(q[2]) - 0.01, LINING)
                if cz is not None and cz - q[2] < res["roof"]:
                    res["roof"] = float(cz - q[2]); where["roof"] = [ang, k + 1, [round(float(v), 2) for v in q]]
            res["over_deck"] = min(res["over_deck"], float(p[:, 2].min() + 3.36))
            res["aft_bulkhead"] = min(res["aft_bulkhead"], float(p[:, 0].min() - (X_AFT + 0.30)))
            res["fore_bulkhead"] = min(res["fore_bulkhead"], float((X_FORE - 0.30) - p[:, 0].max()))
    return {k: round(v, 3) for k, v in res.items()}, where
man = np.load(os.path.join(HERE, "checks", "mako_points.npy"))                      # arms stowed, hatch shut
man_f = man
out_name = "sweep_and_moonpool.json"
out = {"hub": [round(float(v), 3) for v in hub]}
kz = kazah_pts()
out["kazah"] = sweep(kz)
out["mako"] = sweep(man_f)

for f in (1.05, 1.10, 1.15):
    out["mako_x%.2f_larger" % f] = sweep(man_f * f)[0]
# moon pool at BOARD
lip = bpy.data.objects["Moonpool | inner black lip"]; sp = lip.data.splines[0]
poly = np.array([(lip.matrix_world @ p.co.to_3d())[:2] for p in sp.points])
def signed(px, py):
    best, inside = 1e9, False
    n = len(poly)
    for i in range(n):
        ax, ay = poly[i]; bx, by = poly[(i + 1) % n]
        dx, dy = bx - ax, by - ay; l2 = dx * dx + dy * dy
        t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / l2))
        best = min(best, math.hypot(px - ax - t * dx, py - ay - t * dy))
        if (ay > py) != (by > py) and px < ax + (py - ay) * dx / dy:
            inside = not inside
    return best if inside else -best
def plan_fit(P):
    w = P @ np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]).T + np.array([3.015, -3.66, -2.16])
    # the outline in plan: extreme points per 5 cm band in x and in y
    keep = set()
    for ax in (0, 1):
        b = np.round(w[:, ax] / 0.05).astype(int)
        for key in np.unique(b):
            idx = np.where(b == key)[0]
            o = 1 - ax
            keep.add(int(idx[np.argmin(w[idx, o])])); keep.add(int(idx[np.argmax(w[idx, o])]))
    best = (9.0, None)
    for i in keep:
        d = signed(w[i, 0], w[i, 1])
        if d < best[0]:
            best = (d, [round(float(v), 3) for v in w[i]], [round(float(v), 3) for v in P[i]])
    return {"least_m": round(best[0], 3), "world_point": best[1], "tender_point": best[2], "lowest_z_at_board": round(float(w[:, 2].min()), 3)}
out["moonpool_board_mako"] = plan_fit(man)
out["moonpool_board_kazah"] = plan_fit(kz)
json.dump(out, open(os.path.join(HERE, "checks", out_name), "w"), indent=1)
print("SWEEP", json.dumps(out))