#!/usr/bin/env python3
"""Build motion_pairs.json next to an existing summary without re-solving extrinsic.

Use this once for older calibration outputs that predate automatic pair caching.
"""

from __future__ import annotations

import argparse
import json
import sys
from pathlib import Path

import numpy as np

ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
    sys.path.insert(0, str(ROOT))

from imu_lidar.imu_audit import audit_imu
from imu_lidar.imu_io import load_imu_samples
from imu_lidar.keyframes import build_keyframes
from imu_lidar.lidar_io import load_lidar_frames
from imu_lidar.motion_pairs import build_motion_pairs
from imu_lidar.motion_pairs_io import build_motion_pairs_payload, save_motion_pairs


def _load_summary_meta(summary_path: Path) -> tuple[float, np.ndarray, str]:
    summary = json.loads(summary_path.read_text(encoding="utf-8"))
    delta_t = float(summary.get("time_offset_s") or 0.0)
    session = (summary.get("details") or {}).get("sessions", [{}])[0]
    session_id = str(session.get("session_id") or summary_path.parent.name)
    bias = np.asarray(
        (session.get("imu_audit") or {}).get("gyro_bias_rad_s")
        or (session.get("joint") or {}).get("gyro_bias_rad_s")
        or [0.0, 0.0, 0.0],
        dtype=float,
    ).reshape(3)
    return delta_t, bias, session_id


def export_one(
    *,
    lidar: Path,
    imu: Path,
    summary: Path,
    output: Path | None,
    min_rotation_deg: float,
    min_translation_m: float,
) -> Path:
    delta_t, bias_from_summary, session_id = _load_summary_meta(summary)
    imu_series = load_imu_samples(imu)
    # Prefer freshly audited bias if summary bias is missing/zeros.
    if float(np.linalg.norm(bias_from_summary)) < 1e-12:
        bias = audit_imu(imu_series).gyro_bias_rad_s
    else:
        bias = bias_from_summary

    frames = load_lidar_frames(lidar)
    keyframes = build_keyframes(
        frames,
        min_translation_m=min_translation_m,
        min_rotation_deg=min_rotation_deg,
    )
    pair_set = build_motion_pairs(
        session_id=session_id,
        keyframes=list(keyframes.frames),
        keyframe_indices=keyframes.indices,
        imu=imu_series,
        delta_t_s=delta_t,
        gyro_bias_rad_s=bias,
        min_rotation_deg=min_rotation_deg,
        min_translation_m=min_translation_m,
    )
    prepared = [
        {
            "session_id": session_id,
            "time_offset_s": delta_t,
            "gyro_bias_rad_s": np.asarray(bias, dtype=float).reshape(3),
            "pairs": pair_set.pairs,
        }
    ]
    payload = build_motion_pairs_payload(prepared_sessions=prepared)
    out = output or (summary.parent / "motion_pairs.json")
    save_motion_pairs(out, payload)
    print(f"wrote {out} ({len(pair_set.pairs)} pairs, session={session_id}, dt={delta_t:.6f})")
    return out


def main() -> int:
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("--lidar", type=Path, required=True)
    parser.add_argument("--imu", type=Path, required=True)
    parser.add_argument("--summary", type=Path, required=True)
    parser.add_argument("--output", type=Path, default=None, help="Default: <summary_dir>/motion_pairs.json")
    parser.add_argument("--min-pair-rotation-deg", type=float, default=2.0)
    parser.add_argument("--min-pair-translation-m", type=float, default=0.3)
    args = parser.parse_args()
    export_one(
        lidar=args.lidar,
        imu=args.imu,
        summary=args.summary,
        output=args.output,
        min_rotation_deg=args.min_pair_rotation_deg,
        min_translation_m=args.min_pair_translation_m,
    )
    return 0


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