# Copyright (C) 2022-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).

import os
import numpy as np
import quaternion
import habitat_sim
import json
from sklearn.neighbors import NearestNeighbors
import cv2

# OpenCV to habitat camera convention transformation
R_OPENCV2HABITAT = np.stack(
    (habitat_sim.geo.RIGHT, -habitat_sim.geo.UP, habitat_sim.geo.FRONT), axis=0
)
R_HABITAT2OPENCV = R_OPENCV2HABITAT.T
DEG2RAD = np.pi / 180

def compute_camera_intrinsics(height, width, hfov):
    f = width / 2 / np.tan(hfov / 2 * np.pi / 180)
    cu, cv = width / 2, height / 2
    return f, cu, cv

def compute_camera_pose_opencv_convention(camera_position, camera_orientation):
    R_cam2world = quaternion.as_rotation_matrix(camera_orientation) @ R_OPENCV2HABITAT
    t_cam2world = np.asarray(camera_position)
    return R_cam2world, t_cam2world

def compute_pointmap(depthmap, hfov):
    """Compute a HxWx3 pointmap in camera frame from a HxW depth map."""
    height, width = depthmap.shape
    f, cu, cv = compute_camera_intrinsics(height, width, hfov)
    # Cast depth map to point
    z_cam = depthmap
    u, v = np.meshgrid(range(width), range(height))
    x_cam = (u - cu) / f * z_cam
    y_cam = (v - cv) / f * z_cam
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1)
    return X_cam


import quaternion
import habitat_sim
import json
from sklearn.neighbors import NearestNeighbors
import cv2

# OpenCV to habitat camera convention transformation
R_OPENCV2HABITAT = np.stack(
    (habitat_sim.geo.RIGHT, -habitat_sim.geo.UP, habitat_sim.geo.FRONT), axis=0
)
R_HABITAT2OPENCV = R_OPENCV2HABITAT.T
DEG2RAD = np.pi / 180

def compute_camera_intrinsics(height, width, hfov):
    f = width / 2 / np.tan(hfov / 2 * np.pi / 180)
    cu, cv = width / 2, height / 2
    return f, cu, cv

def compute_camera_pose_opencv_convention(camera_position, camera_orientation):
    R_cam2world = quaternion.as_rotation_matrix(camera_orientation) @ R_OPENCV2HABITAT
    t_cam2world = np.asarray(camera_position)
    return R_cam2world, t_cam2world

def compute_pointmap(depthmap, hfov):
    """Compute a HxWx3 pointmap in camera frame from a HxW depth map."""
    height, width = depthmap.shape
    f, cu, cv = compute_camera_intrinsics(height, width, hfov)
    # Cast depth map to point
    z_cam = depthmap
    u, v = np.meshgrid(range(width), range(height))
    x_cam = (u - cu) / f * z_cam
    y_cam = (v - cv) / f * z_cam
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1)
    return X_cam

def compute_pointcloud(depthmap, hfov, camera_position, camera_rotation):
    """Return a 3D point cloud corresponding to valid pixels of the depth map"""
    R_cam2world, t_cam2world = compute_camera_pose_opencv_convention(
        camera_position, camera_rotation
    )


# OpenCV to habitat camera convention transformation
R_OPENCV2HABITAT = np.stack(
    (habitat_sim.geo.RIGHT, -habitat_sim.geo.UP, habitat_sim.geo.FRONT), axis=0
)
R_HABITAT2OPENCV = R_OPENCV2HABITAT.T
DEG2RAD = np.pi / 180

def compute_camera_intrinsics(height, width, hfov):
    f = width / 2 / np.tan(hfov / 2 * np.pi / 180)
    cu, cv = width / 2, height / 2
    return f, cu, cv

def compute_camera_pose_opencv_convention(camera_position, camera_orientation):
    R_cam2world = quaternion.as_rotation_matrix(camera_orientation) @ R_OPENCV2HABITAT
    t_cam2world = np.asarray(camera_position)
    return R_cam2world, t_cam2world

def compute_pointmap(depthmap, hfov):
    """Compute a HxWx3 pointmap in camera frame from a HxW depth map."""
    height, width = depthmap.shape
    f, cu, cv = compute_camera_intrinsics(height, width, hfov)
    # Cast depth map to point
    z_cam = depthmap
    u, v = np.meshgrid(range(width), range(height))
    x_cam = (u - cu) / f * z_cam
    y_cam = (v - cv) / f * z_cam
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1)
    return X_cam

def compute_pointcloud(depthmap, hfov, camera_position, camera_rotation):
    """Return a 3D point cloud corresponding to valid pixels of the depth map"""
    R_cam2world, t_cam2world = compute_camera_pose_opencv_convention(
        camera_position, camera_rotation
    )

    X_cam = compute_pointmap(depthmap=depthmap, hfov=hfov)
    valid_mask = X_cam[:, :, 2] != 0.0

    X_cam = X_cam.reshape(-1, 3)[valid_mask.flatten()]
    X_world = X_cam @ R_cam2world.T + t_cam2world.reshape(1, 3)
    return X_world

def compute_pointcloud_overlaps_scikit(
    pointcloud1, pointcloud2, distance_threshold, compute_symmetric=False
):
    """


def compute_camera_pose_opencv_convention(camera_position, camera_orientation):
    R_cam2world = quaternion.as_rotation_matrix(camera_orientation) @ R_OPENCV2HABITAT
    t_cam2world = np.asarray(camera_position)
    return R_cam2world, t_cam2world

def compute_pointmap(depthmap, hfov):
    """Compute a HxWx3 pointmap in camera frame from a HxW depth map."""
    height, width = depthmap.shape
    f, cu, cv = compute_camera_intrinsics(height, width, hfov)
    # Cast depth map to point
    z_cam = depthmap
    u, v = np.meshgrid(range(width), range(height))
    x_cam = (u - cu) / f * z_cam
    y_cam = (v - cv) / f * z_cam
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1)
    return X_cam

def compute_pointcloud(depthmap, hfov, camera_position, camera_rotation):
    """Return a 3D point cloud corresponding to valid pixels of the depth map"""
    R_cam2world, t_cam2world = compute_camera_pose_opencv_convention(
        camera_position, camera_rotation
    )

    X_cam = compute_pointmap(depthmap=depthmap, hfov=hfov)
    valid_mask = X_cam[:, :, 2] != 0.0

    X_cam = X_cam.reshape(-1, 3)[valid_mask.flatten()]
    X_world = X_cam @ R_cam2world.T + t_cam2world.reshape(1, 3)
    return X_world

def compute_pointcloud_overlaps_scikit(
    pointcloud1, pointcloud2, distance_threshold, compute_symmetric=False
):
    """
    Compute 'overlapping' metrics based on a distance threshold between two point clouds.
    """
    nbrs = NearestNeighbors(n_neighbors=1, algorithm="kd_tree").fit(pointcloud2)
    distances, indices = nbrs.kneighbors(pointcloud1)
    intersection1 = np.count_nonzero(distances.flatten() < distance_threshold)

    data = {"intersection1": intersection1, "size1": len(pointcloud1)}
    if compute_symmetric:
        nbrs = NearestNeighbors(n_neighbors=1, algorithm="kd_tree").fit(pointcloud1)
        distances, indices = nbrs.kneighbors(pointcloud2)
        intersection2 = np.count_nonzero(distances.flatten() < distance_threshold)
        data["intersection2"] = intersection2
        data["size2"] = len(pointcloud2)


    x_cam = (u - cu) / f * z_cam
    y_cam = (v - cv) / f * z_cam
    X_cam = np.stack((x_cam, y_cam, z_cam), axis=-1)
    return X_cam

def compute_pointcloud(depthmap, hfov, camera_position, camera_rotation):
    """Return a 3D point cloud corresponding to valid pixels of the depth map"""
    R_cam2world, t_cam2world = compute_camera_pose_opencv_convention(
        camera_position, camera_rotation
    )

    X_cam = compute_pointmap(depthmap=depthmap, hfov=hfov)
    valid_mask = X_cam[:, :, 2] != 0.0

    X_cam = X_cam.reshape(-1, 3)[valid_mask.flatten()]
    X_world = X_cam @ R_cam2world.T + t_cam2world.reshape(1, 3)
    return X_world

def compute_pointcloud_overlaps_scikit(
    pointcloud1, pointcloud2, distance_threshold, compute_symmetric=False
):
    """
    Compute 'overlapping' metrics based on a distance threshold between two point clouds.
    """
    nbrs = NearestNeighbors(n_neighbors=1, algorithm="kd_tree").fit(pointcloud2)
    distances, indices = nbrs.kneighbors(pointcloud1)
    intersection1 = np.count_nonzero(distances.flatten() < distance_threshold)

    data = {"intersection1": intersection1, "size1": len(pointcloud1)}
    if compute_symmetric:
        nbrs = NearestNeighbors(n_neighbors=1, algorithm="kd_tree").fit(pointcloud1)
        distances, indices = nbrs.kneighbors(pointcloud2)
        intersection2 = np.count_nonzero(distances.flatten() < distance_threshold)
        data["intersection2"] = intersection2
        data["size2"] = len(pointcloud2)

    return data

def _append_camera_parameters(observation, hfov, camera_location, camera_rotation):
    """
    Add camera parameters to the observation dictionnary produced by Habitat-Sim
    In-place modifications.
    """
    R_cam2world, t_cam2world = compute_camera_pose_opencv_convention(
        camera_location, camera_rotation
    )
    height, width = observation["depth"].shape
    f, cu, cv = compute_camera_intrinsics(height, width, hfov)
    K = np.asarray([[f, 0, cu], [0, f, cv], [0, 0, 1.0]])
    observation["camera_intrinsics"] = K
    observation["t_cam2world"] = t_cam2world
    observation["R_cam2world"] = R_cam2world

def look_at(eye, center, up, return_cam2world=True):
    """


    pointcloud1, pointcloud2, distance_threshold, compute_symmetric=False
):
    """
    Compute 'overlapping' metrics based on a distance threshold between two point clouds.
    """
    nbrs = NearestNeighbors(n_neighbors=1, algorithm="kd_tree").fit(pointcloud2)
    distances, indices = nbrs.kneighbors(pointcloud1)
    intersection1 = np.count_nonzero(distances.flatten() < distance_threshold)

    data = {"intersection1": intersection1, "size1": len(pointcloud1)}
    if compute_symmetric:
        nbrs = NearestNeighbors(n_neighbors=1, algorithm="kd_tree").fit(pointcloud1)
        distances, indices = nbrs.kneighbors(pointcloud2)
        intersection2 = np.count_nonzero(distances.flatten() < distance_threshold)
        data["intersection2"] = intersection2
        data["size2"] = len(pointcloud2)

    return data

def _append_camera_parameters(observation, hfov, camera_location, camera_rotation):
    """
    Add camera parameters to the observation dictionnary produced by Habitat-Sim
    In-place modifications.
    """
    R_cam2world, t_cam2world = compute_camera_pose_opencv_convention(
        camera_location, camera_rotation
    )
    height, width = observation["depth"].shape
    f, cu, cv = compute_camera_intrinsics(height, width, hfov)
    K = np.asarray([[f, 0, cu], [0, f, cv], [0, 0, 1.0]])
    observation["camera_intrinsics"] = K
    observation["t_cam2world"] = t_cam2world
    observation["R_cam2world"] = R_cam2world

def look_at(eye, center, up, return_cam2world=True):
    """
    Return camera pose looking at a given center point.
    Analogous of gluLookAt function, using OpenCV camera convention.
    """
    z = center - eye
    z /= np.linalg.norm(z, axis=-1, keepdims=True)
    y = -up
    y = y - np.sum(y * z, axis=-1, keepdims=True) * z
    y /= np.linalg.norm(y, axis=-1, keepdims=True)
    x = np.cross(y, z, axis=-1)

    if return_cam2world:
        R = np.stack((x, y, z), axis=-1)
        t = eye
    else:
        # World to camera transformation
        # Transposed matrix


    return data

def _append_camera_parameters(observation, hfov, camera_location, camera_rotation):
    """
    Add camera parameters to the observation dictionnary produced by Habitat-Sim
    In-place modifications.
    """
    R_cam2world, t_cam2world = compute_camera_pose_opencv_convention(
        camera_location, camera_rotation
    )
    height, width = observation["depth"].shape
    f, cu, cv = compute_camera_intrinsics(height, width, hfov)
    K = np.asarray([[f, 0, cu], [0, f, cv], [0, 0, 1.0]])
    observation["camera_intrinsics"] = K
    observation["t_cam2world"] = t_cam2world
    observation["R_cam2world"] = R_cam2world

def look_at(eye, center, up, return_cam2world=True):
    """
    Return camera pose looking at a given center point.
    Analogous of gluLookAt function, using OpenCV camera convention.
    """
    z = center - eye
    z /= np.linalg.norm(z, axis=-1, keepdims=True)
    y = -up
    y = y - np.sum(y * z, axis=-1, keepdims=True) * z
    y /= np.linalg.norm(y, axis=-1, keepdims=True)
    x = np.cross(y, z, axis=-1)

    if return_cam2world:
        R = np.stack((x, y, z), axis=-1)
        t = eye
    else:
        # World to camera transformation
        # Transposed matrix
        R = np.stack((x, y, z), axis=-2)
        t = -np.einsum("...ij, ...j", R, eye)
    return R, t

def look_at_for_habitat(eye, center, up, return_cam2world=True):
    R, t = look_at(eye, center, up)
    orientation = quaternion.from_rotation_matrix(R @ R_OPENCV2HABITAT.T)
    return orientation, t

def generate_orientation_noise(pan_range, tilt_range, roll_range):
    return (
        quaternion.from_rotation_vector(
            np.random.uniform(*pan_range) * DEG2RAD * habitat_sim.geo.UP
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*tilt_range) * DEG2RAD * habitat_sim.geo.RIGHT
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*roll_range) * DEG2RAD * habitat_sim.geo.FRONT
        )
    )


    Analogous of gluLookAt function, using OpenCV camera convention.
    """
    z = center - eye
    z /= np.linalg.norm(z, axis=-1, keepdims=True)
    y = -up
    y = y - np.sum(y * z, axis=-1, keepdims=True) * z
    y /= np.linalg.norm(y, axis=-1, keepdims=True)
    x = np.cross(y, z, axis=-1)

    if return_cam2world:
        R = np.stack((x, y, z), axis=-1)
        t = eye
    else:
        # World to camera transformation
        # Transposed matrix
        R = np.stack((x, y, z), axis=-2)
        t = -np.einsum("...ij, ...j", R, eye)
    return R, t

def look_at_for_habitat(eye, center, up, return_cam2world=True):
    R, t = look_at(eye, center, up)
    orientation = quaternion.from_rotation_matrix(R @ R_OPENCV2HABITAT.T)
    return orientation, t

def generate_orientation_noise(pan_range, tilt_range, roll_range):
    return (
        quaternion.from_rotation_vector(
            np.random.uniform(*pan_range) * DEG2RAD * habitat_sim.geo.UP
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*tilt_range) * DEG2RAD * habitat_sim.geo.RIGHT
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*roll_range) * DEG2RAD * habitat_sim.geo.FRONT
        )
    )

class NoNaviguableSpaceError(RuntimeError):
    def __init__(self, *args):
        super().__init__(*args)


    y /= np.linalg.norm(y, axis=-1, keepdims=True)
    x = np.cross(y, z, axis=-1)

    if return_cam2world:
        R = np.stack((x, y, z), axis=-1)
        t = eye
    else:
        # World to camera transformation
        # Transposed matrix
        R = np.stack((x, y, z), axis=-2)
        t = -np.einsum("...ij, ...j", R, eye)
    return R, t

def look_at_for_habitat(eye, center, up, return_cam2world=True):
    R, t = look_at(eye, center, up)
    orientation = quaternion.from_rotation_matrix(R @ R_OPENCV2HABITAT.T)
    return orientation, t

def generate_orientation_noise(pan_range, tilt_range, roll_range):
    return (
        quaternion.from_rotation_vector(
            np.random.uniform(*pan_range) * DEG2RAD * habitat_sim.geo.UP
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*tilt_range) * DEG2RAD * habitat_sim.geo.RIGHT
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*roll_range) * DEG2RAD * habitat_sim.geo.FRONT
        )
    )

class NoNaviguableSpaceError(RuntimeError):
    def __init__(self, *args):
        super().__init__(*args)

class MultiviewHabitatSimGenerator:
    def __init__(
        self,
        scene,
        navmesh,
        scene_dataset_config_file,
        resolution=(240, 320),
        views_count=2,
        hfov=60,
        gpu_id=0,
        size=10000,
        minimum_covisibility=0.5,
        transform=None,


NoNaviguableSpaceError

def look_at_for_habitat(eye, center, up, return_cam2world=True):
    R, t = look_at(eye, center, up)
    orientation = quaternion.from_rotation_matrix(R @ R_OPENCV2HABITAT.T)
    return orientation, t

def generate_orientation_noise(pan_range, tilt_range, roll_range):
    return (
        quaternion.from_rotation_vector(
            np.random.uniform(*pan_range) * DEG2RAD * habitat_sim.geo.UP
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*tilt_range) * DEG2RAD * habitat_sim.geo.RIGHT
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*roll_range) * DEG2RAD * habitat_sim.geo.FRONT
        )
    )

class NoNaviguableSpaceError(RuntimeError):
    def __init__(self, *args):
        super().__init__(*args)

class MultiviewHabitatSimGenerator:
    def __init__(
        self,
        scene,
        navmesh,
        scene_dataset_config_file,
        resolution=(240, 320),
        views_count=2,
        hfov=60,
        gpu_id=0,
        size=10000,
        minimum_covisibility=0.5,
        transform=None,
    ):
        self.scene = scene
        self.navmesh = navmesh
        self.scene_dataset_config_file = scene_dataset_config_file
        self.resolution = resolution


MultiviewHabitatSimGenerator

def generate_orientation_noise(pan_range, tilt_range, roll_range):
    return (
        quaternion.from_rotation_vector(
            np.random.uniform(*pan_range) * DEG2RAD * habitat_sim.geo.UP
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*tilt_range) * DEG2RAD * habitat_sim.geo.RIGHT
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*roll_range) * DEG2RAD * habitat_sim.geo.FRONT
        )
    )

class NoNaviguableSpaceError(RuntimeError):
    def __init__(self, *args):
        super().__init__(*args)

class MultiviewHabitatSimGenerator:
    def __init__(
        self,
        scene,
        navmesh,
        scene_dataset_config_file,
        resolution=(240, 320),
        views_count=2,
        hfov=60,
        gpu_id=0,
        size=10000,
        minimum_covisibility=0.5,
        transform=None,
    ):
        self.scene = scene
        self.navmesh = navmesh
        self.scene_dataset_config_file = scene_dataset_config_file
        self.resolution = resolution
        self.views_count = views_count
        assert self.views_count >= 1
        self.hfov = hfov
        self.gpu_id = gpu_id
        self.size = size
        self.transform = transform

        # Noise added to camera orientation
        self.pan_range = (-3, 3)
        self.tilt_range = (-10, 10)
        self.roll_range = (-5, 5)

        # Height range to sample cameras
        self.height_range = (1.2, 1.8)

        # Random steps between the camera views
        self.random_steps_count = 5
        self.random_step_variance = 2.0

        # Minimum fraction of the scene which should be valid (well defined depth)
        self.minimum_valid_fraction = 0.7

        # Distance threshold to see  to select pairs
        self.distance_threshold = 0.05
        # Minimum IoU of a view point cloud with respect to the reference view to be kept.
        self.minimum_covisibility = minimum_covisibility

        # Maximum number of retries.
        self.max_attempts_count = 100

        self.seed = None
        self._lazy_initialization()

    def _lazy_initialization(self):
        # Lazy random seeding and instantiation of the simulator to deal with multiprocessing properly
        if self.seed == None:
            # Re-seed numpy generator
            np.random.seed()
            self.seed = np.random.randint(2**32 - 1)
            sim_cfg = habitat_sim.SimulatorConfiguration()
            sim_cfg.scene_id = self.scene
            if (
                self.scene_dataset_config_file is not None
                and self.scene_dataset_config_file != ""
            ):
                sim_cfg.scene_dataset_config_file = self.scene_dataset_config_file
            sim_cfg.random_seed = self.seed
            sim_cfg.load_semantic_mesh = False
            sim_cfg.gpu_device_id = self.gpu_id

            depth_sensor_spec = habitat_sim.CameraSensorSpec()
            depth_sensor_spec.uuid = "depth"
            depth_sensor_spec.sensor_type = habitat_sim.SensorType.DEPTH
            depth_sensor_spec.resolution = self.resolution
            depth_sensor_spec.hfov = self.hfov
            depth_sensor_spec.position = [0.0, 0.0, 0]
            depth_sensor_spec.orientation

            rgb_sensor_spec = habitat_sim.CameraSensorSpec()
            rgb_sensor_spec.uuid = "color"
            rgb_sensor_spec.sensor_type = habitat_sim.SensorType.COLOR
            rgb_sensor_spec.resolution = self.resolution
            rgb_sensor_spec.hfov = self.hfov
            rgb_sensor_spec.position = [0.0, 0.0, 0]
            agent_cfg = habitat_sim.agent.AgentConfiguration(
                sensor_specifications=[rgb_sensor_spec, depth_sensor_spec]
            )

            cfg = habitat_sim.Configuration(sim_cfg, [agent_cfg])
            self.sim = habitat_sim.Simulator(cfg)
            if self.navmesh is not None and self.navmesh != "":
                # Use pre-computed navmesh when available (usually better than those generated automatically)
                self.sim.pathfinder.load_nav_mesh(self.navmesh)

            if not self.sim.pathfinder.is_loaded:
                # Try to compute a navmesh
                navmesh_settings = habitat_sim.NavMeshSettings()
                navmesh_settings.set_defaults()
                self.sim.recompute_navmesh(self.sim.pathfinder, navmesh_settings, True)

            # Ensure that the navmesh is not empty
            if not self.sim.pathfinder.is_loaded:
                raise NoNaviguableSpaceError(
                    f"No naviguable location (scene: {self.scene} -- navmesh: {self.navmesh})"
                )

            self.agent = self.sim.initialize_agent(agent_id=0)

    def close(self):
        self.sim.close()

    def __del__(self):
        self.sim.close()

    def __len__(self):
        return self.size

    def sample_random_viewpoint(self):
        """Sample a random viewpoint using the navmesh"""
        nav_point = self.sim.pathfinder.get_random_navigable_point()

        # Sample a random viewpoint height
        viewpoint_height = np.random.uniform(*self.height_range)
        viewpoint_position = nav_point + viewpoint_height * habitat_sim.geo.UP
        viewpoint_orientation = quaternion.from_rotation_vector(
            np.random.uniform(0, 2 * np.pi) * habitat_sim.geo.UP
        ) * generate_orientation_noise(self.pan_range, self.tilt_range, self.roll_range)
        return viewpoint_position, viewpoint_orientation, nav_point

    def sample_other_random_viewpoint(self, observed_point, nav_point):
        """Sample a random viewpoint close to an existing one, using the navmesh and a reference observed point."""
        other_nav_point = nav_point

        walk_directions = self.random_step_variance * np.asarray([1, 0, 1])
        for i in range(self.random_steps_count):
            temp = self.sim.pathfinder.snap_point(
                other_nav_point + walk_directions * np.random.normal(size=3)
            )
            # Snapping may return nan when it fails
            if not np.isnan(temp[0]):
                other_nav_point = temp

        other_viewpoint_height = np.random.uniform(*self.height_range)
        other_viewpoint_position = (
            other_nav_point + other_viewpoint_height * habitat_sim.geo.UP
        )

        # Set viewing direction towards the central point
        rotation, position = look_at_for_habitat(
            eye=other_viewpoint_position,
            center=observed_point,
            up=habitat_sim.geo.UP,
            return_cam2world=True,
        )
        rotation = rotation * generate_orientation_noise(
            self.pan_range, self.tilt_range, self.roll_range
        )
        return position, rotation, other_nav_point

    def is_other_pointcloud_overlapping(self, ref_pointcloud, other_pointcloud):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        pixels_count = self.resolution[0] * self.resolution[1]
        valid_fraction = len(other_pointcloud) / pixels_count
        assert valid_fraction <= 1.0 and valid_fraction >= 0.0
        overlap = compute_pointcloud_overlaps_scikit(
            ref_pointcloud,
            other_pointcloud,
            self.distance_threshold,
            compute_symmetric=True,
        )
        covisibility = min(
            overlap["intersection1"] / pixels_count,
            overlap["intersection2"] / pixels_count,
        )
        is_valid = (valid_fraction >= self.minimum_valid_fraction) and (
            covisibility >= self.minimum_covisibility
        )
        return is_valid, valid_fraction, covisibility

    def is_other_viewpoint_overlapping(
        self, ref_pointcloud, observation, position, rotation
    ):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        other_pointcloud = compute_pointcloud(
            observation["depth"], self.hfov, position, rotation
        )
        return self.is_other_pointcloud_overlapping(ref_pointcloud, other_pointcloud)

    def render_viewpoint(self, viewpoint_position, viewpoint_orientation):
        agent_state = habitat_sim.AgentState()
        agent_state.position = viewpoint_position
        agent_state.rotation = viewpoint_orientation
        self.agent.set_state(agent_state)
        viewpoint_observations = self.sim.get_sensor_observations(agent_ids=0)
        _append_camera_parameters(
            viewpoint_observations, self.hfov, viewpoint_position, viewpoint_orientation
        )
        return viewpoint_observations

    def __getitem__(self, useless_idx):
        ref_position, ref_orientation, nav_point = self.sample_random_viewpoint()
        ref_observations = self.render_viewpoint(ref_position, ref_orientation)
        # Extract point cloud
        ref_pointcloud = compute_pointcloud(
            depthmap=ref_observations["depth"],
            hfov=self.hfov,
            camera_position=ref_position,
            camera_rotation=ref_orientation,
        )

        pixels_count = self.resolution[0] * self.resolution[1]
        ref_valid_fraction = len(ref_pointcloud) / pixels_count
        assert ref_valid_fraction <= 1.0 and ref_valid_fraction >= 0.0
        if ref_valid_fraction < self.minimum_valid_fraction:
            # This should produce a recursion error at some point when something is very wrong.
            return self[0]
        # Pick an reference observed point in the point cloud
        observed_point = np.mean(ref_pointcloud, axis=0)

        # Add the first image as reference
        viewpoints_observations = [ref_observations]
        viewpoints_covisibility = [ref_valid_fraction]
        viewpoints_positions = [ref_position]
        viewpoints_orientations = [quaternion.as_float_array(ref_orientation)]
        viewpoints_clouds = [ref_pointcloud]
        viewpoints_valid_fractions = [ref_valid_fraction]

        for _ in range(self.views_count - 1):
            # Generate an other viewpoint using some dummy random walk
            successful_sampling = False
            for sampling_attempt in range(self.max_attempts_count):
                position, rotation, _ = self.sample_other_random_viewpoint(
                    observed_point, nav_point
                )
                # Observation
                other_viewpoint_observations = self.render_viewpoint(position, rotation)
                other_pointcloud = compute_pointcloud(
                    other_viewpoint_observations["depth"], self.hfov, position, rotation
                )

                is_valid, valid_fraction, covisibility = (
                    self.is_other_pointcloud_overlapping(
                        ref_pointcloud, other_pointcloud
                    )
                )
                if is_valid:
                    successful_sampling = True
                    break
            if not successful_sampling:
                print("WARNING: Maximum number of attempts reached.")
                # Dirty hack, try using a novel original viewpoint
                return self[0]
            viewpoints_observations.append(other_viewpoint_observations)
            viewpoints_covisibility.append(covisibility)
            viewpoints_positions.append(position)
            viewpoints_orientations.append(
                quaternion.as_float_array(rotation)
            )  # WXYZ convention for the quaternion encoding.
            viewpoints_clouds.append(other_pointcloud)
            viewpoints_valid_fractions.append(valid_fraction)

        # Estimate relations between all pairs of images
        pairwise_visibility_ratios = np.ones(
            (len(viewpoints_observations), len(viewpoints_observations))
        )
        for i in range(len(viewpoints_observations)):
            pairwise_visibility_ratios[i, i] = viewpoints_valid_fractions[i]
            for j in range(i + 1, len(viewpoints_observations)):
                overlap = compute_pointcloud_overlaps_scikit(
                    viewpoints_clouds[i],
                    viewpoints_clouds[j],
                    self.distance_threshold,
                    compute_symmetric=True,
                )
                pairwise_visibility_ratios[i, j] = (
                    overlap["intersection1"] / pixels_count
                )
                pairwise_visibility_ratios[j, i] = (
                    overlap["intersection2"] / pixels_count
                )

        # IoU is relative to the image 0
        data = {
            "observations": viewpoints_observations,
            "positions": np.asarray(viewpoints_positions),
            "orientations": np.asarray(viewpoints_orientations),
            "covisibility_ratios": np.asarray(viewpoints_covisibility),
            "valid_fractions": np.asarray(viewpoints_valid_fractions, dtype=float),
            "pairwise_visibility_ratios": np.asarray(
                pairwise_visibility_ratios, dtype=float
            ),
        }

        if self.transform is not None:
            data = self.transform(data)
        return data

    def generate_random_spiral_trajectory(
        self,
        images_count=100,
        max_radius=0.5,
        half_turns=5,
        use_constant_orientation=False,
    ):
        """
        Return a list of images corresponding to a spiral trajectory from a random starting point.
        Useful to generate nice visualisations.
        Use an even number of half turns to get a nice "C1-continuous" loop effect
        """
        ref_position, ref_orientation, navpoint = self.sample_random_viewpoint()
        ref_observations = self.render_viewpoint(ref_position, ref_orientation)
        ref_pointcloud = compute_pointcloud(
            depthmap=ref_observations["depth"],
            hfov=self.hfov,
            camera_position=ref_position,
            camera_rotation=ref_orientation,
        )
        pixels_count = self.resolution[0] * self.resolution[1]
        if len(ref_pointcloud) / pixels_count < self.minimum_valid_fraction:
            # Dirty hack: ensure that the valid part of the image is significant
            return self.generate_random_spiral_trajectory(
                images_count, max_radius, half_turns, use_constant_orientation
            )

        # Pick an observed point in the point cloud
        observed_point = np.mean(ref_pointcloud, axis=0)
        ref_R, ref_t = compute_camera_pose_opencv_convention(
            ref_position, ref_orientation
        )

        images = []
        is_valid = []
        # Spiral trajectory, use_constant orientation
        for i, alpha in enumerate(np.linspace(0, 1, images_count)):
            r = max_radius * np.abs(
                np.sin(alpha * np.pi)
            )  # Increase then decrease the radius
            theta = alpha * half_turns * np.pi
            x = r * np.cos(theta)
            y = r * np.sin(theta)
            z = 0.0
            position = (
                ref_position + (ref_R @ np.asarray([x, y, z]).reshape(3, 1)).flatten()
            )
            if use_constant_orientation:
                orientation = ref_orientation
            else:
                # trajectory looking at a mean point in front of the ref observation
                orientation, position = look_at_for_habitat(
                    eye=position, center=observed_point, up=habitat_sim.geo.UP
                )
            observations = self.render_viewpoint(position, orientation)
            images.append(observations["color"][..., :3])
            _is_valid, valid_fraction, iou = self.is_other_viewpoint_overlapping(
                ref_pointcloud, observations, position, orientation
            )
            is_valid.append(_is_valid)
        return images, np.all(is_valid)


def generate_orientation_noise(pan_range, tilt_range, roll_range):
    return (
        quaternion.from_rotation_vector(
            np.random.uniform(*pan_range) * DEG2RAD * habitat_sim.geo.UP
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*tilt_range) * DEG2RAD * habitat_sim.geo.RIGHT
        )
        * quaternion.from_rotation_vector(
            np.random.uniform(*roll_range) * DEG2RAD * habitat_sim.geo.FRONT
        )
    )

class NoNaviguableSpaceError(RuntimeError):
    def __init__(self, *args):
        super().__init__(*args)

class MultiviewHabitatSimGenerator:
    def __init__(
        self,
        scene,
        navmesh,
        scene_dataset_config_file,
        resolution=(240, 320),
        views_count=2,
        hfov=60,
        gpu_id=0,
        size=10000,
        minimum_covisibility=0.5,
        transform=None,
    ):
        self.scene = scene
        self.navmesh = navmesh
        self.scene_dataset_config_file = scene_dataset_config_file
        self.resolution = resolution
        self.views_count = views_count
        assert self.views_count >= 1
        self.hfov = hfov
        self.gpu_id = gpu_id
        self.size = size
        self.transform = transform

        # Noise added to camera orientation
        self.pan_range = (-3, 3)
        self.tilt_range = (-10, 10)
        self.roll_range = (-5, 5)

        # Height range to sample cameras
        self.height_range = (1.2, 1.8)

        # Random steps between the camera views
        self.random_steps_count = 5
        self.random_step_variance = 2.0

        # Minimum fraction of the scene which should be valid (well defined depth)
        self.minimum_valid_fraction = 0.7

        # Distance threshold to see  to select pairs
        self.distance_threshold = 0.05
        # Minimum IoU of a view point cloud with respect to the reference view to be kept.
        self.minimum_covisibility = minimum_covisibility

        # Maximum number of retries.
        self.max_attempts_count = 100

        self.seed = None
        self._lazy_initialization()

    def _lazy_initialization(self):
        # Lazy random seeding and instantiation of the simulator to deal with multiprocessing properly
        if self.seed == None:
            # Re-seed numpy generator
            np.random.seed()
            self.seed = np.random.randint(2**32 - 1)
            sim_cfg = habitat_sim.SimulatorConfiguration()
            sim_cfg.scene_id = self.scene
            if (
                self.scene_dataset_config_file is not None
                and self.scene_dataset_config_file != ""
            ):
                sim_cfg.scene_dataset_config_file = self.scene_dataset_config_file
            sim_cfg.random_seed = self.seed
            sim_cfg.load_semantic_mesh = False
            sim_cfg.gpu_device_id = self.gpu_id

            depth_sensor_spec = habitat_sim.CameraSensorSpec()
            depth_sensor_spec.uuid = "depth"


        self.height_range = (1.2, 1.8)

        # Random steps between the camera views
        self.random_steps_count = 5
        self.random_step_variance = 2.0

        # Minimum fraction of the scene which should be valid (well defined depth)
        self.minimum_valid_fraction = 0.7

        # Distance threshold to see  to select pairs
        self.distance_threshold = 0.05
        # Minimum IoU of a view point cloud with respect to the reference view to be kept.
        self.minimum_covisibility = minimum_covisibility

        # Maximum number of retries.
        self.max_attempts_count = 100

        self.seed = None
        self._lazy_initialization()

    def _lazy_initialization(self):
        # Lazy random seeding and instantiation of the simulator to deal with multiprocessing properly
        if self.seed == None:
            # Re-seed numpy generator
            np.random.seed()
            self.seed = np.random.randint(2**32 - 1)
            sim_cfg = habitat_sim.SimulatorConfiguration()
            sim_cfg.scene_id = self.scene
            if (
                self.scene_dataset_config_file is not None
                and self.scene_dataset_config_file != ""
            ):
                sim_cfg.scene_dataset_config_file = self.scene_dataset_config_file
            sim_cfg.random_seed = self.seed
            sim_cfg.load_semantic_mesh = False
            sim_cfg.gpu_device_id = self.gpu_id

            depth_sensor_spec = habitat_sim.CameraSensorSpec()
            depth_sensor_spec.uuid = "depth"
            depth_sensor_spec.sensor_type = habitat_sim.SensorType.DEPTH
            depth_sensor_spec.resolution = self.resolution
            depth_sensor_spec.hfov = self.hfov
            depth_sensor_spec.position = [0.0, 0.0, 0]
            depth_sensor_spec.orientation

            rgb_sensor_spec = habitat_sim.CameraSensorSpec()
            rgb_sensor_spec.uuid = "color"
            rgb_sensor_spec.sensor_type = habitat_sim.SensorType.COLOR
            rgb_sensor_spec.resolution = self.resolution
            rgb_sensor_spec.hfov = self.hfov
            rgb_sensor_spec.position = [0.0, 0.0, 0]
            agent_cfg = habitat_sim.agent.AgentConfiguration(
                sensor_specifications=[rgb_sensor_spec, depth_sensor_spec]
            )

            cfg = habitat_sim.Configuration(sim_cfg, [agent_cfg])
            self.sim = habitat_sim.Simulator(cfg)
            if self.navmesh is not None and self.navmesh != "":
                # Use pre-computed navmesh when available (usually better than those generated automatically)
                self.sim.pathfinder.load_nav_mesh(self.navmesh)

            if not self.sim.pathfinder.is_loaded:
                # Try to compute a navmesh
                navmesh_settings = habitat_sim.NavMeshSettings()
                navmesh_settings.set_defaults()
                self.sim.recompute_navmesh(self.sim.pathfinder, navmesh_settings, True)

            # Ensure that the navmesh is not empty
            if not self.sim.pathfinder.is_loaded:
                raise NoNaviguableSpaceError(
                    f"No naviguable location (scene: {self.scene} -- navmesh: {self.navmesh})"
                )

            self.agent = self.sim.initialize_agent(agent_id=0)

    def close(self):
        self.sim.close()

    def __del__(self):
        self.sim.close()

    def __len__(self):
        return self.size

    def sample_random_viewpoint(self):
        """Sample a random viewpoint using the navmesh"""
        nav_point = self.sim.pathfinder.get_random_navigable_point()

        # Sample a random viewpoint height
        viewpoint_height = np.random.uniform(*self.height_range)
        viewpoint_position = nav_point + viewpoint_height * habitat_sim.geo.UP
        viewpoint_orientation = quaternion.from_rotation_vector(
            np.random.uniform(0, 2 * np.pi) * habitat_sim.geo.UP
        ) * generate_orientation_noise(self.pan_range, self.tilt_range, self.roll_range)


close

            cfg = habitat_sim.Configuration(sim_cfg, [agent_cfg])
            self.sim = habitat_sim.Simulator(cfg)
            if self.navmesh is not None and self.navmesh != "":
                # Use pre-computed navmesh when available (usually better than those generated automatically)
                self.sim.pathfinder.load_nav_mesh(self.navmesh)

            if not self.sim.pathfinder.is_loaded:
                # Try to compute a navmesh
                navmesh_settings = habitat_sim.NavMeshSettings()
                navmesh_settings.set_defaults()
                self.sim.recompute_navmesh(self.sim.pathfinder, navmesh_settings, True)

            # Ensure that the navmesh is not empty
            if not self.sim.pathfinder.is_loaded:
                raise NoNaviguableSpaceError(
                    f"No naviguable location (scene: {self.scene} -- navmesh: {self.navmesh})"
                )

            self.agent = self.sim.initialize_agent(agent_id=0)

    def close(self):
        self.sim.close()

    def __del__(self):
        self.sim.close()

    def __len__(self):
        return self.size

    def sample_random_viewpoint(self):
        """Sample a random viewpoint using the navmesh"""
        nav_point = self.sim.pathfinder.get_random_navigable_point()

        # Sample a random viewpoint height
        viewpoint_height = np.random.uniform(*self.height_range)
        viewpoint_position = nav_point + viewpoint_height * habitat_sim.geo.UP
        viewpoint_orientation = quaternion.from_rotation_vector(
            np.random.uniform(0, 2 * np.pi) * habitat_sim.geo.UP
        ) * generate_orientation_noise(self.pan_range, self.tilt_range, self.roll_range)
        return viewpoint_position, viewpoint_orientation, nav_point

    def sample_other_random_viewpoint(self, observed_point, nav_point):


                # Use pre-computed navmesh when available (usually better than those generated automatically)
                self.sim.pathfinder.load_nav_mesh(self.navmesh)

            if not self.sim.pathfinder.is_loaded:
                # Try to compute a navmesh
                navmesh_settings = habitat_sim.NavMeshSettings()
                navmesh_settings.set_defaults()
                self.sim.recompute_navmesh(self.sim.pathfinder, navmesh_settings, True)

            # Ensure that the navmesh is not empty
            if not self.sim.pathfinder.is_loaded:
                raise NoNaviguableSpaceError(
                    f"No naviguable location (scene: {self.scene} -- navmesh: {self.navmesh})"
                )

            self.agent = self.sim.initialize_agent(agent_id=0)

    def close(self):
        self.sim.close()

    def __del__(self):
        self.sim.close()

    def __len__(self):
        return self.size

    def sample_random_viewpoint(self):
        """Sample a random viewpoint using the navmesh"""
        nav_point = self.sim.pathfinder.get_random_navigable_point()

        # Sample a random viewpoint height
        viewpoint_height = np.random.uniform(*self.height_range)
        viewpoint_position = nav_point + viewpoint_height * habitat_sim.geo.UP
        viewpoint_orientation = quaternion.from_rotation_vector(
            np.random.uniform(0, 2 * np.pi) * habitat_sim.geo.UP
        ) * generate_orientation_noise(self.pan_range, self.tilt_range, self.roll_range)
        return viewpoint_position, viewpoint_orientation, nav_point

    def sample_other_random_viewpoint(self, observed_point, nav_point):
        """Sample a random viewpoint close to an existing one, using the navmesh and a reference observed point."""
        other_nav_point = nav_point


            if not self.sim.pathfinder.is_loaded:
                # Try to compute a navmesh
                navmesh_settings = habitat_sim.NavMeshSettings()
                navmesh_settings.set_defaults()
                self.sim.recompute_navmesh(self.sim.pathfinder, navmesh_settings, True)

            # Ensure that the navmesh is not empty
            if not self.sim.pathfinder.is_loaded:
                raise NoNaviguableSpaceError(
                    f"No naviguable location (scene: {self.scene} -- navmesh: {self.navmesh})"
                )

            self.agent = self.sim.initialize_agent(agent_id=0)

    def close(self):
        self.sim.close()

    def __del__(self):
        self.sim.close()

    def __len__(self):
        return self.size

    def sample_random_viewpoint(self):
        """Sample a random viewpoint using the navmesh"""
        nav_point = self.sim.pathfinder.get_random_navigable_point()

        # Sample a random viewpoint height
        viewpoint_height = np.random.uniform(*self.height_range)
        viewpoint_position = nav_point + viewpoint_height * habitat_sim.geo.UP
        viewpoint_orientation = quaternion.from_rotation_vector(
            np.random.uniform(0, 2 * np.pi) * habitat_sim.geo.UP
        ) * generate_orientation_noise(self.pan_range, self.tilt_range, self.roll_range)
        return viewpoint_position, viewpoint_orientation, nav_point

    def sample_other_random_viewpoint(self, observed_point, nav_point):
        """Sample a random viewpoint close to an existing one, using the navmesh and a reference observed point."""
        other_nav_point = nav_point

        walk_directions = self.random_step_variance * np.asarray([1, 0, 1])
        for i in range(self.random_steps_count):
            temp = self.sim.pathfinder.snap_point(


                navmesh_settings.set_defaults()
                self.sim.recompute_navmesh(self.sim.pathfinder, navmesh_settings, True)

            # Ensure that the navmesh is not empty
            if not self.sim.pathfinder.is_loaded:
                raise NoNaviguableSpaceError(
                    f"No naviguable location (scene: {self.scene} -- navmesh: {self.navmesh})"
                )

            self.agent = self.sim.initialize_agent(agent_id=0)

    def close(self):
        self.sim.close()

    def __del__(self):
        self.sim.close()

    def __len__(self):
        return self.size

    def sample_random_viewpoint(self):
        """Sample a random viewpoint using the navmesh"""
        nav_point = self.sim.pathfinder.get_random_navigable_point()

        # Sample a random viewpoint height
        viewpoint_height = np.random.uniform(*self.height_range)
        viewpoint_position = nav_point + viewpoint_height * habitat_sim.geo.UP
        viewpoint_orientation = quaternion.from_rotation_vector(
            np.random.uniform(0, 2 * np.pi) * habitat_sim.geo.UP
        ) * generate_orientation_noise(self.pan_range, self.tilt_range, self.roll_range)
        return viewpoint_position, viewpoint_orientation, nav_point

    def sample_other_random_viewpoint(self, observed_point, nav_point):
        """Sample a random viewpoint close to an existing one, using the navmesh and a reference observed point."""
        other_nav_point = nav_point

        walk_directions = self.random_step_variance * np.asarray([1, 0, 1])
        for i in range(self.random_steps_count):
            temp = self.sim.pathfinder.snap_point(
                other_nav_point + walk_directions * np.random.normal(size=3)
            )
            # Snapping may return nan when it fails
            if not np.isnan(temp[0]):
                other_nav_point = temp

        other_viewpoint_height = np.random.uniform(*self.height_range)
        other_viewpoint_position = (
            other_nav_point + other_viewpoint_height * habitat_sim.geo.UP
        )

        # Set viewing direction towards the central point


        self.sim.close()

    def __del__(self):
        self.sim.close()

    def __len__(self):
        return self.size

    def sample_random_viewpoint(self):
        """Sample a random viewpoint using the navmesh"""
        nav_point = self.sim.pathfinder.get_random_navigable_point()

        # Sample a random viewpoint height
        viewpoint_height = np.random.uniform(*self.height_range)
        viewpoint_position = nav_point + viewpoint_height * habitat_sim.geo.UP
        viewpoint_orientation = quaternion.from_rotation_vector(
            np.random.uniform(0, 2 * np.pi) * habitat_sim.geo.UP
        ) * generate_orientation_noise(self.pan_range, self.tilt_range, self.roll_range)
        return viewpoint_position, viewpoint_orientation, nav_point

    def sample_other_random_viewpoint(self, observed_point, nav_point):
        """Sample a random viewpoint close to an existing one, using the navmesh and a reference observed point."""
        other_nav_point = nav_point

        walk_directions = self.random_step_variance * np.asarray([1, 0, 1])
        for i in range(self.random_steps_count):
            temp = self.sim.pathfinder.snap_point(
                other_nav_point + walk_directions * np.random.normal(size=3)
            )
            # Snapping may return nan when it fails
            if not np.isnan(temp[0]):
                other_nav_point = temp

        other_viewpoint_height = np.random.uniform(*self.height_range)
        other_viewpoint_position = (
            other_nav_point + other_viewpoint_height * habitat_sim.geo.UP
        )

        # Set viewing direction towards the central point
        rotation, position = look_at_for_habitat(
            eye=other_viewpoint_position,
            center=observed_point,
            up=habitat_sim.geo.UP,
            return_cam2world=True,
        )
        rotation = rotation * generate_orientation_noise(
            self.pan_range, self.tilt_range, self.roll_range
        )
        return position, rotation, other_nav_point

    def is_other_pointcloud_overlapping(self, ref_pointcloud, other_pointcloud):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        pixels_count = self.resolution[0] * self.resolution[1]
        valid_fraction = len(other_pointcloud) / pixels_count
        assert valid_fraction <= 1.0 and valid_fraction >= 0.0
        overlap = compute_pointcloud_overlaps_scikit(
            ref_pointcloud,
            other_pointcloud,
            self.distance_threshold,
            compute_symmetric=True,
        )
        covisibility = min(
            overlap["intersection1"] / pixels_count,
            overlap["intersection2"] / pixels_count,
        )
        is_valid = (valid_fraction >= self.minimum_valid_fraction) and (
            covisibility >= self.minimum_covisibility
        )


            if not np.isnan(temp[0]):
                other_nav_point = temp

        other_viewpoint_height = np.random.uniform(*self.height_range)
        other_viewpoint_position = (
            other_nav_point + other_viewpoint_height * habitat_sim.geo.UP
        )

        # Set viewing direction towards the central point
        rotation, position = look_at_for_habitat(
            eye=other_viewpoint_position,
            center=observed_point,
            up=habitat_sim.geo.UP,
            return_cam2world=True,
        )
        rotation = rotation * generate_orientation_noise(
            self.pan_range, self.tilt_range, self.roll_range
        )
        return position, rotation, other_nav_point

    def is_other_pointcloud_overlapping(self, ref_pointcloud, other_pointcloud):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        pixels_count = self.resolution[0] * self.resolution[1]
        valid_fraction = len(other_pointcloud) / pixels_count
        assert valid_fraction <= 1.0 and valid_fraction >= 0.0
        overlap = compute_pointcloud_overlaps_scikit(
            ref_pointcloud,
            other_pointcloud,
            self.distance_threshold,
            compute_symmetric=True,
        )
        covisibility = min(
            overlap["intersection1"] / pixels_count,
            overlap["intersection2"] / pixels_count,
        )
        is_valid = (valid_fraction >= self.minimum_valid_fraction) and (
            covisibility >= self.minimum_covisibility
        )
        return is_valid, valid_fraction, covisibility

    def is_other_viewpoint_overlapping(
        self, ref_pointcloud, observation, position, rotation
    ):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        other_pointcloud = compute_pointcloud(
            observation["depth"], self.hfov, position, rotation
        )
        return self.is_other_pointcloud_overlapping(ref_pointcloud, other_pointcloud)

    def render_viewpoint(self, viewpoint_position, viewpoint_orientation):
        agent_state = habitat_sim.AgentState()
        agent_state.position = viewpoint_position
        agent_state.rotation = viewpoint_orientation
        self.agent.set_state(agent_state)
        viewpoint_observations = self.sim.get_sensor_observations(agent_ids=0)
        _append_camera_parameters(
            viewpoint_observations, self.hfov, viewpoint_position, viewpoint_orientation
        )


        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        pixels_count = self.resolution[0] * self.resolution[1]
        valid_fraction = len(other_pointcloud) / pixels_count
        assert valid_fraction <= 1.0 and valid_fraction >= 0.0
        overlap = compute_pointcloud_overlaps_scikit(
            ref_pointcloud,
            other_pointcloud,
            self.distance_threshold,
            compute_symmetric=True,
        )
        covisibility = min(
            overlap["intersection1"] / pixels_count,
            overlap["intersection2"] / pixels_count,
        )
        is_valid = (valid_fraction >= self.minimum_valid_fraction) and (
            covisibility >= self.minimum_covisibility
        )
        return is_valid, valid_fraction, covisibility

    def is_other_viewpoint_overlapping(
        self, ref_pointcloud, observation, position, rotation
    ):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        other_pointcloud = compute_pointcloud(
            observation["depth"], self.hfov, position, rotation
        )
        return self.is_other_pointcloud_overlapping(ref_pointcloud, other_pointcloud)

    def render_viewpoint(self, viewpoint_position, viewpoint_orientation):
        agent_state = habitat_sim.AgentState()
        agent_state.position = viewpoint_position
        agent_state.rotation = viewpoint_orientation
        self.agent.set_state(agent_state)
        viewpoint_observations = self.sim.get_sensor_observations(agent_ids=0)
        _append_camera_parameters(
            viewpoint_observations, self.hfov, viewpoint_position, viewpoint_orientation
        )
        return viewpoint_observations

    def __getitem__(self, useless_idx):
        ref_position, ref_orientation, nav_point = self.sample_random_viewpoint()
        ref_observations = self.render_viewpoint(ref_position, ref_orientation)
        # Extract point cloud
        ref_pointcloud = compute_pointcloud(
            depthmap=ref_observations["depth"],
            hfov=self.hfov,
            camera_position=ref_position,


        )
        covisibility = min(
            overlap["intersection1"] / pixels_count,
            overlap["intersection2"] / pixels_count,
        )
        is_valid = (valid_fraction >= self.minimum_valid_fraction) and (
            covisibility >= self.minimum_covisibility
        )
        return is_valid, valid_fraction, covisibility

    def is_other_viewpoint_overlapping(
        self, ref_pointcloud, observation, position, rotation
    ):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        other_pointcloud = compute_pointcloud(
            observation["depth"], self.hfov, position, rotation
        )
        return self.is_other_pointcloud_overlapping(ref_pointcloud, other_pointcloud)

    def render_viewpoint(self, viewpoint_position, viewpoint_orientation):
        agent_state = habitat_sim.AgentState()
        agent_state.position = viewpoint_position
        agent_state.rotation = viewpoint_orientation
        self.agent.set_state(agent_state)
        viewpoint_observations = self.sim.get_sensor_observations(agent_ids=0)
        _append_camera_parameters(
            viewpoint_observations, self.hfov, viewpoint_position, viewpoint_orientation
        )
        return viewpoint_observations

    def __getitem__(self, useless_idx):
        ref_position, ref_orientation, nav_point = self.sample_random_viewpoint()
        ref_observations = self.render_viewpoint(ref_position, ref_orientation)
        # Extract point cloud
        ref_pointcloud = compute_pointcloud(
            depthmap=ref_observations["depth"],
            hfov=self.hfov,
            camera_position=ref_position,
            camera_rotation=ref_orientation,
        )

        pixels_count = self.resolution[0] * self.resolution[1]
        ref_valid_fraction = len(ref_pointcloud) / pixels_count
        assert ref_valid_fraction <= 1.0 and ref_valid_fraction >= 0.0
        if ref_valid_fraction < self.minimum_valid_fraction:
            # This should produce a recursion error at some point when something is very wrong.
            return self[0]
        # Pick an reference observed point in the point cloud
        observed_point = np.mean(ref_pointcloud, axis=0)


        self, ref_pointcloud, observation, position, rotation
    ):
        """Check if a viewpoint is valid and overlaps significantly with a reference one."""
        # Observation
        other_pointcloud = compute_pointcloud(
            observation["depth"], self.hfov, position, rotation
        )
        return self.is_other_pointcloud_overlapping(ref_pointcloud, other_pointcloud)

    def render_viewpoint(self, viewpoint_position, viewpoint_orientation):
        agent_state = habitat_sim.AgentState()
        agent_state.position = viewpoint_position
        agent_state.rotation = viewpoint_orientation
        self.agent.set_state(agent_state)
        viewpoint_observations = self.sim.get_sensor_observations(agent_ids=0)
        _append_camera_parameters(
            viewpoint_observations, self.hfov, viewpoint_position, viewpoint_orientation
        )
        return viewpoint_observations

    def __getitem__(self, useless_idx):
        ref_position, ref_orientation, nav_point = self.sample_random_viewpoint()
        ref_observations = self.render_viewpoint(ref_position, ref_orientation)
        # Extract point cloud
        ref_pointcloud = compute_pointcloud(
            depthmap=ref_observations["depth"],
            hfov=self.hfov,
            camera_position=ref_position,
            camera_rotation=ref_orientation,
        )

        pixels_count = self.resolution[0] * self.resolution[1]
        ref_valid_fraction = len(ref_pointcloud) / pixels_count
        assert ref_valid_fraction <= 1.0 and ref_valid_fraction >= 0.0
        if ref_valid_fraction < self.minimum_valid_fraction:
            # This should produce a recursion error at some point when something is very wrong.
            return self[0]
        # Pick an reference observed point in the point cloud
        observed_point = np.mean(ref_pointcloud, axis=0)

        # Add the first image as reference
        viewpoints_observations = [ref_observations]
        viewpoints_covisibility = [ref_valid_fraction]
        viewpoints_positions = [ref_position]
        viewpoints_orientations = [quaternion.as_float_array(ref_orientation)]
        viewpoints_clouds = [ref_pointcloud]
        viewpoints_valid_fractions = [ref_valid_fraction]

        for _ in range(self.views_count - 1):
            # Generate an other viewpoint using some dummy random walk
            successful_sampling = False
            for sampling_attempt in range(self.max_attempts_count):
                position, rotation, _ = self.sample_other_random_viewpoint(
                    observed_point, nav_point
                )
                # Observation
                other_viewpoint_observations = self.render_viewpoint(position, rotation)
                other_pointcloud = compute_pointcloud(
                    other_viewpoint_observations["depth"], self.hfov, position, rotation
                )

                is_valid, valid_fraction, covisibility = (
                    self.is_other_pointcloud_overlapping(
                        ref_pointcloud, other_pointcloud
                    )
                )
                if is_valid:
                    successful_sampling = True
                    break
            if not successful_sampling:
                print("WARNING: Maximum number of attempts reached.")
                # Dirty hack, try using a novel original viewpoint
                return self[0]
            viewpoints_observations.append(other_viewpoint_observations)
            viewpoints_covisibility.append(covisibility)
            viewpoints_positions.append(position)
            viewpoints_orientations.append(
                quaternion.as_float_array(rotation)
            )  # WXYZ convention for the quaternion encoding.
            viewpoints_clouds.append(other_pointcloud)
            viewpoints_valid_fractions.append(valid_fraction)

        # Estimate relations between all pairs of images
        pairwise_visibility_ratios = np.ones(
            (len(viewpoints_observations), len(viewpoints_observations))
        )
        for i in range(len(viewpoints_observations)):
            pairwise_visibility_ratios[i, i] = viewpoints_valid_fractions[i]
            for j in range(i + 1, len(viewpoints_observations)):
                overlap = compute_pointcloud_overlaps_scikit(
                    viewpoints_clouds[i],
                    viewpoints_clouds[j],
                    self.distance_threshold,
                    compute_symmetric=True,
                )
                pairwise_visibility_ratios[i, j] = (
                    overlap["intersection1"] / pixels_count
                )
                pairwise_visibility_ratios[j, i] = (
                    overlap["intersection2"] / pixels_count
                )

        # IoU is relative to the image 0
        data = {
            "observations": viewpoints_observations,
            "positions": np.asarray(viewpoints_positions),
            "orientations": np.asarray(viewpoints_orientations),
            "covisibility_ratios": np.asarray(viewpoints_covisibility),
            "valid_fractions": np.asarray(viewpoints_valid_fractions, dtype=float),
            "pairwise_visibility_ratios": np.asarray(
                pairwise_visibility_ratios, dtype=float
            ),
        }

        if self.transform is not None:
            data = self.transform(data)
        return data

    def generate_random_spiral_trajectory(
        self,
        images_count=100,
        max_radius=0.5,
        half_turns=5,
        use_constant_orientation=False,
    ):
        """
        Return a list of images corresponding to a spiral trajectory from a random starting point.
        Useful to generate nice visualisations.
        Use an even number of half turns to get a nice "C1-continuous" loop effect
        """
        ref_position, ref_orientation, navpoint = self.sample_random_viewpoint()
        ref_observations = self.render_viewpoint(ref_position, ref_orientation)
        ref_pointcloud = compute_pointcloud(
            depthmap=ref_observations["depth"],
            hfov=self.hfov,
            camera_position=ref_position,
            camera_rotation=ref_orientation,


                pairwise_visibility_ratios[j, i] = (
                    overlap["intersection2"] / pixels_count
                )

        # IoU is relative to the image 0
        data = {
            "observations": viewpoints_observations,
            "positions": np.asarray(viewpoints_positions),
            "orientations": np.asarray(viewpoints_orientations),
            "covisibility_ratios": np.asarray(viewpoints_covisibility),
            "valid_fractions": np.asarray(viewpoints_valid_fractions, dtype=float),
            "pairwise_visibility_ratios": np.asarray(
                pairwise_visibility_ratios, dtype=float
            ),
        }

        if self.transform is not None:
            data = self.transform(data)
        return data

    def generate_random_spiral_trajectory(
        self,
        images_count=100,
        max_radius=0.5,
        half_turns=5,
        use_constant_orientation=False,
    ):
        """
        Return a list of images corresponding to a spiral trajectory from a random starting point.
        Useful to generate nice visualisations.
        Use an even number of half turns to get a nice "C1-continuous" loop effect
        """
        ref_position, ref_orientation, navpoint = self.sample_random_viewpoint()
        ref_observations = self.render_viewpoint(ref_position, ref_orientation)
        ref_pointcloud = compute_pointcloud(
            depthmap=ref_observations["depth"],
            hfov=self.hfov,
            camera_position=ref_position,
            camera_rotation=ref_orientation,
        )
        pixels_count = self.resolution[0] * self.resolution[1]
        if len(ref_pointcloud) / pixels_count < self.minimum_valid_fraction:
            # Dirty hack: ensure that the valid part of the image is significant
            return self.generate_random_spiral_trajectory(
                images_count, max_radius, half_turns, use_constant_orientation
            )

        # Pick an observed point in the point cloud
        observed_point = np.mean(ref_pointcloud, axis=0)
        ref_R, ref_t = compute_camera_pose_opencv_convention(
            ref_position, ref_orientation
        )

        images = []
        is_valid = []
        # Spiral trajectory, use_constant orientation
        for i, alpha in enumerate(np.linspace(0, 1, images_count)):
            r = max_radius * np.abs(
                np.sin(alpha * np.pi)
            )  # Increase then decrease the radius
            theta = alpha * half_turns * np.pi
            x = r * np.cos(theta)
            y = r * np.sin(theta)
            z = 0.0
            position = (
                ref_position + (ref_R @ np.asarray([x, y, z]).reshape(3, 1)).flatten()
            )
            if use_constant_orientation:
                orientation = ref_orientation
            else:
                # trajectory looking at a mean point in front of the ref observation
                orientation, position = look_at_for_habitat(
                    eye=position, center=observed_point, up=habitat_sim.geo.UP
                )
            observations = self.render_viewpoint(position, orientation)
            images.append(observations["color"][..., :3])
            _is_valid, valid_fraction, iou = self.is_other_viewpoint_overlapping(
                ref_pointcloud, observations, position, orientation
            )
            is_valid.append(_is_valid)
        return images, np.all(is_valid)


# Copyright (C) 2022-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).
"""
Utility script to pack metadata files of the dataset in order to be able to re-generate it elsewhere.
"""
import os
import glob
from tqdm import tqdm
import shutil
import json
from datasets.habitat_sim.paths import *
import argparse
import collections

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("input_dir")
    parser.add_argument("output_dir")
    args = parser.parse_args()

    input_dirname = args.input_dir
    output_dirname = args.output_dir

    input_metadata_filenames = glob.iglob(
        f"{input_dirname}/**/metadata.json", recursive=True
    )

    images_count = collections.defaultdict(lambda: 0)

    os.makedirs(output_dirname)
    for input_filename in tqdm(input_metadata_filenames):
        # Ignore empty files
        with open(input_filename, "r") as f:
            original_metadata = json.load(f)
            if (
                "multiviews" not in original_metadata
                or len(original_metadata["multiviews"]) == 0
            ):
                print("No views in", input_filename)
                continue

        relpath = os.path.relpath(input_filename, input_dirname)
        print(relpath)

        # Copy metadata, while replacing scene paths by generic keys depending on the dataset, for portability.
        # Data paths are sorted by decreasing length to avoid potential bugs due to paths starting by the same string pattern.
        scenes_dataset_paths = dict(
            sorted(SCENES_DATASET.items(), key=lambda x: len(x[1]), reverse=True)
        )
        metadata = dict()
        for key, value in original_metadata.items():
            if key in ("scene_dataset_config_file", "scene", "navmesh") and value != "":
                known_path = False
                for dataset, dataset_path in scenes_dataset_paths.items():
                    if value.startswith(dataset_path):
                        value = os.path.join(
                            dataset, os.path.relpath(value, dataset_path)
                        )
                        known_path = True
                        break
                if not known_path:
                    raise KeyError("Unknown path:" + value)
            metadata[key] = value

        # Compile some general statistics while packing data
        scene_split = metadata["scene"].split("/")
        upper_level = (
            "/".join(scene_split[:2]) if scene_split[0] == "hm3d" else scene_split[0]
        )
        images_count[upper_level] += len(metadata["multiviews"])

        output_filename = os.path.join(output_dirname, relpath)
        os.makedirs(os.path.dirname(output_filename), exist_ok=True)
        with open(output_filename, "w") as f:
            json.dump(metadata, f)

    # Print statistics
    print("Images count:")
    for upper_level, count in images_count.items():
        print(f"- {upper_level}: {count}")


# Copyright (C) 2022-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).

"""
Script generating commandlines to generate image pairs from metadata files.
"""
import os
import glob
from tqdm import tqdm
import argparse

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--input_dir", required=True)
    parser.add_argument("--output_dir", required=True)
    parser.add_argument(
        "--prefix",
        default="",
        help="Commanline prefix, useful e.g. to setup environment.",
    )
    args = parser.parse_args()

    input_metadata_filenames = glob.iglob(
        f"{args.input_dir}/**/metadata.json", recursive=True
    )

    for metadata_filename in tqdm(input_metadata_filenames):
        output_dir = os.path.join(
            args.output_dir,
            os.path.relpath(os.path.dirname(metadata_filename), args.input_dir),
        )
        # Do not process the scene if the metadata file already exists
        if os.path.exists(os.path.join(output_dir, "metadata.json")):
            continue
        commandline = f"{args.prefix}python datasets/habitat_sim/generate_from_metadata.py --metadata_filename={metadata_filename} --output_dir={output_dir}"
        print(commandline)


# Copyright (C) 2022-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).

"""
Script to generate image pairs for a given scene reproducing poses provided in a metadata file.
"""
import os
from datasets.habitat_sim.multiview_habitat_sim_generator import (
    MultiviewHabitatSimGenerator,
)
from datasets.habitat_sim.paths import SCENES_DATASET
import argparse
import quaternion
import PIL.Image
import cv2
import json
from tqdm import tqdm

def generate_multiview_images_from_metadata(
    metadata_filename,
    output_dir,
    overload_params=dict(),
    scene_datasets_paths=None,
    exist_ok=False,
):
    """
    Generate images from a metadata file for reproducibility purposes.
    """
    # Reorder paths by decreasing label length, to avoid collisions when testing if a string by such label
    if scene_datasets_paths is not None:
        scene_datasets_paths = dict(
            sorted(scene_datasets_paths.items(), key=lambda x: len(x[0]), reverse=True)
        )

    with open(metadata_filename, "r") as f:
        input_metadata = json.load(f)
    metadata = dict()
    for key, value in input_metadata.items():
        # Optionally replace some paths
        if key in ("scene_dataset_config_file", "scene", "navmesh") and value != "":
            if scene_datasets_paths is not None:
                for dataset_label, dataset_path in scene_datasets_paths.items():
                    if value.startswith(dataset_label):
                        value = os.path.normpath(
                            os.path.join(
                                dataset_path, os.path.relpath(value, dataset_label)
                            )
                        )
                        break
        metadata[key] = value

    # Overload some parameters
    for key, value in overload_params.items():
        metadata[key] = value

    generation_entries = dict(
        [
            (key, value)
            for key, value in metadata.items()
            if not (key in ("multiviews", "output_dir", "generate_depth"))
        ]
    )
    generate_depth = metadata["generate_depth"]

    os.makedirs(output_dir, exist_ok=exist_ok)

    generator = MultiviewHabitatSimGenerator(**generation_entries)

    # Generate views
    for idx_label, data in tqdm(metadata["multiviews"].items()):
        positions = data["positions"]
        orientations = data["orientations"]
        n = len(positions)
        for oidx in range(n):
            observation = generator.render_viewpoint(
                positions[oidx], quaternion.from_float_array(orientations[oidx])
            )
            observation_label = f"{oidx + 1}"  # Leonid is indexing starting from 1
            # Color image saved using PIL
            img = PIL.Image.fromarray(observation["color"][:, :, :3])
            filename = os.path.join(output_dir, f"{idx_label}_{observation_label}.jpeg")
            img.save(filename)
            if generate_depth:
                # Depth image as EXR file
                filename = os.path.join(
                    output_dir, f"{idx_label}_{observation_label}_depth.exr"
                )
                cv2.imwrite(
                    filename,
                    observation["depth"],
                    [cv2.IMWRITE_EXR_TYPE, cv2.IMWRITE_EXR_TYPE_HALF],
                )
                # Camera parameters
                camera_params = dict(
                    [
                        (key, observation[key].tolist())
                        for key in ("camera_intrinsics", "R_cam2world", "t_cam2world")
                    ]
                )
                filename = os.path.join(
                    output_dir, f"{idx_label}_{observation_label}_camera_params.json"
                )
                with open(filename, "w") as f:
                    json.dump(camera_params, f)
                # Save metadata
    with open(os.path.join(output_dir, "metadata.json"), "w") as f:
        json.dump(metadata, f)

    generator.close()

if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--metadata_filename", required=True)
    parser.add_argument("--output_dir", required=True)
    args = parser.parse_args()

    generate_multiview_images_from_metadata(
        metadata_filename=args.metadata_filename,
        output_dir=args.output_dir,
        scene_datasets_paths=SCENES_DATASET,
        overload_params=dict(),
        exist_ok=True,
    )


# Copyright (C) 2024-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).
#
# --------------------------------------------------------
# modified from DUSt3R

import torch
import torch.nn as nn

from itertools import repeat
import collections.abc
from torch.nn.functional import scaled_dot_product_attention
from functools import partial

def _ntuple(n):
    def parse(x):
        if isinstance(x, collections.abc.Iterable) and not isinstance(x, str):
            return x
        return tuple(repeat(x, n))

    return parse

to_2tuple = _ntuple(2)

def drop_path(
    x, drop_prob: float = 0.0, training: bool = False, scale_by_keep: bool = True
):
    """Drop paths (Stochastic Depth) per sample (when applied in main path of residual blocks)."""
    if drop_prob == 0.0 or not training:
        return x
    keep_prob = 1 - drop_prob
    shape = (x.shape[0],) + (1,) * (
        x.ndim - 1
    )  # work with diff dim tensors, not just 2D ConvNets
    random_tensor = x.new_empty(shape).bernoulli_(keep_prob)
    if keep_prob > 0.0 and scale_by_keep:
        random_tensor.div_(keep_prob)
    return x * random_tensor

class DropPath(nn.Module):
    """Drop paths (Stochastic Depth) per sample  (when applied in main path of residual blocks)."""

    def __init__(self, drop_prob: float = 0.0, scale_by_keep: bool = True):
        super(DropPath, self).__init__()
        self.drop_prob = drop_prob
        self.scale_by_keep = scale_by_keep

    def forward(self, x):
        return drop_path(x, self.drop_prob, self.training, self.scale_by_keep)

    def extra_repr(self):
        return f"drop_prob={round(self.drop_prob,3):0.3f}"

class Mlp(nn.Module):
    """MLP as used in Vision Transformer, MLP-Mixer and related networks"""

    def __init__(
        self,
        in_features,
        hidden_features=None,
        out_features=None,
        act_layer=nn.GELU,
        bias=True,
        drop=0.0,
    ):
        super().__init__()
        out_features = out_features or in_features
        hidden_features = hidden_features or in_features
        bias = to_2tuple(bias)
        drop_probs = to_2tuple(drop)

        self.fc1 = nn.Linear(in_features, hidden_features, bias=bias[0])
        self.act = act_layer()
        self.drop1 = nn.Dropout(drop_probs[0])
        self.fc2 = nn.Linear(hidden_features, out_features, bias=bias[1])
        self.drop2 = nn.Dropout(drop_probs[1])

    def forward(self, x):
        return self.drop2(self.fc2(self.drop1(self.act(self.fc1(x)))))

class Attention(nn.Module):

    def __init__(
        self, dim, rope=None, num_heads=8, qkv_bias=False, attn_drop=0.0, proj_drop=0.0
    ):
        super().__init__()
        self.num_heads = num_heads
        head_dim = dim // num_heads
        self.scale = head_dim**-0.5
        self.qkv = nn.Linear(dim, dim * 3, bias=qkv_bias)
        self.attn_drop = nn.Dropout(attn_drop)
        self.proj = nn.Linear(dim, dim)
        self.proj_drop = nn.Dropout(proj_drop)
        self.rope = rope.float() if rope is not None else None

    def forward(self, x, xpos):
        B, N, C = x.shape

        qkv = (
            self.qkv(x)
            .reshape(B, N, 3, self.num_heads, C // self.num_heads)
            .transpose(1, 3)
        )
        q, k, v = [qkv[:, :, i] for i in range(3)]

        q_type = q.dtype
        k_type = k.dtype
        if self.rope is not None:
            q = q.float()
            k = k.float()
            with torch.autocast(device_type="cuda", enabled=False):
                q = self.rope(q, xpos)
                k = self.rope(k, xpos)
            q = q.to(q_type)
            k = k.to(k_type)

        x = (
            scaled_dot_product_attention(
                query=q, key=k, value=v, dropout_p=self.attn_drop.p, scale=self.scale
            )
            .transpose(1, 2)
            .reshape(B, N, C)
        )

        x = self.proj(x)
        x = self.proj_drop(x)
        return x

class Block(nn.Module):

    def __init__(
        self,
        dim,
        num_heads,
        mlp_ratio=4.0,
        qkv_bias=False,
        drop=0.0,
        attn_drop=0.0,
        drop_path=0.0,
        act_layer=nn.GELU,
        norm_layer=nn.LayerNorm,
        rope=None,
    ):
        super().__init__()
        self.norm1 = norm_layer(dim)
        self.attn = Attention(
            dim,
            rope=rope,
            num_heads=num_heads,
            qkv_bias=qkv_bias,
            attn_drop=attn_drop,
            proj_drop=drop,
        )

        self.drop_path = DropPath(drop_path) if drop_path > 0.0 else nn.Identity()
        self.norm2 = norm_layer(dim)
        mlp_hidden_dim = int(dim * mlp_ratio)
        self.mlp = Mlp(
            in_features=dim,
            hidden_features=mlp_hidden_dim,
            act_layer=act_layer,
            drop=drop,
        )

    def forward(self, x, xpos):
        x = x + self.drop_path(self.attn(self.norm1(x), xpos))
        x = x + self.drop_path(self.mlp(self.norm2(x)))
        return x

class CrossAttention(nn.Module):

    def __init__(
        self, dim, rope=None, num_heads=8, qkv_bias=False, attn_drop=0.0, proj_drop=0.0
    ):
        super().__init__()
        self.num_heads = num_heads
        head_dim = dim // num_heads
        self.scale = head_dim**-0.5

        self.projq = nn.Linear(dim, dim, bias=qkv_bias)
        self.projk = nn.Linear(dim, dim, bias=qkv_bias)
        self.projv = nn.Linear(dim, dim, bias=qkv_bias)
        self.attn_drop = nn.Dropout(attn_drop)
        self.proj = nn.Linear(dim, dim)
        self.proj_drop = nn.Dropout(proj_drop)

        self.rope = rope.float() if rope is not None else None

    def forward(self, query, key, value, qpos, kpos, use_ttt3r=False):
        B, Nq, C = query.shape
        Nk = key.shape[1]
        Nv = value.shape[1]

        q = (
            self.projq(query)
            .reshape(B, Nq, self.num_heads, C // self.num_heads)
            .permute(0, 2, 1, 3)
        )
        k = (
            self.projk(key)
            .reshape(B, Nk, self.num_heads, C // self.num_heads)
            .permute(0, 2, 1, 3)
        )
        v = (
            self.projv(value)
            .reshape(B, Nv, self.num_heads, C // self.num_heads)
            .permute(0, 2, 1, 3)
        )

        q_type = q.dtype
        k_type = k.dtype
        if self.rope is not None:
            if qpos is not None:
                q = q.float()
                with torch.autocast(device_type="cuda", enabled=False):
                    q = self.rope(q, qpos)
                q = q.to(q_type)

            if kpos is not None:
                k = k.float()
                with torch.autocast(device_type="cuda", enabled=False):
                    k = self.rope(k, kpos)
                k = k.to(k_type)

        if use_ttt3r:
            # original attention
            attn = (q @ k.transpose(-2, -1)) * self.scale  # [B, num_heads, Nq, Nk] [1, 16, 768, 1 + 576]
            attn_before_softmax = attn.detach().clone()

            attn = attn.softmax(dim=-1)
            attn = self.attn_drop(attn)
            x = (attn @ v).transpose(1, 2).reshape(B, Nq, C) # [B, Nq, C] [1, 768, 768]

            x = self.proj(x)
            x = self.proj_drop(x)

            return x, attn_before_softmax
        else:
            x = (
                scaled_dot_product_attention(
                    query=q, key=k, value=v, dropout_p=self.attn_drop.p, scale=self.scale
                )
                .transpose(1, 2)
                .reshape(B, Nq, C)
            )

            x = self.proj(x)
            x = self.proj_drop(x)
            return x, None

class DecoderBlock(nn.Module):

    def __init__(
        self,
        dim,
        num_heads,
        mlp_ratio=4.0,
        qkv_bias=False,
        drop=0.0,
        attn_drop=0.0,
        drop_path=0.0,
        act_layer=nn.GELU,
        norm_layer=nn.LayerNorm,
        norm_mem=True,
        rope=None,
    ):
        super().__init__()
        self.norm1 = norm_layer(dim)
        self.attn = Attention(
            dim,
            rope=rope,
            num_heads=num_heads,
            qkv_bias=qkv_bias,
            attn_drop=attn_drop,
            proj_drop=drop,
        )
        self.cross_attn = CrossAttention(
            dim,
            rope=rope,
            num_heads=num_heads,
            qkv_bias=qkv_bias,
            attn_drop=attn_drop,
            proj_drop=drop,
        )
        self.drop_path = DropPath(drop_path) if drop_path > 0.0 else nn.Identity()
        self.norm2 = norm_layer(dim)
        self.norm3 = norm_layer(dim)
        mlp_hidden_dim = int(dim * mlp_ratio)
        self.mlp = Mlp(
            in_features=dim,
            hidden_features=mlp_hidden_dim,
            act_layer=act_layer,
            drop=drop,
        )
        self.norm_y = norm_layer(dim) if norm_mem else nn.Identity()

    def forward(self, x, y, xpos, ypos, use_ttt3r=False):
        x = x + self.drop_path(self.attn(self.norm1(x), xpos))
        y_ = self.norm_y(y)
        cross_attn_output, cross_attn = self.cross_attn(self.norm2(x), y_, y_, xpos, ypos, use_ttt3r=use_ttt3r)
        x = x + self.drop_path(cross_attn_output)
        x = x + self.drop_path(self.mlp(self.norm3(x)))
        return x, y, cross_attn

class CustomDecoderBlock(nn.Module):

    def __init__(
        self,
        dim,
        num_heads,
        mlp_ratio=4.0,
        qkv_bias=False,
        drop=0.0,
        attn_drop=0.0,
        drop_path=0.0,
        act_layer=nn.GELU,
        norm_layer=nn.LayerNorm,
        norm_mem=True,
        rope=None,
    ):
        super().__init__()
        self.norm1 = norm_layer(dim)
        self.attn = Attention(
            dim,
            rope=rope,
            num_heads=num_heads,
            qkv_bias=qkv_bias,
            attn_drop=attn_drop,
            proj_drop=drop,
        )
        self.cross_attn = CrossAttention(
            dim,
            rope=rope,
            num_heads=num_heads,
            qkv_bias=qkv_bias,
            attn_drop=attn_drop,
            proj_drop=drop,
        )
        self.drop_path = DropPath(drop_path) if drop_path > 0.0 else nn.Identity()
        self.norm2 = norm_layer(dim)
        self.norm3 = norm_layer(dim)
        mlp_hidden_dim = int(dim * mlp_ratio)
        self.mlp = Mlp(
            in_features=dim,
            hidden_features=mlp_hidden_dim,
            act_layer=act_layer,
            drop=drop,
        )
        self.norm_y = norm_layer(dim) if norm_mem else nn.Identity()
        self.norm_z = norm_layer(dim) if norm_mem else nn.Identity()

    def forward(self, x, y, z, xpos, ypos):
        x = x + self.drop_path(self.attn(self.norm1(x), xpos))
        y_ = self.norm_y(y)
        z_ = self.norm_z(z)
        x = x + self.drop_path(self.cross_attn(self.norm2(x), y_, z_, xpos, ypos))
        x = x + self.drop_path(self.mlp(self.norm3(x)))
        return x, y

class ModLN(nn.Module):
    """
    Modulation with adaLN.

    References:
    DiT: https://github.com/facebookresearch/DiT/blob/main/models.py#L101
    """

    def __init__(self, inner_dim: int, mod_dim: int, eps: float):
        super().__init__()
        self.norm = nn.LayerNorm(inner_dim, eps=eps)
        self.mlp = nn.Sequential(
            nn.SiLU(),
            nn.Linear(mod_dim, inner_dim * 2),
        )

    @staticmethod
    def modulate(x, shift, scale):

        return x * (1 + scale.unsqueeze(1)) + shift.unsqueeze(1)

    def forward(self, x: torch.Tensor, mod: torch.Tensor) -> torch.Tensor:
        shift, scale = self.mlp(mod).chunk(2, dim=-1)  # [N, D]
        return self.modulate(self.norm(x), shift, scale)  # [N, L, D]

class ConditionModulationBlock(nn.Module):

    def __init__(
        self,
        dim,
        num_heads,
        mlp_ratio=4.0,
        qkv_bias=False,
        drop=0.0,
        attn_drop=0.0,
        drop_path=0.0,
        act_layer=nn.GELU,
        norm_layer=partial(ModLN, eps=1e-6),
        rope=None,
    ):
        super().__init__()
        self.norm1 = norm_layer(dim, dim)
        self.attn = Attention(
            dim,
            rope=rope,
            num_heads=num_heads,
            qkv_bias=qkv_bias,
            attn_drop=attn_drop,
            proj_drop=drop,
        )
        self.drop_path = DropPath(drop_path) if drop_path > 0.0 else nn.Identity()
        self.norm2 = norm_layer(dim, dim)
        mlp_hidden_dim = int(dim * mlp_ratio)
        self.mlp = Mlp(
            in_features=dim,
            hidden_features=mlp_hidden_dim,
            act_layer=act_layer,
            drop=drop,
        )

    def forward(self, x, mod, xpos):
        x = x + self.drop_path(self.attn(self.norm1(x, mod), xpos))
        x = x + self.drop_path(self.mlp(self.norm2(x, mod)))
        return x

class PositionGetter(object):
    """return positions of patches"""

    def __init__(self):
        self.cache_positions = {}

    def __call__(self, b, h, w, device):
        if not (h, w) in self.cache_positions:
            x = torch.arange(w, device=device)
            y = torch.arange(h, device=device)
            self.cache_positions[h, w] = torch.cartesian_prod(y, x)  # (h, w, 2)
        pos = self.cache_positions[h, w].view(1, h * w, 2).expand(b, -1, 2).clone()
        return pos

class PatchEmbed(nn.Module):
    """just adding _init_weights + position getter compared to timm.models.layers.patch_embed.PatchEmbed"""

    def __init__(
        self,
        img_size=224,
        patch_size=16,
        in_chans=3,
        embed_dim=768,
        norm_layer=None,
        flatten=True,
    ):
        super().__init__()
        img_size = to_2tuple(img_size)
        patch_size = to_2tuple(patch_size)
        self.img_size = img_size
        self.patch_size = patch_size
        self.grid_size = (img_size[0] // patch_size[0], img_size[1] // patch_size[1])
        self.num_patches = self.grid_size[0] * self.grid_size[1]
        self.flatten = flatten

        self.proj = nn.Conv2d(
            in_chans, embed_dim, kernel_size=patch_size, stride=patch_size
        )
        self.norm = norm_layer(embed_dim) if norm_layer else nn.Identity()

        self.position_getter = PositionGetter()

    def forward(self, x):
        B, C, H, W = x.shape
        torch._assert(
            H == self.img_size[0],
            f"Input image height ({H}) doesn't match model ({self.img_size[0]}).",
        )
        torch._assert(
            W == self.img_size[1],
            f"Input image width ({W}) doesn't match model ({self.img_size[1]}).",
        )
        x = self.proj(x)
        pos = self.position_getter(B, x.size(2), x.size(3), x.device)
        if self.flatten:
            x = x.flatten(2).transpose(1, 2)  # BCHW -> BNC
        x = self.norm(x)
        return x, pos

    def _init_weights(self):
        w = self.proj.weight.data
        torch.nn.init.xavier_uniform_(w.view([w.shape[0], -1]))

if __name__ == "__main__":
    import os
    import sys

    sys.path.append(os.path.dirname(os.path.dirname(__file__)))
    import dust3r.utils.path_to_croco
    from models.pos_embed import get_2d_sincos_pos_embed, RoPE2D
    from functools import partial
    from torch.utils.checkpoint import checkpoint

    torch.manual_seed(0)

    enc_blocks_ray_map = (
        nn.ModuleList(
            [
                Block(
                    768,
                    16,
                    4,
                    qkv_bias=True,
                    norm_layer=partial(nn.LayerNorm, eps=1e-6),
                    rope=RoPE2D(100),
                )
                for _ in range(2)
            ]
        )
        .cuda()
        .train()
    )

    x = torch.randn(2, 196, 768, requires_grad=True).cuda()
    xpos = torch.arange(0, 196).unsqueeze(0).unsqueeze(-1).repeat(2, 1, 2).cuda().long()
    enc_blocks_ray_map.zero_grad()
    for blk in enc_blocks_ray_map:

        x = checkpoint(blk, x, xpos)
    enc_blocks_ray_map.zero_grad()
    x.sum().backward()

    grad_not_checkpointed = {}
    for name, param in enc_blocks_ray_map.named_parameters():
        grad_not_checkpointed[name] = param.grad.data.clone()
        print(name, grad_not_checkpointed[name])
        break


# Copyright (C) 2024-present Naver Corporation. All rights reserved.
# Licensed under CC BY-NC-SA 4.0 (non-commercial use only).
#
# --------------------------------------------------------
# modified from DUSt3R

import torch
import torch.nn as nn

from itertools import repeat
import collections.abc
from torch.nn.functional import scaled_dot_product_attention
from functools import partial

def _ntuple(n):
    def parse(x):
        if isinstance(x, collections.abc.Iterable) and not isinstance(x, str):
            return x
        return tuple(repeat(x, n))

    return parse

to_2tuple = _ntuple(2)

def drop_path(
    x, drop_prob: float = 0.0, training: bool = False, scale_by_keep: bool = True
):
    """Drop paths (Stochastic Depth) per sample (when applied in main path of residual blocks)."""
    if drop_prob == 0.0 or not training:
        return x
    keep_prob = 1 - drop_prob
    shape = (x.shape[0],) + (1,) * (
        x.ndim - 1
    )  # work with diff dim tensors, not just 2D ConvNets
    random_tensor = x.new_empty(shape).bernoulli_(keep_prob)
    if keep_prob > 0.0 and scale_by_keep:
        random_tensor.div_(keep_prob)
    return x * random_tensor


import torch.nn as nn

from itertools import repeat
import collections.abc
from torch.nn.functional import scaled_dot_product_attention
from functools import partial

def _ntuple(n):
    def parse(x):
        if isinstance(x, collections.abc.Iterable) and not isinstance(x, str):
            return x
        return tuple(repeat(x, n))

    return parse

to_2tuple = _ntuple(2)

def drop_path(
    x, drop_prob: float = 0.0, training: bool = False, scale_by_keep: bool = True
):
    """Drop paths (Stochastic Depth) per sample (when applied in main path of residual blocks)."""
    if drop_prob == 0.0 or not training:
        return x
    keep_prob = 1 - drop_prob
    shape = (x.shape[0],) + (1,) * (
        x.ndim - 1
    )  # work with diff dim tensors, not just 2D ConvNets
    random_tensor = x.new_empty(shape).bernoulli_(keep_prob)
    if keep_prob > 0.0 and scale_by_keep:
        random_tensor.div_(keep_prob)
    return x * random_tensor

class DropPath(nn.Module):
    """Drop paths (Stochastic Depth) per sample  (when applied in main path of residual blocks)."""

    def __init__(self, drop_prob: float = 0.0, scale_by_keep: bool = True):
        super(DropPath, self).__init__()
        self.drop_prob = drop_prob
        self.scale_by_keep = scale_by_keep

    def forward(self, x):
        return drop_path(x, self.drop_prob, self.training, self.scale_by_keep)

    def extra_repr(self):
        return f"drop_prob={round(self.drop_prob,3):0.3f}"

class Mlp(nn.Module):
    """MLP as used in Vision Transformer, MLP-Mixer and related networks"""


DropPath

to_2tuple = _ntuple(2)

def drop_path(
    x, drop_prob: float = 0.0, training: bool = False, scale_by_keep: bool = True
):
    """Drop paths (Stochastic Depth) per sample (when applied in main path of residual blocks)."""
    if drop_prob == 0.0 or not training:
        return x
    keep_prob = 1 - drop_prob
    shape = (x.shape[0],) + (1,) * (
        x.ndim - 1
    )  # work with diff dim tensors, not just 2D ConvNets
    random_tensor = x.new_empty(shape).bernoulli_(keep_prob)
    if keep_prob > 0.0 and scale_by_keep:
        random_tensor.div_(keep_prob)
    return x * random_tensor

class DropPath(nn.Module):
    """Drop paths (Stochastic Depth) per sample  (when applied in main path of residual blocks)."""

    def __init__(self, drop_prob: float = 0.0, scale_by_keep: bool = True):
        super(DropPath, self).__init__()
        self.drop_prob = drop_prob
        self.scale_by_keep = scale_by_keep

    def forward(self, x):
        return drop_path(x, self.drop_prob, self.training, self.scale_by_keep)

    def extra_repr(self):
        return f"drop_prob={round(self.drop_prob,3):0.3f}"

class Mlp(nn.Module):
    """MLP as used in Vision Transformer, MLP-Mixer and related networks"""

    def __init__(
        self,
        in_features,
        hidden_features=None,
        out_features=None,
        act_layer=nn.GELU,
        bias=True,
        drop=0.0,
    ):
        super().__init__()
        out_features = out_features or in_features
        hidden_features = hidden_features or in_features
        bias = to_2tuple(bias)
        drop_probs = to_2tuple(drop)


Mlp

    if keep_prob > 0.0 and scale_by_keep:
        random_tensor.div_(keep_prob)
    return x * random_tensor

class DropPath(nn.Module):
    """Drop paths (Stochastic Depth) per sample  (when applied in main path of residual blocks)."""

    def __init__(self, drop_prob: float = 0.0, scale_by_keep: bool = True):
        super(DropPath, self).__init__()
        self.drop_prob = drop_prob
        self.scale_by_keep = scale_by_keep

    def forward(self, x):
        return drop_path(x, self.drop_prob, self.training, self.scale_by_keep)

    def extra_repr(self):
        return f"drop_prob={round(self.drop_prob,3):0.3f}"

class Mlp(nn.Module):
    """MLP as used in Vision Transformer, MLP-Mixer and related networks"""

    def __init__(
        self,
        in_features,
        hidden_features=None,
        out_features=None,
        act_layer=nn.GELU,
        bias=True,
        drop=0.0,
    ):
        super().__init__()
        out_features = out_features or in_features
        hidden_features = hidden_features or in_features
        bias = to_2tuple(bias)
        drop_probs = to_2tuple(drop)

        self.fc1 = nn.Linear(in_features, hidden_features, bias=bias[0])
        self.act = act_layer()
        self.drop1 = nn.Dropout(drop_probs[0])
        self.fc2 = nn.Linear(hidden_features, out_features, bias=bias[1])
        self.drop2 = nn.Dropout(drop_probs[1])

    def forward(self, x):
        return self.drop2(self.fc2(self.drop1(self.act(self.fc1(x)))))

class Attention(nn.Module):

    def __init__(
        self, dim, rope=None, num_heads=8, qkv_bias=False, attn_drop=0.0, proj_drop=0.0
    ):
        super().__init__()
        self.num_heads = num_heads
        head_dim = dim // num_heads
        self.scale = head_dim**-0.5
        self.qkv = nn.Linear(dim, dim * 3, bias=qkv_bias)
        self.attn_drop = nn.Dropout(attn_drop)
        self.proj = nn.Linear(dim, dim)
        self.proj_drop = nn.Dropout(proj_drop)
        self.rope = rope.float() if rope is not None else None

    def forward(self, x, xpos):
        B, N, C = x.shape