from pointcloudset import Dataset
from pathlib import Path
from argparse import ArgumentTypeError
from subprocess import run
from datetime import timedelta
from matplotlib.colors import Colormap
from matplotlib import colormaps


def load_dataset_from_bag(bag_file_path: Path, pointcloud_topic: str = "/ouster/points") -> Dataset:
    return Dataset.from_file(bag_file_path, topic=pointcloud_topic)


def calculate_average_frame_rate(dataset: Dataset):
    timestamps = dataset.timestamps
    time_deltas = [timestamps[i + 1] - timestamps[i] for i in range(len(timestamps) - 1)]
    average_delta = sum(time_deltas, timedelta()) / len(time_deltas)
    average_frame_rate = 1 / average_delta.total_seconds()
    return average_frame_rate


def existing_file(path_string: str) -> Path:
    path = Path(path_string)
    if not path.exists():
        raise ArgumentTypeError(f"{path} does not exist!")
    if not path.is_file():
        raise ArgumentTypeError(f"{path} is not a valid file!")
    return path


def existing_folder(path_string: str) -> Path:
    path = Path(path_string)
    if not path.exists():
        raise ArgumentTypeError(f"{path} does not exist!")
    if not path.is_folder():
        raise ArgumentTypeError(f"{path} is not a valid folder!")
    return path


def get_colormap_with_special_missing_color(
    colormap_name: str, missing_data_color: str = "black", reverse: bool = False
) -> Colormap:
    colormap = colormaps[colormap_name] if not reverse else colormaps[f"{colormap_name}_r"]
    colormap.set_bad(missing_data_color)
    return colormap


def create_video_from_images(input_images_pattern: str, output_file: Path, frame_rate: int) -> None:
    # Construct the ffmpeg command
    command = [
        "ffmpeg",
        "-y",
        "-framerate",
        str(frame_rate),
        "-i",
        input_images_pattern,
        "-c:v",
        "libx264",
        "-profile:v",
        "high",
        "-crf",
        "20",
        "-pix_fmt",
        "yuv420p",
        output_file.as_posix(),
    ]
    run(command, check=True)