"""Trajectory comparison metrics."""

import numpy as np
import pytest

from gps_denied.testing.metrics import (
    absolute_trajectory_error,
    relative_pose_error,
    trajectory_rmse,
)


def test_rmse_identical_trajectories_is_zero():
    est = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
    gt = est.copy()
    assert trajectory_rmse(est, gt) == pytest.approx(0.0)


def test_rmse_constant_offset():
    # Constant 1m offset in x
    est = np.array([[1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
    gt = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]])
    assert trajectory_rmse(est, gt) == pytest.approx(1.0)


def test_rmse_rejects_length_mismatch():
    est = np.zeros((3, 3))
    gt = np.zeros((4, 3))
    with pytest.raises(ValueError):
        trajectory_rmse(est, gt)


def test_ate_returns_summary_stats():
    est = np.array([[1.0, 0.0, 0.0], [2.0, 0.0, 0.0], [3.0, 0.0, 0.0]])
    gt = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
    result = absolute_trajectory_error(est, gt)
    assert result["rmse"] == pytest.approx(1.0)
    assert result["mean"] == pytest.approx(1.0)
    assert result["max"] == pytest.approx(1.0)


def test_rpe_identical_trajectories_is_zero():
    traj = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0]])
    result = relative_pose_error(traj, traj, delta=1)
    assert result["rmse"] == pytest.approx(0.0)