import numpy as np

from xrprimer.data_structure.camera.camera import BaseCameraParameter


def convert_camera_from_opencv(cam_param: BaseCameraParameter,
                               dst: str) -> BaseCameraParameter:
    """Convert a camera parameter instance from opencv convention into another
    convention.

    Args:
        cam_param (BaseCameraParameter):
            The input camera parameter defined in opencv convention,
            which is an instance of BaseCameraParameter subclass.
        dst (str):
            The name of destination convention.

    Raises:
        NotImplementedError:
            dst convention has not been supported.

    Returns:
        BaseCameraParameter:
            A camera in the same type as input, whose
            direction is cam2world and and convention
            equals to dst.
    """
    assert cam_param.convention == 'opencv'
    # do not modify cam_param, modify the cloned cam
    cam_param_backup = cam_param
    cam_param = cam_param_backup.clone()
    if cam_param.world2cam is True:
        cam_param.inverse_extrinsic()
    if dst == 'opencv':
        return cam_param
    elif dst == 'blender':
        # rotation of euler zxy, (0, 180, 0)
        rot_mat = np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
                            [0.0, 0.0, -1.0]])
        extrinsic_r = np.asarray(cam_param.get_extrinsic_r())
        extrinsic_r = np.matmul(rot_mat, extrinsic_r)
        cam_param.set_KRT(R=extrinsic_r)
        cam_param.convention = 'blender'
        return cam_param
    elif dst == 'unreal':
        cam_location = np.asarray(cam_param.get_extrinsic_t())
        # right hand to left hand
        cam_location *= np.asarray((-1, 1, 1))
        # rotation of euler zxy, (270, 0, 90)
        rot_mat = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0],
                            [0.0, -1.0, 0.0]])
        extrinsic_r = np.asarray(cam_param.get_extrinsic_r())
        extrinsic_r = np.matmul(rot_mat, extrinsic_r)
        cam_param.set_KRT(R=extrinsic_r, T=cam_location)
        cam_param.convention = 'unreal'
        return cam_param
    else:
        cam_param.logger.error(f'Converting a camera from opencv to {dst}' +
                               ' has not been supported yet.')
        raise NotImplementedError
