# DEPRECATED -- All camera classes have been folded into AnyData.
# Use anydata.geometry.camera.Camera / anydata.geometry.cameras.base.CameraBase instead.
# For invert_pose, use anydata.geometry.camera_utils.invert_extrinsics.
# This file is kept for reference only; all classes crash on instantiation.
#
# Original: camera classes replacing vidar's CameraPinhole, CameraOmni, CameraGoPro.
# No nn.Module, no Pose wrapper -- raw [B,4,4] tensors throughout.
# Each method has a brief comment noting the vidar equivalent.

from __future__ import annotations

from abc import ABC, abstractmethod
from typing import Optional, Union

import cv2
import torch
import torch.nn.functional as F
from torch import Tensor


# ---------------------------------------------------------------------------
# Standalone helpers (cf. vidar camera_utils, pose_utils, tensor utils)
# ---------------------------------------------------------------------------

def invert_intrinsics(K: Tensor) -> Tensor:
    """Analytical inverse of a [B,3,3] or [B,4,4] intrinsics matrix.
    (cf. vidar camera_utils.invert_intrinsics)"""
    Kinv = K.clone()
    Kinv[:, 0, 0] = 1.0 / K[:, 0, 0]
    Kinv[:, 1, 1] = 1.0 / K[:, 1, 1]
    Kinv[:, 0, 2] = -K[:, 0, 2] / K[:, 0, 0]
    Kinv[:, 1, 2] = -K[:, 1, 2] / K[:, 1, 1]
    return Kinv


def invert_pose(T: Tensor) -> Tensor:
    """SE3 inverse of [B,4,4] rigid-body transform: R^T, -R^T t.
    (cf. vidar pose_utils.invert_pose)"""
    Tinv = torch.zeros_like(T)
    Tinv[:, 3, 3] = 1.0
    R_t = T[:, :3, :3].transpose(-2, -1)
    Tinv[:, :3, :3] = R_t
    Tinv[:, :3, -1] = torch.bmm(-R_t, T[:, :3, -1:]).squeeze(-1)
    return Tinv


def pixel_grid(hw: tuple[int, int], b: Optional[int] = None,
               with_ones: bool = False, device: Optional[torch.device] = None) -> Tensor:
    """Meshgrid of pixel coordinates with align_corners=True.
    Returns [2/3, H, W] or [B, 2/3, H, W].
    Channel order: [xx (col/x), yy (row/y), (ones)].
    (cf. vidar tensor.pixel_grid, hardcoded align_corners=True)"""
    H, W = hw
    yy, xx = torch.meshgrid(
        torch.linspace(0, H - 1, H, device=device),
        torch.linspace(0, W - 1, W, device=device),
        indexing='ij',
    )
    if with_ones:
        grid = torch.stack([xx, yy, torch.ones(hw, device=device)], 0)
    else:
        grid = torch.stack([xx, yy], 0)
    if b is not None:
        grid = grid.unsqueeze(0).repeat(b, 1, 1, 1)
    return grid


def norm_pixel_grid(grid: Tensor, hw: Optional[tuple[int, int]] = None) -> Tensor:
    """Normalize pixel coords to [-1, +1] with align_corners=True.
    (cf. vidar tensor.norm_pixel_grid)"""
    if hw is None:
        hw = grid.shape[-2:]
    grid = grid.clone()
    grid[:, 0] = 2.0 * grid[:, 0] / (hw[1] - 1) - 1.0
    grid[:, 1] = 2.0 * grid[:, 1] / (hw[0] - 1) - 1.0
    return grid


def cat_channel_ones(tensor: Tensor, dim: int = 1) -> Tensor:
    """Append a channel of ones along *dim*.
    (cf. vidar tensor.cat_channel_ones)"""
    shape = list(tensor.shape)
    shape[dim] = 1
    return torch.cat([tensor, torch.ones(shape, device=tensor.device, dtype=tensor.dtype)], dim=dim)


# ---------------------------------------------------------------------------
# Camera base class
# ---------------------------------------------------------------------------

class CameraBase(ABC):
    """DEPRECATED -- use anydata.geometry.cameras.base.CameraBase instead.
    Lightweight camera base (cf. vidar CameraBase).
    Stored state: _K [B,4,4], _Twc [B,4,4], _hw (H,W)."""

    tag: str = 'base'

    def __init__(
        self,
        K: Optional[Tensor] = None,
        hw: Optional[Union[tuple[int, int], Tensor]] = None,
        Twc: Optional[Tensor] = None,
        Tcw: Optional[Tensor] = None,
    ):
        raise RuntimeError(
            "custom.utils.camera.CameraBase is DEPRECATED. "
            "Use anydata.geometry.cameras.base.CameraBase instead."
        )
        assert not (Twc is not None and Tcw is not None), 'Provide Twc or Tcw, not both'

        # Resolve hw
        if isinstance(hw, Tensor):
            self._hw = tuple(hw.shape[-2:])
        elif hw is not None:
            self._hw = tuple(hw)
        else:
            self._hw = (1, 1)

        # Store K as [B,4,4]
        if K is not None:
            if K.dim() == 2:
                K = K.unsqueeze(0)
            if K.shape[-1] == 3:
                K4 = torch.eye(4, device=K.device, dtype=K.dtype).unsqueeze(0).repeat(K.shape[0], 1, 1)
                K4[:, :3, :3] = K
                K = K4
            self._K = K
        else:
            # Default K from hw (fx=fy=W/2, cx=W/2, cy=H/2)
            H, W = self._hw
            b = Twc.shape[0] if Twc is not None else (Tcw.shape[0] if Tcw is not None else 1)
            dev = Twc.device if Twc is not None else (Tcw.device if Tcw is not None else 'cpu')
            dt = Twc.dtype if Twc is not None else (Tcw.dtype if Tcw is not None else torch.float32)
            K = torch.eye(4, device=dev, dtype=dt).unsqueeze(0).repeat(b, 1, 1)
            K[:, 0, 0] = W / 2.0
            K[:, 1, 1] = H / 2.0
            K[:, 0, 2] = W / 2.0
            K[:, 1, 2] = H / 2.0
            self._K = K

        # Resolve pose as Twc [B,4,4]
        if Tcw is not None:
            Twc = invert_pose(Tcw)
        if Twc is not None:
            if Twc.dim() == 2:
                Twc = Twc.unsqueeze(0)
            self._Twc = Twc
        else:
            b = self._K.shape[0]
            self._Twc = torch.eye(4, device=self._K.device, dtype=self._K.dtype).unsqueeze(0).repeat(b, 1, 1)

    # -- Properties (cf. vidar CameraBase properties) --

    @property
    def K(self) -> Tensor:
        return self._K

    @property
    def hw(self) -> tuple[int, int]:
        return self._hw

    @property
    def wh(self) -> tuple[int, int]:
        return self._hw[::-1]

    @property
    def device(self) -> torch.device:
        return self._K.device

    @property
    def dtype(self) -> torch.dtype:
        return self._K.dtype

    @property
    def pose(self) -> Tensor:
        """Raw Twc matrix [B,4,4] (cf. vidar CameraBase.pose)."""
        return self._Twc

    @property
    def Twc(self) -> Tensor:
        """World-to-camera [B,4,4]."""
        return self._Twc

    @property
    def Tcw(self) -> Tensor:
        """Camera-to-world [B,4,4] (cf. vidar CameraBase.Tcw)."""
        return invert_pose(self._Twc)

    # -- Dunder methods --

    def __str__(self) -> str:
        B = len(self)
        H, W = self._hw
        dev = self._K.device
        dt = self._K.dtype
        # Translation (camera center in world coords)
        center = self.Tcw[:, :3, -1]  # [B,3]
        if B == 1:
            cx_str = ', '.join(f'{v:.3f}' for v in center[0].tolist())
            t_str = f'pos=[{cx_str}]'
        else:
            t_str = f'pos=[{B}x3]'
        # Intrinsics center
        kx, ky = self._K[:, 0, 2], self._K[:, 1, 2]
        if B == 1:
            k_str = f'cx={kx.item():.1f} cy={ky.item():.1f}'
        else:
            k_str = f'cx~{kx.mean().item():.1f} cy~{ky.mean().item():.1f}'
        return (f'{type(self).__name__}(B={B}, {H}x{W}, {self.tag}, '
                f'{t_str}, {k_str}, {dt}, {dev})')

    def __repr__(self) -> str:
        return self.__str__()

    def __len__(self) -> int:
        return self._Twc.shape[0]

    def __getitem__(self, idx) -> 'CameraBase':
        if isinstance(idx, int):
            idx = slice(idx, idx + 1)
        return type(self)(K=self._K[idx], hw=self._hw, Twc=self._Twc[idx])

    # -- Utility methods --

    def clone(self) -> 'CameraBase':
        return type(self)(K=self._K.clone(), hw=self._hw, Twc=self._Twc.clone())

    def to(self, *args, **kwargs) -> 'CameraBase':
        return type(self)(K=self._K.to(*args, **kwargs), hw=self._hw, Twc=self._Twc.to(*args, **kwargs))

    def float(self) -> 'CameraBase':
        return type(self)(K=self._K.float(), hw=self._hw, Twc=self._Twc.float())

    def detach(self) -> 'CameraBase':
        return type(self)(K=self._K.detach(), hw=self._hw, Twc=self._Twc.detach())

    # -- Geometry methods --

    def get_center(self) -> Tensor:
        """Camera center in world coords [B,3].
        (cf. vidar CameraBase.get_center)"""
        return self.Tcw[:, :3, -1]

    def get_origin(self, flatten: bool = False) -> Tensor:
        """Camera center broadcast to image grid [B,3,H,W].
        (cf. vidar CameraBase.get_origin)"""
        orig = self.get_center().view(len(self), 3, 1, 1).repeat(1, 1, *self._hw)
        if flatten:
            orig = orig.reshape(len(self), 3, -1).permute(0, 2, 1)  # [B, HW, 3]
        return orig

    def no_translation(self) -> 'CameraBase':
        """Camera with zeroed translation (rotation only).
        (cf. vidar CameraBase.no_translation)"""
        Twc = self._Twc.clone()
        Twc[:, :3, -1] = 0
        return type(self)(K=self._K.clone(), hw=self._hw, Twc=Twc)

    def to_world(self, points: Tensor) -> Tensor:
        """Transform points from camera to world coords.
        points: [B,3,N] or [B,3,H,W].
        (cf. vidar CameraBase.to_world)"""
        Tcw = self.Tcw
        R = Tcw[:, :3, :3]
        t = Tcw[:, :3, -1:]
        shape = points.shape
        pts = points.reshape(shape[0], 3, -1)
        out = torch.bmm(R, pts) + t
        return out.reshape(shape)

    def from_world(self, points: Tensor) -> Tensor:
        """Transform points from world to camera coords.
        points: [B,3,N] or [B,3,H,W].
        (cf. vidar CameraBase.from_world)"""
        shape = points.shape
        pts = points.reshape(shape[0], 3, -1)
        pts_h = cat_channel_ones(pts, 1)  # [B,4,N]
        local = torch.bmm(self._Twc, pts_h)[:, :3]
        return local.reshape(shape)

    def Pwc(self, from_world: bool = True) -> Tensor:
        """Projection matrix [B,3,4].
        (cf. vidar CameraBase.Pwc)"""
        if not from_world or self._Twc is None:
            return self._K[:, :3]
        return torch.bmm(self._K, self._Twc)[:, :3]

    def reconstruct_depth_map(self, depth: Tensor, to_world: bool = False,
                              euclidean: bool = False) -> Tensor:
        """Reconstruct 3D points from depth map.
        depth: [B,1,H,W]. Returns [B,3,H,W].
        (cf. vidar CameraBase.reconstruct_z_buffer / reconstruct_depth_map)"""
        B = depth.shape[0]
        grid = pixel_grid(self._hw, b=B, with_ones=True, device=depth.device).to(depth.dtype)
        # grid: [B,3,H,W]
        pts = self.lift(grid.reshape(B, 3, -1), depth.reshape(B, 1, -1))
        pts = pts.reshape(B, 3, *self._hw)
        if to_world:
            pts = self.to_world(pts)
        return pts

    def project_points(self, pts: Tensor, from_world: bool = True,
                       flag_invalid: bool = True) -> Tensor:
        """Project 3D points to 2D pixel coordinates.
        pts: [B,3,N] or [B,3,H,W]. Returns [B,N,2] or [B,H,W,2].
        (cf. vidar CameraBase.project_points)"""
        spatial = pts.dim() == 4
        shape = pts.shape
        pts_flat = pts.reshape(shape[0], 3, -1)

        coords, z = self.unlift(pts_flat, from_world=from_world)
        # coords: [B,2,N], z: [B,N]

        # Normalize to [-1, 1]
        coords_norm = coords.clone()
        H, W = self._hw
        coords_norm[:, 0] = 2.0 * coords[:, 0] / (W - 1) - 1.0
        coords_norm[:, 1] = 2.0 * coords[:, 1] / (H - 1) - 1.0

        if flag_invalid:
            invalid = (coords_norm[:, 0].abs() > 1) | (coords_norm[:, 1].abs() > 1) | (z <= 0)
            coords_norm[:, 0][invalid] = -2.0
            coords_norm[:, 1][invalid] = -2.0

        coords_out = coords_norm.permute(0, 2, 1)  # [B,N,2]
        if spatial:
            coords_out = coords_out.reshape(shape[0], *shape[2:], 2)
        return coords_out

    @staticmethod
    def from_list(cams: list['CameraBase']) -> 'CameraBase':
        """Batch cameras by concatenating along batch dim.
        (cf. vidar CameraPinhole.from_list)"""
        K = torch.cat([c.K for c in cams], 0)
        Twc = torch.cat([c.Twc for c in cams], 0)
        return type(cams[0])(K=K, Twc=Twc, hw=cams[0].hw)

    # -- Abstract methods subclasses must implement --

    @abstractmethod
    def lift(self, grid: Tensor, depth: Tensor) -> Tensor:
        """Lift 2D pixel coords + depth to 3D camera-frame points.
        grid: [B,3,N], depth: [B,1,N]. Returns [B,3,N]."""
        ...

    @abstractmethod
    def unlift(self, points: Tensor, from_world: bool = True) -> tuple[Tensor, Tensor]:
        """Project 3D points to 2D pixel coords + depth.
        points: [B,3,N]. Returns (coords [B,2,N], depth [B,N])."""
        ...


# ---------------------------------------------------------------------------
# CameraPinhole
# ---------------------------------------------------------------------------

class CameraPinhole(CameraBase):
    """Standard pinhole camera (cf. vidar CameraPinhole)."""

    tag = 'pinhole'

    def __str__(self) -> str:
        B = len(self)
        base = super().__str__()
        if B == 1:
            f_str = f'fx={self.fx.item():.1f} fy={self.fy.item():.1f}'
        else:
            f_str = f'fx~{self.fx.mean().item():.1f} fy~{self.fy.mean().item():.1f}'
        # Insert focal info before dtype
        return base.replace(f', {self.dtype}', f', {f_str}, {self.dtype}')

    @property
    def invK(self) -> Tensor:
        """Inverse intrinsics [B,4,4].
        (cf. vidar camera_utils.invert_intrinsics)"""
        return invert_intrinsics(self._K)

    @property
    def fx(self) -> Tensor:
        return self._K[:, 0, 0]

    @property
    def fy(self) -> Tensor:
        return self._K[:, 1, 1]

    @property
    def cx(self) -> Tensor:
        return self._K[:, 0, 2]

    @property
    def cy(self) -> Tensor:
        return self._K[:, 1, 2]

    def lift(self, grid: Tensor, depth: Tensor) -> Tensor:
        """invK @ grid * depth. grid: [B,3,N], depth: [B,1,N].
        (cf. vidar CameraPinhole.lift)"""
        rays = torch.bmm(self.invK[:, :3, :3], grid)
        return rays * depth

    def unlift(self, points: Tensor, from_world: bool = True) -> tuple[Tensor, Tensor]:
        """Project 3D points to pixel coords.
        (cf. vidar CameraPinhole.unlift)"""
        projected = torch.bmm(self.Pwc(from_world), cat_channel_ones(points, 1))
        z = projected[:, 2]
        coords = projected[:, :2] / z.unsqueeze(1).clamp(min=1e-7)
        z[projected[:, 2] <= 0] = 0
        return coords, z

    def get_viewdirs(self, normalize: bool = True, to_world: bool = True,
                     flatten: bool = False) -> Tensor:
        """View direction vectors [B,3,H,W].
        (cf. vidar CameraPinhole.get_viewdirs)"""
        B = len(self)
        ones = torch.ones((B, 1, *self._hw), device=self.device, dtype=self.dtype)
        rays = self.no_translation().reconstruct_depth_map(ones, to_world=to_world)
        if normalize:
            rays = F.normalize(rays, dim=1, p=2)
        if flatten:
            rays = rays.reshape(B, 3, -1).permute(0, 2, 1)  # [B,HW,3]
        return rays

    def get_plucker(self, flatten: bool = False, with_orig: bool = False) -> Tensor:
        """Plucker coordinates [B,6/9,H,W] or [B,HW,6/9].
        (cf. vidar CameraPinhole.get_plucker)"""
        B = len(self)
        ones = torch.ones((B, 1, *self._hw), device=self.device, dtype=self.dtype)
        rays = self.no_translation().reconstruct_depth_map(ones, to_world=True)
        rays = F.normalize(rays, dim=1, p=2)  # [B,3,H,W]

        orig = self.get_center().view(B, 3, 1, 1).repeat(1, 1, *self._hw)  # [B,3,H,W]

        orig_flat = orig.reshape(B, 3, -1).permute(0, 2, 1)  # [B,HW,3]
        rays_flat = rays.reshape(B, 3, -1).permute(0, 2, 1)  # [B,HW,3]
        cross = torch.cross(orig_flat, rays_flat, dim=-1)     # [B,HW,3]

        if with_orig:
            plucker = torch.cat([rays_flat, cross, orig_flat], dim=-1)  # [B,HW,9]
        else:
            plucker = torch.cat([rays_flat, cross], dim=-1)  # [B,HW,6]

        if not flatten:
            plucker = plucker.permute(0, 2, 1).reshape(B, -1, *self._hw)  # [B,6/9,H,W]
        return plucker


# ---------------------------------------------------------------------------
# CameraOmni (Scaramuzza omnidirectional model)
# ---------------------------------------------------------------------------

def _polyval(P: Tensor, x: Tensor) -> Tensor:
    """Evaluate polynomial with coefficients P at values x (batched, Horner's method)."""
    result = P[:, 0:1]
    for i in range(1, P.shape[1]):
        result = result * x + P[:, i:i + 1]
    return result


class CameraOmni(CameraBase):
    """Omnidirectional camera using Scaramuzza polynomial model.
    (cf. vidar CameraOmni)"""

    tag = 'omni'

    def __str__(self) -> str:
        B = len(self)
        base = super().__str__()
        max_deg = f'{torch.rad2deg(self.max_theta * 2).mean().item():.0f}deg'
        poly_n = self.poly.shape[1]
        extra = f'max_fov~{max_deg}, poly_n={poly_n}'
        return base.replace(f', {self.dtype}', f', {extra}, {self.dtype}')

    def __init__(self, K: Tensor, hw=None, Twc=None, Tcw=None):
        # K here is the raw omni parameter vector [B,23] or [B,25]
        if K.dim() == 1:
            K = K.unsqueeze(0)

        self.Komni = K.clone()
        self.poly = torch.flip(K[:, 1:6], [1])       # 5 polynomial coeffs (reversed)
        self.inv_poly = torch.flip(K[:, 7:17], [1])   # 10 inverse poly coeffs (reversed)
        self.xc = K[:, 17:18]
        self.yc = K[:, 18:19]
        self.c = K[:, 19:20]
        self.d = K[:, 20:21]
        self.e = K[:, 21:22]
        self.max_theta = torch.deg2rad(K[:, 22:23]) / 2.0
        self.full_hw = (1024, 1024)

        # Build a standard pinhole-like K for the base class
        H_val = hw[0] if hw is not None else 1024
        W_val = hw[1] if hw is not None else 1024
        B = K.shape[0]
        K_pinhole = torch.eye(4, device=K.device, dtype=K.dtype).unsqueeze(0).repeat(B, 1, 1)
        K_pinhole[:, 0, 0] = W_val / 2.0
        K_pinhole[:, 1, 1] = H_val / 2.0
        K_pinhole[:, 0, 2] = W_val / 2.0
        K_pinhole[:, 1, 2] = H_val / 2.0

        # Call base init with the synthetic K
        super().__init__(K=K_pinhole, hw=hw, Twc=Twc, Tcw=Tcw)

    def __getitem__(self, idx):
        if isinstance(idx, int):
            idx = slice(idx, idx + 1)
        cam = CameraOmni(K=self.Komni[idx], hw=self._hw, Twc=self._Twc[idx])
        return cam

    def clone(self):
        return CameraOmni(K=self.Komni.clone(), hw=self._hw, Twc=self._Twc.clone())

    def no_translation(self):
        Twc = self._Twc.clone()
        Twc[:, :3, -1] = 0
        return CameraOmni(K=self.Komni.clone(), hw=self._hw, Twc=Twc)

    def to(self, *args, **kwargs):
        cam = CameraOmni.__new__(CameraOmni)
        cam._K = self._K.to(*args, **kwargs)
        cam._Twc = self._Twc.to(*args, **kwargs)
        cam._hw = self._hw
        cam.Komni = self.Komni.to(*args, **kwargs)
        cam.poly = self.poly.to(*args, **kwargs)
        cam.inv_poly = self.inv_poly.to(*args, **kwargs)
        cam.xc = self.xc.to(*args, **kwargs)
        cam.yc = self.yc.to(*args, **kwargs)
        cam.c = self.c.to(*args, **kwargs)
        cam.d = self.d.to(*args, **kwargs)
        cam.e = self.e.to(*args, **kwargs)
        cam.max_theta = self.max_theta.to(*args, **kwargs)
        cam.full_hw = self.full_hw
        return cam

    def pixelToRay(self, grid: Tensor) -> Tensor:
        """Convert pixel coordinates to ray directions using polynomial model.
        grid: [B,2or3,N]. Returns [B,3,N] (not normalized).
        (cf. vidar CameraOmni.pixelToRay)"""
        x = grid[:, 1] - self.xc   # row coord shifted
        y = grid[:, 0] - self.yc   # col coord shifted

        invdet = 1.0 / (self.c - self.d * self.e)
        # Apply inverse affine correction
        xp = invdet * (x - self.d * y)
        yp = invdet * (-self.e * x + self.c * y)

        rho = torch.sqrt(xp ** 2 + yp ** 2 + 1e-12)
        z = _polyval(self.poly, rho)

        rays = torch.stack([yp, xp, -z], dim=1)  # [B,3,N]
        return rays

    def lift(self, grid: Optional[Tensor] = None, depth: Optional[Tensor] = None) -> Tensor:
        """Lift using omni polynomial projection.
        (cf. vidar CameraOmni.lift)"""
        B = depth.shape[0] if depth is not None else self._Twc.shape[0]
        dev = depth.device if depth is not None else self.device
        dt = depth.dtype if depth is not None else self.dtype

        # Compute rays at full resolution then resize if needed
        full_grid = pixel_grid(self.full_hw, b=B, with_ones=True, device=dev).to(dt)
        full_grid_flat = full_grid.reshape(B, 3, -1)

        rays = self.pixelToRay(full_grid_flat)
        rays[torch.isnan(rays)] = 0.0
        rays = F.normalize(rays, dim=1, p=2)

        rays = rays.reshape(B, 3, *self.full_hw)

        # Resize if target resolution differs from full_hw
        if self._hw != self.full_hw:
            rays = F.interpolate(rays, size=self._hw, mode='bilinear', align_corners=True)
            rays = F.normalize(rays, dim=1, p=2)

        rays = rays.reshape(B, 3, -1)  # [B,3,HW]

        if depth is not None:
            return (rays * depth).float()
        return rays.float()

    def unlift(self, points: Tensor, from_world: bool = True) -> tuple[Tensor, Tensor]:
        """Project 3D points using inverse polynomial.
        (cf. vidar CameraOmni - uses inv_poly for projection)"""
        if from_world:
            points = self.from_world(points)

        x, y, z = points[:, 0], points[:, 1], points[:, 2]
        norm_xy = torch.sqrt(x ** 2 + y ** 2 + 1e-12)
        theta = torch.atan2(-z, norm_xy)
        rho = _polyval(self.inv_poly, theta)

        coords_x = rho * y / norm_xy  # col
        coords_y = rho * x / norm_xy  # row

        # Apply affine distortion
        u = coords_x * self.c + coords_y * self.d + self.yc
        v = coords_x * self.e + coords_y + self.xc

        coords = torch.stack([u, v], dim=1)  # [B,2,N]
        depth = torch.linalg.vector_norm(points, dim=1)
        depth[z > 0] = 0  # behind camera
        return coords, depth

    def get_plucker(self, flatten: bool = False, with_orig: bool = True) -> Tensor:
        """Plucker coordinates [B,6/9,H,W].
        (cf. vidar CameraOmni.get_plucker) -- with_orig=True by default."""
        B = len(self)
        ones = torch.ones((B, 1, *self._hw), device=self.device, dtype=self.dtype)
        rays = self.no_translation().reconstruct_depth_map(ones, to_world=True)
        rays = F.normalize(rays, dim=1, p=2)

        orig = self.get_center().view(B, 3, 1, 1).repeat(1, 1, *self._hw)
        orig_flat = orig.reshape(B, 3, -1).permute(0, 2, 1)
        rays_flat = rays.reshape(B, 3, -1).permute(0, 2, 1)
        cross = torch.cross(orig_flat, rays_flat, dim=-1)

        if with_orig:
            plucker = torch.cat([rays_flat, cross, orig_flat], dim=-1)
        else:
            plucker = torch.cat([rays_flat, cross], dim=-1)

        if not flatten:
            plucker = plucker.permute(0, 2, 1).reshape(B, -1, *self._hw)
        return plucker

    @staticmethod
    def from_list(cams):
        K = torch.cat([c.Komni for c in cams], 0)
        Twc = torch.cat([c.Twc for c in cams], 0)
        return CameraOmni(K=K, Twc=Twc, hw=cams[0].hw)


# ---------------------------------------------------------------------------
# CameraGoPro (fisheye using OpenCV undistortion)
# ---------------------------------------------------------------------------

class CameraGoPro(CameraBase):
    """GoPro fisheye camera using cv2.fisheye.undistortPoints.
    (cf. vidar CameraGoPro)"""

    tag = 'gopro'

    def __str__(self) -> str:
        B = len(self)
        base = super().__str__()
        if B == 1:
            f_str = f'fx={self._K[0,0,0].item():.1f} fy={self._K[0,1,1].item():.1f}'
        else:
            f_str = f'fx~{self._K[:,0,0].mean().item():.1f} fy~{self._K[:,1,1].mean().item():.1f}'
        return base.replace(f', {self.dtype}', f', {f_str}, {self.dtype}')

    def __init__(self, K: Tensor, hw=None, Twc=None, Tcw=None):
        # K: [B,8] raw intrinsics (fx, fy, cx, cy, d0, d1, d2, d3)
        if K.dim() == 1:
            K = K.unsqueeze(0)

        self.Kraw = K.clone()
        # Hardcoded distortion coefficients (as in vidar)
        self.D = torch.tensor([0.036902, 0.057131, -0.057154, 0.018458],
                              device=K.device, dtype=K.dtype)

        # Build standard K matrix for the base class
        B = K.shape[0]
        K_mat = torch.eye(4, device=K.device, dtype=K.dtype).unsqueeze(0).repeat(B, 1, 1)
        if K.shape[1] >= 4:
            K_mat[:, 0, 0] = K[:, 0]  # fx
            K_mat[:, 1, 1] = K[:, 1]  # fy
            K_mat[:, 0, 2] = K[:, 2]  # cx
            K_mat[:, 1, 2] = K[:, 3]  # cy

        super().__init__(K=K_mat, hw=hw, Twc=Twc, Tcw=Tcw)

    def __getitem__(self, idx):
        if isinstance(idx, int):
            idx = slice(idx, idx + 1)
        return CameraGoPro(K=self.Kraw[idx], hw=self._hw, Twc=self._Twc[idx])

    def clone(self):
        return CameraGoPro(K=self.Kraw.clone(), hw=self._hw, Twc=self._Twc.clone())

    def no_translation(self):
        Twc = self._Twc.clone()
        Twc[:, :3, -1] = 0
        return CameraGoPro(K=self.Kraw.clone(), hw=self._hw, Twc=Twc)

    def to(self, *args, **kwargs):
        cam = CameraGoPro.__new__(CameraGoPro)
        cam._K = self._K.to(*args, **kwargs)
        cam._Twc = self._Twc.to(*args, **kwargs)
        cam._hw = self._hw
        cam.Kraw = self.Kraw.to(*args, **kwargs)
        cam.D = self.D.to(*args, **kwargs)
        return cam

    def pixelToRay(self, grid: Tensor) -> Tensor:
        """Convert pixel coords to ray dirs using cv2 fisheye undistortion.
        grid: [B,3,HW]. Returns [B,3,HW].
        (cf. vidar CameraGoPro.pixelToRay)"""
        B = grid.shape[0]
        N = grid.shape[2]
        device = grid.device
        dtype = grid.dtype

        D_np = self.D.cpu().float().numpy().reshape(4, 1)
        rays_list = []

        for b in range(B):
            K_np = self._K[b, :3, :3].cpu().float().numpy()
            uv = grid[b, :2].cpu().float().numpy().T.reshape(-1, 1, 2)  # [N,1,2]
            import numpy as np
            undist = cv2.fisheye.undistortPoints(uv, K_np, D_np, P=np.eye(3))
            undist = undist.reshape(-1, 2).T  # [2,N]
            rays_b = torch.from_numpy(undist).to(device=device, dtype=dtype)
            ones = torch.ones((1, N), device=device, dtype=dtype)
            rays_list.append(torch.cat([rays_b, ones], dim=0))  # [3,N]

        return torch.stack(rays_list, dim=0)  # [B,3,N]

    def lift(self, grid: Optional[Tensor] = None, depth: Optional[Tensor] = None) -> Tensor:
        """Lift using GoPro fisheye model.
        (cf. vidar CameraGoPro.lift)"""
        rays = self.pixelToRay(grid)
        rays[torch.isnan(rays)] = 0.0
        rays = F.normalize(rays, dim=1, p=2)
        if depth is not None:
            return (rays * depth).float()
        return rays.float()

    def unlift(self, points: Tensor, from_world: bool = True) -> tuple[Tensor, Tensor]:
        """Project 3D points using standard pinhole (approximate for GoPro).
        (cf. vidar CameraGoPro - falls back to pinhole unlift)"""
        projected = torch.bmm(self.Pwc(from_world), cat_channel_ones(points, 1))
        z = projected[:, 2]
        coords = projected[:, :2] / z.unsqueeze(1).clamp(min=1e-7)
        z[projected[:, 2] <= 0] = 0
        return coords, z

    def get_plucker(self, flatten: bool = False, with_orig: bool = True) -> Tensor:
        """Plucker coordinates [B,6/9,H,W].
        (cf. vidar CameraGoPro.get_plucker) -- with_orig=True by default."""
        B = len(self)
        ones = torch.ones((B, 1, *self._hw), device=self.device, dtype=self.dtype)
        rays = self.no_translation().reconstruct_depth_map(ones, to_world=True)
        rays = F.normalize(rays, dim=1, p=2)

        orig = self.get_center().view(B, 3, 1, 1).repeat(1, 1, *self._hw)
        orig_flat = orig.reshape(B, 3, -1).permute(0, 2, 1)
        rays_flat = rays.reshape(B, 3, -1).permute(0, 2, 1)
        cross = torch.cross(orig_flat, rays_flat, dim=-1)

        if with_orig:
            plucker = torch.cat([rays_flat, cross, orig_flat], dim=-1)
        else:
            plucker = torch.cat([rays_flat, cross], dim=-1)

        if not flatten:
            plucker = plucker.permute(0, 2, 1).reshape(B, -1, *self._hw)
        return plucker

    @staticmethod
    def from_list(cams):
        K = torch.cat([c.Kraw for c in cams], 0)
        Twc = torch.cat([c.Twc for c in cams], 0)
        return CameraGoPro(K=K, Twc=Twc, hw=cams[0].hw)


# ---------------------------------------------------------------------------
# Factory functions (backward compat with vidar)
# ---------------------------------------------------------------------------

def Camera(K: Optional[Tensor] = None, hw=None, Twc=None, Tcw=None,
           geometry: str = 'pinhole') -> CameraBase:
    """DEPRECATED -- use anydata.geometry.camera.Camera instead."""
    raise RuntimeError(
        "custom.utils.camera.Camera is DEPRECATED. "
        "Use anydata.geometry.camera.Camera instead."
    )
    # Auto-detect geometry from K shape
    if K is not None:
        Kd = K if K.dim() >= 2 else K.unsqueeze(0)
        cols = Kd.shape[-1]
        if cols in (23, 25):
            geometry = 'omni'
        elif cols == 8:
            geometry = 'gopro'

    if geometry == 'omni':
        return CameraOmni(K=K, hw=hw, Twc=Twc, Tcw=Tcw)
    elif geometry == 'gopro':
        return CameraGoPro(K=K, hw=hw, Twc=Twc, Tcw=Tcw)
    else:
        return CameraPinhole(K=K, hw=hw, Twc=Twc, Tcw=Tcw)


def Camera_from_list(cams: list[CameraBase]) -> CameraBase:
    """Batch cameras from a list by concatenating along batch dim.
    (cf. vidar camera.Camera_from_list)"""
    K = torch.cat([cam.K for cam in cams], 0)
    Twc = torch.cat([cam.Twc for cam in cams], 0)
    return Camera(K=K, Twc=Twc, hw=cams[0].hw)