#!/usr/bin/env python3
"""Analyze saved observations for issues."""
import argparse
import numpy as np
from scipy.spatial.transform import Rotation


def main():
    parser = argparse.ArgumentParser(description="Analyze hand-eye observations for issues.")
    parser.add_argument("observations", nargs="?", default="calibration_observations.npz",
                        help="Path to observations .npz file")
    args = parser.parse_args()

    # Load dora observations
    dora_obs = np.load(args.observations, allow_pickle=True)

    print("=== OBSERVATIONS ANALYSIS ===")
    print(f"File: {args.observations}")
    print(f"Keys: {list(dora_obs.keys())}")

    R_gripper2base = dora_obs['R_gripper2base']
    t_gripper2base = dora_obs['t_gripper2base']
    R_target2cam = dora_obs['R_target2cam']
    t_target2cam = dora_obs['t_target2cam']

    n = len(R_gripper2base)
    print(f"Count: {n}")

    print("\n=== GRIPPER POSES (base_T_gripper) ===")
    for i in range(n):
        R = R_gripper2base[i]
        t = t_gripper2base[i].flatten()

        # Check rotation matrix validity
        det = np.linalg.det(R)
        orth = np.allclose(R @ R.T, np.eye(3), atol=1e-6)
        rpy = Rotation.from_matrix(R).as_euler('xyz', degrees=True)

        print(f"\nObs {i+1}:")
        print(f"  det(R)={det:.6f}, orthogonal={orth}")
        print(f"  t(mm)=[{t[0]*1000:.1f}, {t[1]*1000:.1f}, {t[2]*1000:.1f}]")
        print(f"  RPY(deg)=[{rpy[0]:.1f}, {rpy[1]:.1f}, {rpy[2]:.1f}]")

        if not orth:
            print("  WARNING: Rotation matrix not orthogonal!")
        if abs(det - 1.0) > 1e-6:
            print(f"  WARNING: Rotation matrix determinant is {det}, expected 1.0!")

    print("\n=== TARGET POSES (cam_T_target) ===")
    for i in range(n):
        R = R_target2cam[i]
        t = t_target2cam[i].flatten()

        det = np.linalg.det(R)
        orth = np.allclose(R @ R.T, np.eye(3), atol=1e-6)

        print(f"\nObs {i+1}:")
        print(f"  det(R)={det:.6f}, orthogonal={orth}")
        print(f"  t(m)=[{t[0]:.4f}, {t[1]:.4f}, {t[2]:.4f}]")
        print(f"  distance from camera: {np.linalg.norm(t)*1000:.1f}mm")

    # Check for pose diversity (important for calibration)
    print("\n=== POSE DIVERSITY CHECK ===")
    positions = np.array([t_gripper2base[i].flatten() for i in range(n)])
    pos_range = positions.max(axis=0) - positions.min(axis=0)
    print(f"Position range (mm): X={pos_range[0]*1000:.1f}, Y={pos_range[1]*1000:.1f}, Z={pos_range[2]*1000:.1f}")

    rotations = np.array([Rotation.from_matrix(R_gripper2base[i]).as_euler('xyz', degrees=True) for i in range(n)])
    rot_range = rotations.max(axis=0) - rotations.min(axis=0)
    print(f"Rotation range (deg): Roll={rot_range[0]:.1f}, Pitch={rot_range[1]:.1f}, Yaw={rot_range[2]:.1f}")


if __name__ == "__main__":
    main()