#!/usr/bin/env python3
import argparse
import sys

import rclpy
from leo_line_follower.data_saver_node import DataSaverNode


def add_argparse_arguments(parser: argparse.ArgumentParser):
    parser.add_argument(
        "duration",
        default=20.0,
        type=float,
        nargs="?",
        metavar="N",
        help="Duration of recording the data.",
    )
    parser.add_argument(
        "-c",
        "--camera-topic",
        default="camera/image_color/compressed",
        type=str,
        required=False,
        dest="camera_topic",
        help="Name of the topic with view from robot.",
    )
    parser.add_argument(
        "-v",
        "--velocity-topic",
        default="cmd_vel",
        type=str,
        required=False,
        nargs="?",
        dest="vel_topic",
        help="Name of the velocity topic.",
    )
    parser.add_argument(
        "-o",
        "--output",
        default="data",
        type=str,
        required=False,
        nargs="?",
        dest="output_dir",
        help="Name of an output directory for saved images.",
    )


if __name__ == "__main__":
    rclpy.init()

    parser = argparse.ArgumentParser(description="Record video from rover as images.")
    add_argparse_arguments(parser)

    sys_args = rclpy.utilities.remove_ros_args(sys.argv[1:])
    parsed_args = parser.parse_args(sys_args)

    node = DataSaverNode(
        parsed_args.duration,
        parsed_args.camera_topic,
        parsed_args.vel_topic,
        parsed_args.output_dir,
    )

    try:
        while node.running and rclpy.ok():
            rclpy.spin_once(node)
    except KeyboardInterrupt as e:
        node.cleanup()
        node.destroy_node()
        rclpy.try_shutdown()
