"""
The Tender carousel in the vessel interior, as the rigged model.
(The user, 2026-10-13: "have you updated to the rigged version?", then "Yes, swap it in".)

models/docking-arm holds the carousel twice: DockArm_Carousel.blend (parts parented to each other, the hoists driven
by the custom property lower_m on the empties Arm1..3) and DockArm_Carousel_Rigged.blend (the same meshes skinned to
one armature, DockArm_Rig, a bone on every joint and two Pose Mode handles per arm). The interior was built with
the first one. swap_in_rigged() puts the second one in its place:

  * the collections DockArm_Rigged and DockArm_Widgets are appended from the rigged file;
  * DockArm_Rig is parented to the empty DockArm_Root, which stays where it is (the hub), and keeps the pylon and
    the three preview Tenders as its children, so everything that looks for the hub still finds it;
  * the launch arm is let down to BOARD with its handle: pose bone CTRL_Arm1_Hoist, location Y = 5.3;
  * materials and images that came along a second time (M_DockArm_....001) are dropped in favour of the ones the file
    already has, which the user may have tuned;
  * the unrigged parts are removed (33 objects; they are in the backup and in models/docking-arm).

To pose it in Blender: select DockArm_Rig, Pose Mode. The square outboard of each head (CTRL_ArmN_Hoist) moves down
0..5.3 m and unfolds the arm; the ring beside it (CTRL_ArmN_Claws) opens the claws; the bone Turret turns the carousel.
"""
import math
import os

import bpy
from mathutils import Matrix, Vector

HERE = os.path.dirname(os.path.abspath(__file__))
RIG_FILE = os.path.normpath(os.path.join(HERE, "..", "..", "docking-arm", "DockArm_Carousel_Rigged.blend"))
ZONE = "10.7 | Tender carousel"
OLD_COLL = "10.7 | Tender carousel | docking arm (models/docking-arm)"
NEW_COLL = "10.7 | Tender carousel | docking arm, rigged (models/docking-arm)"
WGT_COLL = "10.7 | Tender carousel | rig handle shapes (not exported)"
ROOT = "DockArm_Root"
RIG = "DockArm_Rig"
BOARD = 5.3


def _new_name(old):
    """Name of the rigged part that takes the place of an unrigged one."""
    if old.startswith("SM_DockArm_"):
        return "SK_DockArm_" + old[len("SM_DockArm_"):]
    return "SK_DockArm_" + old                       # Arm1_UpperLink -> SK_DockArm_Arm1_UpperLink


def world_bounds(ob):
    dg = bpy.context.evaluated_depsgraph_get()
    ev = ob.evaluated_get(dg)
    m = ev.to_mesh()
    mw = ob.matrix_world
    lo, hi = Vector((1e9, 1e9, 1e9)), Vector((-1e9, -1e9, -1e9))
    for v in m.vertices:
        q = mw @ v.co
        lo = Vector((min(lo.x, q.x), min(lo.y, q.y), min(lo.z, q.z)))
        hi = Vector((max(hi.x, q.x), max(hi.y, q.y), max(hi.z, q.z)))
    n = len(m.vertices)
    ev.to_mesh_clear()
    return lo, hi, n


def _layer_collection(lc, name):
    if lc.collection.name == name:
        return lc
    for c in lc.children:
        r = _layer_collection(c, name)
        if r is not None:
            return r
    return None


def _drop_duplicates(blocks, before):
    """Datablocks that an append brought in as NAME.001 while NAME was there already: users go to NAME."""
    dropped = []
    for b in list(blocks):
        if b.name in before or b.library is not None:
            continue
        base, dot, num = b.name.rpartition(".")
        if dot and num.isdigit() and base in before and blocks.get(base) is not None:
            b.user_remap(blocks[base])
            dropped.append(b.name)
            blocks.remove(b)
    return dropped


# ---------------------------------------------------------------------------------------------------------------
# The launch arm reaches in.
# (The user, 2026-10-13: "since we have rigged version you can set it so it will fit put the tender1 more center to
# the moon pool using the rigs and allign arm to more inwards so tender can move to the center".)
#
# As modelled, a hoist is a zigzag: shoulder +phi, elbow +2 phi, wrist +phi, phi = asin(lowered / 6), so the head
# stays exactly under the boom tip, 6 m from the hub. In this ship that is 0.88 m outboard of the deck well's edge:
# the Tender on the launch arm hung over the deck and could not go down through the well. The two links are a
# shoulder and an elbow, though, and need not move as a zigzag: let down, they can also carry the head inward.
#
# add_reach() gives every hoist handle (pose bone CTRL_ArmN_Hoist) a custom property reach_in (metres, 0 = the zigzag
# as modelled) and rewrites the mechanism so that the head, still level, goes to
#       inward  x = reach_in * (lowered / 5.3)^2        down  y = lowered
# The inward move grows with the square of the lowering: near the top the upper link would otherwise have to rise
# above the boom, which it cannot. With D = sqrt(x^2 + y^2):
#       shoulder = atan2(y, x) - acos(D / 6)       elbow = pi - 2 acos(D / 6)       wrist = elbow - shoulder
# For reach_in 0 these are phi, 2 phi and phi again. The joints keep their range limits; the hose's fan bones copy the
# joints and follow. All of it is simple driver expressions (no script auto-run needed).
# hang_tenders() parents the three preview Tenders to the head bones, so they go where the arms carry them.
LINK = 3.0               # one link of a hoist
BOARD_Y2 = BOARD * BOARD
REACH_PROP = "reach_in"
HEAD_LESS = "Hoist, less the shoulder"


def _driver(rig, path, index=0):
    for fc in rig.animation_data.drivers:
        if fc.data_path == path and fc.array_index == index:
            return fc
    return None


def _var(drv, name, rig, data_path):
    v = drv.variables.get(name) or drv.variables.new()
    v.name = name
    v.type = "SINGLE_PROP"
    t = v.targets[0]
    t.id_type = "OBJECT"
    t.id = rig
    t.data_path = data_path
    return v


def add_reach(rig):
    """The mechanism described above, on all three arms. Can be run again; returns what it found and did."""
    info = {}
    x = "r*y*y/%.2f" % BOARD_Y2                                             # inward, from the handle's lowering y
    dist = "clamp(sqrt(pow(%s, 2) + y*y)/%.1f, 0.0, 0.99)" % (x, 2 * LINK)   # D / 6
    for n in (1, 2, 3):
        h = rig.pose.bones.get("CTRL_Arm%d_Hoist" % n)
        if h is None:
            continue
        if REACH_PROP not in h:
            h[REACH_PROP] = 0.0
        ui = h.id_properties_ui(REACH_PROP)
        ui.update(min=0.0, max=2.5, soft_min=0.0, soft_max=2.0,
                  description="How far the head has moved in toward the hub when the hoist is at BOARD (m). 0 = straight down")
        hp = 'pose.bones["CTRL_Arm%d_Hoist"]' % n
        done = []
        for bone, expr in (("MCH_Arm%d_Phi" % n, "atan2(y + 0.000000001, %s) - acos(%s)" % (x, dist)),
                           ("MCH_Arm%d_2Phi" % n, "pi - 2.0*acos(%s)" % dist)):
            fc = _driver(rig, 'pose.bones["%s"].rotation_euler' % bone, 0)
            if fc is None:
                raise RuntimeError("no driver on %s: this is not the rig that was expected" % bone)
            d = fc.driver
            _var(d, "y", rig, hp + ".location[1]")
            _var(d, "r", rig, hp + '["%s"]' % REACH_PROP)
            d.expression = expr
            done.append((bone, d.is_simple_expression, d.is_valid))
        # the wrist: elbow minus shoulder, so the head stays level whatever the two do
        head = rig.pose.bones["Arm%d_Head" % n]
        c0 = head.constraints["Hoist"]
        c0.subtarget = "MCH_Arm%d_2Phi" % n
        c1 = head.constraints.get(HEAD_LESS)
        if c1 is None:
            c1 = head.constraints.new("COPY_ROTATION")
            c1.name = HEAD_LESS
            c1.target = rig
            c1.subtarget = "MCH_Arm%d_Phi" % n
            c1.owner_space = c1.target_space = "LOCAL"
            c1.mix_mode = "ADD"
            c1.use_x, c1.use_y, c1.use_z = True, False, False
            c1.invert_x = True
            names = [c.name for c in head.constraints]
            head.constraints.move(names.index(HEAD_LESS), names.index("Hoist") + 1)     # before the joint's range limit
            fc = c1.driver_add("influence")
            fc.driver.type = "SCRIPTED"
            _var(fc.driver, "auto", rig, hp + '["auto"]')
            fc.driver.expression = "auto"
        info["Arm%d" % n] = {"drivers (bone, simple expression, valid)": done, "head_constraints": [c.name for c in head.constraints],
                             REACH_PROP: round(float(h[REACH_PROP]), 3)}
    rig.update_tag()
    bpy.context.view_layer.update()
    return info


def set_wrist_range(rig, degrees):
    """The wrist joints' range limit (it is +-92 degrees in the model's rig, enough for the zigzag, whose wrist never
    passes 62). A head that reaches in far swings its lower link past the vertical: 2.34 m in at BOARD needs 98.8."""
    was = {}
    for n in (1, 2, 3):
        c = rig.pose.bones["Arm%d_Head" % n].constraints.get("Joint range")
        if c is None:
            continue
        was["Arm%d_Head" % n] = round(math.degrees(c.max_x), 2)
        c.max_x = math.radians(degrees)
        c.min_x = -math.radians(degrees)
    rig.update_tag()
    bpy.context.view_layer.update()
    return {"was_deg": was, "now_deg": degrees}


def set_reach(rig, arm, reach):
    h = rig.pose.bones["CTRL_Arm%d_Hoist" % arm]
    h[REACH_PROP] = float(reach)
    rig.update_tag()
    bpy.context.view_layer.update()


def hang_tenders(rig):
    """The preview Tenders become children of the head bones (where they are now), so the rig carries them."""
    bpy.context.view_layer.update()
    done = {}
    for n in (1, 2, 3):
        t = bpy.data.objects.get("_Tender%d" % n)
        bone = "Arm%d_Head" % n
        if t is None or rig.pose.bones.get(bone) is None:
            continue
        if t.parent is rig and t.parent_type == "BONE" and t.parent_bone == bone:
            done[t.name] = "was on %s already" % bone
            continue
        mw = t.matrix_world.copy()
        t.parent = rig
        t.parent_type = "BONE"
        t.parent_bone = bone
        t.matrix_parent_inverse = Matrix.Identity(4)
        bpy.context.view_layer.update()
        t.matrix_world = mw
        done[t.name] = bone
    bpy.context.view_layer.update()
    return done


def joints(rig, arm):
    """The three joint angles of an arm as they come out (degrees, after the range limits), and where the wrist is."""
    dg = bpy.context.evaluated_depsgraph_get()
    ev = rig.evaluated_get(dg)
    out = {}
    for part in ("Upper", "Lower", "Head"):
        pb = ev.pose.bones["Arm%d_%s" % (arm, part)]
        rest = pb.bone.matrix_local if pb.parent is None else pb.parent.bone.matrix_local.inverted() @ pb.bone.matrix_local
        pose = pb.matrix if pb.parent is None else pb.parent.matrix.inverted() @ pb.matrix
        out[part] = round(math.degrees((rest.inverted() @ pose).to_euler("XYZ").x), 2)
    hb = ev.pose.bones["Arm%d_Head" % arm]
    out["wrist_world"] = [round(c, 3) for c in (ev.matrix_world @ hb.head)]
    down = (ev.matrix_world.to_3x3() @ (hb.tail - hb.head)).normalized()
    out["head_tilt_deg"] = round(math.degrees(math.acos(max(-1.0, min(1.0, -down.z)))), 3)      # 0 = level
    return out


def well_outline():
    """The deck well's edge in plan: the lip curve that runs round it (world x, y)."""
    lip = bpy.data.objects["Moonpool | inner black lip"]
    sp = lip.data.splines[0]
    pts = sp.bezier_points if sp.type == "BEZIER" else sp.points
    return [((lip.matrix_world @ (p.co if sp.type == "BEZIER" else p.co.to_3d())).x, (lip.matrix_world @ (p.co if sp.type == "BEZIER" else p.co.to_3d())).y) for p in pts]


def launch_fit(tender="_Tender1", step=5):
    """The launch Tender as it hangs in the file against the deck well: the least distance of its outline in plan
    from the well's edge (positive = inside the well) and from the coaming's inner edge."""
    poly = well_outline()
    t = bpy.data.objects[tender]
    dg = bpy.context.evaluated_depsgraph_get()
    pts = []
    for o in t.instance_collection.objects:
        if o.type != "MESH":
            continue
        ev = o.evaluated_get(dg)
        m = ev.to_mesh()
        mw = t.matrix_world @ o.matrix_world
        for k in range(0, len(m.vertices), step):
            q = mw @ m.vertices[k].co
            pts.append((q.x, q.y, q.z))
        ev.to_mesh_clear()
    n = len(poly)

    def signed(px, py):
        best, inside = 1e9, False
        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
            tt = 0.0 if l2 < 1e-12 else max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / l2))
            d = math.hypot(px - (ax + tt * dx), py - (ay + tt * dy))
            best = min(best, d)
            if (ay > py) != (by > py) and px < ax + (py - ay) * dx / dy:
                inside = not inside
        return best if inside else -best

    # the outline of the Tender in plan is enough: its extreme points per narrow band of x and of y
    bands = {}
    for x, y, z in pts:
        for key, v in ((("x", round(x * 10)), y), (("y", round(y * 10)), x)):
            b = bands.setdefault(key, [1e9, -1e9, x, y, x, y])
            if v < b[0]:
                b[0], b[2], b[3] = v, x, y
            if v > b[1]:
                b[1], b[4], b[5] = v, x, y
    edge = set()
    for b in bands.values():
        edge.add((round(b[2], 3), round(b[3], 3)))
        edge.add((round(b[4], 3), round(b[5], 3)))
    s = [signed(x, y) for x, y in edge]
    ys = [p[1] for p in pts]
    return {"tender_origin": [round(c, 3) for c in t.matrix_world.translation], "tender_y": [round(min(ys), 3), round(max(ys), 3)],
            "tender_lowest_z": round(min(p[2] for p in pts), 3),
            "least_distance_inside_the_well_m": round(min(s), 3), "clear_of_the_coaming_m": round(min(s) - 0.153, 3),
            "outline_points": len(edge)}


def swap_in_rigged(rig_file=RIG_FILE, board=BOARD):
    """Replaces the unrigged carousel of the open file by the rigged one. Returns what was done and how far each
    part of the new one lies from the part it replaces."""
    root = bpy.data.objects[ROOT]
    zone = bpy.data.collections[ZONE]
    old = bpy.data.collections.get(OLD_COLL)
    if bpy.data.objects.get(RIG) is not None:
        return {"note": "DockArm_Rig is in the file already: nothing done"}
    if old is None:
        raise RuntimeError("the unrigged carousel's collection is not in this file")
    old_objs = [o for o in old.all_objects if o is not root]
    before = {o.name: world_bounds(o) for o in old_objs if o.type == "MESH"}
    mats_before = set(m.name for m in bpy.data.materials)
    imgs_before = set(i.name for i in bpy.data.images)
    with bpy.data.libraries.load(rig_file, link=False) as (src, dst):
        dst.collections = [c for c in src.collections if c in ("DockArm_Rigged", "DockArm_Widgets")]
    rigged = next(c for c in dst.collections if c is not None and c.name.startswith("DockArm_Rigged"))
    wgt = next((c for c in dst.collections if c is not None and c.name.startswith("DockArm_Widgets")), None)
    info = {"appended": {rigged.name: len(rigged.objects)}}
    info["materials_dropped_for_the_file's_own"] = _drop_duplicates(bpy.data.materials, mats_before)
    info["images_dropped_for_the_file's_own"] = _drop_duplicates(bpy.data.images, imgs_before)
    for i in [i for i in bpy.data.images if i.name not in imgs_before and i.users == 0]:
        bpy.data.images.remove(i)
    info["materials_new"] = sorted(m.name for m in bpy.data.materials if m.name not in mats_before)
    rigged.name = NEW_COLL
    zone.children.link(rigged)
    if wgt is not None:
        info["appended"][wgt.name] = len(wgt.objects)
        wgt.name = WGT_COLL
        zone.children.link(wgt)
        lc = _layer_collection(bpy.context.view_layer.layer_collection, WGT_COLL)
        if lc is not None:
            lc.exclude = True                              # as in the model's own file: shapes for the handles only
    rig = next(o for o in rigged.objects if o.type == "ARMATURE")
    rig.parent = root
    rig.matrix_parent_inverse = Matrix.Identity(4)
    rig.location = (0.0, 0.0, 0.0)
    rig.rotation_euler = (0.0, 0.0, 0.0)
    h = rig.pose.bones.get("CTRL_Arm1_Hoist")
    if h is not None:
        h.location = (0.0, board, 0.0)                     # arm 1 rests at BOARD over the launch slot
    rig["placed"] = "the rigged carousel of models/docking-arm, swapped in for the unrigged one (build/carousel_rigged.py)"
    root["carousel"] = "rigged: DockArm_Rig (Pose Mode handles CTRL_ArmN_Hoist 0..5.3 m, CTRL_ArmN_Claws)"
    # the unrigged parts go
    names = sorted(o.name for o in old_objs)
    for o in old_objs:
        bpy.data.objects.remove(o, do_unlink=True)
    rigged.objects.link(root)
    old.objects.unlink(root)
    bpy.data.collections.remove(old)
    gone = []
    for me in list(bpy.data.meshes):
        if me.users == 0 and me.name.startswith("SM_DockArm") and not me.use_fake_user:
            gone.append(me.name)
            bpy.data.meshes.remove(me)
    for a in list(bpy.data.armatures):
        if a.users == 0 and "HoseRig" in a.name:
            gone.append(a.name)
            bpy.data.armatures.remove(a)
    info["removed_objects"] = len(names)
    info["removed_data"] = gone
    bpy.context.view_layer.update()
    # each new part against the one it replaces
    worst, rows, missing = 0.0, {}, []
    for oname, (lo, hi, n) in before.items():
        nb = bpy.data.objects.get(_new_name(oname))
        if nb is None:
            missing.append(oname)
            continue
        lo2, hi2, n2 = world_bounds(nb)
        d = max(max(abs(a - b) for a, b in zip(lo, lo2)), max(abs(a - b) for a, b in zip(hi, hi2)))
        rows[oname] = (round(d * 1000, 3), n, n2)
        worst = max(worst, d)
    info["parts_compared"] = len(rows)
    info["largest_difference_of_a_part's_bounds_mm"] = round(worst * 1000, 3)
    info["parts_with_another_vertex_count"] = {k: v[1:] for k, v in rows.items() if v[1] != v[2]}
    info["old_parts_without_a_new_one"] = missing
    info["rig"] = {"bones": len(rig.data.bones), "meshes": sum(1 for o in rigged.objects if o.type == "MESH"),
                   "hoist_handles_m": {b.name: round(b.location.y, 3) for b in rig.pose.bones if b.name.startswith("CTRL_") and b.name.endswith("_Hoist")}}
    return info