# -*- coding: utf-8 -*-
"""生成可达外包络的 ENV 零件（经纬网单件）追加进 web/mesh_arm.json：
  ENV_WL_包络经纬网 —— kind=line，LineSegments 渲染（经向 10°/条、纬向 8mm/环）
包络 = rmax(x) 母线绕 J1 轴（平行 X，过 (y=128, z=610)）整周回转；grp=root 不随关节动。"""
import json
import math
import os
import sys

import numpy as np

from arm.paths import DATA, CACHE
from arm import kinematics as HL

Y_AX = HL.Y_AXIS                                  # 128
Z_AX = HL.Z_RL                                    # 610


def main():
    d = np.load(CACHE + "/ws_cloud.npz")
    xs, rmax = d["xs"], d["rmax"]
    ok = ~np.isnan(rmax)
    xs, rmax = xs[ok], rmax[ok]

    def grid(step_x, dphi_deg):
        """经纬参数网格 -> verts(NX*NP,3), segs(扁平索引对), tris。"""
        xs_e = np.arange(xs.min(), xs.max() + 1e-9, step_x)
        rm_e = np.interp(xs_e, xs, rmax)
        nph = int(round(360.0 / dphi_deg))
        phg = np.linspace(0.0, 2.0 * math.pi, nph, endpoint=False)
        nx = len(xs_e)
        V = np.empty((nx * nph, 3))
        for i, x in enumerate(xs_e):
            r = rm_e[i]
            for j, ph in enumerate(phg):
                V[i * nph + j] = (x, Y_AX + r * math.cos(ph), Z_AX + r * math.sin(ph))
        segs = []
        for i in range(nx - 1):                     # 经线（沿 x）
            for j in range(nph):
                segs += [i * nph + j, (i + 1) * nph + j]
        for i in range(nx):                         # 纬线（沿 φ）
            for j in range(nph):
                j2 = (j + 1) % nph
                segs += [i * nph + j, i * nph + j2]
        tris = []
        for i in range(nx - 1):
            for j in range(nph):
                j2 = (j + 1) % nph
                a, b = i * nph + j, i * nph + j2
                c, e = (i + 1) * nph + j, (i + 1) * nph + j2
                tris += [a, c, b, b, c, e]
        return V, segs, tris

    # 经纬网单件：经向 10°一条、纬向 8mm一环（无需贴片面，线自带罩体感）
    Vl, sl, _tl = grid(8.0, 10.0)

    # 自证：包络尺寸应等于 ws_metrics 极值。
    # 【2026-10-05c 修正】原来是 <1.0 的硬比较，但网格 x 是从 xs.min() 起按 step_x
    # 对齐的（np.arange），最后一个采样点最多比真极值短一个步长 —— 一旦
    # (x_fwd - xs.min()) 不是 step_x 的整数倍，断言就会假失败（实测差 6.46mm，
    # 正好 < 8.0），导致包络注不进去、查看器里"可达包络"整个消失。改为
    # "不得超出、且至多少一个步长"这个真正成立的判据。
    step_x = 8.0
    met = json.load(open(CACHE + "/ws_metrics.json", encoding="utf-8"))
    assert Vl[:, 0].min() >= met["x_back"] - 1e-6, (Vl[:, 0].min(), met["x_back"])
    assert Vl[:, 0].min() <= met["x_back"] + step_x + 1e-6, (Vl[:, 0].min(), met["x_back"])
    assert Vl[:, 0].max() <= met["x_fwd"] + 1e-6, (Vl[:, 0].max(), met["x_fwd"])
    assert Vl[:, 0].max() >= met["x_fwd"] - step_x - 1e-6, (Vl[:, 0].max(), met["x_fwd"])
    rl = np.hypot(Vl[:, 1] - Y_AX, Vl[:, 2] - Z_AX)
    # rl.max() 就是 rmax(x) 网格插值后的峰值，与 met["r_max"]（原始点云极值）之间
    # 只允许有插值/取整量级的差（实测 0.044mm），故给 1.0mm 容差。
    assert abs(rl.max() - met["r_max"]) < 1.0, (rl.max(), met["r_max"])
    assert np.all((Vl[:, 1] - Y_AX) ** 2 + (Vl[:, 2] - Z_AX) ** 2
                  <= (np.interp(Vl[:, 0], xs, rmax) + 1e-6) ** 2), "line"
    print("包络经纬网: 顶点%d 线段%d（经10° x 纬8mm）" % (len(Vl), len(sl) // 2))

    # ---- 追加进 mesh_arm.json ----
    path = DATA + "/mesh_arm.json"
    data = json.load(open(path, encoding="utf-8"))
    data["parts"] = [p for p in data["parts"] if not p["name"].startswith("ENV_")]
    mx = max(p["idx"] for p in data["parts"])
    note_ws = ("可达外包络（运动学，docs/travel.json 滑块限位下的全组合 FK 包络）："
               "r=rmax(x) 绕 J1 轴整周回转。高 %.0f~%.0f mm，前伸 %.0f / 后伸 %.0f mm，"
               "离 J1 轴最远 %.0f mm；包络内含死区（肩下折不进），多关节同极位由控制器避碰。"
               % (met["z_min"], met["z_max"], met["x_fwd"], met["x_back"], met["r_max"]))
    line = dict(name="ENV_WL_包络经纬网", cn="包络经纬网", v=[list(p) for p in Vl],
                s=sl, color=0x60a5fa, printed=False, vol=round(met["vol_dm3"] * 1e6, 1),
                note=note_ws + " 经纬线：经向 10°/条，纬向 8mm/环。",
                grp="root", jaw=0.0, ex=[0.0, 0.0, 0.0], idx=mx + 1,
                kind="line")
    data["parts"] += [line]
    tmp = path + ".tmp"
    with open(tmp, "w", encoding="utf-8") as f:
        json.dump(data, f, ensure_ascii=False, separators=(",", ":"))
    os.replace(tmp, path)
    print("已写入", path, "大小 %.1f MB" % (os.path.getsize(path) / 1e6))


if __name__ == "__main__":
    main()