"""."""

import cv2
import numpy as np

from scipy.spatial.transform import Rotation

from kinematic_tracker.geometry.nu_scenes_giou import NuScenesGiou
from kinematic_tracker.geometry.slow.giou_yaw import giou_yaw
from kinematic_tracker.types import FMT


def test_compare_slow(driver: NuScenesGiou, filters: list[cv2.KalmanFilter], det_rz: FMT) -> None:
    ref_rt = np.zeros((2, 3))
    for t, kf in enumerate(filters):
        trk_z = np.dot(kf.measurementMatrix, kf.statePre)
        rot_trk = Rotation.from_quat(trk_z[3:7, 0], scalar_first=True)
        ol_trk_z = np.concatenate((trk_z[:3, 0], rot_trk.as_euler('xyz'), trk_z[7:, 0]))
        for r, det_z in enumerate(det_rz):
            rot_det = Rotation.from_quat(det_z[3:7], scalar_first=True)
            ol_det_z = np.concatenate((det_z[:3], rot_det.as_euler('xyz'), det_z[7:]))
            ref_rt[r, t] = giou_yaw(ol_trk_z, ol_det_z)

    metric_rt = np.zeros((2, 3))
    driver.compute_giou(det_rz, filters, metric_rt)
    assert np.allclose(metric_rt, ref_rt)
