#!/usr/bin/env python3
"""
check_robot_marker_uniqueness.py

Standalone checker for duplicate / invalid marker IDs in robot.json.

Use this before running bundle adjustment:
    python check_robot_marker_uniqueness.py -robot robot.json

Exit codes:
    0 = no duplicates found
    1 = duplicates or structural problems found
"""

from __future__ import annotations

import argparse
import json
import os
import sys
from collections import defaultdict
from typing import Any, Dict, List, Tuple


def resolve_path(path: str) -> str:
    path = os.path.expanduser(path)
    if os.path.isabs(path):
        return path
    return os.path.abspath(path)


def load_json(path: str) -> Dict[str, Any]:
    with open(resolve_path(path), "r", encoding="utf-8") as f:
        return json.load(f)


def main() -> None:
    parser = argparse.ArgumentParser(description="Check marker ID uniqueness in robot.json")
    parser.add_argument("-robot", "--robot", default="../../data/robot/robot.json", help="Path to robot.json")
    parser.add_argument(
        "--strict",
        action="store_true",
        help="Return exit code 1 if any duplicate or invalid marker is found",
    )
    args = parser.parse_args()

    robot = load_json(args.robot)
    links = robot.get("links", {}) or {}

    id_locations: Dict[int, List[Tuple[str, int, str]]] = defaultdict(list)
    invalid_entries: List[str] = []
    total_markers = 0

    for link_name, link_data in links.items():
        markers = link_data.get("markers", []) or []
        for idx, marker in enumerate(markers):
            total_markers += 1
            marker_id = marker.get("id", None)
            if marker_id is None:
                invalid_entries.append(f"link='{link_name}' marker_index={idx}: missing id")
                continue
            try:
                marker_id_int = int(marker_id)
            except Exception:
                invalid_entries.append(f"link='{link_name}' marker_index={idx}: non-integer id={marker_id!r}")
                continue

            name = str(marker.get("name", f"marker_{marker_id_int}"))
            id_locations[marker_id_int].append((link_name, idx, name))

            pos = marker.get("position", None)
            if pos is None or not isinstance(pos, list) or len(pos) != 3:
                invalid_entries.append(
                    f"link='{link_name}' marker_id={marker_id_int}: invalid/missing 3D position"
                )

    duplicates = {mid: locs for mid, locs in id_locations.items() if len(locs) > 1}

    print(f"[INFO] Total marker entries: {total_markers}")
    print(f"[INFO] Unique marker IDs   : {len(id_locations)}")
    print(f"[INFO] Duplicate IDs       : {len(duplicates)}")
    print(f"[INFO] Invalid entries     : {len(invalid_entries)}")

    if duplicates:
        print("\n[DUPLICATES]")
        for mid in sorted(duplicates.keys()):
            print(f"  marker_id={mid}")
            for link_name, idx, name in duplicates[mid]:
                print(f"    - link='{link_name}' marker_index={idx} name='{name}'")

    if invalid_entries:
        print("\n[INVALID ENTRIES]")
        for item in invalid_entries:
            print(f"  - {item}")

    if duplicates or invalid_entries:
        print("\n[WARN] robot.json is not clean for deterministic constraint generation.")
        if args.strict:
            sys.exit(1)
    else:
        print("\n[OK] No duplicate marker IDs found.")


if __name__ == "__main__":
    main()