"""Compute cameras_extent (getNerfppNorm-style: max dist from mean cam center
* 1.1) and per-camera distance-from-median, to detect outlier-pose clusters
like the tum_fr1_desk_abl1 frames 41-49 issue."""
import sys, numpy as np

sys.path.insert(0, "/srv2/szha0669/blur_slam_exp/repos/BAGS")
from scene.colmap_loader import read_extrinsics_text, qvec2rotmat
from utils.graphics_utils import getWorld2View2

import os

def analyze(images_txt):
    cam_extrinsics = read_extrinsics_text(images_txt)
    names = []
    centers = []
    for key in cam_extrinsics:
        ext = cam_extrinsics[key]
        R = np.transpose(qvec2rotmat(ext.qvec))
        T = np.array(ext.tvec)
        W2C = getWorld2View2(R, T)
        C2W = np.linalg.inv(W2C)
        centers.append(C2W[:3, 3])
        names.append(ext.name)
    centers = np.array(centers)
    mean_c = centers.mean(axis=0)
    median_c = np.median(centers, axis=0)
    dist_from_mean = np.linalg.norm(centers - mean_c, axis=1)
    dist_from_median = np.linalg.norm(centers - median_c, axis=1)
    cameras_extent = dist_from_mean.max() * 1.1
    order = np.argsort(names)
    print(f"  n_cams={len(names)}, cameras_extent={cameras_extent:.4f}")
    print(f"  dist_from_median: min={dist_from_median.min():.3f} max={dist_from_median.max():.3f} mean={dist_from_median.mean():.3f}")
    # flag outliers: dist_from_median > 3x mean
    thresh = 3 * dist_from_median.mean()
    outliers = [(names[i], dist_from_median[i]) for i in range(len(names)) if dist_from_median[i] > thresh]
    if outliers:
        outliers.sort(key=lambda x: -x[1])
        print(f"  OUTLIERS (dist_from_median > {thresh:.3f}, n={len(outliers)}):")
        for n, d in outliers[:20]:
            print(f"    {n}: {d:.3f}")
    else:
        print(f"  no outliers (thresh={thresh:.3f})")

BASE = "/home/szha0669/storage/blur_slam_exp/data/i2slam_trigsplat"
for scene in ["tum_fr1_desk_abl1", "tum_fr1_desk_abl1_nooutlier", "tum_fr2_xyz_abl1", "tum_fr3_office_abl1"]:
    print(f"=== {scene} ===")
    analyze(f"{BASE}/{scene}/sparse/0/images.txt")