import numpy as np
import pytest
import open3d as o3d
from scipy.spatial.transform import Rotation
from aruco.icp_registration import (
    ICPConfig,
    ICPResult,
    ICPMetrics,
    extract_near_floor_band,
    extract_scene_points,
    preprocess_point_cloud,
    compute_overlap_xz,
    compute_overlap_3d,
    apply_gravity_constraint,
    pairwise_icp,
    build_pose_graph,
    refine_with_icp,
    compute_fpfh_features,
    global_registration,
)
from aruco.ground_plane import FloorPlane


def test_extract_near_floor_band_basic():
    points = np.array(
        [[0, -0.1, 0], [0, 0.1, 0], [0, 0.2, 0], [0, 0.4, 0]], dtype=np.float64
    )

    floor_y = 0.0
    band_height = 0.3
    floor_normal = np.array([0, 1, 0], dtype=np.float64)

    result = extract_near_floor_band(points, floor_y, band_height, floor_normal)
    assert len(result) == 2
    assert np.all(result[:, 1] >= 0)
    assert np.all(result[:, 1] <= 0.3)


def test_extract_near_floor_band_empty():
    points = np.zeros((0, 3))
    result = extract_near_floor_band(points, 0.0, 0.3, np.array([0, 1, 0]))
    assert len(result) == 0


def test_extract_near_floor_band_all_in():
    points = np.random.uniform(0, 0.2, (100, 3))
    points[:, 1] = np.random.uniform(0.05, 0.25, 100)
    result = extract_near_floor_band(points, 0.0, 0.3, np.array([0, 1, 0]))
    assert len(result) == 100


def test_extract_scene_points_floor_mode_legacy():
    points = np.array(
        [[0, -0.1, 0], [0, 0.1, 0], [0, 0.2, 0], [0, 0.4, 0]], dtype=np.float64
    )
    floor_y = 0.0
    band_height = 0.3
    floor_normal = np.array([0, 1, 0], dtype=np.float64)

    # Should match extract_near_floor_band exactly
    expected = extract_near_floor_band(points, floor_y, band_height, floor_normal)
    result = extract_scene_points(
        points, floor_y, floor_normal, mode="floor", band_height=band_height
    )

    np.testing.assert_array_equal(result, expected)


def test_extract_scene_points_full_mode():
    points = np.random.rand(100, 3)
    floor_y = 0.0
    floor_normal = np.array([0, 1, 0], dtype=np.float64)

    result = extract_scene_points(points, floor_y, floor_normal, mode="full")
    np.testing.assert_array_equal(result, points)


def test_extract_scene_points_hybrid_vertical():
    # Create floor points + vertical wall points
    floor_pts = np.random.uniform(-1, 1, (100, 3))
    floor_pts[:, 1] = 0.1  # Within band

    wall_pts = np.random.uniform(-1, 1, (100, 3))
    wall_pts[:, 0] = 2.0  # Wall at x=2
    # Wall points are tall, outside floor band
    wall_pts[:, 1] = np.random.uniform(0.5, 2.0, 100)

    points = np.vstack([floor_pts, wall_pts])
    floor_y = 0.0
    floor_normal = np.array([0, 1, 0], dtype=np.float64)

    # Hybrid should capture both
    result = extract_scene_points(
        points, floor_y, floor_normal, mode="hybrid", band_height=0.3
    )

    # Should have more points than just floor band
    floor_only = extract_near_floor_band(points, floor_y, 0.3, floor_normal)
    assert len(result) > len(floor_only)

    # Should include high points (walls)
    assert np.any(result[:, 1] > 0.3)


def test_extract_scene_points_hybrid_fallback():
    # Only floor points, no vertical structure
    points = np.random.uniform(-1, 1, (100, 3))
    points[:, 1] = 0.1

    floor_y = 0.0
    floor_normal = np.array([0, 1, 0], dtype=np.float64)

    result = extract_scene_points(
        points, floor_y, floor_normal, mode="hybrid", band_height=0.3
    )

    # Should fall back to floor points
    floor_only = extract_near_floor_band(points, floor_y, 0.3, floor_normal)
    np.testing.assert_array_equal(result, floor_only)


def test_preprocess_point_cloud_sor():
    # Create a dense cluster + sparse outliers
    cluster = np.random.normal(0, 0.1, (100, 3))
    outliers = np.random.uniform(2, 3, (5, 3))
    points = np.vstack([cluster, outliers])

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)

    cleaned = preprocess_point_cloud(pcd, voxel_size=0.02)

    # Should remove outliers
    assert len(cleaned.points) < len(points)
    assert len(cleaned.points) >= 90  # Keep most inliers


def test_compute_overlap_xz_full():
    points_a = np.array([[0, 0, 0], [1, 0, 1]])
    points_b = np.array([[0, 0, 0], [1, 0, 1]])
    area = compute_overlap_xz(points_a, points_b)
    assert abs(area - 1.0) < 1e-6


def test_compute_overlap_xz_no():
    points_a = np.array([[0, 0, 0], [1, 0, 1]])
    points_b = np.array([[2, 0, 2], [3, 0, 3]])
    area = compute_overlap_xz(points_a, points_b)
    assert area == 0.0


def test_compute_overlap_xz_partial():
    points_a = np.array([[0, 0, 0], [1, 0, 1]])
    points_b = np.array([[0.5, 0, 0.5], [1.5, 0, 1.5]])
    area = compute_overlap_xz(points_a, points_b)
    assert abs(area - 0.25) < 1e-6


def test_compute_overlap_xz_with_margin():
    points_a = np.array([[0, 0, 0], [1, 0, 1]])
    points_b = np.array([[1.2, 0, 0], [2.2, 0, 1]])
    area_no_margin = compute_overlap_xz(points_a, points_b)
    area_with_margin = compute_overlap_xz(points_a, points_b, margin=0.5)
    assert area_no_margin == 0.0
    assert area_with_margin > 0.0


def test_compute_overlap_3d_full():
    points_a = np.array([[0, 0, 0], [1, 1, 1]])
    points_b = np.array([[0, 0, 0], [1, 1, 1]])
    volume = compute_overlap_3d(points_a, points_b)
    assert abs(volume - 1.0) < 1e-6


def test_compute_overlap_3d_no():
    points_a = np.array([[0, 0, 0], [1, 1, 1]])
    points_b = np.array([[2, 2, 2], [3, 3, 3]])
    volume = compute_overlap_3d(points_a, points_b)
    assert volume == 0.0


def test_compute_overlap_3d_partial():
    # Overlap in [0.5, 1.0] for all axes -> 0.5^3 = 0.125
    points_a = np.array([[0, 0, 0], [1, 1, 1]])
    points_b = np.array([[0.5, 0.5, 0.5], [1.5, 1.5, 1.5]])
    volume = compute_overlap_3d(points_a, points_b)
    assert abs(volume - 0.125) < 1e-6


def test_compute_overlap_3d_empty():
    points_a = np.zeros((0, 3))
    points_b = np.array([[0, 0, 0], [1, 1, 1]])
    assert compute_overlap_3d(points_a, points_b) == 0.0
    assert compute_overlap_3d(points_b, points_a) == 0.0


def test_apply_gravity_constraint_identity():
    T = np.eye(4)
    result = apply_gravity_constraint(T, T)
    np.testing.assert_allclose(result, T, atol=1e-6)


def test_apply_gravity_constraint_preserves_yaw():
    T_orig = np.eye(4)
    R_icp = Rotation.from_euler("xyz", [0, 10, 0], degrees=True).as_matrix()
    T_icp = np.eye(4)
    T_icp[:3, :3] = R_icp

    result = apply_gravity_constraint(T_icp, T_orig, penalty_weight=10.0)

    res_euler = Rotation.from_matrix(result[:3, :3]).as_euler("xyz", degrees=True)
    assert abs(res_euler[1] - 10.0) < 1e-6
    assert abs(res_euler[0]) < 1e-6
    assert abs(res_euler[2]) < 1e-6


def test_apply_gravity_constraint_regularizes_pitch_roll():
    T_orig = np.eye(4)
    R_icp = Rotation.from_euler("xyz", [10, 0, 10], degrees=True).as_matrix()
    T_icp = np.eye(4)
    T_icp[:3, :3] = R_icp

    result = apply_gravity_constraint(T_icp, T_orig, penalty_weight=9.0)

    res_euler = Rotation.from_matrix(result[:3, :3]).as_euler("xyz", degrees=True)
    assert abs(res_euler[0] - 1.0) < 1e-2
    assert abs(res_euler[2] - 1.0) < 1e-2


def create_box_pcd(size=1.0, num_points=500, seed=42):
    rng = np.random.default_rng(seed)
    points = rng.uniform(0, size, (num_points, 3))
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    return pcd


def test_pairwise_icp_identity():
    pcd = create_box_pcd()
    config = ICPConfig(min_fitness=0.1)
    result = pairwise_icp(pcd, pcd, config, np.eye(4))

    assert result.converged
    assert result.fitness > 0.9
    np.testing.assert_allclose(result.transformation, np.eye(4), atol=1e-3)


def test_pairwise_icp_known_transform():
    source = create_box_pcd()
    T_true = np.eye(4)
    T_true[:3, :3] = Rotation.from_euler("y", 5, degrees=True).as_matrix()
    T_true[:3, 3] = [0.05, 0, 0.02]

    target = o3d.geometry.PointCloud()
    target.points = o3d.utility.Vector3dVector(
        (np.asarray(source.points) @ T_true[:3, :3].T) + T_true[:3, 3]
    )

    config = ICPConfig(min_fitness=0.5, voxel_size=0.01)
    result = pairwise_icp(source, target, config, np.eye(4))

    assert result.converged
    np.testing.assert_allclose(result.transformation, T_true, atol=1e-2)


def test_pairwise_icp_known_transform_gicp():
    source = create_box_pcd()
    T_true = np.eye(4)
    T_true[:3, :3] = Rotation.from_euler("y", 5, degrees=True).as_matrix()
    T_true[:3, 3] = [0.05, 0, 0.02]

    target = o3d.geometry.PointCloud()
    target.points = o3d.utility.Vector3dVector(
        (np.asarray(source.points) @ T_true[:3, :3].T) + T_true[:3, 3]
    )

    # GICP usually needs normals, which pairwise_icp estimates internally
    config = ICPConfig(min_fitness=0.5, voxel_size=0.01, method="gicp")
    result = pairwise_icp(source, target, config, np.eye(4))

    assert result.converged
    np.testing.assert_allclose(result.transformation, T_true, atol=1e-2)


def test_pairwise_icp_insufficient_points():
    source = o3d.geometry.PointCloud()
    source.points = o3d.utility.Vector3dVector(np.random.rand(5, 3))
    target = o3d.geometry.PointCloud()
    target.points = o3d.utility.Vector3dVector(np.random.rand(5, 3))

    config = ICPConfig(min_fitness=0.9)
    result = pairwise_icp(source, target, config, np.eye(4))
    assert not result.converged


def test_robust_kernel_tukey():
    source = create_box_pcd()
    T_true = np.eye(4)
    T_true[:3, 3] = [0.05, 0, 0.02]

    target = o3d.geometry.PointCloud()
    target.points = o3d.utility.Vector3dVector(
        (np.asarray(source.points) @ T_true[:3, :3].T) + T_true[:3, 3]
    )

    # Add some outliers to test robustness
    outliers = np.random.uniform(2, 3, (10, 3))
    target_points = np.vstack([np.asarray(target.points), outliers])
    target.points = o3d.utility.Vector3dVector(target_points)

    config = ICPConfig(
        min_fitness=0.5,
        voxel_size=0.01,
        robust_kernel="tukey",
        robust_kernel_k=0.1,
    )
    result = pairwise_icp(source, target, config, np.eye(4))

    assert result.converged
    # Should still converge close to T_true despite outliers
    np.testing.assert_allclose(result.transformation, T_true, atol=2e-2)


def test_robust_kernel_tukey_gicp():
    source = create_box_pcd()
    T_true = np.eye(4)
    T_true[:3, 3] = [0.05, 0, 0.02]

    target = o3d.geometry.PointCloud()
    target.points = o3d.utility.Vector3dVector(
        (np.asarray(source.points) @ T_true[:3, :3].T) + T_true[:3, 3]
    )

    # Add some outliers
    outliers = np.random.uniform(2, 3, (10, 3))
    target_points = np.vstack([np.asarray(target.points), outliers])
    target.points = o3d.utility.Vector3dVector(target_points)

    config = ICPConfig(
        min_fitness=0.5,
        voxel_size=0.01,
        method="gicp",
        robust_kernel="tukey",
        robust_kernel_k=0.1,
    )
    result = pairwise_icp(source, target, config, np.eye(4))

    assert result.converged
    np.testing.assert_allclose(result.transformation, T_true, atol=2e-2)


def test_build_pose_graph_basic():
    serials = ["cam1", "cam2"]
    extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}

    res = ICPResult(
        transformation=np.eye(4),
        fitness=1.0,
        inlier_rmse=0.0,
        information_matrix=np.eye(6),
        converged=True,
    )
    pair_results = {("cam1", "cam2"): res}

    graph = build_pose_graph(serials, extrinsics, pair_results, "cam1")
    assert len(graph.nodes) == 2
    assert len(graph.edges) == 1


def test_build_pose_graph_disconnected():
    serials = ["cam1", "cam2", "cam3"]
    extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4), "cam3": np.eye(4)}

    res = ICPResult(
        transformation=np.eye(4),
        fitness=1.0,
        inlier_rmse=0.0,
        information_matrix=np.eye(6),
        converged=True,
    )
    pair_results = {("cam1", "cam2"): res}

    graph = build_pose_graph(serials, extrinsics, pair_results, "cam1")
    assert len(graph.nodes) == 2
    assert len(graph.edges) == 1


def test_refine_with_icp_synthetic_offset():
    import aruco.icp_registration
    import aruco.ground_plane

    box_points = create_box_pcd(size=0.5).points
    box_points = np.asarray(box_points)
    box_points[:, 1] -= 1.0
    box_points[:, 2] += 2.0

    def mock_unproject(depth, K, stride=1, **kwargs):
        if depth[0, 0] == 1.0:
            return box_points
        else:
            return box_points - np.array([1.0, 0, 0])

    orig_unproject = aruco.ground_plane.unproject_depth_to_points
    aruco.ground_plane.unproject_depth_to_points = mock_unproject

    try:
        camera_data = {
            "cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
            "cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
        }
        T_w1 = np.eye(4)
        T_w2_est = np.eye(4)
        T_w2_est[0, 3] = 1.05

        extrinsics = {"cam1": T_w1, "cam2": T_w2_est}

        floor_planes = {
            "cam1": FloorPlane(normal=np.array([0, 1, 0]), d=1.0),
            "cam2": FloorPlane(normal=np.array([0, 1, 0]), d=1.0),
        }

        config = ICPConfig(
            min_overlap_area=0.01,
            min_fitness=0.1,
            voxel_size=0.05,
            max_iterations=[20, 10, 5],
            max_translation_m=3.0,
        )

        new_extrinsics, metrics = refine_with_icp(
            camera_data, extrinsics, floor_planes, config
        )

        assert metrics.success
        assert metrics.num_cameras_optimized == 2
        assert abs(new_extrinsics["cam2"][0, 3] - T_w2_est[0, 3]) > 0.01

    finally:
        aruco.ground_plane.unproject_depth_to_points = orig_unproject
        if "orig_global_reg" in locals():
            aruco.icp_registration.global_registration = locals()["orig_global_reg"]


def test_refine_with_icp_floor_mode_regression(monkeypatch):
    import aruco.icp_registration
    import aruco.ground_plane

    mock_extract = []

    def mock_extract_scene_points(
        points, floor_y, floor_normal, mode="floor", band_height=0.3
    ):
        mock_extract.append(mode)
        return points

    mock_overlap_xz = []

    def mock_compute_overlap_xz(pa, pb, margin=0.0):
        mock_overlap_xz.append((len(pa), len(pb)))
        return 10.0

    mock_overlap_3d = []

    def mock_compute_overlap_3d(pa, pb, margin=0.0):
        mock_overlap_3d.append((len(pa), len(pb)))
        return 10.0

    monkeypatch.setattr(
        aruco.icp_registration, "extract_scene_points", mock_extract_scene_points
    )
    monkeypatch.setattr(
        aruco.icp_registration, "compute_overlap_xz", mock_compute_overlap_xz
    )
    monkeypatch.setattr(
        aruco.icp_registration, "compute_overlap_3d", mock_compute_overlap_3d
    )

    # Mock unproject to return enough points
    def mock_unproject(depth, K, stride=1, **kwargs):
        return np.random.rand(200, 3)

    monkeypatch.setattr(aruco.ground_plane, "unproject_depth_to_points", mock_unproject)

    camera_data = {
        "cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
        "cam2": {"depth": np.ones((10, 10)), "K": np.eye(3)},
    }
    extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
    floor_planes = {
        "cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
        "cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
    }

    config = ICPConfig(region="floor", overlap_mode="xz")

    refine_with_icp(camera_data, extrinsics, floor_planes, config)

    assert "floor" in mock_extract
    assert len(mock_overlap_xz) > 0
    assert len(mock_overlap_3d) == 0


def test_refine_with_icp_hybrid_mode_integration(monkeypatch):
    import aruco.icp_registration
    import aruco.ground_plane

    mock_extract = []

    def mock_extract_scene_points(
        points, floor_y, floor_normal, mode="floor", band_height=0.3
    ):
        mock_extract.append(mode)
        return points

    mock_overlap_3d = []

    def mock_compute_overlap_3d(pa, pb, margin=0.0):
        mock_overlap_3d.append((len(pa), len(pb)))
        return 10.0

    monkeypatch.setattr(
        aruco.icp_registration, "extract_scene_points", mock_extract_scene_points
    )
    monkeypatch.setattr(
        aruco.icp_registration, "compute_overlap_3d", mock_compute_overlap_3d
    )

    # Mock unproject to return enough points
    def mock_unproject(depth, K, stride=1, **kwargs):
        return np.random.rand(200, 3)

    monkeypatch.setattr(aruco.ground_plane, "unproject_depth_to_points", mock_unproject)

    camera_data = {
        "cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
        "cam2": {"depth": np.ones((10, 10)), "K": np.eye(3)},
    }
    extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
    floor_planes = {
        "cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
        "cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
    }

    config = ICPConfig(region="hybrid", overlap_mode="3d")

    refine_with_icp(camera_data, extrinsics, floor_planes, config)

    assert "hybrid" in mock_extract
    assert len(mock_overlap_3d) > 0


def test_refine_with_icp_sor_preprocessing(monkeypatch):
    import aruco.icp_registration
    import aruco.ground_plane

    mock_preprocess = []

    def mock_preprocess_point_cloud(pcd, voxel_size):
        mock_preprocess.append(voxel_size)
        return pcd

    monkeypatch.setattr(
        aruco.icp_registration, "preprocess_point_cloud", mock_preprocess_point_cloud
    )

    # Mock unproject to return enough points
    def mock_unproject(depth, K, stride=1, **kwargs):
        return np.random.rand(200, 3)

    monkeypatch.setattr(aruco.ground_plane, "unproject_depth_to_points", mock_unproject)

    # Mock extract_scene_points to return enough points
    def mock_extract_scene_points(points, *args, **kwargs):
        return points

    monkeypatch.setattr(
        aruco.icp_registration, "extract_scene_points", mock_extract_scene_points
    )

    camera_data = {
        "cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
        "cam2": {"depth": np.ones((10, 10)), "K": np.eye(3)},
    }
    extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
    floor_planes = {
        "cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
        "cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
    }

    config = ICPConfig(region="floor")

    refine_with_icp(camera_data, extrinsics, floor_planes, config)

    assert len(mock_preprocess) > 0
    assert mock_preprocess[0] == config.voxel_size


def test_per_pair_logging_all_pairs(monkeypatch):
    """Verify all pairs are logged regardless of convergence."""
    import aruco.icp_registration
    import aruco.ground_plane

    # Mock unproject & extract
    monkeypatch.setattr(
        aruco.ground_plane,
        "unproject_depth_to_points",
        lambda *args, **kwargs: np.random.rand(200, 3),
    )
    monkeypatch.setattr(
        aruco.icp_registration,
        "extract_scene_points",
        lambda points, *args, **kwargs: points,
    )
    monkeypatch.setattr(
        aruco.icp_registration, "compute_overlap_xz", lambda *args, **kwargs: 10.0
    )

    # Mock pairwise_icp to FAIL
    def mock_pairwise_icp_fail(*args, **kwargs):
        return ICPResult(
            transformation=np.eye(4),
            fitness=0.0,
            inlier_rmse=1.0,
            information_matrix=np.eye(6),
            converged=False,
        )

    monkeypatch.setattr(aruco.icp_registration, "pairwise_icp", mock_pairwise_icp_fail)

    camera_data = {
        "cam1": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
        "cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
    }
    extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
    floor_planes = {
        "cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0),
        "cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0),
    }

    config = ICPConfig()
    new_ext, metrics = refine_with_icp(camera_data, extrinsics, floor_planes, config)

    # Even though it failed, it should be in per_pair_results
    assert ("cam1", "cam2") in metrics.per_pair_results
    assert metrics.per_pair_results[("cam1", "cam2")].converged is False
    assert metrics.num_pairs_converged == 0


def test_refine_with_icp_global_init_fallback_low_fitness():
    """Verify fallback to original init when global registration has low fitness."""
    import aruco.icp_registration
    import aruco.ground_plane

    # Mock global_registration to return low fitness
    orig_global_reg = aruco.icp_registration.global_registration

    class MockResult:
        def __init__(self):
            self.fitness = 0.05  # Below 0.1 threshold
            self.transformation = np.eye(4)
            self.transformation[0, 3] = 100.0  # Crazy transform

    def mock_global_reg(*args, **kwargs):
        return MockResult()

    aruco.icp_registration.global_registration = mock_global_reg

    # Mock unproject
    box_points = create_box_pcd(size=0.5).points
    box_points = np.asarray(box_points)

    def mock_unproject(depth, K, stride=1, **kwargs):
        if depth[0, 0] == 1.0:
            return box_points
        else:
            return box_points  # Perfect overlap

    orig_unproject = aruco.ground_plane.unproject_depth_to_points
    aruco.ground_plane.unproject_depth_to_points = mock_unproject

    try:
        camera_data = {
            "cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
            "cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
        }
        extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
        floor_planes = {
            "cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
            "cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
        }

        config = ICPConfig(global_init=True, min_overlap_area=0.01)

        new_extrinsics, metrics = refine_with_icp(
            camera_data, extrinsics, floor_planes, config
        )

        # Should have ignored the crazy transform from global reg
        assert np.allclose(new_extrinsics["cam2"], np.eye(4), atol=0.1)

    finally:
        aruco.ground_plane.unproject_depth_to_points = orig_unproject
        aruco.icp_registration.global_registration = orig_global_reg


def test_refine_with_icp_global_init_fallback_bounds_check():
    """Verify fallback when global registration exceeds safety bounds."""
    import aruco.icp_registration
    import aruco.ground_plane

    # Mock global_registration to return unsafe transform
    orig_global_reg = aruco.icp_registration.global_registration

    class MockResult:
        def __init__(self):
            self.fitness = 1.0  # High fitness
            self.transformation = np.eye(4)
            self.transformation[0, 3] = 10.0  # 10m translation (unsafe)

    def mock_global_reg(*args, **kwargs):
        return MockResult()

    aruco.icp_registration.global_registration = mock_global_reg

    # Mock unproject
    box_points = create_box_pcd(size=0.5).points
    box_points = np.asarray(box_points)

    def mock_unproject(depth, K, stride=1, **kwargs):
        if depth[0, 0] == 1.0:
            return box_points
        else:
            return box_points  # Perfect overlap

    orig_unproject = aruco.ground_plane.unproject_depth_to_points
    aruco.ground_plane.unproject_depth_to_points = mock_unproject

    try:
        camera_data = {
            "cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
            "cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
        }
        extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
        floor_planes = {
            "cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
            "cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
        }

        config = ICPConfig(
            global_init=True,
            min_overlap_area=0.01,
            max_translation_m=0.5,  # Strict bound
        )

        new_extrinsics, metrics = refine_with_icp(
            camera_data, extrinsics, floor_planes, config
        )

        # Should have ignored the unsafe transform
        assert np.allclose(new_extrinsics["cam2"], np.eye(4), atol=0.1)

    finally:
        aruco.ground_plane.unproject_depth_to_points = orig_unproject
        aruco.icp_registration.global_registration = orig_global_reg