"""."""

from time import perf_counter

import cv2
import numpy as np

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


def get_filters(trk_tz: FMT) -> list[cv2.KalmanFilter]:
    eye10 = np.eye(10)
    filters = []
    for trk_z in trk_tz:
        kf = cv2.KalmanFilter(10, 10, 0, cv2.CV_64F)
        kf.statePre = trk_z.copy().reshape(10, 1)
        kf.measurementMatrix = eye10
        filters.append(kf)
    return filters


def test_it() -> None:
    """."""
    det_rz = np.linspace(1, 120, 120).reshape(12, 10)
    det_rz[:, 4:6] = 0.0
    det_rz[:, 3:7] /= np.linalg.norm(det_rz[:, 3:7], axis=1).reshape(12, 1)
    trk_tz = np.linspace(1, 230, 230).reshape(23, 10)
    trk_tz[:, 4:6] = 0.0
    trk_tz[:, 3:7] /= np.linalg.norm(trk_tz[:, 3:7], axis=1).reshape(23, 1)
    filters = get_filters(trk_tz)
    drv_g_iou_yaw = MetricGIoUYaw(200, 700, 10, (0, 1, 2, 3, 6, 7, 8, 9))
    drv_probe = MetricIouLikeTwoYaw(200, 700, 10, (0, 1, 2, 3, 6, 7, 8, 9))

    start_time = perf_counter()
    _metric_rt = drv_g_iou_yaw.compute_metric(det_rz, filters)
    time_ref_iou = perf_counter() - start_time

    start_time = perf_counter()
    _metric_rt = drv_probe.compute_metric(det_rz, filters)
    time_two_yaw = perf_counter() - start_time
    print(time_ref_iou, time_two_yaw, time_ref_iou / time_two_yaw)
    assert time_two_yaw < time_ref_iou
