"""."""

import math

import numpy as np
import pytest

from kinematic_tracker.geometry.slow.giou_yaw import giou_yaw


def test_giou_3d() -> None:
    cuboid1 = np.array([0.0, 0, 0, 0, 0, 0, 4, 2, 2])  # x,y,z,roll,pitch,yaw,length,width,height
    cuboid2 = np.array([2.0, 0, 0, 0, 0, 0, 4, 2, 2])  # certain overlap and axis aligned
    cuboid3 = np.array([2.0, 0, 0, 0, 0, math.pi / 4, 4, 2, 2])  # certain overlap and yaw rotation
    cuboid4 = np.array([2.0, 0, 0, 0, 0, 0, 4, 2, 1])  # certain overlap and height diff
    cuboid5 = np.array([6.0, 0, 0, 0, 0, 0, 4, 2, 2])  # no overlap and axis align
    cuboid6 = np.array([6.0, 0, 0, 0, 0, math.pi / 4, 4, 2, 2])  # no overlap and yaw rotation
    cuboid7 = np.array([6.0, 0, 0, 0, 0, 0, 4, 2, 1])  # no overlap and height diff
    cuboid8 = np.array([16.0, 0, 0, 0, 0, 0, 4, 2, 1])  # no overlap and height diff
    assert giou_yaw(cuboid1, cuboid1) == pytest.approx(1.0)
    assert giou_yaw(cuboid1, cuboid2) == pytest.approx(0.6666666666666666)
    assert giou_yaw(cuboid1, cuboid3) == pytest.approx(0.5055352698662247)
    assert giou_yaw(cuboid1, cuboid4) == pytest.approx(0.5166666666666667)
    assert giou_yaw(cuboid1, cuboid5) == pytest.approx(0.4)  # -0.2 / 2.0 + 0.5
    assert giou_yaw(cuboid1, cuboid6) == pytest.approx(0.2761423749153967)
    assert giou_yaw(cuboid1, cuboid7) == pytest.approx(0.3)  # -0.4 / 2.0 + 0.5
    assert giou_yaw(cuboid1, cuboid8) == pytest.approx(0.15)
