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

import rclpy

from leo_line_follower.line_follower_node import LineFollowerNode


def add_argparse_arguments(parser: argparse.ArgumentParser):
    parser.add_argument(
        "-p",
        "--path",
        default="~/Downloads/saved_model.tflite",
        type=str,
        required=True,
        dest="model_path",
        help="Path to the saved CNN model.",
    )
    parser.add_argument(
        "-v",
        "--velocity",
        default="cmd_vel",
        type=str,
        required=False,
        dest="velocity_topic",
        help="Name of the topic with Twist messages for rover.",
    )
    parser.add_argument(
        "-c",
        "--camera-topic",
        default="camera/image_color/compressed",
        type=str,
        required=False,
        dest="image_topic",
        help="Name of the topic with Image messages from rover.",
    )


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

    parser = argparse.ArgumentParser(description="Run line_follower node.")
    add_argparse_arguments(parser)

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

    node = LineFollowerNode(
        parsed_args.model_path, parsed_args.velocity_topic, parsed_args.image_topic
    )

    try:
        rclpy.spin(node)
    except KeyboardInterrupt as e:
        node.cleanup()
        node.destroy_node()
        rclpy.try_shutdown()
