"""."""

import numpy as np

from nuscenes.utils.data_classes import Box
from pyquaternion import Quaternion

from association_studio.common import FVT


def from_vec_z(vec_z: np.ndarray) -> Box:
    return Box(center=vec_z[:3], orientation=Quaternion(vec_z[3:7]), size=vec_z[7:10])


def fill_vec_z(box: Box, vec_z: FVT) -> None:
    vec_z[:3] = box.center
    vec_z[3:7] = box.orientation.q
    vec_z[7:] = box.wlh


def rotate_by_quat(q_rotate: Quaternion, q_io: Quaternion) -> None:
    """Rotate by quaternion.

    Note:
        The operation preserves the positivity of the scalar part of the quaternion.

    Args:
        q_rotate: quaternion representing the rotation.
        q_io: quaternion to be rotated.
    """
    q = q_io * q_rotate
    q = -q if q.q[0] < 0.0 else q
    q_io.q[:] = q.q[:]
