import copy


def cam_to_lidar(pointcloud, projection_mats):
    """
    Takes in lidar in velo coords, returns lidar points in camera coords
    :param pointcloud: (n_points, 4) np.array (x,y,z,r) in velodyne coordinates
    :return lidar_cam_coords: (n_points, 4) np.array (x,y,z,r) in camera coordinates
    """

    lidar_velo_coords = copy.deepcopy(pointcloud)
    reflectances = copy.deepcopy(lidar_velo_coords[:, -1]) #copy reflectances column
    lidar_velo_coords[:, -1] = 1 # for multiplying with homogeneous matrix
    lidar_cam_coords = projection_mats['Tr_velo_to_cam'].dot(lidar_velo_coords.transpose())
    lidar_cam_coords = lidar_cam_coords.transpose()
    lidar_cam_coords[:, -1] = reflectances

    return lidar_cam_coords