"""."""

import math

import numpy as np
import pytest

from kinematic_tracker.geometry.slow.create_polygon import create_polygon
from kinematic_tracker.geometry.slow.giou_yaw_shapely import giou_3d_shapely


def test_giou_3d_shapely() -> None:
    zz1 = np.array([0.0, 0, 0, 0, 0, 0, 4, 2, 2])  # x,y,z,roll,pitch,yaw,length,width,height
    p1 = create_polygon(zz1)
    zz2 = np.array([2.0, 0, 0, 0, 0, 0, 4, 2, 2])  # certain overlap and axis aligned
    p2 = create_polygon(zz2)
    zz3 = np.array([2.0, 0, 0, 0, 0, math.pi / 4, 4, 2, 2])  # certain overlap and yaw rotation
    p3 = create_polygon(zz3)
    zz4 = np.array([2.0, 0, 0, 0, 0, 0, 4, 2, 1])  # certain overlap and height diff
    p4 = create_polygon(zz4)
    zz5 = np.array([6.0, 0, 0, 0, 0, 0, 4, 2, 2])  # no overlap and axis align
    p5 = create_polygon(zz5)
    zz6 = np.array([6.0, 0, 0, 0, 0, math.pi / 4, 4, 2, 2])  # no overlap and yaw rotation
    p6 = create_polygon(zz6)
    zz7 = np.array([6.0, 0, 0, 0, 0, 0, 4, 2, 1])  # no overlap and height diff
    p7 = create_polygon(zz7)
    zz8 = np.array([16.0, 0, 0, 0, 0, 0, 4, 2, 1])  # no overlap and height diff
    p8 = create_polygon(zz8)
    assert giou_3d_shapely(zz1, p1, zz1, p1) == pytest.approx(1.0)
    assert giou_3d_shapely(zz1, p1, zz2, p2) == pytest.approx(0.6666666666666666)
    assert giou_3d_shapely(zz1, p1, zz3, p3) == pytest.approx(0.5055352698662247)
    assert giou_3d_shapely(zz1, p1, zz4, p4) == pytest.approx(0.5166666666666667)
    assert giou_3d_shapely(zz1, p1, zz5, p5) == pytest.approx(0.4)  # -0.2 / 2.0 + 0.5
    assert giou_3d_shapely(zz1, p1, zz6, p6) == pytest.approx(0.2761423749153967)
    assert giou_3d_shapely(zz1, p1, zz7, p7) == pytest.approx(0.3)  # -0.4 / 2.0 + 0.5
    assert giou_3d_shapely(zz1, p1, zz8, p8) == pytest.approx(0.15)
