import numpy as np

from shapely import Polygon

from kinematic_tracker.types import FVT

from .coordinate_rotate import coordinate_rotate


def create_polygon(cuboid: FVT) -> Polygon:
    assert isinstance(cuboid, np.ndarray) and cuboid.ndim == 1 and cuboid.shape[0] == 9
    # assumes cuboid is in format [x, y, z, roll, pitch, yaw, length, width, height] as in OpenLABEL
    x = cuboid[0]
    y = cuboid[1]

    yaw_angle = cuboid[5]
    length = cuboid[6]
    width = cuboid[7]

    # Create a polygon with the given parameters
    polygon_coordinates = [
        (length / 2, -width / 2),
        (length / 2, width / 2),
        (-length / 2, width / 2),
        (-length / 2, -width / 2),
    ]
    for i in range(4):
        new_x, new_y = coordinate_rotate(
            polygon_coordinates[i][0], polygon_coordinates[i][1], -yaw_angle
        )
        polygon_coordinates[i] = (new_x + x, new_y + y)
    return Polygon(polygon_coordinates)
