# Stage 6: replace the crotch with the MALE game body's construction (Jeremy's directive —
# the male is already the smooth doll-like build; the female must match it exactly).
#
#   blender --background --python 06_crotch.py -- <03_final_geometry.blend> <male.glb> <out.blend>
#
# Method: detect the crotch saddle landmark on both bodies (lowest midline point of the torso
# between the leg roots), scale the male crotch patch by body-height ratio, translate saddle to
# saddle, subdivide the patch to a smooth cage, and snap the female crotch window onto it with
# a ring-depth blend. The male GLB is in METRES; this sculpt is 0.9792 units tall.
import bpy, sys, time
from collections import deque
import numpy as np
from mathutils import Vector
from mathutils.bvhtree import BVHTree

argv = sys.argv[sys.argv.index("--") + 1:]
BLEND, MALE, OUT = argv[0], argv[1], argv[2]
t0 = time.time()

WIN_R_Z = 0.042            # female window: half-height around the saddle
WIN_R_X = 0.034            # half-width — must NOT reach the inner-thigh walls
SNAP_MAX = 0.020           # reject snaps to far surfaces (a bad cage grabbed thighs at 60 mm)
BLEND_RINGS = 8


def log(m):
    print(f"[crotch {time.time()-t0:6.1f}s] {m}", flush=True)


bpy.ops.wm.open_mainfile(filepath=BLEND)
ob = max([o for o in bpy.data.objects if o.type == 'MESH'],
         key=lambda o: len(o.data.vertices))
me = ob.data
n_v = len(me.vertices)
co = np.empty(n_v * 3)
me.vertices.foreach_get("co", co)
co = co.reshape(-1, 3)
f_height = co[:, 2].max() - co[:, 2].min()

# female saddle: lowest midline torso point between the legs (the briefs bridge the crotch,
# so the midline strip is continuous surface)
mid_f = (np.abs(co[:, 0]) < 0.01) & (co[:, 2] > 0.38) & (co[:, 2] < 0.50)
saddle_f = co[mid_f][np.argmin(co[mid_f, 2])]
log(f"female: height {f_height:.4f}, saddle {saddle_f}")

# ---- male donor ----
before = set(bpy.data.objects)
bpy.ops.import_scene.gltf(filepath=MALE)
new = [o for o in bpy.data.objects if o not in before]
male = max([o for o in new if o.type == 'MESH'], key=lambda o: len(o.data.vertices))
mme = male.data
nm = len(mme.vertices)
mco = np.empty(nm * 3)
mme.vertices.foreach_get("co", mco)
mco = mco.reshape(-1, 3)
m_height = mco[:, 2].max() - mco[:, 2].min()
mid_m = (np.abs(mco[:, 0]) < 0.02) & (mco[:, 2] > 0.55 * m_height) * (mco[:, 2] < 0.75 * m_height)
if not mid_m.any():
    mid_m = (np.abs(mco[:, 0]) < 0.02)
saddle_m = mco[mid_m][np.argmin(mco[mid_m, 2])]
s = f_height / m_height
log(f"male '{male.name}': {nm}v height {m_height:.3f} m, saddle {saddle_m}, scale {s:.4f}")

# male crotch patch: faces whose verts lie near the saddle (generous; the cage is only a target)
sel_m = (np.abs(mco[:, 0] - saddle_m[0]) < WIN_R_X / s * 1.6) \
        & (np.abs(mco[:, 2] - saddle_m[2]) < WIN_R_Z / s * 1.6)
log(f"male patch verts: {sel_m.sum()}")

# transform male verts into female space — and AUTO-DETECT front/back orientation: the male
# body may face the opposite way; test identity and y-mirror, keep whichever cage lands closer
# to the female window verts
mco_t = (mco - saddle_m) * s + saddle_f
mco_t_flip = mco_t.copy()
mco_t_flip[:, 1] = 2 * saddle_f[1] - mco_t[:, 1]

# build patch mesh -> subdivide -> BVH
ml_tot = np.empty(len(mme.polygons), dtype=np.int32)
mme.polygons.foreach_get("loop_total", ml_tot)
ml_start = np.empty(len(mme.polygons), dtype=np.int32)
mme.polygons.foreach_get("loop_start", ml_start)
ml_v = np.empty(len(mme.loops), dtype=np.int32)
mme.loops.foreach_get("vertex_index", ml_v)
faces = []
for fs, ft in zip(ml_start, ml_tot):
    idxs = ml_v[fs:fs + ft]
    if sel_m[idxs].all():
        faces.append(idxs.tolist())
log(f"male patch faces: {len(faces)}")
used = sorted(set(i for f in faces for i in f))
remap = {g: i for i, g in enumerate(used)}
pm = bpy.data.meshes.new("crotch_patch")
pm.from_pydata([Vector(mco_t[i]) for i in used], [],
               [[remap[i] for i in f] for f in faces])
pm.update()
po = bpy.data.objects.new("crotch_patch", pm)
bpy.context.collection.objects.link(po)
sub = po.modifiers.new("s", 'SUBSURF')
sub.levels = 2
bpy.context.view_layer.objects.active = po
bpy.ops.object.modifier_apply(modifier="s")
dme = po.data
dv = np.empty(len(dme.vertices) * 3)
dme.vertices.foreach_get("co", dv)
dv = dv.reshape(-1, 3)
dl_tot = np.empty(len(dme.polygons), dtype=np.int32)
dme.polygons.foreach_get("loop_total", dl_tot)
dl_start = np.empty(len(dme.polygons), dtype=np.int32)
dme.polygons.foreach_get("loop_start", dl_start)
dl_v = np.empty(len(dme.loops), dtype=np.int32)
dme.loops.foreach_get("vertex_index", dl_v)
polys_d = [dl_v[fs:fs + ft].tolist() for fs, ft in zip(dl_start, dl_tot)]
bvh = BVHTree.FromPolygons([Vector(v) for v in dv], polys_d,
                           all_triangles=False, epsilon=0.0)
dv_f = dv.copy()
dv_f[:, 1] = 2 * saddle_f[1] - dv[:, 1]
bvh_f = BVHTree.FromPolygons([Vector(v) for v in dv_f], polys_d,
                             all_triangles=False, epsilon=0.0)
for o in new + [po]:
    bpy.data.objects.remove(o, do_unlink=True)
log("donor cages ready (both orientations)")

# ---- female window snap with ring-depth blend ----
win = (np.abs(co[:, 0] - saddle_f[0]) < WIN_R_X) \
      & (np.abs(co[:, 2] - saddle_f[2]) < WIN_R_Z)
widx = np.nonzero(win)[0]
log(f"female window: {len(widx)} verts")

n_e = len(me.edges)
ev = np.empty(n_e * 2, dtype=np.int32)
me.edges.foreach_get("vertices", ev)
ev = ev.reshape(-1, 2)
order = np.concatenate([ev[:, 0], ev[:, 1]])
nbr = np.concatenate([ev[:, 1], ev[:, 0]])
srt = np.argsort(order, kind="stable")
o_s = order[srt]
n_s = nbr[srt]
ptr = np.searchsorted(o_s, np.arange(n_v + 1))
depth = np.zeros(n_v, dtype=np.int32)
dq = deque()
seen = np.zeros(n_v, dtype=bool)
for a, b in ev:
    if win[a] != win[b]:
        sv = a if win[a] else b
        if not seen[sv]:
            seen[sv] = True
            depth[sv] = 1
            dq.append(sv)
while dq:
    c = dq.popleft()
    for nb in n_s[ptr[c]:ptr[c + 1]]:
        if win[nb] and not seen[nb]:
            seen[nb] = True
            depth[nb] = depth[c] + 1
            dq.append(nb)
depth[win & ~seen] = BLEND_RINGS + 2
wgt = np.clip(depth[widx] / float(BLEND_RINGS), 0.0, 1.0)
wgt = wgt * wgt * (3 - 2 * wgt)

# orientation pick: median nearest-distance over a sample of window verts
samp = widx[::37]
def med_d(tree):
    ds = []
    for i in samp:
        h = tree.find_nearest(Vector(co[i]), 0.08)
        ds.append(h[3] if h[0] is not None else 0.08)
    return float(np.median(ds))
d_id, d_fl = med_d(bvh), med_d(bvh_f)
use = bvh if d_id <= d_fl else bvh_f
log(f"orientation: identity {d_id:.4f} vs flipped {d_fl:.4f} -> "
    f"{'identity' if d_id <= d_fl else 'flipped'}")

co_new = co.copy()
moved = 0
for k, i in enumerate(widx):
    hit = use.find_nearest(Vector(co[i]), SNAP_MAX)
    if hit[0] is None:
        continue
    tgt = np.array(hit[0])
    co_new[i] += (tgt - co[i]) * wgt[k]
    moved += 1
delta = np.linalg.norm(co_new - co, axis=1)
log(f"snapped {moved} verts, max move {delta.max():.4f}")

me.vertices.foreach_set("co", co_new.reshape(-1))
me.update()
if me.has_custom_normals:
    vn = np.empty(n_v * 3, dtype=np.float32)
    me.vertices.foreach_get("normal", vn)
    me.normals_split_custom_set_from_vertices(vn.reshape(-1, 3))
bpy.context.preferences.filepaths.save_version = 0  # no .blend1 autosave
bpy.ops.wm.save_as_mainfile(filepath=OUT)
log(f"WROTE {OUT}")
print("CROTCH_DONE")