import torch


def convert_to_radar_frame(pixel_coords, config):
    """Converts pixel_coords (B x N x 2) from pixel coordinates to metric coordinates in the radar frame.
    Args:
        pixel_coords (torch.tensor): (B,N,2) pixel coordinates
        config (json): parse configuration file
    Returns:
        torch.tensor: (B,N,2) metric coordinates
    """
    cart_pixel_width = config['cart_pixel_width']
    cart_resolution = config['cart_resolution']
    gpuid = config['gpuid']
    if (cart_pixel_width % 2) == 0:
        cart_min_range = (cart_pixel_width / 2 - 0.5) * cart_resolution
    else:
        cart_min_range = cart_pixel_width // 2 * cart_resolution
    B, N, _ = pixel_coords.size()
    R = torch.tensor([[0, -cart_resolution], [cart_resolution, 0]]).expand(B, 2, 2).to(gpuid)
    t = torch.tensor([[cart_min_range], [-cart_min_range]]).expand(B, 2, N).to(gpuid)
    return (torch.bmm(R, pixel_coords.transpose(2, 1)) + t).transpose(2, 1)