# Copyright 2025 Duatic AG
#
# Redistribution and use in source and binary forms, with or without modification, are permitted provided that
# the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this list of conditions, and
#    the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions, and
#    the following disclaimer in the documentation and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or
#    promote products derived from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
# WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
# PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
# TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
# HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    DeclareLaunchArgument,
    IncludeLaunchDescription,
    OpaqueFunction,
)

from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution

from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node


def launch_setup(context, *args, **kwargs):
    # Package Directories
    pkg_dynaarm_bringup = FindPackageShare("duatic_dynaarm_bringup")
    pkg_dynaarm_description = FindPackageShare("duatic_dynaarm_description")

    # Use the actual description of this package !
    pkg_dynaarm_single_arm_example_description = PathJoinSubstitution(
        [
            FindPackageShare("duatic_dynaarm_single_example_description"),
            "urdf",
            "dynaarm_single_example.urdf.xacro",
        ]
    )

    # Dynaarm Bringup
    dynaarm_bringup = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([pkg_dynaarm_bringup, "launch", "real.launch.py"])
        ),
        launch_arguments={
            "namespace": LaunchConfiguration("namespace"),
            "ethercat_bus": LaunchConfiguration("ethercat_bus"),
            "urdf_file_path": pkg_dynaarm_single_arm_example_description,
            "controllers_config": LaunchConfiguration("controllers_config"),
            "version": LaunchConfiguration("version"),
        }.items(),
    )

    # Show RVIZ (optional)
    rviz = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        namespace=LaunchConfiguration("namespace"),
        arguments=["-d", PathJoinSubstitution([pkg_dynaarm_description, "config", "config.rviz"])],
        output={"both": "log"},
        remappings=[("/tf", "tf"), ("/tf_static", "tf_static")],
        condition=IfCondition(LaunchConfiguration("start_rviz")),
    )

    # Gamepad input
    joy_node = Node(
        package="joy",
        namespace=LaunchConfiguration("namespace"),
        executable="game_controller_node",
        output="screen",
        parameters=[{"autorepeat_rate": 100.0}],
    )

    nodes_to_start = [
        dynaarm_bringup,
        rviz,
        joy_node,
    ]

    return nodes_to_start


def generate_launch_description():

    declared_arguments = [
        DeclareLaunchArgument(
            name="namespace",
            default_value="",
        ),
        DeclareLaunchArgument(
            name="ethercat_bus",
            default_value="enp86s0",
            description="The ethercat bus id or name of the robot.",
        ),
        DeclareLaunchArgument(
            "controllers_config",
            default_value=get_package_share_directory("duatic_dynaarm_single_example")
            + "/config/controllers.yaml",
            description="Path to the controllers config file",
        ),
        DeclareLaunchArgument(
            "start_rviz",
            default_value="true",
            description="Start RViz2 automatically with this launch file.",
        ),
        DeclareLaunchArgument(
            name="version",
            default_value="corydoras12",
            choices=["baracuda12", "corydoras12"],
            description="Select the desired version of robot ",
        ),
    ]

    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])