import cv2
import numpy as np
import pytest

from kinematic_tracker.association.metric_iou_like_two_yaw import MetricIouLikeTwoYaw
from kinematic_tracker.types import FMT


def test_normal(driver: MetricIouLikeTwoYaw, filters: list[cv2.KalmanFilter], det_rz: FMT) -> None:
    metric_rt = driver.compute_metric(det_rz, filters)
    # print(metric_rt.tolist())
    ref = [
        [0.9999999076485746, 0.42566881900326753, 0.2858275223868109],
        [0.42566881900326753, 0.9999999066050226, 0.6108711227388299],
    ]
    assert metric_rt == pytest.approx(np.array(ref))


# xxx need to benchmark comparing the real GIoU-yaw metric.
