"""Offline, deterministic convex parts from the pinned CAD (requires numpy/scipy).

Runtime imports the checked-in JSON only; no extra browser/Python teleop dependency.
Do not use one hull for the entire gripper: that would fill its open jaw space.
"""

import argparse
import hashlib
import json
import struct
from pathlib import Path

import numpy as np
from scipy.spatial import ConvexHull

ROOT = Path(__file__).resolve().parents[2]
# Cuts in the original STL's millimeters, with 1 mm overlap at each seam.
# Each plane keeps sign * coordinate[axis] <= limit.
PARTS = [
    ("fixed_palm", "Wrist_Roll_08c-v1", [(2, 1, 39.0)]),
    ("fixed_finger", "Wrist_Roll_08c-v1", [(2, -1, -38.0)]),
    ("moving_hinge", "Moving_Jaw_08d-v1", [(1, -1, 22.0)]),
    ("moving_finger", "Moving_Jaw_08d-v1", [(1, 1, -21.0), (1, -1, 60.0)]),
    ("moving_tip", "Moving_Jaw_08d-v1", [(1, 1, -59.0)]),
]
# Bound to prepare_assets.py's pinned source; never silently regenerate for another CAD.
SOURCE_HASHES = {
    "Wrist_Roll_08c-v1.stl": "87507f73f485c2cacb3dc83924a712069fbd68f72b82d4a31a2cfe7f58c9e4c9",
    "Moving_Jaw_08d-v1.stl": "71caabee267376791210950b3b4e2f7968d9b57b92ed3f57711c03f2b5666912",
}


def clip_polygon(polygon, axis, sign, limit):
    result = []
    for previous, current in zip(polygon[-1:] + polygon[:-1], polygon, strict=True):
        a = sign * previous[axis] - limit
        b = sign * current[axis] - limit
        if (a <= 0) != (b <= 0):
            result.append(previous + (current - previous) * (a / (a - b)))
        if b <= 0:
            result.append(current)
    return result


def generate(source, *, definitions=PARTS, source_hashes=SOURCE_HASHES, revision=2):
    """Shared CAD-frame convex clipping; callers bind their own source hashes/cuts."""
    parts = []
    for name, mesh, planes in definitions:
        raw = (source / (mesh + ".stl")).read_bytes()
        digest = hashlib.sha256(raw).hexdigest()
        if digest != source_hashes[mesh + ".stl"]:
            raise ValueError(f"不支持的碰撞体 CAD：{mesh}.stl ({digest})")
        count = struct.unpack_from("<I", raw, 80)[0]
        if len(raw) != 84 + count * 50:
            raise ValueError(f"无效的二进制 STL：{mesh}")
        records = np.frombuffer(
            raw,
            offset=84,
            dtype=np.dtype(
                [
                    ("normal", "<f4", (3,)),
                    ("vertex", "<f4", (3, 3)),
                    ("attribute", "<u2"),
                ]
            ),
        )
        points = []
        for triangle in records["vertex"]:
            polygon = [point.astype(float) for point in triangle]
            for axis, sign, limit in planes:
                if polygon:
                    polygon = clip_polygon(polygon, axis, sign, limit)
            points.extend(polygon)
        # Clip triangles, not just vertices, so long faces crossing cuts leave no holes.
        points = np.unique(np.round(points, 3), axis=0)  # 1 micrometer grid in CAD mm.
        vertices = points[ConvexHull(points).vertices] / 1000  # MJCF meters.
        vertices = vertices[np.lexsort(vertices.T[::-1])]
        parts.append(
            {
                "name": name,
                "visual": mesh + "_visual",
                "vertices": " ".join(f"{value:.6f}" for value in vertices.flat),
            }
        )
    return {
        "revision": revision,
        "source": {
            "repository": "https://github.com/SIGRobotics-UIUC/LeKiwi",
            "revision": "efa608d7ee5a495a4803b1d28cd0c955b4f1e033",
            "license": "Apache-2.0",
        },
        "sources": source_hashes,
        "parts": parts,
    }


def main():
    parser = argparse.ArgumentParser(description="从固定版本 STL 重建 LeKiwi 分段夹爪碰撞体")
    parser.add_argument("--source", type=Path, default=ROOT / "build/lekiwi/URDF/meshes")
    parser.add_argument(
        "--output", type=Path, default=ROOT / "robot_profiles/lekiwi-gripper-collision.json"
    )
    parser.add_argument("--check", action="store_true", help="只验证已签入的碰撞数据可重建")
    args = parser.parse_args()
    result = generate(args.source)
    if args.check:
        if json.loads(args.output.read_text()) != result:
            raise ValueError("夹爪碰撞数据与固定 CAD/生成器不一致")
    else:
        args.output.parent.mkdir(parents=True, exist_ok=True)
        args.output.write_text(json.dumps(result, indent=2) + "\n")
    status = "验证通过" if args.check else args.output
    print(f"LeKiwi 夹爪碰撞体：{len(result['parts'])} 个分段凸包，{status}")


if __name__ == "__main__":
    main()