#!/usr/bin/env python3
"""Build a concise backend comparison and select the recommended result."""
import argparse
import json
from pathlib import Path

import numpy as np
from scipy.spatial.transform import Rotation


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--open3d", required=True)
    parser.add_argument("--small", required=True)
    parser.add_argument("--open3d-quality", required=True)
    parser.add_argument("--small-quality", required=True)
    parser.add_argument("--open3d-check", required=True)
    parser.add_argument("--small-check", required=True)
    parser.add_argument("--output", required=True)
    parser.add_argument("--recommended-output", required=True)
    args = parser.parse_args()

    open_result = json.loads(Path(args.open3d).read_text(encoding="utf-8-sig"))
    small_result = json.loads(Path(args.small).read_text(encoding="utf-8-sig"))
    open_quality = json.loads(Path(args.open3d_quality).read_text(encoding="utf-8-sig"))
    small_quality = json.loads(Path(args.small_quality).read_text(encoding="utf-8-sig"))
    open_check = json.loads(Path(args.open3d_check).read_text(encoding="utf-8-sig"))
    small_check = json.loads(Path(args.small_check).read_text(encoding="utf-8-sig"))
    x_open = np.asarray(open_result["matrix_4x4"], float)
    x_small = np.asarray(small_result["matrix_4x4"], float)
    delta = np.linalg.inv(x_open) @ x_small

    def compact(result, quality, check):
        estimate = result["estimation"]["residuals"]
        auxiliary = check["metrics"]
        return {
            "translation_m": result["translation_m"],
            "rotation_rpy_deg_xyz": result["rotation_rpy_deg_xyz"],
            "estimation_pairs": estimate["pairs"],
            "estimation_translation_rms_m": estimate["translation_m"]["rms"],
            "estimation_rotation_rms_deg": estimate["rotation_deg"]["rms"],
            "bootstrap_std": result["bootstrap"]["std"],
            "initial_B_loop_closure": quality["accepted_loop_closure"],
            "batch1_auxiliary_pairs": auxiliary["pairs"],
            "batch1_auxiliary_translation_rms_m": auxiliary["translation_m"]["rms"],
            "batch1_auxiliary_rotation_rms_deg": auxiliary["rotation_deg"]["rms"],
        }

    summary = {
        "recommended_backend": "open3d_gicp",
        "selection_reason": (
            "The two X estimates agree closely; Open3D has lower second-batch AX residual, "
            "better B loop closure, and lower first-batch auxiliary residual."
        ),
        "coordinate_convention": "T_body_lidar maps raw LiDAR points into rear-axle body frame",
        "measured_extrinsic_used_as_initial": False,
        "second_batch_role": "estimation (dense RTK)",
        "first_batch_role": "auxiliary check only (sparse RTK)",
        "backend_difference": {
            "translation_m": float(np.linalg.norm(delta[:3, 3])),
            "rotation_deg": float(np.rad2deg(Rotation.from_matrix(delta[:3, :3]).magnitude())),
        },
        "open3d_gicp": compact(open_result, open_quality, open_check),
        "small_gicp": compact(small_result, small_quality, small_check),
        "important_limit": (
            "Backend agreement is strong, but AX rotation RMS remains about one degree. "
            "This is not a centimetre-grade absolute certification."
        ),
    }
    output = Path(args.output)
    output.parent.mkdir(parents=True, exist_ok=True)
    output.write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
    recommended = dict(open_result)
    recommended["selection"] = {
        "recommended_backend": "open3d_gicp",
        "comparison_summary": str(output.name),
        "backend_difference": summary["backend_difference"],
        "warning": summary["important_limit"],
    }
    Path(args.recommended_output).write_text(
        json.dumps(recommended, ensure_ascii=False, indent=2), encoding="utf-8"
    )
    print(json.dumps(summary, ensure_ascii=False, indent=2))


if __name__ == "__main__":
    main()