#!/usr/bin/env python3
"""Compare two homogeneous-extrinsic JSON files in parameter space and on SE(3)."""

import argparse
import json
from pathlib import Path

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


def main() -> int:
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--reference", type=Path, required=True)
    parser.add_argument("--candidate", type=Path, required=True)
    parser.add_argument("--output", type=Path, required=True)
    args = parser.parse_args()
    reference = json.loads(args.reference.read_text(encoding="utf-8-sig"))
    candidate = json.loads(args.candidate.read_text(encoding="utf-8-sig"))
    a = np.asarray(reference["matrix_4x4"], dtype=float)
    b = np.asarray(candidate["matrix_4x4"], dtype=float)
    delta = np.linalg.inv(a) @ b
    result = {
        "convention": "delta = inverse(reference) @ candidate",
        "reference": str(args.reference.resolve()),
        "candidate": str(args.candidate.resolve()),
        "candidate_minus_reference_translation_xyz_m": (b[:3, 3] - a[:3, 3]).tolist(),
        "candidate_minus_reference_rpy_xyz_deg": (
            np.asarray(candidate["rotation_rpy_deg_xyz"], float)
            - np.asarray(reference["rotation_rpy_deg_xyz"], float)
        ).tolist(),
        "relative_translation_norm_m": float(np.linalg.norm(delta[:3, 3])),
        "relative_rotation_deg": float(np.degrees(Rotation.from_matrix(delta[:3, :3]).magnitude())),
        "relative_matrix_4x4": delta.tolist(),
    }
    args.output.parent.mkdir(parents=True, exist_ok=True)
    args.output.write_text(json.dumps(result, ensure_ascii=False, indent=2), encoding="utf-8")
    print(json.dumps(result, ensure_ascii=False, indent=2))
    return 0


if __name__ == "__main__":
    raise SystemExit(main())