from configargparse import ArgParser, YAMLConfigFileParser, ArgumentDefaultsRawHelpFormatter
from sys import exit
from open3d.geometry import Geometry
from open3d.visualization import Visualizer
from open3d.io import write_pinhole_camera_parameters
from pathlib import Path


from util import (
    load_dataset,
    existing_file,
)


def create_camera_settings(geometry: Geometry, camera_config_output_json_path: Path) -> None:
    vis = Visualizer()
    vis.create_window()
    vis.add_geometry(geometry)
    vis.run()
    parameters = vis.get_view_control().convert_to_pinhole_camera_parameters()
    write_pinhole_camera_parameters(camera_config_output_json_path.as_posix(), parameters)
    print(f"Written camera config to {camera_config_output_json_path.as_posix()}!")
    vis.destroy_window()


def main() -> int:
    parser = ArgParser(
        config_file_parser_class=YAMLConfigFileParser,
        default_config_files=["create_camera_settings_config.yaml"],
        formatter_class=ArgumentDefaultsRawHelpFormatter,
        description="Use an interactive window to move the camera and export the camera view at the moment of closing"
        " the window into a JSON file",
    )
    parser.add_argument("--render-config-file", is_config_file=True, help="yaml config file path")
    parser.add_argument("--input-bag-path", required=True, type=existing_file, help="path to bag file")
    parser.add_argument(
        "--camera-config-output-json-path",
        default=Path("./saved_camera_settings.json"),
        type=Path,
        help="path the camera settings json file should be saved to",
    )
    parser.add_argument(
        "--bag-pointcloud-index",
        type=int,
        default=0,
        help="index of the frame inside the experiment bag that should be displayed in 3d",
    )

    args = parser.parse_args()
    print("Creating camera settings!")
    print("Move the view in the window to the desired camera position" " and then close the window using the ESC key!")
    dataset = load_dataset(args.input_bag_path)
    open3d_pointcloud = dataset[args.bag_pointcloud_index].to_instance("open3d")
    create_camera_settings(open3d_pointcloud, args.camera_config_output_json_path)

    return 0


if __name__ == "__main__":
    exit(main())